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
12/**
13 * @file GeneralSFMFactor.h
14 *
15 * @brief a general SFM factor with an unknown calibration
16 *
17 * @date Dec 15, 2010
18 * @author Kai Ni
19 */
20
21#pragma once
22
23#include <gtsam/geometry/PinholeCamera.h>
24#include <gtsam/geometry/Point2.h>
25#include <gtsam/geometry/Point3.h>
26#include <gtsam/geometry/Pose3.h>
27#include <gtsam/nonlinear/NonlinearFactor.h>
28#include <gtsam/linear/BinaryJacobianFactor.h>
29#include <gtsam/linear/NoiseModel.h>
30#include <gtsam/base/concepts.h>
31#include <gtsam/base/Manifold.h>
32#include <gtsam/base/Matrix.h>
33#include <gtsam/base/SymmetricBlockMatrix.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#if GTSAM_ENABLE_BOOST_SERIALIZATION
40#include <boost/serialization/nvp.hpp>
41#endif
42#include <iostream>
43#include <string>
44
45namespace boost {
46namespace serialization {
47class access;
48} /* namespace serialization */
49} /* namespace boost */
50
51namespace gtsam {
52
53/**
54 * Non-linear factor for a constraint derived from a 2D measurement.
55 * The calibration is unknown here compared to GenericProjectionFactor
56 * @ingroup slam
57 */
58template<class CAMERA, class LANDMARK>
59class GeneralSFMFactor: public NoiseModelFactorN<CAMERA, LANDMARK> {
60
61 GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA)
62 GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK)
63
64 static const int DimC = FixedDimension<CAMERA>::value;
65 static const int DimL = FixedDimension<LANDMARK>::value;
66 typedef Eigen::Matrix<double, 2, DimC> JacobianC;
67 typedef Eigen::Matrix<double, 2, DimL> JacobianL;
68
69protected:
70
71 Point2 measured_; ///< the 2D measurement
72
73public:
74
75 typedef GeneralSFMFactor<CAMERA, LANDMARK> This;///< typedef for this object
76 typedef NoiseModelFactorN<CAMERA, LANDMARK> Base;///< typedef for the base class
77
78 // Provide access to the Matrix& version of evaluateError:
79 using Base::evaluateError;
80
81 // shorthand for a smart pointer to a factor
82 typedef std::shared_ptr<This> shared_ptr;
83
84 /**
85 * Constructor
86 * @param measured is the 2 dimensional location of point in image (the measurement)
87 * @param model is the standard deviation of the measurements
88 * @param cameraKey is the index of the camera
89 * @param landmarkKey is the index of the landmark
90 */
91 GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model,
92 Key cameraKey, Key landmarkKey)
93 : Base(model, cameraKey, landmarkKey), measured_(measured) {}
94
95 GeneralSFMFactor() : measured_(0.0, 0.0) {} ///< default constructor
96 ///< constructor that takes a Point2
97 GeneralSFMFactor(const Point2& p) : measured_(p) {}
98 ///< constructor that takes doubles x,y to make a Point2
99 GeneralSFMFactor(double x, double y) : measured_(x, y) {}
100
101 ~GeneralSFMFactor() override {} ///< destructor
102
103 /// @return a deep copy of this factor
104 gtsam::NonlinearFactor::shared_ptr clone() const override {
105 return std::static_pointer_cast<gtsam::NonlinearFactor>(
106 r: gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
107
108 /**
109 * print
110 * @param s optional string naming the factor
111 * @param keyFormatter optional formatter for printing out Symbols
112 */
113 void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
114 Base::print(s, keyFormatter);
115 traits<Point2>::Print(m: measured_, str: s + ".z");
116 }
117
118 /**
119 * equals
120 */
121 bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
122 const This* e = dynamic_cast<const This*>(&p);
123 return e && Base::equals(p, tol) && traits<Point2>::Equals(v1: this->measured_, v2: e->measured_, tol);
124 }
125
126 /** h(x)-z */
127 Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
128 OptionalMatrixType H1, OptionalMatrixType H2) const override {
129 try {
130 return camera.project2(point,H1,H2) - measured_;
131 }
132 catch( CheiralityException& e [[maybe_unused]]) {
133 if (H1) *H1 = JacobianC::Zero();
134 if (H2) *H2 = JacobianL::Zero();
135 //TODO Print the exception via logging
136 return Z_2x1;
137 }
138 }
139
140 /// Linearize using fixed-size matrices
141 std::shared_ptr<GaussianFactor> linearize(const Values& values) const override {
142 // Only linearize if the factor is active
143 if (!this->active(values)) return std::shared_ptr<JacobianFactor>();
144
145 const Key key1 = this->key1(), key2 = this->key2();
146 JacobianC H1;
147 JacobianL H2;
148 Vector2 b;
149 try {
150 const CAMERA& camera = values.at<CAMERA>(key1);
151 const LANDMARK& point = values.at<LANDMARK>(key2);
152 b = measured() - camera.project2(point, H1, H2);
153 } catch (CheiralityException& e [[maybe_unused]]) {
154 H1.setZero();
155 H2.setZero();
156 b.setZero();
157 //TODO Print the exception via logging
158 }
159
160 // Whiten the system if needed
161 const SharedNoiseModel& noiseModel = this->noiseModel();
162 if (noiseModel && !noiseModel->isUnit()) {
163 // TODO: implement WhitenSystem for fixed size matrices and include
164 // above
165 H1 = noiseModel->Whiten(H: H1);
166 H2 = noiseModel->Whiten(H: H2);
167 b = noiseModel->Whiten(H: b);
168 }
169
170 // Create new (unit) noiseModel, preserving constraints if applicable
171 SharedDiagonal model;
172 if (noiseModel && noiseModel->isConstrained()) {
173 model = std::static_pointer_cast<noiseModel::Constrained>(r: noiseModel)->unit();
174 }
175
176 return std::make_shared<BinaryJacobianFactor<2, DimC, DimL> >(key1, H1, key2, H2, b, model);
177 }
178
179 /** return the measured */
180 inline const Point2 measured() const {
181 return measured_;
182 }
183
184private:
185#if GTSAM_ENABLE_BOOST_SERIALIZATION
186 /** Serialization function */
187 friend class boost::serialization::access;
188 template<class Archive>
189 void serialize(Archive & ar, const unsigned int /*version*/) {
190 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
191 ar & boost::serialization::make_nvp("NoiseModelFactor2",
192 boost::serialization::base_object<Base>(*this));
193 ar & BOOST_SERIALIZATION_NVP(measured_);
194 }
195#endif
196};
197
198template<class CAMERA, class LANDMARK>
199struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
200 GeneralSFMFactor<CAMERA, LANDMARK> > {
201};
202
203/**
204 * Non-linear factor for a constraint derived from a 2D measurement.
205 * Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera..
206 */
207template<class CALIBRATION>
208class GeneralSFMFactor2: public NoiseModelFactorN<Pose3, Point3, CALIBRATION> {
209
210 GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
211 static const int DimK = FixedDimension<CALIBRATION>::value;
212
213protected:
214
215 Point2 measured_; ///< the 2D measurement
216
217public:
218
219 typedef GeneralSFMFactor2<CALIBRATION> This;
220 typedef PinholeCamera<CALIBRATION> Camera;///< typedef for camera type
221 typedef NoiseModelFactorN<Pose3, Point3, CALIBRATION> Base;///< typedef for the base class
222
223 // shorthand for a smart pointer to a factor
224 typedef std::shared_ptr<This> shared_ptr;
225
226 /**
227 * Constructor
228 * @param measured is the 2 dimensional location of point in image (the measurement)
229 * @param model is the standard deviation of the measurements
230 * @param poseKey is the index of the camera
231 * @param landmarkKey is the index of the landmark
232 * @param calibKey is the index of the calibration
233 */
234 GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) :
235 Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {}
236 GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor
237
238 ~GeneralSFMFactor2() override {} ///< destructor
239
240 /// @return a deep copy of this factor
241 gtsam::NonlinearFactor::shared_ptr clone() const override {
242 return std::static_pointer_cast<gtsam::NonlinearFactor>(
243 r: gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
244
245 /**
246 * print
247 * @param s optional string naming the factor
248 * @param keyFormatter optional formatter useful for printing Symbols
249 */
250 void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
251 Base::print(s, keyFormatter);
252 traits<Point2>::Print(m: measured_, str: s + ".z");
253 }
254
255 /**
256 * equals
257 */
258 bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
259 const This* e = dynamic_cast<const This*>(&p);
260 return e && Base::equals(p, tol) && traits<Point2>::Equals(v1: this->measured_, v2: e->measured_, tol);
261 }
262
263 /** h(x)-z */
264 Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
265 OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override {
266 try {
267 Camera camera(pose3,calib);
268 return camera.project(point, H1, H2, H3) - measured_;
269 }
270 catch( CheiralityException& e) {
271 if (H1) *H1 = Matrix::Zero(rows: 2, cols: 6);
272 if (H2) *H2 = Matrix::Zero(rows: 2, cols: 3);
273 if (H3) *H3 = Matrix::Zero(rows: 2, cols: DimK);
274 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
275 << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
276 }
277 return Z_2x1;
278 }
279
280 /** return the measured */
281 inline const Point2 measured() const {
282 return measured_;
283 }
284
285private:
286#if GTSAM_ENABLE_BOOST_SERIALIZATION
287 /** Serialization function */
288 friend class boost::serialization::access;
289 template<class Archive>
290 void serialize(Archive & ar, const unsigned int /*version*/) {
291 // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
292 ar & boost::serialization::make_nvp("NoiseModelFactor3",
293 boost::serialization::base_object<Base>(*this));
294 ar & BOOST_SERIALIZATION_NVP(measured_);
295 }
296#endif
297};
298
299template<class CALIBRATION>
300struct traits<GeneralSFMFactor2<CALIBRATION> > : Testable<
301 GeneralSFMFactor2<CALIBRATION> > {
302};
303
304} //namespace
305