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 its.image 16 import its.device 17 import its.objects 18 import time 19 import math 20 import pylab 21 import os.path 22 import matplotlib 23 import matplotlib.pyplot 24 import json 25 import Image 26 import numpy 27 import cv2 28 import bisect 29 import scipy.spatial 30 import sys 31 32 NAME = os.path.basename(__file__).split(".")[0] 33 34 # Capture 210 QVGA frames (which is 7s at 30fps) 35 N = 210 36 W,H = 320,240 37 38 FEATURE_PARAMS = dict( maxCorners = 50, 39 qualityLevel = 0.3, 40 minDistance = 7, 41 blockSize = 7 ) 42 43 LK_PARAMS = dict( winSize = (15, 15), 44 maxLevel = 2, 45 criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 46 10, 0.03)) 47 48 # Constants to convert between different time units (for clarity). 49 SEC_TO_NSEC = 1000*1000*1000.0 50 SEC_TO_MSEC = 1000.0 51 MSEC_TO_NSEC = 1000*1000.0 52 MSEC_TO_SEC = 1/1000.0 53 NSEC_TO_SEC = 1/(1000*1000*1000.0) 54 NSEC_TO_MSEC = 1/(1000*1000.0) 55 56 # Pass/fail thresholds. 57 THRESH_MAX_CORR_DIST = 0.005 58 THRESH_MAX_SHIFT_MS = 2 59 THRESH_MIN_ROT = 0.001 60 61 def main(): 62 """Test if image and motion sensor events are well synchronized. 63 64 The instructions for running this test are in the SensorFusion.pdf file in 65 the same directory as this test. 66 67 The command-line argument "replay" may be optionally provided. Without this 68 argument, the test will collect a new set of camera+gyro data from the 69 device and then analyze it (and it will also dump this data to files in the 70 current directory). If the "replay" argument is provided, then the script 71 will instead load the dumped data from a previous run and analyze that 72 instead. This can be helpful for developers who are digging for additional 73 information on their measurements. 74 """ 75 76 # Collect or load the camera+gyro data. All gyro events as well as camera 77 # timestamps are in the "events" dictionary, and "frames" is a list of 78 # RGB images as numpy arrays. 79 if "replay" not in sys.argv: 80 events, frames = collect_data() 81 else: 82 events, frames = load_data() 83 84 # Sanity check camera timestamps are enclosed by sensor timestamps 85 # This will catch bugs where camera and gyro timestamps go completely out 86 # of sync 87 cam_times = get_cam_times(events["cam"]) 88 min_cam_time = min(cam_times) * NSEC_TO_SEC 89 max_cam_time = max(cam_times) * NSEC_TO_SEC 90 gyro_times = [e["time"] for e in events["gyro"]] 91 min_gyro_time = min(gyro_times) * NSEC_TO_SEC 92 max_gyro_time = max(gyro_times) * NSEC_TO_SEC 93 if not (min_cam_time > min_gyro_time and max_cam_time < max_gyro_time): 94 print "Test failed: camera timestamps [%f,%f] " \ 95 "are not enclosed by gyro timestamps [%f, %f]" % ( 96 min_cam_time, max_cam_time, min_gyro_time, max_gyro_time) 97 assert(0) 98 99 # Compute the camera rotation displacements (rad) between each pair of 100 # adjacent frames. 101 cam_rots = get_cam_rotations(frames) 102 if max(abs(cam_rots)) < THRESH_MIN_ROT: 103 print "Device wasn't moved enough" 104 assert(0) 105 106 # Find the best offset (time-shift) to align the gyro and camera motion 107 # traces; this function integrates the shifted gyro data between camera 108 # samples for a range of candidate shift values, and returns the shift that 109 # result in the best correlation. 110 offset = get_best_alignment_offset(cam_times, cam_rots, events["gyro"]) 111 112 # Plot the camera and gyro traces after applying the best shift. 113 cam_times = cam_times + offset*SEC_TO_NSEC 114 gyro_rots = get_gyro_rotations(events["gyro"], cam_times) 115 plot_rotations(cam_rots, gyro_rots) 116 117 # Pass/fail based on the offset and also the correlation distance. 118 dist = scipy.spatial.distance.correlation(cam_rots,gyro_rots) 119 print "Best correlation of %f at shift of %.2fms"%(dist, offset*SEC_TO_MSEC) 120 assert(dist < THRESH_MAX_CORR_DIST) 121 assert(abs(offset) < THRESH_MAX_SHIFT_MS*MSEC_TO_SEC) 122 123 def get_best_alignment_offset(cam_times, cam_rots, gyro_events): 124 """Find the best offset to align the camera and gyro traces. 125 126 Uses a correlation distance metric between the curves, where a smaller 127 value means that the curves are better-correlated. 128 129 Args: 130 cam_times: Array of N camera times, one for each frame. 131 cam_rots: Array of N-1 camera rotation displacements (rad). 132 gyro_events: List of gyro event objects. 133 134 Returns: 135 Offset (seconds) of the best alignment. 136 """ 137 # Measure the corr. dist. over a shift of up to +/- 100ms (1ms step size). 138 # Get the shift corresponding to the best (lowest) score. 139 candidates = range(-100,101) 140 dists = [] 141 for shift in candidates: 142 times = cam_times + shift*MSEC_TO_NSEC 143 gyro_rots = get_gyro_rotations(gyro_events, times) 144 dists.append(scipy.spatial.distance.correlation(cam_rots,gyro_rots)) 145 best_corr_dist = min(dists) 146 best_shift = candidates[dists.index(best_corr_dist)] 147 148 # Fit a curve to the corr. dist. data to measure the minima more 149 # accurately, by looking at the correlation distances within a range of 150 # +/- 20ms from the measured best score; note that this will use fewer 151 # than the full +/- 20 range for the curve fit if the measured score 152 # (which is used as the center of the fit) is within 20ms of the edge of 153 # the +/- 100ms candidate range. 154 i = len(dists)/2 + best_shift 155 candidates = candidates[i-20:i+21] 156 dists = dists[i-20:i+21] 157 a,b,c = numpy.polyfit(candidates, dists, 2) 158 exact_best_shift = -b/(2*a) 159 if abs(best_shift - exact_best_shift) > 2.0 or a <= 0 or c <= 0: 160 print "Test failed; bad fit to time-shift curve" 161 assert(0) 162 163 xfit = [x/10.0 for x in xrange(candidates[0]*10,candidates[-1]*10)] 164 yfit = [a*x*x+b*x+c for x in xfit] 165 fig = matplotlib.pyplot.figure() 166 pylab.plot(candidates, dists, 'r', label="data") 167 pylab.plot(xfit, yfit, 'b', label="fit") 168 pylab.plot([exact_best_shift+x for x in [-0.1,0,0.1]], [0,0.01,0], 'b') 169 pylab.xlabel("Relative horizontal shift between curves (ms)") 170 pylab.ylabel("Correlation distance") 171 pylab.legend() 172 matplotlib.pyplot.savefig("%s_plot_shifts.png" % (NAME)) 173 174 return exact_best_shift * MSEC_TO_SEC 175 176 def plot_rotations(cam_rots, gyro_rots): 177 """Save a plot of the camera vs. gyro rotational measurements. 178 179 Args: 180 cam_rots: Array of N-1 camera rotation measurements (rad). 181 gyro_rots: Array of N-1 gyro rotation measurements (rad). 182 """ 183 # For the plot, scale the rotations to be in degrees. 184 fig = matplotlib.pyplot.figure() 185 cam_rots = cam_rots * (360/(2*math.pi)) 186 gyro_rots = gyro_rots * (360/(2*math.pi)) 187 pylab.plot(range(len(cam_rots)), cam_rots, 'r', label="camera") 188 pylab.plot(range(len(gyro_rots)), gyro_rots, 'b', label="gyro") 189 pylab.legend() 190 pylab.xlabel("Camera frame number") 191 pylab.ylabel("Angular displacement between adjacent camera frames (deg)") 192 pylab.xlim([0, len(cam_rots)]) 193 matplotlib.pyplot.savefig("%s_plot.png" % (NAME)) 194 195 def get_gyro_rotations(gyro_events, cam_times): 196 """Get the rotation values of the gyro. 197 198 Integrates the gyro data between each camera frame to compute an angular 199 displacement. Uses simple Euler approximation to implement the 200 integration. 201 202 Args: 203 gyro_events: List of gyro event objects. 204 cam_times: Array of N camera times, one for each frame. 205 206 Returns: 207 Array of N-1 gyro rotation measurements (rad). 208 """ 209 all_times = numpy.array([e["time"] for e in gyro_events]) 210 all_rots = numpy.array([e["z"] for e in gyro_events]) 211 gyro_rots = [] 212 # Integrate the gyro data between each pair of camera frame times. 213 for icam in range(len(cam_times)-1): 214 # Get the window of gyro samples within the current pair of frames. 215 tcam0 = cam_times[icam] 216 tcam1 = cam_times[icam+1] 217 igyrowindow0 = bisect.bisect(all_times, tcam0) 218 igyrowindow1 = bisect.bisect(all_times, tcam1) 219 sgyro = 0 220 # Integrate samples within the window. 221 for igyro in range(igyrowindow0, igyrowindow1): 222 vgyro0 = all_rots[igyro] 223 vgyro1 = all_rots[igyro+1] 224 tgyro0 = all_times[igyro] 225 tgyro1 = all_times[igyro+1] 226 vgyro = 0.5 * (vgyro0 + vgyro1) 227 deltatgyro = (tgyro1 - tgyro0) * NSEC_TO_SEC 228 sgyro += vgyro * deltatgyro 229 # Handle the fractional intervals at the sides of the window. 230 for side,igyro in enumerate([igyrowindow0-1, igyrowindow1]): 231 vgyro0 = all_rots[igyro] 232 vgyro1 = all_rots[igyro+1] 233 tgyro0 = all_times[igyro] 234 tgyro1 = all_times[igyro+1] 235 vgyro = 0.5 * (vgyro0 + vgyro1) 236 deltatgyro = (tgyro1 - tgyro0) * NSEC_TO_SEC 237 if side == 0: 238 f = (tcam0 - tgyro0) / (tgyro1 - tgyro0) 239 sgyro += vgyro * deltatgyro * (1.0 - f) 240 else: 241 f = (tcam1 - tgyro0) / (tgyro1 - tgyro0) 242 sgyro += vgyro * deltatgyro * f 243 gyro_rots.append(sgyro) 244 gyro_rots = numpy.array(gyro_rots) 245 return gyro_rots 246 247 def get_cam_rotations(frames): 248 """Get the rotations of the camera between each pair of frames. 249 250 Takes N frames and returns N-1 angular displacements corresponding to the 251 rotations between adjacent pairs of frames, in radians. 252 253 Args: 254 frames: List of N images (as RGB numpy arrays). 255 256 Returns: 257 Array of N-1 camera rotation measurements (rad). 258 """ 259 gframes = [] 260 for frame in frames: 261 frame = (frame * 255.0).astype(numpy.uint8) 262 gframes.append(cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)) 263 rots = [] 264 for i in range(1,len(gframes)): 265 gframe0 = gframes[i-1] 266 gframe1 = gframes[i] 267 p0 = cv2.goodFeaturesToTrack(gframe0, mask=None, **FEATURE_PARAMS) 268 p1,st,_ = cv2.calcOpticalFlowPyrLK(gframe0, gframe1, p0, None, 269 **LK_PARAMS) 270 tform = procrustes_rotation(p0[st==1], p1[st==1]) 271 # TODO: Choose the sign for the rotation so the cam matches the gyro 272 rot = -math.atan2(tform[0, 1], tform[0, 0]) 273 rots.append(rot) 274 if i == 1: 275 # Save a debug visualization of the features that are being 276 # tracked in the first frame. 277 frame = frames[i] 278 for x,y in p0[st==1]: 279 cv2.circle(frame, (x,y), 3, (100,100,255), -1) 280 its.image.write_image(frame, "%s_features.jpg"%(NAME)) 281 return numpy.array(rots) 282 283 def get_cam_times(cam_events): 284 """Get the camera frame times. 285 286 Args: 287 cam_events: List of (start_exposure, exposure_time, readout_duration) 288 tuples, one per captured frame, with times in nanoseconds. 289 290 Returns: 291 frame_times: Array of N times, one corresponding to the "middle" of 292 the exposure of each frame. 293 """ 294 # Assign a time to each frame that assumes that the image is instantly 295 # captured in the middle of its exposure. 296 starts = numpy.array([start for start,exptime,readout in cam_events]) 297 exptimes = numpy.array([exptime for start,exptime,readout in cam_events]) 298 readouts = numpy.array([readout for start,exptime,readout in cam_events]) 299 frame_times = starts + (exptimes + readouts) / 2.0 300 return frame_times 301 302 def load_data(): 303 """Load a set of previously captured data. 304 305 Returns: 306 events: Dictionary containing all gyro events and cam timestamps. 307 frames: List of RGB images as numpy arrays. 308 """ 309 with open("%s_events.txt"%(NAME), "r") as f: 310 events = json.loads(f.read()) 311 n = len(events["cam"]) 312 frames = [] 313 for i in range(n): 314 img = Image.open("%s_frame%03d.jpg"%(NAME,i)) 315 w,h = img.size[0:2] 316 frames.append(numpy.array(img).reshape(h,w,3) / 255.0) 317 return events, frames 318 319 def collect_data(): 320 """Capture a new set of data from the device. 321 322 Captures both motion data and camera frames, while the user is moving 323 the device in a proscribed manner. 324 325 Returns: 326 events: Dictionary containing all gyro events and cam timestamps. 327 frames: List of RGB images as numpy arrays. 328 """ 329 with its.device.ItsSession() as cam: 330 print "Starting sensor event collection" 331 cam.start_sensor_events() 332 333 # Sleep a few seconds for gyro events to stabilize. 334 time.sleep(5) 335 336 # TODO: Ensure that OIS is disabled; set to DISABLE and wait some time. 337 338 # Capture the frames. 339 props = cam.get_camera_properties() 340 fmt = {"format":"yuv", "width":W, "height":H} 341 s,e,_,_,_ = cam.do_3a(get_results=True) 342 req = its.objects.manual_capture_request(s, e) 343 print "Capturing %dx%d with sens. %d, exp. time %.1fms" % ( 344 W, H, s, e*NSEC_TO_MSEC) 345 caps = cam.do_capture([req]*N, fmt) 346 347 # Get the gyro events. 348 print "Reading out sensor events" 349 gyro = cam.get_sensor_events()["gyro"] 350 351 # Combine the events into a single structure. 352 print "Dumping event data" 353 starts = [c["metadata"]["android.sensor.timestamp"] for c in caps] 354 exptimes = [c["metadata"]["android.sensor.exposureTime"] for c in caps] 355 readouts = [c["metadata"]["android.sensor.rollingShutterSkew"] 356 for c in caps] 357 events = {"gyro": gyro, "cam": zip(starts,exptimes,readouts)} 358 with open("%s_events.txt"%(NAME), "w") as f: 359 f.write(json.dumps(events)) 360 361 # Convert the frames to RGB. 362 print "Dumping frames" 363 frames = [] 364 for i,c in enumerate(caps): 365 img = its.image.convert_capture_to_rgb_image(c) 366 frames.append(img) 367 its.image.write_image(img, "%s_frame%03d.jpg"%(NAME,i)) 368 369 return events, frames 370 371 def procrustes_rotation(X, Y): 372 """ 373 Procrustes analysis determines a linear transformation (translation, 374 reflection, orthogonal rotation and scaling) of the points in Y to best 375 conform them to the points in matrix X, using the sum of squared errors 376 as the goodness of fit criterion. 377 378 Args: 379 X, Y: Matrices of target and input coordinates. 380 381 Returns: 382 The rotation component of the transformation that maps X to Y. 383 """ 384 X0 = (X-X.mean(0)) / numpy.sqrt(((X-X.mean(0))**2.0).sum()) 385 Y0 = (Y-Y.mean(0)) / numpy.sqrt(((Y-Y.mean(0))**2.0).sum()) 386 U,s,Vt = numpy.linalg.svd(numpy.dot(X0.T, Y0),full_matrices=False) 387 return numpy.dot(Vt.T, U.T) 388 389 if __name__ == '__main__': 390 main() 391 392
