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 
26 #include <gtsam/inference/Symbol.h>
27 #include <gtsam/slam/dataset.h>
28 
29 #include <optional>
30 #include <vector>
31 
32 namespace gtsam {
33 
43 template<class CAMERA>
44 class SmartProjectionFactor: public SmartFactorBase<CAMERA> {
45 
46 public:
47 
48 private:
52 
53 protected:
54 
59 
63  mutable std::vector<Pose3, Eigen::aligned_allocator<Pose3> >
65 
67  public:
68 
70  typedef std::shared_ptr<This> shared_ptr;
71 
73  typedef CAMERA Camera;
75 
80 
87  const SharedNoiseModel& sharedNoiseModel,
89  : Base(sharedNoiseModel),
90  params_(params),
91  result_(TriangulationResult::Degenerate()) {}
92 
95  }
96 
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 
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 
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++) {
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) {
158  cameraPosesTriangulation_.reserve(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 
174 
175  size_t m = cameras.size();
176  if (m < 2) // if we have a single pose the corresponding factor is uninformative
178 
179  bool retriangulate = decideIfTriangulate(cameras);
180  if (retriangulate)
182  params_.triangulation);
183  return result_;
184  }
185 
193  triangulateSafe(cameras); // imperative, might reset result_
194  return bool(result_);
195  }
196 
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 
213 
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;
227 
228  // Whiten using noise model
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 {
244  else
245  // failed: return empty
246  return std::shared_ptr<RegularImplicitSchurFactor<CAMERA> >();
247  }
248 
250  std::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
251  const Cameras& cameras, double lambda) const {
254  else
255  // failed: return empty
256  return std::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->keys_);
257  }
258 
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 
266  std::shared_ptr<JacobianFactor> createJacobianSVDFactor(
267  const Cameras& cameras, double lambda) const {
270  else
271  // failed: return empty
272  return std::make_shared<JacobianFactorSVD<Base::Dim, 2> >(this->keys_);
273  }
274 
276  virtual std::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
277  const Values& values, double lambda = 0.0) const {
278  return createHessianFactor(this->cameras(values), lambda);
279  }
280 
282  virtual std::shared_ptr<RegularImplicitSchurFactor<CAMERA> > linearizeToImplicit(
283  const Values& values, double lambda = 0.0) const {
284  return createRegularImplicitSchurFactor(this->cameras(values), lambda);
285  }
286 
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 
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:
304  case IMPLICIT_SCHUR:
306  case JACOBIAN_SVD:
308  case JACOBIAN_Q:
310  default:
311  throw std::runtime_error("SmartFactorlinearize: unknown mode");
312  }
313  }
314 
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 
328  std::shared_ptr<GaussianFactor> linearize(
329  const Values& values) const override {
330  return linearizeDamped(values);
331  }
332 
338  bool nonDegenerate = triangulateForLinearize(cameras);
339  if (nonDegenerate) {
340  cameras.project2(*result_, nullptr, &E);
341  }
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 
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  std::optional<Point3> externalPoint = {}) 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 
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 
469 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
470  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 
484 template<class CAMERA>
485 struct traits<SmartProjectionFactor<CAMERA> > : public Testable<
486  SmartProjectionFactor<CAMERA> > {
487 };
488 
489 } // \ namespace gtsam
gtsam::SmartProjectionParams::triangulation
TriangulationParameters triangulation
Definition: SmartFactorParams.h:49
gtsam::SmartProjectionFactor::error
double error(const Values &values) const override
Calculate total reprojection error.
Definition: SmartProjectionFactor.h:433
SmartFactorBase.h
Base class to create smart factors on poses or cameras.
gtsam::SmartFactorBase::unwhitenedError
Vector unwhitenedError(const Cameras &cameras, const POINT &point, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) const
Definition: SmartFactorBase.h:209
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
gtsam::SmartProjectionFactor::triangulateAndComputeE
bool triangulateAndComputeE(Matrix &E, const Values &values) const
Definition: SmartProjectionFactor.h:349
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
SmartFactorParams.h
Collect common parameters for SmartProjection and SmartStereoProjection factors.
gtsam::HESSIAN
@ HESSIAN
Definition: SmartFactorParams.h:31
gtsam::TriangulationResult::outlier
bool outlier() const
Definition: triangulation.h:668
gtsam::SmartProjectionParams
Definition: SmartFactorParams.h:42
gtsam::SmartProjectionFactor::triangulateAndComputeJacobians
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
gtsam::SmartProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartProjectionFactor.h:114
gtsam::SmartFactorBase::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: SmartFactorBase.h:176
gtsam::CameraSet::project2
ZVector project2(const POINT &point, FBlocks *Fs=nullptr, Matrix *E=nullptr) const
Definition: CameraSet.h:108
gtsam::JACOBIAN_Q
@ JACOBIAN_Q
Definition: SmartFactorParams.h:31
gtsam::SmartProjectionFactor::isOutlier
bool isOutlier() const
Definition: SmartProjectionFactor.h:462
gtsam::SmartFactorBase::Fs
FBlocks Fs
Definition: SmartFactorBase.h:86
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::SmartProjectionFactor::SmartProjectionCameraFactor
SmartProjectionFactor< CAMERA > SmartProjectionCameraFactor
Definition: SmartProjectionFactor.h:51
gtsam::SmartProjectionFactor::params_
SmartProjectionParams params_
Definition: SmartProjectionFactor.h:57
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::Factor
Definition: Factor.h:70
gtsam::SmartFactorBase::Dim
static const int Dim
Camera dimension.
Definition: SmartFactorBase.h:61
gtsam::SmartProjectionFactor::point
TriangulationResult point(const Values &values) const
Definition: SmartProjectionFactor.h:447
gtsam::SmartProjectionFactor::createJacobianQFactor
std::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Values &values, double lambda) const
Create JacobianFactorQ factor, takes values.
Definition: SmartProjectionFactor.h:260
gtsam::SmartProjectionFactor::isDegenerate
bool isDegenerate() const
Definition: SmartProjectionFactor.h:456
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::SmartProjectionFactor::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, std::optional< Point3 > externalPoint={}) const
Definition: SmartProjectionFactor.h:411
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Definition: NonlinearFactor.h:141
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::SmartProjectionFactor::computeJacobiansWithTriangulatedPoint
void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
Definition: SmartProjectionFactor.h:357
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
triangulation.h
Functions for triangulation.
gtsam::SmartProjectionFactor::triangulateSafe
TriangulationResult triangulateSafe(const Cameras &cameras) const
Call gtsam::triangulateSafe iff we need to re-triangulate.
Definition: SmartProjectionFactor.h:173
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
gtsam::SmartProjectionFactor::isValid
bool isValid() const
Is result valid?
Definition: SmartProjectionFactor.h:453
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::SmartProjectionFactor::linearizeDamped
std::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double lambda=0.0) const
Definition: SmartProjectionFactor.h:320
gtsam::SmartProjectionFactor::Cameras
CameraSet< CAMERA > Cameras
Definition: SmartProjectionFactor.h:74
gtsam::SmartProjectionParams::degeneracyMode
DegeneracyMode degeneracyMode
How to linearize the factor.
Definition: SmartFactorParams.h:45
gtsam::ZERO_ON_DEGENERACY
@ ZERO_ON_DEGENERACY
Definition: SmartFactorParams.h:36
gtsam::HANDLE_INFINITY
@ HANDLE_INFINITY
Definition: SmartFactorParams.h:36
gtsam::SmartProjectionParams::linearizationMode
LinearizationMode linearizationMode
How to linearize the factor.
Definition: SmartFactorParams.h:44
gtsam::IMPLICIT_SCHUR
@ IMPLICIT_SCHUR
Definition: SmartFactorParams.h:31
gtsam::SmartProjectionFactor::linearizeToHessian
virtual std::shared_ptr< RegularHessianFactor< Base::Dim > > linearizeToHessian(const Values &values, double lambda=0.0) const
Linearize to a Hessianfactor.
Definition: SmartProjectionFactor.h:276
gtsam::SmartProjectionFactor::triangulateAndComputeE
bool triangulateAndComputeE(Matrix &E, const Cameras &cameras) const
Definition: SmartProjectionFactor.h:337
gtsam::SmartProjectionFactor::linearizeToImplicit
virtual std::shared_ptr< RegularImplicitSchurFactor< CAMERA > > linearizeToImplicit(const Values &values, double lambda=0.0) const
Linearize to an Implicit Schur factor.
Definition: SmartProjectionFactor.h:282
gtsam::SmartProjectionFactor::result_
TriangulationResult result_
result from triangulateSafe
Definition: SmartProjectionFactor.h:62
dataset.h
utility functions for loading datasets
gtsam::SmartFactorBase::createRegularImplicitSchurFactor
std::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:387
gtsam::SmartFactorBase::whitenJacobians
void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
Whiten the Jacobians computed by computeJacobians using noiseModel_.
Definition: SmartFactorBase.h:378
gtsam::CameraSet::SchurComplement
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)
Definition: CameraSet.h:174
gtsam::KeyFormatter
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
gtsam::SmartProjectionFactor::linearizeToJacobian
virtual std::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > linearizeToJacobian(const Values &values, double lambda=0.0) const
Linearize to a JacobianfactorQ.
Definition: SmartProjectionFactor.h:288
gtsam::JACOBIAN_SVD
@ JACOBIAN_SVD
Definition: SmartFactorParams.h:31
gtsam::SmartProjectionFactor::createJacobianQFactor
std::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Cameras &cameras, double lambda) const
Create JacobianFactorQ factor.
Definition: SmartProjectionFactor.h:250
gtsam::SmartProjectionFactor::reprojectionErrorAfterTriangulation
Vector reprojectionErrorAfterTriangulation(const Values &values) const
Calculate vector of re-projection errors, before applying noise model.
Definition: SmartProjectionFactor.h:396
gtsam::SmartFactorBase::createJacobianQFactor
std::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:400
gtsam::TriangulationResult::valid
bool valid() const
Definition: triangulation.h:666
gtsam::SmartProjectionFactor::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition: SmartProjectionFactor.h:328
Symbol.h
gtsam::SmartProjectionFactor::SmartProjectionFactor
SmartProjectionFactor()
Definition: SmartProjectionFactor.h:79
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:698
conf.version
version
Definition: gtsam/3rdparty/GeographicLib/python/doc/conf.py:67
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::SmartProjectionFactor::createHessianFactor
std::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:198
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::SmartProjectionFactor::triangulateForLinearize
bool triangulateForLinearize(const Cameras &cameras) const
Possibly re-triangulate before calculating Jacobians.
Definition: SmartProjectionFactor.h:192
gtsam::SmartProjectionFactor::isFarPoint
bool isFarPoint() const
Definition: SmartProjectionFactor.h:465
gtsam::SmartProjectionFactor::triangulateAndComputeJacobiansSVD
bool triangulateAndComputeJacobiansSVD(typename Base::FBlocks &Fs, Matrix &Enull, Vector &b, const Values &values) const
takes values
Definition: SmartProjectionFactor.h:385
gtsam::SmartFactorBase::createJacobianSVDFactor
std::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
Definition: SmartFactorBase.h:417
gtsam::TriangulationResult::Degenerate
static TriangulationResult Degenerate()
Definition: triangulation.h:656
gtsam::SmartFactorBase::cameras
virtual Cameras cameras(const Values &values) const
Collect all cameras: important that in key order.
Definition: SmartFactorBase.h:163
lambda
static double lambda[]
Definition: jv.c:524
gtsam::SmartProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: SmartProjectionFactor.h:102
gtsam::TriangulationResult::degenerate
bool degenerate() const
Definition: triangulation.h:667
E
DiscreteKey E(5, 2)
gtsam::SmartFactorBase::FBlocks
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Definition: SmartFactorBase.h:64
gtsam::SmartFactorBase
Base class for smart factors. This base class has no internal point, but it has a measurement,...
Definition: SmartFactorBase.h:51
gtsam::b
const G & b
Definition: Group.h:79
gtsam::SmartProjectionFactor::~SmartProjectionFactor
~SmartProjectionFactor() override
Definition: SmartProjectionFactor.h:94
gtsam::SmartProjectionFactor
Definition: SmartProjectionFactor.h:44
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:127
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:88
gtsam::traits
Definition: Group.h:36
gtsam::SmartProjectionFactor::isPointBehindCamera
bool isPointBehindCamera() const
Definition: SmartProjectionFactor.h:459
gtsam::Values
Definition: Values.h:65
gtsam::TriangulationResult::behindCamera
bool behindCamera() const
Definition: triangulation.h:670
gtsam::SmartProjectionFactor::SmartProjectionFactor
SmartProjectionFactor(const SharedNoiseModel &sharedNoiseModel, const SmartProjectionParams &params=SmartProjectionParams())
Definition: SmartProjectionFactor.h:86
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::SmartFactorBase::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, const POINT &point) const
Definition: SmartFactorBase.h:298
gtsam::SmartProjectionParams::retriangulationThreshold
double retriangulationThreshold
threshold to decide whether to re-triangulate
Definition: SmartFactorParams.h:50
gtsam::SmartProjectionFactor::Camera
CAMERA Camera
shorthand for a set of cameras
Definition: SmartProjectionFactor.h:73
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::TriangulationResult
Definition: triangulation.h:638
gtsam::SmartProjectionFactor::This
SmartProjectionFactor< CAMERA > This
Definition: SmartProjectionFactor.h:50
gtsam::SmartProjectionFactor::cameraPosesTriangulation_
std::vector< Pose3, Eigen::aligned_allocator< Pose3 > > cameraPosesTriangulation_
current triangulation poses
Definition: SmartProjectionFactor.h:64
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
Base
Definition: test_virtual_functions.cpp:156
gtsam::SmartProjectionFactor::linearizeDamped
std::shared_ptr< GaussianFactor > linearizeDamped(const Cameras &cameras, const double lambda=0.0) const
Definition: SmartProjectionFactor.h:298
gtsam::SmartFactorBase::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartFactorBase.h:189
gtsam::SmartProjectionFactor::point
TriangulationResult point() const
Definition: SmartProjectionFactor.h:442
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:53
gtsam::SmartProjectionFactor::createRegularImplicitSchurFactor
std::shared_ptr< RegularImplicitSchurFactor< CAMERA > > createRegularImplicitSchurFactor(const Cameras &cameras, double lambda) const
Definition: SmartProjectionFactor.h:240
gtsam::SmartProjectionFactor::Base
SmartFactorBase< CAMERA > Base
Definition: SmartProjectionFactor.h:49
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::SmartFactorBase::computeJacobiansSVD
void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
Definition: SmartFactorBase.h:331
gtsam::TriangulationResult::farPoint
bool farPoint() const
Definition: triangulation.h:669
gtsam::SmartFactorBase::measured_
ZVector measured_
Definition: SmartFactorBase.h:80
gtsam::SmartProjectionFactor::createJacobianSVDFactor
std::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, double lambda) const
Different (faster) way to compute a JacobianFactorSVD factor.
Definition: SmartProjectionFactor.h:266
gtsam::SmartFactorBase::computeJacobians
void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const
Definition: SmartFactorBase.h:316
gtsam::SmartProjectionFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionFactor.h:70


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:30