gtsam  4.0.0
gtsam
gtsam::noiseModel::Diagonal Class Reference

Detailed Description

A diagonal noise model implements a diagonal covariance matrix, with the elements of the diagonal specified in a Vector.

This class has no public constructors, instead, use the static constructor functions Sigmas etc...

+ Inheritance diagram for gtsam::noiseModel::Diagonal:

Public Member Functions

virtual void print (const std::string &name) const
 
virtual Vector sigmas () const
 Calculate standard deviations.
 
virtual Vector whiten (const Vector &v) const
 Whiten an error vector.
 
virtual Vector unwhiten (const Vector &v) const
 Unwhiten an error vector.
 
virtual Matrix Whiten (const Matrix &H) const
 Multiply a derivative with R (derivative of whiten) Equivalent to whitening each column of the input matrix.
 
virtual void WhitenInPlace (Matrix &H) const
 In-place version.
 
virtual void WhitenInPlace (Eigen::Block< Matrix > H) const
 In-place version.
 
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
 
virtual Matrix R () const
 Return R itself, but note that Whiten(H) is cheaper than R*H.
 
- Public Member Functions inherited from gtsam::noiseModel::Gaussian
virtual bool equals (const Base &expected, double tol=1e-9) const
 
virtual double Mahalanobis (const Vector &v) const
 Mahalanobis distance v'*R'*R*v = <R*v,R*v>
 
virtual double distance (const Vector &v) const
 
virtual void WhitenSystem (std::vector< Matrix > &A, Vector &b) const
 Whiten a system, in place as well.
 
virtual void WhitenSystem (Matrix &A, Vector &b) const
 
virtual void WhitenSystem (Matrix &A1, Matrix &A2, Vector &b) const
 
virtual void WhitenSystem (Matrix &A1, Matrix &A2, Matrix &A3, Vector &b) const
 
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 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
 
virtual bool isUnit () const
 true if a unit noise model, saves slow/clumsy dynamic casting
 
size_t dim () const
 Dimensionality.
 
virtual void whitenInPlace (Vector &v) const
 in-place whiten, override if can be done more efficiently
 
virtual void unwhitenInPlace (Vector &v) const
 in-place unwhiten, override if can be done more efficiently
 
virtual void whitenInPlace (Eigen::Block< Vector > &v) const
 in-place whiten, override if can be done more efficiently
 
virtual void unwhitenInPlace (Eigen::Block< Vector > &v) const
 in-place unwhiten, override if can be done more efficiently
 

Static Public Member Functions

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< 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
 

Protected Member Functions

 Diagonal ()
 protected constructor - no initializations
 
 Diagonal (const Vector &sigmas)
 constructor to allow for disabling initialization of invsigmas
 
- Protected Member Functions inherited from gtsam::noiseModel::Gaussian
 Gaussian (size_t dim=1, const boost::optional< Matrix > &sqrt_information=boost::none)
 protected constructor takes square root information matrix
 

Protected Attributes

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_
 

Friends

class boost::serialization::access
 Serialization function.
 

Member Function Documentation

◆ Precisions()

static shared_ptr gtsam::noiseModel::Diagonal::Precisions ( const Vector &  precisions,
bool  smart = true 
)
inlinestatic

A diagonal noise model created by specifying a Vector of precisions, i.e.

i.e. the diagonal of the information matrix, i.e., weights

◆ Sigmas()

Diagonal::shared_ptr gtsam::noiseModel::Diagonal::Sigmas ( const Vector &  sigmas,
bool  smart = true 
)
static

A diagonal noise model created by specifying a Vector of sigmas, i.e.

standard deviations, the diagonal of the square root covariance matrix.

◆ Variances()

Diagonal::shared_ptr gtsam::noiseModel::Diagonal::Variances ( const Vector &  variances,
bool  smart = true 
)
static

A diagonal noise model created by specifying a Vector of variances, i.e.

i.e. the diagonal of the covariance matrix.

Parameters
variancesA vector containing the variances of this noise model
smartcheck if can be simplified to derived class

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