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 15 import bisect 16 import json 17 import math 18 import os.path 19 import sys 20 import time 21 22 import cv2 23 import its.caps 24 import its.device 25 import its.image 26 import its.objects 27 import matplotlib 28 from matplotlib import pylab 29 import matplotlib.pyplot 30 import numpy 31 from PIL import Image 32 import scipy.spatial 33 34 NAME = os.path.basename(__file__).split(".")[0] 35 36 W, H = 640, 480 37 FPS = 30 38 TEST_LENGTH = 7 # seconds 39 FEATURE_MARGIN = 0.20 # Only take feature points from the center 20% 40 # so that the rotation measured have much less of rolling 41 # shutter effect 42 43 MIN_FEATURE_PTS = 30 # Minimum number of feature points required to 44 # perform rotation analysis 45 46 MAX_CAM_FRM_RANGE_SEC = 9.0 # Maximum allowed camera frame range. When this 47 # number is significantly larger than 7 seconds, 48 # usually system is in some busy/bad states. 49 50 MIN_GYRO_SMP_RATE = 100.0 # Minimum gyro sample rate 51 52 FEATURE_PARAMS = dict(maxCorners=240, 53 qualityLevel=0.3, 54 minDistance=7, 55 blockSize=7) 56 57 LK_PARAMS = dict(winSize=(15, 15), 58 maxLevel=2, 59 criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 60 10, 0.03)) 61 62 # Constants to convert between different units (for clarity). 63 SEC_TO_NSEC = 1000*1000*1000.0 64 SEC_TO_MSEC = 1000.0 65 MSEC_TO_NSEC = 1000*1000.0 66 MSEC_TO_SEC = 1/1000.0 67 NSEC_TO_SEC = 1/(1000*1000*1000.0) 68 NSEC_TO_MSEC = 1/(1000*1000.0) 69 CM_TO_M = 1/100.0 70 71 # PASS/FAIL thresholds. 72 THRESH_MAX_CORR_DIST = 0.005 73 THRESH_MAX_SHIFT_MS = 1 74 THRESH_MIN_ROT = 0.001 75 76 # lens facing 77 FACING_FRONT = 0 78 FACING_BACK = 1 79 FACING_EXTERNAL = 2 80 81 # Chart distance 82 CHART_DISTANCE = 25 # cm 83 84 85 def main(): 86 """Test if image and motion sensor events are well synchronized. 87 88 The instructions for running this test are in the SensorFusion.pdf file in 89 the same directory as this test. 90 91 Note that if fps*test_length is too large, write speeds may become a 92 bottleneck and camera capture will slow down or stop. 93 94 Command line arguments: 95 fps: FPS to capture with during the test 96 img_size: Comma-separated dimensions of captured images (defaults to 97 640x480). Ex: "img_size=<width>,<height>" 98 replay: Without this argument, the test will collect a new set of 99 camera+gyro data from the device and then analyze it (and 100 it will also dump this data to files in the current 101 directory). If the "replay" argument is provided, then the 102 script will instead load the dumped data from a previous 103 run and analyze that instead. This can be helpful for 104 developers who are digging for additional information on 105 their measurements. 106 test_length: How long the test should run for (in seconds) 107 """ 108 109 fps = FPS 110 w, h = W, H 111 test_length = TEST_LENGTH 112 for s in sys.argv[1:]: 113 if s[:4] == "fps=" and len(s) > 4: 114 fps = int(s[4:]) 115 elif s[:9] == "img_size=" and len(s) > 9: 116 # Split by comma and convert each dimension to int. 117 [w, h] = map(int, s[9:].split(",")) 118 elif s[:12] == "test_length=" and len(s) > 12: 119 test_length = float(s[12:]) 120 121 # Collect or load the camera+gyro data. All gyro events as well as camera 122 # timestamps are in the "events" dictionary, and "frames" is a list of 123 # RGB images as numpy arrays. 124 if "replay" not in sys.argv: 125 if w * h > 640 * 480 or fps * test_length > 300: 126 warning_str = ( 127 "Warning: Your test parameters may require fast write speeds " 128 "to run smoothly. If you run into problems, consider smaller " 129 "values of \'w\', \'h\', \'fps\', or \'test_length\'." 130 ) 131 print warning_str 132 events, frames = collect_data(fps, w, h, test_length) 133 else: 134 events, frames, _, h = load_data() 135 136 # Sanity check camera timestamps are enclosed by sensor timestamps 137 # This will catch bugs where camera and gyro timestamps go completely out 138 # of sync 139 cam_times = get_cam_times(events["cam"]) 140 min_cam_time = min(cam_times) * NSEC_TO_SEC 141 max_cam_time = max(cam_times) * NSEC_TO_SEC 142 gyro_times = [e["time"] for e in events["gyro"]] 143 min_gyro_time = min(gyro_times) * NSEC_TO_SEC 144 max_gyro_time = max(gyro_times) * NSEC_TO_SEC 145 if not (min_cam_time > min_gyro_time and max_cam_time < max_gyro_time): 146 fail_str = ("Test failed: " 147 "camera timestamps [%f,%f] " 148 "are not enclosed by " 149 "gyro timestamps [%f, %f]" 150 ) % (min_cam_time, max_cam_time, 151 min_gyro_time, max_gyro_time) 152 print fail_str 153 assert 0 154 155 cam_frame_range = max_cam_time - min_cam_time 156 gyro_time_range = max_gyro_time - min_gyro_time 157 gyro_smp_per_sec = len(gyro_times) / gyro_time_range 158 print "Camera frame range", max_cam_time - min_cam_time 159 print "Gyro samples per second", gyro_smp_per_sec 160 assert cam_frame_range < MAX_CAM_FRM_RANGE_SEC 161 assert gyro_smp_per_sec > MIN_GYRO_SMP_RATE 162 163 # Compute the camera rotation displacements (rad) between each pair of 164 # adjacent frames. 165 cam_rots = get_cam_rotations(frames, events["facing"], h) 166 if max(abs(cam_rots)) < THRESH_MIN_ROT: 167 print "Device wasn't moved enough" 168 assert 0 169 170 # Find the best offset (time-shift) to align the gyro and camera motion 171 # traces; this function integrates the shifted gyro data between camera 172 # samples for a range of candidate shift values, and returns the shift that 173 # result in the best correlation. 174 offset = get_best_alignment_offset(cam_times, cam_rots, events["gyro"]) 175 176 # Plot the camera and gyro traces after applying the best shift. 177 cam_times += offset*SEC_TO_NSEC 178 gyro_rots = get_gyro_rotations(events["gyro"], cam_times) 179 plot_rotations(cam_rots, gyro_rots) 180 181 # Pass/fail based on the offset and also the correlation distance. 182 dist = scipy.spatial.distance.correlation(cam_rots, gyro_rots) 183 print "Best correlation of %f at shift of %.2fms"%(dist, offset*SEC_TO_MSEC) 184 assert dist < THRESH_MAX_CORR_DIST 185 assert abs(offset) < THRESH_MAX_SHIFT_MS*MSEC_TO_SEC 186 187 188 def get_best_alignment_offset(cam_times, cam_rots, gyro_events): 189 """Find the best offset to align the camera and gyro traces. 190 191 Uses a correlation distance metric between the curves, where a smaller 192 value means that the curves are better-correlated. 193 194 Args: 195 cam_times: Array of N camera times, one for each frame. 196 cam_rots: Array of N-1 camera rotation displacements (rad). 197 gyro_events: List of gyro event objects. 198 199 Returns: 200 Offset (seconds) of the best alignment. 201 """ 202 # Measure the corr. dist. over a shift of up to +/- 50ms (0.5ms step size). 203 # Get the shift corresponding to the best (lowest) score. 204 candidates = numpy.arange(-50, 50.5, 0.5).tolist() 205 dists = [] 206 for shift in candidates: 207 times = cam_times + shift*MSEC_TO_NSEC 208 gyro_rots = get_gyro_rotations(gyro_events, times) 209 dists.append(scipy.spatial.distance.correlation(cam_rots, gyro_rots)) 210 best_corr_dist = min(dists) 211 best_shift = candidates[dists.index(best_corr_dist)] 212 213 print "Best shift without fitting is ", best_shift, "ms" 214 215 # Fit a curve to the corr. dist. data to measure the minima more 216 # accurately, by looking at the correlation distances within a range of 217 # +/- 10ms from the measured best score; note that this will use fewer 218 # than the full +/- 10 range for the curve fit if the measured score 219 # (which is used as the center of the fit) is within 10ms of the edge of 220 # the +/- 50ms candidate range. 221 i = dists.index(best_corr_dist) 222 candidates = candidates[i-20:i+21] 223 dists = dists[i-20:i+21] 224 a, b, c = numpy.polyfit(candidates, dists, 2) 225 exact_best_shift = -b/(2*a) 226 if abs(best_shift - exact_best_shift) > 2.0 or a <= 0 or c <= 0: 227 print "Test failed; bad fit to time-shift curve" 228 print "best_shift %f, exact_best_shift %f, a %f, c %f" % ( 229 best_shift, exact_best_shift, a, c) 230 assert 0 231 232 xfit = numpy.arange(candidates[0], candidates[-1], 0.05).tolist() 233 yfit = [a*x*x+b*x+c for x in xfit] 234 matplotlib.pyplot.figure() 235 pylab.plot(candidates, dists, "r", label="data") 236 pylab.plot(xfit, yfit, "", label="fit") 237 pylab.plot([exact_best_shift+x for x in [-0.1, 0, 0.1]], [0, 0.01, 0], "b") 238 pylab.xlabel("Relative horizontal shift between curves (ms)") 239 pylab.ylabel("Correlation distance") 240 pylab.legend() 241 matplotlib.pyplot.savefig("%s_plot_shifts.png" % (NAME)) 242 243 return exact_best_shift * MSEC_TO_SEC 244 245 246 def plot_rotations(cam_rots, gyro_rots): 247 """Save a plot of the camera vs. gyro rotational measurements. 248 249 Args: 250 cam_rots: Array of N-1 camera rotation measurements (rad). 251 gyro_rots: Array of N-1 gyro rotation measurements (rad). 252 """ 253 # For the plot, scale the rotations to be in degrees. 254 scale = 360/(2*math.pi) 255 matplotlib.pyplot.figure() 256 cam_rots *= scale 257 gyro_rots *= scale 258 pylab.plot(range(len(cam_rots)), cam_rots, "r", label="camera") 259 pylab.plot(range(len(gyro_rots)), gyro_rots, "b", label="gyro") 260 pylab.legend() 261 pylab.xlabel("Camera frame number") 262 pylab.ylabel("Angular displacement between adjacent camera frames (deg)") 263 pylab.xlim([0, len(cam_rots)]) 264 matplotlib.pyplot.savefig("%s_plot.png" % (NAME)) 265 266 267 def get_gyro_rotations(gyro_events, cam_times): 268 """Get the rotation values of the gyro. 269 270 Integrates the gyro data between each camera frame to compute an angular 271 displacement. 272 273 Args: 274 gyro_events: List of gyro event objects. 275 cam_times: Array of N camera times, one for each frame. 276 277 Returns: 278 Array of N-1 gyro rotation measurements (rad). 279 """ 280 all_times = numpy.array([e["time"] for e in gyro_events]) 281 all_rots = numpy.array([e["z"] for e in gyro_events]) 282 gyro_rots = [] 283 # Integrate the gyro data between each pair of camera frame times. 284 for icam in range(len(cam_times)-1): 285 # Get the window of gyro samples within the current pair of frames. 286 tcam0 = cam_times[icam] 287 tcam1 = cam_times[icam+1] 288 igyrowindow0 = bisect.bisect(all_times, tcam0) 289 igyrowindow1 = bisect.bisect(all_times, tcam1) 290 sgyro = 0 291 # Integrate samples within the window. 292 for igyro in range(igyrowindow0, igyrowindow1): 293 vgyro = all_rots[igyro+1] 294 tgyro0 = all_times[igyro] 295 tgyro1 = all_times[igyro+1] 296 deltatgyro = (tgyro1 - tgyro0) * NSEC_TO_SEC 297 sgyro += vgyro * deltatgyro 298 # Handle the fractional intervals at the sides of the window. 299 for side, igyro in enumerate([igyrowindow0-1, igyrowindow1]): 300 vgyro = all_rots[igyro+1] 301 tgyro0 = all_times[igyro] 302 tgyro1 = all_times[igyro+1] 303 deltatgyro = (tgyro1 - tgyro0) * NSEC_TO_SEC 304 if side == 0: 305 f = (tcam0 - tgyro0) / (tgyro1 - tgyro0) 306 sgyro += vgyro * deltatgyro * (1.0 - f) 307 else: 308 f = (tcam1 - tgyro0) / (tgyro1 - tgyro0) 309 sgyro += vgyro * deltatgyro * f 310 gyro_rots.append(sgyro) 311 gyro_rots = numpy.array(gyro_rots) 312 return gyro_rots 313 314 315 def get_cam_rotations(frames, facing, h): 316 """Get the rotations of the camera between each pair of frames. 317 318 Takes N frames and returns N-1 angular displacements corresponding to the 319 rotations between adjacent pairs of frames, in radians. 320 321 Args: 322 frames: List of N images (as RGB numpy arrays). 323 facing: Direction camera is facing 324 h: Pixel height of each frame 325 326 Returns: 327 Array of N-1 camera rotation measurements (rad). 328 """ 329 gframes = [] 330 for frame in frames: 331 frame = (frame * 255.0).astype(numpy.uint8) 332 gframes.append(cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)) 333 rots = [] 334 335 ymin = h*(1-FEATURE_MARGIN)/2 336 ymax = h*(1+FEATURE_MARGIN)/2 337 for i in range(1, len(gframes)): 338 gframe0 = gframes[i-1] 339 gframe1 = gframes[i] 340 p0 = cv2.goodFeaturesToTrack(gframe0, mask=None, **FEATURE_PARAMS) 341 # p0's shape is N * 1 * 2 342 mask = (p0[:, 0, 1] >= ymin) & (p0[:, 0, 1] <= ymax) 343 p0_filtered = p0[mask] 344 num_features = len(p0_filtered) 345 if num_features < MIN_FEATURE_PTS: 346 print "Not enough feature points in frame", i 347 print "Need at least %d features, got %d" % ( 348 MIN_FEATURE_PTS, num_features) 349 assert 0 350 else: 351 print "Number of features in frame %d is %d" % (i, num_features) 352 p1, st, _ = cv2.calcOpticalFlowPyrLK(gframe0, gframe1, p0_filtered, 353 None, **LK_PARAMS) 354 tform = procrustes_rotation(p0_filtered[st == 1], p1[st == 1]) 355 if facing == FACING_BACK: 356 rot = -math.atan2(tform[0, 1], tform[0, 0]) 357 elif facing == FACING_FRONT: 358 rot = math.atan2(tform[0, 1], tform[0, 0]) 359 else: 360 print "Unknown lens facing", facing 361 assert 0 362 rots.append(rot) 363 if i == 1: 364 # Save a debug visualization of the features that are being 365 # tracked in the first frame. 366 frame = frames[i] 367 for x, y in p0_filtered[st == 1]: 368 cv2.circle(frame, (x, y), 3, (100, 100, 255), -1) 369 its.image.write_image(frame, "%s_features.png" % NAME) 370 return numpy.array(rots) 371 372 373 def get_cam_times(cam_events): 374 """Get the camera frame times. 375 376 Args: 377 cam_events: List of (start_exposure, exposure_time, readout_duration) 378 tuples, one per captured frame, with times in nanoseconds. 379 380 Returns: 381 frame_times: Array of N times, one corresponding to the "middle" of 382 the exposure of each frame. 383 """ 384 # Assign a time to each frame that assumes that the image is instantly 385 # captured in the middle of its exposure. 386 starts = numpy.array([start for start, exptime, readout in cam_events]) 387 exptimes = numpy.array([exptime for start, exptime, readout in cam_events]) 388 readouts = numpy.array([readout for start, exptime, readout in cam_events]) 389 frame_times = starts + (exptimes + readouts) / 2.0 390 return frame_times 391 392 393 def load_data(): 394 """Load a set of previously captured data. 395 396 Returns: 397 events: Dictionary containing all gyro events and cam timestamps. 398 frames: List of RGB images as numpy arrays. 399 w: Pixel width of frames 400 h: Pixel height of frames 401 """ 402 with open("%s_events.txt" % NAME, "r") as f: 403 events = json.loads(f.read()) 404 n = len(events["cam"]) 405 frames = [] 406 for i in range(n): 407 img = Image.open("%s_frame%03d.png" % (NAME, i)) 408 w, h = img.size[0:2] 409 frames.append(numpy.array(img).reshape(h, w, 3) / 255.0) 410 return events, frames, w, h 411 412 413 def collect_data(fps, w, h, test_length): 414 """Capture a new set of data from the device. 415 416 Captures both motion data and camera frames, while the user is moving 417 the device in a proscribed manner. 418 419 Args: 420 fps: FPS to capture with 421 w: Pixel width of frames 422 h: Pixel height of frames 423 test_length: How long the test should run for (in seconds) 424 425 Returns: 426 events: Dictionary containing all gyro events and cam timestamps. 427 frames: List of RGB images as numpy arrays. 428 """ 429 with its.device.ItsSession() as cam: 430 props = cam.get_camera_properties() 431 its.caps.skip_unless(its.caps.sensor_fusion(props) and 432 its.caps.manual_sensor(props) and 433 props["android.lens.facing"] != FACING_EXTERNAL) 434 435 print "Starting sensor event collection" 436 cam.start_sensor_events() 437 438 # Sleep a while for gyro events to stabilize. 439 time.sleep(0.5) 440 441 # Capture the frames. OIS is disabled for manual captures. 442 facing = props["android.lens.facing"] 443 if facing != FACING_FRONT and facing != FACING_BACK: 444 print "Unknown lens facing", facing 445 assert 0 446 447 fmt = {"format": "yuv", "width": w, "height": h} 448 s, e, _, _, _ = cam.do_3a(get_results=True, do_af=False) 449 req = its.objects.manual_capture_request(s, e) 450 req["android.lens.focusDistance"] = 1 / (CHART_DISTANCE * CM_TO_M) 451 req["android.control.aeTargetFpsRange"] = [fps, fps] 452 req["android.sensor.frameDuration"] = int(1000.0/fps * MSEC_TO_NSEC) 453 print "Capturing %dx%d with sens. %d, exp. time %.1fms at %dfps" % ( 454 w, h, s, e*NSEC_TO_MSEC, fps) 455 caps = cam.do_capture([req]*int(fps*test_length), fmt) 456 457 # Capture a bit more gyro samples for use in 458 # get_best_alignment_offset 459 time.sleep(0.2) 460 461 # Get the gyro events. 462 print "Reading out sensor events" 463 gyro = cam.get_sensor_events()["gyro"] 464 print "Number of gyro samples", len(gyro) 465 466 # Combine the events into a single structure. 467 print "Dumping event data" 468 starts = [c["metadata"]["android.sensor.timestamp"] for c in caps] 469 exptimes = [c["metadata"]["android.sensor.exposureTime"] for c in caps] 470 readouts = [c["metadata"]["android.sensor.rollingShutterSkew"] 471 for c in caps] 472 events = {"gyro": gyro, "cam": zip(starts, exptimes, readouts), 473 "facing": facing} 474 with open("%s_events.txt" % NAME, "w") as f: 475 f.write(json.dumps(events)) 476 477 # Convert the frames to RGB. 478 print "Dumping frames" 479 frames = [] 480 for i, c in enumerate(caps): 481 img = its.image.convert_capture_to_rgb_image(c) 482 frames.append(img) 483 its.image.write_image(img, "%s_frame%03d.png" % (NAME, i)) 484 485 return events, frames 486 487 488 def procrustes_rotation(X, Y): 489 """Performs a Procrustes analysis to conform points in X to Y. 490 491 Procrustes analysis determines a linear transformation (translation, 492 reflection, orthogonal rotation and scaling) of the points in Y to best 493 conform them to the points in matrix X, using the sum of squared errors 494 as the goodness of fit criterion. 495 496 Args: 497 X: Target coordinate matrix 498 Y: Input coordinate matrix 499 500 Returns: 501 The rotation component of the transformation that maps X to Y. 502 """ 503 X0 = (X-X.mean(0)) / numpy.sqrt(((X-X.mean(0))**2.0).sum()) 504 Y0 = (Y-Y.mean(0)) / numpy.sqrt(((Y-Y.mean(0))**2.0).sum()) 505 U, _, Vt = numpy.linalg.svd(numpy.dot(X0.T, Y0), full_matrices=False) 506 return numpy.dot(Vt.T, U.T) 507 508 509 if __name__ == "__main__": 510 main() 511
