1
2/**
3 * @file InvDepthFactorVariant3.h
4 * @brief Inverse Depth Factor based on Civera09tro, Montiel06rss.
5 * Landmarks are parameterized as (theta,phi,rho). The factor involves
6 * two poses and a landmark. The first pose is the reference frame
7 * from which (theta, phi, rho) is measured.
8 * @author Chris Beall, Stephen Williams
9 */
10
11#pragma once
12
13#include <gtsam/nonlinear/NonlinearFactor.h>
14#include <gtsam/geometry/PinholeCamera.h>
15#include <gtsam/geometry/Cal3_S2.h>
16#include <gtsam/geometry/Pose3.h>
17#include <gtsam/geometry/Point2.h>
18#include <gtsam/base/numericalDerivative.h>
19
20namespace gtsam {
21
22/**
23 * Binary factor representing the first visual measurement using an inverse-depth parameterization
24 */
25class InvDepthFactorVariant3a: public NoiseModelFactorN<Pose3, Vector3> {
26protected:
27
28 // Keep a copy of measurement and calibration for I/O
29 Point2 measured_; ///< 2D measurement
30 Cal3_S2::shared_ptr K_; ///< shared pointer to calibration object
31
32public:
33
34 /// shorthand for base class type
35 typedef NoiseModelFactor2<Pose3, Vector3> Base;
36
37 // Provide access to the Matrix& version of evaluateError:
38 using Base::evaluateError;
39
40 /// shorthand for this class
41 typedef InvDepthFactorVariant3a This;
42
43 /// shorthand for a smart pointer to a factor
44 typedef std::shared_ptr<This> shared_ptr;
45
46 /// Default constructor
47 InvDepthFactorVariant3a() :
48 measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
49 }
50
51 /**
52 * Constructor
53 * TODO: Mark argument order standard (keys, measurement, parameters)
54 * @param measured is the 2 dimensional location of point in image (the measurement)
55 * @param model is the standard deviation
56 * @param poseKey is the index of the camera pose
57 * @param pointKey is the index of the landmark
58 * @param invDepthKey is the index of inverse depth
59 * @param K shared pointer to the constant calibration
60 */
61 InvDepthFactorVariant3a(const Key poseKey, const Key landmarkKey,
62 const Point2& measured, const Cal3_S2::shared_ptr& K, const SharedNoiseModel& model) :
63 Base(model, poseKey, landmarkKey), measured_(measured), K_(K) {}
64
65 /** Virtual destructor */
66 ~InvDepthFactorVariant3a() override {}
67
68 /**
69 * print
70 * @param s optional string naming the factor
71 * @param keyFormatter optional formatter useful for printing Symbols
72 */
73 void print(const std::string& s = "InvDepthFactorVariant3a",
74 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
75 Base::print(s, keyFormatter);
76 traits<Point2>::Print(m: measured_, str: s + ".z");
77 }
78
79 /// equals
80 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
81 const This *e = dynamic_cast<const This*>(&p);
82 return e
83 && Base::equals(f: p, tol)
84 && traits<Point2>::Equals(v1: this->measured_, v2: e->measured_, tol)
85 && this->K_->equals(K: *e->K_, tol);
86 }
87
88 Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const {
89 try {
90 // Calculate the 3D coordinates of the landmark in the Pose frame
91 double theta = landmark(0), phi = landmark(1), rho = landmark(2);
92 Point3 pose_P_landmark(cos(x: phi)*sin(x: theta)/rho, sin(x: phi)/rho, cos(x: phi)*cos(x: theta)/rho);
93 // Convert the landmark to world coordinates
94 Point3 world_P_landmark = pose.transformFrom(point: pose_P_landmark);
95 // Project landmark into Pose2
96 PinholeCamera<Cal3_S2> camera(pose, *K_);
97 return camera.project(pw: world_P_landmark) - measured_;
98 } catch( CheiralityException& e) {
99 std::cout << e.what()
100 << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<1>()) << "," << DefaultKeyFormatter(this->key<2>()) << "]"
101 << " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) << "]"
102 << std::endl;
103 return Vector::Ones(newSize: 2) * 2.0 * K_->fx();
104 }
105 return (Vector(1) << 0.0).finished();
106 }
107
108 /// Evaluate error h(x)-z and optionally derivatives
109 Vector evaluateError(const Pose3& pose, const Vector3& landmark,
110 OptionalMatrixType H1, OptionalMatrixType H2) const override {
111
112 if(H1) {
113 (*H1) = numericalDerivative11<Vector, Pose3>(
114 h: std::bind(f: &InvDepthFactorVariant3a::inverseDepthError, args: this,
115 args: std::placeholders::_1, args: landmark),
116 x: pose);
117 }
118 if(H2) {
119 (*H2) = numericalDerivative11<Vector, Vector3>(
120 h: std::bind(f: &InvDepthFactorVariant3a::inverseDepthError, args: this, args: pose,
121 args: std::placeholders::_1),
122 x: landmark);
123 }
124
125 return inverseDepthError(pose, landmark);
126 }
127
128 /** return the measurement */
129 const Point2& imagePoint() const {
130 return measured_;
131 }
132
133 /** return the calibration object */
134 const Cal3_S2::shared_ptr calibration() const {
135 return K_;
136 }
137
138private:
139
140#if GTSAM_ENABLE_BOOST_SERIALIZATION ///
141 /// Serialization function
142 friend class boost::serialization::access;
143 template<class ARCHIVE>
144 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
145 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
146 ar & BOOST_SERIALIZATION_NVP(measured_);
147 ar & BOOST_SERIALIZATION_NVP(K_);
148 }
149#endif
150};
151
152/**
153 * Ternary factor representing a visual measurement using an inverse-depth parameterization
154 */
155class InvDepthFactorVariant3b: public NoiseModelFactorN<Pose3, Pose3, Vector3> {
156protected:
157
158 // Keep a copy of measurement and calibration for I/O
159 Point2 measured_; ///< 2D measurement
160 Cal3_S2::shared_ptr K_; ///< shared pointer to calibration object
161
162public:
163
164 /// shorthand for base class type
165 typedef NoiseModelFactorN<Pose3, Pose3, Vector3> Base;
166
167 /// shorthand for this class
168 typedef InvDepthFactorVariant3b This;
169
170 /// shorthand for a smart pointer to a factor
171 typedef std::shared_ptr<This> shared_ptr;
172
173 /// Default constructor
174 InvDepthFactorVariant3b() :
175 measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
176 }
177
178 /**
179 * Constructor
180 * TODO: Mark argument order standard (keys, measurement, parameters)
181 * @param measured is the 2 dimensional location of point in image (the measurement)
182 * @param model is the standard deviation
183 * @param poseKey is the index of the camera pose
184 * @param pointKey is the index of the landmark
185 * @param invDepthKey is the index of inverse depth
186 * @param K shared pointer to the constant calibration
187 */
188 InvDepthFactorVariant3b(const Key poseKey1, const Key poseKey2, const Key landmarkKey,
189 const Point2& measured, const Cal3_S2::shared_ptr& K, const SharedNoiseModel& model) :
190 Base(model, poseKey1, poseKey2, landmarkKey), measured_(measured), K_(K) {}
191
192 /** Virtual destructor */
193 ~InvDepthFactorVariant3b() override {}
194
195 /**
196 * print
197 * @param s optional string naming the factor
198 * @param keyFormatter optional formatter useful for printing Symbols
199 */
200 void print(const std::string& s = "InvDepthFactorVariant3",
201 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
202 Base::print(s, keyFormatter);
203 traits<Point2>::Print(m: measured_, str: s + ".z");
204 }
205
206 /// equals
207 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
208 const This *e = dynamic_cast<const This*>(&p);
209 return e
210 && Base::equals(f: p, tol)
211 && traits<Point2>::Equals(v1: this->measured_, v2: e->measured_, tol)
212 && this->K_->equals(K: *e->K_, tol);
213 }
214
215 Vector inverseDepthError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark) const {
216 try {
217 // Calculate the 3D coordinates of the landmark in the Pose1 frame
218 double theta = landmark(0), phi = landmark(1), rho = landmark(2);
219 Point3 pose1_P_landmark(cos(x: phi)*sin(x: theta)/rho, sin(x: phi)/rho, cos(x: phi)*cos(x: theta)/rho);
220 // Convert the landmark to world coordinates
221 Point3 world_P_landmark = pose1.transformFrom(point: pose1_P_landmark);
222 // Project landmark into Pose2
223 PinholeCamera<Cal3_S2> camera(pose2, *K_);
224 return camera.project(pw: world_P_landmark) - measured_;
225 } catch( CheiralityException& e) {
226 std::cout << e.what()
227 << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<1>()) << "," << DefaultKeyFormatter(this->key<3>()) << "]"
228 << " moved behind camera " << DefaultKeyFormatter(this->key<2>())
229 << std::endl;
230 return Vector::Ones(newSize: 2) * 2.0 * K_->fx();
231 }
232 return (Vector(1) << 0.0).finished();
233 }
234
235 /// Evaluate error h(x)-z and optionally derivatives
236 Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark,
237 OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override {
238
239 if(H1)
240 (*H1) = numericalDerivative11<Vector, Pose3>(
241 h: std::bind(f: &InvDepthFactorVariant3b::inverseDepthError, args: this,
242 args: std::placeholders::_1, args: pose2, args: landmark),
243 x: pose1);
244
245 if(H2)
246 (*H2) = numericalDerivative11<Vector, Pose3>(
247 h: std::bind(f: &InvDepthFactorVariant3b::inverseDepthError, args: this, args: pose1,
248 args: std::placeholders::_1, args: landmark),
249 x: pose2);
250
251 if(H3)
252 (*H3) = numericalDerivative11<Vector, Vector3>(
253 h: std::bind(f: &InvDepthFactorVariant3b::inverseDepthError, args: this, args: pose1,
254 args: pose2, args: std::placeholders::_1),
255 x: landmark);
256
257 return inverseDepthError(pose1, pose2, landmark);
258 }
259
260 /** return the measurement */
261 const Point2& imagePoint() const {
262 return measured_;
263 }
264
265 /** return the calibration object */
266 const Cal3_S2::shared_ptr calibration() const {
267 return K_;
268 }
269
270private:
271
272#if GTSAM_ENABLE_BOOST_SERIALIZATION
273 friend class boost::serialization::access;
274 /// Serialization function
275 template<class ARCHIVE>
276 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
277 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
278 ar & BOOST_SERIALIZATION_NVP(measured_);
279 ar & BOOST_SERIALIZATION_NVP(K_);
280 }
281#endif
282};
283
284} // \ namespace gtsam
285