93 lines
3.0 KiB
C++
93 lines
3.0 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file StereoCamera.h
|
|
* @brief A Stereo Camera based on two Simple Cameras
|
|
* @author Chris Beall
|
|
*/
|
|
|
|
#include <gtsam/geometry/StereoCamera.h>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2& K,
|
|
double baseline) :
|
|
leftCamPose_(leftCamPose), K_(K), baseline_(baseline) {
|
|
Vector calibration = K_.vector();
|
|
fx_ = calibration(0);
|
|
fy_ = calibration(1);
|
|
cx_ = calibration(3);
|
|
cy_ = calibration(4);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
StereoPoint2 StereoCamera::project(const Point3& point,
|
|
boost::optional<Matrix&> Dproject_stereo_pose,
|
|
boost::optional<Matrix&> Dproject_stereo_point) const {
|
|
|
|
Point3 cameraPoint = leftCamPose_.transform_to(point);
|
|
|
|
if (Dproject_stereo_pose) {
|
|
Matrix D_cameraPoint_pose;
|
|
Point3 cameraPoint = pose().transform_to(point, D_cameraPoint_pose,
|
|
boost::none);
|
|
|
|
Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian
|
|
Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose;
|
|
|
|
Matrix D_projection_intrinsic = Duncalibrate2(K()); // 3x3
|
|
*Dproject_stereo_pose = D_projection_intrinsic * D_intrinsic_pose;
|
|
}
|
|
|
|
if (Dproject_stereo_point) {
|
|
Matrix D_cameraPoint_point;
|
|
Point3 cameraPoint = pose().transform_to(point, boost::none,
|
|
D_cameraPoint_point);
|
|
|
|
Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian
|
|
Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point;
|
|
|
|
Matrix D_projection_intrinsic = Duncalibrate2(K()); // 3x3
|
|
*Dproject_stereo_point = D_projection_intrinsic * D_intrinsic_point;
|
|
}
|
|
|
|
double d = 1.0 / cameraPoint.z();
|
|
double uL = cx_ + d * fx_ * cameraPoint.x();
|
|
double uR = cx_ + d * fx_ * (cameraPoint.x() - baseline_);
|
|
double v = cy_ + d * fy_ * cameraPoint.y();
|
|
return StereoPoint2(uL, uR, v);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
|
|
double d = 1.0 / P.z(), d2 = d * d;
|
|
return Matrix_(3, 3, d, 0.0, -P.x() * d2, d, 0.0, -(P.x() - baseline()) * d2,
|
|
0.0, d, -P.y() * d2);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Matrix StereoCamera::Duncalibrate2(const Cal3_S2& K) {
|
|
Vector calibration = K.vector();
|
|
Vector calibration2(3);
|
|
calibration2(0) = calibration(0);
|
|
calibration2(1) = calibration(0);
|
|
calibration2(2) = calibration(1);
|
|
return diag(calibration2);
|
|
|
|
}
|
|
|
|
}
|