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 cv2 32import its_base_test 33import camera_properties_utils 34import capture_request_utils 35import image_processing_utils 36import its_session_utils 37import sensor_fusion_utils 38 39_CAM_FRAME_RANGE_MAX = 9.0 # Seconds: max allowed camera frame range. 40_FEATURE_MARGIN = 0.20 # Only take feature points from center 20% so that 41 # rotation measured has less rolling shutter effect. 42_FEATURE_PTS_MIN = 30 # Min number of feature pts to perform rotation analysis. 43# cv2.goodFeatures to track. 44# 'POSTMASK' is the measurement method in all previous versions of Android. 45# 'POSTMASK' finds best features on entire frame and then masks the features 46# to the vertical center FEATURE_MARGIN for the measurement. 47# 'PREMASK' is a new measurement that is used when FEATURE_PTS_MIN is not 48# found in frame. This finds the best 2*FEATURE_PTS_MIN in the FEATURE_MARGIN 49# part of the frame. 50_CV2_FEATURE_PARAMS_POSTMASK = dict(maxCorners=240, 51 qualityLevel=0.3, 52 minDistance=7, 53 blockSize=7) 54_CV2_FEATURE_PARAMS_PREMASK = dict(maxCorners=2*_FEATURE_PTS_MIN, 55 qualityLevel=0.3, 56 minDistance=7, 57 blockSize=7) 58_GYRO_SAMP_RATE_MIN = 100.0 # Samples/second: min gyro sample rate. 59_CV2_LK_PARAMS = dict(winSize=(15, 15), 60 maxLevel=2, 61 criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 62 10, 0.03)) # cv2.calcOpticalFlowPyrLK params. 63 64_NAME = os.path.splitext(os.path.basename(__file__))[0] 65_NUM_ROTATIONS = 10 66_START_FRAME = 1 67_FRAME_DELTA_TOL = 1.5 # 50% margin over nominal FPS of captures 68 69# Constants to convert between different units (for clarity). 70_SEC_TO_MSEC = 1000 71_MSEC_TO_NSEC = 1000*1000 72_NSEC_TO_SEC = 1.0E-9 73_CM_TO_M = 1E-2 74_RADS_TO_DEGS = 180/math.pi 75 76# PASS/FAIL thresholds. 77_CORR_DIST_THRESH_MAX = 0.005 78_OFFSET_MS_THRESH_MAX = 1 # mseconds 79_ROTATION_PER_FRAME_MIN = 0.001 # rads/s 80 81# PARAMs from S refactor. 82 83# Set maximum exposure time to reduce motion blur 84# blur = velocity * exp_time * N_pixels / FOV 85# blur: 3 pixels, v: chart R (17.7cm) / rot_time (1.5s), N: 640, FOV: 30cm 86_EXP_MAX = 4*_MSEC_TO_NSEC # 4 ms 87_GYRO_INIT_WAIT_TIME = 0.5 # Seconds to wait for gyro to stabilize. 88_GYRO_POST_WAIT_TIME = 0.2 # Seconds to wait to capture some extra gyro data. 89_IMG_SIZE_MAX = 640 * 480 # Maximum image size. 90_NUM_FRAMES_MAX = 300 # fps*test_length should be < this for smooth captures. 91_NUM_GYRO_PTS_TO_AVG = 20 # Number of gyroscope events to average. 92 93 94def _collect_data(cam, fps, w, h, test_length, rot_rig, chart_dist, log_path): 95 """Capture a new set of data from the device. 96 97 Captures camera frames while the user is moving the device in the proscribed 98 manner. Note since the capture request is for a manual request, optical 99 image stabilization (OIS) is disabled. 100 101 Args: 102 cam: camera object. 103 fps: frames per second capture rate. 104 w: pixel width of frames. 105 h: pixel height of frames. 106 test_length: length of time for test in seconds. 107 rot_rig: dict with 'cntl' and 'ch' defined. 108 chart_dist: float value of distance to chart in meters. 109 log_path: location to save data. 110 111 Returns: 112 frames: list of RGB images as numpy arrays. 113 """ 114 logging.debug('Starting sensor event collection') 115 props = cam.get_camera_properties() 116 props = cam.override_with_hidden_physical_camera_props(props) 117 camera_properties_utils.skip_unless( 118 camera_properties_utils.sensor_fusion_capable(props)) 119 120 # Start camera rotation. 121 p = multiprocessing.Process( 122 target=sensor_fusion_utils.rotation_rig, 123 args=(rot_rig['cntl'], rot_rig['ch'], _NUM_ROTATIONS,)) 124 p.start() 125 126 cam.start_sensor_events() 127 128 # Sleep a while for gyro events to stabilize. 129 time.sleep(_GYRO_INIT_WAIT_TIME) 130 131 # Capture frames. 132 facing = props['android.lens.facing'] 133 if (facing != camera_properties_utils.LENS_FACING_FRONT and 134 facing != camera_properties_utils.LENS_FACING_BACK): 135 raise AssertionError(f'Unknown lens facing: {facing}.') 136 137 fmt = {'format': 'yuv', 'width': w, 'height': h} 138 s, e, _, _, _ = cam.do_3a(get_results=True, do_af=False) 139 logging.debug('3A ISO: %d, exp: %.3fms', s, e/_MSEC_TO_NSEC) 140 if e > _EXP_MAX: 141 logging.debug('Re-assigning exposure time') 142 s *= int(round(e / _EXP_MAX)) 143 e = _EXP_MAX 144 s_max = props['android.sensor.info.sensitivityRange'][1] 145 if s > s_max: 146 logging.debug('Calculated ISO too large! ISO: %d, MAX: %d ' 147 'Setting ISO to max analog gain value.', s, s_max) 148 s = s_max 149 req = capture_request_utils.manual_capture_request(s, e) 150 capture_request_utils.turn_slow_filters_off(props, req) 151 fd_min = props['android.lens.info.minimumFocusDistance'] 152 fd_chart = 1 / chart_dist 153 fd_meas = min(fd_min, fd_chart) 154 logging.debug('fd_min: %.3f, fd_chart: %.3f, fd_meas: %.3f', 155 fd_min, fd_chart, fd_meas) 156 req['android.lens.focusDistance'] = fd_meas 157 req['android.control.aeTargetFpsRange'] = [fps, fps] 158 req['android.sensor.frameDuration'] = int(1 / _NSEC_TO_SEC / fps) 159 logging.debug('Capturing %dx%d with ISO %d, exp %.3fms at %dfps', 160 w, h, s, e/_MSEC_TO_NSEC, fps) 161 caps = cam.do_capture([req] * int(fps * test_length), fmt) 162 163 # Capture a bit more gyro samples for use in get_best_alignment_offset 164 time.sleep(_GYRO_POST_WAIT_TIME) 165 166 # Get the gyro events. 167 logging.debug('Reading out inertial sensor events') 168 gyro = cam.get_sensor_events()['gyro'] 169 logging.debug('Number of gyro samples %d', len(gyro)) 170 171 # Combine the gyro and camera events into a single structure. 172 logging.debug('Dumping event data') 173 starts = [cap['metadata']['android.sensor.timestamp'] for cap in caps] 174 exptimes = [cap['metadata']['android.sensor.exposureTime'] for cap in caps] 175 readouts = [cap['metadata']['android.sensor.rollingShutterSkew'] 176 for cap in caps] 177 events = {'gyro': gyro, 'cam': list(zip(starts, exptimes, readouts)), 178 'facing': facing} 179 with open('%s_events.txt' % os.path.join(log_path, _NAME), 'w') as f: 180 f.write(json.dumps(events)) 181 182 # Convert frames to RGB. 183 logging.debug('Dumping frames') 184 frames = [] 185 for i, cap in enumerate(caps): 186 img = image_processing_utils.convert_capture_to_rgb_image(cap) 187 frames.append(img) 188 image_processing_utils.write_image(img, '%s_frame%03d.png' % ( 189 os.path.join(log_path, _NAME), i)) 190 return events, frames 191 192 193def _plot_gyro_events(gyro_events, log_path): 194 """Plot x, y, and z on the gyro events. 195 196 Samples are grouped into NUM_GYRO_PTS_TO_AVG groups and averaged to minimize 197 random spikes in data. 198 199 Args: 200 gyro_events: List of gyroscope events. 201 log_path: Text to location to save data. 202 """ 203 204 nevents = (len(gyro_events) // _NUM_GYRO_PTS_TO_AVG) * _NUM_GYRO_PTS_TO_AVG 205 gyro_events = gyro_events[:nevents] 206 times = np.array([(e['time'] - gyro_events[0]['time']) * _NSEC_TO_SEC 207 for e in gyro_events]) 208 x = np.array([e['x'] for e in gyro_events]) 209 y = np.array([e['y'] for e in gyro_events]) 210 z = np.array([e['z'] for e in gyro_events]) 211 212 # Group samples into size-N groups & average each together to minimize random 213 # spikes in data. 214 times = times[_NUM_GYRO_PTS_TO_AVG//2::_NUM_GYRO_PTS_TO_AVG] 215 x = x.reshape(nevents//_NUM_GYRO_PTS_TO_AVG, _NUM_GYRO_PTS_TO_AVG).mean(1) 216 y = y.reshape(nevents//_NUM_GYRO_PTS_TO_AVG, _NUM_GYRO_PTS_TO_AVG).mean(1) 217 z = z.reshape(nevents//_NUM_GYRO_PTS_TO_AVG, _NUM_GYRO_PTS_TO_AVG).mean(1) 218 219 pylab.figure(_NAME) 220 # x & y on same axes 221 pylab.subplot(2, 1, 1) 222 pylab.title(_NAME + ' (mean of %d pts)' % _NUM_GYRO_PTS_TO_AVG) 223 pylab.plot(times, x, 'r', label='x') 224 pylab.plot(times, y, 'g', label='y') 225 pylab.ylabel('gyro x & y movement (rads/s)') 226 pylab.legend() 227 228 # z on separate axes 229 pylab.subplot(2, 1, 2) 230 pylab.plot(times, z, 'b', label='z') 231 pylab.xlabel('time (seconds)') 232 pylab.ylabel('gyro z movement (rads/s)') 233 pylab.legend() 234 matplotlib.pyplot.savefig( 235 '%s_gyro_events.png' % (os.path.join(log_path, _NAME))) 236 237 238def _get_cam_times(cam_events, fps): 239 """Get the camera frame times. 240 241 Assign a time to each frame. Assumes the image is instantly captured in the 242 middle of exposure. 243 244 Args: 245 cam_events: List of (start_exposure, exposure_time, readout_duration) 246 tuples, one per captured frame, with times in nanoseconds. 247 fps: float of frames per second value 248 249 Returns: 250 frame_times: Array of N times, one corresponding to the 'middle' of the 251 exposure of each frame. 252 """ 253 starts = np.array([start for start, exptime, readout in cam_events]) 254 max_frame_delta_ms = (np.amax(np.subtract(starts[1:], starts[0:-1])) / 255 _MSEC_TO_NSEC) 256 logging.debug('Maximum frame delta: %.3f ms', max_frame_delta_ms) 257 frame_delta_tol_ms = _FRAME_DELTA_TOL * (1 / fps) * _SEC_TO_MSEC 258 if max_frame_delta_ms > frame_delta_tol_ms: 259 raise AssertionError(f'Frame drop! Max delta: {max_frame_delta_ms:.3f}ms, ' 260 f'ATOL: {frame_delta_tol_ms}ms') 261 exptimes = np.array([exptime for start, exptime, readout in cam_events]) 262 if not np.all(exptimes == exptimes[0]): 263 raise AssertionError(f'Exposure times vary in frames! {exptimes}') 264 readouts = np.array([readout for start, exptime, readout in cam_events]) 265 if not np.all(readouts == readouts[0]): 266 raise AssertionError(f'Rolling shutter not always equal! {readouts}') 267 frame_times = starts + (exptimes + readouts) / 2.0 268 return frame_times 269 270 271def _procrustes_rotation(x, y): 272 """Performs a Procrustes analysis to conform points in x to y. 273 274 Procrustes analysis determines a linear transformation (translation, 275 reflection, orthogonal rotation and scaling) of the points in y to best 276 conform them to the points in matrix x, using the sum of squared errors 277 as the metric for fit criterion. 278 279 Args: 280 x: Target coordinate matrix 281 y: Input coordinate matrix 282 283 Returns: 284 The rotation component of the transformation that maps x to y. 285 """ 286 x0 = (x-x.mean(0)) / np.sqrt(((x-x.mean(0))**2.0).sum()) 287 y0 = (y-y.mean(0)) / np.sqrt(((y-y.mean(0))**2.0).sum()) 288 u, _, vt = np.linalg.svd(np.dot(x0.T, y0), full_matrices=False) 289 return np.dot(vt.T, u.T) 290 291 292def _get_cam_rotations(frames, facing, h, log_path): 293 """Get the rotations of the camera between each pair of frames. 294 295 Takes N frames and returns N-1 angular displacements corresponding to the 296 rotations between adjacent pairs of frames, in radians. 297 Only takes feature points from center so that rotation measured has less 298 rolling shutter effect. 299 Requires FEATURE_PTS_MIN to have enough data points for accurate measurements. 300 Uses FEATURE_PARAMS for cv2 to identify features in checkerboard images. 301 Ensures camera rotates enough. 302 303 Args: 304 frames: List of N images (as RGB numpy arrays). 305 facing: Direction camera is facing. 306 h: Pixel height of each frame. 307 log_path: Location for data. 308 309 Returns: 310 numpy array of N-1 camera rotation measurements (rad). 311 """ 312 gframes = [] 313 file_name_stem = os.path.join(log_path, _NAME) 314 for frame in frames: 315 frame = (frame * 255.0).astype(np.uint8) # cv2 uses [0, 255] 316 gframes.append(cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)) 317 num_frames = len(gframes) 318 logging.debug('num_frames: %d', num_frames) 319 320 # create mask 321 ymin = int(h * (1 - _FEATURE_MARGIN) / 2) 322 ymax = int(h * (1 + _FEATURE_MARGIN) / 2) 323 pre_mask = np.zeros_like(gframes[0]) 324 pre_mask[ymin:ymax, :] = 255 325 326 for masking in ['post', 'pre']: # Do post-masking (original) method 1st 327 logging.debug('Using %s masking method', masking) 328 rots = [] 329 for i in range(1, num_frames): 330 j = i - 1 331 gframe0 = gframes[j] 332 gframe1 = gframes[i] 333 if masking == 'post': 334 p0 = cv2.goodFeaturesToTrack( 335 gframe0, mask=None, **_CV2_FEATURE_PARAMS_POSTMASK) 336 post_mask = (p0[:, 0, 1] >= ymin) & (p0[:, 0, 1] <= ymax) 337 p0_filtered = p0[post_mask] 338 else: 339 p0_filtered = cv2.goodFeaturesToTrack( 340 gframe0, mask=pre_mask, **_CV2_FEATURE_PARAMS_PREMASK) 341 num_features = len(p0_filtered) 342 if num_features < _FEATURE_PTS_MIN: 343 for pt in p0_filtered: 344 x, y = pt[0][0], pt[0][1] 345 cv2.circle(frames[j], (x, y), 3, (100, 255, 255), -1) 346 image_processing_utils.write_image( 347 frames[j], f'{file_name_stem}_features{j+_START_FRAME:03d}.png') 348 msg = (f'Not enough features in frame {j+_START_FRAME}. Need at least ' 349 f'{_FEATURE_PTS_MIN} features, got {num_features}.') 350 if masking == 'pre': 351 raise AssertionError(msg) 352 else: 353 logging.debug(msg) 354 break 355 else: 356 logging.debug('Number of features in frame %s is %d', 357 str(j+_START_FRAME).zfill(3), num_features) 358 p1, st, _ = cv2.calcOpticalFlowPyrLK(gframe0, gframe1, p0_filtered, None, 359 **_CV2_LK_PARAMS) 360 tform = _procrustes_rotation(p0_filtered[st == 1], p1[st == 1]) 361 if facing == camera_properties_utils.LENS_FACING_BACK: 362 rot = -math.atan2(tform[0, 1], tform[0, 0]) 363 elif facing == camera_properties_utils.LENS_FACING_FRONT: 364 rot = math.atan2(tform[0, 1], tform[0, 0]) 365 else: 366 raise AssertionError(f'Unknown lens facing: {facing}.') 367 rots.append(rot) 368 if i == 1: 369 # Save debug visualization of features that are being 370 # tracked in the first frame. 371 frame = frames[j] 372 for x, y in p0_filtered[st == 1]: 373 cv2.circle(frame, (x, y), 3, (100, 255, 255), -1) 374 image_processing_utils.write_image( 375 frame, f'{file_name_stem}_features{j+_START_FRAME:03d}.png') 376 if i == num_frames-1: 377 logging.debug('Correct num of frames found: %d', i) 378 break # exit if enough features in all frames 379 if i != num_frames-1: 380 raise AssertionError('Neither method found enough features in all frames') 381 382 rots = np.array(rots) 383 rot_per_frame_max = max(abs(rots)) 384 logging.debug('Max rotation: %.4f radians', rot_per_frame_max) 385 if rot_per_frame_max < _ROTATION_PER_FRAME_MIN: 386 raise AssertionError(f'Device not moved enough: {rot_per_frame_max:.3f} ' 387 f'movement. THRESH: {_ROTATION_PER_FRAME_MIN}.') 388 389 return rots 390 391 392def _plot_best_shift(best, coeff, x, y, log_path): 393 """Saves a plot the best offset, fit data and x,y data. 394 395 Args: 396 best: x value of best fit data. 397 coeff: 3 element np array. Return of np.polyfit(x,y) for 2nd order fit. 398 x: np array of x data that was fit. 399 y: np array of y data that was fit. 400 log_path: where to store data. 401 """ 402 xfit = np.arange(x[0], x[-1], 0.05).tolist() 403 yfit = [coeff[0]*x*x + coeff[1]*x + coeff[2] for x in xfit] 404 pylab.figure() 405 pylab.title(f'{_NAME} Gyro/Camera Time Correlation') 406 pylab.plot(x, y, 'ro', label='data', alpha=0.7) 407 pylab.plot(xfit, yfit, 'b', label='fit', alpha=0.7) 408 pylab.plot(best, min(yfit), 'g*', label='best', markersize=10) 409 pylab.ticklabel_format(axis='y', style='sci', scilimits=(-3, -3)) 410 pylab.xlabel('Relative horizontal shift between curves (ms)') 411 pylab.ylabel('Correlation distance') 412 pylab.legend() 413 matplotlib.pyplot.savefig( 414 '%s_plot_shifts.png' % os.path.join(log_path, _NAME)) 415 416 417def _plot_rotations(cam_rots, gyro_rots, log_path): 418 """Saves a plot of the camera vs. gyro rotational measurements. 419 420 Args: 421 cam_rots: Array of camera rotation measurements (rads). 422 gyro_rots: Array of gyro rotation measurements (rads). 423 log_path: Location to store data. 424 """ 425 # For plot, scale rotations to degrees. 426 pylab.figure() 427 pylab.title(f'{_NAME} Gyro/Camera Rotations') 428 pylab.plot(range(len(cam_rots)), cam_rots*_RADS_TO_DEGS, '-r.', 429 label='camera', alpha=0.7) 430 pylab.plot(range(len(gyro_rots)), gyro_rots*_RADS_TO_DEGS, '-b.', 431 label='gyro', alpha=0.7) 432 pylab.xlabel('Camera frame number') 433 pylab.ylabel('Angular displacement between adjacent camera frames (degrees)') 434 pylab.xlim([0, len(cam_rots)]) 435 pylab.legend() 436 matplotlib.pyplot.savefig( 437 '%s_plot_rotations.png' % os.path.join(log_path, _NAME)) 438 439 440def load_data(): 441 """Load a set of previously captured data. 442 443 Returns: 444 events: Dictionary containing all gyro events and cam timestamps. 445 frames: List of RGB images as numpy arrays. 446 w: Pixel width of frames 447 h: Pixel height of frames 448 """ 449 with open(f'{_NAME}_events.txt', 'r') as f: 450 events = json.loads(f.read()) 451 n = len(events['cam']) 452 frames = [] 453 for i in range(n): 454 img = image_processing_utils.read_image(f'{_NAME}_frame{i:03d}.png') 455 w, h = img.size[0:2] 456 frames.append(np.array(img).reshape((h, w, 3)) / 255) 457 return events, frames, w, h 458 459 460class SensorFusionTest(its_base_test.ItsBaseTest): 461 """Tests if image and motion sensor events are well synchronized. 462 463 Tests gyro and camera timestamp differences while camera is rotating. 464 Test description is in SensorFusion.pdf file. Test rotates phone in proscribed 465 manner and captures images. 466 467 Camera rotation is determined from images and from gyroscope events. 468 Timestamp offset between gyro and camera is determined using scipy 469 spacial correlation distance. The min value is determined as the optimum. 470 471 PASS/FAIL based on the offset and also the correlation distance. 472 """ 473 474 def _assert_gyro_encompasses_camera(self, cam_times, gyro_times): 475 """Confirms the camera events are bounded by the gyroscope events. 476 477 Also ensures: 478 1. Camera frame range is less than MAX_CAMERA_FRAME_RANGE. When camera 479 frame range is significantly larger than spec, the system is usually in a 480 busy/bad state. 481 2. Gyro samples per second are greater than GYRO_SAMP_RATE_MIN 482 483 Args: 484 cam_times: numpy array of camera times. 485 gyro_times: List of 'gyro' times. 486 """ 487 min_cam_time = min(cam_times) * _NSEC_TO_SEC 488 max_cam_time = max(cam_times) * _NSEC_TO_SEC 489 min_gyro_time = min(gyro_times) * _NSEC_TO_SEC 490 max_gyro_time = max(gyro_times) * _NSEC_TO_SEC 491 if not (min_cam_time > min_gyro_time and max_cam_time < max_gyro_time): 492 raise AssertionError( 493 f'Camera timestamps [{min_cam_time}, {max_cam_time}] not ' 494 f'enclosed by gyro timestamps [{min_gyro_time}, {max_gyro_time}]') 495 496 cam_frame_range = max_cam_time - min_cam_time 497 logging.debug('Camera frame range: %f', cam_frame_range) 498 499 gyro_time_range = max_gyro_time - min_gyro_time 500 gyro_smp_per_sec = len(gyro_times) / gyro_time_range 501 logging.debug('Gyro samples per second: %f', gyro_smp_per_sec) 502 if cam_frame_range > _CAM_FRAME_RANGE_MAX: 503 raise AssertionError(f'Camera frame range, {cam_frame_range}s, too high!') 504 if gyro_smp_per_sec < _GYRO_SAMP_RATE_MIN: 505 raise AssertionError(f'Gyro sample rate, {gyro_smp_per_sec}S/s, low!') 506 507 def test_sensor_fusion(self): 508 # Determine if '--replay' in cmd line 509 replay = False 510 for s in list(sys.argv[1:]): 511 if '--replay' in s: 512 replay = True 513 logging.info('test_sensor_fusion.py not run. Using existing data set.') 514 515 rot_rig = {} 516 fps = float(self.fps) 517 img_w, img_h = self.img_w, self.img_h 518 test_length = float(self.test_length) 519 log_path = self.log_path 520 chart_distance = self.chart_distance * _CM_TO_M 521 522 if img_w * img_h > _IMG_SIZE_MAX or fps * test_length > _NUM_FRAMES_MAX: 523 logging.debug( 524 'Warning: Your test parameters may require fast write speeds' 525 ' to run smoothly. If you run into problems, consider' 526 " smaller values of 'w', 'h', 'fps', or 'test_length'.") 527 528 if replay: 529 events, frames, _, h = load_data() 530 else: 531 with its_session_utils.ItsSession( 532 device_id=self.dut.serial, 533 camera_id=self.camera_id, 534 hidden_physical_id=self.hidden_physical_id) as cam: 535 536 rot_rig['cntl'] = self.rotator_cntl 537 rot_rig['ch'] = self.rotator_ch 538 events, frames = _collect_data(cam, fps, img_w, img_h, test_length, 539 rot_rig, chart_distance, log_path) 540 logging.debug('Start frame: %d', _START_FRAME) 541 542 _plot_gyro_events(events['gyro'], log_path) 543 544 # Validity check on gyro/camera timestamps 545 cam_times = _get_cam_times( 546 events['cam'][_START_FRAME:len(events['cam'])], fps) 547 gyro_times = [e['time'] for e in events['gyro']] 548 self._assert_gyro_encompasses_camera(cam_times, gyro_times) 549 550 # Compute cam rotation displacement(rads) between pairs of adjacent frames. 551 cam_rots = _get_cam_rotations( 552 frames[_START_FRAME:len(frames)], events['facing'], img_h, log_path) 553 logging.debug('cam_rots: %s', str(cam_rots)) 554 gyro_rots = sensor_fusion_utils.get_gyro_rotations( 555 events['gyro'], cam_times) 556 _plot_rotations(cam_rots, gyro_rots, log_path) 557 558 # Find the best offset. 559 offset_ms, coeffs, candidates, distances = sensor_fusion_utils.get_best_alignment_offset( 560 cam_times, cam_rots, events['gyro']) 561 _plot_best_shift(offset_ms, coeffs, candidates, distances, log_path) 562 563 # Calculate correlation distance with best offset. 564 corr_dist = scipy.spatial.distance.correlation(cam_rots, gyro_rots) 565 logging.debug('Best correlation of %f at shift of %.3fms', 566 corr_dist, offset_ms) 567 568 # Assert PASS/FAIL criteria. 569 if corr_dist > _CORR_DIST_THRESH_MAX: 570 raise AssertionError(f'Poor gyro/camera correlation: {corr_dist:.6f}, ' 571 f'TOL: {_CORR_DIST_THRESH_MAX}.') 572 if abs(offset_ms) > _OFFSET_MS_THRESH_MAX: 573 raise AssertionError('Offset too large. Measured (ms): ' 574 f'{offset_ms:.3f}, TOL: {_OFFSET_MS_THRESH_MAX}.') 575 576if __name__ == '__main__': 577 test_runner.main() 578