gtsam  4.1.0
gtsam
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>
23 #include <gtsam/dllexport.h>
24 #include <gtsam/linear/LossFunctions.h>
25 
26 #include <boost/serialization/nvp.hpp>
27 #include <boost/serialization/extended_type_info.hpp>
28 #include <boost/serialization/singleton.hpp>
29 #include <boost/serialization/shared_ptr.hpp>
30 #include <boost/serialization/optional.hpp>
31 
32 namespace gtsam {
33 
35  namespace noiseModel {
36 
37  // Forward declaration
38  class Gaussian;
39  class Diagonal;
40  class Constrained;
41  class Isotropic;
42  class Unit;
43  class RobustModel;
44 
45  //---------------------------------------------------------------------------------------
46 
53  class GTSAM_EXPORT Base {
54 
55  public:
56  typedef boost::shared_ptr<Base> shared_ptr;
57 
58  protected:
59 
60  size_t dim_;
61 
62  public:
63 
65  Base(size_t dim = 1):dim_(dim) {}
66  virtual ~Base() {}
67 
69  virtual bool isConstrained() const { return false; } // default false
70 
72  virtual bool isUnit() const { return false; } // default false
73 
75  inline size_t dim() const { return dim_;}
76 
77  virtual void print(const std::string& name = "") const = 0;
78 
79  virtual bool equals(const Base& expected, double tol=1e-9) const = 0;
80 
82  virtual Vector sigmas() const;
83 
85  virtual Vector whiten(const Vector& v) const = 0;
86 
88  virtual Matrix Whiten(const Matrix& H) const = 0;
89 
91  virtual Vector unwhiten(const Vector& v) const = 0;
92 
94  virtual double squaredMahalanobisDistance(const Vector& v) const;
95 
97  virtual double mahalanobisDistance(const Vector& v) const {
98  return std::sqrt(squaredMahalanobisDistance(v));
99  }
100 
102  virtual double loss(const double squared_distance) const {
103  return 0.5 * squared_distance;
104  }
105 
106  virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const = 0;
107  virtual void WhitenSystem(Matrix& A, Vector& b) const = 0;
108  virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0;
109  virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const = 0;
110 
112  virtual void whitenInPlace(Vector& v) const {
113  v = whiten(v);
114  }
115 
117  virtual void unwhitenInPlace(Vector& v) const {
118  v = unwhiten(v);
119  }
120 
122  virtual void whitenInPlace(Eigen::Block<Vector>& v) const {
123  v = whiten(v);
124  }
125 
127  virtual void unwhitenInPlace(Eigen::Block<Vector>& v) const {
128  v = unwhiten(v);
129  }
130 
132  virtual Vector unweightedWhiten(const Vector& v) const {
133  return whiten(v);
134  }
135 
137  virtual double weight(const Vector& v) const { return 1.0; }
138 
139  private:
141  friend class boost::serialization::access;
142  template<class ARCHIVE>
143  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
144  ar & BOOST_SERIALIZATION_NVP(dim_);
145  }
146  };
147 
148  //---------------------------------------------------------------------------------------
149 
162  class GTSAM_EXPORT Gaussian: public Base {
163 
164  protected:
165 
167  boost::optional<Matrix> sqrt_information_;
168 
169  private:
170 
174  const Matrix& thisR() const {
175  // should never happen
176  if (!sqrt_information_) throw std::runtime_error("Gaussian: has no R matrix");
177  return *sqrt_information_;
178  }
179 
180  protected:
181 
183  Gaussian(size_t dim = 1, const boost::optional<Matrix>& sqrt_information = boost::none) :
184  Base(dim), sqrt_information_(sqrt_information) {
185  }
186 
187  public:
188 
189  typedef boost::shared_ptr<Gaussian> shared_ptr;
190 
191  virtual ~Gaussian() {}
192 
198  static shared_ptr SqrtInformation(const Matrix& R, bool smart = true);
199 
205  static shared_ptr Information(const Matrix& M, bool smart = true);
206 
212  static shared_ptr Covariance(const Matrix& covariance, bool smart = true);
213 
214  void print(const std::string& name) const override;
215  bool equals(const Base& expected, double tol=1e-9) const override;
216  Vector sigmas() const override;
217  Vector whiten(const Vector& v) const override;
218  Vector unwhiten(const Vector& v) const override;
219 
224  Matrix Whiten(const Matrix& H) const override;
225 
229  virtual void WhitenInPlace(Matrix& H) const;
230 
234  virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;
235 
239  void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
240  void WhitenSystem(Matrix& A, Vector& b) const override;
241  void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
242  void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
243 
253  virtual boost::shared_ptr<Diagonal> QR(Matrix& Ab) const;
254 
256  virtual Matrix R() const { return thisR();}
257 
259  virtual Matrix information() const { return R().transpose() * R(); }
260 
262  virtual Matrix covariance() const;
263 
264  private:
266  friend class boost::serialization::access;
267  template<class ARCHIVE>
268  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
269  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
270  ar & BOOST_SERIALIZATION_NVP(sqrt_information_);
271  }
272 
273  }; // Gaussian
274 
275  //---------------------------------------------------------------------------------------
276 
282  class GTSAM_EXPORT Diagonal : public Gaussian {
283  protected:
284 
290  Vector sigmas_, invsigmas_, precisions_;
291 
292  protected:
294  Diagonal();
295 
297  Diagonal(const Vector& sigmas);
298 
299  public:
300 
301  typedef boost::shared_ptr<Diagonal> shared_ptr;
302 
303  virtual ~Diagonal() {}
304 
309  static shared_ptr Sigmas(const Vector& sigmas, bool smart = true);
310 
317  static shared_ptr Variances(const Vector& variances, bool smart = true);
318 
323  static shared_ptr Precisions(const Vector& precisions, bool smart = true) {
324  return Variances(precisions.array().inverse(), smart);
325  }
326 
327  void print(const std::string& name) const override;
328  Vector sigmas() const override { return sigmas_; }
329  Vector whiten(const Vector& v) const override;
330  Vector unwhiten(const Vector& v) const override;
331  Matrix Whiten(const Matrix& H) const override;
332  void WhitenInPlace(Matrix& H) const override;
333  void WhitenInPlace(Eigen::Block<Matrix> H) const override;
334 
338  inline double sigma(size_t i) const { return sigmas_(i); }
339 
343  inline const Vector& invsigmas() const { return invsigmas_; }
344  inline double invsigma(size_t i) const {return invsigmas_(i);}
345 
349  inline const Vector& precisions() const { return precisions_; }
350  inline double precision(size_t i) const {return precisions_(i);}
351 
355  Matrix R() const override {
356  return invsigmas().asDiagonal();
357  }
358 
359  private:
361  friend class boost::serialization::access;
362  template<class ARCHIVE>
363  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
364  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Gaussian);
365  ar & BOOST_SERIALIZATION_NVP(sigmas_);
366  ar & BOOST_SERIALIZATION_NVP(invsigmas_);
367  }
368  }; // Diagonal
369 
370  //---------------------------------------------------------------------------------------
371 
384  class GTSAM_EXPORT Constrained : public Diagonal {
385  protected:
386 
387  // Sigmas are contained in the base class
388  Vector mu_;
389 
396  Constrained(const Vector& sigmas = Z_1x1);
397 
403  Constrained(const Vector& mu, const Vector& sigmas);
404 
405  public:
406 
407  typedef boost::shared_ptr<Constrained> shared_ptr;
408 
409  ~Constrained() {}
410 
412  bool isConstrained() const override { return true; }
413 
415  bool constrained(size_t i) const;
416 
418  const Vector& mu() const { return mu_; }
419 
424  static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas);
425 
430  static shared_ptr MixedSigmas(const Vector& sigmas) {
431  return MixedSigmas(Vector::Constant(sigmas.size(), 1000.0), sigmas);
432  }
433 
438  static shared_ptr MixedSigmas(double m, const Vector& sigmas) {
439  return MixedSigmas(Vector::Constant(sigmas.size(), m), sigmas);
440  }
441 
446  static shared_ptr MixedVariances(const Vector& mu, const Vector& variances) {
447  return shared_ptr(new Constrained(mu, variances.cwiseSqrt()));
448  }
449  static shared_ptr MixedVariances(const Vector& variances) {
450  return shared_ptr(new Constrained(variances.cwiseSqrt()));
451  }
452 
457  static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions) {
458  return MixedVariances(mu, precisions.array().inverse());
459  }
460  static shared_ptr MixedPrecisions(const Vector& precisions) {
461  return MixedVariances(precisions.array().inverse());
462  }
463 
464  double squaredMahalanobisDistance(const Vector& v) const override;
465 
467  static shared_ptr All(size_t dim) {
468  return shared_ptr(new Constrained(Vector::Constant(dim, 1000.0), Vector::Constant(dim,0)));
469  }
470 
472  static shared_ptr All(size_t dim, const Vector& mu) {
473  return shared_ptr(new Constrained(mu, Vector::Constant(dim,0)));
474  }
475 
477  static shared_ptr All(size_t dim, double mu) {
478  return shared_ptr(new Constrained(Vector::Constant(dim, mu), Vector::Constant(dim,0)));
479  }
480 
481  void print(const std::string& name) const override;
482 
484  Vector whiten(const Vector& v) const override;
485 
488  Matrix Whiten(const Matrix& H) const override;
489  void WhitenInPlace(Matrix& H) const override;
490  void WhitenInPlace(Eigen::Block<Matrix> H) const override;
491 
501  Diagonal::shared_ptr QR(Matrix& Ab) const override;
502 
507  shared_ptr unit() const;
508 
509  private:
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 
518  }; // Constrained
519 
520  //---------------------------------------------------------------------------------------
521 
526  class GTSAM_EXPORT Isotropic : public Diagonal {
527  protected:
528  double sigma_, invsigma_;
529 
531  Isotropic(size_t dim, double sigma) :
532  Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
533 
534  /* dummy constructor to allow for serialization */
535  Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {}
536 
537  public:
538 
539  virtual ~Isotropic() {}
540 
541  typedef boost::shared_ptr<Isotropic> shared_ptr;
542 
546  static shared_ptr Sigma(size_t dim, double sigma, bool smart = true);
547 
554  static shared_ptr Variance(size_t dim, double variance, bool smart = true);
555 
559  static shared_ptr Precision(size_t dim, double precision, bool smart = true) {
560  return Variance(dim, 1.0/precision, smart);
561  }
562 
563  void print(const std::string& name) const override;
564  double squaredMahalanobisDistance(const Vector& v) const override;
565  Vector whiten(const Vector& v) const override;
566  Vector unwhiten(const Vector& v) const override;
567  Matrix Whiten(const Matrix& H) const override;
568  void WhitenInPlace(Matrix& H) const override;
569  void whitenInPlace(Vector& v) const override;
570  void WhitenInPlace(Eigen::Block<Matrix> H) const override;
571 
575  inline double sigma() const { return sigma_; }
576 
577  private:
579  friend class boost::serialization::access;
580  template<class ARCHIVE>
581  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
582  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal);
583  ar & BOOST_SERIALIZATION_NVP(sigma_);
584  ar & BOOST_SERIALIZATION_NVP(invsigma_);
585  }
586 
587  };
588 
589  //---------------------------------------------------------------------------------------
590 
594  class GTSAM_EXPORT Unit : public Isotropic {
595  protected:
596 
597  Unit(size_t dim=1): Isotropic(dim,1.0) {}
598 
599  public:
600 
601  typedef boost::shared_ptr<Unit> shared_ptr;
602 
603  ~Unit() {}
604 
608  static shared_ptr Create(size_t dim) {
609  return shared_ptr(new Unit(dim));
610  }
611 
613  bool isUnit() const override { return true; }
614 
615  void print(const std::string& name) const override;
616  double squaredMahalanobisDistance(const Vector& v) const override {return v.dot(v); }
617  Vector whiten(const Vector& v) const override { return v; }
618  Vector unwhiten(const Vector& v) const override { return v; }
619  Matrix Whiten(const Matrix& H) const override { return H; }
620  void WhitenInPlace(Matrix& /*H*/) const override {}
621  void WhitenInPlace(Eigen::Block<Matrix> /*H*/) const override {}
622  void whitenInPlace(Vector& /*v*/) const override {}
623  void unwhitenInPlace(Vector& /*v*/) const override {}
624  void whitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
625  void unwhitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
626 
627  private:
629  friend class boost::serialization::access;
630  template<class ARCHIVE>
631  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
632  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Isotropic);
633  }
634  };
635 
653  class GTSAM_EXPORT Robust : public Base {
654  public:
655  typedef boost::shared_ptr<Robust> shared_ptr;
656 
657  protected:
660 
661  const RobustModel::shared_ptr robust_;
662  const NoiseModel::shared_ptr noise_;
663 
664  public:
665 
667  Robust() {};
668 
670  Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
671  : Base(noise->dim()), robust_(robust), noise_(noise) {}
672 
674  ~Robust() {}
675 
676  void print(const std::string& name) const override;
677  bool equals(const Base& expected, double tol=1e-9) const override;
678 
680  const RobustModel::shared_ptr& robust() const { return robust_; }
681 
683  const NoiseModel::shared_ptr& noise() const { return noise_; }
684 
685  // TODO: functions below are dummy but necessary for the noiseModel::Base
686  inline Vector whiten(const Vector& v) const override
687  { Vector r = v; this->WhitenSystem(r); return r; }
688  inline Matrix Whiten(const Matrix& A) const override
689  { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
690  inline Vector unwhiten(const Vector& /*v*/) const override
691  { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
692 
693  double loss(const double squared_distance) const override {
694  return robust_->loss(std::sqrt(squared_distance));
695  }
696 
697  // TODO: these are really robust iterated re-weighting support functions
698  virtual void WhitenSystem(Vector& b) const;
699  void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
700  void WhitenSystem(Matrix& A, Vector& b) const override;
701  void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
702  void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
703 
704  Vector unweightedWhiten(const Vector& v) const override {
705  return noise_->unweightedWhiten(v);
706  }
707  double weight(const Vector& v) const override {
708  // Todo(mikebosse): make the robust weight function input a vector.
709  return robust_->weight(v.norm());
710  }
711 
712  static shared_ptr Create(
713  const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);
714 
715  private:
717  friend class boost::serialization::access;
718  template<class ARCHIVE>
719  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
720  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
721  ar & boost::serialization::make_nvp("robust_", const_cast<RobustModel::shared_ptr&>(robust_));
722  ar & boost::serialization::make_nvp("noise_", const_cast<NoiseModel::shared_ptr&>(noise_));
723  }
724  };
725 
726  // Helper function
727  GTSAM_EXPORT boost::optional<Vector> checkIfDiagonal(const Matrix M);
728 
729  } // namespace noiseModel
730 
734  typedef noiseModel::Base::shared_ptr SharedNoiseModel;
735  typedef noiseModel::Gaussian::shared_ptr SharedGaussian;
736  typedef noiseModel::Diagonal::shared_ptr SharedDiagonal;
737  typedef noiseModel::Constrained::shared_ptr SharedConstrained;
738  typedef noiseModel::Isotropic::shared_ptr SharedIsotropic;
739 
741  template<> struct traits<noiseModel::Gaussian> : public Testable<noiseModel::Gaussian> {};
742  template<> struct traits<noiseModel::Diagonal> : public Testable<noiseModel::Diagonal> {};
743  template<> struct traits<noiseModel::Constrained> : public Testable<noiseModel::Constrained> {};
744  template<> struct traits<noiseModel::Isotropic> : public Testable<noiseModel::Isotropic> {};
745  template<> struct traits<noiseModel::Unit> : public Testable<noiseModel::Unit> {};
746 
747 } //\ namespace gtsam
748 
749 
gtsam::noiseModel::Diagonal::sigma
double sigma(size_t i) const
Return standard deviations (sqrt of diagonal)
Definition: NoiseModel.h:338
Testable.h
Concept check for values that can be used in unit tests.
gtsam::noiseModel::Unit::squaredMahalanobisDistance
double squaredMahalanobisDistance(const Vector &v) const override
Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
Definition: NoiseModel.h:616
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition: NoiseModel.h:608
gtsam::noiseModel::Constrained::mu_
Vector mu_
Penalty function weight - needs to be large enough to dominate soft constraints.
Definition: NoiseModel.h:388
gtsam::noiseModel::Unit::unwhitenInPlace
void unwhitenInPlace(Vector &) const override
in-place unwhiten, override if can be done more efficiently
Definition: NoiseModel.h:623
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::noiseModel::Diagonal::sigmas_
Vector sigmas_
Standard deviations (sigmas), their inverse and inverse square (weights/precisions) These are all com...
Definition: NoiseModel.h:290
gtsam::noiseModel::Isotropic::Precision
static shared_ptr Precision(size_t dim, double precision, bool smart=true)
An isotropic noise model created by specifying a precision.
Definition: NoiseModel.h:559
gtsam::noiseModel::Gaussian::Gaussian
Gaussian(size_t dim=1, const boost::optional< Matrix > &sqrt_information=boost::none)
protected constructor takes square root information matrix
Definition: NoiseModel.h:183
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim)
Fully constrained variations.
Definition: NoiseModel.h:467
gtsam::noiseModel::Unit::unwhiten
Vector unwhiten(const Vector &v) const override
Unwhiten an error vector.
Definition: NoiseModel.h:618
gtsam::noiseModel::Base::loss
virtual double loss(const double squared_distance) const
loss function, input is Mahalanobis distance
Definition: NoiseModel.h:102
gtsam::noiseModel::Unit::WhitenInPlace
void WhitenInPlace(Matrix &) const override
In-place version.
Definition: NoiseModel.h:620
gtsam::noiseModel::Robust::Whiten
Matrix Whiten(const Matrix &A) const override
Whiten a matrix.
Definition: NoiseModel.h:688
gtsam::noiseModel::Diagonal
A diagonal noise model implements a diagonal covariance matrix, with the elements of the diagonal spe...
Definition: NoiseModel.h:282
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::noiseModel::Base::unweightedWhiten
virtual Vector unweightedWhiten(const Vector &v) const
Useful function for robust noise models to get the unweighted but whitened error.
Definition: NoiseModel.h:132
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::noiseModel::Gaussian::R
virtual Matrix R() const
Return R itself, but note that Whiten(H) is cheaper than R*H.
Definition: NoiseModel.h:256
gtsam::noiseModel::Gaussian::information
virtual Matrix information() const
Compute information matrix.
Definition: NoiseModel.h:259
gtsam::noiseModel::mEstimator::Base
Definition: LossFunctions.h:58
gtsam::noiseModel::Constrained::MixedSigmas
static shared_ptr MixedSigmas(double m, const Vector &sigmas)
A diagonal noise model created by specifying a Vector of standard devations, some of which might be z...
Definition: NoiseModel.h:438
gtsam::noiseModel::Unit::Whiten
Matrix Whiten(const Matrix &H) const override
Multiply a derivative with R (derivative of whiten) Equivalent to whitening each column of the input ...
Definition: NoiseModel.h:619
gtsam::noiseModel::Robust::~Robust
~Robust()
Destructor.
Definition: NoiseModel.h:674
gtsam::noiseModel::Robust::whiten
Vector whiten(const Vector &v) const override
Whiten an error vector.
Definition: NoiseModel.h:686
gtsam::noiseModel::Base::weight
virtual double weight(const Vector &v) const
get the weight from the effective loss function on residual vector v
Definition: NoiseModel.h:137
gtsam::noiseModel::Isotropic::sigma
double sigma() const
Return standard deviation.
Definition: NoiseModel.h:575
gtsam::noiseModel::Base::whitenInPlace
virtual void whitenInPlace(Eigen::Block< Vector > &v) const
in-place whiten, override if can be done more efficiently
Definition: NoiseModel.h:122
gtsam::noiseModel::Isotropic
An isotropic noise model corresponds to a scaled diagonal covariance To construct,...
Definition: NoiseModel.h:526
gtsam::noiseModel::Base::whiten
virtual Vector whiten(const Vector &v) const =0
Whiten an error vector.
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::noiseModel::Robust::unwhiten
Vector unwhiten(const Vector &) const override
Unwhiten an error vector.
Definition: NoiseModel.h:690
gtsam::noiseModel::Unit::unwhitenInPlace
void unwhitenInPlace(Eigen::Block< Vector > &) const override
in-place unwhiten, override if can be done more efficiently
Definition: NoiseModel.h:625
gtsam::noiseModel::Diagonal::R
Matrix R() const override
Return R itself, but note that Whiten(H) is cheaper than R*H.
Definition: NoiseModel.h:355
gtsam::noiseModel::Base::isConstrained
virtual bool isConstrained() const
true if a constrained noise model, saves slow/clumsy dynamic casting
Definition: NoiseModel.h:69
gtsam::noiseModel::Robust::noise_
const NoiseModel::shared_ptr noise_
noise model used
Definition: NoiseModel.h:662
gtsam::noiseModel::Diagonal::sigmas
Vector sigmas() const override
Calculate standard deviations.
Definition: NoiseModel.h:328
gtsam::noiseModel::Robust::noise
const NoiseModel::shared_ptr & noise() const
Return the contained noise model.
Definition: NoiseModel.h:683
gtsam::noiseModel::Base::Base
Base(size_t dim=1)
primary constructor
Definition: NoiseModel.h:65
gtsam::noiseModel::Base::mahalanobisDistance
virtual double mahalanobisDistance(const Vector &v) const
Mahalanobis distance.
Definition: NoiseModel.h:97
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::noiseModel::Unit::whitenInPlace
void whitenInPlace(Eigen::Block< Vector > &) const override
in-place whiten, override if can be done more efficiently
Definition: NoiseModel.h:624
gtsam::noiseModel::Constrained::mu
const Vector & mu() const
Access mu as a vector.
Definition: NoiseModel.h:418
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::noiseModel::Constrained::MixedSigmas
static shared_ptr MixedSigmas(const Vector &sigmas)
A diagonal noise model created by specifying a Vector of standard devations, some of which might be z...
Definition: NoiseModel.h:430
gtsam::noiseModel::Base::Whiten
virtual Matrix Whiten(const Matrix &H) const =0
Whiten a matrix.
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim, const Vector &mu)
Fully constrained variations.
Definition: NoiseModel.h:472
gtsam::noiseModel::Base::isUnit
virtual bool isUnit() const
true if a unit noise model, saves slow/clumsy dynamic casting
Definition: NoiseModel.h:72
gtsam::noiseModel::Robust::Robust
Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
Constructor.
Definition: NoiseModel.h:670
gtsam::noiseModel::Robust
Base class for robust error models The robust M-estimators above simply tell us how to re-weight the ...
Definition: NoiseModel.h:653
gtsam::noiseModel::Robust::loss
double loss(const double squared_distance) const override
loss function, input is Mahalanobis distance
Definition: NoiseModel.h:693
gtsam::noiseModel::Constrained
A Constrained constrained model is a specialization of Diagonal which allows some or all of the sigma...
Definition: NoiseModel.h:384
gtsam::noiseModel::Unit::WhitenInPlace
void WhitenInPlace(Eigen::Block< Matrix >) const override
In-place version.
Definition: NoiseModel.h:621
gtsam::noiseModel::Base::unwhitenInPlace
virtual void unwhitenInPlace(Vector &v) const
in-place unwhiten, override if can be done more efficiently
Definition: NoiseModel.h:117
gtsam::noiseModel::Gaussian
Gaussian implements the mathematical model |R*x|^2 = |y|^2 with R'*R=inv(Sigma) where y = whiten(x) =...
Definition: NoiseModel.h:162
gtsam::noiseModel::Isotropic::Isotropic
Isotropic(size_t dim, double sigma)
protected constructor takes sigma
Definition: NoiseModel.h:531
gtsam::noiseModel::Base::whitenInPlace
virtual void whitenInPlace(Vector &v) const
in-place whiten, override if can be done more efficiently
Definition: NoiseModel.h:112
gtsam::noiseModel::Unit
Unit: i.i.d.
Definition: NoiseModel.h:594
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::noiseModel::Constrained::MixedPrecisions
static shared_ptr MixedPrecisions(const Vector &mu, const Vector &precisions)
A diagonal noise model created by specifying a Vector of precisions, some of which might be inf.
Definition: NoiseModel.h:457
gtsam::noiseModel::Base::unwhitenInPlace
virtual void unwhitenInPlace(Eigen::Block< Vector > &v) const
in-place unwhiten, override if can be done more efficiently
Definition: NoiseModel.h:127
gtsam::noiseModel::Base
noiseModel::Base is the abstract base class for all noise models.
Definition: NoiseModel.h:53
gtsam::noiseModel::Unit::whiten
Vector whiten(const Vector &v) const override
Whiten an error vector.
Definition: NoiseModel.h:617
gtsam::noiseModel::Constrained::isConstrained
bool isConstrained() const override
true if a constrained noise mode, saves slow/clumsy dynamic casting
Definition: NoiseModel.h:412
gtsam::noiseModel::Unit::whitenInPlace
void whitenInPlace(Vector &) const override
in-place whiten, override if can be done more efficiently
Definition: NoiseModel.h:622
gtsam::noiseModel::Robust::weight
double weight(const Vector &v) const override
get the weight from the effective loss function on residual vector v
Definition: NoiseModel.h:707
gtsam::noiseModel::Robust::robust
const RobustModel::shared_ptr & robust() const
Return the contained robust error function.
Definition: NoiseModel.h:680
gtsam::noiseModel::Diagonal::precisions
const Vector & precisions() const
Return precisions.
Definition: NoiseModel.h:349
gtsam::noiseModel::Gaussian::sqrt_information_
boost::optional< Matrix > sqrt_information_
Matrix square root of information matrix (R)
Definition: NoiseModel.h:167
gtsam::noiseModel::Constrained::MixedVariances
static shared_ptr MixedVariances(const Vector &mu, const Vector &variances)
A diagonal noise model created by specifying a Vector of standard devations, some of which might be z...
Definition: NoiseModel.h:446
gtsam::noiseModel::Robust::Robust
Robust()
Default Constructor for serialization.
Definition: NoiseModel.h:667
gtsam::noiseModel::Diagonal::invsigmas
const Vector & invsigmas() const
Return sqrt precisions.
Definition: NoiseModel.h:343
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim, double mu)
Fully constrained variations with a mu parameter.
Definition: NoiseModel.h:477
gtsam::noiseModel::Robust::unweightedWhiten
Vector unweightedWhiten(const Vector &v) const override
Useful function for robust noise models to get the unweighted but whitened error.
Definition: NoiseModel.h:704
gtsam::noiseModel::Base::unwhiten
virtual Vector unwhiten(const Vector &v) const =0
Unwhiten an error vector.
gtsam::noiseModel::Diagonal::Precisions
static shared_ptr Precisions(const Vector &precisions, bool smart=true)
A diagonal noise model created by specifying a Vector of precisions, i.e.
Definition: NoiseModel.h:323
gtsam::noiseModel::Base::dim
size_t dim() const
Dimensionality.
Definition: NoiseModel.h:75
gtsam::noiseModel::Robust::robust_
const RobustModel::shared_ptr robust_
robust error function used
Definition: NoiseModel.h:661
gtsam::noiseModel::Unit::isUnit
bool isUnit() const override
true if a unit noise model, saves slow/clumsy dynamic casting
Definition: NoiseModel.h:613