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 time
19import math
20import pylab
21import os.path
22import matplotlib
23import matplotlib.pyplot
24import json
25import Image
26import numpy
27import cv2
28import bisect
29import scipy.spatial
30import sys
31
32NAME = os.path.basename(__file__).split(".")[0]
33
34# Capture 210 QVGA frames (which is 7s at 30fps)
35N = 210
36W,H = 320,240
37
38FEATURE_PARAMS = dict( maxCorners = 50,
39                       qualityLevel = 0.3,
40                       minDistance = 7,
41                       blockSize = 7 )
42
43LK_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).
49SEC_TO_NSEC = 1000*1000*1000.0
50SEC_TO_MSEC = 1000.0
51MSEC_TO_NSEC = 1000*1000.0
52MSEC_TO_SEC = 1/1000.0
53NSEC_TO_SEC = 1/(1000*1000*1000.0)
54NSEC_TO_MSEC = 1/(1000*1000.0)
55
56# Pass/fail thresholds.
57THRESH_MAX_CORR_DIST = 0.005
58THRESH_MAX_SHIFT_MS = 2
59THRESH_MIN_ROT = 0.001
60
61def 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
123def 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
176def 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
195def 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
247def 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
283def 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
302def 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
319def 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
371def 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
389if __name__ == '__main__':
390    main()
391
392