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 
187  virtual double logDetR() const;
188 
189  public:
190 
191  typedef std::shared_ptr<Gaussian> shared_ptr;
192 
194  Gaussian(size_t dim = 1,
195  const std::optional<Matrix>& sqrt_information = {})
196  : Base(dim), sqrt_information_(sqrt_information) {}
197 
198  ~Gaussian() override {}
199 
205  static shared_ptr SqrtInformation(const Matrix& R, bool smart = true);
206 
212  static shared_ptr Information(const Matrix& M, bool smart = true);
213 
219  static shared_ptr Covariance(const Matrix& covariance, bool smart = true);
220 
221  void print(const std::string& name) const override;
222  bool equals(const Base& expected, double tol=1e-9) const override;
223  Vector sigmas() const override;
224  Vector whiten(const Vector& v) const override;
225  Vector unwhiten(const Vector& v) const override;
226 
231  Matrix Whiten(const Matrix& H) const override;
232 
236  virtual void WhitenInPlace(Matrix& H) const;
237 
241  virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;
242 
246  void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
247  void WhitenSystem(Matrix& A, Vector& b) const override;
248  void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
249  void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
250 
260  virtual std::shared_ptr<Diagonal> QR(Matrix& Ab) const;
261 
263  virtual Matrix R() const { return thisR();}
264 
266  virtual Matrix information() const;
267 
269  virtual Matrix covariance() const;
270 
272  double logDeterminant() const;
273 
280  double negLogConstant() const;
281 
282  private:
283 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
284 
285  friend class boost::serialization::access;
286  template<class ARCHIVE>
287  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
288  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
289  ar & BOOST_SERIALIZATION_NVP(sqrt_information_);
290  }
291 #endif
292  }; // Gaussian
293 
294  //---------------------------------------------------------------------------------------
295 
301  class GTSAM_EXPORT Diagonal : public Gaussian {
302  protected:
303 
309  Vector sigmas_, invsigmas_, precisions_;
310 
312  Diagonal(const Vector& sigmas);
313 
315  virtual double logDetR() const override;
316 
317  public:
319  Diagonal();
320 
321  typedef std::shared_ptr<Diagonal> shared_ptr;
322 
323  ~Diagonal() override {}
324 
329  static shared_ptr Sigmas(const Vector& sigmas, bool smart = true);
330 
337  static shared_ptr Variances(const Vector& variances, bool smart = true);
338 
343  static shared_ptr Precisions(const Vector& precisions, bool smart = true);
344 
345  void print(const std::string& name) const override;
346  Vector sigmas() const override { return sigmas_; }
347  Vector whiten(const Vector& v) const override;
348  Vector unwhiten(const Vector& v) const override;
349  Matrix Whiten(const Matrix& H) const override;
350  void WhitenInPlace(Matrix& H) const override;
351  void WhitenInPlace(Eigen::Block<Matrix> H) const override;
352 
356  inline double sigma(size_t i) const { return sigmas_(i); }
357 
361  inline const Vector& invsigmas() const { return invsigmas_; }
362  inline double invsigma(size_t i) const {return invsigmas_(i);}
363 
367  inline const Vector& precisions() const { return precisions_; }
368  inline double precision(size_t i) const {return precisions_(i);}
369 
373  Matrix R() const override {
374  return invsigmas().asDiagonal();
375  }
376 
377  private:
378 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
379 
380  friend class boost::serialization::access;
381  template<class ARCHIVE>
382  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
383  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Gaussian);
384  ar & BOOST_SERIALIZATION_NVP(sigmas_);
385  ar & BOOST_SERIALIZATION_NVP(invsigmas_);
386  }
387 #endif
388  }; // Diagonal
389 
390  //---------------------------------------------------------------------------------------
391 
404  class GTSAM_EXPORT Constrained : public Diagonal {
405  protected:
406 
407  // Sigmas are contained in the base class
409 
415  Constrained(const Vector& mu, const Vector& sigmas);
416 
417  public:
418 
419  typedef std::shared_ptr<Constrained> shared_ptr;
420 
427  Constrained(const Vector& sigmas = Z_1x1);
428 
429  ~Constrained() override {}
430 
432  bool isConstrained() const override { return true; }
433 
435  bool constrained(size_t i) const;
436 
438  const Vector& mu() const { return mu_; }
439 
444  static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas);
445 
450  static shared_ptr MixedSigmas(const Vector& sigmas);
451 
456  static shared_ptr MixedSigmas(double m, const Vector& sigmas);
457 
462  static shared_ptr MixedVariances(const Vector& mu, const Vector& variances);
463  static shared_ptr MixedVariances(const Vector& variances);
464 
469  static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions);
470  static shared_ptr MixedPrecisions(const Vector& precisions);
471 
477  double squaredMahalanobisDistance(const Vector& v) const override;
478 
480  static shared_ptr All(size_t dim) {
481  return shared_ptr(new Constrained(Vector::Constant(dim, 1000.0), Vector::Constant(dim,0)));
482  }
483 
485  static shared_ptr All(size_t dim, const Vector& mu) {
486  return shared_ptr(new Constrained(mu, Vector::Constant(dim,0)));
487  }
488 
490  static shared_ptr All(size_t dim, double mu) {
491  return shared_ptr(new Constrained(Vector::Constant(dim, mu), Vector::Constant(dim,0)));
492  }
493 
494  void print(const std::string& name) const override;
495 
497  Vector whiten(const Vector& v) const override;
498 
501  Matrix Whiten(const Matrix& H) const override;
502  void WhitenInPlace(Matrix& H) const override;
503  void WhitenInPlace(Eigen::Block<Matrix> H) const override;
504 
514  Diagonal::shared_ptr QR(Matrix& Ab) const override;
515 
520  shared_ptr unit() const;
521 
522  private:
523 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
524 
525  friend class boost::serialization::access;
526  template<class ARCHIVE>
527  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
528  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal);
529  ar & BOOST_SERIALIZATION_NVP(mu_);
530  }
531 #endif
532 
533  }; // Constrained
534 
535  //---------------------------------------------------------------------------------------
536 
541  class GTSAM_EXPORT Isotropic : public Diagonal {
542  protected:
543  double sigma_, invsigma_;
544 
546  Isotropic(size_t dim, double sigma) :
547  Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
548 
550  virtual double logDetR() const override;
551 
552  public:
553 
554  /* dummy constructor to allow for serialization */
555  Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {}
556 
557  ~Isotropic() override {}
558 
559  typedef std::shared_ptr<Isotropic> shared_ptr;
560 
564  static shared_ptr Sigma(size_t dim, double sigma, bool smart = true);
565 
572  static shared_ptr Variance(size_t dim, double variance, bool smart = true);
573 
577  static shared_ptr Precision(size_t dim, double precision, bool smart = true) {
578  return Variance(dim, 1.0/precision, smart);
579  }
580 
581  void print(const std::string& name) const override;
582  double squaredMahalanobisDistance(const Vector& v) const override;
583  Vector whiten(const Vector& v) const override;
584  Vector unwhiten(const Vector& v) const override;
585  Matrix Whiten(const Matrix& H) const override;
586  void WhitenInPlace(Matrix& H) const override;
587  void whitenInPlace(Vector& v) const override;
588  void WhitenInPlace(Eigen::Block<Matrix> H) const override;
589 
593  inline double sigma() const { return sigma_; }
594 
595  private:
596 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
597 
598  friend class boost::serialization::access;
599  template<class ARCHIVE>
600  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
601  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal);
602  ar & BOOST_SERIALIZATION_NVP(sigma_);
603  ar & BOOST_SERIALIZATION_NVP(invsigma_);
604  }
605 #endif
606 
607  };
608 
609  //---------------------------------------------------------------------------------------
610 
614  class GTSAM_EXPORT Unit : public Isotropic {
615  protected:
617  virtual double logDetR() const override;
618 
619  public:
620 
621  typedef std::shared_ptr<Unit> shared_ptr;
622 
624  Unit(size_t dim=1): Isotropic(dim,1.0) {}
625 
626  ~Unit() override {}
627 
631  static shared_ptr Create(size_t dim) {
632  return shared_ptr(new Unit(dim));
633  }
634 
636  bool isUnit() const override { return true; }
637 
638  void print(const std::string& name) const override;
639  double squaredMahalanobisDistance(const Vector& v) const override;
640  Vector whiten(const Vector& v) const override { return v; }
641  Vector unwhiten(const Vector& v) const override { return v; }
642  Matrix Whiten(const Matrix& H) const override { return H; }
643  void WhitenInPlace(Matrix& /*H*/) const override {}
644  void WhitenInPlace(Eigen::Block<Matrix> /*H*/) const override {}
645  void whitenInPlace(Vector& /*v*/) const override {}
646  void unwhitenInPlace(Vector& /*v*/) const override {}
647  void whitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
648  void unwhitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
649 
650  private:
651 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
652 
653  friend class boost::serialization::access;
654  template<class ARCHIVE>
655  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
656  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Isotropic);
657  }
658 #endif
659  };
660 
678  class GTSAM_EXPORT Robust : public Base {
679  public:
680  typedef std::shared_ptr<Robust> shared_ptr;
681 
682  protected:
685 
688 
689  public:
690 
692  Robust() {}
693 
696  : Base(noise->dim()), robust_(robust), noise_(noise) {}
697 
699  ~Robust() override {}
700 
701  void print(const std::string& name) const override;
702  bool equals(const Base& expected, double tol=1e-9) const override;
703 
705  const RobustModel::shared_ptr& robust() const { return robust_; }
706 
708  const NoiseModel::shared_ptr& noise() const { return noise_; }
709 
710  // Functions below are dummy but necessary for the noiseModel::Base
711  inline Vector whiten(const Vector& v) const override
712  { Vector r = v; this->WhitenSystem(r); return r; }
713  inline Matrix Whiten(const Matrix& A) const override
714  { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
715  inline Vector unwhiten(const Vector& /*v*/) const override
716  { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
718  double loss(const double squared_distance) const override {
719  return robust_->loss(std::sqrt(squared_distance));
720  }
721 
722  // NOTE: This is special because in whiten the base version will do the reweighting
723  // which is incorrect!
724  double squaredMahalanobisDistance(const Vector& v) const override {
725  return noise_->squaredMahalanobisDistance(v);
726  }
727 
728  // These are really robust iterated re-weighting support functions
729  virtual void WhitenSystem(Vector& b) const;
730  void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
731  void WhitenSystem(Matrix& A, Vector& b) const override;
732  void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
733  void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
734 
735  Vector unweightedWhiten(const Vector& v) const override;
736  double weight(const Vector& v) const override;
737 
738  static shared_ptr Create(
739  const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);
740 
741  private:
742 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
743 
744  friend class boost::serialization::access;
745  template<class ARCHIVE>
746  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
747  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
748  ar & boost::serialization::make_nvp("robust_", const_cast<RobustModel::shared_ptr&>(robust_));
749  ar & boost::serialization::make_nvp("noise_", const_cast<NoiseModel::shared_ptr&>(noise_));
750  }
751 #endif
752  };
753 
754  // Helper function
755  GTSAM_EXPORT std::optional<Vector> checkIfDiagonal(const Matrix& M);
756 
757  } // namespace noiseModel
758 
767 
769  template<> struct traits<noiseModel::Gaussian> : public Testable<noiseModel::Gaussian> {};
770  template<> struct traits<noiseModel::Diagonal> : public Testable<noiseModel::Diagonal> {};
771  template<> struct traits<noiseModel::Constrained> : public Testable<noiseModel::Constrained> {};
772  template<> struct traits<noiseModel::Isotropic> : public Testable<noiseModel::Isotropic> {};
773  template<> struct traits<noiseModel::Unit> : public Testable<noiseModel::Unit> {};
774 
775 } //\ namespace gtsam
776 
777 
gtsam::noiseModel::Isotropic::Isotropic
Isotropic()
Definition: NoiseModel.h:555
Gaussian
double Gaussian(double mu, double sigma, double z)
Gaussian density function.
Definition: testGaussianMixture.cpp:47
H
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
Definition: gnuplot_common_settings.hh:74
gtsam::noiseModel::Base::isUnit
virtual bool isUnit() const
true if a unit noise model, saves slow/clumsy dynamic casting
Definition: NoiseModel.h:76
gtsam::noiseModel::Base::loss
virtual double loss(const double squared_distance) const
loss function, input is Mahalanobis distance
Definition: NoiseModel.h:106
name
Annotation for function names.
Definition: attr.h:51
gtsam::noiseModel::Diagonal::invsigma
double invsigma(size_t i) const
Definition: NoiseModel.h:362
Eigen::Block
Expression of a fixed-size or dynamic-size block.
Definition: Block.h:103
A3
static const double A3[]
Definition: expn.h:8
gtsam::noiseModel::Robust::robust
const RobustModel::shared_ptr & robust() const
Return the contained robust error function.
Definition: NoiseModel.h:705
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::noiseModel::Base
Definition: NoiseModel.h:57
gtsam::noiseModel::Unit::unwhitenInPlace
void unwhitenInPlace(Eigen::Block< Vector > &) const override
Definition: NoiseModel.h:648
gtsam::noiseModel::Unit::unwhitenInPlace
void unwhitenInPlace(Vector &) const override
Definition: NoiseModel.h:646
Testable.h
Concept check for values that can be used in unit tests.
gtsam::noiseModel::Base::shared_ptr
std::shared_ptr< Base > shared_ptr
Definition: NoiseModel.h:60
test_constructor::sigmas
Vector1 sigmas
Definition: testHybridNonlinearFactor.cpp:52
gtsam::noiseModel::Unit::WhitenInPlace
void WhitenInPlace(Matrix &) const override
Definition: NoiseModel.h:643
mu
double mu
Definition: testBoundingConstraint.cpp:37
gtsam::noiseModel::Unit
Definition: NoiseModel.h:614
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::noiseModel::Diagonal
Definition: NoiseModel.h:301
gtsam::noiseModel::Base::mahalanobisDistance
virtual double mahalanobisDistance(const Vector &v) const
Mahalanobis distance.
Definition: NoiseModel.h:101
gtsam::noiseModel::Gaussian::Gaussian
Gaussian(size_t dim=1, const std::optional< Matrix > &sqrt_information={})
Definition: NoiseModel.h:194
gtsam::noiseModel::Gaussian::sqrt_information_
std::optional< Matrix > sqrt_information_
Definition: NoiseModel.h:173
gtsam::noiseModel::Isotropic::sigma_
double sigma_
Definition: NoiseModel.h:543
gtsam::noiseModel::Unit::shared_ptr
std::shared_ptr< Unit > shared_ptr
Definition: NoiseModel.h:621
gtsam::noiseModel::Diagonal::invsigmas
const Vector & invsigmas() const
Definition: NoiseModel.h:361
B
Definition: test_numpy_dtypes.cpp:299
gtsam::noiseModel::Base::~Base
virtual ~Base()
Definition: NoiseModel.h:70
gtsam::noiseModel::Gaussian
Definition: NoiseModel.h:168
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::noiseModel::Isotropic::Isotropic
Isotropic(size_t dim, double sigma)
Definition: NoiseModel.h:546
gtsam::SharedConstrained
noiseModel::Constrained::shared_ptr SharedConstrained
Definition: NoiseModel.h:765
gtsam::noiseModel::Constrained::~Constrained
~Constrained() override
Definition: NoiseModel.h:429
gtsam::noiseModel::Isotropic::Precision
static shared_ptr Precision(size_t dim, double precision, bool smart=true)
Definition: NoiseModel.h:577
gtsam::noiseModel::Base::dim
size_t dim() const
Dimensionality.
Definition: NoiseModel.h:79
gtsam::noiseModel::Gaussian::shared_ptr
std::shared_ptr< Gaussian > shared_ptr
Definition: NoiseModel.h:191
gtsam::noiseModel::Unit::Unit
Unit(size_t dim=1)
Definition: NoiseModel.h:624
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::noiseModel::Constrained
Definition: NoiseModel.h:404
gtsam::noiseModel::Isotropic::sigma
double sigma() const
Definition: NoiseModel.h:593
sampling::sigma
static const double sigma
Definition: testGaussianBayesNet.cpp:170
gtsam::noiseModel::Diagonal::sigmas
Vector sigmas() const override
Calculate standard deviations.
Definition: NoiseModel.h:346
gtsam::noiseModel::Base::unweightedWhiten
virtual Vector unweightedWhiten(const Vector &v) const
Definition: NoiseModel.h:136
gtsam::noiseModel::Base::isConstrained
virtual bool isConstrained() const
true if a constrained noise model, saves slow/clumsy dynamic casting
Definition: NoiseModel.h:73
gtsam::noiseModel::Constrained::mu_
Vector mu_
Penalty function weight - needs to be large enough to dominate soft constraints.
Definition: NoiseModel.h:408
gtsam::noiseModel::Constrained::mu
const Vector & mu() const
Access mu as a vector.
Definition: NoiseModel.h:438
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::noiseModel::Robust::Robust
Robust()
Default Constructor for serialization.
Definition: NoiseModel.h:692
gtsam::noiseModel::mEstimator::Base::shared_ptr
std::shared_ptr< Base > shared_ptr
Definition: LossFunctions.h:71
A
Definition: test_numpy_dtypes.cpp:298
gtsam::noiseModel::Base::weight
virtual double weight(const Vector &v) const
Definition: NoiseModel.h:141
gtsam::noiseModel::Robust::~Robust
~Robust() override
Destructor.
Definition: NoiseModel.h:699
gtsam::noiseModel::Isotropic
Definition: NoiseModel.h:541
gtsam::noiseModel::Diagonal::precision
double precision(size_t i) const
Definition: NoiseModel.h:368
gtsam::noiseModel::Base::unwhitenInPlace
virtual void unwhitenInPlace(Vector &v) const
Definition: NoiseModel.h:121
gtsam::internal::logDeterminant
double logDeterminant(const typename BAYESTREE::sharedClique &clique)
Definition: GaussianBayesTree-inl.h:42
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:631
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
A2
static const double A2[]
Definition: expn.h:7
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::noiseModel::Unit::isUnit
bool isUnit() const override
true if a unit noise model, saves slow/clumsy dynamic casting
Definition: NoiseModel.h:636
gtsam::noiseModel::Diagonal::sigma
double sigma(size_t i) const
Definition: NoiseModel.h:356
gtsam::noiseModel::Diagonal::sigmas_
Vector sigmas_
Definition: NoiseModel.h:309
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim)
Definition: NoiseModel.h:480
gtsam::noiseModel::Robust
Definition: NoiseModel.h:678
gtsam::noiseModel::Robust::whiten
Vector whiten(const Vector &v) const override
Whiten an error vector.
Definition: NoiseModel.h:711
gtsam::SharedGaussian
noiseModel::Gaussian::shared_ptr SharedGaussian
Definition: NoiseModel.h:763
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::noiseModel::Robust::robust_
const RobustModel::shared_ptr robust_
robust error function used
Definition: NoiseModel.h:686
gtsam::noiseModel::Unit::whitenInPlace
void whitenInPlace(Eigen::Block< Vector > &) const override
Definition: NoiseModel.h:647
gtsam::noiseModel::Isotropic::~Isotropic
~Isotropic() override
Definition: NoiseModel.h:557
gtsam::noiseModel::Unit::whiten
Vector whiten(const Vector &v) const override
Whiten an error vector.
Definition: NoiseModel.h:640
gtsam::noiseModel::Robust::RobustModel
mEstimator::Base RobustModel
Definition: NoiseModel.h:683
LossFunctions.h
gtsam::noiseModel::checkIfDiagonal
std::optional< Vector > checkIfDiagonal(const Matrix &M)
Definition: NoiseModel.cpp:47
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim, const Vector &mu)
Definition: NoiseModel.h:485
gtsam::noiseModel::Diagonal::precisions
const Vector & precisions() const
Definition: NoiseModel.h:367
gtsam::noiseModel::Robust::noise_
const NoiseModel::shared_ptr noise_
noise model used
Definition: NoiseModel.h:687
gtsam::equals
Definition: Testable.h:112
std_optional_serialization.h
gtsam::noiseModel::Robust::NoiseModel
noiseModel::Base NoiseModel
Definition: NoiseModel.h:684
gtsam::b
const G & b
Definition: Group.h:79
gtsam::noiseModel::Diagonal::~Diagonal
~Diagonal() override
Definition: NoiseModel.h:323
gtsam::noiseModel::Robust::loss
double loss(const double squared_distance) const override
Compute loss from the m-estimator using the Mahalanobis distance.
Definition: NoiseModel.h:718
gtsam::noiseModel::Base::unwhitenInPlace
virtual void unwhitenInPlace(Eigen::Block< Vector > &v) const
Definition: NoiseModel.h:131
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
precision
cout precision(2)
gtsam::traits
Definition: Group.h:36
gtsam::noiseModel::Constrained::isConstrained
bool isConstrained() const override
true if a constrained noise mode, saves slow/clumsy dynamic casting
Definition: NoiseModel.h:432
gtsam::noiseModel::Base::whitenInPlace
virtual void whitenInPlace(Vector &v) const
Definition: NoiseModel.h:116
gtsam::noiseModel::Base::Base
Base(size_t dim=1)
primary constructor
Definition: NoiseModel.h:69
gtsam::noiseModel::Diagonal::shared_ptr
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:321
gtsam::noiseModel::Robust::unwhiten
Vector unwhiten(const Vector &) const override
Unwhiten an error vector.
Definition: NoiseModel.h:715
gtsam::noiseModel::Gaussian::R
virtual Matrix R() const
Return R itself, but note that Whiten(H) is cheaper than R*H.
Definition: NoiseModel.h:263
gtsam::Diagonal
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix &covariance)
Definition: ScenarioRunner.h:27
A1
static const double A1[]
Definition: expn.h:6
gtsam::noiseModel::Robust::shared_ptr
std::shared_ptr< Robust > shared_ptr
Definition: NoiseModel.h:680
exampleQR::Ab
Matrix Ab
Definition: testNoiseModel.cpp:207
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::noiseModel::Robust::Robust
Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
Constructor.
Definition: NoiseModel.h:695
gtsam::noiseModel::Robust::Whiten
Matrix Whiten(const Matrix &A) const override
Whiten a matrix.
Definition: NoiseModel.h:713
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::SharedIsotropic
noiseModel::Isotropic::shared_ptr SharedIsotropic
Definition: NoiseModel.h:766
gtsam::noiseModel::Unit::unwhiten
Vector unwhiten(const Vector &v) const override
Unwhiten an error vector.
Definition: NoiseModel.h:641
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::noiseModel::Isotropic::shared_ptr
std::shared_ptr< Isotropic > shared_ptr
Definition: NoiseModel.h:559
gtsam::noiseModel::Base::whitenInPlace
virtual void whitenInPlace(Eigen::Block< Vector > &v) const
Definition: NoiseModel.h:126
gtsam::noiseModel::Diagonal::R
Matrix R() const override
Definition: NoiseModel.h:373
gtsam::noiseModel::Base::dim_
size_t dim_
Definition: NoiseModel.h:64
gtsam::noiseModel::Robust::noise
const NoiseModel::shared_ptr & noise() const
Return the contained noise model.
Definition: NoiseModel.h:708
gtsam::noiseModel::mEstimator::Base
Definition: LossFunctions.h:66
gtsam::noiseModel::Gaussian::~Gaussian
~Gaussian() override
Definition: NoiseModel.h:198
gtsam::noiseModel::Constrained::shared_ptr
std::shared_ptr< Constrained > shared_ptr
Definition: NoiseModel.h:419
gtsam::noiseModel::Robust::squaredMahalanobisDistance
double squaredMahalanobisDistance(const Vector &v) const override
Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
Definition: NoiseModel.h:724
gtsam::noiseModel::Unit::~Unit
~Unit() override
Definition: NoiseModel.h:626
gtsam::noiseModel::Gaussian::thisR
const Matrix & thisR() const
Definition: NoiseModel.h:180
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::noiseModel::Unit::whitenInPlace
void whitenInPlace(Vector &) const override
Definition: NoiseModel.h:645
gtsam::noiseModel::Unit::WhitenInPlace
void WhitenInPlace(Eigen::Block< Matrix >) const override
Definition: NoiseModel.h:644
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim, double mu)
Definition: NoiseModel.h:490
gtsam::noiseModel::Unit::Whiten
Matrix Whiten(const Matrix &H) const override
Definition: NoiseModel.h:642
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:50