GTSAM  4.0.2
C++ library for smoothing and mapping (SAM)
Public Types | Public Member Functions | Static Public Member Functions | Protected Attributes | List of all members
gtsam::noiseModel::Unit Class Reference

#include <NoiseModel.h>

Inheritance diagram for gtsam::noiseModel::Unit:
Inheritance graph
[legend]
Collaboration diagram for gtsam::noiseModel::Unit:
Collaboration graph
[legend]

Public Types

typedef std::shared_ptr< Unitshared_ptr
 

Public Member Functions

 Unit (size_t dim=1)
 
bool isUnit () const override
 true if a unit noise model, saves slow/clumsy dynamic casting
 
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>
 
Vector whiten (const Vector &v) const override
 Whiten an error vector.
 
Vector unwhiten (const Vector &v) const override
 Unwhiten an error vector.
 
Matrix Whiten (const Matrix &H) const override
 
void WhitenInPlace (Matrix &) const override
 
void WhitenInPlace (Eigen::Block< Matrix >) const override
 
void whitenInPlace (Vector &) const override
 
void unwhitenInPlace (Vector &) const override
 
void whitenInPlace (Eigen::Block< Vector > &) const override
 
void unwhitenInPlace (Eigen::Block< Vector > &) const override
 
double sigma () const
 
double sigma (size_t i) const
 
Vector sigmas () const override
 Calculate standard deviations.
 
const Vector & invsigmas () const
 
double invsigma (size_t i) const
 
const Vector & precisions () const
 
double precision (size_t i) const
 
Matrix R () const override
 
bool equals (const Base &expected, double tol=1e-9) const override
 
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 std::shared_ptr< DiagonalQR (Matrix &Ab) const
 
virtual Matrix information () const
 Compute information matrix.
 
virtual Matrix covariance () const
 Compute covariance matrix.
 
virtual bool isConstrained () const
 true if a constrained noise model, saves slow/clumsy dynamic casting
 
size_t dim () const
 Dimensionality.
 
virtual double mahalanobisDistance (const Vector &v) const
 Mahalanobis distance.
 
virtual double loss (const double squared_distance) const
 loss function, input is Mahalanobis distance
 
virtual Vector unweightedWhiten (const Vector &v) const
 
virtual double weight (const Vector &v) const
 

Static Public Member Functions

static shared_ptr Create (size_t dim)
 
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)
 
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)
 
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)
 

Protected Attributes

double sigma_
 
double invsigma_
 
Vector sigmas_
 
Vector invsigmas_
 
Vector precisions_
 
std::optional< Matrix > sqrt_information_
 
size_t dim_
 

Detailed Description

Unit: i.i.d. unit-variance noise on all m dimensions.

Constructor & Destructor Documentation

◆ Unit()

gtsam::noiseModel::Unit::Unit ( size_t  dim = 1)
inline

constructor for serialization

Member Function Documentation

◆ Covariance()

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

A Gaussian noise model created by specifying a covariance matrix.

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

◆ Create()

static shared_ptr gtsam::noiseModel::Unit::Create ( size_t  dim)
inlinestatic

Create a unit covariance noise model

◆ Information()

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

A Gaussian noise model created by specifying an information matrix.

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

◆ invsigmas()

const Vector& gtsam::noiseModel::Diagonal::invsigmas ( ) const
inlineinherited

Return sqrt precisions

◆ Precision()

static shared_ptr gtsam::noiseModel::Isotropic::Precision ( size_t  dim,
double  precision,
bool  smart = true 
)
inlinestaticinherited

An isotropic noise model created by specifying a precision

◆ Precisions()

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

A diagonal noise model created by specifying a Vector of precisions, i.e. i.e. the diagonal of the information matrix, i.e., weights

◆ precisions()

const Vector& gtsam::noiseModel::Diagonal::precisions ( ) const
inlineinherited

Return precisions

◆ QR()

virtual std::shared_ptr<Diagonal> gtsam::noiseModel::Gaussian::QR ( Matrix &  Ab) const
virtualinherited

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.

◆ R()

Matrix gtsam::noiseModel::Diagonal::R ( ) const
inlineoverridevirtualinherited

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

Reimplemented from gtsam::noiseModel::Gaussian.

◆ sigma() [1/2]

double gtsam::noiseModel::Diagonal::sigma ( size_t  i) const
inlineinherited

Return standard deviations (sqrt of diagonal)

◆ Sigma()

static shared_ptr gtsam::noiseModel::Isotropic::Sigma ( size_t  dim,
double  sigma,
bool  smart = true 
)
staticinherited

An isotropic noise model created by specifying a standard devation sigma

◆ sigma() [2/2]

double gtsam::noiseModel::Isotropic::sigma ( ) const
inlineinherited

Return standard deviation

◆ Sigmas()

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

A diagonal noise model created by specifying a Vector of sigmas, i.e. standard deviations, the diagonal of the square root covariance matrix.

◆ SqrtInformation()

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

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

◆ unweightedWhiten()

virtual Vector gtsam::noiseModel::Base::unweightedWhiten ( const Vector &  v) const
inlinevirtualinherited

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

Reimplemented in gtsam::noiseModel::Robust.

◆ unwhitenInPlace() [1/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.

◆ unwhitenInPlace() [2/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.

◆ Variance()

static shared_ptr gtsam::noiseModel::Isotropic::Variance ( size_t  dim,
double  variance,
bool  smart = true 
)
staticinherited

An isotropic noise model created by specifying a variance = sigma^2.

Parameters
dimThe dimension of this noise model
varianceThe variance of this noise model
smartcheck if can be simplified to derived class

◆ Variances()

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

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

◆ weight()

virtual double gtsam::noiseModel::Base::weight ( const Vector &  v) const
inlinevirtualinherited

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

Reimplemented in gtsam::noiseModel::Robust.

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

◆ WhitenInPlace() [1/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 ( Eigen::Block< Matrix >  H) const
inlineoverridevirtual

In-place version

Reimplemented from gtsam::noiseModel::Isotropic.

◆ whitenInPlace() [1/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.

◆ whitenInPlace() [2/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.

◆ WhitenSystem()

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

Whiten a system, in place as well

Implements gtsam::noiseModel::Base.

Member Data Documentation

◆ sigmas_

Vector gtsam::noiseModel::Diagonal::sigmas_
protectedinherited

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.

◆ sqrt_information_

std::optional<Matrix> gtsam::noiseModel::Gaussian::sqrt_information_
protectedinherited

Matrix square root of information matrix (R)


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