gtsam 4.2.0
gtsam
GeneralSFMFactor.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, 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
21#pragma once
22
30#include <gtsam/base/concepts.h>
31#include <gtsam/base/Manifold.h>
32#include <gtsam/base/Matrix.h>
34#include <gtsam/base/types.h>
35#include <gtsam/base/Testable.h>
36#include <gtsam/base/Vector.h>
37#include <gtsam/base/timing.h>
38
39#include <boost/none.hpp>
40#include <boost/optional/optional.hpp>
41#include <boost/serialization/nvp.hpp>
42#include <boost/smart_ptr/shared_ptr.hpp>
43#include <iostream>
44#include <string>
45
46namespace boost {
47namespace serialization {
48class access;
49} /* namespace serialization */
50} /* namespace boost */
51
52namespace gtsam {
53
59template<class CAMERA, class LANDMARK>
60class GeneralSFMFactor: public NoiseModelFactorN<CAMERA, LANDMARK> {
61
62 GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA)
63 GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK)
64
65 static const int DimC = FixedDimension<CAMERA>::value;
66 static const int DimL = FixedDimension<LANDMARK>::value;
67 typedef Eigen::Matrix<double, 2, DimC> JacobianC;
68 typedef Eigen::Matrix<double, 2, DimL> JacobianL;
69
70protected:
71
73
74public:
75
78
79 // shorthand for a smart pointer to a factor
80 typedef boost::shared_ptr<This> shared_ptr;
81
90 Key cameraKey, Key landmarkKey)
91 : Base(model, cameraKey, landmarkKey), measured_(measured) {}
92
93 GeneralSFMFactor() : measured_(0.0, 0.0) {}
95 GeneralSFMFactor(const Point2& p) : measured_(p) {}
97 GeneralSFMFactor(double x, double y) : measured_(x, y) {}
99 ~GeneralSFMFactor() override {}
100
102 gtsam::NonlinearFactor::shared_ptr clone() const override {
103 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
104 gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
105
111 void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
112 Base::print(s, keyFormatter);
113 traits<Point2>::Print(measured_, s + ".z");
114 }
115
119 bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
120 const This* e = dynamic_cast<const This*>(&p);
121 return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
122 }
123
125 Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
126 boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const override {
127 try {
128 return camera.project2(point,H1,H2) - measured_;
129 }
130 catch( CheiralityException& e) {
131 if (H1) *H1 = JacobianC::Zero();
132 if (H2) *H2 = JacobianL::Zero();
133 //TODO Print the exception via logging
134 return Z_2x1;
135 }
136 }
137
139 boost::shared_ptr<GaussianFactor> linearize(const Values& values) const override {
140 // Only linearize if the factor is active
141 if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
142
143 const Key key1 = this->key1(), key2 = this->key2();
144 JacobianC H1;
145 JacobianL H2;
146 Vector2 b;
147 try {
148 const CAMERA& camera = values.at<CAMERA>(key1);
149 const LANDMARK& point = values.at<LANDMARK>(key2);
150 b = measured() - camera.project2(point, H1, H2);
151 } catch (CheiralityException& e) {
152 H1.setZero();
153 H2.setZero();
154 b.setZero();
155 //TODO Print the exception via logging
156 }
157
158 // Whiten the system if needed
159 const SharedNoiseModel& noiseModel = this->noiseModel();
160 if (noiseModel && !noiseModel->isUnit()) {
161 // TODO: implement WhitenSystem for fixed size matrices and include
162 // above
163 H1 = noiseModel->Whiten(H1);
164 H2 = noiseModel->Whiten(H2);
165 b = noiseModel->Whiten(b);
166 }
167
168 // Create new (unit) noiseModel, preserving constraints if applicable
169 SharedDiagonal model;
170 if (noiseModel && noiseModel->isConstrained()) {
171 model = boost::static_pointer_cast<noiseModel::Constrained>(noiseModel)->unit();
172 }
173
174 return boost::make_shared<BinaryJacobianFactor<2, DimC, DimL> >(key1, H1, key2, H2, b, model);
175 }
176
178 inline const Point2 measured() const {
179 return measured_;
180 }
181
182private:
184 friend class boost::serialization::access;
185 template<class Archive>
186 void serialize(Archive & ar, const unsigned int /*version*/) {
187 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
188 ar & boost::serialization::make_nvp("NoiseModelFactor2",
189 boost::serialization::base_object<Base>(*this));
190 ar & BOOST_SERIALIZATION_NVP(measured_);
191 }
192};
193
194template<class CAMERA, class LANDMARK>
195struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
196 GeneralSFMFactor<CAMERA, LANDMARK> > {
197};
198
203template<class CALIBRATION>
204class GeneralSFMFactor2: public NoiseModelFactorN<Pose3, Point3, CALIBRATION> {
205
206 GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
207 static const int DimK = FixedDimension<CALIBRATION>::value;
208
209protected:
210
212
213public:
214
218
219 // shorthand for a smart pointer to a factor
220 typedef boost::shared_ptr<This> shared_ptr;
221
230 GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) :
231 Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {}
233
234 ~GeneralSFMFactor2() override {}
235
237 gtsam::NonlinearFactor::shared_ptr clone() const override {
238 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
239 gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
240
246 void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
247 Base::print(s, keyFormatter);
249 }
250
254 bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
255 const This* e = dynamic_cast<const This*>(&p);
256 return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
257 }
258
260 Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
261 boost::optional<Matrix&> H1=boost::none,
262 boost::optional<Matrix&> H2=boost::none,
263 boost::optional<Matrix&> H3=boost::none) const override
264 {
265 try {
266 Camera camera(pose3,calib);
267 return camera.project(point, H1, H2, H3) - measured_;
268 }
269 catch( CheiralityException& e) {
270 if (H1) *H1 = Matrix::Zero(2, 6);
271 if (H2) *H2 = Matrix::Zero(2, 3);
272 if (H3) *H3 = Matrix::Zero(2, DimK);
273 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
274 << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
275 }
276 return Z_2x1;
277 }
278
280 inline const Point2 measured() const {
281 return measured_;
282 }
283
284private:
287 template<class Archive>
288 void serialize(Archive & ar, const unsigned int /*version*/) {
289 // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
290 ar & boost::serialization::make_nvp("NoiseModelFactor3",
291 boost::serialization::base_object<Base>(*this));
292 ar & BOOST_SERIALIZATION_NVP(measured_);
293 }
294};
295
296template<class CALIBRATION>
297struct traits<GeneralSFMFactor2<CALIBRATION> > : Testable<
298 GeneralSFMFactor2<CALIBRATION> > {
299};
300
301} //namespace
Concept check for values that can be used in unit tests.
Base class and basic functions for Manifold types.
Access to matrices via blocks of pre-defined sizes.
Timing utilities.
typedef and functions to augment Eigen's VectorXd
typedef and functions to augment Eigen's MatrixXd
Typedefs for easier changing of types.
3D Point
2D Point
3D Pose
Base class for all pinhole cameras.
A binary JacobianFactor specialization that uses fixed matrix math for speed.
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition: Manifold.h:164
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
Definition: CalibratedCamera.h:32
A pinhole camera class that has a Pose3 and a Calibration.
Definition: PinholeCamera.h:33
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition: PinholePose.h:118
A 3D pose (R,t) : (Rot3,Point3)
Definition: Pose3.h:37
Definition: Factor.h:68
Nonlinear factor base class.
Definition: NonlinearFactor.h:42
NonlinearFactor()
Default constructor for I/O only.
Definition: NonlinearFactor.h:58
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition: NonlinearFactor.h:118
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:74
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:82
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:223
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition: NonlinearFactor.h:400
Non-linear factor for a constraint derived from a 2D measurement.
Definition: GeneralSFMFactor.h:60
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
Linearize using fixed-size matrices.
Definition: GeneralSFMFactor.h:138
Vector evaluateError(const CAMERA &camera, const LANDMARK &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
h(x)-z
Definition: GeneralSFMFactor.h:124
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: GeneralSFMFactor.h:101
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: GeneralSFMFactor.h:118
const Point2 measured() const
return the measured
Definition: GeneralSFMFactor.h:177
GeneralSFMFactor(const Point2 &measured, const SharedNoiseModel &model, Key cameraKey, Key landmarkKey)
Constructor.
Definition: GeneralSFMFactor.h:89
GeneralSFMFactor()
default constructor
Definition: GeneralSFMFactor.h:93
GeneralSFMFactor< CAMERA, LANDMARK > This
typedef for this object
Definition: GeneralSFMFactor.h:76
~GeneralSFMFactor() override
destructor
Definition: GeneralSFMFactor.h:98
NoiseModelFactorN< CAMERA, LANDMARK > Base
typedef for the base class
Definition: GeneralSFMFactor.h:77
void print(const std::string &s="SFMFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: GeneralSFMFactor.h:110
friend class boost::serialization::access
Serialization function.
Definition: GeneralSFMFactor.h:183
Point2 measured_
the 2D measurement
Definition: GeneralSFMFactor.h:72
Non-linear factor for a constraint derived from a 2D measurement.
Definition: GeneralSFMFactor.h:204
GeneralSFMFactor2()
default constructor
Definition: GeneralSFMFactor.h:232
~GeneralSFMFactor2() override
destructor
Definition: GeneralSFMFactor.h:234
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: GeneralSFMFactor.h:237
NoiseModelFactorN< Pose3, Point3, CALIBRATION > Base
typedef for the base class
Definition: GeneralSFMFactor.h:217
Vector evaluateError(const Pose3 &pose3, const Point3 &point, const CALIBRATION &calib, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
h(x)-z
Definition: GeneralSFMFactor.h:260
void print(const std::string &s="SFMFactor2", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: GeneralSFMFactor.h:246
Point2 measured_
the 2D measurement
Definition: GeneralSFMFactor.h:211
const Point2 measured() const
return the measured
Definition: GeneralSFMFactor.h:280
PinholeCamera< CALIBRATION > Camera
typedef for camera type
Definition: GeneralSFMFactor.h:216
GeneralSFMFactor2(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key landmarkKey, Key calibKey)
Constructor.
Definition: GeneralSFMFactor.h:230
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: GeneralSFMFactor.h:254
friend class boost::serialization::access
Serialization function.
Definition: GeneralSFMFactor.h:286
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...