gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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:
52 typedef SmartProjectionFactor<CAMERA> SmartProjectionCameraFactor;
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.
utility functions for loading datasets
Base class to create smart factors on poses or cameras.
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
Template to create a binary predicate.
Definition Testable.h:111
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
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