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