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    # Check that 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 %s" % str(i-1).zfill(3)
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 %s is %d" % (
352                    str(i-1).zfill(3), num_features)
353        p1, st, _ = cv2.calcOpticalFlowPyrLK(gframe0, gframe1, p0_filtered,
354                                             None, **LK_PARAMS)
355        tform = procrustes_rotation(p0_filtered[st == 1], p1[st == 1])
356        if facing == FACING_BACK:
357            rot = -math.atan2(tform[0, 1], tform[0, 0])
358        elif facing == FACING_FRONT:
359            rot = math.atan2(tform[0, 1], tform[0, 0])
360        else:
361            print "Unknown lens facing", facing
362            assert 0
363        rots.append(rot)
364        if i == 1:
365            # Save a debug visualization of the features that are being
366            # tracked in the first frame.
367            frame = frames[i]
368            for x, y in p0_filtered[st == 1]:
369                cv2.circle(frame, (x, y), 3, (100, 100, 255), -1)
370            its.image.write_image(frame, "%s_features.png" % NAME)
371    return numpy.array(rots)
372
373
374def get_cam_times(cam_events):
375    """Get the camera frame times.
376
377    Args:
378        cam_events: List of (start_exposure, exposure_time, readout_duration)
379            tuples, one per captured frame, with times in nanoseconds.
380
381    Returns:
382        frame_times: Array of N times, one corresponding to the "middle" of
383            the exposure of each frame.
384    """
385    # Assign a time to each frame that assumes that the image is instantly
386    # captured in the middle of its exposure.
387    starts = numpy.array([start for start, exptime, readout in cam_events])
388    exptimes = numpy.array([exptime for start, exptime, readout in cam_events])
389    readouts = numpy.array([readout for start, exptime, readout in cam_events])
390    frame_times = starts + (exptimes + readouts) / 2.0
391    return frame_times
392
393
394def load_data():
395    """Load a set of previously captured data.
396
397    Returns:
398        events: Dictionary containing all gyro events and cam timestamps.
399        frames: List of RGB images as numpy arrays.
400        w:      Pixel width of frames
401        h:      Pixel height of frames
402    """
403    with open("%s_events.txt" % NAME, "r") as f:
404        events = json.loads(f.read())
405    n = len(events["cam"])
406    frames = []
407    for i in range(n):
408        img = Image.open("%s_frame%03d.png" % (NAME, i))
409        w, h = img.size[0:2]
410        frames.append(numpy.array(img).reshape(h, w, 3) / 255.0)
411    return events, frames, w, h
412
413
414def collect_data(fps, w, h, test_length):
415    """Capture a new set of data from the device.
416
417    Captures both motion data and camera frames, while the user is moving
418    the device in a proscribed manner.
419
420    Args:
421        fps:         FPS to capture with
422        w:           Pixel width of frames
423        h:           Pixel height of frames
424        test_length: How long the test should run for (in seconds)
425
426    Returns:
427        events: Dictionary containing all gyro events and cam timestamps.
428        frames: List of RGB images as numpy arrays.
429    """
430    with its.device.ItsSession() as cam:
431        props = cam.get_camera_properties()
432        props = cam.override_with_hidden_physical_camera_props(props)
433        its.caps.skip_unless(its.caps.read_3a and
434                             its.caps.sensor_fusion(props) and
435                             props["android.lens.facing"] != FACING_EXTERNAL and
436                             cam.get_sensors().get("gyro"))
437
438        print "Starting sensor event collection"
439        cam.start_sensor_events()
440
441        # Sleep a while for gyro events to stabilize.
442        time.sleep(0.5)
443
444        # Capture the frames. OIS is disabled for manual captures.
445        facing = props["android.lens.facing"]
446        if facing != FACING_FRONT and facing != FACING_BACK:
447            print "Unknown lens facing", facing
448            assert 0
449
450        fmt = {"format": "yuv", "width": w, "height": h}
451        s, e, _, _, _ = cam.do_3a(get_results=True, do_af=False)
452        req = its.objects.manual_capture_request(s, e)
453        its.objects.turn_slow_filters_off(props, req)
454        fd_min = props["android.lens.info.minimumFocusDistance"]
455        fd_chart = 1 / (CHART_DISTANCE * CM_TO_M)
456        if fd_min < fd_chart:
457            req["android.lens.focusDistance"] = fd_min
458        else:
459            req["android.lens.focusDistance"] = fd_chart
460        req["android.control.aeTargetFpsRange"] = [fps, fps]
461        req["android.sensor.frameDuration"] = int(1000.0/fps * MSEC_TO_NSEC)
462        print "Capturing %dx%d with sens. %d, exp. time %.1fms at %dfps" % (
463                w, h, s, e*NSEC_TO_MSEC, fps)
464        caps = cam.do_capture([req]*int(fps*test_length), fmt)
465
466        # Capture a bit more gyro samples for use in
467        # get_best_alignment_offset
468        time.sleep(0.2)
469
470        # Get the gyro events.
471        print "Reading out sensor events"
472        gyro = cam.get_sensor_events()["gyro"]
473        print "Number of gyro samples", len(gyro)
474
475        # Combine the events into a single structure.
476        print "Dumping event data"
477        starts = [c["metadata"]["android.sensor.timestamp"] for c in caps]
478        exptimes = [c["metadata"]["android.sensor.exposureTime"] for c in caps]
479        readouts = [c["metadata"]["android.sensor.rollingShutterSkew"]
480                    for c in caps]
481        events = {"gyro": gyro, "cam": zip(starts, exptimes, readouts),
482                  "facing": facing}
483        with open("%s_events.txt" % NAME, "w") as f:
484            f.write(json.dumps(events))
485
486        # Convert the frames to RGB.
487        print "Dumping frames"
488        frames = []
489        for i, c in enumerate(caps):
490            img = its.image.convert_capture_to_rgb_image(c)
491            frames.append(img)
492            its.image.write_image(img, "%s_frame%03d.png" % (NAME, i))
493
494        return events, frames
495
496
497def procrustes_rotation(X, Y):
498    """Performs a Procrustes analysis to conform points in X to Y.
499
500    Procrustes analysis determines a linear transformation (translation,
501    reflection, orthogonal rotation and scaling) of the points in Y to best
502    conform them to the points in matrix X, using the sum of squared errors
503    as the goodness of fit criterion.
504
505    Args:
506        X: Target coordinate matrix
507        Y: Input coordinate matrix
508
509    Returns:
510        The rotation component of the transformation that maps X to Y.
511    """
512    X0 = (X-X.mean(0)) / numpy.sqrt(((X-X.mean(0))**2.0).sum())
513    Y0 = (Y-Y.mean(0)) / numpy.sqrt(((Y-Y.mean(0))**2.0).sum())
514    U, _, Vt = numpy.linalg.svd(numpy.dot(X0.T, Y0), full_matrices=False)
515    return numpy.dot(Vt.T, U.T)
516
517
518if __name__ == "__main__":
519    main()
520