190f5ba538bf40bcf4fd41049c7bf4296d3ffc9c7Benjamin Peterson/* 2246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg * image_projector.cpp - Calculate 2D image projective matrix 38ab27dfa135dc1f3f3f156db2ae5e13c46eeb5a4Brett Cannon * 4246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg * Copyright (c) 2017 Intel Corporation 5246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg * 6246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg * Licensed under the Apache License, Version 2.0 (the "License"); 7246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg * you may not use this file except in compliance with the License. 8246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg * You may obtain a copy of the License at 9246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg * 10246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg * http://www.apache.org/licenses/LICENSE-2.0 11246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg * 12246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg * Unless required by applicable law or agreed to in writing, software 134ac9ce4f9a33a330a555ae8015e2f2ae2780fafbBenjamin Peterson * distributed under the License is distributed on an "AS IS" BASIS, 14246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg * See the License for the specific language governing permissions and 16246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg * limitations under the License. 17246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg * 18246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg * Author: Zong Wei <wei.zong@intel.com> 19246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg */ 208ab27dfa135dc1f3f3f156db2ae5e13c46eeb5a4Brett Cannon 21246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg#include "image_projector.h" 22246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 23246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburgnamespace XCam { 24246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 25246d847475d041a823dbc3cc3aceae46e97de937Marc-André LemburgImageProjector::ImageProjector (CalibrationParams ¶ms) 26246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg : _calib_params (params) 27246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg{ 28b9f4feab1b9c9ffa8ea29af3d82bc536f9f3005aSteve Dower set_camera_intrinsics( 29b9f4feab1b9c9ffa8ea29af3d82bc536f9f3005aSteve Dower params.focal_x, 30246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg params.focal_y, 31246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg params.offset_x, 32380f417e15dd36e6656a4f65e9c5ccd75134b8c1Marc-André Lemburg params.offset_y, 33380f417e15dd36e6656a4f65e9c5ccd75134b8c1Marc-André Lemburg params.skew); 34380f417e15dd36e6656a4f65e9c5ccd75134b8c1Marc-André Lemburg} 35b9f4feab1b9c9ffa8ea29af3d82bc536f9f3005aSteve Dower 36e52e3786c67e16b7b8adba71227f9e3c5350d7a6Alexandre VassalottiImageProjector::ImageProjector ( 37fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters double focal_x, 38fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters double focal_y, 39fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters double offset_x, 40cdc7923f6d1c9731d81f299569b1ea72de7eb212Marc-André Lemburg double offset_y, 4191e83e209d66d4b7a892c9360f9e524d6e282191Marc-André Lemburg double skew) 42366a0feb9a7794a9f72ab0b37d36b6427e6d3bd1Marc-André Lemburg{ 43246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg set_camera_intrinsics( 44246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg focal_x, 45246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg focal_y, 46246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg offset_x, 47246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg offset_y, 48246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg skew); 49246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg} 50246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 51246d847475d041a823dbc3cc3aceae46e97de937Marc-André LemburgQuaternd 52246d847475d041a823dbc3cc3aceae46e97de937Marc-André LemburgImageProjector::interp_orientation ( 53246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg int64_t frame_ts, 54246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg const std::vector<Vec4d> &orientation, 55246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg const std::vector<int64_t> &orient_ts, 56246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg int& index) 57246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg{ 58246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg if (orientation.empty () || orient_ts.empty ()) { 59246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg return Quaternd (); 60246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg } 61246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 62246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg int count = orient_ts.size (); 63eb9957065acaafbf3d8ebee4770ecb13ea0c07c0Martin Panter if (count == 1) { 64246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg return Quaternd(orientation[0]); 65246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg } 66246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 67246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg int i = index; 68246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg XCAM_ASSERT(0 <= i && i < count); 69246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 70246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg while (i >= 0 && orient_ts[i] > frame_ts) { 71246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg i--; 72246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg } 73246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg if (i < 0) return Quaternd (orientation[0]); 74246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 75246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg while (i + 1 < count && orient_ts[i + 1] < frame_ts) { 76246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg i++; 77246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg } 78246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg if (i >= count) return Quaternd (orientation[count - 1]); 79246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 80246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg index = i; 81246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 82246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg double weight_start = (orient_ts[i + 1] - frame_ts) / (orient_ts[i + 1] - orient_ts[i]); 83246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg double weight_end = 1.0f - weight_start; 84246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg XCAM_ASSERT (weight_start >= 0 && weight_start <= 1.0); 85246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg XCAM_ASSERT (weight_end >= 0 && weight_end <= 1.0); 86246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 87246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg return Quaternd (orientation[i] * weight_start + orientation[i + 1] * weight_end); 88246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg //return Quaternd (quat[i]).slerp(weight_start, Quaternd (quat[i + 1])); 89246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg} 90246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 91246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg// rotate coordinate system keeps the handedness of original coordinate system unchanged 92246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg// 93246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg// axis_to_x: defines the axis of the new cooridinate system that 9446a9900e099a82cda7328b0de16d6fffe52ee62aBenjamin Peterson// coincide with the X axis of the original coordinate system. 95246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg// axis_to_y: defines the axis of the new cooridinate system that 96246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg// coincide with the Y axis of the original coordinate system. 97246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg// 98246d847475d041a823dbc3cc3aceae46e97de937Marc-André LemburgMat3d 99246d847475d041a823dbc3cc3aceae46e97de937Marc-André LemburgImageProjector::rotate_coordinate_system ( 100246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg CoordinateAxisType axis_to_x, 101246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg CoordinateAxisType axis_to_y) 102246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg{ 103246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg Mat3d t_mat; 104246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg if (axis_to_x == AXIS_X && axis_to_y == AXIS_MINUS_Z) { 105246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg t_mat = Mat3d (Vec3d (1, 0, 0), 106246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg Vec3d (0, 0, -1), 107246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg Vec3d (0, 1, 0)); 108246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg } else if (axis_to_x == AXIS_X && axis_to_y == AXIS_MINUS_Y) { 109246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg t_mat = Mat3d (Vec3d (1, 0, 0), 110246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg Vec3d (0, -1, 0), 111246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg Vec3d (0, 0, -1)); 112246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg } else if (axis_to_x == AXIS_X && axis_to_y == AXIS_Z) { 113bb59d89ceeb1abfb4d73c7fc60b534e4464adf35Berker Peksag t_mat = Mat3d (Vec3d (1, 0, 0), 114246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg Vec3d (0, 0, 1), 11568386bc0b88eb4b7c9aeb4f753114dc85f8df5b6Larry Hastings Vec3d (0, -1, 0)); 116fc990e942fb55be78e8352f4913749e91cac381dJesus Cea } else if (axis_to_x == AXIS_MINUS_Z && axis_to_y == AXIS_Y) { 117246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg t_mat = Mat3d (Vec3d (0, 0, -1), 1181392f71c3927c9d81969200f5dfff9fb832cde0bBerker Peksag Vec3d (0, 1, 0), 1191392f71c3927c9d81969200f5dfff9fb832cde0bBerker Peksag Vec3d (1, 0, 0)); 120e52e3786c67e16b7b8adba71227f9e3c5350d7a6Alexandre Vassalotti } else if (axis_to_x == AXIS_MINUS_X && axis_to_y == AXIS_Y) { 121e52e3786c67e16b7b8adba71227f9e3c5350d7a6Alexandre Vassalotti t_mat = Mat3d (Vec3d (-1, 0, 0), 122e52e3786c67e16b7b8adba71227f9e3c5350d7a6Alexandre Vassalotti Vec3d (0, 1, 0), 123e52e3786c67e16b7b8adba71227f9e3c5350d7a6Alexandre Vassalotti Vec3d (0, 0, -1)); 124e52e3786c67e16b7b8adba71227f9e3c5350d7a6Alexandre Vassalotti } else if (axis_to_x == AXIS_Z && axis_to_y == AXIS_Y) { 125e52e3786c67e16b7b8adba71227f9e3c5350d7a6Alexandre Vassalotti t_mat = Mat3d (Vec3d (0, 0, 1), 126e52e3786c67e16b7b8adba71227f9e3c5350d7a6Alexandre Vassalotti Vec3d (0, 1, 0), 127e52e3786c67e16b7b8adba71227f9e3c5350d7a6Alexandre Vassalotti Vec3d (-1, 0, 0)); 128ced3936894cec493b93c1144c543d51048268288Victor Stinner } else if (axis_to_x == AXIS_MINUS_Y && axis_to_y == AXIS_X) { 129e52e3786c67e16b7b8adba71227f9e3c5350d7a6Alexandre Vassalotti t_mat = Mat3d (Vec3d (0, -1, 0), 130e52e3786c67e16b7b8adba71227f9e3c5350d7a6Alexandre Vassalotti Vec3d (1, 0, 0), 131e52e3786c67e16b7b8adba71227f9e3c5350d7a6Alexandre Vassalotti Vec3d (0, 0, 1)); 132e52e3786c67e16b7b8adba71227f9e3c5350d7a6Alexandre Vassalotti } else if (axis_to_x == AXIS_MINUS_X && axis_to_y == AXIS_MINUS_Y) { 133e52e3786c67e16b7b8adba71227f9e3c5350d7a6Alexandre Vassalotti t_mat = Mat3d (Vec3d (-1, 0, 0), 134e52e3786c67e16b7b8adba71227f9e3c5350d7a6Alexandre Vassalotti Vec3d (0, -1, 0), 135620c48b7ea7a1ad3af23725afdac1e6a2b3e6cefVictor Stinner Vec3d (0, 0, 1)); 136620c48b7ea7a1ad3af23725afdac1e6a2b3e6cefVictor Stinner } else if (axis_to_x == AXIS_Y && axis_to_y == AXIS_MINUS_X) { 137620c48b7ea7a1ad3af23725afdac1e6a2b3e6cefVictor Stinner t_mat = Mat3d (Vec3d (0, 1, 0), 138620c48b7ea7a1ad3af23725afdac1e6a2b3e6cefVictor Stinner Vec3d (-1, 0, 0), 139246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg Vec3d (0, 0, 1)); 140246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg } else { 141ba7c226095703f63c78b00e56f1db8d99ac3a54aAntoine Pitrou t_mat = Mat3d (); 142ba7c226095703f63c78b00e56f1db8d99ac3a54aAntoine Pitrou } 143ba7c226095703f63c78b00e56f1db8d99ac3a54aAntoine Pitrou return t_mat; 144ba7c226095703f63c78b00e56f1db8d99ac3a54aAntoine Pitrou} 145ba7c226095703f63c78b00e56f1db8d99ac3a54aAntoine Pitrou 146366a0feb9a7794a9f72ab0b37d36b6427e6d3bd1Marc-André Lemburg// mirror coordinate system will change the handedness of original coordinate system 147ced3936894cec493b93c1144c543d51048268288Victor Stinner// 148246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg// axis_mirror: defines the axis that coordinate system mirror on 149ba7c226095703f63c78b00e56f1db8d99ac3a54aAntoine Pitrou// 150246d847475d041a823dbc3cc3aceae46e97de937Marc-André LemburgMat3d 1518ab27dfa135dc1f3f3f156db2ae5e13c46eeb5a4Brett CannonImageProjector::mirror_coordinate_system (CoordinateAxisType axis_mirror) 1528ab27dfa135dc1f3f3f156db2ae5e13c46eeb5a4Brett Cannon{ 153246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg Mat3d t_mat; 154246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 155246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg switch (axis_mirror) { 156246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg case AXIS_X: 157246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg case AXIS_MINUS_X: 1588ab27dfa135dc1f3f3f156db2ae5e13c46eeb5a4Brett Cannon t_mat = Mat3d (Vec3d (-1, 0, 0), 1598ab27dfa135dc1f3f3f156db2ae5e13c46eeb5a4Brett Cannon Vec3d (0, 1, 0), 160246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg Vec3d (0, 0, 1)); 161246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg break; 162246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg case AXIS_Y: 163246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg case AXIS_MINUS_Y: 164fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters t_mat = Mat3d (Vec3d (1, 0, 0), 165fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters Vec3d (0, -1, 0), 166fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters Vec3d (0, 0, 1)); 167fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters break; 168fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters case AXIS_Z: 16946ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka case AXIS_MINUS_Z: 17046ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka t_mat = Mat3d (Vec3d (1, 0, 0), 17146ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka Vec3d (0, 1, 0), 17246ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka Vec3d (0, 0, -1)); 17346ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka break; 17446ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka default: 17546ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka t_mat = Mat3d (); 17646ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka break; 17746ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka } 17846ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka 17946ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka return t_mat; 18046ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka} 18146ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka 18246ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka// transform coordinate system will change the handedness of original coordinate system 18346ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka// 18446ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka// axis_to_x: defines the axis of the new cooridinate system that 18546ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka// coincide with the X axis of the original coordinate system. 18646ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka// axis_to_y: defines the axis of the new cooridinate system that 187246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg// coincide with the Y axis of the original coordinate system. 18846ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka// axis_mirror: defines the axis that coordinate system mirror on 18946ba6c8563922f043cad6423202ee0119614c807Serhiy StorchakaMat3d 19046ba6c8563922f043cad6423202ee0119614c807Serhiy StorchakaImageProjector::transform_coordinate_system (CoordinateSystemConv &transform) 19146ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka{ 19246ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka return mirror_coordinate_system (transform.axis_mirror) * 19346ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka rotate_coordinate_system (transform.axis_to_x, transform.axis_to_y); 19446ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka} 19546ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka 19646ba6c8563922f043cad6423202ee0119614c807Serhiy StorchakaMat3d 19746ba6c8563922f043cad6423202ee0119614c807Serhiy StorchakaImageProjector::align_coordinate_system ( 19846ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka CoordinateSystemConv &world_to_device, 19946ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka Mat3d &extrinsics, 20046ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka CoordinateSystemConv &device_to_image) 20146ba6c8563922f043cad6423202ee0119614c807Serhiy Storchaka{ 202ced3936894cec493b93c1144c543d51048268288Victor Stinner return transform_coordinate_system (world_to_device) 203246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg * extrinsics 204ced3936894cec493b93c1144c543d51048268288Victor Stinner * transform_coordinate_system (device_to_image); 205246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg} 2060eadaac7dc3ae49974c105ff9e8c1e98a04d7d5aTim Peters 207246d847475d041a823dbc3cc3aceae46e97de937Marc-André LemburgXCamReturn 208246d847475d041a823dbc3cc3aceae46e97de937Marc-André LemburgImageProjector::set_sensor_calibration (CalibrationParams ¶ms) 209246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg{ 210246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg XCamReturn ret = XCAM_RETURN_NO_ERROR; 211246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 212246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg _calib_params = params; 213246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg set_camera_intrinsics ( 214246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg params.focal_x, 215246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg params.focal_y, 2167dde792e62c8adeaf5d633dc89e18d16067add8eFlorent Xicluna params.offset_x, 2179d72bb452bced3a100f07f8a9e30c4495a9ec41aNeal Norwitz params.offset_y, 218246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg params.skew); 219ced3936894cec493b93c1144c543d51048268288Victor Stinner 220246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg return ret; 221246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg} 222246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 2239d72bb452bced3a100f07f8a9e30c4495a9ec41aNeal NorwitzXCamReturn 224246d847475d041a823dbc3cc3aceae46e97de937Marc-André LemburgImageProjector::set_camera_intrinsics ( 2259d72bb452bced3a100f07f8a9e30c4495a9ec41aNeal Norwitz double focal_x, 226246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg double focal_y, 227ced3936894cec493b93c1144c543d51048268288Victor Stinner double offset_x, 228246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg double offset_y, 229246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg double skew) 230246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg{ 2317dde792e62c8adeaf5d633dc89e18d16067add8eFlorent Xicluna XCamReturn ret = XCAM_RETURN_NO_ERROR; 2329d72bb452bced3a100f07f8a9e30c4495a9ec41aNeal Norwitz 233246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg _intrinsics = Mat3d (Vec3d (focal_x, skew, offset_x), 234246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg Vec3d (0, focal_y, offset_y), 235246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg Vec3d (0, 0, 1)); 236ced3936894cec493b93c1144c543d51048268288Victor Stinner 237246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg XCAM_LOG_DEBUG("Intrinsic Matrix(3x3) \n"); 238246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg XCAM_LOG_DEBUG("intrinsic = [ %lf, %lf, %lf ; %lf, %lf, %lf ; %lf, %lf, %lf ] \n", 23930b9d5d3af043fc2687ad11a188a34fe355e20efEzio Melotti _intrinsics(0, 0), _intrinsics(0, 1), _intrinsics(0, 2), 240246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg _intrinsics(1, 0), _intrinsics(1, 1), _intrinsics(1, 2), 241843c734ddda3e317eee97dfdd32ff15ceede22fcGuido van Rossum _intrinsics(2, 0), _intrinsics(2, 1), _intrinsics(2, 2)); 242246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg return ret; 243246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg} 244246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 245246d847475d041a823dbc3cc3aceae46e97de937Marc-André LemburgMat3d 246246d847475d041a823dbc3cc3aceae46e97de937Marc-André LemburgImageProjector::calc_camera_extrinsics ( 247246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg const int64_t frame_ts, 248ced3936894cec493b93c1144c543d51048268288Victor Stinner const std::vector<int64_t> &pose_ts, 249246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg const std::vector<Vec4d> &orientation, 250ced3936894cec493b93c1144c543d51048268288Victor Stinner const std::vector<Vec3d> &translation) 251246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg{ 252fd036451bf0e0ade8783e21df801abf7be96d020Antoine Pitrou if (pose_ts.empty () || orientation.empty () || translation.empty ()) { 253fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters return Mat3d (); 25444b548dda872c0d4f30afd6b44fd74b053a55ad8R David Murray } 25544b548dda872c0d4f30afd6b44fd74b053a55ad8R David Murray 25644b548dda872c0d4f30afd6b44fd74b053a55ad8R David Murray int index = 0; 257fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters const double ts = frame_ts + _calib_params.gyro_delay; 25844b548dda872c0d4f30afd6b44fd74b053a55ad8R David Murray Quaternd quat = interp_orientation (ts, orientation, pose_ts, index) + 25944b548dda872c0d4f30afd6b44fd74b053a55ad8R David Murray Quaternd (_calib_params.gyro_drift); 26044b548dda872c0d4f30afd6b44fd74b053a55ad8R David Murray 261fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters Mat3d extrinsics = quat.rotation_matrix (); 262fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters 2639fe394c1be9401c2b207b943f82e30af4ee32ab6Thomas Wouters XCAM_LOG_DEBUG("Extrinsic Matrix(3x3) \n"); 264fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters XCAM_LOG_DEBUG("extrinsic = [ %lf, %lf, %lf; %lf, %lf, %lf; %lf, %lf, %lf ] \n", 265fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters extrinsics(0, 0), extrinsics(0, 1), extrinsics(0, 2), 266fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters extrinsics(1, 0), extrinsics(1, 1), extrinsics(1, 2), 267dd15f6c315f20c1a9a540dd757cd63e27dbe9f3cChristian Heimes extrinsics(2, 0), extrinsics(2, 1), extrinsics(2, 2)); 268dd15f6c315f20c1a9a540dd757cd63e27dbe9f3cChristian Heimes 269dd15f6c315f20c1a9a540dd757cd63e27dbe9f3cChristian Heimes return extrinsics; 270ab88803a8addf36ae3fc7e9270ebbed869f7f191Antoine Pitrou} 271fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters 272fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas WoutersMat3d 2739fe394c1be9401c2b207b943f82e30af4ee32ab6Thomas WoutersImageProjector::calc_camera_extrinsics ( 274250014793e9ed7d6cfc6e25ac502bcb124895d23Benjamin Peterson const int64_t frame_ts, 275250014793e9ed7d6cfc6e25ac502bcb124895d23Benjamin Peterson DevicePoseList &pose_list) 276250014793e9ed7d6cfc6e25ac502bcb124895d23Benjamin Peterson{ 277250014793e9ed7d6cfc6e25ac502bcb124895d23Benjamin Peterson if (pose_list.empty ()) { 278250014793e9ed7d6cfc6e25ac502bcb124895d23Benjamin Peterson return Mat3d (); 279250014793e9ed7d6cfc6e25ac502bcb124895d23Benjamin Peterson } 280fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters 281fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters int index = 0; 282fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters 283fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters std::vector<Vec4d> orientation; 284fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters std::vector<int64_t> orient_ts; 285fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters std::vector<Vec3d> translation; 286fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters 287fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters for (DevicePoseList::iterator iter = pose_list.begin (); iter != pose_list.end (); ++iter) 288fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters { 289fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters SmartPtr<DevicePose> pose = *iter; 290fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters 29130b9d5d3af043fc2687ad11a188a34fe355e20efEzio Melotti orientation.push_back (Vec4d (pose->orientation[0], 2929d72bb452bced3a100f07f8a9e30c4495a9ec41aNeal Norwitz pose->orientation[1], 293fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters pose->orientation[2], 294fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters pose->orientation[3])); 295fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters 296fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters orient_ts.push_back (pose->timestamp); 297fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters 298fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters translation.push_back (Vec3d (pose->translation[0], 299fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters pose->translation[1], 300fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters pose->translation[2])); 301fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters 302fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters } 3032f3742b0d8dbe025f2f9e496938a6d344e6881adBerker Peksag 3042f3742b0d8dbe025f2f9e496938a6d344e6881adBerker Peksag const int64_t ts = frame_ts + _calib_params.gyro_delay; 3058d8221f0d674ea032337a82d1ad1d0d146510390Berker Peksag Quaternd quat = interp_orientation (ts, orientation, orient_ts, index) + 3062f3742b0d8dbe025f2f9e496938a6d344e6881adBerker Peksag Quaternd (_calib_params.gyro_drift); 3072f3742b0d8dbe025f2f9e496938a6d344e6881adBerker Peksag 3082f3742b0d8dbe025f2f9e496938a6d344e6881adBerker Peksag Mat3d extrinsics = quat.rotation_matrix (); 3092f3742b0d8dbe025f2f9e496938a6d344e6881adBerker Peksag 3102f3742b0d8dbe025f2f9e496938a6d344e6881adBerker Peksag XCAM_LOG_DEBUG("Extrinsic Matrix(3x3) \n"); 311246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg XCAM_LOG_DEBUG("extrinsic = [ %lf, %lf, %lf; %lf, %lf, %lf; %lf, %lf, %lf ] \n", 3128ab27dfa135dc1f3f3f156db2ae5e13c46eeb5a4Brett Cannon extrinsics(0, 0), extrinsics(0, 1), extrinsics(0, 2), 313246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg extrinsics(1, 0), extrinsics(1, 1), extrinsics(1, 2), 314246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg extrinsics(2, 0), extrinsics(2, 1), extrinsics(2, 2)); 315246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 316246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg return extrinsics; 317246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg} 318fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters 319fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas WoutersMat3d 320fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas WoutersImageProjector::calc_projective ( 321fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters Mat3d &extrinsic0, 322fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters Mat3d &extrinsic1) 323fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters{ 324fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters Mat3d intrinsic = get_camera_intrinsics (); 325fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters 326fc7bb8c786fd9cb3b1ab84e1976620d0ab545777Thomas Wouters return intrinsic * extrinsic0 * extrinsic1.transpose () * intrinsic.inverse (); 327ced3936894cec493b93c1144c543d51048268288Victor Stinner} 328246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 329246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg} 330246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg 331246d847475d041a823dbc3cc3aceae46e97de937Marc-André Lemburg