gtsam  4.1.0
gtsam
EssentialMatrixFactor.h
1 /*
2  * @file EssentialMatrixFactor.cpp
3  * @brief EssentialMatrixFactor class
4  * @author Frank Dellaert
5  * @date December 17, 2013
6  */
7 
8 #pragma once
9 
11 #include <gtsam/geometry/EssentialMatrix.h>
13 #include <iostream>
14 
15 namespace gtsam {
16 
20 class EssentialMatrixFactor: public NoiseModelFactor1<EssentialMatrix> {
21 
22  Vector3 vA_, vB_;
23 
26 
27 public:
28 
36  EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
37  const SharedNoiseModel& model) :
38  Base(model, key) {
41  }
42 
51  template<class CALIBRATION>
52  EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
53  const SharedNoiseModel& model, boost::shared_ptr<CALIBRATION> K) :
54  Base(model, key) {
55  assert(K);
56  vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA));
57  vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB));
58  }
59 
61  gtsam::NonlinearFactor::shared_ptr clone() const override {
62  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
63  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
64  }
65 
67  void print(const std::string& s = "",
68  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
69  Base::print(s);
70  std::cout << " EssentialMatrixFactor with measurements\n ("
71  << vA_.transpose() << ")' and (" << vB_.transpose() << ")'"
72  << std::endl;
73  }
74 
76  Vector evaluateError(const EssentialMatrix& E, boost::optional<Matrix&> H =
77  boost::none) const override {
78  Vector error(1);
79  error << E.error(vA_, vB_, H);
80  return error;
81  }
82 
83 public:
85 };
86 
91 class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix, double> {
92 
93  Point3 dP1_;
94  Point2 pn_;
95  double f_;
96 
99 
100 public:
101 
110  EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
111  const SharedNoiseModel& model) :
112  Base(model, key1, key2), dP1_(EssentialMatrix::Homogeneous(pA)), pn_(pB) {
113  f_ = 1.0;
114  }
115 
125  template<class CALIBRATION>
126  EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
127  const SharedNoiseModel& model, boost::shared_ptr<CALIBRATION> K) :
128  Base(model, key1, key2), dP1_(
129  EssentialMatrix::Homogeneous(K->calibrate(pA))), pn_(K->calibrate(pB)) {
130  f_ = 0.5 * (K->fx() + K->fy());
131  }
132 
134  gtsam::NonlinearFactor::shared_ptr clone() const override {
135  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
136  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
137  }
138 
140  void print(const std::string& s = "",
141  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
142  Base::print(s);
143  std::cout << " EssentialMatrixFactor2 with measurements\n ("
144  << dP1_.transpose() << ")' and (" << pn_.transpose()
145  << ")'" << std::endl;
146  }
147 
148  /*
149  * Vector of errors returns 2D vector
150  * @param E essential matrix
151  * @param d inverse depth d
152  */
153  Vector evaluateError(const EssentialMatrix& E, const double& d,
154  boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
155  boost::none) const override {
156 
157  // We have point x,y in image 1
158  // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
159  // We then convert to second camera by P2 = 1R2'*(P1-1T2)
160  // The homogeneous coordinates of can be written as
161  // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2)
162  // where we multiplied with d which yields equivalent homogeneous coordinates.
163  // Note that this is just the homography 2R1 for d==0
164  // The point d*P1 = (x,y,1) is computed in constructor as dP1_
165 
166  // Project to normalized image coordinates, then uncalibrate
167  Point2 pn(0,0);
168  if (!DE && !Dd) {
169 
170  Point3 _1T2 = E.direction().point3();
171  Point3 d1T2 = d * _1T2;
172  Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2)
173  pn = PinholeBase::Project(dP2);
174 
175  } else {
176 
177  // Calculate derivatives. TODO if slow: optimize with Mathematica
178  // 3*2 3*3 3*3
179  Matrix D_1T2_dir, DdP2_rot, DP2_point;
180 
181  Point3 _1T2 = E.direction().point3(D_1T2_dir);
182  Point3 d1T2 = d * _1T2;
183  Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
184 
185  Matrix23 Dpn_dP2;
186  pn = PinholeBase::Project(dP2, Dpn_dP2);
187 
188  if (DE) {
189  Matrix DdP2_E(3, 5);
190  DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2)
191  *DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5)
192  }
193 
194  if (Dd) // efficient backwards computation:
195  // (2*3) * (3*3) * (3*1)
196  *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2));
197 
198  }
199  Point2 reprojectionError = pn - pn_;
200  return f_ * reprojectionError;
201  }
202 
203 public:
205 };
206 // EssentialMatrixFactor2
207 
214 
217 
218  Rot3 cRb_;
219 
220 public:
221 
231  EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
232  const Rot3& cRb, const SharedNoiseModel& model) :
233  EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) {
234  }
235 
245  template<class CALIBRATION>
246  EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
247  const Rot3& cRb, const SharedNoiseModel& model,
248  boost::shared_ptr<CALIBRATION> K) :
249  EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {
250  }
251 
253  gtsam::NonlinearFactor::shared_ptr clone() const override {
254  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
255  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
256  }
257 
259  void print(const std::string& s = "",
260  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
261  Base::print(s);
262  std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl;
263  }
264 
265  /*
266  * Vector of errors returns 2D vector
267  * @param E essential matrix
268  * @param d inverse depth d
269  */
270  Vector evaluateError(const EssentialMatrix& E, const double& d,
271  boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
272  boost::none) const override {
273  if (!DE) {
274  // Convert E from body to camera frame
275  EssentialMatrix cameraE = cRb_ * E;
276  // Evaluate error
277  return Base::evaluateError(cameraE, d, boost::none, Dd);
278  } else {
279  // Version with derivatives
280  Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5
281  EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
282  Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd);
283  *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5)
284  return e;
285  }
286  }
287 
288 public:
290 };
291 // EssentialMatrixFactor3
292 
293 }// gtsam
294 
gtsam::EssentialMatrixFactor2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: EssentialMatrixFactor.h:134
PinholeCamera.h
Base class for all pinhole cameras.
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:76
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:63
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::NoiseModelFactor1
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:271
gtsam::EssentialMatrixFactor3
Binary factor that optimizes for E and inverse depth d: assumes measurement in image 2 is perfect,...
Definition: EssentialMatrixFactor.h:213
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::EssentialMatrixFactor2::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: EssentialMatrixFactor.h:140
gtsam::NoiseModelFactor2< EssentialMatrix, double >::key1
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:387
gtsam::EssentialMatrixFactor::EssentialMatrixFactor
EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Constructor.
Definition: EssentialMatrixFactor.h:36
gtsam::EssentialMatrixFactor2
Binary factor that optimizes for E and inverse depth d: assumes measurement in image 2 is perfect,...
Definition: EssentialMatrixFactor.h:91
gtsam::EssentialMatrixFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: EssentialMatrixFactor.h:67
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:246
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::EssentialMatrixFactor2::evaluateError
Vector evaluateError(const EssentialMatrix &E, const double &d, boost::optional< Matrix & > DE=boost::none, boost::optional< Matrix & > Dd=boost::none) const override
Override this method to finish implementing a binary factor.
Definition: EssentialMatrixFactor.h:153
gtsam::EssentialMatrix::Homogeneous
static Vector3 Homogeneous(const Point2 &p)
Static function to convert Point2 to homogeneous coordinates.
Definition: EssentialMatrix.h:34
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:126
gtsam::EssentialMatrixFactor3::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: EssentialMatrixFactor.h:259
NonlinearFactor.h
Non-linear factor base classes.
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::EssentialMatrixFactor3::evaluateError
Vector evaluateError(const EssentialMatrix &E, const double &d, boost::optional< Matrix & > DE=boost::none, boost::optional< Matrix & > Dd=boost::none) const override
Override this method to finish implementing a binary factor.
Definition: EssentialMatrixFactor.h:270
gtsam::EssentialMatrixFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: EssentialMatrixFactor.h:61
gtsam::Rot3
Definition: Rot3.h:59
gtsam::EssentialMatrixFactor3::EssentialMatrixFactor3
EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const Rot3 &cRb, const SharedNoiseModel &model)
Constructor.
Definition: EssentialMatrixFactor.h:231
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
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::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::KeyFormatter
boost::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::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::NoiseModelFactor2
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:355
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:35
gtsam::EssentialMatrixFactor2::EssentialMatrixFactor2
EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Constructor.
Definition: EssentialMatrixFactor.h:110
gtsam::EssentialMatrixFactor
Factor that evaluates epipolar error p'Ep for given essential matrix.
Definition: EssentialMatrixFactor.h:20
gtsam::EssentialMatrixFactor3::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: EssentialMatrixFactor.h:253
gtsam::EssentialMatrixFactor::EssentialMatrixFactor
EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K)
Constructor.
Definition: EssentialMatrixFactor.h:52
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition: NonlinearFactor.cpp:119