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