Loading...
Searching...
No Matches
NoiseModel.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/base/Testable.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/dllexport.h>
#include <gtsam/linear/LossFunctions.h>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/extended_type_info.hpp>
#include <boost/serialization/singleton.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/optional.hpp>
namespace gtsam {
namespace noiseModel {
// Forward declaration
class Gaussian;
class Diagonal;
class Constrained;
class Isotropic;
class Unit;
class RobustModel;
//---------------------------------------------------------------------------------------
class GTSAM_EXPORT Base {
public:
typedef boost::shared_ptr<Base> shared_ptr;
protected:
size_t dim_;
public:
Base(size_t dim = 1):dim_(dim) {}
virtual ~Base() {}
virtual bool isConstrained() const { return false; } // default false
virtual bool isUnit() const { return false; } // default false
inline size_t dim() const { return dim_;}
virtual void print(const std::string& name = "") const = 0;
virtual bool equals(const Base& expected, double tol=1e-9) const = 0;
virtual Vector sigmas() const;
virtual Vector whiten(const Vector& v) const = 0;
virtual Matrix Whiten(const Matrix& H) const = 0;
virtual Vector unwhiten(const Vector& v) const = 0;
virtual double squaredMahalanobisDistance(const Vector& v) const;
virtual double mahalanobisDistance(const Vector& v) const {
return std::sqrt(squaredMahalanobisDistance(v));
}
virtual double loss(const double squared_distance) const {
return 0.5 * squared_distance;
}
virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const = 0;
virtual void WhitenSystem(Matrix& A, Vector& b) const = 0;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const = 0;
virtual void whitenInPlace(Vector& v) const {
v = whiten(v);
}
virtual void unwhitenInPlace(Vector& v) const {
v = unwhiten(v);
}
virtual void whitenInPlace(Eigen::Block<Vector>& v) const {
v = whiten(v);
}
virtual void unwhitenInPlace(Eigen::Block<Vector>& v) const {
v = unwhiten(v);
}
virtual Vector unweightedWhiten(const Vector& v) const {
return whiten(v);
}
virtual double weight(const Vector& v) const { return 1.0; }
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(dim_);
}
};
//---------------------------------------------------------------------------------------
class GTSAM_EXPORT Gaussian: public Base {
protected:
boost::optional<Matrix> sqrt_information_;
private:
const Matrix& thisR() const {
// should never happen
if (!sqrt_information_) throw std::runtime_error("Gaussian: has no R matrix");
return *sqrt_information_;
}
public:
typedef boost::shared_ptr<Gaussian> shared_ptr;
Gaussian(size_t dim = 1,
const boost::optional<Matrix>& sqrt_information = boost::none)
: Base(dim), sqrt_information_(sqrt_information) {}
~Gaussian() override {}
static shared_ptr SqrtInformation(const Matrix& R, bool smart = true);
static shared_ptr Information(const Matrix& M, bool smart = true);
static shared_ptr Covariance(const Matrix& covariance, bool smart = true);
void print(const std::string& name) const override;
bool equals(const Base& expected, double tol=1e-9) const override;
Vector sigmas() const override;
Vector whiten(const Vector& v) const override;
Vector unwhiten(const Vector& v) const override;
Matrix Whiten(const Matrix& H) const override;
virtual void WhitenInPlace(Matrix& H) const;
virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;
void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
void WhitenSystem(Matrix& A, Vector& b) const override;
void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
virtual boost::shared_ptr<Diagonal> QR(Matrix& Ab) const;
virtual Matrix R() const { return thisR();}
virtual Matrix information() const;
virtual Matrix covariance() const;
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(sqrt_information_);
}
}; // Gaussian
//---------------------------------------------------------------------------------------
class GTSAM_EXPORT Diagonal : public Gaussian {
protected:
Vector sigmas_, invsigmas_, precisions_;
protected:
Diagonal(const Vector& sigmas);
public:
Diagonal();
typedef boost::shared_ptr<Diagonal> shared_ptr;
~Diagonal() override {}
static shared_ptr Sigmas(const Vector& sigmas, bool smart = true);
static shared_ptr Variances(const Vector& variances, bool smart = true);
static shared_ptr Precisions(const Vector& precisions, bool smart = true);
void print(const std::string& name) const override;
Vector sigmas() const override { return sigmas_; }
Vector whiten(const Vector& v) const override;
Vector unwhiten(const Vector& v) const override;
Matrix Whiten(const Matrix& H) const override;
void WhitenInPlace(Matrix& H) const override;
void WhitenInPlace(Eigen::Block<Matrix> H) const override;
inline double sigma(size_t i) const { return sigmas_(i); }
inline const Vector& invsigmas() const { return invsigmas_; }
inline double invsigma(size_t i) const {return invsigmas_(i);}
inline const Vector& precisions() const { return precisions_; }
inline double precision(size_t i) const {return precisions_(i);}
Matrix R() const override {
return invsigmas().asDiagonal();
}
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Gaussian);
ar & BOOST_SERIALIZATION_NVP(sigmas_);
ar & BOOST_SERIALIZATION_NVP(invsigmas_);
}
}; // Diagonal
//---------------------------------------------------------------------------------------
class GTSAM_EXPORT Constrained : public Diagonal {
protected:
// Sigmas are contained in the base class
Vector mu_;
Constrained(const Vector& mu, const Vector& sigmas);
public:
typedef boost::shared_ptr<Constrained> shared_ptr;
Constrained(const Vector& sigmas = Z_1x1);
~Constrained() override {}
bool isConstrained() const override { return true; }
bool constrained(size_t i) const;
const Vector& mu() const { return mu_; }
static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas);
static shared_ptr MixedSigmas(const Vector& sigmas);
static shared_ptr MixedSigmas(double m, const Vector& sigmas);
static shared_ptr MixedVariances(const Vector& mu, const Vector& variances);
static shared_ptr MixedVariances(const Vector& variances);
static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions);
static shared_ptr MixedPrecisions(const Vector& precisions);
double squaredMahalanobisDistance(const Vector& v) const override;
static shared_ptr All(size_t dim) {
return shared_ptr(new Constrained(Vector::Constant(dim, 1000.0), Vector::Constant(dim,0)));
}
static shared_ptr All(size_t dim, const Vector& mu) {
return shared_ptr(new Constrained(mu, Vector::Constant(dim,0)));
}
static shared_ptr All(size_t dim, double mu) {
return shared_ptr(new Constrained(Vector::Constant(dim, mu), Vector::Constant(dim,0)));
}
void print(const std::string& name) const override;
Vector whiten(const Vector& v) const override;
Matrix Whiten(const Matrix& H) const override;
void WhitenInPlace(Matrix& H) const override;
void WhitenInPlace(Eigen::Block<Matrix> H) const override;
Diagonal::shared_ptr QR(Matrix& Ab) const override;
shared_ptr unit() const;
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal);
ar & BOOST_SERIALIZATION_NVP(mu_);
}
}; // Constrained
//---------------------------------------------------------------------------------------
class GTSAM_EXPORT Isotropic : public Diagonal {
protected:
double sigma_, invsigma_;
Isotropic(size_t dim, double sigma) :
Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
public:
/* dummy constructor to allow for serialization */
Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {}
~Isotropic() override {}
typedef boost::shared_ptr<Isotropic> shared_ptr;
static shared_ptr Sigma(size_t dim, double sigma, bool smart = true);
static shared_ptr Variance(size_t dim, double variance, bool smart = true);
static shared_ptr Precision(size_t dim, double precision, bool smart = true) {
return Variance(dim, 1.0/precision, smart);
}
void print(const std::string& name) const override;
double squaredMahalanobisDistance(const Vector& v) const override;
Vector whiten(const Vector& v) const override;
Vector unwhiten(const Vector& v) const override;
Matrix Whiten(const Matrix& H) const override;
void WhitenInPlace(Matrix& H) const override;
void whitenInPlace(Vector& v) const override;
void WhitenInPlace(Eigen::Block<Matrix> H) const override;
inline double sigma() const { return sigma_; }
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal);
ar & BOOST_SERIALIZATION_NVP(sigma_);
ar & BOOST_SERIALIZATION_NVP(invsigma_);
}
};
//---------------------------------------------------------------------------------------
class GTSAM_EXPORT Unit : public Isotropic {
public:
typedef boost::shared_ptr<Unit> shared_ptr;
Unit(size_t dim=1): Isotropic(dim,1.0) {}
~Unit() override {}
static shared_ptr Create(size_t dim) {
return shared_ptr(new Unit(dim));
}
bool isUnit() const override { return true; }
void print(const std::string& name) const override;
double squaredMahalanobisDistance(const Vector& v) const override;
Vector whiten(const Vector& v) const override { return v; }
Vector unwhiten(const Vector& v) const override { return v; }
Matrix Whiten(const Matrix& H) const override { return H; }
void WhitenInPlace(Matrix& /*H*/) const override {}
void WhitenInPlace(Eigen::Block<Matrix> /*H*/) const override {}
void whitenInPlace(Vector& /*v*/) const override {}
void unwhitenInPlace(Vector& /*v*/) const override {}
void whitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
void unwhitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Isotropic);
}
};
class GTSAM_EXPORT Robust : public Base {
public:
typedef boost::shared_ptr<Robust> shared_ptr;
protected:
typedef mEstimator::Base RobustModel;
typedef noiseModel::Base NoiseModel;
const RobustModel::shared_ptr robust_;
const NoiseModel::shared_ptr noise_;
public:
Robust() {};
Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
: Base(noise->dim()), robust_(robust), noise_(noise) {}
~Robust() override {}
void print(const std::string& name) const override;
bool equals(const Base& expected, double tol=1e-9) const override;
const RobustModel::shared_ptr& robust() const { return robust_; }
const NoiseModel::shared_ptr& noise() const { return noise_; }
// Functions below are dummy but necessary for the noiseModel::Base
inline Vector whiten(const Vector& v) const override
{ Vector r = v; this->WhitenSystem(r); return r; }
inline Matrix Whiten(const Matrix& A) const override
{ Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
inline Vector unwhiten(const Vector& /*v*/) const override
{ throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
double loss(const double squared_distance) const override {
return robust_->loss(std::sqrt(squared_distance));
}
// NOTE: This is special because in whiten the base version will do the reweighting
// which is incorrect!
double squaredMahalanobisDistance(const Vector& v) const override {
return noise_->squaredMahalanobisDistance(v);
}
// These are really robust iterated re-weighting support functions
virtual void WhitenSystem(Vector& b) const;
void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
void WhitenSystem(Matrix& A, Vector& b) const override;
void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
Vector unweightedWhiten(const Vector& v) const override;
double weight(const Vector& v) const override;
static shared_ptr Create(
const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & boost::serialization::make_nvp("robust_", const_cast<RobustModel::shared_ptr&>(robust_));
ar & boost::serialization::make_nvp("noise_", const_cast<NoiseModel::shared_ptr&>(noise_));
}
};
// Helper function
GTSAM_EXPORT boost::optional<Vector> checkIfDiagonal(const Matrix& M);
} // namespace noiseModel
typedef noiseModel::Base::shared_ptr SharedNoiseModel;
typedef noiseModel::Gaussian::shared_ptr SharedGaussian;
typedef noiseModel::Diagonal::shared_ptr SharedDiagonal;
typedef noiseModel::Constrained::shared_ptr SharedConstrained;
typedef noiseModel::Isotropic::shared_ptr SharedIsotropic;
template<> struct traits<noiseModel::Gaussian> : public Testable<noiseModel::Gaussian> {};
template<> struct traits<noiseModel::Diagonal> : public Testable<noiseModel::Diagonal> {};
template<> struct traits<noiseModel::Constrained> : public Testable<noiseModel::Constrained> {};
template<> struct traits<noiseModel::Isotropic> : public Testable<noiseModel::Isotropic> {};
template<> struct traits<noiseModel::Unit> : public Testable<noiseModel::Unit> {};
} //\ namespace gtsam
Testable.h
Concept check for values that can be used in unit tests.
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::noiseModel
All noise models live in the noiseModel namespace.
Definition LossFunctions.cpp:27
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::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::noiseModel::mEstimator::Base
Pure virtual class for all robust error function classes.
Definition LossFunctions.h:63
gtsam::noiseModel::Base
noiseModel::Base is the abstract base class for all noise models.
Definition NoiseModel.h:53
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::Base::whitenInPlace
virtual void whitenInPlace(Vector &v) const
in-place whiten, override if can be done more efficiently
Definition NoiseModel.h:112
gtsam::noiseModel::Base::dim
size_t dim() const
Dimensionality.
Definition NoiseModel.h:75
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::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::Base::whiten
virtual Vector whiten(const Vector &v) const =0
Whiten an error vector.
gtsam::noiseModel::Base::mahalanobisDistance
virtual double mahalanobisDistance(const Vector &v) const
Mahalanobis distance.
Definition NoiseModel.h:97
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::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::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::noiseModel::Base::squaredMahalanobisDistance
virtual double squaredMahalanobisDistance(const Vector &v) const
Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>.
Definition NoiseModel.cpp:79
gtsam::noiseModel::Base::unwhiten
virtual Vector unwhiten(const Vector &v) const =0
Unwhiten an error vector.
gtsam::noiseModel::Base::loss
virtual double loss(const double squared_distance) const
loss function, input is Mahalanobis distance
Definition NoiseModel.h:102
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::Base::Whiten
virtual Matrix Whiten(const Matrix &H) const =0
Whiten a matrix.
gtsam::noiseModel::Base::Base
Base(size_t dim=1)
primary constructor
Definition NoiseModel.h:65
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::Gaussian::R
virtual Matrix R() const
Return R itself, but note that Whiten(H) is cheaper than R*H.
Definition NoiseModel.h:255
gtsam::noiseModel::Gaussian::Gaussian
Gaussian(size_t dim=1, const boost::optional< Matrix > &sqrt_information=boost::none)
constructor takes square root information matrix
Definition NoiseModel.h:186
gtsam::noiseModel::Gaussian::sqrt_information_
boost::optional< Matrix > sqrt_information_
Matrix square root of information matrix (R).
Definition NoiseModel.h:167
gtsam::noiseModel::Diagonal
A diagonal noise model implements a diagonal covariance matrix, with the elements of the diagonal spe...
Definition NoiseModel.h:281
gtsam::noiseModel::Diagonal::R
Matrix R() const override
Return R itself, but note that Whiten(H) is cheaper than R*H.
Definition NoiseModel.h:352
gtsam::noiseModel::Diagonal::sigmas_
Vector sigmas_
Standard deviations (sigmas), their inverse and inverse square (weights/precisions) These are all com...
Definition NoiseModel.h:289
gtsam::noiseModel::Diagonal::sigma
double sigma(size_t i) const
Return standard deviations (sqrt of diagonal).
Definition NoiseModel.h:335
gtsam::noiseModel::Diagonal::invsigmas
const Vector & invsigmas() const
Return sqrt precisions.
Definition NoiseModel.h:340
gtsam::noiseModel::Diagonal::sigmas
Vector sigmas() const override
Calculate standard deviations.
Definition NoiseModel.h:325
gtsam::noiseModel::Diagonal::precisions
const Vector & precisions() const
Return precisions.
Definition NoiseModel.h:346
gtsam::noiseModel::Diagonal::Diagonal
Diagonal(const Vector &sigmas)
constructor to allow for disabling initialization of invsigmas
Definition NoiseModel.cpp:253
gtsam::noiseModel::Constrained
A Constrained constrained model is a specialization of Diagonal which allows some or all of the sigma...
Definition NoiseModel.h:381
gtsam::noiseModel::Constrained::isConstrained
bool isConstrained() const override
true if a constrained noise mode, saves slow/clumsy dynamic casting
Definition NoiseModel.h:409
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim, const Vector &mu)
Fully constrained variations.
Definition NoiseModel.h:462
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim, double mu)
Fully constrained variations with a mu parameter.
Definition NoiseModel.h:467
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim)
Fully constrained variations.
Definition NoiseModel.h:457
gtsam::noiseModel::Constrained::mu
const Vector & mu() const
Access mu as a vector.
Definition NoiseModel.h:415
gtsam::noiseModel::Constrained::Constrained
Constrained(const Vector &mu, const Vector &sigmas)
Constructor that prevents any inf values from appearing in invsigmas or precisions.
Definition NoiseModel.cpp:343
gtsam::noiseModel::Constrained::mu_
Vector mu_
Penalty function weight - needs to be large enough to dominate soft constraints.
Definition NoiseModel.h:385
gtsam::noiseModel::Isotropic
An isotropic noise model corresponds to a scaled diagonal covariance To construct,...
Definition NoiseModel.h:516
gtsam::noiseModel::Isotropic::sigma
double sigma() const
Return standard deviation.
Definition NoiseModel.h:565
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:549
gtsam::noiseModel::Isotropic::Isotropic
Isotropic(size_t dim, double sigma)
protected constructor takes sigma
Definition NoiseModel.h:521
gtsam::noiseModel::Isotropic::Variance
static shared_ptr Variance(size_t dim, double variance, bool smart=true)
An isotropic noise model created by specifying a variance = sigma^2.
Definition NoiseModel.cpp:603
gtsam::noiseModel::Unit
Unit: i.i.d.
Definition NoiseModel.h:584
gtsam::noiseModel::Unit::WhitenInPlace
void WhitenInPlace(Eigen::Block< Matrix >) const override
In-place version.
Definition NoiseModel.h:610
gtsam::noiseModel::Unit::unwhiten
Vector unwhiten(const Vector &v) const override
Unwhiten an error vector.
Definition NoiseModel.h:607
gtsam::noiseModel::Unit::whitenInPlace
void whitenInPlace(Eigen::Block< Vector > &) const override
in-place whiten, override if can be done more efficiently
Definition NoiseModel.h:613
gtsam::noiseModel::Unit::isUnit
bool isUnit() const override
true if a unit noise model, saves slow/clumsy dynamic casting
Definition NoiseModel.h:602
gtsam::noiseModel::Unit::Unit
Unit(size_t dim=1)
constructor for serialization
Definition NoiseModel.h:590
gtsam::noiseModel::Unit::whiten
Vector whiten(const Vector &v) const override
Whiten an error vector.
Definition NoiseModel.h:606
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition NoiseModel.h:597
gtsam::noiseModel::Unit::unwhitenInPlace
void unwhitenInPlace(Vector &) const override
in-place unwhiten, override if can be done more efficiently
Definition NoiseModel.h:612
gtsam::noiseModel::Unit::unwhitenInPlace
void unwhitenInPlace(Eigen::Block< Vector > &) const override
in-place unwhiten, override if can be done more efficiently
Definition NoiseModel.h:614
gtsam::noiseModel::Unit::whitenInPlace
void whitenInPlace(Vector &) const override
in-place whiten, override if can be done more efficiently
Definition NoiseModel.h:611
gtsam::noiseModel::Unit::WhitenInPlace
void WhitenInPlace(Matrix &) const override
In-place version.
Definition NoiseModel.h:609
gtsam::noiseModel::Unit::Whiten
Matrix Whiten(const Matrix &H) const override
Whiten a matrix.
Definition NoiseModel.h:608
gtsam::noiseModel::Robust::robust
const RobustModel::shared_ptr & robust() const
Return the contained robust error function.
Definition NoiseModel.h:669
gtsam::noiseModel::Robust::Robust
Robust()
Default Constructor for serialization.
Definition NoiseModel.h:656
gtsam::noiseModel::Robust::Robust
Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
Constructor.
Definition NoiseModel.h:659
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:688
gtsam::noiseModel::Robust::noise_
const NoiseModel::shared_ptr noise_
noise model used
Definition NoiseModel.h:651
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:682
gtsam::noiseModel::Robust::unwhiten
Vector unwhiten(const Vector &) const override
Unwhiten an error vector.
Definition NoiseModel.h:679
gtsam::noiseModel::Robust::Whiten
Matrix Whiten(const Matrix &A) const override
Whiten a matrix.
Definition NoiseModel.h:677
gtsam::noiseModel::Robust::robust_
const RobustModel::shared_ptr robust_
robust error function used
Definition NoiseModel.h:650
gtsam::noiseModel::Robust::~Robust
~Robust() override
Destructor.
Definition NoiseModel.h:663
gtsam::noiseModel::Robust::noise
const NoiseModel::shared_ptr & noise() const
Return the contained noise model.
Definition NoiseModel.h:672
gtsam::noiseModel::Robust::whiten
Vector whiten(const Vector &v) const override
Whiten an error vector.
Definition NoiseModel.h:675