/*
|
* image_projector.cpp - Calculate 2D image projective matrix
|
*
|
* Copyright (c) 2017 Intel Corporation
|
*
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
* you may not use this file except in compliance with the License.
|
* You may obtain a copy of the License at
|
*
|
* http://www.apache.org/licenses/LICENSE-2.0
|
*
|
* Unless required by applicable law or agreed to in writing, software
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
* See the License for the specific language governing permissions and
|
* limitations under the License.
|
*
|
* Author: Zong Wei <wei.zong@intel.com>
|
*/
|
|
#include "image_projector.h"
|
|
namespace XCam {
|
|
ImageProjector::ImageProjector (CalibrationParams ¶ms)
|
: _calib_params (params)
|
{
|
set_camera_intrinsics(
|
params.focal_x,
|
params.focal_y,
|
params.offset_x,
|
params.offset_y,
|
params.skew);
|
}
|
|
ImageProjector::ImageProjector (
|
double focal_x,
|
double focal_y,
|
double offset_x,
|
double offset_y,
|
double skew)
|
{
|
set_camera_intrinsics(
|
focal_x,
|
focal_y,
|
offset_x,
|
offset_y,
|
skew);
|
}
|
|
Quaternd
|
ImageProjector::interp_orientation (
|
int64_t frame_ts,
|
const std::vector<Vec4d> &orientation,
|
const std::vector<int64_t> &orient_ts,
|
int& index)
|
{
|
if (orientation.empty () || orient_ts.empty ()) {
|
return Quaternd ();
|
}
|
|
int count = orient_ts.size ();
|
if (count == 1) {
|
return Quaternd(orientation[0]);
|
}
|
|
int i = index;
|
XCAM_ASSERT(0 <= i && i < count);
|
|
while (i >= 0 && orient_ts[i] > frame_ts) {
|
i--;
|
}
|
if (i < 0) return Quaternd (orientation[0]);
|
|
while (i + 1 < count && orient_ts[i + 1] < frame_ts) {
|
i++;
|
}
|
if (i >= count) return Quaternd (orientation[count - 1]);
|
|
index = i;
|
|
double weight_start = (orient_ts[i + 1] - frame_ts) / (orient_ts[i + 1] - orient_ts[i]);
|
double weight_end = 1.0f - weight_start;
|
XCAM_ASSERT (weight_start >= 0 && weight_start <= 1.0);
|
XCAM_ASSERT (weight_end >= 0 && weight_end <= 1.0);
|
|
return Quaternd (orientation[i] * weight_start + orientation[i + 1] * weight_end);
|
//return Quaternd (quat[i]).slerp(weight_start, Quaternd (quat[i + 1]));
|
}
|
|
// rotate coordinate system keeps the handedness of original coordinate system unchanged
|
//
|
// axis_to_x: defines the axis of the new cooridinate system that
|
// coincide with the X axis of the original coordinate system.
|
// axis_to_y: defines the axis of the new cooridinate system that
|
// coincide with the Y axis of the original coordinate system.
|
//
|
Mat3d
|
ImageProjector::rotate_coordinate_system (
|
CoordinateAxisType axis_to_x,
|
CoordinateAxisType axis_to_y)
|
{
|
Mat3d t_mat;
|
if (axis_to_x == AXIS_X && axis_to_y == AXIS_MINUS_Z) {
|
t_mat = Mat3d (Vec3d (1, 0, 0),
|
Vec3d (0, 0, -1),
|
Vec3d (0, 1, 0));
|
} else if (axis_to_x == AXIS_X && axis_to_y == AXIS_MINUS_Y) {
|
t_mat = Mat3d (Vec3d (1, 0, 0),
|
Vec3d (0, -1, 0),
|
Vec3d (0, 0, -1));
|
} else if (axis_to_x == AXIS_X && axis_to_y == AXIS_Z) {
|
t_mat = Mat3d (Vec3d (1, 0, 0),
|
Vec3d (0, 0, 1),
|
Vec3d (0, -1, 0));
|
} else if (axis_to_x == AXIS_MINUS_Z && axis_to_y == AXIS_Y) {
|
t_mat = Mat3d (Vec3d (0, 0, -1),
|
Vec3d (0, 1, 0),
|
Vec3d (1, 0, 0));
|
} else if (axis_to_x == AXIS_MINUS_X && axis_to_y == AXIS_Y) {
|
t_mat = Mat3d (Vec3d (-1, 0, 0),
|
Vec3d (0, 1, 0),
|
Vec3d (0, 0, -1));
|
} else if (axis_to_x == AXIS_Z && axis_to_y == AXIS_Y) {
|
t_mat = Mat3d (Vec3d (0, 0, 1),
|
Vec3d (0, 1, 0),
|
Vec3d (-1, 0, 0));
|
} else if (axis_to_x == AXIS_MINUS_Y && axis_to_y == AXIS_X) {
|
t_mat = Mat3d (Vec3d (0, -1, 0),
|
Vec3d (1, 0, 0),
|
Vec3d (0, 0, 1));
|
} else if (axis_to_x == AXIS_MINUS_X && axis_to_y == AXIS_MINUS_Y) {
|
t_mat = Mat3d (Vec3d (-1, 0, 0),
|
Vec3d (0, -1, 0),
|
Vec3d (0, 0, 1));
|
} else if (axis_to_x == AXIS_Y && axis_to_y == AXIS_MINUS_X) {
|
t_mat = Mat3d (Vec3d (0, 1, 0),
|
Vec3d (-1, 0, 0),
|
Vec3d (0, 0, 1));
|
} else {
|
t_mat = Mat3d ();
|
}
|
return t_mat;
|
}
|
|
// mirror coordinate system will change the handedness of original coordinate system
|
//
|
// axis_mirror: defines the axis that coordinate system mirror on
|
//
|
Mat3d
|
ImageProjector::mirror_coordinate_system (CoordinateAxisType axis_mirror)
|
{
|
Mat3d t_mat;
|
|
switch (axis_mirror) {
|
case AXIS_X:
|
case AXIS_MINUS_X:
|
t_mat = Mat3d (Vec3d (-1, 0, 0),
|
Vec3d (0, 1, 0),
|
Vec3d (0, 0, 1));
|
break;
|
case AXIS_Y:
|
case AXIS_MINUS_Y:
|
t_mat = Mat3d (Vec3d (1, 0, 0),
|
Vec3d (0, -1, 0),
|
Vec3d (0, 0, 1));
|
break;
|
case AXIS_Z:
|
case AXIS_MINUS_Z:
|
t_mat = Mat3d (Vec3d (1, 0, 0),
|
Vec3d (0, 1, 0),
|
Vec3d (0, 0, -1));
|
break;
|
default:
|
t_mat = Mat3d ();
|
break;
|
}
|
|
return t_mat;
|
}
|
|
// transform coordinate system will change the handedness of original coordinate system
|
//
|
// axis_to_x: defines the axis of the new cooridinate system that
|
// coincide with the X axis of the original coordinate system.
|
// axis_to_y: defines the axis of the new cooridinate system that
|
// coincide with the Y axis of the original coordinate system.
|
// axis_mirror: defines the axis that coordinate system mirror on
|
Mat3d
|
ImageProjector::transform_coordinate_system (CoordinateSystemConv &transform)
|
{
|
return mirror_coordinate_system (transform.axis_mirror) *
|
rotate_coordinate_system (transform.axis_to_x, transform.axis_to_y);
|
}
|
|
Mat3d
|
ImageProjector::align_coordinate_system (
|
CoordinateSystemConv &world_to_device,
|
Mat3d &extrinsics,
|
CoordinateSystemConv &device_to_image)
|
{
|
return transform_coordinate_system (world_to_device)
|
* extrinsics
|
* transform_coordinate_system (device_to_image);
|
}
|
|
XCamReturn
|
ImageProjector::set_sensor_calibration (CalibrationParams ¶ms)
|
{
|
XCamReturn ret = XCAM_RETURN_NO_ERROR;
|
|
_calib_params = params;
|
set_camera_intrinsics (
|
params.focal_x,
|
params.focal_y,
|
params.offset_x,
|
params.offset_y,
|
params.skew);
|
|
return ret;
|
}
|
|
XCamReturn
|
ImageProjector::set_camera_intrinsics (
|
double focal_x,
|
double focal_y,
|
double offset_x,
|
double offset_y,
|
double skew)
|
{
|
XCamReturn ret = XCAM_RETURN_NO_ERROR;
|
|
_intrinsics = Mat3d (Vec3d (focal_x, skew, offset_x),
|
Vec3d (0, focal_y, offset_y),
|
Vec3d (0, 0, 1));
|
|
XCAM_LOG_DEBUG("Intrinsic Matrix(3x3) \n");
|
XCAM_LOG_DEBUG("intrinsic = [ %lf, %lf, %lf ; %lf, %lf, %lf ; %lf, %lf, %lf ] \n",
|
_intrinsics(0, 0), _intrinsics(0, 1), _intrinsics(0, 2),
|
_intrinsics(1, 0), _intrinsics(1, 1), _intrinsics(1, 2),
|
_intrinsics(2, 0), _intrinsics(2, 1), _intrinsics(2, 2));
|
return ret;
|
}
|
|
Mat3d
|
ImageProjector::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)
|
{
|
if (pose_ts.empty () || orientation.empty () || translation.empty ()) {
|
return Mat3d ();
|
}
|
|
int index = 0;
|
const double ts = frame_ts + _calib_params.gyro_delay;
|
Quaternd quat = interp_orientation (ts, orientation, pose_ts, index) +
|
Quaternd (_calib_params.gyro_drift);
|
|
Mat3d extrinsics = quat.rotation_matrix ();
|
|
XCAM_LOG_DEBUG("Extrinsic Matrix(3x3) \n");
|
XCAM_LOG_DEBUG("extrinsic = [ %lf, %lf, %lf; %lf, %lf, %lf; %lf, %lf, %lf ] \n",
|
extrinsics(0, 0), extrinsics(0, 1), extrinsics(0, 2),
|
extrinsics(1, 0), extrinsics(1, 1), extrinsics(1, 2),
|
extrinsics(2, 0), extrinsics(2, 1), extrinsics(2, 2));
|
|
return extrinsics;
|
}
|
|
Mat3d
|
ImageProjector::calc_camera_extrinsics (
|
const int64_t frame_ts,
|
DevicePoseList &pose_list)
|
{
|
if (pose_list.empty ()) {
|
return Mat3d ();
|
}
|
|
int index = 0;
|
|
std::vector<Vec4d> orientation;
|
std::vector<int64_t> orient_ts;
|
std::vector<Vec3d> translation;
|
|
for (DevicePoseList::iterator iter = pose_list.begin (); iter != pose_list.end (); ++iter)
|
{
|
SmartPtr<DevicePose> pose = *iter;
|
|
orientation.push_back (Vec4d (pose->orientation[0],
|
pose->orientation[1],
|
pose->orientation[2],
|
pose->orientation[3]));
|
|
orient_ts.push_back (pose->timestamp);
|
|
translation.push_back (Vec3d (pose->translation[0],
|
pose->translation[1],
|
pose->translation[2]));
|
|
}
|
|
const int64_t ts = frame_ts + _calib_params.gyro_delay;
|
Quaternd quat = interp_orientation (ts, orientation, orient_ts, index) +
|
Quaternd (_calib_params.gyro_drift);
|
|
Mat3d extrinsics = quat.rotation_matrix ();
|
|
XCAM_LOG_DEBUG("Extrinsic Matrix(3x3) \n");
|
XCAM_LOG_DEBUG("extrinsic = [ %lf, %lf, %lf; %lf, %lf, %lf; %lf, %lf, %lf ] \n",
|
extrinsics(0, 0), extrinsics(0, 1), extrinsics(0, 2),
|
extrinsics(1, 0), extrinsics(1, 1), extrinsics(1, 2),
|
extrinsics(2, 0), extrinsics(2, 1), extrinsics(2, 2));
|
|
return extrinsics;
|
}
|
|
Mat3d
|
ImageProjector::calc_projective (
|
Mat3d &extrinsic0,
|
Mat3d &extrinsic1)
|
{
|
Mat3d intrinsic = get_camera_intrinsics ();
|
|
return intrinsic * extrinsic0 * extrinsic1.transpose () * intrinsic.inverse ();
|
}
|
|
}
|
