gtsam 4.2.0
gtsam
TriangulationFactor.h
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
18#pragma once
19
22#include <boost/make_shared.hpp>
23#include <boost/lexical_cast.hpp>
24
25namespace gtsam {
26
32template<class CAMERA>
34
35public:
36
38 using Camera = CAMERA;
39
40protected:
41
44
47
49 using Measurement = typename CAMERA::Measurement;
50
51 // Keep a copy of measurement and calibration for I/O
52 const CAMERA camera_;
54
55 // verbosity handling for Cheirality Exceptions
56 const bool throwCheirality_;
57 const bool verboseCheirality_;
58
59public:
60 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
61
63 using shared_ptr = boost::shared_ptr<This>;
64
68 }
69
79 TriangulationFactor(const CAMERA& camera, const Measurement& measured,
80 const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false,
81 bool verboseCheirality = false) :
82 Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_(
84 if (model && model->dim() != traits<Measurement>::dimension)
85 throw std::invalid_argument(
86 "TriangulationFactor must be created with "
87 + boost::lexical_cast<std::string>((int) traits<Measurement>::dimension)
88 + "-dimensional noise model.");
89 }
90
93 }
94
96 gtsam::NonlinearFactor::shared_ptr clone() const override {
97 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
98 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
99 }
100
106 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
107 DefaultKeyFormatter) const override {
108 std::cout << s << "TriangulationFactor,";
109 camera_.print("camera");
111 Base::print("", keyFormatter);
112 }
113
115 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
116 const This *e = dynamic_cast<const This*>(&p);
117 return e && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol)
118 && traits<Measurement>::Equals(this->measured_, e->measured_, tol);
119 }
120
122 Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
123 boost::none) const override {
124 try {
125 return traits<Measurement>::Local(measured_, camera_.project2(point, boost::none, H2));
126 } catch (CheiralityException& e) {
127 if (H2)
128 *H2 = Matrix::Zero(traits<Measurement>::dimension, 3);
130 std::cout << e.what() << ": Landmark "
131 << DefaultKeyFormatter(this->key()) << " moved behind camera"
132 << std::endl;
134 throw e;
135 return camera_.defaultErrorWhenTriangulatingBehindCamera();
136 }
137 }
138
141 mutable Matrix A;
142 mutable Vector b;
143
149 boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
150 // Only linearize if the factor is active
151 if (!this->active(x))
152 return boost::shared_ptr<JacobianFactor>();
153
154 // Allocate memory for Jacobian factor, do only once
155 if (Ab.rows() == 0) {
156 std::vector<size_t> dimensions(1, 3);
160 }
161
162 // Would be even better if we could pass blocks to project
163 const Point3& point = x.at<Point3>(key());
164 b = traits<Measurement>::Local(camera_.project2(point, boost::none, A), measured_);
165 if (noiseModel_)
166 this->noiseModel_->WhitenSystem(A, b);
167
168 Ab(0) = A;
169 Ab(1) = b;
170
171 return boost::make_shared<JacobianFactor>(this->keys_, Ab);
172 }
173
175 const Measurement& measured() const {
176 return measured_;
177 }
178
180 inline bool verboseCheirality() const {
181 return verboseCheirality_;
182 }
183
185 inline bool throwCheirality() const {
186 return throwCheirality_;
187 }
188
189private:
190
193 template<class ARCHIVE>
194 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
195 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
196 ar & BOOST_SERIALIZATION_NVP(camera_);
197 ar & BOOST_SERIALIZATION_NVP(measured_);
198 ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
199 ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
200 }
201};
202} // \ namespace gtsam
203
Calibrated camera for which only pose is unknown.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
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
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition: VerticalBlockMatrix.h:43
DenseIndex rows() const
Row size.
Definition: VerticalBlockMatrix.h:115
Definition: CalibratedCamera.h:32
Definition: Factor.h:68
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:85
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
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
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
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:65
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:361
Non-linear factor for a constraint derived from a 2D measurement.
Definition: TriangulationFactor.h:33
VerticalBlockMatrix Ab
thread-safe (?) scratch memory for linearize
Definition: TriangulationFactor.h:140
const bool throwCheirality_
If true, rethrows Cheirality exceptions (default: false)
Definition: TriangulationFactor.h:56
bool throwCheirality() const
return flag for throwing cheirality exceptions
Definition: TriangulationFactor.h:185
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: TriangulationFactor.h:106
Vector evaluateError(const Point3 &point, boost::optional< Matrix & > H2=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: TriangulationFactor.h:122
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: TriangulationFactor.h:96
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Linearize to a JacobianFactor, does not support constrained noise model ! Hence .
Definition: TriangulationFactor.h:149
TriangulationFactor(const CAMERA &camera, const Measurement &measured, const SharedNoiseModel &model, Key pointKey, bool throwCheirality=false, bool verboseCheirality=false)
Constructor with exception-handling flags.
Definition: TriangulationFactor.h:79
typename CAMERA::Measurement Measurement
shorthand for measurement type, e.g. Point2 or StereoPoint2
Definition: TriangulationFactor.h:49
const Measurement & measured() const
return the measurement
Definition: TriangulationFactor.h:175
bool verboseCheirality() const
return verbosity
Definition: TriangulationFactor.h:180
CAMERA Camera
CAMERA type.
Definition: TriangulationFactor.h:38
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: TriangulationFactor.h:115
~TriangulationFactor() override
Virtual destructor.
Definition: TriangulationFactor.h:92
const bool verboseCheirality_
If true, prints text for Cheirality exceptions (default: false)
Definition: TriangulationFactor.h:57
friend class boost::serialization::access
Serialization function.
Definition: TriangulationFactor.h:192
TriangulationFactor()
Default constructor.
Definition: TriangulationFactor.h:66
TriangulationFactor< CAMERA > This
shorthand for this class
Definition: TriangulationFactor.h:46
const Measurement measured_
2D measurement
Definition: TriangulationFactor.h:53
const CAMERA camera_
CAMERA in which this landmark was seen.
Definition: TriangulationFactor.h:52