gtsam 4.2.0
gtsam
SmartProjectionFactor.h
Go to the documentation of this file.
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
20#pragma once
21
24
27#include <gtsam/slam/dataset.h>
28
29#include <boost/optional.hpp>
30#include <boost/make_shared.hpp>
31#include <vector>
32
33namespace gtsam {
34
44template<class CAMERA>
46
47public:
48
49private:
53
54protected:
55
60
64 mutable std::vector<Pose3, Eigen::aligned_allocator<Pose3> >
67
68 public:
69
71 typedef boost::shared_ptr<This> shared_ptr;
72
74 typedef CAMERA Camera;
76
81
88 const SharedNoiseModel& sharedNoiseModel,
90 : Base(sharedNoiseModel),
91 params_(params),
92 result_(TriangulationResult::Degenerate()) {}
93
96 }
97
103 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
104 DefaultKeyFormatter) const override {
105 std::cout << s << "SmartProjectionFactor\n";
106 std::cout << "linearizationMode: " << params_.linearizationMode
107 << std::endl;
108 std::cout << "triangulationParameters:\n" << params_.triangulation
109 << std::endl;
110 std::cout << "result:\n" << result_ << std::endl;
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 && params_.linearizationMode == e->params_.linearizationMode
118 && Base::equals(p, tol);
119 }
120
129 // Several calls to linearize will be done from the same linearization
130 // point, hence it is not needed to re-triangulate. Note that this is not
131 // yet "selecting linearization", that will come later, and we only check if
132 // the current linearization is the "same" (up to tolerance) w.r.t. the last
133 // time we triangulated the point.
134
135 size_t m = cameras.size();
136
137 bool retriangulate = false;
138
139 // Definitely true if we do not have a previous linearization point or the
140 // new linearization point includes more poses.
141 if (cameraPosesTriangulation_.empty()
142 || cameras.size() != cameraPosesTriangulation_.size())
143 retriangulate = true;
144
145 // Otherwise, check poses against cache.
146 if (!retriangulate) {
147 for (size_t i = 0; i < cameras.size(); i++) {
148 if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
149 params_.retriangulationThreshold)) {
150 retriangulate = true; // at least two poses are different, hence we retriangulate
151 break;
152 }
153 }
154 }
155
156 // Store the current poses used for triangulation if we will re-triangulate.
157 if (retriangulate) {
159 cameraPosesTriangulation_.reserve(m);
160 for (size_t i = 0; i < m; i++)
161 // cameraPosesTriangulation_[i] = cameras[i].pose();
162 cameraPosesTriangulation_.push_back(cameras[i].pose());
163 }
164
165 return retriangulate;
166 }
167
175
176 size_t m = cameras.size();
177 if (m < 2) // if we have a single pose the corresponding factor is uninformative
178 return TriangulationResult::Degenerate();
179
180 bool retriangulate = decideIfTriangulate(cameras);
181 if (retriangulate)
183 params_.triangulation);
184 return result_;
185 }
186
194 triangulateSafe(cameras); // imperative, might reset result_
195 return bool(result_);
196 }
197
199 boost::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor(
200 const Cameras& cameras, const double lambda = 0.0,
201 bool diagonalDamping = false) const {
202 size_t numKeys = this->keys_.size();
203 // Create structures for Hessian Factors
204 KeyVector js;
205 std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
206 std::vector<Vector> gs(numKeys);
207
208 if (this->measured_.size() != cameras.size())
209 throw std::runtime_error(
210 "SmartProjectionHessianFactor: this->measured_"
211 ".size() inconsistent with input");
212
214
215 if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
216 // failed: return"empty" Hessian
217 for (Matrix& m : Gs) m = Matrix::Zero(Base::Dim, Base::Dim);
218 for (Vector& v : gs) v = Vector::Zero(Base::Dim);
219 return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
220 Gs, gs, 0.0);
221 }
222
223 // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
224 typename Base::FBlocks Fs;
225 Matrix E;
226 Vector b;
228
229 // Whiten using noise model
230 Base::whitenJacobians(Fs, E, b);
231
232 // build augmented hessian
233 SymmetricBlockMatrix augmentedHessian = //
234 Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping);
235
236 return boost::make_shared<RegularHessianFactor<Base::Dim> >(
237 this->keys_, augmentedHessian);
238 }
239
240 // Create RegularImplicitSchurFactor factor.
241 boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > createRegularImplicitSchurFactor(
242 const Cameras& cameras, double lambda) const {
245 else
246 // failed: return empty
247 return boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> >();
248 }
249
251 boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
252 const Cameras& cameras, double lambda) const {
255 else
256 // failed: return empty
257 return boost::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->keys_);
258 }
259
261 boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
262 const Values& values, double lambda) const {
263 return createJacobianQFactor(this->cameras(values), lambda);
264 }
265
267 boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
268 const Cameras& cameras, double lambda) const {
271 else
272 // failed: return empty
273 return boost::make_shared<JacobianFactorSVD<Base::Dim, 2> >(this->keys_);
274 }
275
277 virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
278 const Values& values, double lambda = 0.0) const {
279 return createHessianFactor(this->cameras(values), lambda);
280 }
281
283 virtual boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > linearizeToImplicit(
284 const Values& values, double lambda = 0.0) const {
285 return createRegularImplicitSchurFactor(this->cameras(values), lambda);
286 }
287
289 virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > linearizeToJacobian(
290 const Values& values, double lambda = 0.0) const {
291 return createJacobianQFactor(this->cameras(values), lambda);
292 }
293
299 boost::shared_ptr<GaussianFactor> linearizeDamped(const Cameras& cameras,
300 const double lambda = 0.0) const {
301 // depending on flag set on construction we may linearize to different linear factors
302 switch (params_.linearizationMode) {
303 case HESSIAN:
304 return createHessianFactor(cameras, lambda);
305 case IMPLICIT_SCHUR:
306 return createRegularImplicitSchurFactor(cameras, lambda);
307 case JACOBIAN_SVD:
308 return createJacobianSVDFactor(cameras, lambda);
309 case JACOBIAN_Q:
310 return createJacobianQFactor(cameras, lambda);
311 default:
312 throw std::runtime_error("SmartFactorlinearize: unknown mode");
313 }
314 }
315
321 boost::shared_ptr<GaussianFactor> linearizeDamped(const Values& values,
322 const double lambda = 0.0) const {
323 // depending on flag set on construction we may linearize to different linear factors
324 Cameras cameras = this->cameras(values);
325 return linearizeDamped(cameras, lambda);
326 }
327
329 boost::shared_ptr<GaussianFactor> linearize(
330 const Values& values) const override {
331 return linearizeDamped(values);
332 }
333
338 bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
339 bool nonDegenerate = triangulateForLinearize(cameras);
340 if (nonDegenerate)
341 cameras.project2(*result_, boost::none, E);
342 return nonDegenerate;
343 }
344
349 bool triangulateAndComputeE(Matrix& E, const Values& values) const {
350 Cameras cameras = this->cameras(values);
352 }
353
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
370 }
371 }
372
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)
381 return nonDegenerate;
382 }
383
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)
392 return nonDegenerate;
393 }
394
396 Vector reprojectionErrorAfterTriangulation(const Values& values) const {
397 Cameras cameras = this->cameras(values);
398 bool nonDegenerate = triangulateForLinearize(cameras);
399 if (nonDegenerate)
401 else
402 return Vector::Zero(cameras.size() * 2);
403 }
404
412 boost::optional<Point3> externalPoint = boost::none) const {
413
414 if (externalPoint)
415 result_ = TriangulationResult(*externalPoint);
416 else
418
419 if (result_)
420 // All good, just use version in base class
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
433 double error(const Values& values) const override {
434 if (this->active(values)) {
436 } else { // else of active flag
437 return 0.0;
438 }
439 }
440
443 return result_;
444 }
445
447 TriangulationResult point(const Values& values) const {
448 Cameras cameras = this->cameras(values);
449 return triangulateSafe(cameras);
450 }
451
453 bool isValid() const { return result_.valid(); }
454
456 bool isDegenerate() const { return result_.degenerate(); }
457
459 bool isPointBehindCamera() const { return result_.behindCamera(); }
460
462 bool isOutlier() const { return result_.outlier(); }
463
465 bool isFarPoint() const { return result_.farPoint(); }
466
467 private:
468
471 template<class ARCHIVE>
472 void serialize(ARCHIVE & ar, const unsigned int version) {
473 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
474 ar & BOOST_SERIALIZATION_NVP(params_);
475 ar & BOOST_SERIALIZATION_NVP(result_);
476 ar & BOOST_SERIALIZATION_NVP(cameraPosesTriangulation_);
477 }
478}
479;
480
482template<class CAMERA>
483struct traits<SmartProjectionFactor<CAMERA> > : public Testable<
484 SmartProjectionFactor<CAMERA> > {
485};
486
487} // \ namespace gtsam
Functions for triangulation.
Collect common parameters for SmartProjection and SmartStereoProjection factors.
Base class to create smart factors on poses or cameras.
utility functions for loading datasets
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
triangulateSafe: extensive checking of the outcome
Definition: triangulation.h:680
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition: NoiseModel.h:724
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 blocks.
Definition: SymmetricBlockMatrix.h:52
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > > > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * ...
Definition: CameraSet.h:150
ZVector project2(const POINT &point, boost::optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Project a point (possibly Unit3 at infinity), with derivatives Note that F is a sparse block-diagonal...
Definition: CameraSet.h:108
TriangulationResult is an optional point, along with the reasons why it is invalid.
Definition: triangulation.h:626
Represents a 3D point on a unit sphere.
Definition: Unit3.h:43
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
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:65
Base class for smart factors.
Definition: SmartFactorBase.h:50
void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const
Compute F, E, and b (called below in both vanilla and SVD versions), where F is a vector of derivativ...
Definition: SmartFactorBase.h:285
boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > createRegularImplicitSchurFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
Return Jacobians as RegularImplicitSchurFactor with raw access.
Definition: SmartFactorBase.h:356
static const int Dim
Camera dimension.
Definition: SmartFactorBase.h:60
virtual Cameras cameras(const Values &values) const
Collect all cameras: important that in key order.
Definition: SmartFactorBase.h:162
double totalReprojectionError(const Cameras &cameras, const POINT &point) const
Calculate the error of the factor.
Definition: SmartFactorBase.h:267
void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
SVD version that produces smaller Jacobian matrices by doing an SVD decomposition on E,...
Definition: SmartFactorBase.h:300
ZVector measured_
Measurements for each of the m views.
Definition: SmartFactorBase.h:79
Vector unwhitenedError(const Cameras &cameras, const POINT &point, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives.
Definition: SmartFactorBase.h:204
void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
Whiten the Jacobians computed by computeJacobians using noiseModel_.
Definition: SmartFactorBase.h:347
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
Return Jacobians as JacobianFactorSVD.
Definition: SmartFactorBase.h:386
boost::shared_ptr< JacobianFactorQ< Dim, ZDim > > createJacobianQFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
Return Jacobians as JacobianFactorQ.
Definition: SmartFactorBase.h:369
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: SmartFactorBase.h:174
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartFactorBase.h:187
Definition: SmartFactorParams.h:42
LinearizationMode linearizationMode
How to linearize the factor.
Definition: SmartFactorParams.h:44
double retriangulationThreshold
threshold to decide whether to re-triangulate
Definition: SmartFactorParams.h:50
DegeneracyMode degeneracyMode
How to linearize the factor.
Definition: SmartFactorParams.h:45
SmartProjectionFactor: triangulates point and keeps an estimate of it around.
Definition: SmartProjectionFactor.h:45
bool decideIfTriangulate(const Cameras &cameras) const
Check if the new linearization point is the same as the one used for previous triangulation.
Definition: SmartProjectionFactor.h:128
boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Values &values, double lambda) const
Create JacobianFactorQ factor, takes values.
Definition: SmartProjectionFactor.h:261
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, double lambda) const
Different (faster) way to compute a JacobianFactorSVD factor.
Definition: SmartProjectionFactor.h:267
void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
Compute F, E only (called below in both vanilla and SVD versions) Assumes the point has been computed...
Definition: SmartProjectionFactor.h:357
virtual boost::shared_ptr< RegularHessianFactor< Base::Dim > > linearizeToHessian(const Values &values, double lambda=0.0) const
Linearize to a Hessianfactor.
Definition: SmartProjectionFactor.h:277
bool isOutlier() const
return the outlier state
Definition: SmartProjectionFactor.h:462
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: SmartProjectionFactor.h:103
bool triangulateAndComputeE(Matrix &E, const Values &values) const
Triangulate and compute derivative of error with respect to point.
Definition: SmartProjectionFactor.h:349
boost::shared_ptr< GaussianFactor > linearizeDamped(const Cameras &cameras, const double lambda=0.0) const
Linearize to Gaussian Factor.
Definition: SmartProjectionFactor.h:299
bool isFarPoint() const
return the farPoint state
Definition: SmartProjectionFactor.h:465
TriangulationResult result_
result from triangulateSafe
Definition: SmartProjectionFactor.h:63
boost::shared_ptr< RegularHessianFactor< Base::Dim > > createHessianFactor(const Cameras &cameras, const double lambda=0.0, bool diagonalDamping=false) const
Create a Hessianfactor that is an approximation of error(p).
Definition: SmartProjectionFactor.h:199
TriangulationResult triangulateSafe(const Cameras &cameras) const
Call gtsam::triangulateSafe iff we need to re-triangulate.
Definition: SmartProjectionFactor.h:174
~SmartProjectionFactor() override
Virtual destructor.
Definition: SmartProjectionFactor.h:95
double error(const Values &values) const override
Calculate total reprojection error.
Definition: SmartProjectionFactor.h:433
bool isValid() const
Is result valid?
Definition: SmartProjectionFactor.h:453
std::vector< Pose3, Eigen::aligned_allocator< Pose3 > > cameraPosesTriangulation_
current triangulation poses
Definition: SmartProjectionFactor.h:65
bool triangulateAndComputeE(Matrix &E, const Cameras &cameras) const
Triangulate and compute derivative of error with respect to point.
Definition: SmartProjectionFactor.h:338
virtual boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > linearizeToImplicit(const Values &values, double lambda=0.0) const
Linearize to an Implicit Schur factor.
Definition: SmartProjectionFactor.h:283
double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
Calculate the error of the factor.
Definition: SmartProjectionFactor.h:411
virtual boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > linearizeToJacobian(const Values &values, double lambda=0.0) const
Linearize to a JacobianfactorQ.
Definition: SmartProjectionFactor.h:289
bool isDegenerate() const
return the degenerate state
Definition: SmartProjectionFactor.h:456
bool triangulateAndComputeJacobians(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const
Version that takes values, and creates the point.
Definition: SmartProjectionFactor.h:374
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartProjectionFactor.h:115
CAMERA Camera
shorthand for a set of cameras
Definition: SmartProjectionFactor.h:74
boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Cameras &cameras, double lambda) const
Create JacobianFactorQ factor.
Definition: SmartProjectionFactor.h:251
friend class boost::serialization::access
Serialization function.
Definition: SmartProjectionFactor.h:470
TriangulationResult point(const Values &values) const
COMPUTE the landmark.
Definition: SmartProjectionFactor.h:447
SmartProjectionFactor(const SharedNoiseModel &sharedNoiseModel, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition: SmartProjectionFactor.h:87
bool isPointBehindCamera() const
return the cheirality status flag
Definition: SmartProjectionFactor.h:459
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition: SmartProjectionFactor.h:329
bool triangulateAndComputeJacobiansSVD(typename Base::FBlocks &Fs, Matrix &Enull, Vector &b, const Values &values) const
takes values
Definition: SmartProjectionFactor.h:385
Vector reprojectionErrorAfterTriangulation(const Values &values) const
Calculate vector of re-projection errors, before applying noise model.
Definition: SmartProjectionFactor.h:396
TriangulationResult point() const
return the landmark
Definition: SmartProjectionFactor.h:442
bool triangulateForLinearize(const Cameras &cameras) const
Possibly re-triangulate before calculating Jacobians.
Definition: SmartProjectionFactor.h:193
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionFactor.h:71
SmartProjectionFactor()
Default constructor, only for serialization.
Definition: SmartProjectionFactor.h:80
boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double lambda=0.0) const
Linearize to Gaussian Factor.
Definition: SmartProjectionFactor.h:321