Public Types | Public Member Functions | Static Public Member Functions | Protected Types | Protected Attributes | List of all members
gtsam::noiseModel::Robust Class Reference

#include <NoiseModel.h>

Inheritance diagram for gtsam::noiseModel::Robust:
Inheritance graph
[legend]

Public Types

typedef std::shared_ptr< Robustshared_ptr
 
- Public Types inherited from gtsam::noiseModel::Base
typedef std::shared_ptr< Baseshared_ptr
 

Public Member Functions

bool equals (const Base &expected, double tol=1e-9) const override
 
double loss (const double squared_distance) const override
 Compute loss from the m-estimator using the Mahalanobis distance. More...
 
const NoiseModel::shared_ptrnoise () const
 Return the contained noise model. More...
 
void print (const std::string &name) const override
 
 Robust ()
 Default Constructor for serialization. More...
 
const RobustModel::shared_ptrrobust () const
 Return the contained robust error function. More...
 
 Robust (const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
 Constructor. More...
 
double squaredMahalanobisDistance (const Vector &v) const override
 Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v> More...
 
Vector unweightedWhiten (const Vector &v) const override
 
Vector unwhiten (const Vector &) const override
 Unwhiten an error vector. More...
 
double weight (const Vector &v) const override
 
Matrix Whiten (const Matrix &A) const override
 Whiten a matrix. More...
 
Vector whiten (const Vector &v) const override
 Whiten an error vector. More...
 
void WhitenSystem (Matrix &A, Vector &b) const override
 
void WhitenSystem (Matrix &A1, Matrix &A2, Matrix &A3, Vector &b) const override
 
void WhitenSystem (Matrix &A1, Matrix &A2, Vector &b) const override
 
void WhitenSystem (std::vector< Matrix > &A, Vector &b) const override
 
virtual void WhitenSystem (Vector &b) const
 
 ~Robust () override
 Destructor. More...
 
- Public Member Functions inherited from gtsam::noiseModel::Base
 Base (size_t dim=1)
 primary constructor More...
 
size_t dim () const
 Dimensionality. More...
 
virtual bool isConstrained () const
 true if a constrained noise model, saves slow/clumsy dynamic casting More...
 
virtual bool isUnit () const
 true if a unit noise model, saves slow/clumsy dynamic casting More...
 
virtual double mahalanobisDistance (const Vector &v) const
 Mahalanobis distance. More...
 
virtual Vector sigmas () const
 Calculate standard deviations. More...
 
virtual void unwhitenInPlace (Eigen::Block< Vector > &v) const
 
virtual void unwhitenInPlace (Vector &v) const
 
virtual void whitenInPlace (Eigen::Block< Vector > &v) const
 
virtual void whitenInPlace (Vector &v) const
 
virtual ~Base ()
 

Static Public Member Functions

static shared_ptr Create (const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
 

Protected Types

typedef noiseModel::Base NoiseModel
 
typedef mEstimator::Base RobustModel
 

Protected Attributes

const NoiseModel::shared_ptr noise_
 noise model used More...
 
const RobustModel::shared_ptr robust_
 robust error function used More...
 
- Protected Attributes inherited from gtsam::noiseModel::Base
size_t dim_
 

Detailed Description

Base class for robust error models The robust M-estimators above simply tell us how to re-weight the residual, and are isotropic kernels, in that they do not allow for correlated noise. They also have no way to scale the residual values, e.g., dividing by a single standard deviation. Hence, the actual robust noise model below does this scaling/whitening in sequence, by passing both a standard noise model and a robust estimator.

Taking as an example noise = Isotropic::Create(d, sigma), we first divide the residuals uw = |Ax-b| by sigma by "whitening" the system (A,b), obtaining r = |Ax-b|/sigma, and then we pass the now whitened residual 'r' through the robust M-estimator. This is currently done by multiplying with sqrt(w), because the residuals will be squared again in error, yielding 0.5 \sum w(r)*r^2.

In other words, while sigma is expressed in the native residual units, a parameter like k in the Huber norm is expressed in whitened units, i.e., "nr of sigmas".

Definition at line 657 of file NoiseModel.h.

Member Typedef Documentation

◆ NoiseModel

Definition at line 663 of file NoiseModel.h.

◆ RobustModel

Definition at line 662 of file NoiseModel.h.

◆ shared_ptr

Definition at line 659 of file NoiseModel.h.

Constructor & Destructor Documentation

◆ Robust() [1/2]

gtsam::noiseModel::Robust::Robust ( )
inline

Default Constructor for serialization.

Definition at line 671 of file NoiseModel.h.

◆ Robust() [2/2]

gtsam::noiseModel::Robust::Robust ( const RobustModel::shared_ptr  robust,
const NoiseModel::shared_ptr  noise 
)
inline

Constructor.

Definition at line 674 of file NoiseModel.h.

◆ ~Robust()

gtsam::noiseModel::Robust::~Robust ( )
inlineoverride

Destructor.

Definition at line 678 of file NoiseModel.h.

Member Function Documentation

◆ Create()

Robust::shared_ptr gtsam::noiseModel::Robust::Create ( const RobustModel::shared_ptr robust,
const NoiseModel::shared_ptr  noise 
)
static

Definition at line 704 of file NoiseModel.cpp.

◆ equals()

bool gtsam::noiseModel::Robust::equals ( const Base expected,
double  tol = 1e-9 
) const
overridevirtual

Implements gtsam::noiseModel::Base.

Definition at line 666 of file NoiseModel.cpp.

◆ loss()

double gtsam::noiseModel::Robust::loss ( const double  squared_distance) const
inlineoverridevirtual

Compute loss from the m-estimator using the Mahalanobis distance.

Reimplemented from gtsam::noiseModel::Base.

Definition at line 697 of file NoiseModel.h.

◆ noise()

const NoiseModel::shared_ptr& gtsam::noiseModel::Robust::noise ( ) const
inline

Return the contained noise model.

Definition at line 687 of file NoiseModel.h.

◆ print()

void gtsam::noiseModel::Robust::print ( const std::string &  name) const
overridevirtual

Implements gtsam::noiseModel::Base.

Definition at line 661 of file NoiseModel.cpp.

◆ robust()

const RobustModel::shared_ptr& gtsam::noiseModel::Robust::robust ( ) const
inline

Return the contained robust error function.

Definition at line 684 of file NoiseModel.h.

◆ squaredMahalanobisDistance()

double gtsam::noiseModel::Robust::squaredMahalanobisDistance ( const Vector v) const
inlineoverridevirtual

Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>

Reimplemented from gtsam::noiseModel::Base.

Definition at line 703 of file NoiseModel.h.

◆ unweightedWhiten()

Vector gtsam::noiseModel::Robust::unweightedWhiten ( const Vector v) const
overridevirtual

Useful function for robust noise models to get the unweighted but whitened error

Reimplemented from gtsam::noiseModel::Base.

Definition at line 697 of file NoiseModel.cpp.

◆ unwhiten()

Vector gtsam::noiseModel::Robust::unwhiten ( const Vector v) const
inlineoverridevirtual

Unwhiten an error vector.

Implements gtsam::noiseModel::Base.

Definition at line 694 of file NoiseModel.h.

◆ weight()

double gtsam::noiseModel::Robust::weight ( const Vector v) const
overridevirtual

get the weight from the effective loss function on residual vector v

Reimplemented from gtsam::noiseModel::Base.

Definition at line 700 of file NoiseModel.cpp.

◆ Whiten()

Matrix gtsam::noiseModel::Robust::Whiten ( const Matrix H) const
inlineoverridevirtual

Whiten a matrix.

Implements gtsam::noiseModel::Base.

Definition at line 692 of file NoiseModel.h.

◆ whiten()

Vector gtsam::noiseModel::Robust::whiten ( const Vector v) const
inlineoverridevirtual

Whiten an error vector.

Implements gtsam::noiseModel::Base.

Definition at line 690 of file NoiseModel.h.

◆ WhitenSystem() [1/5]

void gtsam::noiseModel::Robust::WhitenSystem ( Matrix A,
Vector b 
) const
overridevirtual

Implements gtsam::noiseModel::Base.

Definition at line 682 of file NoiseModel.cpp.

◆ WhitenSystem() [2/5]

void gtsam::noiseModel::Robust::WhitenSystem ( Matrix A1,
Matrix A2,
Matrix A3,
Vector b 
) const
overridevirtual

Implements gtsam::noiseModel::Base.

Definition at line 692 of file NoiseModel.cpp.

◆ WhitenSystem() [3/5]

void gtsam::noiseModel::Robust::WhitenSystem ( Matrix A1,
Matrix A2,
Vector b 
) const
overridevirtual

Implements gtsam::noiseModel::Base.

Definition at line 687 of file NoiseModel.cpp.

◆ WhitenSystem() [4/5]

void gtsam::noiseModel::Robust::WhitenSystem ( std::vector< Matrix > &  A,
Vector b 
) const
overridevirtual

Implements gtsam::noiseModel::Base.

Definition at line 677 of file NoiseModel.cpp.

◆ WhitenSystem() [5/5]

void gtsam::noiseModel::Robust::WhitenSystem ( Vector b) const
virtual

Definition at line 672 of file NoiseModel.cpp.

Member Data Documentation

◆ noise_

const NoiseModel::shared_ptr gtsam::noiseModel::Robust::noise_
protected

noise model used

Definition at line 666 of file NoiseModel.h.

◆ robust_

const RobustModel::shared_ptr gtsam::noiseModel::Robust::robust_
protected

robust error function used

Definition at line 665 of file NoiseModel.h.


The documentation for this class was generated from the following files:


gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:18:17