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 
42  // Provide access to the Matrix& version of evaluateError:
43  using Base::evaluateError;
44 
54  const SharedNoiseModel& model)
55  : Base(model, key) {
58  }
59 
69  template <class CALIBRATION>
71  const SharedNoiseModel& model,
72  std::shared_ptr<CALIBRATION> K)
73  : Base(model, key) {
74  assert(K);
75  vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA));
76  vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB));
77  }
78 
81  return std::static_pointer_cast<gtsam::NonlinearFactor>(
83  }
84 
86  void print(
87  const std::string& s = "",
88  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
89  Base::print(s);
90  std::cout << " EssentialMatrixFactor with measurements\n ("
91  << vA_.transpose() << ")' and (" << vB_.transpose() << ")'"
92  << std::endl;
93  }
94 
97  const EssentialMatrix& E, OptionalMatrixType H) const override {
98  Vector error(1);
99  error << E.error(vA_, vB_, H);
100  return error;
101  }
102 
103  public:
105 };
106 
112  : public NoiseModelFactorN<EssentialMatrix, double> {
115  double f_;
116 
119 
120  public:
121 
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  */
182  const EssentialMatrix& E, const double& d,
183  OptionalMatrixType DE, 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 
245  // Provide access to the Matrix& version of evaluateError:
246  using Base::evaluateError;
247 
258  const Rot3& cRb, const SharedNoiseModel& model)
260 
270  template <class CALIBRATION>
272  const Rot3& cRb, const SharedNoiseModel& model,
273  std::shared_ptr<CALIBRATION> K)
275 
278  return std::static_pointer_cast<gtsam::NonlinearFactor>(
280  }
281 
283  void print(
284  const std::string& s = "",
285  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
286  Base::print(s);
287  std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl;
288  }
289 
290  /*
291  * Vector of errors returns 2D vector
292  * @param E essential matrix
293  * @param d inverse depth d
294  */
296  const EssentialMatrix& E, const double& d,
297  OptionalMatrixType DE, OptionalMatrixType Dd) const override {
298  if (!DE) {
299  // Convert E from body to camera frame
300  EssentialMatrix cameraE = cRb_ * E;
301  // Evaluate error
302  return Base::evaluateError(cameraE, d, OptionalNone, Dd);
303  } else {
304  // Version with derivatives
305  Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5
306  EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
307  // Using the pointer version of evaluateError since the Base class (EssentialMatrixFactor2)
308  // does not have the matrix reference version of 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 
347  // Provide access to the Matrix& version of evaluateError:
348  using Base::evaluateError;
349 
359  EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB,
360  const SharedNoiseModel& model)
361  : Base(model, keyE, keyK), pA_(pA), pB_(pB) {}
362 
365  return std::static_pointer_cast<gtsam::NonlinearFactor>(
367  }
368 
370  void print(
371  const std::string& s = "",
372  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
373  Base::print(s);
374  std::cout << " EssentialMatrixFactor4 with measurements\n ("
375  << pA_.transpose() << ")' and (" << pB_.transpose() << ")'"
376  << std::endl;
377  }
378 
389  const EssentialMatrix& E, const CALIBRATION& K,
390  OptionalMatrixType H1, OptionalMatrixType H2) const override {
391  // converting from pixel coordinates to normalized coordinates cA and cB
392  JacobianCalibration cA_H_K; // dcA/dK
393  JacobianCalibration cB_H_K; // dcB/dK
394  Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, OptionalNone);
395  Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, OptionalNone);
396 
397  // convert to homogeneous coordinates
400 
401  if (H2) {
402  // compute the jacobian of error w.r.t K
403 
404  // error function f = vA.T * E * vB
405  // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK
406  // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK
407  // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]]
408  *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
409  vA.transpose() * E.matrix().leftCols<2>() * cB_H_K;
410  }
411 
412  Vector error(1);
413  error << E.error(vA, vB, H1);
414 
415  return error;
416  }
417 
418  public:
420 };
421 // EssentialMatrixFactor4
422 
423 } // namespace gtsam
gtsam::EssentialMatrixFactor4::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: EssentialMatrixFactor.h:370
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:78
EssentialMatrix.h
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::EssentialMatrixFactor::evaluateError
Vector evaluateError(const EssentialMatrix &E, OptionalMatrixType H) const override
vector of errors returns 1D vector
Definition: EssentialMatrixFactor.h:96
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:136
gtsam::EssentialMatrixFactor3::EssentialMatrixFactor3
EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const Rot3 &cRb, const SharedNoiseModel &model)
Definition: EssentialMatrixFactor.h:257
gtsam::NoiseModelFactorN< EssentialMatrix >::evaluateError
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0
asiaCPTs::pA
ADT pA
Definition: testAlgebraicDecisionTree.cpp:153
gtsam::EssentialMatrixFactor::vA_
Vector3 vA_
Definition: EssentialMatrixFactor.h:35
gtsam::EssentialMatrixFactor3
Definition: EssentialMatrixFactor.h:237
gtsam::EssentialMatrixFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: EssentialMatrixFactor.h:80
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:43
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:38
gtsam::EssentialMatrixFactor2::pn_
Point2 pn_
Measurement in image 2, in ideal coordinates.
Definition: EssentialMatrixFactor.h:114
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:364
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::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:271
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:49
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:431
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactor.cpp:72
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::EssentialMatrixFactor2::Base
NoiseModelFactorN< EssentialMatrix, double > Base
Definition: EssentialMatrixFactor.h:117
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:731
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:735
gtsam::EssentialMatrixFactor::EssentialMatrixFactor
EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, std::shared_ptr< CALIBRATION > K)
Definition: EssentialMatrixFactor.h:70
gtsam::EssentialMatrixFactor4::Base
NoiseModelFactorN< EssentialMatrix, CALIBRATION > Base
Definition: EssentialMatrixFactor.h:339
gtsam
traits
Definition: SFMdata.h:40
K
#define K
Definition: igam.h:8
gtsam::EssentialMatrixFactor2
Definition: EssentialMatrixFactor.h:111
gtsam::EssentialMatrixFactor4::EssentialMatrixFactor4
EssentialMatrixFactor4(Key keyE, Key keyK, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Definition: EssentialMatrixFactor.h:359
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::EssentialMatrixFactor2::f_
double f_
approximate conversion factor for error scaling
Definition: EssentialMatrixFactor.h:115
gtsam::EssentialMatrixFactor4::pA_
Point2 pA_
Definition: EssentialMatrixFactor.h:337
gtsam::EssentialMatrixFactor3::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: EssentialMatrixFactor.h:277
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:55
gtsam::EssentialMatrixFactor3::evaluateError
Vector evaluateError(const EssentialMatrix &E, const double &d, OptionalMatrixType DE, OptionalMatrixType Dd) const override
Definition: EssentialMatrixFactor.h:295
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:582
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::EssentialMatrixFactor4::evaluateError
Vector evaluateError(const EssentialMatrix &E, const CALIBRATION &K, OptionalMatrixType H1, OptionalMatrixType H2) const override
Calculate the algebraic epipolar error pA' (K^-1)' E K pB.
Definition: EssentialMatrixFactor.h:388
gtsam::EssentialMatrixFactor2::This
EssentialMatrixFactor2 This
Definition: EssentialMatrixFactor.h:118
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:283
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:53
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:113
gtsam::EssentialMatrixFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: EssentialMatrixFactor.h:86


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:30