NoiseModel.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 
19 #pragma once
20 
21 #include <gtsam/base/Testable.h>
22 #include <gtsam/base/Matrix.h>
24 #include <gtsam/dllexport.h>
26 
27 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
28 #include <boost/serialization/nvp.hpp>
29 #include <boost/serialization/extended_type_info.hpp>
30 #include <boost/serialization/singleton.hpp>
31 #include <boost/serialization/shared_ptr.hpp>
32 #endif
33 
34 #include <optional>
35 
36 namespace gtsam {
37 
39  namespace noiseModel {
40 
41  // Forward declaration
42  class Gaussian;
43  class Diagonal;
44  class Constrained;
45  class Isotropic;
46  class Unit;
47  class RobustModel;
48 
49  //---------------------------------------------------------------------------------------
50 
57  class GTSAM_EXPORT Base {
58 
59  public:
60  typedef std::shared_ptr<Base> shared_ptr;
61 
62  protected:
63 
64  size_t dim_;
65 
66  public:
67 
69  Base(size_t dim = 1):dim_(dim) {}
70  virtual ~Base() {}
71 
73  virtual bool isConstrained() const { return false; } // default false
74 
76  virtual bool isUnit() const { return false; } // default false
77 
79  inline size_t dim() const { return dim_;}
80 
81  virtual void print(const std::string& name = "") const = 0;
82 
83  virtual bool equals(const Base& expected, double tol=1e-9) const = 0;
84 
86  virtual Vector sigmas() const;
87 
89  virtual Vector whiten(const Vector& v) const = 0;
90 
92  virtual Matrix Whiten(const Matrix& H) const = 0;
93 
95  virtual Vector unwhiten(const Vector& v) const = 0;
96 
98  virtual double squaredMahalanobisDistance(const Vector& v) const;
99 
101  virtual double mahalanobisDistance(const Vector& v) const {
102  return std::sqrt(squaredMahalanobisDistance(v));
103  }
104 
106  virtual double loss(const double squared_distance) const {
107  return 0.5 * squared_distance;
108  }
109 
110  virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const = 0;
111  virtual void WhitenSystem(Matrix& A, Vector& b) const = 0;
112  virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0;
113  virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const = 0;
114 
116  virtual void whitenInPlace(Vector& v) const {
117  v = whiten(v);
118  }
119 
121  virtual void unwhitenInPlace(Vector& v) const {
122  v = unwhiten(v);
123  }
124 
126  virtual void whitenInPlace(Eigen::Block<Vector>& v) const {
127  v = whiten(v);
128  }
129 
131  virtual void unwhitenInPlace(Eigen::Block<Vector>& v) const {
132  v = unwhiten(v);
133  }
134 
136  virtual Vector unweightedWhiten(const Vector& v) const {
137  return whiten(v);
138  }
139 
141  virtual double weight(const Vector& v) const { return 1.0; }
142 
143  private:
144 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
145 
146  friend class boost::serialization::access;
147  template<class ARCHIVE>
148  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
149  ar & BOOST_SERIALIZATION_NVP(dim_);
150  }
151 #endif
152  };
153 
154  //---------------------------------------------------------------------------------------
155 
168  class GTSAM_EXPORT Gaussian: public Base {
169 
170  protected:
171 
173  std::optional<Matrix> sqrt_information_;
174 
175  private:
176 
180  const Matrix& thisR() const {
181  // should never happen
182  if (!sqrt_information_) throw std::runtime_error("Gaussian: has no R matrix");
183  return *sqrt_information_;
184  }
185 
186 
187  public:
188 
189  typedef std::shared_ptr<Gaussian> shared_ptr;
190 
192  Gaussian(size_t dim = 1,
193  const std::optional<Matrix>& sqrt_information = {})
194  : Base(dim), sqrt_information_(sqrt_information) {}
195 
196  ~Gaussian() override {}
197 
203  static shared_ptr SqrtInformation(const Matrix& R, bool smart = true);
204 
210  static shared_ptr Information(const Matrix& M, bool smart = true);
211 
217  static shared_ptr Covariance(const Matrix& covariance, bool smart = true);
218 
219  void print(const std::string& name) const override;
220  bool equals(const Base& expected, double tol=1e-9) const override;
221  Vector sigmas() const override;
222  Vector whiten(const Vector& v) const override;
223  Vector unwhiten(const Vector& v) const override;
224 
229  Matrix Whiten(const Matrix& H) const override;
230 
234  virtual void WhitenInPlace(Matrix& H) const;
235 
239  virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;
240 
244  void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
245  void WhitenSystem(Matrix& A, Vector& b) const override;
246  void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
247  void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
248 
258  virtual std::shared_ptr<Diagonal> QR(Matrix& Ab) const;
259 
261  virtual Matrix R() const { return thisR();}
262 
264  virtual Matrix information() const;
265 
267  virtual Matrix covariance() const;
268 
269  private:
270 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
271 
272  friend class boost::serialization::access;
273  template<class ARCHIVE>
274  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
275  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
276  ar & BOOST_SERIALIZATION_NVP(sqrt_information_);
277  }
278 #endif
279  }; // Gaussian
280 
281  //---------------------------------------------------------------------------------------
282 
288  class GTSAM_EXPORT Diagonal : public Gaussian {
289  protected:
290 
296  Vector sigmas_, invsigmas_, precisions_;
297 
298  protected:
299 
301  Diagonal(const Vector& sigmas);
302 
303  public:
305  Diagonal();
306 
307  typedef std::shared_ptr<Diagonal> shared_ptr;
308 
309  ~Diagonal() override {}
310 
315  static shared_ptr Sigmas(const Vector& sigmas, bool smart = true);
316 
323  static shared_ptr Variances(const Vector& variances, bool smart = true);
324 
329  static shared_ptr Precisions(const Vector& precisions, bool smart = true);
330 
331  void print(const std::string& name) const override;
332  Vector sigmas() const override { return sigmas_; }
333  Vector whiten(const Vector& v) const override;
334  Vector unwhiten(const Vector& v) const override;
335  Matrix Whiten(const Matrix& H) const override;
336  void WhitenInPlace(Matrix& H) const override;
337  void WhitenInPlace(Eigen::Block<Matrix> H) const override;
338 
342  inline double sigma(size_t i) const { return sigmas_(i); }
343 
347  inline const Vector& invsigmas() const { return invsigmas_; }
348  inline double invsigma(size_t i) const {return invsigmas_(i);}
349 
353  inline const Vector& precisions() const { return precisions_; }
354  inline double precision(size_t i) const {return precisions_(i);}
355 
359  Matrix R() const override {
360  return invsigmas().asDiagonal();
361  }
362 
363  private:
364 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
365 
366  friend class boost::serialization::access;
367  template<class ARCHIVE>
368  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
369  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Gaussian);
370  ar & BOOST_SERIALIZATION_NVP(sigmas_);
371  ar & BOOST_SERIALIZATION_NVP(invsigmas_);
372  }
373 #endif
374  }; // Diagonal
375 
376  //---------------------------------------------------------------------------------------
377 
390  class GTSAM_EXPORT Constrained : public Diagonal {
391  protected:
392 
393  // Sigmas are contained in the base class
395 
401  Constrained(const Vector& mu, const Vector& sigmas);
402 
403  public:
404 
405  typedef std::shared_ptr<Constrained> shared_ptr;
406 
413  Constrained(const Vector& sigmas = Z_1x1);
414 
415  ~Constrained() override {}
416 
418  bool isConstrained() const override { return true; }
419 
421  bool constrained(size_t i) const;
422 
424  const Vector& mu() const { return mu_; }
425 
430  static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas);
431 
436  static shared_ptr MixedSigmas(const Vector& sigmas);
437 
442  static shared_ptr MixedSigmas(double m, const Vector& sigmas);
443 
448  static shared_ptr MixedVariances(const Vector& mu, const Vector& variances);
449  static shared_ptr MixedVariances(const Vector& variances);
450 
455  static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions);
456  static shared_ptr MixedPrecisions(const Vector& precisions);
457 
463  double squaredMahalanobisDistance(const Vector& v) const override;
464 
466  static shared_ptr All(size_t dim) {
467  return shared_ptr(new Constrained(Vector::Constant(dim, 1000.0), Vector::Constant(dim,0)));
468  }
469 
471  static shared_ptr All(size_t dim, const Vector& mu) {
472  return shared_ptr(new Constrained(mu, Vector::Constant(dim,0)));
473  }
474 
476  static shared_ptr All(size_t dim, double mu) {
477  return shared_ptr(new Constrained(Vector::Constant(dim, mu), Vector::Constant(dim,0)));
478  }
479 
480  void print(const std::string& name) const override;
481 
483  Vector whiten(const Vector& v) const override;
484 
487  Matrix Whiten(const Matrix& H) const override;
488  void WhitenInPlace(Matrix& H) const override;
489  void WhitenInPlace(Eigen::Block<Matrix> H) const override;
490 
500  Diagonal::shared_ptr QR(Matrix& Ab) const override;
501 
506  shared_ptr unit() const;
507 
508  private:
509 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
510 
511  friend class boost::serialization::access;
512  template<class ARCHIVE>
513  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
514  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal);
515  ar & BOOST_SERIALIZATION_NVP(mu_);
516  }
517 #endif
518 
519  }; // Constrained
520 
521  //---------------------------------------------------------------------------------------
522 
527  class GTSAM_EXPORT Isotropic : public Diagonal {
528  protected:
529  double sigma_, invsigma_;
530 
532  Isotropic(size_t dim, double sigma) :
533  Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
534 
535  public:
536 
537  /* dummy constructor to allow for serialization */
538  Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {}
539 
540  ~Isotropic() override {}
541 
542  typedef std::shared_ptr<Isotropic> shared_ptr;
543 
547  static shared_ptr Sigma(size_t dim, double sigma, bool smart = true);
548 
555  static shared_ptr Variance(size_t dim, double variance, bool smart = true);
556 
560  static shared_ptr Precision(size_t dim, double precision, bool smart = true) {
561  return Variance(dim, 1.0/precision, smart);
562  }
563 
564  void print(const std::string& name) const override;
565  double squaredMahalanobisDistance(const Vector& v) const override;
566  Vector whiten(const Vector& v) const override;
567  Vector unwhiten(const Vector& v) const override;
568  Matrix Whiten(const Matrix& H) const override;
569  void WhitenInPlace(Matrix& H) const override;
570  void whitenInPlace(Vector& v) const override;
571  void WhitenInPlace(Eigen::Block<Matrix> H) const override;
572 
576  inline double sigma() const { return sigma_; }
577 
578  private:
579 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
580 
581  friend class boost::serialization::access;
582  template<class ARCHIVE>
583  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
584  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal);
585  ar & BOOST_SERIALIZATION_NVP(sigma_);
586  ar & BOOST_SERIALIZATION_NVP(invsigma_);
587  }
588 #endif
589 
590  };
591 
592  //---------------------------------------------------------------------------------------
593 
597  class GTSAM_EXPORT Unit : public Isotropic {
598  public:
599 
600  typedef std::shared_ptr<Unit> shared_ptr;
601 
603  Unit(size_t dim=1): Isotropic(dim,1.0) {}
604 
605  ~Unit() override {}
606 
610  static shared_ptr Create(size_t dim) {
611  return shared_ptr(new Unit(dim));
612  }
613 
615  bool isUnit() const override { return true; }
616 
617  void print(const std::string& name) const override;
618  double squaredMahalanobisDistance(const Vector& v) const override;
619  Vector whiten(const Vector& v) const override { return v; }
620  Vector unwhiten(const Vector& v) const override { return v; }
621  Matrix Whiten(const Matrix& H) const override { return H; }
622  void WhitenInPlace(Matrix& /*H*/) const override {}
623  void WhitenInPlace(Eigen::Block<Matrix> /*H*/) const override {}
624  void whitenInPlace(Vector& /*v*/) const override {}
625  void unwhitenInPlace(Vector& /*v*/) const override {}
626  void whitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
627  void unwhitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
628 
629  private:
630 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
631 
632  friend class boost::serialization::access;
633  template<class ARCHIVE>
634  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
635  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Isotropic);
636  }
637 #endif
638  };
639 
657  class GTSAM_EXPORT Robust : public Base {
658  public:
659  typedef std::shared_ptr<Robust> shared_ptr;
660 
661  protected:
664 
667 
668  public:
669 
671  Robust() {}
672 
675  : Base(noise->dim()), robust_(robust), noise_(noise) {}
676 
678  ~Robust() override {}
679 
680  void print(const std::string& name) const override;
681  bool equals(const Base& expected, double tol=1e-9) const override;
682 
684  const RobustModel::shared_ptr& robust() const { return robust_; }
685 
687  const NoiseModel::shared_ptr& noise() const { return noise_; }
688 
689  // Functions below are dummy but necessary for the noiseModel::Base
690  inline Vector whiten(const Vector& v) const override
691  { Vector r = v; this->WhitenSystem(r); return r; }
692  inline Matrix Whiten(const Matrix& A) const override
693  { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
694  inline Vector unwhiten(const Vector& /*v*/) const override
695  { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
697  double loss(const double squared_distance) const override {
698  return robust_->loss(std::sqrt(squared_distance));
699  }
700 
701  // NOTE: This is special because in whiten the base version will do the reweighting
702  // which is incorrect!
703  double squaredMahalanobisDistance(const Vector& v) const override {
704  return noise_->squaredMahalanobisDistance(v);
705  }
706 
707  // These are really robust iterated re-weighting support functions
708  virtual void WhitenSystem(Vector& b) const;
709  void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
710  void WhitenSystem(Matrix& A, Vector& b) const override;
711  void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
712  void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
713 
714  Vector unweightedWhiten(const Vector& v) const override;
715  double weight(const Vector& v) const override;
716 
717  static shared_ptr Create(
718  const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);
719 
720  private:
721 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
722 
723  friend class boost::serialization::access;
724  template<class ARCHIVE>
725  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
726  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
727  ar & boost::serialization::make_nvp("robust_", const_cast<RobustModel::shared_ptr&>(robust_));
728  ar & boost::serialization::make_nvp("noise_", const_cast<NoiseModel::shared_ptr&>(noise_));
729  }
730 #endif
731  };
732 
733  // Helper function
734  GTSAM_EXPORT std::optional<Vector> checkIfDiagonal(const Matrix& M);
735 
736  } // namespace noiseModel
737 
746 
748  template<> struct traits<noiseModel::Gaussian> : public Testable<noiseModel::Gaussian> {};
749  template<> struct traits<noiseModel::Diagonal> : public Testable<noiseModel::Diagonal> {};
750  template<> struct traits<noiseModel::Constrained> : public Testable<noiseModel::Constrained> {};
751  template<> struct traits<noiseModel::Isotropic> : public Testable<noiseModel::Isotropic> {};
752  template<> struct traits<noiseModel::Unit> : public Testable<noiseModel::Unit> {};
753 
754 } //\ namespace gtsam
755 
756 
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
Matrix3f m
Matrix Whiten(const Matrix &H) const override
Definition: NoiseModel.h:621
static shared_ptr All(size_t dim, const Vector &mu)
Definition: NoiseModel.h:471
Unit(size_t dim=1)
Definition: NoiseModel.h:603
static Matrix A1
Vector mu_
Penalty function weight - needs to be large enough to dominate soft constraints.
Definition: NoiseModel.h:394
double squaredMahalanobisDistance(const Vector &v) const override
Squared Mahalanobis distance v&#39;*R&#39;*R*v = <R*v,R*v>
Definition: NoiseModel.h:703
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
virtual double weight(const Vector &v) const
Definition: NoiseModel.h:141
Concept check for values that can be used in unit tests.
virtual void whitenInPlace(Eigen::Block< Vector > &v) const
Definition: NoiseModel.h:126
std::string serialize(const T &input)
serializes to a string
virtual void unwhitenInPlace(Eigen::Block< Vector > &v) const
Definition: NoiseModel.h:131
const Vector & invsigmas() const
Definition: NoiseModel.h:347
Matrix expected
Definition: testMatrix.cpp:971
const RobustModel::shared_ptr & robust() const
Return the contained robust error function.
Definition: NoiseModel.h:684
std::shared_ptr< Base > shared_ptr
Definition: LossFunctions.h:70
double mu
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
virtual double mahalanobisDistance(const Vector &v) const
Mahalanobis distance.
Definition: NoiseModel.h:101
std::shared_ptr< Unit > shared_ptr
Definition: NoiseModel.h:600
Rot2 R(Rot2::fromAngle(0.1))
void unwhitenInPlace(Eigen::Block< Vector > &) const override
Definition: NoiseModel.h:627
Gaussian(size_t dim=1, const std::optional< Matrix > &sqrt_information={})
Definition: NoiseModel.h:192
noiseModel::Isotropic::shared_ptr SharedIsotropic
Definition: NoiseModel.h:745
virtual bool isUnit() const
true if a unit noise model, saves slow/clumsy dynamic casting
Definition: NoiseModel.h:76
virtual bool isConstrained() const
true if a constrained noise model, saves slow/clumsy dynamic casting
Definition: NoiseModel.h:73
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
void WhitenInPlace(Eigen::Block< Matrix >) const override
Definition: NoiseModel.h:623
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:610
bool isConstrained() const override
true if a constrained noise mode, saves slow/clumsy dynamic casting
Definition: NoiseModel.h:418
virtual void whitenInPlace(Vector &v) const
Definition: NoiseModel.h:116
std::shared_ptr< Constrained > shared_ptr
Definition: NoiseModel.h:405
double sigma(size_t i) const
Definition: NoiseModel.h:342
std::optional< Matrix > sqrt_information_
Definition: NoiseModel.h:173
static shared_ptr All(size_t dim, double mu)
Definition: NoiseModel.h:476
virtual double loss(const double squared_distance) const
loss function, input is Mahalanobis distance
Definition: NoiseModel.h:106
const Matrix & thisR() const
Definition: NoiseModel.h:180
virtual Vector unweightedWhiten(const Vector &v) const
Definition: NoiseModel.h:136
const RobustModel::shared_ptr robust_
robust error function used
Definition: NoiseModel.h:665
Eigen::VectorXd Vector
Definition: Vector.h:38
Vector whiten(const Vector &v) const override
Whiten an error vector.
Definition: NoiseModel.h:690
const Vector & precisions() const
Definition: NoiseModel.h:353
Robust()
Default Constructor for serialization.
Definition: NoiseModel.h:671
mEstimator::Base RobustModel
Definition: NoiseModel.h:662
Vector sigmas() const override
Calculate standard deviations.
Definition: NoiseModel.h:332
const NoiseModel::shared_ptr noise_
noise model used
Definition: NoiseModel.h:666
std::shared_ptr< Base > shared_ptr
Definition: NoiseModel.h:60
void WhitenInPlace(Matrix &) const override
Definition: NoiseModel.h:622
void unwhitenInPlace(Vector &) const override
Definition: NoiseModel.h:625
Array< int, Dynamic, 1 > v
double loss(const double squared_distance) const override
Compute loss from the m-estimator using the Mahalanobis distance.
Definition: NoiseModel.h:697
virtual void unwhitenInPlace(Vector &v) const
Definition: NoiseModel.h:121
Array< double, 1, 3 > e(1./3., 0.5, 2.)
~Robust() override
Destructor.
Definition: NoiseModel.h:678
std::shared_ptr< Isotropic > shared_ptr
Definition: NoiseModel.h:542
const G & b
Definition: Group.h:86
void whitenInPlace(Eigen::Block< Vector > &) const override
Definition: NoiseModel.h:626
Base(size_t dim=1)
primary constructor
Definition: NoiseModel.h:69
Vector whiten(const Vector &v) const override
Whiten an error vector.
Definition: NoiseModel.h:619
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
noiseModel::Constrained::shared_ptr SharedConstrained
Definition: NoiseModel.h:744
traits
Definition: chartTesting.h:28
double invsigma(size_t i) const
Definition: NoiseModel.h:348
std::optional< Vector > checkIfDiagonal(const Matrix &M)
Definition: NoiseModel.cpp:47
std::shared_ptr< Robust > shared_ptr
Definition: NoiseModel.h:659
Expression of a fixed-size or dynamic-size block.
Definition: Block.h:103
noiseModel::Base NoiseModel
Definition: NoiseModel.h:663
cout precision(2)
const Vector & mu() const
Access mu as a vector.
Definition: NoiseModel.h:424
const NoiseModel::shared_ptr & noise() const
Return the contained noise model.
Definition: NoiseModel.h:687
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix &covariance)
Matrix R() const override
Definition: NoiseModel.h:359
static const double sigma
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Annotation for function names.
Definition: attr.h:48
const G double tol
Definition: Group.h:86
double precision(size_t i) const
Definition: NoiseModel.h:354
std::shared_ptr< Gaussian > shared_ptr
Definition: NoiseModel.h:189
Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
Constructor.
Definition: NoiseModel.h:674
void whitenInPlace(Vector &) const override
Definition: NoiseModel.h:624
The matrix class, also used for vectors and row-vectors.
Vector unwhiten(const Vector &) const override
Unwhiten an error vector.
Definition: NoiseModel.h:694
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:307
Vector unwhiten(const Vector &v) const override
Unwhiten an error vector.
Definition: NoiseModel.h:620
size_t dim() const
Dimensionality.
Definition: NoiseModel.h:79
static shared_ptr Precision(size_t dim, double precision, bool smart=true)
Definition: NoiseModel.h:560
Matrix Whiten(const Matrix &A) const override
Whiten a matrix.
Definition: NoiseModel.h:692
noiseModel::Gaussian::shared_ptr SharedGaussian
Definition: NoiseModel.h:742
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
Isotropic(size_t dim, double sigma)
Definition: NoiseModel.h:532
virtual Matrix R() const
Return R itself, but note that Whiten(H) is cheaper than R*H.
Definition: NoiseModel.h:261
static shared_ptr All(size_t dim)
Definition: NoiseModel.h:466
bool isUnit() const override
true if a unit noise model, saves slow/clumsy dynamic casting
Definition: NoiseModel.h:615


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:57