SmartStereoProjectionFactor.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 
21 #pragma once
22 
23 #include <gtsam/geometry/Pose3.h>
26 #include <gtsam/inference/Symbol.h>
30 #include <gtsam/slam/dataset.h>
31 #include <gtsam_unstable/dllexport.h>
32 
33 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
34 #include <boost/serialization/optional.hpp>
35 #endif
36 
37 #include <optional>
38 #include <vector>
39 
40 namespace gtsam {
41 
42 /*
43  * Parameters for the smart stereo projection factors (identical to the SmartProjectionParams)
44  */
46 
56  : public SmartFactorBase<StereoCamera> {
57  private:
58 
60 
61 protected:
62 
67 
71  mutable std::vector<Pose3> cameraPosesTriangulation_;
72 
74 public:
75 
77  typedef std::shared_ptr<SmartStereoProjectionFactor> shared_ptr;
78 
81 
86 
93  const std::optional<Pose3> body_P_sensor = {}) :
94  Base(sharedNoiseModel, body_P_sensor), //
95  params_(params), //
97  }
98 
101  }
102 
108  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
109  DefaultKeyFormatter) const override {
110  std::cout << s << "SmartStereoProjectionFactor\n";
111  std::cout << "linearizationMode:\n" << params_.linearizationMode << std::endl;
112  std::cout << "triangulationParameters:\n" << params_.triangulation << std::endl;
113  std::cout << "result:\n" << result_ << std::endl;
114  Base::print("", keyFormatter);
115  }
116 
118  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
120  dynamic_cast<const SmartStereoProjectionFactor*>(&p);
121  return e && params_.linearizationMode == e->params_.linearizationMode
122  && Base::equals(p, tol);
123  }
124 
126  bool decideIfTriangulate(const Cameras& cameras) const {
127  // several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate
128  // Note that this is not yet "selecting linearization", that will come later, and we only check if the
129  // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point_
130 
131  size_t m = cameras.size();
132 
133  bool retriangulate = false;
134 
135  // if we do not have a previous linearization point_ or the new linearization point_ includes more poses
136  if (cameraPosesTriangulation_.empty()
137  || cameras.size() != cameraPosesTriangulation_.size())
138  retriangulate = true;
139 
140  if (!retriangulate) {
141  for (size_t i = 0; i < cameras.size(); i++) {
144  retriangulate = true; // at least two poses are different, hence we retriangulate
145  break;
146  }
147  }
148  }
149 
150  if (retriangulate) { // we store the current poses used for triangulation
152  cameraPosesTriangulation_.reserve(m);
153  for (size_t i = 0; i < m; i++)
154  // cameraPosesTriangulation_[i] = cameras[i].pose();
155  cameraPosesTriangulation_.push_back(cameras[i].pose());
156  }
157 
158  return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation
159  }
160 
161 // /// triangulateSafe
162 // size_t triangulateSafe(const Values& values) const {
163 // return triangulateSafe(this->cameras(values));
164 // }
165 
168 
169  size_t m = cameras.size();
170  bool retriangulate = decideIfTriangulate(cameras);
171 
172  // triangulate stereo measurements by treating each stereocamera as a pair of monocular cameras
173  MonoCameras monoCameras;
174  MonoMeasurements monoMeasured;
175  for(size_t i = 0; i < m; i++) {
176  const Pose3 leftPose = cameras[i].pose();
177  const Cal3_S2 monoCal = cameras[i].calibration().calibration();
178  const MonoCamera leftCamera_i(leftPose,monoCal);
179  const Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0));
180  const Pose3 rightPose = leftPose.compose( left_Pose_right );
181  const MonoCamera rightCamera_i(rightPose,monoCal);
182  const StereoPoint2 zi = measured_[i];
183  monoCameras.push_back(leftCamera_i);
184  monoMeasured.push_back(Point2(zi.uL(),zi.v()));
185  if(!std::isnan(zi.uR())){ // if right point is valid
186  monoCameras.push_back(rightCamera_i);
187  monoMeasured.push_back(Point2(zi.uR(),zi.v()));
188  }
189  }
190  if (retriangulate)
191  result_ = gtsam::triangulateSafe(monoCameras, monoMeasured,
193  return result_;
194  }
195 
198  triangulateSafe(cameras); // imperative, might reset result_
199  return bool(result_);
200  }
201 
203  std::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor(
204  const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping =
205  false) const {
206 
207  size_t numKeys = this->keys_.size();
208  // Create structures for Hessian Factors
209  KeyVector js;
210  std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
211  std::vector<Vector> gs(numKeys);
212 
213  if (this->measured_.size() != cameras.size())
214  throw std::runtime_error("SmartStereoProjectionHessianFactor: this->"
215  "measured_.size() inconsistent with input");
216 
218 
220  // failed: return"empty" Hessian
221  for(Matrix& m: Gs)
222  m = Matrix::Zero(Base::Dim, Base::Dim);
223  for(Vector& v: gs)
224  v = Vector::Zero(Base::Dim);
225  return std::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
226  Gs, gs, 0.0);
227  }
228 
229  // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
231  Matrix F, E;
232  Vector b;
234 
235  // Whiten using noise model
237 
238  // build augmented hessian
239  SymmetricBlockMatrix augmentedHessian = //
240  Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping);
241 
242  return std::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
243  augmentedHessian);
244  }
245 
246  // create factor
247 // std::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > createRegularImplicitSchurFactor(
248 // const Cameras& cameras, double lambda) const {
249 // if (triangulateForLinearize(cameras))
250 // return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
251 // else
252 // // failed: return empty
253 // return std::shared_ptr<RegularImplicitSchurFactor<StereoCamera> >();
254 // }
255 //
256 // /// create factor
257 // std::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > createJacobianQFactor(
258 // const Cameras& cameras, double lambda) const {
259 // if (triangulateForLinearize(cameras))
260 // return Base::createJacobianQFactor(cameras, *result_, lambda);
261 // else
262 // // failed: return empty
263 // return std::make_shared<JacobianFactorQ<Base::Dim, Base::ZDim> >(this->keys_);
264 // }
265 //
266 // /// Create a factor, takes values
267 // std::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > createJacobianQFactor(
268 // const Values& values, double lambda) const {
269 // return createJacobianQFactor(this->cameras(values), lambda);
270 // }
271 
273  std::shared_ptr<JacobianFactor> createJacobianSVDFactor(
274  const Cameras& cameras, double lambda) const {
277  else
278  return std::make_shared<JacobianFactorSVD<Base::Dim, ZDim> >(this->keys_);
279  }
280 
281 // /// linearize to a Hessianfactor
282 // virtual std::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
283 // const Values& values, double lambda = 0.0) const {
284 // return createHessianFactor(this->cameras(values), lambda);
285 // }
286 
287 // /// linearize to an Implicit Schur factor
288 // virtual std::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > linearizeToImplicit(
289 // const Values& values, double lambda = 0.0) const {
290 // return createRegularImplicitSchurFactor(this->cameras(values), lambda);
291 // }
292 //
293 // /// linearize to a JacobianfactorQ
294 // virtual std::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > linearizeToJacobian(
295 // const Values& values, double lambda = 0.0) const {
296 // return createJacobianQFactor(this->cameras(values), lambda);
297 // }
298 
304  std::shared_ptr<GaussianFactor> linearizeDamped(const Cameras& cameras,
305  const double lambda = 0.0) const {
306  // depending on flag set on construction we may linearize to different linear factors
307  switch (params_.linearizationMode) {
308  case HESSIAN:
310 // case IMPLICIT_SCHUR:
311 // return createRegularImplicitSchurFactor(cameras, lambda);
312  case JACOBIAN_SVD:
314 // case JACOBIAN_Q:
315 // return createJacobianQFactor(cameras, lambda);
316  default:
317  throw std::runtime_error("SmartStereoFactorlinearize: unknown mode");
318  }
319  }
320 
326  std::shared_ptr<GaussianFactor> linearizeDamped(const Values& values,
327  const double lambda = 0.0) const {
328  // depending on flag set on construction we may linearize to different linear factors
329  Cameras cameras = this->cameras(values);
330  return linearizeDamped(cameras, lambda);
331  }
332 
334  std::shared_ptr<GaussianFactor> linearize(
335  const Values& values) const override {
336  return linearizeDamped(values);
337  }
338 
344  bool nonDegenerate = triangulateForLinearize(cameras);
345  if (nonDegenerate)
346  cameras.project2(*result_, nullptr, &E);
347  return nonDegenerate;
348  }
349 
354  bool triangulateAndComputeE(Matrix& E, const Values& values) const {
355  Cameras cameras = this->cameras(values);
357  }
358 
359 
364  FBlocks& Fs,
365  Matrix& E, Vector& b,
366  const Cameras& cameras) const {
367 
368  if (!result_) {
369  throw ("computeJacobiansWithTriangulatedPoint");
370 // // Handle degeneracy
371 // // TODO check flag whether we should do this
372 // Unit3 backProjected; /* = cameras[0].backprojectPointAtInfinity(
373 // this->measured_.at(0)); */
374 //
375 // Base::computeJacobians(Fs, E, b, cameras, backProjected);
376  } else {
377  // valid result: just return Base version
379  }
380  }
381 
384  FBlocks& Fs, Matrix& E, Vector& b,
385  const Values& values) const {
386  Cameras cameras = this->cameras(values);
387  bool nonDegenerate = triangulateForLinearize(cameras);
388  if (nonDegenerate)
390  return nonDegenerate;
391  }
392 
395  FBlocks& Fs, Matrix& Enull, Vector& b,
396  const Values& values) const {
397  Cameras cameras = this->cameras(values);
398  bool nonDegenerate = triangulateForLinearize(cameras);
399  if (nonDegenerate)
401  return nonDegenerate;
402  }
403 
406  Cameras cameras = this->cameras(values);
407  bool nonDegenerate = triangulateForLinearize(cameras);
408  if (nonDegenerate)
410  else
411  return Vector::Zero(cameras.size() * Base::ZDim);
412  }
413 
421  std::optional<Point3> externalPoint = {}) const {
422 
423  if (externalPoint)
424  result_ = TriangulationResult(*externalPoint);
425  else
427 
428  if (result_)
429  // All good, just use version in base class
431  else if (params_.degeneracyMode == HANDLE_INFINITY) {
432  throw(std::runtime_error("Backproject at infinity not implemented for SmartStereo."));
433 // // Otherwise, manage the exceptions with rotation-only factors
434 // const StereoPoint2& z0 = this->measured_.at(0);
435 // Unit3 backprojected; //= cameras.front().backprojectPointAtInfinity(z0);
436 //
437 // return Base::totalReprojectionError(cameras, backprojected);
438  } else {
439  // if we don't want to manage the exceptions we discard the factor
440  return 0.0;
441  }
442  }
443 
445  double error(const Values& values) const override {
446  if (this->active(values)) {
448  } else { // else of active flag
449  return 0.0;
450  }
451  }
452 
458  const Cameras& cameras, Vector& ue,
459  typename Cameras::FBlocks* Fs = nullptr,
460  Matrix* E = nullptr) const override {
461  // when using stereo cameras, some of the measurements might be missing:
462  for (size_t i = 0; i < cameras.size(); i++) {
463  const StereoPoint2& z = measured_.at(i);
464  if (std::isnan(z.uR())) // if the right 2D measurement is invalid
465  {
466  if (Fs) { // delete influence of right point on jacobian Fs
467  MatrixZD& Fi = Fs->at(i);
468  for (size_t ii = 0; ii < Dim; ii++) Fi(1, ii) = 0.0;
469  }
470  if (E) // delete influence of right point on jacobian E
471  E->row(ZDim * i + 1) = Matrix::Zero(1, E->cols());
472 
473  // set the corresponding entry of vector ue to zero
474  ue(ZDim * i + 1) = 0.0;
475  }
476  }
477  }
478 
481  return result_;
482  }
483 
486  Cameras cameras = this->cameras(values);
487  return triangulateSafe(cameras);
488  }
489 
491  bool isValid() const { return result_.valid(); }
492 
494  bool isDegenerate() const { return result_.degenerate(); }
495 
497  bool isPointBehindCamera() const { return result_.behindCamera(); }
498 
500  bool isOutlier() const { return result_.outlier(); }
501 
503  bool isFarPoint() const { return result_.farPoint(); }
504 
505 private:
506 
507 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
508  friend class boost::serialization::access;
510  template<class ARCHIVE>
511  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
512  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
513  ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality);
514  ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality);
515  }
516 #endif
517 };
518 
520 template<>
522  SmartStereoProjectionFactor> {
523 };
524 
525 } // \ namespace gtsam
gtsam::SmartStereoProjectionFactor::triangulateAndComputeJacobians
bool triangulateAndComputeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const
Version that takes values, and creates the point.
Definition: SmartStereoProjectionFactor.h:383
gtsam::SmartStereoProjectionFactor::isOutlier
bool isOutlier() const
Definition: SmartStereoProjectionFactor.h:500
gtsam::SmartProjectionParams::triangulation
TriangulationParameters triangulation
Definition: SmartFactorParams.h:49
SmartFactorBase.h
Base class to create smart factors on poses or cameras.
gtsam::SmartStereoProjectionFactor::reprojectionErrorAfterTriangulation
Vector reprojectionErrorAfterTriangulation(const Values &values) const
Calculate vector of re-projection errors, before applying noise model.
Definition: SmartStereoProjectionFactor.h:405
gtsam::SmartStereoProjectionFactor::isFarPoint
bool isFarPoint() const
Definition: SmartStereoProjectionFactor.h:503
gtsam::SmartFactorBase< StereoCamera >::unwhitenedError
Vector unwhitenedError(const Cameras &cameras, const POINT &point, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) const
Definition: SmartFactorBase.h:213
gtsam::StereoPoint2::uL
double uL() const
get uL
Definition: StereoPoint2.h:110
gtsam::SmartProjectionParams::verboseCheirality
bool verboseCheirality
If true, prints text for Cheirality exceptions (default: false)
Definition: SmartFactorParams.h:56
gtsam::PinholeCamera::MeasurementVector
Point2Vector MeasurementVector
Definition: PinholeCamera.h:42
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
gtsam::SmartStereoProjectionFactor::~SmartStereoProjectionFactor
~SmartStereoProjectionFactor() override
Definition: SmartStereoProjectionFactor.h:100
gtsam::SmartStereoProjectionFactor::point
TriangulationResult point(const Values &values) const
Definition: SmartStereoProjectionFactor.h:485
gtsam::SmartFactorBase< StereoCamera >::body_P_sensor
Pose3 body_P_sensor() const
Definition: SmartFactorBase.h:444
gtsam::SmartStereoProjectionFactor::linearizeDamped
std::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double lambda=0.0) const
Definition: SmartStereoProjectionFactor.h:326
gtsam::SmartStereoProjectionFactor::Cameras
CameraSet< StereoCamera > Cameras
Vector of cameras.
Definition: SmartStereoProjectionFactor.h:80
gtsam::SmartStereoProjectionFactor::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, std::optional< Point3 > externalPoint={}) const
Definition: SmartStereoProjectionFactor.h:420
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:672
gtsam::SmartProjectionParams
Definition: SmartFactorParams.h:42
gtsam::SmartStereoProjectionFactor::Base
SmartFactorBase< StereoCamera > Base
Definition: SmartStereoProjectionFactor.h:59
gtsam::SmartStereoProjectionFactor::correctForMissingMeasurements
void correctForMissingMeasurements(const Cameras &cameras, Vector &ue, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) const override
Definition: SmartStereoProjectionFactor.h:457
gtsam::SmartStereoProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartStereoProjectionFactor.h:118
gtsam::SmartStereoProjectionFactor::computeJacobiansWithTriangulatedPoint
void computeJacobiansWithTriangulatedPoint(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
Definition: SmartStereoProjectionFactor.h:363
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
gtsam::SmartStereoProjectionFactor::error
double error(const Values &values) const override
Calculate total reprojection error.
Definition: SmartStereoProjectionFactor.h:445
gtsam::SmartFactorBase< StereoCamera >::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: SmartFactorBase.h:180
gtsam::CameraSet::project2
ZVector project2(const POINT &point, FBlocks *Fs=nullptr, Matrix *E=nullptr) const
Definition: CameraSet.h:109
gtsam::StereoPoint2::v
double v() const
get v
Definition: StereoPoint2.h:116
gtsam::SmartStereoProjectionFactor::point
TriangulationResult point() const
Definition: SmartStereoProjectionFactor.h:480
gtsam::SmartFactorBase< StereoCamera >::Fs
FBlocks Fs
Definition: SmartFactorBase.h:86
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::SmartFactorBase< StereoCamera >::Dim
static const int Dim
Camera dimension.
Definition: SmartFactorBase.h:61
isnan
#define isnan(X)
Definition: main.h:93
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Definition: NonlinearFactor.h:142
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
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::SmartStereoProjectionFactor::triangulateAndComputeE
bool triangulateAndComputeE(Matrix &E, const Cameras &cameras) const
Definition: SmartStereoProjectionFactor.h:343
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:37
gtsam::SmartStereoProjectionFactor::createHessianFactor
std::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: SmartStereoProjectionFactor.h:203
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::SmartProjectionParams::degeneracyMode
DegeneracyMode degeneracyMode
How to linearize the factor.
Definition: SmartFactorParams.h:45
gtsam::SmartStereoProjectionParams
SmartProjectionParams SmartStereoProjectionParams
Definition: SmartStereoProjectionFactor.h:45
gtsam::ZERO_ON_DEGENERACY
@ ZERO_ON_DEGENERACY
Definition: SmartFactorParams.h:36
gtsam::SmartStereoProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: SmartStereoProjectionFactor.h:108
gtsam::HANDLE_INFINITY
@ HANDLE_INFINITY
Definition: SmartFactorParams.h:36
gtsam::SmartProjectionParams::linearizationMode
LinearizationMode linearizationMode
How to linearize the factor.
Definition: SmartFactorParams.h:44
gtsam::SmartStereoProjectionFactor::triangulateForLinearize
bool triangulateForLinearize(const Cameras &cameras) const
triangulate
Definition: SmartStereoProjectionFactor.h:197
gtsam::SmartStereoProjectionFactor::shared_ptr
std::shared_ptr< SmartStereoProjectionFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartStereoProjectionFactor.h:77
dataset.h
utility functions for loading datasets
gtsam::SmartStereoProjectionFactor
Definition: SmartStereoProjectionFactor.h:55
gtsam::SmartFactorBase< StereoCamera >::whitenJacobians
void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
Whiten the Jacobians computed by computeJacobians using noiseModel_.
Definition: SmartFactorBase.h:382
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
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:175
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::Pose3
Definition: Pose3.h:37
gtsam::SmartStereoProjectionFactor::MonoMeasurements
MonoCamera::MeasurementVector MonoMeasurements
Definition: SmartStereoProjectionFactor.h:85
gtsam::SmartStereoProjectionFactor::cameraPosesTriangulation_
std::vector< Pose3 > cameraPosesTriangulation_
current triangulation poses
Definition: SmartStereoProjectionFactor.h:71
gtsam::JACOBIAN_SVD
@ JACOBIAN_SVD
Definition: SmartFactorParams.h:31
StereoFactor.h
A non-linear factor for stereo measurements.
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::SmartStereoProjectionFactor::params_
const SmartStereoProjectionParams params_
Definition: SmartStereoProjectionFactor.h:65
gtsam::SmartStereoProjectionFactor::MonoCameras
CameraSet< MonoCamera > MonoCameras
Definition: SmartStereoProjectionFactor.h:84
gtsam::SmartStereoProjectionFactor::SmartStereoProjectionFactor
SmartStereoProjectionFactor(const SharedNoiseModel &sharedNoiseModel, const SmartStereoProjectionParams &params=SmartStereoProjectionParams(), const std::optional< Pose3 > body_P_sensor={})
Definition: SmartStereoProjectionFactor.h:91
gtsam::TriangulationResult::valid
bool valid() const
Definition: triangulation.h:670
Symbol.h
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:702
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::SmartFactorBase< StereoCamera >::createJacobianSVDFactor
std::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
Definition: SmartFactorBase.h:421
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
gtsam::TriangulationResult::Degenerate
static TriangulationResult Degenerate()
Definition: triangulation.h:660
gtsam::SmartFactorBase< StereoCamera >::cameras
virtual Cameras cameras(const Values &values) const
Collect all cameras: important that in key order.
Definition: SmartFactorBase.h:167
lambda
static double lambda[]
Definition: jv.c:524
gtsam::SmartStereoProjectionFactor::isPointBehindCamera
bool isPointBehindCamera() const
Definition: SmartStereoProjectionFactor.h:497
gtsam::TriangulationResult::degenerate
bool degenerate() const
Definition: triangulation.h:671
gtsam::SmartStereoProjectionFactor::decideIfTriangulate
bool decideIfTriangulate(const Cameras &cameras) const
Check if the new linearization point_ is the same as the one used for previous triangulation.
Definition: SmartStereoProjectionFactor.h:126
E
DiscreteKey E(5, 2)
gtsam::SmartFactorBase< StereoCamera >::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
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::SmartFactorBase< StereoCamera >::ZDim
static const int ZDim
Measurement dimension.
Definition: SmartFactorBase.h:62
gtsam::Values
Definition: Values.h:65
gtsam::TriangulationResult::behindCamera
bool behindCamera() const
Definition: triangulation.h:674
gtsam::StereoPoint2
Definition: StereoPoint2.h:34
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:69
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::SmartStereoProjectionFactor::triangulateSafe
TriangulationResult triangulateSafe(const Cameras &cameras) const
triangulateSafe
Definition: SmartStereoProjectionFactor.h:167
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::SmartProjectionParams::throwCheirality
bool throwCheirality
If true, re-throws Cheirality exceptions (default: false)
Definition: SmartFactorParams.h:55
gtsam::SmartFactorBase< StereoCamera >::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, const POINT &point) const
Definition: SmartFactorBase.h:302
gtsam::StereoPoint2::uR
double uR() const
get uR
Definition: StereoPoint2.h:113
gtsam::SmartProjectionParams::retriangulationThreshold
double retriangulationThreshold
threshold to decide whether to re-triangulate
Definition: SmartFactorParams.h:50
baseline
const double baseline
Definition: testSmartStereoFactor_iSAM2.cpp:51
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::SmartStereoProjectionFactor::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition: SmartStereoProjectionFactor.h:334
gtsam::TriangulationResult
Definition: triangulation.h:642
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::SmartStereoProjectionFactor::isDegenerate
bool isDegenerate() const
Definition: SmartStereoProjectionFactor.h:494
gtsam::SmartStereoProjectionFactor::isValid
bool isValid() const
Is result valid?
Definition: SmartStereoProjectionFactor.h:491
Base
Definition: test_virtual_functions.cpp:156
gtsam::SmartFactorBase< StereoCamera >::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartFactorBase.h:193
gtsam::SmartStereoProjectionFactor::result_
TriangulationResult result_
result from triangulateSafe
Definition: SmartStereoProjectionFactor.h:70
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:53
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::SmartStereoProjectionFactor::triangulateAndComputeE
bool triangulateAndComputeE(Matrix &E, const Values &values) const
Definition: SmartStereoProjectionFactor.h:354
gtsam::SmartFactorBase< StereoCamera >::computeJacobiansSVD
void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
Definition: SmartFactorBase.h:335
gtsam::SmartStereoProjectionFactor::createJacobianSVDFactor
std::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, double lambda) const
different (faster) way to compute Jacobian factor
Definition: SmartStereoProjectionFactor.h:273
gtsam::SmartStereoProjectionFactor::triangulateAndComputeJacobiansSVD
bool triangulateAndComputeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Values &values) const
takes values
Definition: SmartStereoProjectionFactor.h:394
gtsam::TriangulationResult::farPoint
bool farPoint() const
Definition: triangulation.h:673
gtsam::CameraSet::FBlocks
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Definition: CameraSet.h:79
Pose3.h
3D Pose manifold SO(3) x R^3 and group SE(3)
StereoCamera.h
A Stereo Camera based on two Simple Cameras.
gtsam::SmartStereoProjectionFactor::MonoCamera
PinholeCamera< Cal3_S2 > MonoCamera
Vector of monocular cameras (stereo treated as 2 monocular)
Definition: SmartStereoProjectionFactor.h:83
gtsam::SmartFactorBase< StereoCamera >::measured_
ZVector measured_
Definition: SmartFactorBase.h:80
gtsam::SmartStereoProjectionFactor::linearizeDamped
std::shared_ptr< GaussianFactor > linearizeDamped(const Cameras &cameras, const double lambda=0.0) const
Definition: SmartStereoProjectionFactor.h:304
gtsam::SmartFactorBase< StereoCamera >::computeJacobians
void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const
Definition: SmartFactorBase.h:320


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:13:28