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 
65  const SmartStereoProjectionParams params_;
67 
71  mutable std::vector<Pose3> cameraPosesTriangulation_;
72 
74 public:
75 
77  typedef std::shared_ptr<SmartStereoProjectionFactor> shared_ptr;
78 
81 
86 
92  const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
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++) {
142  if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
143  params_.retriangulationThreshold)) {
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
151  cameraPosesTriangulation_.clear();
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,
192  params_.triangulation);
193  return result_;
194  }
195 
197  bool triangulateForLinearize(const Cameras& cameras) const {
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 
217  triangulateSafe(cameras);
218 
219  if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
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;
233  computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
234 
235  // Whiten using noise model
236  Base::whitenJacobians(Fs, E, b);
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 {
275  if (triangulateForLinearize(cameras))
276  return Base::createJacobianSVDFactor(cameras, *result_, lambda);
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:
309  return createHessianFactor(cameras, lambda);
310 // case IMPLICIT_SCHUR:
311 // return createRegularImplicitSchurFactor(cameras, lambda);
312  case JACOBIAN_SVD:
313  return createJacobianSVDFactor(cameras, lambda);
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 
343  bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
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);
356  return triangulateAndComputeE(E, cameras);
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
378  Base::computeJacobians(Fs, E, b, cameras, *result_);
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)
389  computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
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)
400  Base::computeJacobiansSVD(Fs, Enull, b, cameras, *result_);
401  return nonDegenerate;
402  }
403 
406  Cameras cameras = this->cameras(values);
407  bool nonDegenerate = triangulateForLinearize(cameras);
408  if (nonDegenerate)
409  return Base::unwhitenedError(cameras, *result_);
410  else
411  return Vector::Zero(cameras.size() * Base::ZDim);
412  }
413 
420  double totalReprojectionError(const Cameras& cameras,
421  std::optional<Point3> externalPoint = {}) const {
422 
423  if (externalPoint)
424  result_ = TriangulationResult(*externalPoint);
425  else
426  result_ = triangulateSafe(cameras);
427 
428  if (result_)
429  // All good, just use version in base class
430  return Base::totalReprojectionError(cameras, *result_);
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)) {
447  return totalReprojectionError(Base::cameras(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
Matrix3f m
virtual Cameras cameras(const Values &values) const
Collect all cameras: important that in key order.
void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const
MonoCamera::MeasurementVector MonoMeasurements
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
bool triangulateForLinearize(const Cameras &cameras) const
triangulate
PinholeCamera< Cal3_S2 > MonoCamera
Vector of monocular cameras (stereo treated as 2 monocular)
double totalReprojectionError(const Cameras &cameras, const POINT &point) const
Key F(std::uint64_t j)
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
std::string serialize(const T &input)
serializes to a string
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
bool triangulateAndComputeE(Matrix &E, const Cameras &cameras) const
DegeneracyMode degeneracyMode
How to linearize the factor.
std::vector< Pose3 > cameraPosesTriangulation_
current triangulation poses
Vector2 Point2
Definition: Point2.h:32
const double baseline
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
leaf::MyValues values
TriangulationParameters triangulation
void computeJacobiansWithTriangulatedPoint(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:87
double error(const Values &values) const override
Calculate total reprojection error.
std::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, double lambda) const
different (faster) way to compute Jacobian factor
SmartFactorBase< StereoCamera > Base
SmartStereoProjectionFactor(const SharedNoiseModel &sharedNoiseModel, const SmartStereoProjectionParams &params=SmartStereoProjectionParams(), const std::optional< Pose3 > body_P_sensor={})
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
bool triangulateAndComputeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Values &values) const
takes values
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:171
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
static const SmartProjectionParams params
bool decideIfTriangulate(const Cameras &cameras) const
Check if the new linearization point_ is the same as the one used for previous triangulation.
std::shared_ptr< SmartStereoProjectionFactor > shared_ptr
shorthand for a smart pointer to a factor
double totalReprojectionError(const Cameras &cameras, std::optional< Point3 > externalPoint={}) const
Base class to create smart factors on poses or cameras.
ZVector project2(const POINT &point, FBlocks *Fs=nullptr, Matrix *E=nullptr) const
Definition: CameraSet.h:108
bool throwCheirality
If true, re-throws Cheirality exceptions (default: false)
bool verboseCheirality
If true, prints text for Cheirality exceptions (default: false)
Base class for smart factors. This base class has no internal point, but it has a measurement...
const SmartStereoProjectionParams params_
Eigen::VectorXd Vector
Definition: Vector.h:38
void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
CameraSet< StereoCamera > Cameras
Vector of cameras.
bool triangulateAndComputeE(Matrix &E, const Values &values) const
std::shared_ptr< GaussianFactor > linearizeDamped(const Cameras &cameras, const double lambda=0.0) const
Functions for triangulation.
Array< int, Dynamic, 1 > v
double v() const
get v
Definition: StereoPoint2.h:116
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
LinearizationMode linearizationMode
How to linearize the factor.
TriangulationResult point(const Values &values) const
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Definition: CameraSet.h:78
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
triangulateSafe: extensive checking of the outcome
Class compose(const Class &g) const
Definition: Lie.h:48
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
const G & b
Definition: Group.h:86
A non-linear factor for stereo measurements.
std::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
cout<< "The eigenvalues of A are:"<< endl<< ces.eigenvalues()<< endl;cout<< "The matrix of eigenvectors, V, is:"<< endl<< ces.eigenvectors()<< endl<< endl;complex< float > lambda
traits
Definition: chartTesting.h:28
TriangulationResult triangulateSafe(const Cameras &cameras) const
triangulateSafe
void correctForMissingMeasurements(const Cameras &cameras, Vector &ue, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) const override
DiscreteKey E(5, 2)
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
A Stereo Camera based on two Simple Cameras.
virtual bool active(const Values &) const
double uL() const
get uL
Definition: StereoPoint2.h:110
static const int Dim
Camera dimension.
static TriangulationResult Degenerate()
float * p
TriangulationResult result_
result from triangulateSafe
Point2Vector MeasurementVector
Definition: PinholeCamera.h:42
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
The matrix class, also used for vectors and row-vectors.
void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
Whiten the Jacobians computed by computeJacobians using noiseModel_.
3D Pose
double uR() const
get uR
Definition: StereoPoint2.h:113
utility functions for loading datasets
std::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
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)
bool triangulateAndComputeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const
Version that takes values, and creates the point.
static const int ZDim
Measurement dimension.
Collect common parameters for SmartProjection and SmartStereoProjection factors.
#define isnan(X)
Definition: main.h:93
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
SmartProjectionParams SmartStereoProjectionParams
Vector reprojectionErrorAfterTriangulation(const Values &values) const
Calculate vector of re-projection errors, before applying noise model.
Vector unwhitenedError(const Cameras &cameras, const POINT &point, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) const
std::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double lambda=0.0) const
double retriangulationThreshold
threshold to decide whether to re-triangulate


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:52