gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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
Base class and basic functions for Manifold types.
typedef and functions to augment Eigen's MatrixXd
Access to matrices via blocks of pre-defined sizes.
Concept check for values that can be used in unit tests.
typedef and functions to augment Eigen's VectorXd
Timing utilities.
Typedefs for easier changing of types.
Base class for all pinhole cameras.
2D Point
3D Point
3D Pose
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
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
Nonlinear factor base class.
Definition NonlinearFactor.h:42
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
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...