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 
25 
27 #include <gtsam/geometry/Pose3.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  */
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 
82 
88  const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
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 {
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
147  cameraPosesTriangulation_.clear();
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 
193  bool triangulateForLinearize(const Cameras& cameras) const {
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 
213  triangulateSafe(cameras);
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().
227  Matrix F, E;
228  Vector b;
229  computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
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 {
271  if (triangulateForLinearize(cameras))
272  return Base::createJacobianSVDFactor(cameras, *result_, lambda);
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
374  Base::computeJacobians(Fs, E, b, cameras, *result_);
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)
385  computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
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 
402  Cameras cameras = this->cameras(values);
403  bool nonDegenerate = triangulateForLinearize(cameras);
404  if (nonDegenerate)
405  return Base::unwhitenedError(cameras, *result_);
406  else
407  return Vector::Zero(cameras.size() * Base::ZDim);
408  }
409 
416  double totalReprojectionError(const Cameras& cameras,
417  boost::optional<Point3> externalPoint = boost::none) const {
418 
419  if (externalPoint)
420  result_ = TriangulationResult(*externalPoint);
421  else
422  result_ = triangulateSafe(cameras);
423 
424  if (result_)
425  // All good, just use version in base class
426  return Base::totalReprojectionError(cameras, *result_);
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 
452  void correctForMissingMeasurements(const Cameras& cameras, Vector& ue,
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 
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
Matrix3f m
ZVector project2(const POINT &point, boost::optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Definition: CameraSet.h:107
double uR() const
get uR
Definition: StereoPoint2.h:109
MonoCamera::MeasurementVector MonoMeasurements
double uL() const
get uL
Definition: StereoPoint2.h:106
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, double lambda) const
different (faster) way to compute Jacobian factor
TriangulationResult point(const Values &values) const
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
bool triangulateAndComputeE(Matrix &E, const Values &values) const
Key E(std::uint64_t j)
PinholeCamera< Cal3_S2 > MonoCamera
Vector of monocular cameras (stereo treated as 2 monocular)
TriangulationResult triangulateSafe(const Cameras &cameras) const
triangulateSafe
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
bool decideIfTriangulate(const Cameras &cameras) const
Check if the new linearization point_ is the same as the one used for previous triangulation.
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
DegeneracyMode degeneracyMode
How to linearize the factor.
Vector2 Point2
Definition: Point2.h:27
ArrayXcf v
Definition: Cwise_arg.cpp:1
virtual Cameras cameras(const Values &values) const
Collect all cameras: important that in key order.
const double baseline
boost::shared_ptr< GaussianFactor > linearizeDamped(const Cameras &cameras, const double lambda=0.0) const
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
boost::shared_ptr< SmartStereoProjectionFactor > shared_ptr
shorthand for a smart pointer to a factor
leaf::MyValues values
TriangulationParameters triangulation
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double lambda=0.0) const
double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
double error(const Values &values) const override
Calculate total reprojection error.
SmartFactorBase< StereoCamera > Base
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
A set of cameras, all with their own calibration.
Definition: CameraSet.h:35
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
bool triangulateAndComputeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const
Version that takes values, and creates the point.
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)
virtual bool active(const Values &) const
bool triangulateForLinearize(const Cameras &cameras) const
triangulate
Base class to create smart factors on poses or cameras.
bool throwCheirality
If true, re-throws Cheirality exceptions (default: false)
bool triangulateAndComputeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Values &values) const
takes values
double v() const
get v
Definition: StereoPoint2.h:112
Base class for smart factors This base class has no internal point, but it has a measurement, noise model and an optional sensor pose. This class mainly computes the derivatives and returns them as a variety of factors. The methods take a Cameras argument, which should behave like PinholeCamera, and the value of a point, which is kept in the base class.
const SmartStereoProjectionParams params_
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
Class compose(const Class &g) const
Definition: Lie.h:47
Signature::Row F
Definition: Signature.cpp:53
Eigen::VectorXd Vector
Definition: Vector.h:38
CameraSet< StereoCamera > Cameras
Vector of cameras.
Vector reprojectionErrorAfterTriangulation(const Values &values) const
Calculate vector of re-projection errors, before applying noise model.
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
void computeJacobiansWithTriangulatedPoint(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
Functions for triangulation.
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
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
LinearizationMode linearizationMode
How to linearize the factor.
static SmartStereoProjectionParams params
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
triangulateSafe: extensive checking of the outcome
const G & b
Definition: Group.h:83
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
void serialize(ARCHIVE &ar, const unsigned int)
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
A Stereo Camera based on two Simple Cameras.
void correctForMissingMeasurements(const Cameras &cameras, Vector &ue, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const override
void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const
static const int Dim
Camera dimension.
double totalReprojectionError(const Cameras &cameras, const POINT &point) const
friend class boost::serialization::access
Serialization function.
void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
Whiten the Jacobians computed by computeJacobians using noiseModel_.
float * p
bool triangulateAndComputeE(Matrix &E, const Cameras &cameras) const
TriangulationResult result_
result from triangulateSafe
Point2Vector MeasurementVector
Definition: PinholeCamera.h:42
const G double tol
Definition: Group.h:83
Vector3 Point3
Definition: Point3.h:35
SmartStereoProjectionFactor(const SharedNoiseModel &sharedNoiseModel, const SmartStereoProjectionParams &params=SmartStereoProjectionParams(), const boost::optional< Pose3 > body_P_sensor=boost::none)
The matrix class, also used for vectors and row-vectors.
Vector unwhitenedError(const Cameras &cameras, const POINT &point, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
3D Pose
utility functions for loading datasets
static const int ZDim
Measurement dimension.
#define isnan(X)
Definition: main.h:72
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734
SmartProjectionParams SmartStereoProjectionParams
void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
SVD version.
static SymmetricBlockMatrix SchurComplement(const FBlocks &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
Definition: CameraSet.h:149


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:44:16