Loading...
Searching...
No Matches
EssentialMatrixFactor.h
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2014, 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 EssentialMatrixFactor.h
* @brief EssentialMatrixFactor class
* @author Frank Dellaert
* @author Ayush Baid
* @author Akshay Krishnan
* @date December 17, 2013
*/
#pragma once
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <iostream>
namespace gtsam {
class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
Vector3 vA_, vB_;
typedef NoiseModelFactorN<EssentialMatrix> Base;
typedef EssentialMatrixFactor This;
public:
EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
const SharedNoiseModel& model)
: Base(model, key) {
vA_ = EssentialMatrix::Homogeneous(pA);
vB_ = EssentialMatrix::Homogeneous(pB);
}
template <class CALIBRATION>
EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
const SharedNoiseModel& model,
boost::shared_ptr<CALIBRATION> K)
: Base(model, key) {
assert(K);
vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA));
vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB));
}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s);
std::cout << " EssentialMatrixFactor with measurements\n ("
<< vA_.transpose() << ")' and (" << vB_.transpose() << ")'"
<< std::endl;
}
Vector evaluateError(
const EssentialMatrix& E,
boost::optional<Matrix&> H = boost::none) const override {
Vector error(1);
error << E.error(vA_, vB_, H);
return error;
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
class EssentialMatrixFactor2
: public NoiseModelFactorN<EssentialMatrix, double> {
Point3 dP1_;
Point2 pn_;
double f_;
typedef NoiseModelFactorN<EssentialMatrix, double> Base;
typedef EssentialMatrixFactor2 This;
public:
EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
const SharedNoiseModel& model)
: Base(model, key1, key2),
dP1_(EssentialMatrix::Homogeneous(pA)),
pn_(pB) {
f_ = 1.0;
}
template <class CALIBRATION>
EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
const SharedNoiseModel& model,
boost::shared_ptr<CALIBRATION> K)
: Base(model, key1, key2),
dP1_(EssentialMatrix::Homogeneous(K->calibrate(pA))),
pn_(K->calibrate(pB)) {
f_ = 0.5 * (K->fx() + K->fy());
}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s);
std::cout << " EssentialMatrixFactor2 with measurements\n ("
<< dP1_.transpose() << ")' and (" << pn_.transpose() << ")'"
<< std::endl;
}
/*
* Vector of errors returns 2D vector
* @param E essential matrix
* @param d inverse depth d
*/
Vector evaluateError(
const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none,
boost::optional<Matrix&> Dd = boost::none) const override {
// We have point x,y in image 1
// Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
// We then convert to second camera by P2 = 1R2'*(P1-1T2)
// The homogeneous coordinates of can be written as
// 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2)
// where we multiplied with d which yields equivalent homogeneous
// coordinates. Note that this is just the homography 2R1 for d==0 The point
// d*P1 = (x,y,1) is computed in constructor as dP1_
// Project to normalized image coordinates, then uncalibrate
Point2 pn(0, 0);
if (!DE && !Dd) {
Point3 _1T2 = E.direction().point3();
Point3 d1T2 = d * _1T2;
Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2)
pn = PinholeBase::Project(dP2);
} else {
// Calculate derivatives. TODO if slow: optimize with Mathematica
// 3*2 3*3 3*3
Matrix D_1T2_dir, DdP2_rot, DP2_point;
Point3 _1T2 = E.direction().point3(D_1T2_dir);
Point3 d1T2 = d * _1T2;
Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
Matrix23 Dpn_dP2;
pn = PinholeBase::Project(dP2, Dpn_dP2);
if (DE) {
Matrix DdP2_E(3, 5);
DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2)
*DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5)
}
if (Dd) // efficient backwards computation:
// (2*3) * (3*3) * (3*1)
*Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2));
}
Point2 reprojectionError = pn - pn_;
return f_ * reprojectionError;
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// EssentialMatrixFactor2
class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
typedef EssentialMatrixFactor2 Base;
typedef EssentialMatrixFactor3 This;
Rot3 cRb_;
public:
EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
const Rot3& cRb, const SharedNoiseModel& model)
: EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) {}
template <class CALIBRATION>
EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
const Rot3& cRb, const SharedNoiseModel& model,
boost::shared_ptr<CALIBRATION> K)
: EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s);
std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl;
}
/*
* Vector of errors returns 2D vector
* @param E essential matrix
* @param d inverse depth d
*/
Vector evaluateError(
const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none,
boost::optional<Matrix&> Dd = boost::none) const override {
if (!DE) {
// Convert E from body to camera frame
EssentialMatrix cameraE = cRb_ * E;
// Evaluate error
return Base::evaluateError(cameraE, d, boost::none, Dd);
} else {
// Version with derivatives
Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5
EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd);
*DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5)
return e;
}
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// EssentialMatrixFactor3
template <class CALIBRATION>
class EssentialMatrixFactor4
: public NoiseModelFactorN<EssentialMatrix, CALIBRATION> {
private:
Point2 pA_, pB_;
typedef NoiseModelFactorN<EssentialMatrix, CALIBRATION> Base;
typedef EssentialMatrixFactor4 This;
static constexpr int DimK = FixedDimension<CALIBRATION>::value;
typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
public:
EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB,
const SharedNoiseModel& model)
: Base(model, keyE, keyK), pA_(pA), pB_(pB) {}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s);
std::cout << " EssentialMatrixFactor4 with measurements\n ("
<< pA_.transpose() << ")' and (" << pB_.transpose() << ")'"
<< std::endl;
}
Vector evaluateError(
const EssentialMatrix& E, const CALIBRATION& K,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
// converting from pixel coordinates to normalized coordinates cA and cB
JacobianCalibration cA_H_K; // dcA/dK
JacobianCalibration cB_H_K; // dcB/dK
Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, boost::none);
Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, boost::none);
// convert to homogeneous coordinates
Vector3 vA = EssentialMatrix::Homogeneous(cA);
Vector3 vB = EssentialMatrix::Homogeneous(cB);
if (H2) {
// compute the jacobian of error w.r.t K
// error function f = vA.T * E * vB
// H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK
// where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK
// and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]]
*H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
vA.transpose() * E.matrix().leftCols<2>() * cB_H_K;
}
Vector error(1);
error << E.error(vA, vB, H1);
return error;
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// EssentialMatrixFactor4
} // namespace gtsam
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
PinholeCamera.h
Base class for all pinhole cameras.
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::PinholeBase::Project
static Point2 Project(const Point3 &pc, OptionalJacobian< 2, 3 > Dpoint=boost::none)
Project from 3D point in camera coordinates into image Does not throw a CheiralityException,...
Definition CalibratedCamera.cpp:88
gtsam::EssentialMatrix
An essential matrix is like a Pose3, except with translation up to scale It is named after the 3*3 ma...
Definition EssentialMatrix.h:26
gtsam::EssentialMatrix::Homogeneous
static Vector3 Homogeneous(const Point2 &p)
Static function to convert Point2 to homogeneous coordinates.
Definition EssentialMatrix.h:34
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
gtsam::NoiseModelFactorN< EssentialMatrix >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::NoiseModelFactorN< EssentialMatrix >::key
Key key() const
Definition NonlinearFactor.h:518
gtsam::EssentialMatrixFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixFactor.h:76
gtsam::EssentialMatrixFactor::EssentialMatrixFactor
EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixFactor.h:49
gtsam::EssentialMatrixFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition EssentialMatrixFactor.h:82
gtsam::EssentialMatrixFactor::EssentialMatrixFactor
EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K)
Constructor.
Definition EssentialMatrixFactor.h:66
gtsam::EssentialMatrixFactor::evaluateError
Vector evaluateError(const EssentialMatrix &E, boost::optional< Matrix & > H=boost::none) const override
vector of errors returns 1D vector
Definition EssentialMatrixFactor.h:92
gtsam::EssentialMatrixFactor2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixFactor.h:154
gtsam::EssentialMatrixFactor2::EssentialMatrixFactor2
EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K)
Constructor.
Definition EssentialMatrixFactor.h:144
gtsam::EssentialMatrixFactor2::EssentialMatrixFactor2
EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixFactor.h:126
gtsam::EssentialMatrixFactor2::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition EssentialMatrixFactor.h:160
gtsam::EssentialMatrixFactor3::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition EssentialMatrixFactor.h:273
gtsam::EssentialMatrixFactor3::EssentialMatrixFactor3
EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const Rot3 &cRb, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixFactor.h:247
gtsam::EssentialMatrixFactor3::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixFactor.h:267
gtsam::EssentialMatrixFactor3::EssentialMatrixFactor3
EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const Rot3 &cRb, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K)
Constructor.
Definition EssentialMatrixFactor.h:261
gtsam::EssentialMatrixFactor4::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition EssentialMatrixFactor.h:355
gtsam::EssentialMatrixFactor4::evaluateError
Vector evaluateError(const EssentialMatrix &E, const CALIBRATION &K, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Calculate the algebraic epipolar error pA' (K^-1)' E K pB.
Definition EssentialMatrixFactor.h:373
gtsam::EssentialMatrixFactor4::EssentialMatrixFactor4
EssentialMatrixFactor4(Key keyE, Key keyK, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixFactor.h:344
gtsam::EssentialMatrixFactor4::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixFactor.h:349