gtsam  4.1.0
gtsam
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 
24 #include <gtsam/slam/SmartFactorParams.h>
25 
27 #include <gtsam/geometry/Pose3.h>
29 #include <gtsam/slam/StereoFactor.h>
30 #include <gtsam/inference/Symbol.h>
31 #include <gtsam/slam/dataset.h>
32 
33 #include <boost/optional.hpp>
34 #include <boost/make_shared.hpp>
35 #include <vector>
36 
37 namespace gtsam {
38 
39 /*
40  * Parameters for the smart stereo projection factors (identical to the SmartProjectionParams)
41  */
42 typedef SmartProjectionParams SmartStereoProjectionParams;
43 
52 class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera> {
53 private:
54 
56 
57 protected:
58 
61  const SmartStereoProjectionParams params_;
63 
67  mutable std::vector<Pose3> cameraPosesTriangulation_;
68 
70 public:
71 
73  typedef boost::shared_ptr<SmartStereoProjectionFactor> shared_ptr;
74 
77 
81  typedef MonoCamera::MeasurementVector MonoMeasurements;
82 
89  const boost::optional<Pose3> body_P_sensor = boost::none) :
90  Base(sharedNoiseModel, body_P_sensor), //
91  params_(params), //
92  result_(TriangulationResult::Degenerate()) {
93  }
94 
97  }
98 
104  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
105  DefaultKeyFormatter) const override {
106  std::cout << s << "SmartStereoProjectionFactor\n";
107  std::cout << "linearizationMode:\n" << params_.linearizationMode << std::endl;
108  std::cout << "triangulationParameters:\n" << params_.triangulation << 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 SmartStereoProjectionFactor *e =
116  dynamic_cast<const SmartStereoProjectionFactor*>(&p);
117  return e && params_.linearizationMode == e->params_.linearizationMode
118  && Base::equals(p, tol);
119  }
120 
122  bool decideIfTriangulate(const Cameras& cameras) const {
123  // several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate
124  // Note that this is not yet "selecting linearization", that will come later, and we only check if the
125  // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point_
126 
127  size_t m = cameras.size();
128 
129  bool retriangulate = false;
130 
131  // if we do not have a previous linearization point_ or the new linearization point_ includes more poses
132  if (cameraPosesTriangulation_.empty()
133  || cameras.size() != cameraPosesTriangulation_.size())
134  retriangulate = true;
135 
136  if (!retriangulate) {
137  for (size_t i = 0; i < cameras.size(); i++) {
138  if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
139  params_.retriangulationThreshold)) {
140  retriangulate = true; // at least two poses are different, hence we retriangulate
141  break;
142  }
143  }
144  }
145 
146  if (retriangulate) { // we store the current poses used for triangulation
148  cameraPosesTriangulation_.reserve(m);
149  for (size_t i = 0; i < m; i++)
150  // cameraPosesTriangulation_[i] = cameras[i].pose();
151  cameraPosesTriangulation_.push_back(cameras[i].pose());
152  }
153 
154  return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation
155  }
156 
157 // /// triangulateSafe
158 // size_t triangulateSafe(const Values& values) const {
159 // return triangulateSafe(this->cameras(values));
160 // }
161 
164 
165  size_t m = cameras.size();
166  bool retriangulate = decideIfTriangulate(cameras);
167 
168  // triangulate stereo measurements by treating each stereocamera as a pair of monocular cameras
169  MonoCameras monoCameras;
170  MonoMeasurements monoMeasured;
171  for(size_t i = 0; i < m; i++) {
172  const Pose3 leftPose = cameras[i].pose();
173  const Cal3_S2 monoCal = cameras[i].calibration().calibration();
174  const MonoCamera leftCamera_i(leftPose,monoCal);
175  const Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0));
176  const Pose3 rightPose = leftPose.compose( left_Pose_right );
177  const MonoCamera rightCamera_i(rightPose,monoCal);
178  const StereoPoint2 zi = measured_[i];
179  monoCameras.push_back(leftCamera_i);
180  monoMeasured.push_back(Point2(zi.uL(),zi.v()));
181  if(!std::isnan(zi.uR())){ // if right point is valid
182  monoCameras.push_back(rightCamera_i);
183  monoMeasured.push_back(Point2(zi.uR(),zi.v()));
184  }
185  }
186  if (retriangulate)
187  result_ = gtsam::triangulateSafe(monoCameras, monoMeasured,
188  params_.triangulation);
189  return result_;
190  }
191 
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, bool diagonalDamping =
201  false) const {
202 
203  size_t numKeys = this->keys_.size();
204  // Create structures for Hessian Factors
205  KeyVector js;
206  std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
207  std::vector<Vector> gs(numKeys);
208 
209  if (this->measured_.size() != cameras.size())
210  throw std::runtime_error("SmartStereoProjectionHessianFactor: this->"
211  "measured_.size() inconsistent with input");
212 
214 
215  if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
216  // failed: return"empty" Hessian
217  for(Matrix& m: Gs)
218  m = Matrix::Zero(Base::Dim, Base::Dim);
219  for(Vector& v: gs)
220  v = Vector::Zero(Base::Dim);
221  return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
222  Gs, gs, 0.0);
223  }
224 
225  // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
226  Base::FBlocks Fs;
227  Matrix F, E;
228  Vector b;
230 
231  // Whiten using noise model
232  Base::whitenJacobians(Fs, E, b);
233 
234  // build augmented hessian
235  SymmetricBlockMatrix augmentedHessian = //
236  Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping);
237 
238  return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
239  augmentedHessian);
240  }
241 
242  // create factor
243 // boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > createRegularImplicitSchurFactor(
244 // const Cameras& cameras, double lambda) const {
245 // if (triangulateForLinearize(cameras))
246 // return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
247 // else
248 // // failed: return empty
249 // return boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> >();
250 // }
251 //
252 // /// create factor
253 // boost::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > createJacobianQFactor(
254 // const Cameras& cameras, double lambda) const {
255 // if (triangulateForLinearize(cameras))
256 // return Base::createJacobianQFactor(cameras, *result_, lambda);
257 // else
258 // // failed: return empty
259 // return boost::make_shared<JacobianFactorQ<Base::Dim, Base::ZDim> >(this->keys_);
260 // }
261 //
262 // /// Create a factor, takes values
263 // boost::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > createJacobianQFactor(
264 // const Values& values, double lambda) const {
265 // return createJacobianQFactor(this->cameras(values), lambda);
266 // }
267 
269  boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
270  const Cameras& cameras, double lambda) const {
273  else
274  return boost::make_shared<JacobianFactorSVD<Base::Dim, ZDim> >(this->keys_);
275  }
276 
277 // /// linearize to a Hessianfactor
278 // virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
279 // const Values& values, double lambda = 0.0) const {
280 // return createHessianFactor(this->cameras(values), lambda);
281 // }
282 
283 // /// linearize to an Implicit Schur factor
284 // virtual boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > linearizeToImplicit(
285 // const Values& values, double lambda = 0.0) const {
286 // return createRegularImplicitSchurFactor(this->cameras(values), lambda);
287 // }
288 //
289 // /// linearize to a JacobianfactorQ
290 // virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > linearizeToJacobian(
291 // const Values& values, double lambda = 0.0) const {
292 // return createJacobianQFactor(this->cameras(values), lambda);
293 // }
294 
300  boost::shared_ptr<GaussianFactor> linearizeDamped(const Cameras& cameras,
301  const double lambda = 0.0) const {
302  // depending on flag set on construction we may linearize to different linear factors
303  switch (params_.linearizationMode) {
304  case HESSIAN:
305  return createHessianFactor(cameras, lambda);
306 // case IMPLICIT_SCHUR:
307 // return createRegularImplicitSchurFactor(cameras, lambda);
308  case JACOBIAN_SVD:
309  return createJacobianSVDFactor(cameras, lambda);
310 // case JACOBIAN_Q:
311 // return createJacobianQFactor(cameras, lambda);
312  default:
313  throw std::runtime_error("SmartStereoFactorlinearize: unknown mode");
314  }
315  }
316 
322  boost::shared_ptr<GaussianFactor> linearizeDamped(const Values& values,
323  const double lambda = 0.0) const {
324  // depending on flag set on construction we may linearize to different linear factors
325  Cameras cameras = this->cameras(values);
326  return linearizeDamped(cameras, lambda);
327  }
328 
330  boost::shared_ptr<GaussianFactor> linearize(
331  const Values& values) const override {
332  return linearizeDamped(values);
333  }
334 
339  bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
340  bool nonDegenerate = triangulateForLinearize(cameras);
341  if (nonDegenerate)
342  cameras.project2(*result_, boost::none, E);
343  return nonDegenerate;
344  }
345 
350  bool triangulateAndComputeE(Matrix& E, const Values& values) const {
351  Cameras cameras = this->cameras(values);
352  return triangulateAndComputeE(E, cameras);
353  }
354 
355 
360  FBlocks& Fs,
361  Matrix& E, Vector& b,
362  const Cameras& cameras) const {
363 
364  if (!result_) {
365  throw ("computeJacobiansWithTriangulatedPoint");
366 // // Handle degeneracy
367 // // TODO check flag whether we should do this
368 // Unit3 backProjected; /* = cameras[0].backprojectPointAtInfinity(
369 // this->measured_.at(0)); */
370 //
371 // Base::computeJacobians(Fs, E, b, cameras, backProjected);
372  } else {
373  // valid result: just return Base version
375  }
376  }
377 
380  FBlocks& Fs, Matrix& E, Vector& b,
381  const Values& values) const {
382  Cameras cameras = this->cameras(values);
383  bool nonDegenerate = triangulateForLinearize(cameras);
384  if (nonDegenerate)
386  return nonDegenerate;
387  }
388 
391  FBlocks& Fs, Matrix& Enull, Vector& b,
392  const Values& values) const {
393  Cameras cameras = this->cameras(values);
394  bool nonDegenerate = triangulateForLinearize(cameras);
395  if (nonDegenerate)
396  Base::computeJacobiansSVD(Fs, Enull, b, cameras, *result_);
397  return nonDegenerate;
398  }
399 
401  Vector reprojectionErrorAfterTriangulation(const Values& values) const {
402  Cameras cameras = this->cameras(values);
403  bool nonDegenerate = triangulateForLinearize(cameras);
404  if (nonDegenerate)
406  else
407  return Vector::Zero(cameras.size() * Base::ZDim);
408  }
409 
417  boost::optional<Point3> externalPoint = boost::none) const {
418 
419  if (externalPoint)
420  result_ = TriangulationResult(*externalPoint);
421  else
423 
424  if (result_)
425  // All good, just use version in base class
427  else if (params_.degeneracyMode == HANDLE_INFINITY) {
428  throw(std::runtime_error("Backproject at infinity not implemented for SmartStereo."));
429 // // Otherwise, manage the exceptions with rotation-only factors
430 // const StereoPoint2& z0 = this->measured_.at(0);
431 // Unit3 backprojected; //= cameras.front().backprojectPointAtInfinity(z0);
432 //
433 // return Base::totalReprojectionError(cameras, backprojected);
434  } else {
435  // if we don't want to manage the exceptions we discard the factor
436  return 0.0;
437  }
438  }
439 
441  double error(const Values& values) const override {
442  if (this->active(values)) {
443  return totalReprojectionError(Base::cameras(values));
444  } else { // else of active flag
445  return 0.0;
446  }
447  }
448 
453  boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
454  boost::optional<Matrix&> E = boost::none) const override
455  {
456  // when using stereo cameras, some of the measurements might be missing:
457  for(size_t i=0; i < cameras.size(); i++){
458  const StereoPoint2& z = measured_.at(i);
459  if(std::isnan(z.uR())) // if the right pixel is invalid
460  {
461  if(Fs){ // delete influence of right point on jacobian Fs
462  MatrixZD& Fi = Fs->at(i);
463  for(size_t ii=0; ii<Dim; ii++)
464  Fi(1,ii) = 0.0;
465  }
466  if(E) // delete influence of right point on jacobian E
467  E->row(ZDim * i + 1) = Matrix::Zero(1, E->cols());
468 
469  // set the corresponding entry of vector ue to zero
470  ue(ZDim * i + 1) = 0.0;
471  }
472  }
473  }
474 
477  return result_;
478  }
479 
481  TriangulationResult point(const Values& values) const {
482  Cameras cameras = this->cameras(values);
483  return triangulateSafe(cameras);
484  }
485 
487  bool isValid() const { return result_.valid(); }
488 
490  bool isDegenerate() const { return result_.degenerate(); }
491 
493  bool isPointBehindCamera() const { return result_.behindCamera(); }
494 
496  bool isOutlier() const { return result_.outlier(); }
497 
499  bool isFarPoint() const { return result_.farPoint(); }
500 
501 private:
502 
505  template<class ARCHIVE>
506  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
507  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
508  ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality);
509  ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality);
510  }
511 };
512 
514 template<>
516  SmartStereoProjectionFactor> {
517 };
518 
519 } // \ namespace gtsam
gtsam::SmartProjectionParams::retriangulationThreshold
double retriangulationThreshold
threshold to decide whether to re-triangulate
Definition: SmartFactorParams.h:50
gtsam::SmartStereoProjectionFactor::isOutlier
bool isOutlier() const
return the outlier state
Definition: SmartStereoProjectionFactor.h:496
gtsam::SmartStereoProjectionFactor::shared_ptr
boost::shared_ptr< SmartStereoProjectionFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartStereoProjectionFactor.h:73
gtsam::SmartFactorBase< StereoCamera >::ZDim
static const int ZDim
Measurement dimension.
Definition: SmartFactorBase.h:58
gtsam::SmartStereoProjectionFactor::MonoCamera
PinholeCamera< Cal3_S2 > MonoCamera
Vector of monocular cameras (stereo treated as 2 monocular)
Definition: SmartStereoProjectionFactor.h:79
gtsam::StereoPoint2::uR
double uR() const
get uR
Definition: StereoPoint2.h:109
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:122
gtsam::SmartStereoProjectionFactor::createJacobianSVDFactor
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, double lambda) const
different (faster) way to compute Jacobian factor
Definition: SmartStereoProjectionFactor.h:269
gtsam::SmartStereoProjectionFactor::SmartStereoProjectionFactor
SmartStereoProjectionFactor(const SharedNoiseModel &sharedNoiseModel, const SmartStereoProjectionParams &params=SmartStereoProjectionParams(), const boost::optional< Pose3 > body_P_sensor=boost::none)
Constructor.
Definition: SmartStereoProjectionFactor.h:87
gtsam::SmartStereoProjectionFactor::error
double error(const Values &values) const override
Calculate total reprojection error.
Definition: SmartStereoProjectionFactor.h:441
gtsam::SmartProjectionParams
Definition: SmartFactorParams.h:42
gtsam::SmartStereoProjectionFactor::cameraPosesTriangulation_
std::vector< Pose3 > cameraPosesTriangulation_
current triangulation poses
Definition: SmartStereoProjectionFactor.h:67
gtsam::SmartFactorBase< StereoCamera >::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, const POINT &point) const
Calculate the error of the factor.
Definition: SmartFactorBase.h:255
gtsam::SmartFactorBase< StereoCamera >::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::SmartProjectionParams::verboseCheirality
bool verboseCheirality
If true, prints text for Cheirality exceptions (default: false)
Definition: SmartFactorParams.h:56
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::StereoPoint2::uL
double uL() const
get uL
Definition: StereoPoint2.h:106
gtsam::StereoPoint2
Definition: StereoPoint2.h:32
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::SmartStereoProjectionFactor::triangulateAndComputeE
bool triangulateAndComputeE(Matrix &E, const Values &values) const
Triangulate and compute derivative of error with respect to point.
Definition: SmartStereoProjectionFactor.h:350
gtsam::SmartStereoProjectionFactor::triangulateForLinearize
bool triangulateForLinearize(const Cameras &cameras) const
triangulate
Definition: SmartStereoProjectionFactor.h:193
gtsam::SmartStereoProjectionFactor::correctForMissingMeasurements
void correctForMissingMeasurements(const Cameras &cameras, Vector &ue, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const override
This corrects the Jacobians and error vector for the case in which the right pixel in the monocular c...
Definition: SmartStereoProjectionFactor.h:452
gtsam::TriangulationResult
TriangulationResult is an optional point, along with the reasons why it is invalid.
Definition: triangulation.h:369
gtsam::SmartStereoProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartStereoProjectionFactor.h:114
gtsam::SmartProjectionParams::degeneracyMode
DegeneracyMode degeneracyMode
How to linearize the factor.
Definition: SmartFactorParams.h:45
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
gtsam::Cal3_S2
Definition: Cal3_S2.h:33
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:35
Pose3.h
3D Pose
gtsam::SmartStereoProjectionFactor::point
TriangulationResult point(const Values &values) const
COMPUTE the landmark.
Definition: SmartStereoProjectionFactor.h:481
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::SmartStereoProjectionFactor::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
Calculate the error of the factor.
Definition: SmartStereoProjectionFactor.h:416
gtsam::SmartStereoProjectionFactor::triangulateAndComputeE
bool triangulateAndComputeE(Matrix &E, const Cameras &cameras) const
Triangulate and compute derivative of error with respect to point.
Definition: SmartStereoProjectionFactor.h:339
dataset.h
utility functions for loading datasets
gtsam::SmartStereoProjectionFactor::reprojectionErrorAfterTriangulation
Vector reprojectionErrorAfterTriangulation(const Values &values) const
Calculate vector of re-projection errors, before applying noise model.
Definition: SmartStereoProjectionFactor.h:401
gtsam::SmartStereoProjectionFactor
SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around.
Definition: SmartStereoProjectionFactor.h:52
gtsam::SmartStereoProjectionFactor::isFarPoint
bool isFarPoint() const
return the farPoint state
Definition: SmartStereoProjectionFactor.h:499
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:379
gtsam::SmartFactorBase< StereoCamera >::cameras
virtual Cameras cameras(const Values &values) const
Collect all cameras: important that in key order.
Definition: SmartFactorBase.h:163
gtsam::SmartStereoProjectionFactor::access
friend class boost::serialization::access
Serialization function.
Definition: SmartStereoProjectionFactor.h:504
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::SmartStereoProjectionFactor::~SmartStereoProjectionFactor
virtual ~SmartStereoProjectionFactor()
Virtual destructor.
Definition: SmartStereoProjectionFactor.h:96
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
gtsam::SmartStereoProjectionFactor::computeJacobiansWithTriangulatedPoint
void computeJacobiansWithTriangulatedPoint(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: SmartStereoProjectionFactor.h:359
gtsam::SmartFactorBase< StereoCamera >::whitenJacobians
void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
Whiten the Jacobians computed by computeJacobians using noiseModel_.
Definition: SmartFactorBase.h:331
gtsam::SmartStereoProjectionFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition: SmartStereoProjectionFactor.h:330
gtsam::SmartStereoProjectionFactor::triangulateSafe
TriangulationResult triangulateSafe(const Cameras &cameras) const
triangulateSafe
Definition: SmartStereoProjectionFactor.h:163
gtsam::SmartFactorBase< StereoCamera >::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: SmartFactorBase.h:175
gtsam::SmartStereoProjectionFactor::isValid
bool isValid() const
Is result valid?
Definition: SmartStereoProjectionFactor.h:487
gtsam::SmartStereoProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: SmartStereoProjectionFactor.h:104
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::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition: Point2.h:27
gtsam::SmartStereoProjectionFactor::isPointBehindCamera
bool isPointBehindCamera() const
return the cheirality status flag
Definition: SmartStereoProjectionFactor.h:493
gtsam::Rot3
Definition: Rot3.h:59
gtsam::SmartFactorBase< StereoCamera >::computeJacobiansSVD
void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
SVD version.
Definition: SmartFactorBase.h:284
gtsam::SmartProjectionParams::linearizationMode
LinearizationMode linearizationMode
How to linearize the factor.
Definition: SmartFactorParams.h:44
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::Pose3
Definition: Pose3.h:37
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::SmartStereoProjectionFactor::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: SmartStereoProjectionFactor.h:199
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::SmartStereoProjectionFactor::isDegenerate
bool isDegenerate() const
return the degenerate state
Definition: SmartStereoProjectionFactor.h:490
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< StereoCamera >::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartFactorBase.h:188
gtsam::SmartFactorBase< StereoCamera >::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::SmartStereoProjectionFactor::triangulateAndComputeJacobiansSVD
bool triangulateAndComputeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Values &values) const
takes values
Definition: SmartStereoProjectionFactor.h:390
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::SmartStereoProjectionFactor::linearizeDamped
boost::shared_ptr< GaussianFactor > linearizeDamped(const Cameras &cameras, const double lambda=0.0) const
Linearize to Gaussian Factor.
Definition: SmartStereoProjectionFactor.h:300
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
gtsam::SmartStereoProjectionFactor::result_
TriangulationResult result_
result from triangulateSafe
Definition: SmartStereoProjectionFactor.h:66
gtsam::SmartFactorBase< StereoCamera >::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::SmartProjectionParams::throwCheirality
bool throwCheirality
If true, re-throws Cheirality exceptions (default: false)
Definition: SmartFactorParams.h:55
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:52
SmartFactorBase.h
Base class to create smart factors on poses or cameras.
gtsam::SmartFactorBase< StereoCamera >::Dim
static const int Dim
Camera dimension.
Definition: SmartFactorBase.h:57
Symbol.h
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
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::SmartStereoProjectionFactor::Cameras
CameraSet< StereoCamera > Cameras
Vector of cameras.
Definition: SmartStereoProjectionFactor.h:76
gtsam::SmartStereoProjectionFactor::point
TriangulationResult point() const
return the landmark
Definition: SmartStereoProjectionFactor.h:476
gtsam::SmartStereoProjectionFactor::linearizeDamped
boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double lambda=0.0) const
Linearize to Gaussian Factor.
Definition: SmartStereoProjectionFactor.h:322
gtsam::StereoPoint2::v
double v() const
get v
Definition: StereoPoint2.h:112
gtsam::SmartFactorBase< StereoCamera >::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
StereoCamera.h
A Stereo Camera based on two Simple Cameras.