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 SmartProjectionFactor.h
14 * @brief Smart factor on cameras (pose + calibration)
15 * @author Luca Carlone
16 * @author Zsolt Kira
17 * @author Frank Dellaert
18 */
19
20#pragma once
21
22#include <gtsam/slam/SmartFactorBase.h>
23#include <gtsam/slam/SmartFactorParams.h>
24
25#include <gtsam/geometry/triangulation.h>
26#include <gtsam/inference/Symbol.h>
27#include <gtsam/slam/dataset.h>
28
29#include <optional>
30#include <vector>
31
32namespace gtsam {
33
34/**
35 * SmartProjectionFactor: triangulates point and keeps an estimate of it around.
36 * This factor operates with monocular cameras, where a camera is expected to
37 * behave like PinholeCamera or PinholePose. This factor is intended
38 * to be used directly with PinholeCamera, which optimizes the camera pose
39 * and calibration. This also requires that values contains the involved
40 * cameras (instead of poses and calibrations separately).
41 * If the calibration is fixed use SmartProjectionPoseFactor instead!
42 */
43template<class CAMERA>
44class SmartProjectionFactor: public SmartFactorBase<CAMERA> {
45
46public:
47
48private:
49 typedef SmartFactorBase<CAMERA> Base;
50 typedef SmartProjectionFactor<CAMERA> This;
51 typedef SmartProjectionFactor<CAMERA> SmartProjectionCameraFactor;
52
53protected:
54
55 /// @name Parameters
56 /// @{
57 SmartProjectionParams params_;
58 /// @}
59
60 /// @name Caching triangulation
61 /// @{
62 mutable TriangulationResult result_; ///< result from triangulateSafe
63 mutable std::vector<Pose3, Eigen::aligned_allocator<Pose3> >
64 cameraPosesTriangulation_; ///< current triangulation poses
65 /// @}
66
67 public:
68
69 /// shorthand for a smart pointer to a factor
70 typedef std::shared_ptr<This> shared_ptr;
71
72 /// shorthand for a set of cameras
73 typedef CAMERA Camera;
74 typedef CameraSet<CAMERA> Cameras;
75
76 /**
77 * Default constructor, only for serialization
78 */
79 SmartProjectionFactor() {}
80
81 /**
82 * Constructor
83 * @param sharedNoiseModel isotropic noise model for the 2D feature measurements
84 * @param params parameters for the smart projection factors
85 */
86 SmartProjectionFactor(
87 const SharedNoiseModel& sharedNoiseModel,
88 const SmartProjectionParams& params = SmartProjectionParams())
89 : Base(sharedNoiseModel),
90 params_(params),
91 result_(TriangulationResult::Degenerate()) {}
92
93 /** Virtual destructor */
94 ~SmartProjectionFactor() override {
95 }
96
97 /**
98 * print
99 * @param s optional string naming the factor
100 * @param keyFormatter optional formatter useful for printing Symbols
101 */
102 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
103 DefaultKeyFormatter) const override {
104 std::cout << s << "SmartProjectionFactor\n";
105 std::cout << "linearizationMode: " << params_.linearizationMode
106 << std::endl;
107 std::cout << "triangulationParameters:\n" << params_.triangulation
108 << std::endl;
109 std::cout << "result:\n" << result_ << std::endl;
110 Base::print("", keyFormatter);
111 }
112
113 /// equals
114 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
115 const This *e = dynamic_cast<const This*>(&p);
116 return e && params_.linearizationMode == e->params_.linearizationMode
117 && Base::equals(p, tol);
118 }
119
120 /**
121 * @brief Check if the new linearization point is the same as the one used for
122 * previous triangulation.
123 *
124 * @param cameras
125 * @return true if we need to re-triangulate.
126 */
127 bool decideIfTriangulate(const Cameras& cameras) const {
128 // Several calls to linearize will be done from the same linearization
129 // point, hence it is not needed to re-triangulate. Note that this is not
130 // yet "selecting linearization", that will come later, and we only check if
131 // the current linearization is the "same" (up to tolerance) w.r.t. the last
132 // time we triangulated the point.
133
134 size_t m = cameras.size();
135
136 bool retriangulate = false;
137
138 // Definitely true if we do not have a previous linearization point or the
139 // new linearization point includes more poses.
140 if (cameraPosesTriangulation_.empty()
141 || cameras.size() != cameraPosesTriangulation_.size())
142 retriangulate = true;
143
144 // Otherwise, check poses against cache.
145 if (!retriangulate) {
146 for (size_t i = 0; i < cameras.size(); i++) {
147 if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
148 params_.retriangulationThreshold)) {
149 retriangulate = true; // at least two poses are different, hence we retriangulate
150 break;
151 }
152 }
153 }
154
155 // Store the current poses used for triangulation if we will re-triangulate.
156 if (retriangulate) {
157 cameraPosesTriangulation_.clear();
158 cameraPosesTriangulation_.reserve(n: m);
159 for (size_t i = 0; i < m; i++)
160 // cameraPosesTriangulation_[i] = cameras[i].pose();
161 cameraPosesTriangulation_.push_back(cameras[i].pose());
162 }
163
164 return retriangulate;
165 }
166
167 /**
168 * @brief Call gtsam::triangulateSafe iff we need to re-triangulate.
169 *
170 * @param cameras
171 * @return TriangulationResult
172 */
173 TriangulationResult triangulateSafe(const Cameras& cameras) const {
174
175 size_t m = cameras.size();
176 if (m < 2) // if we have a single pose the corresponding factor is uninformative
177 return TriangulationResult::Degenerate();
178
179 bool retriangulate = decideIfTriangulate(cameras);
180 if (retriangulate)
181 result_ = gtsam::triangulateSafe(cameras, this->measured_,
182 params_.triangulation);
183 return result_;
184 }
185
186 /**
187 * @brief Possibly re-triangulate before calculating Jacobians.
188 *
189 * @param cameras
190 * @return true if we could safely triangulate
191 */
192 bool triangulateForLinearize(const Cameras& cameras) const {
193 triangulateSafe(cameras); // imperative, might reset result_
194 return bool(result_);
195 }
196
197 /// Create a Hessianfactor that is an approximation of error(p).
198 std::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor(
199 const Cameras& cameras, const double lambda = 0.0,
200 bool diagonalDamping = false) const {
201 size_t numKeys = this->keys_.size();
202 // Create structures for Hessian Factors
203 KeyVector js;
204 std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
205 std::vector<Vector> gs(numKeys);
206
207 if (this->measured_.size() != cameras.size())
208 throw std::runtime_error(
209 "SmartProjectionHessianFactor: this->measured_"
210 ".size() inconsistent with input");
211
212 triangulateSafe(cameras);
213
214 if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
215 // failed: return"empty" Hessian
216 for (Matrix& m : Gs) m = Matrix::Zero(Base::Dim, Base::Dim);
217 for (Vector& v : gs) v = Vector::Zero(Base::Dim);
218 return std::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
219 Gs, gs, 0.0);
220 }
221
222 // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
223 typename Base::FBlocks Fs;
224 Matrix E;
225 Vector b;
226 computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
227
228 // Whiten using noise model
229 Base::whitenJacobians(Fs, E, b);
230
231 // build augmented hessian
232 SymmetricBlockMatrix augmentedHessian = //
233 Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping);
234
235 return std::make_shared<RegularHessianFactor<Base::Dim> >(
236 this->keys_, augmentedHessian);
237 }
238
239 // Create RegularImplicitSchurFactor factor.
240 std::shared_ptr<RegularImplicitSchurFactor<CAMERA> > createRegularImplicitSchurFactor(
241 const Cameras& cameras, double lambda) const {
242 if (triangulateForLinearize(cameras))
243 return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
244 else
245 // failed: return empty
246 return std::shared_ptr<RegularImplicitSchurFactor<CAMERA> >();
247 }
248
249 /// Create JacobianFactorQ factor.
250 std::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
251 const Cameras& cameras, double lambda) const {
252 if (triangulateForLinearize(cameras))
253 return Base::createJacobianQFactor(cameras, *result_, lambda);
254 else
255 // failed: return empty
256 return std::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->keys_);
257 }
258
259 /// Create JacobianFactorQ factor, takes values.
260 std::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
261 const Values& values, double lambda) const {
262 return createJacobianQFactor(this->cameras(values), lambda);
263 }
264
265 /// Different (faster) way to compute a JacobianFactorSVD factor.
266 std::shared_ptr<JacobianFactor> createJacobianSVDFactor(
267 const Cameras& cameras, double lambda) const {
268 if (triangulateForLinearize(cameras))
269 return Base::createJacobianSVDFactor(cameras, *result_, lambda);
270 else
271 // failed: return empty
272 return std::make_shared<JacobianFactorSVD<Base::Dim, 2> >(this->keys_);
273 }
274
275 /// Linearize to a Hessianfactor.
276 virtual std::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
277 const Values& values, double lambda = 0.0) const {
278 return createHessianFactor(cameras: this->cameras(values), lambda);
279 }
280
281 /// Linearize to an Implicit Schur factor.
282 virtual std::shared_ptr<RegularImplicitSchurFactor<CAMERA> > linearizeToImplicit(
283 const Values& values, double lambda = 0.0) const {
284 return createRegularImplicitSchurFactor(cameras: this->cameras(values), lambda);
285 }
286
287 /// Linearize to a JacobianfactorQ.
288 virtual std::shared_ptr<JacobianFactorQ<Base::Dim, 2> > linearizeToJacobian(
289 const Values& values, double lambda = 0.0) const {
290 return createJacobianQFactor(this->cameras(values), lambda);
291 }
292
293 /**
294 * Linearize to Gaussian Factor
295 * @param values Values structure which must contain camera poses for this factor
296 * @return a Gaussian factor
297 */
298 std::shared_ptr<GaussianFactor> linearizeDamped(const Cameras& cameras,
299 const double lambda = 0.0) const {
300 // depending on flag set on construction we may linearize to different linear factors
301 switch (params_.linearizationMode) {
302 case HESSIAN:
303 return createHessianFactor(cameras, lambda);
304 case IMPLICIT_SCHUR:
305 return createRegularImplicitSchurFactor(cameras, lambda);
306 case JACOBIAN_SVD:
307 return createJacobianSVDFactor(cameras, lambda);
308 case JACOBIAN_Q:
309 return createJacobianQFactor(cameras, lambda);
310 default:
311 throw std::runtime_error("SmartFactorlinearize: unknown mode");
312 }
313 }
314
315 /**
316 * Linearize to Gaussian Factor
317 * @param values Values structure which must contain camera poses for this factor
318 * @return a Gaussian factor
319 */
320 std::shared_ptr<GaussianFactor> linearizeDamped(const Values& values,
321 const double lambda = 0.0) const {
322 // depending on flag set on construction we may linearize to different linear factors
323 Cameras cameras = this->cameras(values);
324 return linearizeDamped(cameras, lambda);
325 }
326
327 /// linearize
328 std::shared_ptr<GaussianFactor> linearize(
329 const Values& values) const override {
330 return linearizeDamped(values);
331 }
332
333 /**
334 * Triangulate and compute derivative of error with respect to point
335 * @return whether triangulation worked
336 */
337 bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
338 bool nonDegenerate = triangulateForLinearize(cameras);
339 if (nonDegenerate) {
340 cameras.project2(*result_, nullptr, &E);
341 }
342 return nonDegenerate;
343 }
344
345 /**
346 * Triangulate and compute derivative of error with respect to point
347 * @return whether triangulation worked
348 */
349 bool triangulateAndComputeE(Matrix& E, const Values& values) const {
350 Cameras cameras = this->cameras(values);
351 return triangulateAndComputeE(E, cameras);
352 }
353
354 /// Compute F, E only (called below in both vanilla and SVD versions)
355 /// Assumes the point has been computed
356 /// Note E can be 2m*3 or 2m*2, in case point is degenerate
357 void computeJacobiansWithTriangulatedPoint(
358 typename Base::FBlocks& Fs, Matrix& E, Vector& b,
359 const Cameras& cameras) const {
360
361 if (!result_) {
362 // Handle degeneracy
363 // TODO check flag whether we should do this
364 Unit3 backProjected = cameras[0].backprojectPointAtInfinity(
365 this->measured_.at(0));
366 Base::computeJacobians(Fs, E, b, cameras, backProjected);
367 } else {
368 // valid result: just return Base version
369 Base::computeJacobians(Fs, E, b, cameras, *result_);
370 }
371 }
372
373 /// Version that takes values, and creates the point
374 bool triangulateAndComputeJacobians(
375 typename Base::FBlocks& Fs, Matrix& E, Vector& b,
376 const Values& values) const {
377 Cameras cameras = this->cameras(values);
378 bool nonDegenerate = triangulateForLinearize(cameras);
379 if (nonDegenerate)
380 computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
381 return nonDegenerate;
382 }
383
384 /// takes values
385 bool triangulateAndComputeJacobiansSVD(
386 typename Base::FBlocks& Fs, Matrix& Enull, Vector& b,
387 const Values& values) const {
388 Cameras cameras = this->cameras(values);
389 bool nonDegenerate = triangulateForLinearize(cameras);
390 if (nonDegenerate)
391 Base::computeJacobiansSVD(Fs, Enull, b, cameras, *result_);
392 return nonDegenerate;
393 }
394
395 /// Calculate vector of re-projection errors, before applying noise model
396 Vector reprojectionErrorAfterTriangulation(const Values& values) const {
397 Cameras cameras = this->cameras(values);
398 bool nonDegenerate = triangulateForLinearize(cameras);
399 if (nonDegenerate)
400 return Base::unwhitenedError(cameras, *result_);
401 else
402 return Vector::Zero(cameras.size() * 2);
403 }
404
405 /**
406 * Calculate the error of the factor.
407 * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
408 * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
409 * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
410 */
411 double totalReprojectionError(const Cameras& cameras,
412 std::optional<Point3> externalPoint = {}) const {
413
414 if (externalPoint)
415 result_ = TriangulationResult(*externalPoint);
416 else
417 result_ = triangulateSafe(cameras);
418
419 if (result_)
420 // All good, just use version in base class
421 return Base::totalReprojectionError(cameras, *result_);
422 else if (params_.degeneracyMode == HANDLE_INFINITY) {
423 // Otherwise, manage the exceptions with rotation-only factors
424 Unit3 backprojected = cameras.front().backprojectPointAtInfinity(
425 this->measured_.at(0));
426 return Base::totalReprojectionError(cameras, backprojected);
427 } else
428 // if we don't want to manage the exceptions we discard the factor
429 return 0.0;
430 }
431
432 /// Calculate total reprojection error
433 double error(const Values& values) const override {
434 if (this->active(values)) {
435 return totalReprojectionError(cameras: Base::cameras(values));
436 } else { // else of active flag
437 return 0.0;
438 }
439 }
440
441 /** return the landmark */
442 TriangulationResult point() const {
443 return result_;
444 }
445
446 /** COMPUTE the landmark */
447 TriangulationResult point(const Values& values) const {
448 Cameras cameras = this->cameras(values);
449 return triangulateSafe(cameras);
450 }
451
452 /// Is result valid?
453 bool isValid() const { return result_.valid(); }
454
455 /** return the degenerate state */
456 bool isDegenerate() const { return result_.degenerate(); }
457
458 /** return the cheirality status flag */
459 bool isPointBehindCamera() const { return result_.behindCamera(); }
460
461 /** return the outlier state */
462 bool isOutlier() const { return result_.outlier(); }
463
464 /** return the farPoint state */
465 bool isFarPoint() const { return result_.farPoint(); }
466
467 private:
468
469#if GTSAM_ENABLE_BOOST_SERIALIZATION ///
470 /// Serialization function
471 friend class boost::serialization::access;
472 template<class ARCHIVE>
473 void serialize(ARCHIVE & ar, const unsigned int version) {
474 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
475 ar & BOOST_SERIALIZATION_NVP(params_);
476 ar & BOOST_SERIALIZATION_NVP(result_);
477 ar & BOOST_SERIALIZATION_NVP(cameraPosesTriangulation_);
478 }
479#endif
480}
481;
482
483/// traits
484template<class CAMERA>
485struct traits<SmartProjectionFactor<CAMERA> > : public Testable<
486 SmartProjectionFactor<CAMERA> > {
487};
488
489} // \ namespace gtsam
490