23#include <gtsam/dllexport.h>
24#include <gtsam/linear/LossFunctions.h>
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>
35 namespace noiseModel {
56 typedef boost::shared_ptr<Base> shared_ptr;
65 Base(
size_t dim = 1):dim_(dim) {}
72 virtual bool isUnit()
const {
return false; }
75 inline size_t dim()
const {
return dim_;}
77 virtual void print(
const std::string& name =
"")
const = 0;
79 virtual bool equals(
const Base& expected,
double tol=1e-9)
const = 0;
82 virtual Vector sigmas()
const;
85 virtual Vector
whiten(
const Vector& v)
const = 0;
88 virtual Matrix
Whiten(
const Matrix& H)
const = 0;
91 virtual Vector
unwhiten(
const Vector& v)
const = 0;
94 virtual double squaredMahalanobisDistance(
const Vector& v)
const;
98 return std::sqrt(squaredMahalanobisDistance(v));
102 virtual double loss(
const double squared_distance)
const {
103 return 0.5 * squared_distance;
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;
137 virtual double weight(
const Vector& v)
const {
return 1.0; }
141 friend class boost::serialization::access;
142 template<
class ARCHIVE>
143 void serialize(ARCHIVE & ar,
const unsigned int ) {
144 ar & BOOST_SERIALIZATION_NVP(dim_);
174 const Matrix& thisR()
const {
176 if (!sqrt_information_)
throw std::runtime_error(
"Gaussian: has no R matrix");
177 return *sqrt_information_;
183 typedef boost::shared_ptr<Gaussian> shared_ptr;
187 const boost::optional<Matrix>& sqrt_information = boost::none)
188 :
Base(dim), sqrt_information_(sqrt_information) {}
197 static shared_ptr SqrtInformation(
const Matrix& R,
bool smart =
true);
204 static shared_ptr Information(
const Matrix& M,
bool smart =
true);
211 static shared_ptr Covariance(
const Matrix& covariance,
bool smart =
true);
213 void print(
const std::string& name)
const override;
214 bool equals(
const Base& expected,
double tol=1e-9)
const override;
215 Vector sigmas()
const override;
216 Vector whiten(
const Vector& v)
const override;
217 Vector unwhiten(
const Vector& v)
const override;
223 Matrix Whiten(
const Matrix& H)
const override;
228 virtual void WhitenInPlace(Matrix& H)
const;
233 virtual void WhitenInPlace(Eigen::Block<Matrix> H)
const;
238 void WhitenSystem(std::vector<Matrix>& A, Vector& b)
const override;
239 void WhitenSystem(Matrix& A, Vector& b)
const override;
240 void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b)
const override;
241 void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b)
const override;
252 virtual boost::shared_ptr<Diagonal> QR(Matrix& Ab)
const;
255 virtual Matrix
R()
const {
return thisR();}
258 virtual Matrix information()
const;
261 virtual Matrix covariance()
const;
265 friend class boost::serialization::access;
266 template<
class ARCHIVE>
267 void serialize(ARCHIVE & ar,
const unsigned int ) {
268 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
269 ar & BOOST_SERIALIZATION_NVP(sqrt_information_);
300 typedef boost::shared_ptr<Diagonal> shared_ptr;
308 static shared_ptr Sigmas(
const Vector& sigmas,
bool smart =
true);
316 static shared_ptr Variances(
const Vector& variances,
bool smart =
true);
322 static shared_ptr Precisions(
const Vector& precisions,
bool smart =
true);
324 void print(
const std::string& name)
const override;
325 Vector
sigmas()
const override {
return sigmas_; }
326 Vector whiten(
const Vector& v)
const override;
327 Vector unwhiten(
const Vector& v)
const override;
328 Matrix Whiten(
const Matrix& H)
const override;
329 void WhitenInPlace(Matrix& H)
const override;
330 void WhitenInPlace(Eigen::Block<Matrix> H)
const override;
335 inline double sigma(
size_t i)
const {
return sigmas_(i); }
340 inline const Vector&
invsigmas()
const {
return invsigmas_; }
341 inline double invsigma(
size_t i)
const {
return invsigmas_(i);}
346 inline const Vector&
precisions()
const {
return precisions_; }
347 inline double precision(
size_t i)
const {
return precisions_(i);}
352 Matrix
R()
const override {
353 return invsigmas().asDiagonal();
358 friend class boost::serialization::access;
359 template<
class ARCHIVE>
360 void serialize(ARCHIVE & ar,
const unsigned int ) {
361 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Gaussian);
362 ar & BOOST_SERIALIZATION_NVP(sigmas_);
363 ar & BOOST_SERIALIZATION_NVP(invsigmas_);
392 Constrained(
const Vector& mu,
const Vector& sigmas);
396 typedef boost::shared_ptr<Constrained> shared_ptr;
412 bool constrained(
size_t i)
const;
415 const Vector&
mu()
const {
return mu_; }
421 static shared_ptr MixedSigmas(
const Vector& mu,
const Vector& sigmas);
427 static shared_ptr MixedSigmas(
const Vector& sigmas);
433 static shared_ptr MixedSigmas(
double m,
const Vector& sigmas);
439 static shared_ptr MixedVariances(
const Vector& mu,
const Vector& variances);
440 static shared_ptr MixedVariances(
const Vector& variances);
446 static shared_ptr MixedPrecisions(
const Vector& mu,
const Vector& precisions);
447 static shared_ptr MixedPrecisions(
const Vector& precisions);
454 double squaredMahalanobisDistance(
const Vector& v)
const override;
457 static shared_ptr
All(
size_t dim) {
458 return shared_ptr(
new Constrained(Vector::Constant(dim, 1000.0), Vector::Constant(dim,0)));
462 static shared_ptr
All(
size_t dim,
const Vector& mu) {
463 return shared_ptr(
new Constrained(mu, Vector::Constant(dim,0)));
467 static shared_ptr
All(
size_t dim,
double mu) {
468 return shared_ptr(
new Constrained(Vector::Constant(dim, mu), Vector::Constant(dim,0)));
471 void print(
const std::string& name)
const override;
474 Vector whiten(
const Vector& v)
const override;
478 Matrix Whiten(
const Matrix& H)
const override;
479 void WhitenInPlace(Matrix& H)
const override;
480 void WhitenInPlace(Eigen::Block<Matrix> H)
const override;
491 Diagonal::shared_ptr QR(Matrix& Ab)
const override;
497 shared_ptr unit()
const;
501 friend class boost::serialization::access;
502 template<
class ARCHIVE>
503 void serialize(ARCHIVE & ar,
const unsigned int ) {
504 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Diagonal);
505 ar & BOOST_SERIALIZATION_NVP(mu_);
518 double sigma_, invsigma_;
522 Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
529 ~Isotropic()
override {}
531 typedef boost::shared_ptr<Isotropic> shared_ptr;
536 static shared_ptr Sigma(
size_t dim,
double sigma,
bool smart =
true);
544 static shared_ptr Variance(
size_t dim,
double variance,
bool smart =
true);
549 static shared_ptr
Precision(
size_t dim,
double precision,
bool smart =
true) {
550 return Variance(dim, 1.0/precision, smart);
553 void print(
const std::string& name)
const override;
554 double squaredMahalanobisDistance(
const Vector& v)
const override;
555 Vector whiten(
const Vector& v)
const override;
556 Vector unwhiten(
const Vector& v)
const override;
557 Matrix Whiten(
const Matrix& H)
const override;
558 void WhitenInPlace(Matrix& H)
const override;
559 void whitenInPlace(Vector& v)
const override;
560 void WhitenInPlace(Eigen::Block<Matrix> H)
const override;
565 inline double sigma()
const {
return sigma_; }
569 friend class boost::serialization::access;
570 template<
class ARCHIVE>
571 void serialize(ARCHIVE & ar,
const unsigned int ) {
572 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Diagonal);
573 ar & BOOST_SERIALIZATION_NVP(sigma_);
574 ar & BOOST_SERIALIZATION_NVP(invsigma_);
587 typedef boost::shared_ptr<Unit> shared_ptr;
598 return shared_ptr(
new Unit(dim));
602 bool isUnit()
const override {
return true; }
604 void print(
const std::string& name)
const override;
605 double squaredMahalanobisDistance(
const Vector& v)
const override;
606 Vector
whiten(
const Vector& v)
const override {
return v; }
607 Vector
unwhiten(
const Vector& v)
const override {
return v; }
608 Matrix
Whiten(
const Matrix& H)
const override {
return H; }
618 friend class boost::serialization::access;
619 template<
class ARCHIVE>
620 void serialize(ARCHIVE & ar,
const unsigned int ) {
621 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Isotropic);
644 typedef boost::shared_ptr<Robust> shared_ptr;
659 Robust(
const RobustModel::shared_ptr robust,
const NoiseModel::shared_ptr noise)
660 :
Base(noise->dim()), robust_(robust), noise_(noise) {}
665 void print(
const std::string& name)
const override;
666 bool equals(
const Base& expected,
double tol=1e-9)
const override;
669 const RobustModel::shared_ptr&
robust()
const {
return robust_; }
672 const NoiseModel::shared_ptr&
noise()
const {
return noise_; }
675 inline Vector
whiten(
const Vector& v)
const override
676 { Vector r = v; this->WhitenSystem(r);
return r; }
677 inline Matrix
Whiten(
const Matrix& A)
const override
678 { Vector b; Matrix B=A; this->WhitenSystem(B,b);
return B; }
679 inline Vector
unwhiten(
const Vector& )
const override
680 {
throw std::invalid_argument(
"unwhiten is not currently supported for robust noise models."); }
682 double loss(
const double squared_distance)
const override {
683 return robust_->loss(std::sqrt(squared_distance));
689 return noise_->squaredMahalanobisDistance(v);
693 virtual void WhitenSystem(Vector& b)
const;
694 void WhitenSystem(std::vector<Matrix>& A, Vector& b)
const override;
695 void WhitenSystem(Matrix& A, Vector& b)
const override;
696 void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b)
const override;
697 void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b)
const override;
699 Vector unweightedWhiten(
const Vector& v)
const override;
700 double weight(
const Vector& v)
const override;
702 static shared_ptr Create(
703 const RobustModel::shared_ptr &robust,
const NoiseModel::shared_ptr noise);
707 friend class boost::serialization::access;
708 template<
class ARCHIVE>
709 void serialize(ARCHIVE & ar,
const unsigned int ) {
710 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
711 ar & boost::serialization::make_nvp(
"robust_",
const_cast<RobustModel::shared_ptr&
>(robust_));
712 ar & boost::serialization::make_nvp(
"noise_",
const_cast<NoiseModel::shared_ptr&
>(noise_));
717 GTSAM_EXPORT boost::optional<Vector> checkIfDiagonal(
const Matrix& M);
725 typedef noiseModel::Gaussian::shared_ptr SharedGaussian;
726 typedef noiseModel::Diagonal::shared_ptr SharedDiagonal;
727 typedef noiseModel::Constrained::shared_ptr SharedConstrained;
728 typedef noiseModel::Isotropic::shared_ptr SharedIsotropic;
731 template<>
struct traits<noiseModel::Gaussian> :
public Testable<noiseModel::Gaussian> {};
732 template<>
struct traits<noiseModel::Diagonal> :
public Testable<noiseModel::Diagonal> {};
733 template<>
struct traits<noiseModel::Constrained> :
public Testable<noiseModel::Constrained> {};
734 template<>
struct traits<noiseModel::Isotropic> :
public Testable<noiseModel::Isotropic> {};
735 template<>
struct traits<noiseModel::Unit> :
public Testable<noiseModel::Unit> {};
Concept check for values that can be used in unit tests.
typedef and functions to augment Eigen's MatrixXd
Global functions in a separate testing namespace.
Definition chartTesting.h:28
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
Template to create a binary predicate.
Definition Testable.h:111
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
Pure virtual class for all robust error function classes.
Definition LossFunctions.h:63
noiseModel::Base is the abstract base class for all noise models.
Definition NoiseModel.h:53
virtual bool isConstrained() const
true if a constrained noise model, saves slow/clumsy dynamic casting
Definition NoiseModel.h:69
virtual void whitenInPlace(Vector &v) const
in-place whiten, override if can be done more efficiently
Definition NoiseModel.h:112
size_t dim() const
Dimensionality.
Definition NoiseModel.h:75
virtual void unwhitenInPlace(Eigen::Block< Vector > &v) const
in-place unwhiten, override if can be done more efficiently
Definition NoiseModel.h:127
virtual void whitenInPlace(Eigen::Block< Vector > &v) const
in-place whiten, override if can be done more efficiently
Definition NoiseModel.h:122
virtual Vector whiten(const Vector &v) const =0
Whiten an error vector.
virtual double mahalanobisDistance(const Vector &v) const
Mahalanobis distance.
Definition NoiseModel.h:97
virtual bool isUnit() const
true if a unit noise model, saves slow/clumsy dynamic casting
Definition NoiseModel.h:72
virtual double weight(const Vector &v) const
get the weight from the effective loss function on residual vector v
Definition NoiseModel.h:137
virtual Vector unweightedWhiten(const Vector &v) const
Useful function for robust noise models to get the unweighted but whitened error.
Definition NoiseModel.h:132
virtual Vector unwhiten(const Vector &v) const =0
Unwhiten an error vector.
virtual double loss(const double squared_distance) const
loss function, input is Mahalanobis distance
Definition NoiseModel.h:102
virtual void unwhitenInPlace(Vector &v) const
in-place unwhiten, override if can be done more efficiently
Definition NoiseModel.h:117
virtual Matrix Whiten(const Matrix &H) const =0
Whiten a matrix.
Base(size_t dim=1)
primary constructor
Definition NoiseModel.h:65
Gaussian implements the mathematical model |R*x|^2 = |y|^2 with R'*R=inv(Sigma) where y = whiten(x) =...
Definition NoiseModel.h:162
virtual Matrix R() const
Return R itself, but note that Whiten(H) is cheaper than R*H.
Definition NoiseModel.h:255
Gaussian(size_t dim=1, const boost::optional< Matrix > &sqrt_information=boost::none)
constructor takes square root information matrix
Definition NoiseModel.h:186
boost::optional< Matrix > sqrt_information_
Matrix square root of information matrix (R)
Definition NoiseModel.h:167
A diagonal noise model implements a diagonal covariance matrix, with the elements of the diagonal spe...
Definition NoiseModel.h:281
Matrix R() const override
Return R itself, but note that Whiten(H) is cheaper than R*H.
Definition NoiseModel.h:352
Vector sigmas_
Standard deviations (sigmas), their inverse and inverse square (weights/precisions) These are all com...
Definition NoiseModel.h:289
double sigma(size_t i) const
Return standard deviations (sqrt of diagonal)
Definition NoiseModel.h:335
const Vector & invsigmas() const
Return sqrt precisions.
Definition NoiseModel.h:340
Vector sigmas() const override
Calculate standard deviations.
Definition NoiseModel.h:325
const Vector & precisions() const
Return precisions.
Definition NoiseModel.h:346
A Constrained constrained model is a specialization of Diagonal which allows some or all of the sigma...
Definition NoiseModel.h:381
bool isConstrained() const override
true if a constrained noise mode, saves slow/clumsy dynamic casting
Definition NoiseModel.h:409
static shared_ptr All(size_t dim, const Vector &mu)
Fully constrained variations.
Definition NoiseModel.h:462
static shared_ptr All(size_t dim, double mu)
Fully constrained variations with a mu parameter.
Definition NoiseModel.h:467
static shared_ptr All(size_t dim)
Fully constrained variations.
Definition NoiseModel.h:457
const Vector & mu() const
Access mu as a vector.
Definition NoiseModel.h:415
Vector mu_
Penalty function weight - needs to be large enough to dominate soft constraints.
Definition NoiseModel.h:385
An isotropic noise model corresponds to a scaled diagonal covariance To construct,...
Definition NoiseModel.h:516
double sigma() const
Return standard deviation.
Definition NoiseModel.h:565
static shared_ptr Precision(size_t dim, double precision, bool smart=true)
An isotropic noise model created by specifying a precision.
Definition NoiseModel.h:549
Isotropic(size_t dim, double sigma)
protected constructor takes sigma
Definition NoiseModel.h:521
Unit: i.i.d.
Definition NoiseModel.h:584
void WhitenInPlace(Eigen::Block< Matrix >) const override
In-place version.
Definition NoiseModel.h:610
Vector unwhiten(const Vector &v) const override
Unwhiten an error vector.
Definition NoiseModel.h:607
void whitenInPlace(Eigen::Block< Vector > &) const override
in-place whiten, override if can be done more efficiently
Definition NoiseModel.h:613
bool isUnit() const override
true if a unit noise model, saves slow/clumsy dynamic casting
Definition NoiseModel.h:602
Unit(size_t dim=1)
constructor for serialization
Definition NoiseModel.h:590
Vector whiten(const Vector &v) const override
Whiten an error vector.
Definition NoiseModel.h:606
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition NoiseModel.h:597
void unwhitenInPlace(Vector &) const override
in-place unwhiten, override if can be done more efficiently
Definition NoiseModel.h:612
void unwhitenInPlace(Eigen::Block< Vector > &) const override
in-place unwhiten, override if can be done more efficiently
Definition NoiseModel.h:614
void whitenInPlace(Vector &) const override
in-place whiten, override if can be done more efficiently
Definition NoiseModel.h:611
void WhitenInPlace(Matrix &) const override
In-place version.
Definition NoiseModel.h:609
Matrix Whiten(const Matrix &H) const override
Whiten a matrix.
Definition NoiseModel.h:608
Base class for robust error models The robust M-estimators above simply tell us how to re-weight the ...
Definition NoiseModel.h:642
const RobustModel::shared_ptr & robust() const
Return the contained robust error function.
Definition NoiseModel.h:669
Robust()
Default Constructor for serialization.
Definition NoiseModel.h:656
Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
Constructor.
Definition NoiseModel.h:659
double squaredMahalanobisDistance(const Vector &v) const override
Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
Definition NoiseModel.h:688
const NoiseModel::shared_ptr noise_
noise model used
Definition NoiseModel.h:651
double loss(const double squared_distance) const override
Compute loss from the m-estimator using the Mahalanobis distance.
Definition NoiseModel.h:682
Vector unwhiten(const Vector &) const override
Unwhiten an error vector.
Definition NoiseModel.h:679
Matrix Whiten(const Matrix &A) const override
Whiten a matrix.
Definition NoiseModel.h:677
const RobustModel::shared_ptr robust_
robust error function used
Definition NoiseModel.h:650
~Robust() override
Destructor.
Definition NoiseModel.h:663
const NoiseModel::shared_ptr & noise() const
Return the contained noise model.
Definition NoiseModel.h:672
Vector whiten(const Vector &v) const override
Whiten an error vector.
Definition NoiseModel.h:675