#include <NoiseModel.h>
Public Types | |
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 |
Public Member Functions | |
Diagonal () | |
double | invsigma (size_t i) const |
const Vector & | invsigmas () const |
double | precision (size_t i) const |
const Vector & | precisions () const |
void | print (const std::string &name) const override |
Matrix | R () const override |
double | sigma (size_t i) const |
Vector | sigmas () const override |
Calculate standard deviations. More... | |
Vector | unwhiten (const Vector &v) const override |
Unwhiten an error vector. More... | |
Matrix | Whiten (const Matrix &H) const override |
Vector | whiten (const Vector &v) const override |
Whiten an error vector. More... | |
void | WhitenInPlace (Eigen::Block< Matrix > H) const override |
void | WhitenInPlace (Matrix &H) const override |
~Diagonal () override | |
Public Member Functions inherited from gtsam::noiseModel::Gaussian | |
virtual Matrix | covariance () const |
Compute covariance matrix. More... | |
bool | equals (const Base &expected, double tol=1e-9) const override |
Gaussian (size_t dim=1, const std::optional< Matrix > &sqrt_information={}) | |
virtual Matrix | information () const |
Compute information matrix. More... | |
double | logDeterminant () const |
Compute the log of |Σ|. More... | |
double | negLogConstant () const |
Compute the negative log of the normalization constant for a Gaussian noise model k = 1/\sqrt(|2πΣ|). More... | |
virtual std::shared_ptr< Diagonal > | QR (Matrix &Ab) const |
void | WhitenSystem (Matrix &A, Vector &b) const override |
void | WhitenSystem (Matrix &A1, Matrix &A2, Matrix &A3, Vector &b) const override |
void | WhitenSystem (Matrix &A1, Matrix &A2, Vector &b) const override |
void | WhitenSystem (std::vector< Matrix > &A, 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 (Eigen::Block< Vector > &v) const |
virtual void | unwhitenInPlace (Vector &v) const |
virtual double | weight (const Vector &v) const |
virtual void | whitenInPlace (Eigen::Block< Vector > &v) const |
virtual void | whitenInPlace (Vector &v) const |
virtual | ~Base () |
Static Public Member Functions | |
static shared_ptr | Precisions (const Vector &precisions, bool smart=true) |
static shared_ptr | Sigmas (const Vector &sigmas, bool smart=true) |
static shared_ptr | Variances (const Vector &variances, bool smart=true) |
Static Public Member Functions inherited from gtsam::noiseModel::Gaussian | |
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 | |
Diagonal (const Vector &sigmas) | |
virtual double | logDetR () const override |
Compute the log of |R|. Used for computing log(|Σ|) More... | |
Protected Attributes | |
Vector | invsigmas_ |
Vector | precisions_ |
Vector | sigmas_ |
Protected Attributes inherited from gtsam::noiseModel::Gaussian | |
std::optional< Matrix > | sqrt_information_ |
Protected Attributes inherited from gtsam::noiseModel::Base | |
size_t | dim_ |
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...
Definition at line 301 of file NoiseModel.h.
typedef std::shared_ptr<Diagonal> gtsam::noiseModel::Diagonal::shared_ptr |
Definition at line 321 of file NoiseModel.h.
|
protected |
constructor to allow for disabling initialization of invsigmas
Definition at line 275 of file NoiseModel.cpp.
gtsam::noiseModel::Diagonal::Diagonal | ( | ) |
constructor - no initializations, for serialization
Definition at line 272 of file NoiseModel.cpp.
|
inlineoverride |
Definition at line 323 of file NoiseModel.h.
|
inline |
Definition at line 362 of file NoiseModel.h.
|
inline |
Return sqrt precisions
Definition at line 361 of file NoiseModel.h.
|
overrideprotectedvirtual |
Compute the log of |R|. Used for computing log(|Σ|)
Reimplemented from gtsam::noiseModel::Gaussian.
Reimplemented in gtsam::noiseModel::Unit, and gtsam::noiseModel::Isotropic.
Definition at line 343 of file NoiseModel.cpp.
|
inline |
Definition at line 368 of file NoiseModel.h.
|
inline |
Return precisions
Definition at line 367 of file NoiseModel.h.
|
static |
A diagonal noise model created by specifying a Vector of precisions, i.e. i.e. the diagonal of the information matrix, i.e., weights
Definition at line 311 of file NoiseModel.cpp.
|
overridevirtual |
Reimplemented from gtsam::noiseModel::Gaussian.
Reimplemented in gtsam::noiseModel::Unit, gtsam::noiseModel::Isotropic, and gtsam::noiseModel::Constrained.
Definition at line 317 of file NoiseModel.cpp.
|
inlineoverridevirtual |
Return R itself, but note that Whiten(H) is cheaper than R*H
Reimplemented from gtsam::noiseModel::Gaussian.
Definition at line 373 of file NoiseModel.h.
|
inline |
Return standard deviations (sqrt of diagonal)
Definition at line 356 of file NoiseModel.h.
|
inlineoverridevirtual |
Calculate standard deviations.
Reimplemented from gtsam::noiseModel::Gaussian.
Definition at line 346 of file NoiseModel.h.
|
static |
A diagonal noise model created by specifying a Vector of sigmas, i.e. standard deviations, the diagonal of the square root covariance matrix.
Definition at line 291 of file NoiseModel.cpp.
Unwhiten an error vector.
Reimplemented from gtsam::noiseModel::Gaussian.
Reimplemented in gtsam::noiseModel::Unit, and gtsam::noiseModel::Isotropic.
Definition at line 326 of file NoiseModel.cpp.
|
static |
A diagonal noise model created by specifying a Vector of variances, i.e. i.e. the diagonal of the covariance matrix.
variances | A vector containing the variances of this noise model |
smart | check if can be simplified to derived class |
Definition at line 283 of file NoiseModel.cpp.
Multiply a derivative with R (derivative of whiten) Equivalent to whitening each column of the input matrix.
Reimplemented from gtsam::noiseModel::Gaussian.
Reimplemented in gtsam::noiseModel::Unit, gtsam::noiseModel::Isotropic, and gtsam::noiseModel::Constrained.
Definition at line 330 of file NoiseModel.cpp.
Whiten an error vector.
Reimplemented from gtsam::noiseModel::Gaussian.
Reimplemented in gtsam::noiseModel::Unit, gtsam::noiseModel::Isotropic, and gtsam::noiseModel::Constrained.
Definition at line 322 of file NoiseModel.cpp.
|
overridevirtual |
In-place version
Reimplemented from gtsam::noiseModel::Gaussian.
Reimplemented in gtsam::noiseModel::Unit, gtsam::noiseModel::Isotropic, and gtsam::noiseModel::Constrained.
Definition at line 338 of file NoiseModel.cpp.
|
overridevirtual |
In-place version
Reimplemented from gtsam::noiseModel::Gaussian.
Reimplemented in gtsam::noiseModel::Isotropic, gtsam::noiseModel::Constrained, and gtsam::noiseModel::Unit.
Definition at line 334 of file NoiseModel.cpp.
|
protected |
Definition at line 309 of file NoiseModel.h.
|
protected |
Definition at line 309 of file NoiseModel.h.
|
protected |
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.
Definition at line 309 of file NoiseModel.h.