113 lines
3.7 KiB
C++
113 lines
3.7 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 CalibratedCamera.cpp
|
|
* @brief Calibrated camera for which only pose is unknown
|
|
* @date Aug 17, 2009
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
#include <gtsam/geometry/Pose2.h>
|
|
#include <gtsam/geometry/CalibratedCamera.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
CalibratedCamera::CalibratedCamera(const Pose3& pose) :
|
|
pose_(pose) {
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
CalibratedCamera::CalibratedCamera(const Vector &v) :
|
|
pose_(Pose3::Expmap(v)) {
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Point2 CalibratedCamera::project_to_camera(const Point3& P,
|
|
boost::optional<Matrix&> H1) {
|
|
if (H1) {
|
|
double d = 1.0 / P.z(), d2 = d * d;
|
|
*H1 = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
|
|
}
|
|
return Point2(P.x() / P.z(), P.y() / P.z());
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Point3 CalibratedCamera::backproject_from_camera(const Point2& p,
|
|
const double scale) {
|
|
return Point3(p.x() * scale, p.y() * scale, scale);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
|
|
double st = sin(pose2.theta()), ct = cos(pose2.theta());
|
|
Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
|
|
Rot3 wRc(x, y, z);
|
|
Point3 t(pose2.x(), pose2.y(), height);
|
|
Pose3 pose3(wRc, t);
|
|
return CalibratedCamera(pose3);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Point2 CalibratedCamera::project(const Point3& point,
|
|
boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) const {
|
|
|
|
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
|
|
Point3 q = pose_.transform_to(point, Dpose, Dpoint);
|
|
#else
|
|
Point3 q = pose_.transform_to(point);
|
|
#endif
|
|
Point2 intrinsic = project_to_camera(q);
|
|
|
|
// Check if point is in front of camera
|
|
if (q.z() <= 0)
|
|
throw CheiralityException();
|
|
|
|
if (Dpose || Dpoint) {
|
|
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
|
|
// just implement chain rule
|
|
Matrix H;
|
|
project_to_camera(q,H);
|
|
if (Dpose) *Dpose = H * (*Dpose);
|
|
if (Dpoint) *Dpoint = H * (*Dpoint);
|
|
#else
|
|
// optimized version, see CalibratedCamera.nb
|
|
const double z = q.z(), d = 1.0 / z;
|
|
const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
|
|
if (Dpose)
|
|
*Dpose = (Matrix(2, 6) << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
|
|
-uv, -u, 0., -d, d * v);
|
|
if (Dpoint) {
|
|
const Matrix R(pose_.rotation().matrix());
|
|
*Dpoint = d
|
|
* (Matrix(2, 3) << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
|
|
R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2),
|
|
R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2));
|
|
}
|
|
#endif
|
|
}
|
|
return intrinsic;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
CalibratedCamera CalibratedCamera::retract(const Vector& d) const {
|
|
return CalibratedCamera(pose().retract(d));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Vector CalibratedCamera::localCoordinates(const CalibratedCamera& T2) const {
|
|
return pose().localCoordinates(T2.pose());
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
}
|