gtsam 4.1.1
gtsam
gtsam::noiseModel::Unit Class Reference

Detailed Description

Unit: i.i.d.

unit-variance noise on all m dimensions.

+ Inheritance diagram for gtsam::noiseModel::Unit:

Public Member Functions

 Unit (size_t dim=1)
 constructor for serialization
 
bool isUnit () const override
 true if a unit noise model, saves slow/clumsy dynamic casting More...
 
void print (const std::string &name) const override
 
double squaredMahalanobisDistance (const Vector &v) const override
 Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v> More...
 
Vector whiten (const Vector &v) const override
 Whiten an error vector. More...
 
Vector unwhiten (const Vector &v) const override
 Unwhiten an error vector. More...
 
Matrix Whiten (const Matrix &H) const override
 Multiply a derivative with R (derivative of whiten) Equivalent to whitening each column of the input matrix. More...
 
void WhitenInPlace (Matrix &) const override
 In-place version. More...
 
void WhitenInPlace (Eigen::Block< Matrix >) const override
 In-place version. More...
 
void whitenInPlace (Vector &) const override
 in-place whiten, override if can be done more efficiently More...
 
void unwhitenInPlace (Vector &) const override
 in-place unwhiten, override if can be done more efficiently More...
 
void whitenInPlace (Eigen::Block< Vector > &) const override
 in-place whiten, override if can be done more efficiently More...
 
void unwhitenInPlace (Eigen::Block< Vector > &) const override
 in-place unwhiten, override if can be done more efficiently More...
 
- Public Member Functions inherited from gtsam::noiseModel::Isotropic
void print (const std::string &name) const override
 
double squaredMahalanobisDistance (const Vector &v) const override
 Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v> More...
 
Vector whiten (const Vector &v) const override
 Whiten an error vector. More...
 
Vector unwhiten (const Vector &v) const override
 Unwhiten an error vector. More...
 
Matrix Whiten (const Matrix &H) const override
 Multiply a derivative with R (derivative of whiten) Equivalent to whitening each column of the input matrix. More...
 
void WhitenInPlace (Matrix &H) const override
 In-place version. More...
 
void whitenInPlace (Vector &v) const override
 in-place whiten, override if can be done more efficiently More...
 
void WhitenInPlace (Eigen::Block< Matrix > H) const override
 In-place version. More...
 
double sigma () const
 Return standard deviation.
 
- Public Member Functions inherited from gtsam::noiseModel::Diagonal
 Diagonal ()
 constructor - no initializations, for serialization
 
void print (const std::string &name) const override
 
Vector sigmas () const override
 Calculate standard deviations. More...
 
Vector whiten (const Vector &v) const override
 Whiten an error vector. More...
 
Vector unwhiten (const Vector &v) const override
 Unwhiten an error vector. More...
 
Matrix Whiten (const Matrix &H) const override
 Multiply a derivative with R (derivative of whiten) Equivalent to whitening each column of the input matrix. More...
 
void WhitenInPlace (Matrix &H) const override
 In-place version. More...
 
void WhitenInPlace (Eigen::Block< Matrix > H) const override
 In-place version. More...
 
double sigma (size_t i) const
 Return standard deviations (sqrt of diagonal)
 
const Vector & invsigmas () const
 Return sqrt precisions.
 
double invsigma (size_t i) const
 
const Vector & precisions () const
 Return precisions.
 
double precision (size_t i) const
 
Matrix R () const override
 Return R itself, but note that Whiten(H) is cheaper than R*H. More...
 
- Public Member Functions inherited from gtsam::noiseModel::Gaussian
 Gaussian (size_t dim=1, const boost::optional< Matrix > &sqrt_information=boost::none)
 constructor takes square root information matrix
 
void print (const std::string &name) const override
 
bool equals (const Base &expected, double tol=1e-9) const override
 
Vector sigmas () const override
 Calculate standard deviations. More...
 
Vector whiten (const Vector &v) const override
 Whiten an error vector. More...
 
Vector unwhiten (const Vector &v) const override
 Unwhiten an error vector. More...
 
Matrix Whiten (const Matrix &H) const override
 Multiply a derivative with R (derivative of whiten) Equivalent to whitening each column of the input matrix. More...
 
virtual void WhitenInPlace (Matrix &H) const
 In-place version. More...
 
virtual void WhitenInPlace (Eigen::Block< Matrix > H) const
 In-place version. More...
 
void WhitenSystem (std::vector< Matrix > &A, Vector &b) const override
 Whiten a system, in place as well. More...
 
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< DiagonalQR (Matrix &Ab) const
 Apply appropriately weighted QR factorization to the system [A b] Q' * [A b] = [R d] Dimensions: (r*m) * m*(n+1) = r*(n+1), where r = min(m,n). More...
 
virtual Matrix R () const
 Return R itself, but note that Whiten(H) is cheaper than R*H. More...
 
virtual Matrix information () const
 Compute information matrix.
 
virtual Matrix covariance () const
 Compute covariance matrix.
 
- Public Member Functions inherited from gtsam::noiseModel::Base
 Base (size_t dim=1)
 primary constructor 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...
 
size_t dim () const
 Dimensionality.
 
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
 Calculate standard deviations. More...
 
virtual Vector whiten (const Vector &v) const =0
 Whiten an error vector. More...
 
virtual Matrix Whiten (const Matrix &H) const =0
 Whiten a matrix. More...
 
virtual Vector unwhiten (const Vector &v) const =0
 Unwhiten an error vector. More...
 
virtual double squaredMahalanobisDistance (const Vector &v) const
 Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v> More...
 
virtual double mahalanobisDistance (const Vector &v) const
 Mahalanobis distance.
 
virtual double loss (const double squared_distance) const
 loss function, input is Mahalanobis distance More...
 
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
 in-place whiten, override if can be done more efficiently More...
 
virtual void unwhitenInPlace (Vector &v) const
 in-place unwhiten, override if can be done more efficiently More...
 
virtual void whitenInPlace (Eigen::Block< Vector > &v) const
 in-place whiten, override if can be done more efficiently More...
 
virtual void unwhitenInPlace (Eigen::Block< Vector > &v) const
 in-place unwhiten, override if can be done more efficiently More...
 
virtual Vector unweightedWhiten (const Vector &v) const
 Useful function for robust noise models to get the unweighted but whitened error. More...
 
virtual double weight (const Vector &v) const
 get the weight from the effective loss function on residual vector v More...
 

Static Public Member Functions

static shared_ptr Create (size_t dim)
 Create a unit covariance noise model.
 
- Static Public Member Functions inherited from gtsam::noiseModel::Isotropic
static shared_ptr Sigma (size_t dim, double sigma, bool smart=true)
 An isotropic noise model created by specifying a standard devation sigma.
 
static shared_ptr Variance (size_t dim, double variance, bool smart=true)
 An isotropic noise model created by specifying a variance = sigma^2. More...
 
static shared_ptr Precision (size_t dim, double precision, bool smart=true)
 An isotropic noise model created by specifying a precision.
 
- Static Public Member Functions inherited from gtsam::noiseModel::Diagonal
static shared_ptr Sigmas (const Vector &sigmas, bool smart=true)
 A diagonal noise model created by specifying a Vector of sigmas, i.e. More...
 
static shared_ptr Variances (const Vector &variances, bool smart=true)
 A diagonal noise model created by specifying a Vector of variances, i.e. More...
 
static shared_ptr Precisions (const Vector &precisions, bool smart=true)
 A diagonal noise model created by specifying a Vector of precisions, i.e. More...
 
- Static Public Member Functions inherited from gtsam::noiseModel::Gaussian
static shared_ptr SqrtInformation (const Matrix &R, bool smart=true)
 A Gaussian noise model created by specifying a square root information matrix. More...
 
static shared_ptr Information (const Matrix &M, bool smart=true)
 A Gaussian noise model created by specifying an information matrix. More...
 
static shared_ptr Covariance (const Matrix &covariance, bool smart=true)
 A Gaussian noise model created by specifying a covariance matrix. More...
 

Public Types

typedef boost::shared_ptr< Unitshared_ptr
 
- Public Types inherited from gtsam::noiseModel::Isotropic
typedef boost::shared_ptr< Isotropicshared_ptr
 
- Public Types inherited from gtsam::noiseModel::Diagonal
typedef boost::shared_ptr< Diagonalshared_ptr
 
- Public Types inherited from gtsam::noiseModel::Gaussian
typedef boost::shared_ptr< Gaussianshared_ptr
 
- Public Types inherited from gtsam::noiseModel::Base
typedef boost::shared_ptr< Baseshared_ptr
 

Friends

class boost::serialization::access
 Serialization function.
 

Additional Inherited Members

- Protected Member Functions inherited from gtsam::noiseModel::Isotropic
 Isotropic (size_t dim, double sigma)
 protected constructor takes sigma
 
- Protected Member Functions inherited from gtsam::noiseModel::Diagonal
 Diagonal (const Vector &sigmas)
 constructor to allow for disabling initialization of invsigmas
 
- Protected Attributes inherited from gtsam::noiseModel::Isotropic
double sigma_
 
double invsigma_
 
- Protected Attributes inherited from gtsam::noiseModel::Diagonal
Vector sigmas_
 Standard deviations (sigmas), their inverse and inverse square (weights/precisions) These are all computed at construction: the idea is to use one shared model where computation is done only once, the common use case in many problems.
 
Vector invsigmas_
 
Vector precisions_
 
- Protected Attributes inherited from gtsam::noiseModel::Gaussian
boost::optional< Matrix > sqrt_information_
 Matrix square root of information matrix (R)
 
- Protected Attributes inherited from gtsam::noiseModel::Base
size_t dim_
 

Member Function Documentation

◆ isUnit()

bool gtsam::noiseModel::Unit::isUnit ( ) const
inlineoverridevirtual

true if a unit noise model, saves slow/clumsy dynamic casting

Reimplemented from gtsam::noiseModel::Base.

◆ print()

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

Reimplemented from gtsam::noiseModel::Isotropic.

◆ squaredMahalanobisDistance()

double gtsam::noiseModel::Unit::squaredMahalanobisDistance ( const Vector &  v) const
inlineoverridevirtual

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

Reimplemented from gtsam::noiseModel::Isotropic.

◆ unwhiten()

Vector gtsam::noiseModel::Unit::unwhiten ( const Vector &  v) const
inlineoverridevirtual

Unwhiten an error vector.

Reimplemented from gtsam::noiseModel::Isotropic.

◆ unwhitenInPlace() [1/2]

void gtsam::noiseModel::Unit::unwhitenInPlace ( Eigen::Block< Vector > &  v) const
inlineoverridevirtual

in-place unwhiten, override if can be done more efficiently

Reimplemented from gtsam::noiseModel::Base.

◆ unwhitenInPlace() [2/2]

void gtsam::noiseModel::Unit::unwhitenInPlace ( Vector &  v) const
inlineoverridevirtual

in-place unwhiten, override if can be done more efficiently

Reimplemented from gtsam::noiseModel::Base.

◆ Whiten()

Matrix gtsam::noiseModel::Unit::Whiten ( const Matrix &  H) const
inlineoverridevirtual

Multiply a derivative with R (derivative of whiten) Equivalent to whitening each column of the input matrix.

Reimplemented from gtsam::noiseModel::Isotropic.

◆ whiten()

Vector gtsam::noiseModel::Unit::whiten ( const Vector &  v) const
inlineoverridevirtual

Whiten an error vector.

Reimplemented from gtsam::noiseModel::Isotropic.

◆ WhitenInPlace() [1/2]

void gtsam::noiseModel::Unit::WhitenInPlace ( Eigen::Block< Matrix >  H) const
inlineoverridevirtual

In-place version.

Reimplemented from gtsam::noiseModel::Isotropic.

◆ whitenInPlace() [1/2]

void gtsam::noiseModel::Unit::whitenInPlace ( Eigen::Block< Vector > &  v) const
inlineoverridevirtual

in-place whiten, override if can be done more efficiently

Reimplemented from gtsam::noiseModel::Base.

◆ WhitenInPlace() [2/2]

void gtsam::noiseModel::Unit::WhitenInPlace ( Matrix &  H) const
inlineoverridevirtual

In-place version.

Reimplemented from gtsam::noiseModel::Isotropic.

◆ whitenInPlace() [2/2]

void gtsam::noiseModel::Unit::whitenInPlace ( Vector &  v) const
inlineoverridevirtual

in-place whiten, override if can be done more efficiently

Reimplemented from gtsam::noiseModel::Isotropic.


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