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 bisect
16import json
17import math
18import os.path
19import sys
20import time
21
22import cv2
23import its.caps
24import its.device
25import its.image
26import its.objects
27import matplotlib
28from matplotlib import pylab
29import matplotlib.pyplot
30import numpy
31from PIL import Image
32import scipy.spatial
33
34NAME = os.path.basename(__file__).split(".")[0]
35
36W, H = 640, 480
37FPS = 30
38TEST_LENGTH = 7  # seconds
39FEATURE_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
43MIN_FEATURE_PTS = 30          # Minimum number of feature points required to
44                              # perform rotation analysis
45
46MAX_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
50MIN_GYRO_SMP_RATE = 100.0     # Minimum gyro sample rate
51
52FEATURE_PARAMS = dict(maxCorners=240,
53                      qualityLevel=0.3,
54                      minDistance=7,
55                      blockSize=7)
56
57LK_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).
63SEC_TO_NSEC = 1000*1000*1000.0
64SEC_TO_MSEC = 1000.0
65MSEC_TO_NSEC = 1000*1000.0
66MSEC_TO_SEC = 1/1000.0
67NSEC_TO_SEC = 1/(1000*1000*1000.0)
68NSEC_TO_MSEC = 1/(1000*1000.0)
69CM_TO_M = 1/100.0
70
71# PASS/FAIL thresholds.
72THRESH_MAX_CORR_DIST = 0.005
73THRESH_MAX_SHIFT_MS = 1
74THRESH_MIN_ROT = 0.001
75
76# lens facing
77FACING_FRONT = 0
78FACING_BACK = 1
79FACING_EXTERNAL = 2
80
81# Chart distance
82CHART_DISTANCE = 25  # cm
83
84
85def 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
188def 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
246def 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
267def 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
315def 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
373def 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
393def 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
413def 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
488def 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
509if __name__ == "__main__":
510    main()
511