Big refactor of PinholeCamera: now derives from PinholeBaseK
dellaert
parent
5ed2abd292
commit
57c921c6cf
|
|
@ -18,32 +18,32 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/CalibratedCamera.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <cmath>
|
||||
#include <gtsam/geometry/PinholePose.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A pinhole camera class that has a Pose3 and a Calibration.
|
||||
* Use PinholePose if you will not be optimizing for Calibration
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<typename Calibration>
|
||||
class PinholeCamera {
|
||||
class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
|
||||
|
||||
private:
|
||||
Pose3 pose_;
|
||||
Calibration K_;
|
||||
|
||||
GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
|
||||
typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
|
||||
Calibration K_; ///< Calibration, part of class now
|
||||
|
||||
// Get dimensions of calibration type at compile time
|
||||
static const int DimK = FixedDimension<Calibration>::value;
|
||||
static const int DimK = FixedDimension<Calibration>::value;
|
||||
|
||||
public:
|
||||
|
||||
enum {dimension = 6 + DimK};
|
||||
enum {
|
||||
dimension = 6 + DimK
|
||||
}; ///< Dimension depends on calibration
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
|
@ -54,12 +54,12 @@ public:
|
|||
|
||||
/** constructor with pose */
|
||||
explicit PinholeCamera(const Pose3& pose) :
|
||||
pose_(pose) {
|
||||
Base(pose) {
|
||||
}
|
||||
|
||||
/** constructor with pose and calibration */
|
||||
PinholeCamera(const Pose3& pose, const Calibration& K) :
|
||||
pose_(pose), K_(K) {
|
||||
Base(pose), K_(K) {
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
|
@ -75,12 +75,7 @@ public:
|
|||
*/
|
||||
static PinholeCamera Level(const Calibration &K, const Pose2& pose2,
|
||||
double height) {
|
||||
const double st = sin(pose2.theta()), ct = cos(pose2.theta());
|
||||
const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
|
||||
const Rot3 wRc(x, y, z);
|
||||
const Point3 t(pose2.x(), pose2.y(), height);
|
||||
const Pose3 pose3(wRc, t);
|
||||
return PinholeCamera(pose3, K);
|
||||
return PinholeCamera(CalibratedCamera::LevelPose(pose2, height), K);
|
||||
}
|
||||
|
||||
/// PinholeCamera::level with default calibration
|
||||
|
|
@ -99,28 +94,23 @@ public:
|
|||
*/
|
||||
static PinholeCamera Lookat(const Point3& eye, const Point3& target,
|
||||
const Point3& upVector, const Calibration& K = Calibration()) {
|
||||
Point3 zc = target - eye;
|
||||
zc = zc / zc.norm();
|
||||
Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
|
||||
xc = xc / xc.norm();
|
||||
Point3 yc = zc.cross(xc);
|
||||
Pose3 pose3(Rot3(xc, yc, zc), eye);
|
||||
return PinholeCamera(pose3, K);
|
||||
return PinholeCamera(CalibratedCamera::LookatPose(eye, target, upVector), K);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
explicit PinholeCamera(const Vector &v) {
|
||||
pose_ = Pose3::Expmap(v.head(6));
|
||||
if (v.size() > 6) {
|
||||
K_ = Calibration(v.tail(DimK));
|
||||
}
|
||||
/// Init from vector, can be 6D (default calibration) or dim
|
||||
explicit PinholeCamera(const Vector &v) :
|
||||
Base(v.head<6>()) {
|
||||
if (v.size() > 6)
|
||||
K_ = Calibration(v.tail<DimK>());
|
||||
}
|
||||
|
||||
/// Init from Vector and calibration
|
||||
PinholeCamera(const Vector &v, const Vector &K) :
|
||||
pose_(Pose3::Expmap(v)), K_(K) {
|
||||
Base(v), K_(K) {
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
|
@ -128,14 +118,14 @@ public:
|
|||
/// @{
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const PinholeCamera &camera, double tol = 1e-9) const {
|
||||
return pose_.equals(camera.pose(), tol)
|
||||
&& K_.equals(camera.calibration(), tol);
|
||||
bool equals(const Base &camera, double tol = 1e-9) const {
|
||||
const PinholeCamera* e = dynamic_cast<const PinholeCamera*>(&camera);
|
||||
return Base::equals(camera, tol) && K_.equals(e->calibration(), tol);
|
||||
}
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "PinholeCamera") const {
|
||||
pose_.print(s + ".pose");
|
||||
Base::print(s);
|
||||
K_.print(s + ".calibration");
|
||||
}
|
||||
|
||||
|
|
@ -147,13 +137,8 @@ public:
|
|||
}
|
||||
|
||||
/// return pose
|
||||
Pose3& pose() {
|
||||
return pose_;
|
||||
}
|
||||
|
||||
/// return pose, constant version
|
||||
const Pose3& pose() const {
|
||||
return pose_;
|
||||
return Base::pose();
|
||||
}
|
||||
|
||||
/// return pose, with derivative
|
||||
|
|
@ -162,12 +147,7 @@ public:
|
|||
H->setZero();
|
||||
H->block(0, 0, 6, 6) = I_6x6;
|
||||
}
|
||||
return pose_;
|
||||
}
|
||||
|
||||
/// return calibration
|
||||
Calibration& calibration() {
|
||||
return K_;
|
||||
return Base::pose();
|
||||
}
|
||||
|
||||
/// return calibration
|
||||
|
|
@ -179,12 +159,12 @@ public:
|
|||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// Manifold dimension
|
||||
/// @deprecated
|
||||
size_t dim() const {
|
||||
return dimension;
|
||||
}
|
||||
|
||||
/// Manifold dimension
|
||||
/// @deprecated
|
||||
static size_t Dim() {
|
||||
return dimension;
|
||||
}
|
||||
|
|
@ -194,17 +174,17 @@ public:
|
|||
/// move a cameras according to d
|
||||
PinholeCamera retract(const Vector& d) const {
|
||||
if ((size_t) d.size() == 6)
|
||||
return PinholeCamera(pose().retract(d), calibration());
|
||||
return PinholeCamera(this->pose().retract(d), calibration());
|
||||
else
|
||||
return PinholeCamera(pose().retract(d.head(6)),
|
||||
calibration().retract(d.tail(calibration().dim())));
|
||||
return PinholeCamera(this->pose().retract(d.head(6)),
|
||||
calibration().retract(d.tail(calibration().dim())));
|
||||
}
|
||||
|
||||
/// return canonical coordinate
|
||||
VectorK6 localCoordinates(const PinholeCamera& T2) const {
|
||||
VectorK6 d;
|
||||
// TODO: why does d.head<6>() not compile??
|
||||
d.head(6) = pose().localCoordinates(T2.pose());
|
||||
d.head(6) = this->pose().localCoordinates(T2.pose());
|
||||
d.tail(DimK) = calibration().localCoordinates(T2.calibration());
|
||||
return d;
|
||||
}
|
||||
|
|
@ -218,32 +198,6 @@ public:
|
|||
/// @name Transformations and measurement functions
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* projects a 3-dimensional point in camera coordinates into the
|
||||
* camera and returns a 2-dimensional point, no calibration applied
|
||||
* @param P A point in camera coordinates
|
||||
* @param Dpoint is the 2*3 Jacobian w.r.t. P
|
||||
*/
|
||||
static Point2 project_to_camera(const Point3& P, //
|
||||
OptionalJacobian<2, 3> Dpoint = boost::none) {
|
||||
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
|
||||
if (P.z() <= 0)
|
||||
throw CheiralityException();
|
||||
#endif
|
||||
double d = 1.0 / P.z();
|
||||
const double u = P.x() * d, v = P.y() * d;
|
||||
if (Dpoint)
|
||||
*Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
|
||||
return Point2(u, v);
|
||||
}
|
||||
|
||||
/// Project a point into the image and check depth
|
||||
std::pair<Point2, bool> projectSafe(const Point3& pw) const {
|
||||
const Point3 pc = pose_.transform_to(pw);
|
||||
const Point2 pn = project_to_camera(pc);
|
||||
return std::make_pair(K_.uncalibrate(pn), pc.z() > 0);
|
||||
}
|
||||
|
||||
typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
|
||||
|
||||
/** project a point from world coordinate to the image
|
||||
|
|
@ -252,31 +206,25 @@ public:
|
|||
* @param Dpoint is the Jacobian w.r.t. point3
|
||||
* @param Dcal is the Jacobian w.r.t. calibration
|
||||
*/
|
||||
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose =
|
||||
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none,
|
||||
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
|
||||
OptionalJacobian<2, 3> Dpoint = boost::none,
|
||||
OptionalJacobian<2, DimK> Dcal = boost::none) const {
|
||||
|
||||
// Transform to camera coordinates and check cheirality
|
||||
const Point3 pc = pose_.transform_to(pw);
|
||||
// project to normalized coordinates
|
||||
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
|
||||
|
||||
// Project to normalized image coordinates
|
||||
const Point2 pn = project_to_camera(pc);
|
||||
// uncalibrate to pixel coordinates
|
||||
Matrix2 Dpi_pn;
|
||||
const Point2 pi = calibration().uncalibrate(pn, Dcal,
|
||||
Dpose || Dpoint ? &Dpi_pn : 0);
|
||||
|
||||
if (Dpose || Dpoint) {
|
||||
const double z = pc.z(), d = 1.0 / z;
|
||||
// If needed, apply chain rule
|
||||
if (Dpose)
|
||||
*Dpose = Dpi_pn * *Dpose;
|
||||
if (Dpoint)
|
||||
*Dpoint = Dpi_pn * *Dpoint;
|
||||
|
||||
// uncalibration
|
||||
Matrix2 Dpi_pn;
|
||||
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
|
||||
|
||||
// chain the Jacobian matrices
|
||||
if (Dpose)
|
||||
calculateDpose(pn, d, Dpi_pn, *Dpose);
|
||||
if (Dpoint)
|
||||
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
|
||||
return pi;
|
||||
} else
|
||||
return K_.uncalibrate(pn, Dcal);
|
||||
return pi;
|
||||
}
|
||||
|
||||
/** project a point at infinity from world coordinate to the image
|
||||
|
|
@ -285,149 +233,114 @@ public:
|
|||
* @param Dpoint is the Jacobian w.r.t. point3
|
||||
* @param Dcal is the Jacobian w.r.t. calibration
|
||||
*/
|
||||
Point2 projectPointAtInfinity(const Point3& pw,
|
||||
OptionalJacobian<2, 6> Dpose = boost::none,
|
||||
OptionalJacobian<2, 2> Dpoint = boost::none,
|
||||
Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose =
|
||||
boost::none, OptionalJacobian<2, 2> Dpoint = boost::none,
|
||||
OptionalJacobian<2, DimK> Dcal = boost::none) const {
|
||||
|
||||
if (!Dpose && !Dpoint && !Dcal) {
|
||||
const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
|
||||
const Point2 pn = project_to_camera(pc);// project the point to the camera
|
||||
const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
|
||||
const Point2 pn = Base::project_to_camera(pc); // project the point to the camera
|
||||
return K_.uncalibrate(pn);
|
||||
}
|
||||
|
||||
// world to camera coordinate
|
||||
Matrix3 Dpc_rot, Dpc_point;
|
||||
const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point);
|
||||
const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point);
|
||||
|
||||
Matrix36 Dpc_pose;
|
||||
Dpc_pose.setZero();
|
||||
Dpc_pose.leftCols<3>() = Dpc_rot;
|
||||
|
||||
// camera to normalized image coordinate
|
||||
Matrix23 Dpn_pc;// 2*3
|
||||
const Point2 pn = project_to_camera(pc, Dpn_pc);
|
||||
Matrix23 Dpn_pc; // 2*3
|
||||
const Point2 pn = Base::project_to_camera(pc, Dpn_pc);
|
||||
|
||||
// uncalibration
|
||||
Matrix2 Dpi_pn;// 2*2
|
||||
Matrix2 Dpi_pn; // 2*2
|
||||
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
|
||||
|
||||
// chain the Jacobian matrices
|
||||
const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc;
|
||||
if (Dpose)
|
||||
*Dpose = Dpi_pc * Dpc_pose;
|
||||
*Dpose = Dpi_pc * Dpc_pose;
|
||||
if (Dpoint)
|
||||
*Dpoint = (Dpi_pc * Dpc_point).leftCols<2>();// only 2dof are important for the point (direction-only)
|
||||
*Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only)
|
||||
return pi;
|
||||
}
|
||||
|
||||
/** project a point from world coordinate to the image, fixed Jacobians
|
||||
* @param pw is a point in the world coordinate
|
||||
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
|
||||
* @param Dpoint is the Jacobian w.r.t. point3
|
||||
*/
|
||||
Point2 project2(
|
||||
const Point3& pw, //
|
||||
OptionalJacobian<2, dimension> Dcamera = boost::none,
|
||||
OptionalJacobian<2, 3> Dpoint = boost::none) const {
|
||||
|
||||
const Point3 pc = pose_.transform_to(pw);
|
||||
const Point2 pn = project_to_camera(pc);
|
||||
// project to normalized coordinates
|
||||
Matrix26 Dpose;
|
||||
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
|
||||
|
||||
if (!Dcamera && !Dpoint) {
|
||||
return K_.uncalibrate(pn);
|
||||
} else {
|
||||
const double z = pc.z(), d = 1.0 / z;
|
||||
// uncalibrate to pixel coordinates
|
||||
Matrix2K Dcal;
|
||||
Matrix2 Dpi_pn;
|
||||
const Point2 pi = calibration().uncalibrate(pn, Dcamera ? &Dcal : 0,
|
||||
Dcamera || Dpoint ? &Dpi_pn : 0);
|
||||
|
||||
// uncalibration
|
||||
Matrix2K Dcal;
|
||||
Matrix2 Dpi_pn;
|
||||
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
|
||||
// If needed, calculate derivatives
|
||||
if (Dcamera)
|
||||
*Dcamera << Dpi_pn * Dpose, Dcal;
|
||||
if (Dpoint)
|
||||
*Dpoint = Dpi_pn * (*Dpoint);
|
||||
|
||||
if (Dcamera) { // TODO why does leftCols<6>() not compile ??
|
||||
calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6));
|
||||
(*Dcamera).rightCols(DimK) = Dcal;// Jacobian wrt calib
|
||||
}
|
||||
if (Dpoint) {
|
||||
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
|
||||
}
|
||||
return pi;
|
||||
}
|
||||
}
|
||||
|
||||
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
|
||||
Point3 backproject(const Point2& p, double depth) const {
|
||||
const Point2 pn = K_.calibrate(p);
|
||||
const Point3 pc(pn.x() * depth, pn.y() * depth, depth);
|
||||
return pose_.transform_from(pc);
|
||||
}
|
||||
|
||||
/// backproject a 2-dimensional point to a 3-dimensional point at infinity
|
||||
Point3 backprojectPointAtInfinity(const Point2& p) const {
|
||||
const Point2 pn = K_.calibrate(p);
|
||||
const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
|
||||
return pose_.rotation().rotate(pc);
|
||||
return pi;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate range to a landmark
|
||||
* @param point 3D location of landmark
|
||||
* @param Dcamera the optionally computed Jacobian with respect to pose
|
||||
* @param Dpoint the optionally computed Jacobian with respect to the landmark
|
||||
* @return range (double)
|
||||
*/
|
||||
double range(
|
||||
const Point3& point, //
|
||||
OptionalJacobian<1, dimension> Dcamera = boost::none,
|
||||
OptionalJacobian<1, 3> Dpoint = boost::none) const {
|
||||
double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera =
|
||||
boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const {
|
||||
Matrix16 Dpose_;
|
||||
double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint);
|
||||
double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint);
|
||||
if (Dcamera)
|
||||
*Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
|
||||
*Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate range to another pose
|
||||
* @param pose Other SO(3) pose
|
||||
* @param Dcamera the optionally computed Jacobian with respect to pose
|
||||
* @param Dpose2 the optionally computed Jacobian with respect to the other pose
|
||||
* @return range (double)
|
||||
*/
|
||||
double range(
|
||||
const Pose3& pose, //
|
||||
OptionalJacobian<1, dimension> Dcamera = boost::none,
|
||||
OptionalJacobian<1, 6> Dpose = boost::none) const {
|
||||
double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera =
|
||||
boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const {
|
||||
Matrix16 Dpose_;
|
||||
double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose);
|
||||
double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose);
|
||||
if (Dcamera)
|
||||
*Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
|
||||
*Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate range to another camera
|
||||
* @param camera Other camera
|
||||
* @param Dcamera the optionally computed Jacobian with respect to pose
|
||||
* @param Dother the optionally computed Jacobian with respect to the other camera
|
||||
* @return range (double)
|
||||
*/
|
||||
template<class CalibrationB>
|
||||
double range(
|
||||
const PinholeCamera<CalibrationB>& camera, //
|
||||
double range(const PinholeCamera<CalibrationB>& camera,
|
||||
OptionalJacobian<1, dimension> Dcamera = boost::none,
|
||||
// OptionalJacobian<1, 6 + traits::dimension<CalibrationB>::value> Dother =
|
||||
boost::optional<Matrix&> Dother =
|
||||
boost::none) const {
|
||||
boost::optional<Matrix&> Dother = boost::none) const {
|
||||
Matrix16 Dcamera_, Dother_;
|
||||
double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0,
|
||||
double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0,
|
||||
Dother ? &Dother_ : 0);
|
||||
if (Dcamera) {
|
||||
Dcamera->resize(1, 6 + DimK);
|
||||
*Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
|
||||
}
|
||||
if (Dother) {
|
||||
Dother->resize(1, 6+CalibrationB::dimension);
|
||||
Dother->resize(1, 6 + CalibrationB::dimension);
|
||||
Dother->setZero();
|
||||
Dother->block(0, 0, 1, 6) = Dother_;
|
||||
}
|
||||
|
|
@ -437,12 +350,9 @@ public:
|
|||
/**
|
||||
* Calculate range to another camera
|
||||
* @param camera Other camera
|
||||
* @param Dcamera the optionally computed Jacobian with respect to pose
|
||||
* @param Dother the optionally computed Jacobian with respect to the other camera
|
||||
* @return range (double)
|
||||
*/
|
||||
double range(
|
||||
const CalibratedCamera& camera, //
|
||||
double range(const CalibratedCamera& camera,
|
||||
OptionalJacobian<1, dimension> Dcamera = boost::none,
|
||||
OptionalJacobian<1, 6> Dother = boost::none) const {
|
||||
return range(camera.pose(), Dcamera, Dother);
|
||||
|
|
@ -450,55 +360,13 @@ public:
|
|||
|
||||
private:
|
||||
|
||||
/**
|
||||
* Calculate Jacobian with respect to pose
|
||||
* @param pn projection in normalized coordinates
|
||||
* @param d disparity (inverse depth)
|
||||
* @param Dpi_pn derivative of uncalibrate with respect to pn
|
||||
* @param Dpose Output argument, can be matrix or block, assumed right size !
|
||||
* See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
|
||||
*/
|
||||
template<typename Derived>
|
||||
static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn,
|
||||
Eigen::MatrixBase<Derived> const & Dpose) {
|
||||
// optimized version of derivatives, see CalibratedCamera.nb
|
||||
const double u = pn.x(), v = pn.y();
|
||||
double uv = u * v, uu = u * u, vv = v * v;
|
||||
Matrix26 Dpn_pose;
|
||||
Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
|
||||
assert(Dpose.rows()==2 && Dpose.cols()==6);
|
||||
const_cast<Eigen::MatrixBase<Derived>&>(Dpose) =//
|
||||
Dpi_pn * Dpn_pose;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate Jacobian with respect to point
|
||||
* @param pn projection in normalized coordinates
|
||||
* @param d disparity (inverse depth)
|
||||
* @param Dpi_pn derivative of uncalibrate with respect to pn
|
||||
* @param Dpoint Output argument, can be matrix or block, assumed right size !
|
||||
* See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
|
||||
*/
|
||||
template<typename Derived>
|
||||
static void calculateDpoint(const Point2& pn, double d, const Matrix3& R,
|
||||
const Matrix2& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) {
|
||||
// optimized version of derivatives, see CalibratedCamera.nb
|
||||
const double u = pn.x(), v = pn.y();
|
||||
Matrix23 Dpn_point;
|
||||
Dpn_point <<//
|
||||
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);
|
||||
Dpn_point *= d;
|
||||
assert(Dpoint.rows()==2 && Dpoint.cols()==3);
|
||||
const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) =//
|
||||
Dpi_pn * Dpn_point;
|
||||
}
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(pose_);
|
||||
ar
|
||||
& boost::serialization::make_nvp("PinholeBaseK",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(K_);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@
|
|||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue