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

#include <NoiseModel.h>

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

Public Types

typedef boost::shared_ptr< Gaussianshared_ptr
 
- Public Types inherited from gtsam::noiseModel::Base
typedef boost::shared_ptr< Baseshared_ptr
 

Public Member Functions

virtual Matrix covariance () const
 Compute covariance matrix. More...
 
bool equals (const Base &expected, double tol=1e-9) const override
 
virtual Matrix information () const
 Compute information matrix. More...
 
void print (const std::string &name) const override
 
virtual boost::shared_ptr< DiagonalQR (Matrix &Ab) const
 
virtual Matrix R () const
 Return R itself, but note that Whiten(H) is cheaper than R*H. More...
 
Vector sigmas () const override
 Calculate standard deviations. More...
 
Vector unwhiten (const Vector &v) const override
 Unwhiten an error vector. More...
 
Vector whiten (const Vector &v) const override
 Whiten an error vector. More...
 
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
 
 ~Gaussian () override
 
- 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 loss (const double squared_distance) const
 loss function, input is Mahalanobis distance More...
 
virtual double mahalanobisDistance (const Vector &v) const
 Mahalanobis distance. More...
 
virtual double squaredMahalanobisDistance (const Vector &v) const
 Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v> More...
 
virtual Vector unweightedWhiten (const Vector &v) const
 
virtual void unwhitenInPlace (Vector &v) const
 
virtual void unwhitenInPlace (Eigen::Block< Vector > &v) const
 
virtual double weight (const Vector &v) const
 
virtual void whitenInPlace (Vector &v) const
 
virtual void whitenInPlace (Eigen::Block< Vector > &v) const
 
virtual ~Base ()
 

Static Public Member Functions

static shared_ptr Covariance (const Matrix &covariance, bool smart=true)
 
static shared_ptr Information (const Matrix &M, bool smart=true)
 
static shared_ptr SqrtInformation (const Matrix &R, bool smart=true)
 

Protected Member Functions

 Gaussian (size_t dim=1, const boost::optional< Matrix > &sqrt_information=boost::none)
 

Protected Attributes

boost::optional< Matrixsqrt_information_
 
- Protected Attributes inherited from gtsam::noiseModel::Base
size_t dim_
 

Private Member Functions

template<class ARCHIVE >
void serialize (ARCHIVE &ar, const unsigned int)
 
const MatrixthisR () const
 

Friends

class boost::serialization::access
 

Detailed Description

Gaussian implements the mathematical model |R*x|^2 = |y|^2 with R'*R=inv(Sigma) where y = whiten(x) = R*x x = unwhiten(x) = inv(R)*y as indeed |y|^2 = y'*y = x'*R'*R*x Various derived classes are available that are more efficient. The named constructors return a shared_ptr because, when the smart flag is true, the underlying object might be a derived class such as Diagonal.

Definition at line 162 of file NoiseModel.h.

Member Typedef Documentation

Definition at line 189 of file NoiseModel.h.

Constructor & Destructor Documentation

gtsam::noiseModel::Gaussian::Gaussian ( size_t  dim = 1,
const boost::optional< Matrix > &  sqrt_information = boost::none 
)
inlineprotected

protected constructor takes square root information matrix

Definition at line 183 of file NoiseModel.h.

gtsam::noiseModel::Gaussian::~Gaussian ( )
inlineoverride

Definition at line 191 of file NoiseModel.h.

Member Function Documentation

Gaussian::shared_ptr gtsam::noiseModel::Gaussian::Covariance ( const Matrix covariance,
bool  smart = true 
)
static

A Gaussian noise model created by specifying a covariance matrix.

Parameters
covarianceThe square covariance Matrix
smartcheck if can be simplified to derived class

Definition at line 116 of file NoiseModel.cpp.

Matrix gtsam::noiseModel::Gaussian::covariance ( ) const
virtual

Compute covariance matrix.

Definition at line 149 of file NoiseModel.cpp.

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

Implements gtsam::noiseModel::Base.

Definition at line 141 of file NoiseModel.cpp.

Gaussian::shared_ptr gtsam::noiseModel::Gaussian::Information ( const Matrix M,
bool  smart = true 
)
static

A Gaussian noise model created by specifying an information matrix.

Parameters
MThe information matrix
smartcheck if can be simplified to derived class

Definition at line 99 of file NoiseModel.cpp.

virtual Matrix gtsam::noiseModel::Gaussian::information ( ) const
inlinevirtual

Compute information matrix.

Definition at line 259 of file NoiseModel.h.

void gtsam::noiseModel::Gaussian::print ( const std::string &  name) const
overridevirtual
SharedDiagonal gtsam::noiseModel::Gaussian::QR ( Matrix Ab) const
virtual

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). This routine performs an in-place factorization on Ab. Below-diagonal elements are set to zero by this routine.

Parameters
Abis the m*(n+1) augmented system matrix [A b]
Returns
Empty SharedDiagonal() noise model: R,d are whitened

Reimplemented in gtsam::noiseModel::Constrained.

Definition at line 191 of file NoiseModel.cpp.

virtual Matrix gtsam::noiseModel::Gaussian::R ( ) const
inlinevirtual

Return R itself, but note that Whiten(H) is cheaper than R*H.

Reimplemented in gtsam::noiseModel::Diagonal.

Definition at line 256 of file NoiseModel.h.

template<class ARCHIVE >
void gtsam::noiseModel::Gaussian::serialize ( ARCHIVE &  ar,
const unsigned  int 
)
inlineprivate

Definition at line 268 of file NoiseModel.h.

Vector gtsam::noiseModel::Gaussian::sigmas ( ) const
overridevirtual

Calculate standard deviations.

Reimplemented from gtsam::noiseModel::Base.

Reimplemented in gtsam::noiseModel::Diagonal.

Definition at line 160 of file NoiseModel.cpp.

Gaussian::shared_ptr gtsam::noiseModel::Gaussian::SqrtInformation ( const Matrix R,
bool  smart = true 
)
static

A Gaussian noise model created by specifying a square root information matrix.

Parameters
RThe (upper-triangular) square root information matrix
smartcheck if can be simplified to derived class

Definition at line 85 of file NoiseModel.cpp.

const Matrix& gtsam::noiseModel::Gaussian::thisR ( ) const
inlineprivate

Return R itself, but note that Whiten(H) is cheaper than R*H

Definition at line 174 of file NoiseModel.h.

Vector gtsam::noiseModel::Gaussian::unwhiten ( const Vector v) const
overridevirtual

Unwhiten an error vector.

Implements gtsam::noiseModel::Base.

Reimplemented in gtsam::noiseModel::Unit, gtsam::noiseModel::Isotropic, and gtsam::noiseModel::Diagonal.

Definition at line 170 of file NoiseModel.cpp.

Vector gtsam::noiseModel::Gaussian::whiten ( const Vector v) const
overridevirtual
Matrix gtsam::noiseModel::Gaussian::Whiten ( const Matrix H) const
overridevirtual

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

Implements gtsam::noiseModel::Base.

Reimplemented in gtsam::noiseModel::Unit, gtsam::noiseModel::Isotropic, gtsam::noiseModel::Constrained, and gtsam::noiseModel::Diagonal.

Definition at line 175 of file NoiseModel.cpp.

void gtsam::noiseModel::Gaussian::WhitenInPlace ( Matrix H) const
virtual
void gtsam::noiseModel::Gaussian::WhitenInPlace ( Eigen::Block< Matrix H) const
virtual
void gtsam::noiseModel::Gaussian::WhitenSystem ( std::vector< Matrix > &  A,
Vector b 
) const
overridevirtual

Whiten a system, in place as well

Implements gtsam::noiseModel::Base.

Definition at line 218 of file NoiseModel.cpp.

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

Implements gtsam::noiseModel::Base.

Definition at line 223 of file NoiseModel.cpp.

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

Implements gtsam::noiseModel::Base.

Definition at line 228 of file NoiseModel.cpp.

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

Implements gtsam::noiseModel::Base.

Definition at line 234 of file NoiseModel.cpp.

Friends And Related Function Documentation

friend class boost::serialization::access
friend

Serialization function

Definition at line 266 of file NoiseModel.h.

Member Data Documentation

boost::optional<Matrix> gtsam::noiseModel::Gaussian::sqrt_information_
protected

Matrix square root of information matrix (R)

Definition at line 167 of file NoiseModel.h.


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


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:58:53