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_PERCENT = 1
29CHART_DISTANCE_CM = 22  # cm
30CIRCLE_TOL_PERCENT = 10
31NAME = os.path.basename(__file__).split('.')[0]
32ROTATE_REF_MATRIX = np.array([0, 0, 0, 1])
33TRANS_REF_MATRIX = np.array([0, 0, 0])
34
35
36def rotation_matrix(rotation):
37    """Convert the rotation parameters to 3-axis data.
38
39    Args:
40        rotation:   android.lens.Rotation vector
41    Returns:
42        3x3 matrix w/ rotation parameters
43    """
44    x = rotation[0]
45    y = rotation[1]
46    z = rotation[2]
47    w = rotation[3]
48    return np.array([[1-2*y**2-2*z**2, 2*x*y-2*z*w, 2*x*z+2*y*w],
49                     [2*x*y+2*z*w, 1-2*x**2-2*z**2, 2*y*z-2*x*w],
50                     [2*x*z-2*y*w, 2*y*z+2*x*w, 1-2*x**2-2*y**2]])
51
52
53def find_circle(gray, name):
54    """Find the circle in the image.
55
56    Args:
57        gray:           gray scale image array [0,255]
58        name:           string of file name
59    Returns:
60        circle:         (circle_center_x, circle_center_y, radius)
61    """
62
63    cv2_version = cv2.__version__
64    try:
65        if cv2_version.startswith('2.4.'):
66            circle = cv2.HoughCircles(gray, cv2.cv.CV_HOUGH_GRADIENT,
67                                      1, 20)[0][0]
68        elif cv2_version.startswith('3.2.'):
69            circle = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT,
70                                      1, 20)[0][0]
71    except TypeError:
72        circle = None
73        its.image.write_image(gray[..., np.newaxis]/255.0, name)
74    assert circle is not None, 'No circle found!'
75    return circle
76
77
78def main():
79    """Test the multi camera system parameters related to camera spacing."""
80    chart_distance = CHART_DISTANCE_CM
81    for s in sys.argv[1:]:
82        if s[:5] == 'dist=' and len(s) > 5:
83            chart_distance = float(re.sub('cm', '', s[5:]))
84            print 'Using chart distance: %.1fcm' % chart_distance
85
86    with its.device.ItsSession() as cam:
87        props = cam.get_camera_properties()
88        its.caps.skip_unless(its.caps.compute_target_exposure(props) and
89                             its.caps.per_frame_control(props) and
90                             its.caps.logical_multi_camera(props) and
91                             its.caps.raw16(props) and
92                             its.caps.manual_sensor(props))
93        debug = its.caps.debug_mode()
94        avail_fls = props['android.lens.info.availableFocalLengths']
95
96        max_raw_size = its.objects.get_available_output_sizes('raw', props)[0]
97        w, h = its.objects.get_available_output_sizes(
98                'yuv', props, match_ar_size=max_raw_size)[0]
99
100        # Do 3A and get the values
101        s, e, _, _, fd = cam.do_3a(get_results=True,
102                                   lock_ae=True, lock_awb=True)
103        req = its.objects.manual_capture_request(s, e, fd, True, props)
104
105        # get physical camera properties
106        ids = its.caps.logical_multi_camera_physical_ids(props)
107        props_physical = {}
108        for i in ids:
109            props_physical[i] = cam.get_camera_properties_by_id(i)
110
111        # capture RAWs of 1st 2 cameras
112        cap_raw = {}
113        out_surfaces = [{'format': 'yuv', 'width': w, 'height': h},
114                        {'format': 'raw', 'physicalCamera': ids[0]},
115                        {'format': 'raw', 'physicalCamera': ids[1]}]
116        _, cap_raw[ids[0]], cap_raw[ids[1]] = cam.do_capture(req, out_surfaces)
117
118    size_raw = {}
119    k = {}
120    reference = {}
121    rotation = {}
122    trans = {}
123    circle = {}
124    fl = {}
125    sensor_diag = {}
126    point = {}
127    for i in ids:
128        print 'Starting camera %s' % i
129        # process image
130        img_raw = its.image.convert_capture_to_rgb_image(
131                cap_raw[i], props=props)
132        size_raw[i] = (cap_raw[i]['width'], cap_raw[i]['height'])
133
134        # save images if debug
135        if debug:
136            its.image.write_image(img_raw, '%s_raw_%s.jpg' % (NAME, i))
137
138        # convert to [0, 255] images
139        img_raw *= 255
140
141        # scale to match calibration data
142        img = cv2.resize(img_raw.astype(np.uint8), None, fx=2, fy=2)
143
144        # load parameters for each physical camera
145        ical = props_physical[i]['android.lens.intrinsicCalibration']
146        assert len(ical) == 5, 'android.lens.instrisicCalibration incorrect.'
147        k[i] = np.array([[ical[0], ical[4], ical[2]],
148                         [0, ical[1], ical[3]],
149                         [0, 0, 1]])
150        print ' k:', k[i]
151
152        rotation[i] = np.array(props_physical[i]['android.lens.poseRotation'])
153        print ' rotation:', rotation[i]
154        assert len(rotation[i]) == 4, 'poseRotation has wrong # of params.'
155        trans[i] = np.array(
156                props_physical[i]['android.lens.poseTranslation'])
157        print ' translation:', trans[i]
158        assert len(trans[i]) == 3, 'poseTranslation has wrong # of params.'
159        if ((rotation[i] == ROTATE_REF_MATRIX).all() and
160                    (trans[i] == TRANS_REF_MATRIX).all()):
161            reference[i] = True
162        else:
163            reference[i] = False
164
165        # Apply correction to image (if available)
166        if its.caps.distortion_correction(props):
167            distort = np.array(props_physical[i]['android.lens.distortion'])
168            assert len(distort) == 5, 'radialDistortion has wrong # of params.'
169            cv2_distort = np.array([distort[0], distort[1],
170                                    distort[3], distort[4],
171                                    distort[2]])
172            img = cv2.undistort(img, k[i], cv2_distort)
173            its.image.write_image(img/255.0, '%s_correct_%s.jpg' % (
174                    NAME, i))
175
176        # Find the circles in grayscale image
177        circle[i] = find_circle(cv2.cvtColor(img, cv2.COLOR_BGR2GRAY),
178                                '%s_gray%s.jpg' % (NAME, i))
179
180        # Find focal length & sensor size
181        fl[i] = props_physical[i]['android.lens.info.availableFocalLengths'][0]
182        sensor_diag[i] = math.sqrt(size_raw[i][0] ** 2 + size_raw[i][1] ** 2)
183
184        # Find 3D location of circle centers
185        point[i] = np.dot(np.linalg.inv(k[i]),
186                          np.array([circle[i][0],
187                                    circle[i][1], 1])) * chart_distance * 1.0E-2
188
189    ref_index = (e for e in reference if e).next()
190    print 'reference camera id:', ref_index
191    ref_rotation = rotation[ref_index]
192    ref_rotation = ref_rotation.astype(np.float32)
193    print 'rotation reference:', ref_rotation
194    r = rotation_matrix(ref_rotation)
195    if debug:
196        print 'r:', r
197    t = -1 * trans[ref_index]
198    print 't:', t
199
200    # Estimate ids[0] circle center from ids[1] & params
201    estimated_0 = cv2.projectPoints(point[ids[1]].reshape(1, 3),
202                                    r, t, k[ids[0]], None)[0][0][0]
203    err_0 = np.linalg.norm(estimated_0 - circle[ids[0]][:2])
204    print 'Circle centers [%s]' % ids[0]
205    print 'Measured:      %.1f, %.1f' % (circle[ids[0]][1], circle[ids[0]][0])
206    print 'Calculated:    %.1f, %.1f' % (estimated_0[1],
207                                         estimated_0[0])
208    print 'Error(pixels): %.1f' % err_0
209
210    # Estimate ids[0] circle center from ids[1] & params
211    estimated_1 = cv2.projectPoints(point[ids[0]].reshape(1, 3),
212                                    r.T, -np.dot(r, t), k[ids[1]],
213                                    None)[0][0][0]
214    err_1 = np.linalg.norm(estimated_1 - circle[ids[1]][:2])
215    print 'Circle centers [%s]' % ids[1]
216    print 'Measured:      %.1f, %.1f' % (circle[ids[1]][1], circle[ids[1]][0])
217    print 'Calculated:    %.1f, %.1f' % (estimated_1[1], estimated_1[0])
218    print 'Error(pixels): %.1f' % err_1
219
220    err_0 /= math.sqrt(size_raw[ids[0]][0]**2 + size_raw[ids[0]][1]**2)
221    err_1 /= math.sqrt(size_raw[ids[1]][0]**2 + size_raw[ids[1]][1]**2)
222    msg = '%s -> %s center error too large! val=%.1f%%, THRESH=%.f%%' % (
223            ids[1], ids[0], err_0*100, ALIGN_TOL_PERCENT)
224    assert err_0*100 < ALIGN_TOL_PERCENT, msg
225    msg = '%s -> %s center error too large! val=%.1f%%, THRESH=%.f%%' % (
226            ids[0], ids[1], err_1*100, ALIGN_TOL_PERCENT)
227    assert err_1*100 < ALIGN_TOL_PERCENT, msg
228
229    # Check focal length and circle size if more than 1 focal length
230    if len(avail_fls) > 1:
231        print 'circle_0: %.2f, circle_1: %.2f' % (
232                circle[ids[0]][2], circle[ids[1]][2])
233        print 'fl_0: %.2f, fl_1: %.2f' % (fl[ids[0]], fl[ids[1]])
234        print 'diag_0: %.2f, diag_1: %.2f' % (
235                sensor_diag[ids[0]], sensor_diag[ids[1]])
236        msg = 'Circle size does not scale properly.'
237        assert np.isclose(circle[ids[0]][2]/fl[ids[0]]*sensor_diag[ids[0]],
238                          circle[ids[1]][2]/fl[ids[1]]*sensor_diag[ids[1]],
239                          rtol=CIRCLE_TOL_PERCENT/100.0), msg
240
241
242if __name__ == '__main__':
243    main()
244