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