EssentialMatrixFactor.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2014, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
12 /*
13  * @file EssentialMatrixFactor.h
14  * @brief EssentialMatrixFactor class
15  * @author Frank Dellaert
16  * @author Ayush Baid
17  * @author Akshay Krishnan
18  * @date December 17, 2013
19  */
20 
21 #pragma once
22 
26 
27 #include <iostream>
28 
29 namespace gtsam {
30 
34 class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
36 
39 
40  public:
41  // Provide access to the Matrix& version of evaluateError:
42  using Base::evaluateError;
43 
53  const SharedNoiseModel& model)
54  : Base(model, key) {
57  }
58 
68  template <class CALIBRATION>
70  const SharedNoiseModel& model,
71  std::shared_ptr<CALIBRATION> K)
72  : Base(model, key) {
73 #ifndef NDEBUG
74  if (!K) throw;
75 #endif
76  vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA));
77  vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB));
78  }
79 
82  return std::static_pointer_cast<gtsam::NonlinearFactor>(
84  }
85 
87  void print(
88  const std::string& s = "",
89  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
90  Base::print(s);
91  std::cout << " EssentialMatrixFactor with measurements\n ("
92  << vA_.transpose() << ")' and (" << vB_.transpose() << ")'"
93  << std::endl;
94  }
95 
98  OptionalMatrixType H) const override {
99  Vector error(1);
100  error << E.error(vA_, vB_, H);
101  return error;
102  }
103 
104  public:
106 };
107 
113  : public NoiseModelFactorN<EssentialMatrix, double> {
116  double f_;
117 
120 
121  public:
122  // Provide access to the Matrix& version of evaluateError:
123  using Base::evaluateError;
124 
134  const SharedNoiseModel& model)
135  : Base(model, key1, key2),
136  dP1_(EssentialMatrix::Homogeneous(pA)),
137  pn_(pB) {
138  f_ = 1.0;
139  }
140 
150  template <class CALIBRATION>
152  const SharedNoiseModel& model,
153  std::shared_ptr<CALIBRATION> K)
154  : Base(model, key1, key2),
155  dP1_(EssentialMatrix::Homogeneous(K->calibrate(pA))),
156  pn_(K->calibrate(pB)) {
157  f_ = 0.5 * (K->fx() + K->fy());
158  }
159 
162  return std::static_pointer_cast<gtsam::NonlinearFactor>(
164  }
165 
167  void print(
168  const std::string& s = "",
169  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
170  Base::print(s);
171  std::cout << " EssentialMatrixFactor2 with measurements\n ("
172  << dP1_.transpose() << ")' and (" << pn_.transpose() << ")'"
173  << std::endl;
174  }
175 
176  /*
177  * Vector of errors returns 2D vector
178  * @param E essential matrix
179  * @param d inverse depth d
180  */
181  Vector evaluateError(const EssentialMatrix& E, const double& d,
183  OptionalMatrixType Dd) const override {
184  // We have point x,y in image 1
185  // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
186  // We then convert to second camera by P2 = 1R2'*(P1-1T2)
187  // The homogeneous coordinates of can be written as
188  // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2)
189  // where we multiplied with d which yields equivalent homogeneous
190  // coordinates. Note that this is just the homography 2R1 for d==0 The point
191  // d*P1 = (x,y,1) is computed in constructor as dP1_
192 
193  // Project to normalized image coordinates, then uncalibrate
194  Point2 pn(0, 0);
195  if (!DE && !Dd) {
196  Point3 _1T2 = E.direction().point3();
197  Point3 d1T2 = d * _1T2;
198  Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2)
199  pn = PinholeBase::Project(dP2);
200 
201  } else {
202  // Calculate derivatives. TODO if slow: optimize with Mathematica
203  // 3*2 3*3 3*3
204  Matrix D_1T2_dir, DdP2_rot, DP2_point;
205 
206  Point3 _1T2 = E.direction().point3(D_1T2_dir);
207  Point3 d1T2 = d * _1T2;
208  Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
209 
210  Matrix23 Dpn_dP2;
211  pn = PinholeBase::Project(dP2, Dpn_dP2);
212 
213  if (DE) {
214  Matrix DdP2_E(3, 5);
215  DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2)
216  *DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5)
217  }
218 
219  if (Dd) // efficient backwards computation:
220  // (2*3) * (3*3) * (3*1)
221  *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2));
222  }
223  Point2 reprojectionError = pn - pn_;
224  return f_ * reprojectionError;
225  }
226 
227  public:
229 };
230 // EssentialMatrixFactor2
231 
240 
242 
243  public:
244  // Provide access to the Matrix& version of evaluateError:
245  using Base::evaluateError;
246 
257  const Rot3& cRb, const SharedNoiseModel& model)
259 
269  template <class CALIBRATION>
271  const Rot3& cRb, const SharedNoiseModel& model,
272  std::shared_ptr<CALIBRATION> K)
274 
277  return std::static_pointer_cast<gtsam::NonlinearFactor>(
279  }
280 
282  void print(
283  const std::string& s = "",
284  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
285  Base::print(s);
286  std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl;
287  }
288 
289  /*
290  * Vector of errors returns 2D vector
291  * @param E essential matrix
292  * @param d inverse depth d
293  */
294  Vector evaluateError(const EssentialMatrix& E, const double& d,
296  OptionalMatrixType Dd) const override {
297  if (!DE) {
298  // Convert E from body to camera frame
299  EssentialMatrix cameraE = cRb_ * E;
300  // Evaluate error
301  return Base::evaluateError(cameraE, d, OptionalNone, Dd);
302  } else {
303  // Version with derivatives
304  Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5
305  EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
306  // Using the pointer version of evaluateError since the Base class
307  // (EssentialMatrixFactor2) does not have the matrix reference version of
308  // evaluateError
309  Vector e = Base::evaluateError(cameraE, d, &D_e_cameraE, Dd);
310  *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5)
311  return e;
312  }
313  }
314 
315  public:
317 };
318 // EssentialMatrixFactor3
319 
333 template <class CALIBRATION>
335  : public NoiseModelFactorN<EssentialMatrix, CALIBRATION> {
336  private:
338 
341 
342  static constexpr int DimK = FixedDimension<CALIBRATION>::value;
344 
345  public:
346  // Provide access to the Matrix& version of evaluateError:
347  using Base::evaluateError;
348 
358  EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB,
359  const SharedNoiseModel& model = nullptr)
360  : Base(model, keyE, keyK), pA_(pA), pB_(pB) {}
361 
364  return std::static_pointer_cast<gtsam::NonlinearFactor>(
366  }
367 
369  void print(
370  const std::string& s = "",
371  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
372  Base::print(s);
373  std::cout << " EssentialMatrixFactor4 with measurements\n ("
374  << pA_.transpose() << ")' and (" << pB_.transpose() << ")'"
375  << std::endl;
376  }
377 
387  Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& K,
389  OptionalMatrixType HK) const override {
390  // converting from pixel coordinates to normalized coordinates cA and cB
391  JacobianCalibration cA_H_K; // dcA/dK
392  JacobianCalibration cB_H_K; // dcB/dK
393  Point2 cA = K.calibrate(pA_, HK ? &cA_H_K : 0, OptionalNone);
394  Point2 cB = K.calibrate(pB_, HK ? &cB_H_K : 0, OptionalNone);
395 
396  // convert to homogeneous coordinates
399 
400  if (HK) {
401  // compute the jacobian of error w.r.t K
402 
403  // error function f = vA.T * E * vB
404  // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK
405  // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK
406  // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]]
407  *HK = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
408  vA.transpose() * E.matrix().leftCols<2>() * cB_H_K;
409  }
410 
411  Vector error(1);
412  error << E.error(vA, vB, HE);
413 
414  return error;
415  }
416 
417  public:
419 };
420 // EssentialMatrixFactor4
421 
433 template <class CALIBRATION>
435  : public NoiseModelFactorN<EssentialMatrix, CALIBRATION, CALIBRATION> {
436  private:
438 
441 
442  static constexpr int DimK = FixedDimension<CALIBRATION>::value;
444 
445  public:
446  // Provide access to the Matrix& version of evaluateError:
447  using Base::evaluateError;
448 
459  EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2& pA,
460  const Point2& pB,
461  const SharedNoiseModel& model = nullptr)
462  : Base(model, keyE, keyKa, keyKb), pA_(pA), pB_(pB) {}
463 
466  return std::static_pointer_cast<gtsam::NonlinearFactor>(
468  }
469 
471  void print(
472  const std::string& s = "",
473  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
474  Base::print(s);
475  std::cout << " EssentialMatrixFactor5 with measurements\n ("
476  << pA_.transpose() << ")' and (" << pB_.transpose() << ")'"
477  << std::endl;
478  }
479 
491  Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& Ka,
492  const CALIBRATION& Kb, OptionalMatrixType HE,
493  OptionalMatrixType HKa,
494  OptionalMatrixType HKb) const override {
495  // converting from pixel coordinates to normalized coordinates cA and cB
496  JacobianCalibration cA_H_Ka; // dcA/dKa
497  JacobianCalibration cB_H_Kb; // dcB/dKb
498  Point2 cA = Ka.calibrate(pA_, HKa ? &cA_H_Ka : 0, OptionalNone);
499  Point2 cB = Kb.calibrate(pB_, HKb ? &cB_H_Kb : 0, OptionalNone);
500 
501  // Convert to homogeneous coordinates.
504 
505  if (HKa) {
506  // Compute the jacobian of error w.r.t Ka.
507  *HKa = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_Ka;
508  }
509 
510  if (HKb) {
511  // Compute the jacobian of error w.r.t Kb.
512  *HKb = vA.transpose() * E.matrix().leftCols<2>() * cB_H_Kb;
513  }
514 
515  Vector error(1);
516  error << E.error(vA, vB, HE);
517 
518  return error;
519  }
520 
521  public:
523 };
524 // EssentialMatrixFactor5
525 
526 } // namespace gtsam
gtsam::EssentialMatrixFactor4::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: EssentialMatrixFactor.h:369
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:79
EssentialMatrix.h
gtsam::EssentialMatrixFactor5::Base
NoiseModelFactorN< EssentialMatrix, CALIBRATION, CALIBRATION > Base
Definition: EssentialMatrixFactor.h:439
gtsam::EssentialMatrixFactor2::evaluateError
Vector evaluateError(const EssentialMatrix &E, const double &d, OptionalMatrixType DE, OptionalMatrixType Dd) const override
Definition: EssentialMatrixFactor.h:181
gtsam::EssentialMatrixFactor4::DimK
static constexpr int DimK
Definition: EssentialMatrixFactor.h:342
gtsam::EssentialMatrixFactor5::evaluateError
Vector evaluateError(const EssentialMatrix &E, const CALIBRATION &Ka, const CALIBRATION &Kb, OptionalMatrixType HE, OptionalMatrixType HKa, OptionalMatrixType HKb) const override
Calculate the algebraic epipolar error pA' (Ka^-1)' E Kb pB.
Definition: EssentialMatrixFactor.h:491
gtsam::EssentialMatrixFactor::evaluateError
Vector evaluateError(const EssentialMatrix &E, OptionalMatrixType H) const override
vector of errors returns 1D vector
Definition: EssentialMatrixFactor.h:97
gtsam::EssentialMatrixFactor2::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: EssentialMatrixFactor.h:167
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Definition: NonlinearFactor.cpp:138
gtsam::EssentialMatrixFactor3::EssentialMatrixFactor3
EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const Rot3 &cRb, const SharedNoiseModel &model)
Definition: EssentialMatrixFactor.h:256
gtsam::NoiseModelFactorN< EssentialMatrix >::evaluateError
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0
gtsam::EssentialMatrixFactor4::evaluateError
Vector evaluateError(const EssentialMatrix &E, const CALIBRATION &K, OptionalMatrixType HE, OptionalMatrixType HK) const override
Calculate the algebraic epipolar error pA' (K^-1)' E K pB.
Definition: EssentialMatrixFactor.h:387
asiaCPTs::pA
ADT pA
Definition: testAlgebraicDecisionTree.cpp:153
gtsam::EssentialMatrixFactor::vA_
Vector3 vA_
Definition: EssentialMatrixFactor.h:35
gtsam::EssentialMatrixFactor5::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: EssentialMatrixFactor.h:471
gtsam::EssentialMatrixFactor5::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: EssentialMatrixFactor.h:465
gtsam::EssentialMatrixFactor3
Definition: EssentialMatrixFactor.h:237
gtsam::EssentialMatrixFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: EssentialMatrixFactor.h:81
gtsam::EssentialMatrixFactor2::EssentialMatrixFactor2
EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Definition: EssentialMatrixFactor.h:133
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Factor
Definition: Factor.h:70
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
gtsam::EssentialMatrixFactor3::cRb_
Rot3 cRb_
Rotation from body to camera frame.
Definition: EssentialMatrixFactor.h:241
gtsam::EssentialMatrixFactor3::This
EssentialMatrixFactor3 This
Definition: EssentialMatrixFactor.h:239
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::EssentialMatrixFactor4::EssentialMatrixFactor4
EssentialMatrixFactor4(Key keyE, Key keyK, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model=nullptr)
Definition: EssentialMatrixFactor.h:358
gtsam::EssentialMatrixFactor2::pn_
Point2 pn_
Measurement in image 2, in ideal coordinates.
Definition: EssentialMatrixFactor.h:115
example1::vB
Vector vB(size_t i)
Definition: testEssentialMatrixFactor.cpp:52
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::EssentialMatrixFactor4::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: EssentialMatrixFactor.h:363
example1::vA
Vector vA(size_t i)
Definition: testEssentialMatrixFactor.cpp:51
gtsam::EssentialMatrix::Homogeneous
static Vector3 Homogeneous(const Point2 &p)
Static function to convert Point2 to homogeneous coordinates.
Definition: EssentialMatrix.h:34
gtsam::EssentialMatrixFactor5::pB_
Point2 pB_
points in pixel coordinates
Definition: EssentialMatrixFactor.h:437
gtsam::EssentialMatrixFactor3::EssentialMatrixFactor3
EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const Rot3 &cRb, const SharedNoiseModel &model, std::shared_ptr< CALIBRATION > K)
Definition: EssentialMatrixFactor.h:270
gtsam::EssentialMatrixFactor2::EssentialMatrixFactor2
EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, std::shared_ptr< CALIBRATION > K)
Definition: EssentialMatrixFactor.h:151
OptionalNone
#define OptionalNone
Definition: NonlinearFactor.h:50
gtsam::EssentialMatrixFactor4
Definition: EssentialMatrixFactor.h:334
gtsam::EssentialMatrix
Definition: EssentialMatrix.h:26
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
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::NoiseModelFactorN
Definition: NonlinearFactor.h:432
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactor.cpp:74
gtsam::EssentialMatrixFactor5::EssentialMatrixFactor5
EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model=nullptr)
Definition: EssentialMatrixFactor.h:459
gtsam::EssentialMatrixFactor5::This
EssentialMatrixFactor5 This
Definition: EssentialMatrixFactor.h:440
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::EssentialMatrixFactor2::Base
NoiseModelFactorN< EssentialMatrix, double > Base
Definition: EssentialMatrixFactor.h:118
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::EssentialMatrixFactor3::Base
EssentialMatrixFactor2 Base
Definition: EssentialMatrixFactor.h:238
gtsam::NoiseModelFactorN< EssentialMatrix, double >::key1
Key key1() const
Definition: NonlinearFactor.h:732
gtsam::EssentialMatrixFactor::Base
NoiseModelFactorN< EssentialMatrix > Base
Definition: EssentialMatrixFactor.h:37
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::PinholeBase::Project
static Point2 Project(const Point3 &pc, OptionalJacobian< 2, 3 > Dpoint={})
Definition: CalibratedCamera.cpp:88
E
DiscreteKey E(5, 2)
NonlinearFactor.h
Non-linear factor base classes.
gtsam::NoiseModelFactorN< EssentialMatrix, double >::key2
Key key2() const
Definition: NonlinearFactor.h:736
gtsam::EssentialMatrixFactor5
Definition: EssentialMatrixFactor.h:434
gtsam::EssentialMatrixFactor::EssentialMatrixFactor
EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, std::shared_ptr< CALIBRATION > K)
Definition: EssentialMatrixFactor.h:69
gtsam::EssentialMatrixFactor4::Base
NoiseModelFactorN< EssentialMatrix, CALIBRATION > Base
Definition: EssentialMatrixFactor.h:339
gtsam
traits
Definition: SFMdata.h:40
gtsam::EssentialMatrixFactor5::DimK
static constexpr int DimK
Definition: EssentialMatrixFactor.h:442
K
#define K
Definition: igam.h:8
gtsam::EssentialMatrixFactor2
Definition: EssentialMatrixFactor.h:112
gtsam::EssentialMatrixFactor4::JacobianCalibration
Eigen::Matrix< double, 2, DimK > JacobianCalibration
Definition: EssentialMatrixFactor.h:343
gtsam::EssentialMatrixFactor2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: EssentialMatrixFactor.h:161
gtsam::EssentialMatrixFactor5::pA_
Point2 pA_
Definition: EssentialMatrixFactor.h:437
gtsam::EssentialMatrixFactor2::f_
double f_
approximate conversion factor for error scaling
Definition: EssentialMatrixFactor.h:116
gtsam::EssentialMatrixFactor4::pA_
Point2 pA_
Definition: EssentialMatrixFactor.h:337
gtsam::EssentialMatrixFactor3::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: EssentialMatrixFactor.h:276
gtsam::EssentialMatrixFactor
Definition: EssentialMatrixFactor.h:34
gtsam::EssentialMatrixFactor::This
EssentialMatrixFactor This
Definition: EssentialMatrixFactor.h:38
gtsam::EssentialMatrixFactor4::pB_
Point2 pB_
points in pixel coordinates
Definition: EssentialMatrixFactor.h:337
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:56
gtsam::EssentialMatrixFactor3::evaluateError
Vector evaluateError(const EssentialMatrix &E, const double &d, OptionalMatrixType DE, OptionalMatrixType Dd) const override
Definition: EssentialMatrixFactor.h:294
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::NoiseModelFactorN< EssentialMatrix >::key
Key key() const
Definition: NonlinearFactor.h:583
asiaCPTs::pB
ADT pB
Definition: testAlgebraicDecisionTree.cpp:157
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition: Manifold.h:161
gtsam::EssentialMatrixFactor5::JacobianCalibration
Eigen::Matrix< double, 2, DimK > JacobianCalibration
Definition: EssentialMatrixFactor.h:443
gtsam::EssentialMatrixFactor2::This
EssentialMatrixFactor2 This
Definition: EssentialMatrixFactor.h:119
cRb
gtsam::Rot3 cRb
Definition: testEssentialMatrixFactor.cpp:33
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:279
gtsam::EssentialMatrixFactor3::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: EssentialMatrixFactor.h:282
gtsam::EssentialMatrixFactor::vB_
Vector3 vB_
Homogeneous versions, in ideal coordinates.
Definition: EssentialMatrixFactor.h:35
gtsam::EssentialMatrixFactor::EssentialMatrixFactor
EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Definition: EssentialMatrixFactor.h:52
gtsam::EssentialMatrixFactor4::This
EssentialMatrixFactor4 This
Definition: EssentialMatrixFactor.h:340
gtsam::EssentialMatrixFactor2::dP1_
Point3 dP1_
3D point corresponding to measurement in image 1
Definition: EssentialMatrixFactor.h:114
gtsam::EssentialMatrixFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: EssentialMatrixFactor.h:87


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:31