gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
EssentialMatrixFactor.h
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
23#include <gtsam/geometry/EssentialMatrix.h>
26
27#include <iostream>
28
29namespace gtsam {
30
34class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
35 Vector3 vA_, vB_;
36
39
40 public:
49 EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
50 const SharedNoiseModel& model)
51 : Base(model, key) {
54 }
55
65 template <class CALIBRATION>
66 EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
67 const SharedNoiseModel& model,
68 boost::shared_ptr<CALIBRATION> K)
69 : Base(model, key) {
70 assert(K);
71 vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA));
72 vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB));
73 }
74
76 gtsam::NonlinearFactor::shared_ptr clone() const override {
77 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
78 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
79 }
80
82 void print(
83 const std::string& s = "",
84 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
85 Base::print(s);
86 std::cout << " EssentialMatrixFactor with measurements\n ("
87 << vA_.transpose() << ")' and (" << vB_.transpose() << ")'"
88 << std::endl;
89 }
90
93 const EssentialMatrix& E,
94 boost::optional<Matrix&> H = boost::none) const override {
95 Vector error(1);
96 error << E.error(vA_, vB_, H);
97 return error;
98 }
99
100 public:
102};
103
109 : public NoiseModelFactorN<EssentialMatrix, double> {
110 Point3 dP1_;
111 Point2 pn_;
112 double f_;
113
116
117 public:
126 EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
127 const SharedNoiseModel& model)
128 : Base(model, key1, key2),
129 dP1_(EssentialMatrix::Homogeneous(pA)),
130 pn_(pB) {
131 f_ = 1.0;
132 }
133
143 template <class CALIBRATION>
144 EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
145 const SharedNoiseModel& model,
146 boost::shared_ptr<CALIBRATION> K)
147 : Base(model, key1, key2),
148 dP1_(EssentialMatrix::Homogeneous(K->calibrate(pA))),
149 pn_(K->calibrate(pB)) {
150 f_ = 0.5 * (K->fx() + K->fy());
151 }
152
154 gtsam::NonlinearFactor::shared_ptr clone() const override {
155 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
156 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
157 }
158
160 void print(
161 const std::string& s = "",
162 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
163 Base::print(s);
164 std::cout << " EssentialMatrixFactor2 with measurements\n ("
165 << dP1_.transpose() << ")' and (" << pn_.transpose() << ")'"
166 << std::endl;
167 }
168
169 /*
170 * Vector of errors returns 2D vector
171 * @param E essential matrix
172 * @param d inverse depth d
173 */
174 Vector evaluateError(
175 const EssentialMatrix& E, const double& d,
176 boost::optional<Matrix&> DE = boost::none,
177 boost::optional<Matrix&> Dd = boost::none) const override {
178 // We have point x,y in image 1
179 // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
180 // We then convert to second camera by P2 = 1R2'*(P1-1T2)
181 // The homogeneous coordinates of can be written as
182 // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2)
183 // where we multiplied with d which yields equivalent homogeneous
184 // coordinates. Note that this is just the homography 2R1 for d==0 The point
185 // d*P1 = (x,y,1) is computed in constructor as dP1_
186
187 // Project to normalized image coordinates, then uncalibrate
188 Point2 pn(0, 0);
189 if (!DE && !Dd) {
190 Point3 _1T2 = E.direction().point3();
191 Point3 d1T2 = d * _1T2;
192 Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2)
193 pn = PinholeBase::Project(dP2);
194
195 } else {
196 // Calculate derivatives. TODO if slow: optimize with Mathematica
197 // 3*2 3*3 3*3
198 Matrix D_1T2_dir, DdP2_rot, DP2_point;
199
200 Point3 _1T2 = E.direction().point3(D_1T2_dir);
201 Point3 d1T2 = d * _1T2;
202 Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
203
204 Matrix23 Dpn_dP2;
205 pn = PinholeBase::Project(dP2, Dpn_dP2);
206
207 if (DE) {
208 Matrix DdP2_E(3, 5);
209 DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2)
210 *DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5)
211 }
212
213 if (Dd) // efficient backwards computation:
214 // (2*3) * (3*3) * (3*1)
215 *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2));
216 }
217 Point2 reprojectionError = pn - pn_;
218 return f_ * reprojectionError;
219 }
220
221 public:
223};
224// EssentialMatrixFactor2
225
234
235 Rot3 cRb_;
236
237 public:
247 EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
248 const Rot3& cRb, const SharedNoiseModel& model)
249 : EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) {}
250
260 template <class CALIBRATION>
261 EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
262 const Rot3& cRb, const SharedNoiseModel& model,
263 boost::shared_ptr<CALIBRATION> K)
264 : EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {}
265
267 gtsam::NonlinearFactor::shared_ptr clone() const override {
268 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
269 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
270 }
271
273 void print(
274 const std::string& s = "",
275 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
276 Base::print(s);
277 std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl;
278 }
279
280 /*
281 * Vector of errors returns 2D vector
282 * @param E essential matrix
283 * @param d inverse depth d
284 */
285 Vector evaluateError(
286 const EssentialMatrix& E, const double& d,
287 boost::optional<Matrix&> DE = boost::none,
288 boost::optional<Matrix&> Dd = boost::none) const override {
289 if (!DE) {
290 // Convert E from body to camera frame
291 EssentialMatrix cameraE = cRb_ * E;
292 // Evaluate error
293 return Base::evaluateError(cameraE, d, boost::none, Dd);
294 } else {
295 // Version with derivatives
296 Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5
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; // (2*5) * (5*5)
300 return e;
301 }
302 }
303
304 public:
306};
307// EssentialMatrixFactor3
308
322template <class CALIBRATION>
324 : public NoiseModelFactorN<EssentialMatrix, CALIBRATION> {
325 private:
326 Point2 pA_, pB_;
327
330
331 static constexpr int DimK = FixedDimension<CALIBRATION>::value;
332 typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
333
334 public:
344 EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB,
345 const SharedNoiseModel& model)
346 : Base(model, keyE, keyK), pA_(pA), pB_(pB) {}
347
349 gtsam::NonlinearFactor::shared_ptr clone() const override {
350 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
351 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
352 }
353
355 void print(
356 const std::string& s = "",
357 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
358 Base::print(s);
359 std::cout << " EssentialMatrixFactor4 with measurements\n ("
360 << pA_.transpose() << ")' and (" << pB_.transpose() << ")'"
361 << std::endl;
362 }
363
374 const EssentialMatrix& E, const CALIBRATION& K,
375 boost::optional<Matrix&> H1 = boost::none,
376 boost::optional<Matrix&> H2 = boost::none) const override {
377 // converting from pixel coordinates to normalized coordinates cA and cB
378 JacobianCalibration cA_H_K; // dcA/dK
379 JacobianCalibration cB_H_K; // dcB/dK
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);
382
383 // convert to homogeneous coordinates
384 Vector3 vA = EssentialMatrix::Homogeneous(cA);
385 Vector3 vB = EssentialMatrix::Homogeneous(cB);
386
387 if (H2) {
388 // compute the jacobian of error w.r.t K
389
390 // error function f = vA.T * E * vB
391 // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK
392 // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK
393 // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]]
394 *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
395 vA.transpose() * E.matrix().leftCols<2>() * cB_H_K;
396 }
397
398 Vector error(1);
399 error << E.error(vA, vB, H1);
400
401 return error;
402 }
403
404 public:
406};
407// EssentialMatrixFactor4
408
409} // namespace gtsam
#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