1/*
2 * image_projector.cpp - Calculate 2D image projective matrix
3 *
4 *  Copyright (c) 2017 Intel Corporation
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 *      http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 *
18 * Author: Zong Wei <wei.zong@intel.com>
19 */
20
21#include "image_projector.h"
22
23namespace XCam {
24
25ImageProjector::ImageProjector (CalibrationParams &params)
26    : _calib_params (params)
27{
28    set_camera_intrinsics(
29        params.focal_x,
30        params.focal_y,
31        params.offset_x,
32        params.offset_y,
33        params.skew);
34}
35
36ImageProjector::ImageProjector (
37    double focal_x,
38    double focal_y,
39    double offset_x,
40    double offset_y,
41    double skew)
42{
43    set_camera_intrinsics(
44        focal_x,
45        focal_y,
46        offset_x,
47        offset_y,
48        skew);
49}
50
51Quaternd
52ImageProjector::interp_orientation (
53    int64_t frame_ts,
54    const std::vector<Vec4d> &orientation,
55    const std::vector<int64_t> &orient_ts,
56    int& index)
57{
58    if (orientation.empty () || orient_ts.empty ()) {
59        return Quaternd ();
60    }
61
62    int count = orient_ts.size ();
63    if (count == 1) {
64        return Quaternd(orientation[0]);
65    }
66
67    int i = index;
68    XCAM_ASSERT(0 <= i && i < count);
69
70    while (i >= 0 && orient_ts[i] > frame_ts) {
71        i--;
72    }
73    if (i < 0) return Quaternd (orientation[0]);
74
75    while (i + 1 < count && orient_ts[i + 1] < frame_ts) {
76        i++;
77    }
78    if (i >= count) return Quaternd (orientation[count - 1]);
79
80    index = i;
81
82    double weight_start = (orient_ts[i + 1] - frame_ts) / (orient_ts[i + 1] - orient_ts[i]);
83    double weight_end = 1.0f - weight_start;
84    XCAM_ASSERT (weight_start >= 0 && weight_start <= 1.0);
85    XCAM_ASSERT (weight_end >= 0 && weight_end <= 1.0);
86
87    return Quaternd (orientation[i] * weight_start + orientation[i + 1] * weight_end);
88    //return Quaternd (quat[i]).slerp(weight_start, Quaternd (quat[i + 1]));
89}
90
91// rotate coordinate system keeps the handedness of original coordinate system unchanged
92//
93// axis_to_x: defines the axis of the new cooridinate system that
94//    coincide with the X axis of the original coordinate system.
95// axis_to_y: defines the axis of the new cooridinate system that
96//    coincide with the Y axis of the original coordinate system.
97//
98Mat3d
99ImageProjector::rotate_coordinate_system (
100    CoordinateAxisType axis_to_x,
101    CoordinateAxisType axis_to_y)
102{
103    Mat3d t_mat;
104    if (axis_to_x == AXIS_X && axis_to_y == AXIS_MINUS_Z) {
105        t_mat = Mat3d (Vec3d (1, 0, 0),
106                       Vec3d (0, 0, -1),
107                       Vec3d (0, 1, 0));
108    } else if (axis_to_x == AXIS_X && axis_to_y == AXIS_MINUS_Y) {
109        t_mat = Mat3d (Vec3d (1, 0, 0),
110                       Vec3d (0, -1, 0),
111                       Vec3d (0, 0, -1));
112    } else if (axis_to_x == AXIS_X && axis_to_y == AXIS_Z) {
113        t_mat = Mat3d (Vec3d (1, 0, 0),
114                       Vec3d (0, 0, 1),
115                       Vec3d (0, -1, 0));
116    } else if (axis_to_x == AXIS_MINUS_Z && axis_to_y == AXIS_Y) {
117        t_mat = Mat3d (Vec3d (0, 0, -1),
118                       Vec3d (0, 1, 0),
119                       Vec3d (1, 0, 0));
120    } else if (axis_to_x == AXIS_MINUS_X && axis_to_y == AXIS_Y) {
121        t_mat = Mat3d (Vec3d (-1, 0, 0),
122                       Vec3d (0, 1, 0),
123                       Vec3d (0, 0, -1));
124    } else if (axis_to_x == AXIS_Z && axis_to_y == AXIS_Y) {
125        t_mat = Mat3d (Vec3d (0, 0, 1),
126                       Vec3d (0, 1, 0),
127                       Vec3d (-1, 0, 0));
128    } else if (axis_to_x == AXIS_MINUS_Y && axis_to_y == AXIS_X) {
129        t_mat = Mat3d (Vec3d (0, -1, 0),
130                       Vec3d (1, 0, 0),
131                       Vec3d (0, 0, 1));
132    } else if (axis_to_x == AXIS_MINUS_X && axis_to_y == AXIS_MINUS_Y) {
133        t_mat = Mat3d (Vec3d (-1, 0, 0),
134                       Vec3d (0, -1, 0),
135                       Vec3d (0, 0, 1));
136    } else if (axis_to_x == AXIS_Y && axis_to_y == AXIS_MINUS_X) {
137        t_mat = Mat3d (Vec3d (0, 1, 0),
138                       Vec3d (-1, 0, 0),
139                       Vec3d (0, 0, 1));
140    } else  {
141        t_mat = Mat3d ();
142    }
143    return t_mat;
144}
145
146// mirror coordinate system will change the handedness of original coordinate system
147//
148// axis_mirror: defines the axis that coordinate system mirror on
149//
150Mat3d
151ImageProjector::mirror_coordinate_system (CoordinateAxisType axis_mirror)
152{
153    Mat3d t_mat;
154
155    switch (axis_mirror) {
156    case AXIS_X:
157    case AXIS_MINUS_X:
158        t_mat = Mat3d (Vec3d (-1, 0, 0),
159                       Vec3d (0, 1, 0),
160                       Vec3d (0, 0, 1));
161        break;
162    case AXIS_Y:
163    case AXIS_MINUS_Y:
164        t_mat = Mat3d (Vec3d (1, 0, 0),
165                       Vec3d (0, -1, 0),
166                       Vec3d (0, 0, 1));
167        break;
168    case AXIS_Z:
169    case AXIS_MINUS_Z:
170        t_mat = Mat3d (Vec3d (1, 0, 0),
171                       Vec3d (0, 1, 0),
172                       Vec3d (0, 0, -1));
173        break;
174    default:
175        t_mat = Mat3d ();
176        break;
177    }
178
179    return t_mat;
180}
181
182// transform coordinate system will change the handedness of original coordinate system
183//
184// axis_to_x: defines the axis of the new cooridinate system that
185//    coincide with the X axis of the original coordinate system.
186// axis_to_y: defines the axis of the new cooridinate system that
187//    coincide with the Y axis of the original coordinate system.
188// axis_mirror: defines the axis that coordinate system mirror on
189Mat3d
190ImageProjector::transform_coordinate_system (CoordinateSystemConv &transform)
191{
192    return mirror_coordinate_system (transform.axis_mirror) *
193           rotate_coordinate_system (transform.axis_to_x, transform.axis_to_y);
194}
195
196Mat3d
197ImageProjector::align_coordinate_system (
198    CoordinateSystemConv &world_to_device,
199    Mat3d &extrinsics,
200    CoordinateSystemConv &device_to_image)
201{
202    return transform_coordinate_system (world_to_device)
203           * extrinsics
204           * transform_coordinate_system (device_to_image);
205}
206
207XCamReturn
208ImageProjector::set_sensor_calibration (CalibrationParams &params)
209{
210    XCamReturn ret = XCAM_RETURN_NO_ERROR;
211
212    _calib_params = params;
213    set_camera_intrinsics (
214        params.focal_x,
215        params.focal_y,
216        params.offset_x,
217        params.offset_y,
218        params.skew);
219
220    return ret;
221}
222
223XCamReturn
224ImageProjector::set_camera_intrinsics (
225    double focal_x,
226    double focal_y,
227    double offset_x,
228    double offset_y,
229    double skew)
230{
231    XCamReturn ret = XCAM_RETURN_NO_ERROR;
232
233    _intrinsics = Mat3d (Vec3d (focal_x, skew, offset_x),
234                         Vec3d (0, focal_y, offset_y),
235                         Vec3d (0, 0, 1));
236
237    XCAM_LOG_DEBUG("Intrinsic Matrix(3x3) \n");
238    XCAM_LOG_DEBUG("intrinsic = [ %lf, %lf, %lf ; %lf, %lf, %lf ; %lf, %lf, %lf ] \n",
239                   _intrinsics(0, 0), _intrinsics(0, 1), _intrinsics(0, 2),
240                   _intrinsics(1, 0), _intrinsics(1, 1), _intrinsics(1, 2),
241                   _intrinsics(2, 0), _intrinsics(2, 1), _intrinsics(2, 2));
242    return ret;
243}
244
245Mat3d
246ImageProjector::calc_camera_extrinsics (
247    const int64_t frame_ts,
248    const std::vector<int64_t> &pose_ts,
249    const std::vector<Vec4d> &orientation,
250    const std::vector<Vec3d> &translation)
251{
252    if (pose_ts.empty () || orientation.empty () || translation.empty ()) {
253        return Mat3d ();
254    }
255
256    int index = 0;
257    const double ts = frame_ts + _calib_params.gyro_delay;
258    Quaternd quat = interp_orientation (ts, orientation, pose_ts, index) +
259                    Quaternd (_calib_params.gyro_drift);
260
261    Mat3d extrinsics = quat.rotation_matrix ();
262
263    XCAM_LOG_DEBUG("Extrinsic Matrix(3x3) \n");
264    XCAM_LOG_DEBUG("extrinsic = [ %lf, %lf, %lf; %lf, %lf, %lf; %lf, %lf, %lf ] \n",
265                   extrinsics(0, 0), extrinsics(0, 1), extrinsics(0, 2),
266                   extrinsics(1, 0), extrinsics(1, 1), extrinsics(1, 2),
267                   extrinsics(2, 0), extrinsics(2, 1), extrinsics(2, 2));
268
269    return extrinsics;
270}
271
272Mat3d
273ImageProjector::calc_camera_extrinsics (
274    const int64_t frame_ts,
275    DevicePoseList &pose_list)
276{
277    if (pose_list.empty ()) {
278        return Mat3d ();
279    }
280
281    int index = 0;
282
283    std::vector<Vec4d> orientation;
284    std::vector<int64_t> orient_ts;
285    std::vector<Vec3d> translation;
286
287    for (DevicePoseList::iterator iter = pose_list.begin (); iter != pose_list.end (); ++iter)
288    {
289        SmartPtr<DevicePose> pose = *iter;
290
291        orientation.push_back (Vec4d (pose->orientation[0],
292                                      pose->orientation[1],
293                                      pose->orientation[2],
294                                      pose->orientation[3]));
295
296        orient_ts.push_back (pose->timestamp);
297
298        translation.push_back (Vec3d (pose->translation[0],
299                                      pose->translation[1],
300                                      pose->translation[2]));
301
302    }
303
304    const int64_t ts = frame_ts + _calib_params.gyro_delay;
305    Quaternd quat = interp_orientation (ts, orientation, orient_ts, index) +
306                    Quaternd (_calib_params.gyro_drift);
307
308    Mat3d extrinsics = quat.rotation_matrix ();
309
310    XCAM_LOG_DEBUG("Extrinsic Matrix(3x3) \n");
311    XCAM_LOG_DEBUG("extrinsic = [ %lf, %lf, %lf; %lf, %lf, %lf; %lf, %lf, %lf ] \n",
312                   extrinsics(0, 0), extrinsics(0, 1), extrinsics(0, 2),
313                   extrinsics(1, 0), extrinsics(1, 1), extrinsics(1, 2),
314                   extrinsics(2, 0), extrinsics(2, 1), extrinsics(2, 2));
315
316    return extrinsics;
317}
318
319Mat3d
320ImageProjector::calc_projective (
321    Mat3d &extrinsic0,
322    Mat3d &extrinsic1)
323{
324    Mat3d intrinsic = get_camera_intrinsics ();
325
326    return intrinsic * extrinsic0 * extrinsic1.transpose () * intrinsic.inverse ();
327}
328
329}
330
331