gtsam  4.1.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 
23 #include <gtsam/slam/SmartFactorParams.h>
24 
26 #include <gtsam/inference/Symbol.h>
27 #include <gtsam/slam/dataset.h>
28 
29 #include <boost/optional.hpp>
30 #include <boost/make_shared.hpp>
31 #include <vector>
32 
33 namespace gtsam {
34 
44 template<class CAMERA>
45 class SmartProjectionFactor: public SmartFactorBase<CAMERA> {
46 
47 public:
48 
49 private:
53 
54 protected:
55 
58  SmartProjectionParams params_;
60 
64  mutable std::vector<Pose3, Eigen::aligned_allocator<Pose3> > cameraPosesTriangulation_;
65 
67 public:
68 
70  typedef boost::shared_ptr<This> shared_ptr;
71 
74 
79 
86  const SharedNoiseModel& sharedNoiseModel,
88  : Base(sharedNoiseModel),
89  params_(params),
90  result_(TriangulationResult::Degenerate()) {}
91 
94  }
95 
101  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
102  DefaultKeyFormatter) const override {
103  std::cout << s << "SmartProjectionFactor\n";
104  std::cout << "linearizationMode:\n" << params_.linearizationMode
105  << std::endl;
106  std::cout << "triangulationParameters:\n" << params_.triangulation
107  << std::endl;
108  std::cout << "result:\n" << result_ << std::endl;
109  Base::print("", keyFormatter);
110  }
111 
113  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
114  const This *e = dynamic_cast<const This*>(&p);
115  return e && params_.linearizationMode == e->params_.linearizationMode
116  && Base::equals(p, tol);
117  }
118 
120  bool decideIfTriangulate(const Cameras& cameras) const {
121  // several calls to linearize will be done from the same linearization point, hence it is not needed to re-triangulate
122  // Note that this is not yet "selecting linearization", that will come later, and we only check if the
123  // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point
124 
125  size_t m = cameras.size();
126 
127  bool retriangulate = false;
128 
129  // if we do not have a previous linearization point or the new linearization point includes more poses
130  if (cameraPosesTriangulation_.empty()
131  || cameras.size() != cameraPosesTriangulation_.size())
132  retriangulate = true;
133 
134  if (!retriangulate) {
135  for (size_t i = 0; i < cameras.size(); i++) {
136  if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
137  params_.retriangulationThreshold)) {
138  retriangulate = true; // at least two poses are different, hence we retriangulate
139  break;
140  }
141  }
142  }
143 
144  if (retriangulate) { // we store the current poses used for triangulation
146  cameraPosesTriangulation_.reserve(m);
147  for (size_t i = 0; i < m; i++)
148  // cameraPosesTriangulation_[i] = cameras[i].pose();
149  cameraPosesTriangulation_.push_back(cameras[i].pose());
150  }
151 
152  return retriangulate; // if we arrive to this point all poses are the same and we don't need re-triangulation
153  }
154 
157 
158  size_t m = cameras.size();
159  if (m < 2) // if we have a single pose the corresponding factor is uninformative
160  return TriangulationResult::Degenerate();
161 
162  bool retriangulate = decideIfTriangulate(cameras);
163  if (retriangulate)
165  params_.triangulation);
166  return result_;
167  }
168 
171  triangulateSafe(cameras); // imperative, might reset result_
172  return bool(result_);
173  }
174 
176  boost::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor(
177  const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping =
178  false) const {
179 
180  size_t numKeys = this->keys_.size();
181  // Create structures for Hessian Factors
182  KeyVector js;
183  std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
184  std::vector<Vector> gs(numKeys);
185 
186  if (this->measured_.size() != cameras.size())
187  throw std::runtime_error("SmartProjectionHessianFactor: this->measured_"
188  ".size() inconsistent with input");
189 
191 
192  if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
193  // failed: return"empty" Hessian
194  for(Matrix& m: Gs)
195  m = Matrix::Zero(Base::Dim, Base::Dim);
196  for(Vector& v: gs)
197  v = Vector::Zero(Base::Dim);
198  return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
199  Gs, gs, 0.0);
200  }
201 
202  // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
203  std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> > Fblocks;
204  Matrix E;
205  Vector b;
207 
208  // Whiten using noise model
209  Base::whitenJacobians(Fblocks, E, b);
210 
211  // build augmented hessian
212  SymmetricBlockMatrix augmentedHessian = //
213  Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping);
214 
215  return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
216  augmentedHessian);
217  }
218 
219  // create factor
220  boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > createRegularImplicitSchurFactor(
221  const Cameras& cameras, double lambda) const {
224  else
225  // failed: return empty
226  return boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> >();
227  }
228 
230  boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
231  const Cameras& cameras, double lambda) const {
233  return Base::createJacobianQFactor(cameras, *result_, lambda);
234  else
235  // failed: return empty
236  return boost::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->keys_);
237  }
238 
240  boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
241  const Values& values, double lambda) const {
242  return createJacobianQFactor(this->cameras(values), lambda);
243  }
244 
246  boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
247  const Cameras& cameras, double lambda) const {
250  else
251  // failed: return empty
252  return boost::make_shared<JacobianFactorSVD<Base::Dim, 2> >(this->keys_);
253  }
254 
256  virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
257  const Values& values, double lambda = 0.0) const {
258  return createHessianFactor(this->cameras(values), lambda);
259  }
260 
262  virtual boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > linearizeToImplicit(
263  const Values& values, double lambda = 0.0) const {
264  return createRegularImplicitSchurFactor(this->cameras(values), lambda);
265  }
266 
268  virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > linearizeToJacobian(
269  const Values& values, double lambda = 0.0) const {
270  return createJacobianQFactor(this->cameras(values), lambda);
271  }
272 
278  boost::shared_ptr<GaussianFactor> linearizeDamped(const Cameras& cameras,
279  const double lambda = 0.0) const {
280  // depending on flag set on construction we may linearize to different linear factors
281  switch (params_.linearizationMode) {
282  case HESSIAN:
283  return createHessianFactor(cameras, lambda);
284  case IMPLICIT_SCHUR:
285  return createRegularImplicitSchurFactor(cameras, lambda);
286  case JACOBIAN_SVD:
287  return createJacobianSVDFactor(cameras, lambda);
288  case JACOBIAN_Q:
289  return createJacobianQFactor(cameras, lambda);
290  default:
291  throw std::runtime_error("SmartFactorlinearize: unknown mode");
292  }
293  }
294 
300  boost::shared_ptr<GaussianFactor> linearizeDamped(const Values& values,
301  const double lambda = 0.0) const {
302  // depending on flag set on construction we may linearize to different linear factors
303  Cameras cameras = this->cameras(values);
304  return linearizeDamped(cameras, lambda);
305  }
306 
308  boost::shared_ptr<GaussianFactor> linearize(
309  const Values& values) const override {
310  return linearizeDamped(values);
311  }
312 
317  bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
318  bool nonDegenerate = triangulateForLinearize(cameras);
319  if (nonDegenerate)
320  cameras.project2(*result_, boost::none, E);
321  return nonDegenerate;
322  }
323 
328  bool triangulateAndComputeE(Matrix& E, const Values& values) const {
329  Cameras cameras = this->cameras(values);
330  return triangulateAndComputeE(E, cameras);
331  }
332 
337  std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> >& Fblocks, Matrix& E, Vector& b,
338  const Cameras& cameras) const {
339 
340  if (!result_) {
341  // Handle degeneracy
342  // TODO check flag whether we should do this
343  Unit3 backProjected = cameras[0].backprojectPointAtInfinity(
344  this->measured_.at(0));
345  Base::computeJacobians(Fblocks, E, b, cameras, backProjected);
346  } else {
347  // valid result: just return Base version
348  Base::computeJacobians(Fblocks, E, b, cameras, *result_);
349  }
350  }
351 
354  std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> >& Fblocks, Matrix& E, Vector& b,
355  const Values& values) const {
356  Cameras cameras = this->cameras(values);
357  bool nonDegenerate = triangulateForLinearize(cameras);
358  if (nonDegenerate)
360  return nonDegenerate;
361  }
362 
365  std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> >& Fblocks, Matrix& Enull, Vector& b,
366  const Values& values) const {
367  Cameras cameras = this->cameras(values);
368  bool nonDegenerate = triangulateForLinearize(cameras);
369  if (nonDegenerate)
370  Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_);
371  return nonDegenerate;
372  }
373 
375  Vector reprojectionErrorAfterTriangulation(const Values& values) const {
376  Cameras cameras = this->cameras(values);
377  bool nonDegenerate = triangulateForLinearize(cameras);
378  if (nonDegenerate)
380  else
381  return Vector::Zero(cameras.size() * 2);
382  }
383 
391  boost::optional<Point3> externalPoint = boost::none) const {
392 
393  if (externalPoint)
394  result_ = TriangulationResult(*externalPoint);
395  else
397 
398  if (result_)
399  // All good, just use version in base class
401  else if (params_.degeneracyMode == HANDLE_INFINITY) {
402  // Otherwise, manage the exceptions with rotation-only factors
403  Unit3 backprojected = cameras.front().backprojectPointAtInfinity(
404  this->measured_.at(0));
405  return Base::totalReprojectionError(cameras, backprojected);
406  } else
407  // if we don't want to manage the exceptions we discard the factor
408  return 0.0;
409  }
410 
412  double error(const Values& values) const override {
413  if (this->active(values)) {
414  return totalReprojectionError(Base::cameras(values));
415  } else { // else of active flag
416  return 0.0;
417  }
418  }
419 
422  return result_;
423  }
424 
426  TriangulationResult point(const Values& values) const {
427  Cameras cameras = this->cameras(values);
428  return triangulateSafe(cameras);
429  }
430 
432  bool isValid() const { return result_.valid(); }
433 
435  bool isDegenerate() const { return result_.degenerate(); }
436 
438  bool isPointBehindCamera() const { return result_.behindCamera(); }
439 
441  bool isOutlier() const { return result_.outlier(); }
442 
444  bool isFarPoint() const { return result_.farPoint(); }
445 
446  private:
447 
449  friend class boost::serialization::access;
450  template<class ARCHIVE>
451  void serialize(ARCHIVE & ar, const unsigned int version) {
452  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
453  ar & BOOST_SERIALIZATION_NVP(params_);
454  ar & BOOST_SERIALIZATION_NVP(result_);
455  ar & BOOST_SERIALIZATION_NVP(cameraPosesTriangulation_);
456  }
457 }
458 ;
459 
461 template<class CAMERA>
462 struct traits<SmartProjectionFactor<CAMERA> > : public Testable<
463  SmartProjectionFactor<CAMERA> > {
464 };
465 
466 } // \ namespace gtsam
gtsam::SmartProjectionParams::retriangulationThreshold
double retriangulationThreshold
threshold to decide whether to re-triangulate
Definition: SmartFactorParams.h:50
gtsam::SmartProjectionFactor::isPointBehindCamera
bool isPointBehindCamera() const
return the cheirality status flag
Definition: SmartProjectionFactor.h:438
gtsam::SmartFactorBase::createRegularImplicitSchurFactor
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:340
gtsam::SmartProjectionParams
Definition: SmartFactorParams.h:42
gtsam::SmartProjectionFactor::computeJacobiansWithTriangulatedPoint
void computeJacobiansWithTriangulatedPoint(std::vector< typename Base::MatrixZD, Eigen::aligned_allocator< typename Base::MatrixZD > > &Fblocks, 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:336
gtsam::SmartProjectionFactor::triangulateSafe
TriangulationResult triangulateSafe(const Cameras &cameras) const
triangulateSafe
Definition: SmartProjectionFactor.h:156
gtsam::SmartFactorBase::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, const POINT &point) const
Calculate the error of the factor.
Definition: SmartFactorBase.h:255
gtsam::SmartProjectionFactor::point
TriangulationResult point(const Values &values) const
COMPUTE the landmark.
Definition: SmartProjectionFactor.h:426
gtsam::SmartFactorBase::measured_
ZVector measured_
2D measurement and noise model for each of the m views We keep a copy of measurements for I/O and com...
Definition: SmartFactorBase.h:76
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::SmartProjectionFactor::isDegenerate
bool isDegenerate() const
return the degenerate state
Definition: SmartProjectionFactor.h:435
gtsam::CameraSet::project2
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:107
triangulation.h
Functions for triangulation.
gtsam::SmartProjectionFactor::decideIfTriangulate
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:120
gtsam::SmartFactorBase::createJacobianQFactor
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:355
gtsam::TriangulationResult
TriangulationResult is an optional point, along with the reasons why it is invalid.
Definition: triangulation.h:369
gtsam::SmartProjectionFactor::isFarPoint
bool isFarPoint() const
return the farPoint state
Definition: SmartProjectionFactor.h:444
gtsam::SmartProjectionFactor::triangulateAndComputeJacobiansSVD
bool triangulateAndComputeJacobiansSVD(std::vector< typename Base::MatrixZD, Eigen::aligned_allocator< typename Base::MatrixZD > > &Fblocks, Matrix &Enull, Vector &b, const Values &values) const
takes values
Definition: SmartProjectionFactor.h:364
gtsam::SmartProjectionParams::degeneracyMode
DegeneracyMode degeneracyMode
How to linearize the factor.
Definition: SmartFactorParams.h:45
gtsam::SmartProjectionFactor::point
TriangulationResult point() const
return the landmark
Definition: SmartProjectionFactor.h:421
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
gtsam::SmartProjectionFactor::result_
TriangulationResult result_
result from triangulateSafe
Definition: SmartProjectionFactor.h:63
gtsam::SmartProjectionFactor::SmartProjectionFactor
SmartProjectionFactor(const SharedNoiseModel &sharedNoiseModel, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition: SmartProjectionFactor.h:85
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:35
gtsam::SmartProjectionFactor::triangulateAndComputeE
bool triangulateAndComputeE(Matrix &E, const Cameras &cameras) const
Triangulate and compute derivative of error with respect to point.
Definition: SmartProjectionFactor.h:317
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::SmartProjectionFactor::createJacobianQFactor
boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Cameras &cameras, double lambda) const
create factor
Definition: SmartProjectionFactor.h:230
dataset.h
utility functions for loading datasets
gtsam::SmartProjectionFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionFactor.h:70
gtsam::SmartProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: SmartProjectionFactor.h:101
gtsam::SmartFactorBase::cameras
virtual Cameras cameras(const Values &values) const
Collect all cameras: important that in key order.
Definition: SmartFactorBase.h:163
gtsam::SmartProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartProjectionFactor.h:113
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::SmartProjectionFactor::triangulateForLinearize
bool triangulateForLinearize(const Cameras &cameras) const
triangulate
Definition: SmartProjectionFactor.h:170
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
gtsam::SmartFactorBase::whitenJacobians
void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
Whiten the Jacobians computed by computeJacobians using noiseModel_.
Definition: SmartFactorBase.h:331
gtsam::SmartProjectionFactor::reprojectionErrorAfterTriangulation
Vector reprojectionErrorAfterTriangulation(const Values &values) const
Calculate vector of re-projection errors, before applying noise model.
Definition: SmartProjectionFactor.h:375
gtsam::SmartFactorBase::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: SmartFactorBase.h:175
gtsam::SmartProjectionFactor::SmartProjectionFactor
SmartProjectionFactor()
Default constructor, only for serialization.
Definition: SmartProjectionFactor.h:78
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition: NonlinearFactor.h:106
gtsam::SmartFactorBase::computeJacobiansSVD
void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
SVD version.
Definition: SmartFactorBase.h:284
gtsam::SmartProjectionFactor::~SmartProjectionFactor
virtual ~SmartProjectionFactor()
Virtual destructor.
Definition: SmartProjectionFactor.h:93
gtsam::SmartProjectionParams::linearizationMode
LinearizationMode linearizationMode
How to linearize the factor.
Definition: SmartFactorParams.h:44
gtsam::SmartProjectionFactor::linearizeToHessian
virtual boost::shared_ptr< RegularHessianFactor< Base::Dim > > linearizeToHessian(const Values &values, double lambda=0.0) const
linearize to a Hessianfactor
Definition: SmartProjectionFactor.h:256
gtsam::SmartProjectionFactor::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
Calculate the error of the factor.
Definition: SmartProjectionFactor.h:390
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::KeyFormatter
boost::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
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::SmartProjectionFactor::createJacobianQFactor
boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Values &values, double lambda) const
Create a factor, takes values.
Definition: SmartProjectionFactor.h:240
gtsam::SmartFactorBase
Base class for smart factors This base class has no internal point, but it has a measurement,...
Definition: SmartFactorBase.h:47
gtsam::triangulateSafe
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
triangulateSafe: extensive checking of the outcome
Definition: triangulation.h:440
gtsam::SmartFactorBase::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartFactorBase.h:188
gtsam::SmartProjectionFactor::linearizeToImplicit
virtual boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > linearizeToImplicit(const Values &values, double lambda=0.0) const
linearize to an Implicit Schur factor
Definition: SmartProjectionFactor.h:262
gtsam::SmartFactorBase::unwhitenedError
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:203
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::SmartProjectionFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition: SmartProjectionFactor.h:308
gtsam::SmartProjectionFactor::linearizeToJacobian
virtual boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > linearizeToJacobian(const Values &values, double lambda=0.0) const
linearize to a JacobianfactorQ
Definition: SmartProjectionFactor.h:268
gtsam::SmartProjectionFactor::triangulateAndComputeE
bool triangulateAndComputeE(Matrix &E, const Values &values) const
Triangulate and compute derivative of error with respect to point.
Definition: SmartProjectionFactor.h:328
gtsam::SmartFactorBase::createJacobianSVDFactor
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
Return Jacobians as JacobianFactorSVD TODO lambda is currently ignored.
Definition: SmartFactorBase.h:372
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:52
SmartFactorBase.h
Base class to create smart factors on poses or cameras.
gtsam::SmartFactorBase::Dim
static const int Dim
Camera dimension.
Definition: SmartFactorBase.h:57
gtsam::SmartProjectionFactor::createHessianFactor
boost::shared_ptr< RegularHessianFactor< Base::Dim > > createHessianFactor(const Cameras &cameras, const double lambda=0.0, bool diagonalDamping=false) const
linearize returns a Hessianfactor that is an approximation of error(p)
Definition: SmartProjectionFactor.h:176
Symbol.h
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::SmartProjectionFactor
SmartProjectionFactor: triangulates point and keeps an estimate of it around.
Definition: SmartProjectionFactor.h:45
gtsam::SmartProjectionFactor::isValid
bool isValid() const
Is result valid?
Definition: SmartProjectionFactor.h:432
gtsam::CameraSet::SchurComplement
static SymmetricBlockMatrix SchurComplement(const FBlocks &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:149
gtsam::SmartProjectionFactor::linearizeDamped
boost::shared_ptr< GaussianFactor > linearizeDamped(const Cameras &cameras, const double lambda=0.0) const
Linearize to Gaussian Factor.
Definition: SmartProjectionFactor.h:278
gtsam::SmartProjectionFactor::linearizeDamped
boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double lambda=0.0) const
Linearize to Gaussian Factor.
Definition: SmartProjectionFactor.h:300
gtsam::SmartProjectionFactor::isOutlier
bool isOutlier() const
return the outlier state
Definition: SmartProjectionFactor.h:441
gtsam::SmartProjectionFactor::Cameras
CameraSet< CAMERA > Cameras
shorthand for a set of cameras
Definition: SmartProjectionFactor.h:73
gtsam::SmartProjectionFactor::error
double error(const Values &values) const override
Calculate total reprojection error.
Definition: SmartProjectionFactor.h:412
gtsam::SmartProjectionFactor::cameraPosesTriangulation_
std::vector< Pose3, Eigen::aligned_allocator< Pose3 > > cameraPosesTriangulation_
current triangulation poses
Definition: SmartProjectionFactor.h:64
gtsam::SmartFactorBase::computeJacobians
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:273
gtsam::SmartProjectionFactor::triangulateAndComputeJacobians
bool triangulateAndComputeJacobians(std::vector< typename Base::MatrixZD, Eigen::aligned_allocator< typename Base::MatrixZD > > &Fblocks, Matrix &E, Vector &b, const Values &values) const
Version that takes values, and creates the point.
Definition: SmartProjectionFactor.h:353
gtsam::SmartProjectionFactor::createJacobianSVDFactor
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, double lambda) const
different (faster) way to compute Jacobian factor
Definition: SmartProjectionFactor.h:246