1# Copyright 2014 The Android Open Source Project 2# 3# Licensed under the Apache License, Version 2.0 (the "License"); 4# you may not use this file except in compliance with the License. 5# You may obtain a copy of the License at 6# 7# http://www.apache.org/licenses/LICENSE-2.0 8# 9# Unless required by applicable law or agreed to in writing, software 10# distributed under the License is distributed on an "AS IS" BASIS, 11# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12# See the License for the specific language governing permissions and 13# limitations under the License. 14"""Verify image and inertial sensor events are well synchronized.""" 15 16 17import json 18import logging 19import math 20import os 21import sys 22import threading 23import time 24 25from matplotlib import pylab 26import matplotlib.pyplot 27from mobly import test_runner 28import numpy as np 29import scipy.spatial 30 31import its_base_test 32import camera_properties_utils 33import capture_request_utils 34import image_processing_utils 35import its_session_utils 36import sensor_fusion_utils 37 38_CAM_FRAME_RANGE_MAX = 9.0 # Seconds: max allowed camera frame range. 39_GYRO_SAMP_RATE_MIN = 100.0 # Samples/second: min gyro sample rate. 40_NAME = os.path.splitext(os.path.basename(__file__))[0] 41_ARDUINO_ANGLES = (0, 90) 42_ARDUINO_INIT_WAIT_TIME = 3.0 # Seconds to wait for Arduino comm 43_ARDUINO_MOVE_TIME = 2 44_ARDUINO_SERVO_SPEED = 20 45_NUM_ROTATIONS = 10 46_START_FRAME = 1 47_FRAME_DELTA_TOL = 1.5 # 50% margin over nominal FPS of captures 48_POLYFIT_DEGREES_LEGACY = (2, 3) 49_POLYFIT_DEGREES = (3,) 50 51# Constants to convert between different units (for clarity). 52_SEC_TO_MSEC = 1000 53_MSEC_TO_NSEC = 1000*1000 54_NSEC_TO_SEC = 1.0E-9 55_CM_TO_M = 1E-2 56_RADS_TO_DEGS = 180/math.pi 57 58# PASS/FAIL thresholds. 59_CORR_DIST_THRESH_MAX = 0.005 60_OFFSET_MS_THRESH_MAX = 1 # mseconds 61 62# Set maximum exposure time to reduce motion blur 63# blur = velocity * exp_time * N_pixels / FOV 64# blur: 3 pixels, v: chart R (17.7cm) / rot_time (1.5s), N: 640, FOV: 30cm 65_EXP_MAX = 4*_MSEC_TO_NSEC # 4 ms 66_GYRO_INIT_WAIT_TIME = 0.5 # Seconds to wait for gyro to stabilize. 67_GYRO_POST_WAIT_TIME = 0.2 # Seconds to wait to capture some extra gyro data. 68_IMG_SIZE_MAX = 640 * 480 # Maximum image size. 69_NUM_FRAMES_MAX = 300 # fps*test_length should be < this for smooth captures. 70 71 72def _collect_data(cam, fps, w, h, test_length, rot_rig, chart_dist, 73 name_with_log_path): 74 """Capture a new set of data from the device. 75 76 Captures camera frames while the user is moving the device in the proscribed 77 manner. Note since the capture request is for a manual request, optical 78 image stabilization (OIS) is disabled. 79 80 Args: 81 cam: camera object. 82 fps: frames per second capture rate. 83 w: pixel width of frames. 84 h: pixel height of frames. 85 test_length: length of time for test in seconds. 86 rot_rig: dict with 'cntl' and 'ch' defined. 87 chart_dist: float value of distance to chart in meters. 88 name_with_log_path: file name with location to save data. 89 90 Returns: 91 frames: list of RGB images as numpy arrays. 92 """ 93 logging.debug('Starting sensor event collection') 94 props = cam.get_camera_properties() 95 props = cam.override_with_hidden_physical_camera_props(props) 96 camera_properties_utils.skip_unless( 97 camera_properties_utils.sensor_fusion_capable(props) and 98 cam.get_sensors().get('gyro')) 99 100 # Start camera rotation. 101 serial_port = None 102 if rot_rig['cntl'].lower() == sensor_fusion_utils.ARDUINO_STRING.lower(): 103 # identify port 104 serial_port = sensor_fusion_utils.serial_port_def( 105 sensor_fusion_utils.ARDUINO_STRING) 106 # send test cmd to Arduino until cmd returns properly 107 sensor_fusion_utils.establish_serial_comm(serial_port) 108 p = threading.Thread( 109 target=sensor_fusion_utils.rotation_rig, 110 args=( 111 rot_rig['cntl'], 112 rot_rig['ch'], 113 _NUM_ROTATIONS, 114 _ARDUINO_ANGLES, 115 _ARDUINO_SERVO_SPEED, 116 _ARDUINO_MOVE_TIME, 117 serial_port, 118 ), 119 ) 120 p.start() 121 122 cam.start_sensor_events() 123 124 # Sleep a while for gyro events to stabilize. 125 time.sleep(_GYRO_INIT_WAIT_TIME) 126 if rot_rig['cntl'].lower() == 'arduino': 127 time.sleep(_ARDUINO_INIT_WAIT_TIME) 128 129 # Capture frames. 130 facing = props['android.lens.facing'] 131 if (facing != camera_properties_utils.LENS_FACING_FRONT and 132 facing != camera_properties_utils.LENS_FACING_BACK): 133 raise AssertionError(f'Unknown lens facing: {facing}.') 134 135 fmt = {'format': 'yuv', 'width': w, 'height': h} 136 s, e, _, _, _ = cam.do_3a(get_results=True, do_af=False) 137 logging.debug('3A ISO: %d, exp: %.3fms', s, e/_MSEC_TO_NSEC) 138 if e > _EXP_MAX: 139 logging.debug('Re-assigning exposure time') 140 s *= int(round(e / _EXP_MAX)) 141 e = _EXP_MAX 142 s_max = props['android.sensor.info.sensitivityRange'][1] 143 if s > s_max: 144 logging.debug('Calculated ISO too large! ISO: %d, MAX: %d ' 145 'Setting ISO to max analog gain value.', s, s_max) 146 s = s_max 147 req = capture_request_utils.manual_capture_request(s, e) 148 capture_request_utils.turn_slow_filters_off(props, req) 149 fd_min = props['android.lens.info.minimumFocusDistance'] 150 fd_chart = 1 / chart_dist 151 fd_meas = min(fd_min, fd_chart) 152 logging.debug('fd_min: %.3f, fd_chart: %.3f, fd_meas: %.3f', 153 fd_min, fd_chart, fd_meas) 154 req['android.lens.focusDistance'] = fd_meas 155 req['android.control.aeTargetFpsRange'] = [fps, fps] 156 req['android.sensor.frameDuration'] = int(1 / _NSEC_TO_SEC / fps) 157 logging.debug('Capturing %dx%d with ISO %d, exp %.3fms at %dfps', 158 w, h, s, e/_MSEC_TO_NSEC, fps) 159 caps = cam.do_capture([req] * int(fps * test_length), fmt) 160 161 # Capture a bit more gyro samples for use in get_best_alignment_offset 162 time.sleep(_GYRO_POST_WAIT_TIME) 163 164 # Get the gyro events. 165 logging.debug('Reading out inertial sensor events') 166 gyro = cam.get_sensor_events()['gyro'] 167 logging.debug('Number of gyro samples %d', len(gyro)) 168 169 # Combine the gyro and camera events into a single structure. 170 logging.debug('Dumping event data') 171 starts = [cap['metadata']['android.sensor.timestamp'] for cap in caps] 172 exptimes = [cap['metadata']['android.sensor.exposureTime'] for cap in caps] 173 readouts = [cap['metadata']['android.sensor.rollingShutterSkew'] 174 for cap in caps] 175 events = {'gyro': gyro, 'cam': list(zip(starts, exptimes, readouts)), 176 'facing': facing} 177 with open(f'{name_with_log_path}_events.txt', 'w') as f: 178 f.write(json.dumps(events)) 179 180 # Convert frames to RGB. 181 logging.debug('Dumping frames') 182 frames = [] 183 for i, cap in enumerate(caps): 184 img = image_processing_utils.convert_capture_to_rgb_image(cap) 185 frames.append(img) 186 image_processing_utils.write_image( 187 img, f'{name_with_log_path}_frame{i:03d}.png') 188 return events, frames 189 190 191def _get_cam_times(cam_events, fps): 192 """Get the camera frame times. 193 194 Assign a time to each frame. Assumes the image is instantly captured in the 195 middle of exposure. 196 197 Args: 198 cam_events: List of (start_exposure, exposure_time, readout_duration) 199 tuples, one per captured frame, with times in nanoseconds. 200 fps: float of frames per second value 201 202 Returns: 203 frame_times: Array of N times, one corresponding to the 'middle' of the 204 exposure of each frame. 205 """ 206 starts = np.array([start for start, exptime, readout in cam_events]) 207 max_frame_delta_ms = (np.amax(np.subtract(starts[1:], starts[0:-1])) / 208 _MSEC_TO_NSEC) 209 logging.debug('Maximum frame delta: %.3f ms', max_frame_delta_ms) 210 frame_delta_tol_ms = _FRAME_DELTA_TOL * (1 / fps) * _SEC_TO_MSEC 211 if max_frame_delta_ms > frame_delta_tol_ms: 212 raise AssertionError(f'Frame drop! Max delta: {max_frame_delta_ms:.3f}ms, ' 213 f'ATOL: {frame_delta_tol_ms}ms') 214 exptimes = np.array([exptime for start, exptime, readout in cam_events]) 215 if not np.all(exptimes == exptimes[0]): 216 raise AssertionError(f'Exposure times vary in frames! {exptimes}') 217 readouts = np.array([readout for start, exptime, readout in cam_events]) 218 if not np.all(readouts == readouts[0]): 219 raise AssertionError(f'Rolling shutter not always equal! {readouts}') 220 frame_times = starts + (exptimes + readouts) / 2.0 221 return frame_times 222 223 224def _plot_best_shift(best, coeff, x, y, name_with_log_path, degree): 225 """Saves a plot the best offset, fit data and x,y data. 226 227 Args: 228 best: x value of best fit data. 229 coeff: 3 element np array. Return of np.polyfit(x,y) for 2nd order fit. 230 x: np array of x data that was fit. 231 y: np array of y data that was fit. 232 name_with_log_path: file name with where to store data. 233 degree: degree of polynomial used to fit. 234 """ 235 xfit = np.arange(x[0], x[-1], 0.05).tolist() 236 polynomial = sensor_fusion_utils.polynomial_from_coefficients(coeff) 237 yfit = [polynomial(x) for x in xfit] 238 logging.debug('Degree %s best x: %s, y: %s', degree, best, min(yfit)) 239 pylab.figure() 240 pylab.title(f'{_NAME} Degree: {degree} Gyro/Camera Time Correlation') 241 pylab.plot(x, y, 'ro', label='data', alpha=0.7) 242 pylab.plot(xfit, yfit, 'b', label='fit', alpha=0.7) 243 pylab.plot(best, min(yfit), 'g*', label='best', markersize=10) 244 pylab.ticklabel_format(axis='y', style='sci', scilimits=(-3, -3)) 245 pylab.xlabel('Relative horizontal shift between curves (ms)') 246 pylab.ylabel('Correlation distance') 247 pylab.legend() 248 matplotlib.pyplot.savefig( 249 f'{name_with_log_path}_plot_shifts_degree{degree}.png') 250 251 252def _plot_rotations(cam_rots, gyro_rots, name_with_log_path): 253 """Saves a plot of the camera vs. gyro rotational measurements. 254 255 Args: 256 cam_rots: Array of camera rotation measurements (rads). 257 gyro_rots: Array of gyro rotation measurements (rads). 258 name_with_log_path: File name with location to store data. 259 """ 260 # For plot, scale rotations to degrees. 261 pylab.figure() 262 pylab.title(f'{_NAME} Gyro/Camera Rotations') 263 pylab.plot(range(len(cam_rots)), cam_rots*_RADS_TO_DEGS, '-r.', 264 label='camera', alpha=0.7) 265 pylab.plot(range(len(gyro_rots)), gyro_rots*_RADS_TO_DEGS, '-b.', 266 label='gyro', alpha=0.7) 267 pylab.xlabel('Camera frame number') 268 pylab.ylabel('Angular displacement between adjacent camera frames (degrees)') 269 pylab.xlim([0, len(cam_rots)]) 270 pylab.legend() 271 matplotlib.pyplot.savefig(f'{name_with_log_path}_plot_rotations.png') 272 273 274def load_data(): 275 """Load a set of previously captured data. 276 277 Returns: 278 events: Dictionary containing all gyro events and cam timestamps. 279 frames: List of RGB images as numpy arrays. 280 w: Pixel width of frames 281 h: Pixel height of frames 282 """ 283 with open(f'{_NAME}_events.txt', 'r') as f: 284 events = json.loads(f.read()) 285 n = len(events['cam']) 286 frames = [] 287 for i in range(n): 288 img = image_processing_utils.read_image(f'{_NAME}_frame{i:03d}.png') 289 w, h = img.size[0:2] 290 frames.append(np.array(img).reshape((h, w, 3)) / 255) 291 return events, frames, w, h 292 293 294class SensorFusionTest(its_base_test.ItsBaseTest): 295 """Tests if image and motion sensor events are well synchronized. 296 297 Tests gyro and camera timestamp differences while camera is rotating. 298 Test description is in SensorFusion.pdf file. Test rotates phone in proscribed 299 manner and captures images. 300 301 Camera rotation is determined from images and from gyroscope events. 302 Timestamp offset between gyro and camera is determined using scipy 303 spacial correlation distance. The min value is determined as the optimum. 304 305 PASS/FAIL based on the offset and also the correlation distance. 306 """ 307 308 def _assert_gyro_encompasses_camera(self, cam_times, gyro_times): 309 """Confirms the camera events are bounded by the gyroscope events. 310 311 Also ensures: 312 1. Camera frame range is less than MAX_CAMERA_FRAME_RANGE. When camera 313 frame range is significantly larger than spec, the system is usually in a 314 busy/bad state. 315 2. Gyro samples per second are greater than GYRO_SAMP_RATE_MIN 316 317 Args: 318 cam_times: numpy array of camera times. 319 gyro_times: List of 'gyro' times. 320 """ 321 min_cam_time = min(cam_times) * _NSEC_TO_SEC 322 max_cam_time = max(cam_times) * _NSEC_TO_SEC 323 min_gyro_time = min(gyro_times) * _NSEC_TO_SEC 324 max_gyro_time = max(gyro_times) * _NSEC_TO_SEC 325 if not (min_cam_time > min_gyro_time and max_cam_time < max_gyro_time): 326 raise AssertionError( 327 f'Camera timestamps [{min_cam_time}, {max_cam_time}] not ' 328 f'enclosed by gyro timestamps [{min_gyro_time}, {max_gyro_time}]') 329 330 cam_frame_range = max_cam_time - min_cam_time 331 logging.debug('Camera frame range: %f', cam_frame_range) 332 333 gyro_time_range = max_gyro_time - min_gyro_time 334 gyro_smp_per_sec = len(gyro_times) / gyro_time_range 335 logging.debug('Gyro samples per second: %f', gyro_smp_per_sec) 336 if cam_frame_range > _CAM_FRAME_RANGE_MAX: 337 raise AssertionError( 338 f'Camera frame range, {cam_frame_range}s, too high!') 339 if gyro_smp_per_sec < _GYRO_SAMP_RATE_MIN: 340 raise AssertionError(f'Gyro sample rate, {gyro_smp_per_sec}S/s, low!') 341 342 def test_sensor_fusion(self): 343 # Determine if '--replay' in cmd line 344 replay = False 345 for s in list(sys.argv[1:]): 346 if '--replay' in s: 347 replay = True 348 logging.info('test_sensor_fusion.py not run. Using existing data set.') 349 350 rot_rig = {} 351 fps = float(self.fps) 352 img_w, img_h = self.img_w, self.img_h 353 test_length = float(self.test_length) 354 name_with_log_path = os.path.join(self.log_path, _NAME) 355 chart_distance = self.chart_distance * _CM_TO_M 356 357 if img_w * img_h > _IMG_SIZE_MAX or fps * test_length > _NUM_FRAMES_MAX: 358 logging.debug( 359 'Warning: Your test parameters may require fast write speeds' 360 ' to run smoothly. If you run into problems, consider' 361 " smaller values of 'w', 'h', 'fps', or 'test_length'.") 362 363 if replay: 364 events, frames, _, _ = load_data() 365 else: 366 with its_session_utils.ItsSession( 367 device_id=self.dut.serial, 368 camera_id=self.camera_id, 369 hidden_physical_id=self.hidden_physical_id) as cam: 370 371 rot_rig['cntl'] = self.rotator_cntl 372 rot_rig['ch'] = self.rotator_ch 373 events, frames = _collect_data( 374 cam, fps, img_w, img_h, test_length, rot_rig, chart_distance, 375 name_with_log_path) 376 logging.debug('Start frame: %d', _START_FRAME) 377 378 sensor_fusion_utils.plot_gyro_events(events['gyro'], _NAME, self.log_path) 379 380 # Validity check on gyro/camera timestamps 381 cam_times = _get_cam_times( 382 events['cam'][_START_FRAME:len(events['cam'])], fps) 383 gyro_times = [e['time'] for e in events['gyro']] 384 self._assert_gyro_encompasses_camera(cam_times, gyro_times) 385 386 # Compute cam rotation displacement(rads) between pairs of adjacent frames. 387 cam_rots = sensor_fusion_utils.get_cam_rotations( 388 frames[_START_FRAME:len(frames)], events['facing'], img_h, 389 name_with_log_path, _START_FRAME) 390 logging.debug('cam_rots: %s', str(cam_rots)) 391 gyro_rots = sensor_fusion_utils.get_gyro_rotations( 392 events['gyro'], cam_times) 393 _plot_rotations(cam_rots, gyro_rots, name_with_log_path) 394 395 # Find the best offset. Starting with Android 14, use 3rd order polynomial 396 first_api_level = its_session_utils.get_first_api_level(self.dut.serial) 397 polyfit_degrees = list(_POLYFIT_DEGREES) 398 if first_api_level <= its_session_utils.ANDROID13_API_LEVEL: 399 polyfit_degrees = list(_POLYFIT_DEGREES_LEGACY) 400 logging.debug('Attempting to fit data to polynomials of degrees: %s', 401 polyfit_degrees) 402 for degree in polyfit_degrees: 403 output = sensor_fusion_utils.get_best_alignment_offset( 404 cam_times, cam_rots, events['gyro'], degree=degree 405 ) 406 if not output: 407 logging.debug('Degree %d was not a good fit.', degree) 408 continue 409 logging.debug('Degree %d was a good fit.', degree) 410 offset_ms, coeffs, candidates, distances = output 411 _plot_best_shift( 412 offset_ms, coeffs, candidates, distances, name_with_log_path, degree) 413 break 414 else: 415 raise AssertionError( 416 f'No degree in {polyfit_degrees} was a good fit for the data!' 417 ) 418 419 # Calculate correlation distance with best offset. 420 corr_dist = scipy.spatial.distance.correlation(cam_rots, gyro_rots) 421 logging.debug('Best correlation of %f at shift of %.3fms', 422 corr_dist, offset_ms) 423 424 # Assert PASS/FAIL criteria. 425 if corr_dist > _CORR_DIST_THRESH_MAX: 426 raise AssertionError(f'Poor gyro/camera correlation: {corr_dist:.6f}, ' 427 f'TOL: {_CORR_DIST_THRESH_MAX}.') 428 if abs(offset_ms) > _OFFSET_MS_THRESH_MAX: 429 raise AssertionError('Offset too large. Measured (ms): ' 430 f'{offset_ms:.3f}, TOL: {_OFFSET_MS_THRESH_MAX}.') 431 432if __name__ == '__main__': 433 test_runner.main() 434