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 (at) intel.com> 19 */ 20 21 #include "image_projector.h" 22 23 namespace XCam { 24 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 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 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 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 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 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 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 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 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 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 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 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
