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
23 namespace XCam {
24
ImageProjector(CalibrationParams & params)25 ImageProjector::ImageProjector (CalibrationParams ¶ms)
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
ImageProjector(double focal_x,double focal_y,double offset_x,double offset_y,double skew)36 ImageProjector::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
51 Quaternd
interp_orientation(int64_t frame_ts,const std::vector<Vec4d> & orientation,const std::vector<int64_t> & orient_ts,int & index)52 ImageProjector::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 //
98 Mat3d
rotate_coordinate_system(CoordinateAxisType axis_to_x,CoordinateAxisType axis_to_y)99 ImageProjector::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 //
150 Mat3d
mirror_coordinate_system(CoordinateAxisType axis_mirror)151 ImageProjector::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
189 Mat3d
transform_coordinate_system(CoordinateSystemConv & transform)190 ImageProjector::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
196 Mat3d
align_coordinate_system(CoordinateSystemConv & world_to_device,Mat3d & extrinsics,CoordinateSystemConv & device_to_image)197 ImageProjector::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
207 XCamReturn
set_sensor_calibration(CalibrationParams & params)208 ImageProjector::set_sensor_calibration (CalibrationParams ¶ms)
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
223 XCamReturn
set_camera_intrinsics(double focal_x,double focal_y,double offset_x,double offset_y,double skew)224 ImageProjector::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
245 Mat3d
calc_camera_extrinsics(const int64_t frame_ts,const std::vector<int64_t> & pose_ts,const std::vector<Vec4d> & orientation,const std::vector<Vec3d> & translation)246 ImageProjector::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
272 Mat3d
calc_camera_extrinsics(const int64_t frame_ts,DevicePoseList & pose_list)273 ImageProjector::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
319 Mat3d
calc_projective(Mat3d & extrinsic0,Mat3d & extrinsic1)320 ImageProjector::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