23#include <gtsam/geometry/EssentialMatrix.h>
65 template <
class CALIBRATION>
68 boost::shared_ptr<CALIBRATION> K)
76 gtsam::NonlinearFactor::shared_ptr
clone()
const override {
77 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
78 gtsam::NonlinearFactor::shared_ptr(
new This(*
this)));
83 const std::string& s =
"",
84 const KeyFormatter& keyFormatter = DefaultKeyFormatter)
const override {
86 std::cout <<
" EssentialMatrixFactor with measurements\n ("
87 << vA_.transpose() <<
")' and (" << vB_.transpose() <<
")'"
94 boost::optional<Matrix&> H = boost::none)
const override {
96 error << E.error(vA_, vB_, H);
128 :
Base(model, key1, key2),
143 template <
class CALIBRATION>
146 boost::shared_ptr<CALIBRATION> K)
147 :
Base(model, key1, key2),
149 pn_(K->calibrate(pB)) {
150 f_ = 0.5 * (K->fx() + K->fy());
154 gtsam::NonlinearFactor::shared_ptr
clone()
const override {
155 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
156 gtsam::NonlinearFactor::shared_ptr(
new This(*
this)));
161 const std::string& s =
"",
162 const KeyFormatter& keyFormatter = DefaultKeyFormatter)
const override {
164 std::cout <<
" EssentialMatrixFactor2 with measurements\n ("
165 << dP1_.transpose() <<
")' and (" << pn_.transpose() <<
")'"
174 Vector evaluateError(
176 boost::optional<Matrix&> DE = boost::none,
177 boost::optional<Matrix&> Dd = boost::none)
const override {
190 Point3 _1T2 = E.direction().point3();
192 Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2);
198 Matrix D_1T2_dir, DdP2_rot, DP2_point;
200 Point3 _1T2 = E.direction().point3(D_1T2_dir);
202 Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
209 DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir;
210 *DE = f_ * Dpn_dP2 * DdP2_E;
215 *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2));
217 Point2 reprojectionError = pn - pn_;
218 return f_ * reprojectionError;
260 template <
class CALIBRATION>
263 boost::shared_ptr<CALIBRATION> K)
267 gtsam::NonlinearFactor::shared_ptr
clone()
const override {
268 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
269 gtsam::NonlinearFactor::shared_ptr(
new This(*
this)));
274 const std::string& s =
"",
275 const KeyFormatter& keyFormatter = DefaultKeyFormatter)
const override {
277 std::cout <<
" EssentialMatrixFactor3 with rotation " << cRb_ << std::endl;
285 Vector evaluateError(
287 boost::optional<Matrix&> DE = boost::none,
288 boost::optional<Matrix&> Dd = boost::none)
const override {
293 return Base::evaluateError(cameraE, d, boost::none, Dd);
296 Matrix D_e_cameraE, D_cameraE_E;
297 EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
298 Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd);
299 *DE = D_e_cameraE * D_cameraE_E;
322template <
class CALIBRATION>
332 typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
346 :
Base(model, keyE, keyK), pA_(pA), pB_(pB) {}
349 gtsam::NonlinearFactor::shared_ptr
clone()
const override {
350 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
351 gtsam::NonlinearFactor::shared_ptr(
new This(*
this)));
356 const std::string& s =
"",
357 const KeyFormatter& keyFormatter = DefaultKeyFormatter)
const override {
359 std::cout <<
" EssentialMatrixFactor4 with measurements\n ("
360 << pA_.transpose() <<
")' and (" << pB_.transpose() <<
")'"
375 boost::optional<Matrix&> H1 = boost::none,
376 boost::optional<Matrix&> H2 = boost::none)
const override {
378 JacobianCalibration cA_H_K;
379 JacobianCalibration cB_H_K;
380 Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, boost::none);
381 Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, boost::none);
394 *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
395 vA.transpose() * E.matrix().leftCols<2>() * cB_H_K;
399 error << E.error(vA, vB, H1);
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Base class for all pinhole cameras.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
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
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
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
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
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
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
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
static Vector3 Homogeneous(const Point2 &p)
Static function to convert Point2 to homogeneous coordinates.
Definition EssentialMatrix.h:34
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
Factor that evaluates epipolar error p'Ep for given essential matrix.
Definition EssentialMatrixFactor.h:34
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixFactor.h:76
EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixFactor.h:49
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition EssentialMatrixFactor.h:82
EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K)
Constructor.
Definition EssentialMatrixFactor.h:66
Vector evaluateError(const EssentialMatrix &E, boost::optional< Matrix & > H=boost::none) const override
vector of errors returns 1D vector
Definition EssentialMatrixFactor.h:92
Binary factor that optimizes for E and inverse depth d: assumes measurement in image 2 is perfect,...
Definition EssentialMatrixFactor.h:109
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixFactor.h:154
EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K)
Constructor.
Definition EssentialMatrixFactor.h:144
EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixFactor.h:126
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition EssentialMatrixFactor.h:160
Binary factor that optimizes for E and inverse depth d: assumes measurement in image 2 is perfect,...
Definition EssentialMatrixFactor.h:231
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition EssentialMatrixFactor.h:273
EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const Rot3 &cRb, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixFactor.h:247
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixFactor.h:267
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
Binary factor that optimizes for E and calibration K using the algebraic epipolar error (K^-1 pA)'E (...
Definition EssentialMatrixFactor.h:324
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition EssentialMatrixFactor.h:355
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
EssentialMatrixFactor4(Key keyE, Key keyK, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixFactor.h:344
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixFactor.h:349