1# Copyright 2018 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 math
16import os.path
17import re
18import sys
19import cv2
20
21import its.caps
22import its.device
23import its.image
24import its.objects
25
26import numpy as np
27
28ALIGN_TOL_MM = 4.0E-3  # mm
29ALIGN_TOL = 0.01  # multiplied by sensor diagonal to convert to pixels
30CHART_DISTANCE_CM = 22  # cm
31CIRCLE_RTOL = 0.1
32GYRO_REFERENCE = 1
33NAME = os.path.basename(__file__).split('.')[0]
34TRANS_REF_MATRIX = np.array([0, 0, 0])
35
36
37def convert_to_world_coordinates(x, y, r, t, k, z_w):
38    """Convert x,y coordinates to world coordinates.
39
40    Conversion equation is:
41    A = [[x*r[2][0] - dot(k_row0, r_col0), x*r_[2][1] - dot(k_row0, r_col1)],
42         [y*r[2][0] - dot(k_row1, r_col0), y*r_[2][1] - dot(k_row1, r_col1)]]
43    b = [[z_w*dot(k_row0, r_col2) + dot(k_row0, t) - x*(r[2][2]*z_w + t[2])],
44         [z_w*dot(k_row1, r_col2) + dot(k_row1, t) - y*(r[2][2]*z_w + t[2])]]
45
46    [[x_w], [y_w]] = inv(A) * b
47
48    Args:
49        x:      x location in pixel space
50        y:      y location in pixel space
51        r:      rotation matrix
52        t:      translation matrix
53        k:      intrinsic matrix
54        z_w:    z distance in world space
55
56    Returns:
57        x_w:    x in meters in world space
58        y_w:    y in meters in world space
59    """
60    c_1 = r[2, 2] * z_w + t[2]
61    k_x1 = np.dot(k[0, :], r[:, 0])
62    k_x2 = np.dot(k[0, :], r[:, 1])
63    k_x3 = z_w * np.dot(k[0, :], r[:, 2]) + np.dot(k[0, :], t)
64    k_y1 = np.dot(k[1, :], r[:, 0])
65    k_y2 = np.dot(k[1, :], r[:, 1])
66    k_y3 = z_w * np.dot(k[1, :], r[:, 2]) + np.dot(k[1, :], t)
67
68    a = np.array([[x*r[2][0]-k_x1, x*r[2][1]-k_x2],
69                  [y*r[2][0]-k_y1, y*r[2][1]-k_y2]])
70    b = np.array([[k_x3-x*c_1], [k_y3-y*c_1]])
71    return np.dot(np.linalg.inv(a), b)
72
73
74def convert_to_image_coordinates(p_w, r, t, k):
75    p_c = np.dot(r, p_w) + t
76    p_h = np.dot(k, p_c)
77    return p_h[0] / p_h[2], p_h[1] / p_h[2]
78
79
80def rotation_matrix(rotation):
81    """Convert the rotation parameters to 3-axis data.
82
83    Args:
84        rotation:   android.lens.Rotation vector
85    Returns:
86        3x3 matrix w/ rotation parameters
87    """
88    x = rotation[0]
89    y = rotation[1]
90    z = rotation[2]
91    w = rotation[3]
92    return np.array([[1-2*y**2-2*z**2, 2*x*y-2*z*w, 2*x*z+2*y*w],
93                     [2*x*y+2*z*w, 1-2*x**2-2*z**2, 2*y*z-2*x*w],
94                     [2*x*z-2*y*w, 2*y*z+2*x*w, 1-2*x**2-2*y**2]])
95
96
97# TODO: merge find_circle() & test_aspect_ratio_and_crop.measure_aspect_ratio()
98# for a unified circle script that is and in pymodules/image.py
99def find_circle(gray, name):
100    """Find the black circle in the image.
101
102    Args:
103        gray:           numpy grayscale array with pixel values in [0,255].
104        name:           string of file name.
105    Returns:
106        circle:         (circle_center_x, circle_center_y, radius)
107    """
108    size = gray.shape
109    # otsu threshold to binarize the image
110    _, img_bw = cv2.threshold(np.uint8(gray), 0, 255,
111                              cv2.THRESH_BINARY + cv2.THRESH_OTSU)
112
113    # connected component
114    cv2_version = cv2.__version__
115    if cv2_version.startswith('2.4.'):
116        contours, hierarchy = cv2.findContours(255-img_bw, cv2.RETR_TREE,
117                                               cv2.CHAIN_APPROX_SIMPLE)
118    elif cv2_version.startswith('3.2.'):
119        _, contours, hierarchy = cv2.findContours(255-img_bw, cv2.RETR_TREE,
120                                                  cv2.CHAIN_APPROX_SIMPLE)
121
122    # Check each component and find the black circle
123    min_cmpt = size[0] * size[1] * 0.005
124    max_cmpt = size[0] * size[1] * 0.35
125    num_circle = 0
126    for ct, hrch in zip(contours, hierarchy[0]):
127        # The radius of the circle is 1/3 of the length of the square, meaning
128        # around 1/3 of the area of the square
129        # Parental component should exist and the area is acceptable.
130        # The contour of a circle should have at least 5 points
131        child_area = cv2.contourArea(ct)
132        if (hrch[3] == -1 or child_area < min_cmpt or child_area > max_cmpt
133                    or len(ct) < 15):
134            continue
135        # Check the shapes of current component and its parent
136        child_shape = component_shape(ct)
137        parent = hrch[3]
138        prt_shape = component_shape(contours[parent])
139        prt_area = cv2.contourArea(contours[parent])
140        dist_x = abs(child_shape['ctx']-prt_shape['ctx'])
141        dist_y = abs(child_shape['cty']-prt_shape['cty'])
142        # 1. 0.56*Parent's width < Child's width < 0.76*Parent's width.
143        # 2. 0.56*Parent's height < Child's height < 0.76*Parent's height.
144        # 3. Child's width > 0.1*Image width
145        # 4. Child's height > 0.1*Image height
146        # 5. 0.25*Parent's area < Child's area < 0.45*Parent's area
147        # 6. Child == 0, and Parent == 255
148        # 7. Center of Child and center of parent should overlap
149        if (prt_shape['width'] * 0.56 < child_shape['width']
150                    < prt_shape['width'] * 0.76
151                    and prt_shape['height'] * 0.56 < child_shape['height']
152                    < prt_shape['height'] * 0.76
153                    and child_shape['width'] > 0.1 * size[1]
154                    and child_shape['height'] > 0.1 * size[0]
155                    and 0.30 * prt_area < child_area < 0.50 * prt_area
156                    and img_bw[child_shape['cty']][child_shape['ctx']] == 0
157                    and img_bw[child_shape['top']][child_shape['left']] == 255
158                    and dist_x < 0.1 * child_shape['width']
159                    and dist_y < 0.1 * child_shape['height']):
160            # Calculate circle center and size
161            circle_ctx = float(child_shape['ctx'])
162            circle_cty = float(child_shape['cty'])
163            circle_w = float(child_shape['width'])
164            circle_h = float(child_shape['height'])
165            num_circle += 1
166            # If more than one circle found, break
167            if num_circle == 2:
168                break
169    its.image.write_image(gray[..., np.newaxis]/255.0, name)
170
171    if num_circle == 0:
172        print 'No black circle was detected. Please take pictures according',
173        print 'to instruction carefully!\n'
174        assert num_circle == 1
175
176    if num_circle > 1:
177        print 'More than one black circle was detected. Background of scene',
178        print 'may be too complex.\n'
179        assert num_circle == 1
180    return [circle_ctx, circle_cty, (circle_w+circle_h)/4.0]
181
182
183def component_shape(contour):
184    """Measure the shape of a connected component.
185
186    Args:
187        contour: return from cv2.findContours. A list of pixel coordinates of
188        the contour.
189
190    Returns:
191        The most left, right, top, bottom pixel location, height, width, and
192        the center pixel location of the contour.
193    """
194    shape = {'left': np.inf, 'right': 0, 'top': np.inf, 'bottom': 0,
195             'width': 0, 'height': 0, 'ctx': 0, 'cty': 0}
196    for pt in contour:
197        if pt[0][0] < shape['left']:
198            shape['left'] = pt[0][0]
199        if pt[0][0] > shape['right']:
200            shape['right'] = pt[0][0]
201        if pt[0][1] < shape['top']:
202            shape['top'] = pt[0][1]
203        if pt[0][1] > shape['bottom']:
204            shape['bottom'] = pt[0][1]
205    shape['width'] = shape['right'] - shape['left'] + 1
206    shape['height'] = shape['bottom'] - shape['top'] + 1
207    shape['ctx'] = (shape['left']+shape['right'])/2
208    shape['cty'] = (shape['top']+shape['bottom'])/2
209    return shape
210
211
212def define_reference_camera(pose_reference, cam_reference):
213    """Determine the reference camera.
214
215    Args:
216        pose_reference: 0 for cameras, 1 for gyro
217        cam_reference:  dict with key of physical camera and value True/False
218    Returns:
219        i_ref:          physical id of reference camera
220        i_2nd:          physical id of secondary camera
221    """
222
223    if pose_reference == GYRO_REFERENCE:
224        print 'pose_reference is GYRO'
225        i_ref = list(cam_reference.keys())[0]  # pick first camera as ref
226        i_2nd = list(cam_reference.keys())[1]
227    else:
228        print 'pose_reference is CAMERA'
229        i_ref = (k for (k, v) in cam_reference.iteritems() if v).next()
230        i_2nd = (k for (k, v) in cam_reference.iteritems() if not v).next()
231    return i_ref, i_2nd
232
233
234def main():
235    """Test the multi camera system parameters related to camera spacing.
236
237    Using the multi-camera physical cameras, take a picture of scene4
238    (a black circle and surrounding square on a white background) with
239    one of the physical cameras. Then find the circle center. Using the
240    parameters:
241        android.lens.poseReference
242        android.lens.poseTranslation
243        android.lens.poseRotation
244        android.lens.instrinsicCalibration
245        android.lens.distortion (if available)
246    project the circle center to the world coordinates for each camera.
247    Compare the difference between the two cameras' circle centers in
248    world coordinates.
249
250    Reproject the world coordinates back to pixel coordinates and compare
251    against originals as a validity check.
252
253    Compare the circle sizes if the focal lengths of the cameras are
254    different using
255        android.lens.availableFocalLengths.
256    """
257    chart_distance = CHART_DISTANCE_CM
258    for s in sys.argv[1:]:
259        if s[:5] == 'dist=' and len(s) > 5:
260            chart_distance = float(re.sub('cm', '', s[5:]))
261            print 'Using chart distance: %.1fcm' % chart_distance
262    chart_distance *= 1.0E-2
263
264    with its.device.ItsSession() as cam:
265        props = cam.get_camera_properties()
266        its.caps.skip_unless(its.caps.read_3a(props) and
267                             its.caps.per_frame_control(props) and
268                             its.caps.logical_multi_camera(props))
269
270        # Find physical camera IDs and those that support RGB raw
271        ids = its.caps.logical_multi_camera_physical_ids(props)
272        props_physical = {}
273        physical_ids = []
274        physical_raw_ids = []
275        for i in ids:
276            # Find YUV capable physical cameras
277            prop = cam.get_camera_properties_by_id(i)
278            physical_ids.append(i)
279            props_physical[i] = cam.get_camera_properties_by_id(i)
280            # Find first 2 RAW+RGB capable physical cameras
281            if (its.caps.raw16(prop) and not its.caps.mono_camera(props)
282                        and len(physical_raw_ids) < 2):
283                physical_raw_ids.append(i)
284
285        debug = its.caps.debug_mode()
286        avail_fls = props['android.lens.info.availableFocalLengths']
287        pose_reference = props['android.lens.poseReference']
288
289        # Find highest resolution image and determine formats
290        fmts = ['yuv']
291        its.caps.skip_unless(len(physical_ids) >= 2)
292        if len(physical_raw_ids) == 2:
293            fmts.insert(0, 'raw')  # insert in first location in list
294        else:
295            physical_ids = ids[0:2]
296
297        w, h = its.objects.get_available_output_sizes('yuv', props)[0]
298
299        # do captures on 2 cameras
300        caps = {}
301        for i, fmt in enumerate(fmts):
302            physicalSizes = {}
303
304            capture_cam_ids = physical_ids
305            if fmt == 'raw':
306                capture_cam_ids = physical_raw_ids
307
308            for physical_id in capture_cam_ids:
309                configs = props_physical[physical_id]['android.scaler.streamConfigurationMap']\
310                                   ['availableStreamConfigurations']
311                if fmt == 'raw':
312                    fmt_codes = 0x20
313                    fmt_configs = [cfg for cfg in configs if cfg['format'] == fmt_codes]
314                else:
315                    fmt_codes = 0x23
316                    fmt_configs = [cfg for cfg in configs if cfg['format'] == fmt_codes]
317
318                out_configs = [cfg for cfg in fmt_configs if cfg['input'] is False]
319                out_sizes = [(cfg['width'], cfg['height']) for cfg in out_configs]
320                physicalSizes[physical_id] = max(out_sizes, key=lambda item: item[1])
321
322            out_surfaces = [{'format': 'yuv', 'width': w, 'height': h},
323                            {'format': fmt, 'physicalCamera': capture_cam_ids[0],
324                             'width': physicalSizes[capture_cam_ids[0]][0],
325                             'height': physicalSizes[capture_cam_ids[0]][1]},
326                            {'format': fmt, 'physicalCamera': capture_cam_ids[1],
327                             'width': physicalSizes[capture_cam_ids[1]][0],
328                             'height': physicalSizes[capture_cam_ids[1]][1]},]
329
330            out_surfaces_supported = cam.is_stream_combination_supported(out_surfaces)
331            its.caps.skip_unless(out_surfaces_supported)
332
333            # Do 3A and get the values
334            s, e, _, _, fd = cam.do_3a(get_results=True,
335                                       lock_ae=True, lock_awb=True)
336            if fmt == 'raw':
337                e_corrected = e * 2  # brighten RAW images
338            else:
339                e_corrected = e
340            print 'out_surfaces:', out_surfaces
341            req = its.objects.manual_capture_request(s, e_corrected, fd)
342            _, caps[(fmt, capture_cam_ids[0])], caps[(fmt, capture_cam_ids[1])] = cam.do_capture(
343                    req, out_surfaces)
344
345    for j, fmt in enumerate(fmts):
346        size = {}
347        k = {}
348        cam_reference = {}
349        r = {}
350        t = {}
351        circle = {}
352        fl = {}
353        sensor_diag = {}
354        capture_cam_ids = physical_ids
355        if fmt == 'raw':
356            capture_cam_ids = physical_raw_ids
357        print '\nFormat:', fmt
358        for i in capture_cam_ids:
359            # process image
360            img = its.image.convert_capture_to_rgb_image(
361                    caps[(fmt, i)], props=props_physical[i])
362            size[i] = (caps[fmt, i]['width'], caps[fmt, i]['height'])
363
364            # save images if debug
365            if debug:
366                its.image.write_image(img, '%s_%s_%s.jpg' % (NAME, fmt, i))
367
368            # convert to [0, 255] images
369            img *= 255
370
371            # scale to match calibration data if RAW
372            if fmt == 'raw':
373                img = cv2.resize(img.astype(np.uint8), None, fx=2, fy=2)
374            else:
375                img = img.astype(np.uint8)
376
377            # load parameters for each physical camera
378            ical = props_physical[i]['android.lens.intrinsicCalibration']
379            assert len(ical) == 5, 'android.lens.instrisicCalibration incorrect.'
380            k[i] = np.array([[ical[0], ical[4], ical[2]],
381                             [0, ical[1], ical[3]],
382                             [0, 0, 1]])
383            if j == 0:
384                print 'Camera %s' % i
385                print ' k:', k[i]
386
387            rotation = np.array(props_physical[i]['android.lens.poseRotation'])
388            if j == 0:
389                print ' rotation:', rotation
390            assert len(rotation) == 4, 'poseRotation has wrong # of params.'
391            r[i] = rotation_matrix(rotation)
392
393            t[i] = np.array(props_physical[i]['android.lens.poseTranslation'])
394            if j == 0:
395                print ' translation:', t[i]
396            assert len(t[i]) == 3, 'poseTranslation has wrong # of params.'
397            if (t[i] == TRANS_REF_MATRIX).all():
398                cam_reference[i] = True
399            else:
400                cam_reference[i] = False
401
402            # API spec defines poseTranslation as the world coordinate p_w_cam of
403            # optics center. When applying [R|t] to go from world coordinates to
404            # camera coordinates, we need -R*p_w_cam of the coordinate reported in
405            # metadata.
406            # ie. for a camera with optical center at world coordinate (5, 4, 3)
407            # and identity rotation, to convert a world coordinate into the
408            # camera's coordinate, we need a translation vector of [-5, -4, -3]
409            # so that: [I|[-5, -4, -3]^T] * [5, 4, 3]^T = [0,0,0]^T
410            t[i] = -1.0 * np.dot(r[i], t[i])
411            if debug and j == 1:
412                print 't:', t[i]
413                print 'r:', r[i]
414
415            # Correct lens distortion to image (if available)
416            if its.caps.distortion_correction(props_physical[i]) and fmt == 'raw':
417                distort = np.array(props_physical[i]['android.lens.distortion'])
418                assert len(distort) == 5, 'distortion has wrong # of params.'
419                cv2_distort = np.array([distort[0], distort[1],
420                                        distort[3], distort[4],
421                                        distort[2]])
422                print ' cv2 distortion params:', cv2_distort
423                its.image.write_image(img/255.0, '%s_%s_%s.jpg' % (
424                        NAME, fmt, i))
425                img = cv2.undistort(img, k[i], cv2_distort)
426                its.image.write_image(img/255.0, '%s_%s_correct_%s.jpg' % (
427                        NAME, fmt, i))
428
429            # Find the circles in grayscale image
430            circle[i] = find_circle(cv2.cvtColor(img, cv2.COLOR_BGR2GRAY),
431                                    '%s_%s_gray_%s.jpg' % (NAME, fmt, i))
432            print 'Circle radius ', i, ': ', circle[i][2]
433
434            # Undo zoom to image (if applicable). Assume that the maximum
435            # physical YUV image size is close to active array size.
436            if fmt == 'yuv':
437                ar = props_physical[i]['android.sensor.info.activeArraySize']
438                arw = ar['right'] - ar['left']
439                arh = ar['bottom'] - ar['top']
440                cr = caps[(fmt, i)]['metadata']['android.scaler.cropRegion']
441                crw = cr['right'] - cr['left']
442                crh = cr['bottom'] - cr['top']
443                # Assume pixels remain square after zoom, so use same zoom
444                # ratios for x and y.
445                zoom_ratio = min(1.0 * arw / crw, 1.0 * arh / crh)
446                circle[i][0] = cr['left'] + circle[i][0] / zoom_ratio
447                circle[i][1] = cr['top'] + circle[i][1] / zoom_ratio
448                circle[i][2] = circle[i][2] / zoom_ratio
449
450            # Find focal length & sensor size
451            fl[i] = props_physical[i]['android.lens.info.availableFocalLengths'][0]
452            sensor_diag[i] = math.sqrt(size[i][0] ** 2 + size[i][1] ** 2)
453
454        i_ref, i_2nd = define_reference_camera(pose_reference, cam_reference)
455        print 'reference camera: %s, secondary camera: %s' % (i_ref, i_2nd)
456
457        # Convert circle centers to real world coordinates
458        x_w = {}
459        y_w = {}
460        if props['android.lens.facing']:
461            # API spec defines +z is pointing out from screen
462            print 'lens facing BACK'
463            chart_distance *= -1
464        for i in [i_ref, i_2nd]:
465            x_w[i], y_w[i] = convert_to_world_coordinates(
466                    circle[i][0], circle[i][1], r[i], t[i], k[i], chart_distance)
467
468        # Back convert to image coordinates for round-trip check
469        x_p = {}
470        y_p = {}
471        x_p[i_2nd], y_p[i_2nd] = convert_to_image_coordinates(
472                [x_w[i_ref], y_w[i_ref], chart_distance],
473                r[i_2nd], t[i_2nd], k[i_2nd])
474        x_p[i_ref], y_p[i_ref] = convert_to_image_coordinates(
475                [x_w[i_2nd], y_w[i_2nd], chart_distance],
476                r[i_ref], t[i_ref], k[i_ref])
477
478        # Summarize results
479        for i in [i_ref, i_2nd]:
480            print ' Camera: %s' % i
481            print ' x, y (pixels): %.1f, %.1f' % (circle[i][0], circle[i][1])
482            print ' x_w, y_w (mm): %.2f, %.2f' % (x_w[i]*1.0E3, y_w[i]*1.0E3)
483            print ' x_p, y_p (pixels): %.1f, %.1f' % (x_p[i], y_p[i])
484
485        # Check center locations
486        err = np.linalg.norm(np.array([x_w[i_ref], y_w[i_ref]]) -
487                             np.array([x_w[i_2nd], y_w[i_2nd]]))
488        print 'Center location err (mm): %.2f' % (err*1E3)
489        msg = 'Center locations %s <-> %s too different!' % (i_ref, i_2nd)
490        msg += ' val=%.2fmm, THRESH=%.fmm' % (err*1E3, ALIGN_TOL_MM*1E3)
491        assert err < ALIGN_TOL, msg
492
493        # Check projections back into pixel space
494        for i in [i_ref, i_2nd]:
495            err = np.linalg.norm(np.array([circle[i][0], circle[i][1]]) -
496                                 np.array([x_p[i], y_p[i]]))
497            print 'Camera %s projection error (pixels): %.1f' % (i, err)
498            tol = ALIGN_TOL * sensor_diag[i]
499            msg = 'Camera %s project locations too different!' % i
500            msg += ' diff=%.2f, TOL=%.2f' % (err, tol)
501            assert err < tol, msg
502
503        # Check focal length and circle size if more than 1 focal length
504        if len(avail_fls) > 1:
505            print 'Circle radii (pixels); ref: %.1f, 2nd: %.1f' % (
506                    circle[i_ref][2], circle[i_2nd][2])
507            print 'Focal lengths (diopters); ref: %.2f, 2nd: %.2f' % (
508                    fl[i_ref], fl[i_2nd])
509            print 'Sensor diagonals (pixels); ref: %.2f, 2nd: %.2f' % (
510                    sensor_diag[i_ref], sensor_diag[i_2nd])
511            msg = 'Circle size scales improperly! RTOL=%.1f' % CIRCLE_RTOL
512            msg += '\nMetric: radius/focal_length*sensor_diag should be equal.'
513            assert np.isclose(circle[i_ref][2]/fl[i_ref]*sensor_diag[i_ref],
514                              circle[i_2nd][2]/fl[i_2nd]*sensor_diag[i_2nd],
515                              rtol=CIRCLE_RTOL), msg
516
517if __name__ == '__main__':
518    main()
519