Detailed Description

A Constrained constrained model is a specialization of Diagonal which allows some or all of the sigmas to be zero, forcing the error to be zero there.

All other Gaussian models are guaranteed to have a non-singular square-root information matrix, but this class is specifically equipped to deal with singular noise models, specifically: whiten will return zero on those components that have zero sigma and zero error, unchanged otherwise.

While a hard constraint may seem to be a case in which there is infinite error, we do not ever produce an error value of infinity to allow for constraints to actually be optimized rather than self-destructing if not initialized correctly.

Inheritance diagram for gtsam::Constrained:

Public Member Functions
	Constrained (const Vector &sigmas=Z_1x1)
	protected constructor takes sigmas.
bool	isConstrained () const override
	true if a constrained noise mode, saves slow/clumsy dynamic casting
bool	constrained (size_t i) const
	Return true if a particular dimension is free or constrained.
const Vector &	mu () const
	Access mu as a vector.
double	squaredMahalanobisDistance (const Vector &v) const override
	The squaredMahalanobisDistance function for a constrained noise model, for non-constrained versions, uses sigmas, otherwise uses the penalty function with mu.
void	print (const std::string &name) const override
Vector	whiten (const Vector &v) const override
	Calculates error vector with weights applied.
void	whitenInPlace (Vector &v) const override
	in-place whiten, override if can be done more efficiently
void	whitenInPlace (Eigen::Block< Vector > &v) const override
	in-place whiten, override if can be done more efficiently
Matrix	Whiten (const Matrix &H) const override
	Whitening functions will perform partial whitening on rows with a non-zero sigma.
void	WhitenInPlace (Matrix &H) const override
	In-place version.
void	WhitenInPlace (Eigen::Block< Matrix > H) const override
	In-place version.
Matrix	informationFromA (const Matrix &A) const
	Compute A' * diag(precisions) * A using constrained precisions.
Diagonal::shared_ptr	QR (Matrix &Ab) const override
	Apply QR factorization to the system [A b], taking into account constraints Q' * [A b] = [R d] Dimensions: (rm) m(n+1) = r(n+1), where r = min(m,n).
shared_ptr	unit () const
	Returns a Unit version of a constrained noise model in which constrained sigmas remain constrained and the rest are unit scaled.
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.
const Vector &	sigmasRef () const
	Return standard deviations without copying.
Vector	whiten (const Vector &v) const override
	Whiten an error vector.
Vector	unwhiten (const Vector &v) const override
	Unwhiten an error vector.
void	whitenInPlace (Vector &v) const override
	in-place whiten, override if can be done more efficiently
void	unwhitenInPlace (Vector &v) const override
	in-place unwhiten, override if can be done more efficiently
Matrix	Whiten (const Matrix &H) const override
	Whiten a matrix.
void	WhitenInPlace (Matrix &H) const override
	In-place version.
void	WhitenInPlace (Eigen::Block< Matrix > H) const override
	In-place version.
void	whitenInPlace (Eigen::Block< Vector > &v) const override
	in-place whiten, override if can be done more efficiently
void	unwhitenInPlace (Eigen::Block< Vector > &v) const override
	in-place unwhiten, override if can be done more efficiently
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.
Public Member Functions inherited from gtsam::noiseModel::Gaussian
	Gaussian (size_t dim=1, const std::optional< Matrix > &sqrt_information={})
	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.
Vector	whiten (const Vector &v) const override
	Whiten an error vector.
Vector	unwhiten (const Vector &v) const override
	Unwhiten an error vector.
void	unwhitenInPlace (Vector &v) const override
	in-place unwhiten, override if can be done more efficiently
void	unwhitenInPlace (Eigen::Block< Vector > &v) const override
	in-place unwhiten, override if can be done more efficiently
Matrix	Whiten (const Matrix &H) const override
	Multiply a derivative with R (derivative of whiten) Equivalent to whitening each column of the input matrix.
void	WhitenSystem (std::vector< Matrix > &A, Vector &b) const override
	Whiten a system, in place as well.
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 Matrix	information () const
	Compute information matrix.
virtual Matrix	covariance () const
	Compute covariance matrix.
double	logDeterminant () const
	Compute the log of \|Σ\|.
double	negLogConstant () const
	Compute the negative log of the normalization constant for a Gaussian noise model k = 1/\sqrt(\|2πΣ\|).
Public Member Functions inherited from gtsam::noiseModel::Base
	Base (size_t dim=1)
	primary constructor
virtual bool	isUnit () const
	true if a unit 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 squared Mahalanobis distance.
virtual double	loss (const Vector &v) const
	Evaluate the loss of an unwhitened residual v.
virtual Vector	unweightedWhiten (const Vector &v) const
	Useful function for robust noise models to get the unweighted but whitened error.
virtual double	weight (const Vector &v) const
	get the weight from the effective loss function on residual vector v

Static Public Member Functions
static shared_ptr	MixedSigmas (const Vector &mu, const Vector &sigmas)
	A diagonal noise model created by specifying a Vector of standard deviations, some of which might be zero.
static shared_ptr	MixedSigmas (const Vector &sigmas)
	A diagonal noise model created by specifying a Vector of standard deviations, some of which might be zero.
static shared_ptr	MixedSigmas (double m, const Vector &sigmas)
	A diagonal noise model created by specifying a Vector of standard deviations, some of which might be zero.
static shared_ptr	MixedVariances (const Vector &mu, const Vector &variances)
	A diagonal noise model created by specifying a Vector of standard deviations, some of which might be zero.
static shared_ptr	MixedVariances (const Vector &variances)
static shared_ptr	MixedPrecisions (const Vector &mu, const Vector &precisions)
	A diagonal noise model created by specifying a Vector of precisions, some of which might be inf.
static shared_ptr	MixedPrecisions (const Vector &precisions)
static shared_ptr	All (size_t dim)
	Fully constrained variations.
static shared_ptr	All (size_t dim, const Vector &mu)
	Fully constrained variations.
static shared_ptr	All (size_t dim, double mu)
	Fully constrained variations with a mu parameter.
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.
static shared_ptr	Variances (const Vector &variances, bool smart=true)
	A diagonal noise model created by specifying a Vector of variances, i.e.
static shared_ptr	Precisions (const Vector &precisions, bool smart=true)
	A diagonal noise model created by specifying a Vector of precisions, i.e.
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.
static shared_ptr	Information (const Matrix &M, bool smart=true)
	A Gaussian noise model created by specifying an information matrix.
static shared_ptr	Covariance (const Matrix &covariance, bool smart=true)
	A Gaussian noise model created by specifying a covariance matrix.

Public Types
typedef std::shared_ptr< Constrained >	shared_ptr
Public Types inherited from gtsam::noiseModel::Diagonal
typedef std::shared_ptr< Diagonal >	shared_ptr
Public Types inherited from gtsam::noiseModel::Gaussian
typedef std::shared_ptr< Gaussian >	shared_ptr
Public Types inherited from gtsam::noiseModel::Base
typedef std::shared_ptr< Base >	shared_ptr

Protected Member Functions
	Constrained (const Vector &mu, const Vector &sigmas)
	Constructor that prevents inf values from appearing in invsigmas, while preserving infinite precisions for constrained entries.
Protected Member Functions inherited from gtsam::noiseModel::Diagonal
	Diagonal (const Vector &sigmas)
	constructor to allow for disabling initialization of invsigmas
virtual double	logDetR () const override
	Compute the log of \|R\|. Used for computing log(\|Σ\|).

Protected Attributes
Vector	mu_
	Penalty function weight - needs to be large enough to dominate soft constraints.
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
std::optional< Matrix >	sqrt_information_
	Matrix square root of information matrix (R).
Protected Attributes inherited from gtsam::noiseModel::Base
size_t	dim_

Constructor & Destructor Documentation

◆ Constrained() [1/2]

gtsam::noiseModel::Constrained::Constrained	(	const Vector &	mu,
		const Vector &	sigmas )

protected

Constructor that prevents inf values from appearing in invsigmas, while preserving infinite precisions for constrained entries.

Allows for specifying mu.

◆ Constrained() [2/2]

gtsam::noiseModel::Constrained::Constrained ( const Vector & sigmas = Z_1x1 )

protected constructor takes sigmas.

prevents any inf values from appearing in invsigmas or precisions. mu set to large default value (1000.0)

Member Function Documentation

◆ informationFromA()

Matrix gtsam::noiseModel::Constrained::informationFromA ( const Matrix & A ) const

Compute A' * diag(precisions) * A using constrained precisions.

Infinite precisions yield infinite entries where the row has support.

◆ isConstrained()

bool gtsam::noiseModel::Constrained::isConstrained ( ) const

inlineoverridevirtual

true if a constrained noise mode, saves slow/clumsy dynamic casting

Reimplemented from gtsam::noiseModel::Base.

◆ print()

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

overridevirtual

Implements gtsam::noiseModel::Base.

◆ QR()

SharedDiagonal gtsam::noiseModel::Constrained::QR ( Matrix & Ab ) const

overridevirtual

Apply QR factorization to the system [A b], taking into account constraints 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

Ab	is the m*(n+1) augmented system matrix [A b]

Returns: diagonal noise model can be all zeros, mixed, or not-constrained

Reimplemented from gtsam::noiseModel::Gaussian.

◆ squaredMahalanobisDistance()

double gtsam::noiseModel::Constrained::squaredMahalanobisDistance ( const Vector & v ) const

overridevirtual

The squaredMahalanobisDistance function for a constrained noise model, for non-constrained versions, uses sigmas, otherwise uses the penalty function with mu.

Reimplemented from gtsam::noiseModel::Base.

◆ Whiten()

Matrix gtsam::noiseModel::Constrained::Whiten ( const Matrix & H ) const

overridevirtual

Whitening functions will perform partial whitening on rows with a non-zero sigma.

Other rows remain untouched.

Implements gtsam::noiseModel::Base.

◆ whiten()

Vector gtsam::noiseModel::Constrained::whiten ( const Vector & v ) const

overridevirtual

Calculates error vector with weights applied.

Implements gtsam::noiseModel::Base.

◆ WhitenInPlace() [1/2]

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

overridevirtual

In-place version.

Reimplemented from gtsam::noiseModel::Gaussian.

◆ WhitenInPlace() [2/2]

void gtsam::noiseModel::Constrained::WhitenInPlace ( Matrix & H ) const

overridevirtual

In-place version.

Reimplemented from gtsam::noiseModel::Gaussian.

◆ whitenInPlace() [1/2]

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

overridevirtual

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

Reimplemented from gtsam::noiseModel::Base.

◆ whitenInPlace() [2/2]

void gtsam::noiseModel::Constrained::whitenInPlace ( Vector & v ) const

overridevirtual

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

Reimplemented from gtsam::noiseModel::Base.

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

/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/linear/NoiseModel.h
/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/linear/NoiseModel.cpp

Detailed Description

Public Member Functions

Static Public Member Functions

Public Types

Protected Member Functions

Protected Attributes

Constructor & Destructor Documentation

◆ Constrained() [1/2]

◆ Constrained() [2/2]

Member Function Documentation

◆ informationFromA()

◆ isConstrained()

◆ print()

◆ QR()

◆ squaredMahalanobisDistance()

◆ Whiten()

◆ whiten()

◆ WhitenInPlace() [1/2]

◆ WhitenInPlace() [2/2]

◆ whitenInPlace() [1/2]

◆ whitenInPlace() [2/2]