Class CPosePDFGaussianInf

Defined in File CPosePDFGaussianInf.h

Inheritance Relationships

Base Type

public mrpt::poses::CPosePDF (Class CPosePDF)

Class Documentation

class CPosePDFGaussianInf : public mrpt::poses::CPosePDF 

A Probability Density function (PDF) of a 2D pose \( p(\mathbf{x}) = [x ~ y ~ \phi ]^t \) as a Gaussian with a mean and the inverse of the covariance.

This class implements a PDF as a mono-modal Gaussian distribution in its information form, that is, keeping the inverse of the covariance matrix instead of the covariance matrix itself.

This class is the dual of CPosePDFGaussian.

See also

getMean, getCovarianceAndMean

virtual void copyFrom(const CPosePDF &o) override: Copy operator, translating if necessary (for example, between particles and gaussian representations)

void copyFrom(const CPose3DPDF &o): Copy operator, translating if necessary (for example, between particles and gaussian representations)

bool saveToTextFile(const std::string &file) const override: Save PDF’s particles to a text file, containing the 2D pose in the first line, then the covariance matrix in next 3 lines.

virtual void changeCoordinatesReference(const CPose3D &newReferenceBase) override: this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which “to project” the current pdf. Result PDF substituted the currently stored one in the object

void changeCoordinatesReference(const CPose2D &newReferenceBase): this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which “to project” the current pdf. Result PDF substituted the currently stored one in the object.

void rotateCov(const double ang): Rotate the covariance matrix by replacing it by \( \mathbf{R}~\mathbf{COV}~\mathbf{R}^t \), where \( \mathbf{R} = \left[ \begin{array}{ccc} \cos\alpha & -\sin\alpha & 0 \\ \sin\alpha & \cos\alpha & 0 \\ 0 & 0 & 1 \end{array}\right] \).

void inverseComposition(const CPosePDFGaussianInf &x, const CPosePDFGaussianInf &ref): Set \( this = x1 \ominus x0 \) , computing the mean using the “-” operator and the covariances through the corresponding Jacobians (For ‘x0’ and ‘x1’ being independent variables!).

void inverseComposition(const CPosePDFGaussianInf &x1, const CPosePDFGaussianInf &x0, const mrpt::math::CMatrixDouble33 &COV_01): Set \( this = x1 \ominus x0 \) , computing the mean using the “-” operator and the covariances through the corresponding Jacobians (Given the 3x3 cross-covariance matrix of variables x0 and x1).

void drawSingleSample(CPose2D &outPart) const override: Draws a single sample from the distribution

void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble> &outSamples) const override: Draws a number of samples from the distribution, and saves as a list of 1x3 vectors, where each row contains a (x,y,phi) datum.

virtual void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double minMahalanobisDistToDrop = 0) override

Bayesian fusion of two points gauss. distributions, then save the result in this object. The process is as follows:

(x1,S1): Mean and variance of the p1 distribution.
(x2,S2): Mean and variance of the p2 distribution.
(x,S): Mean and variance of the resulting distribution.

\( S = (S_1^{-1} + S_2^{-1})^{-1} \) \( x = S ( S_1^{-1} x_1 + S_2^{-1} x_2 ) \)

virtual void inverse(CPosePDF &o) const override: Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF

void operator+=(const CPose2D &Ap): Makes: thisPDF = thisPDF + Ap, where “+” is pose composition (both the mean, and the covariance matrix are updated).

double evaluatePDF(const CPose2D &x) const: Evaluates the PDF at a given point

double evaluateNormalizedPDF(const CPose2D &x) const: Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].

double mahalanobisDistanceTo(const CPosePDFGaussianInf &theOther): Computes the Mahalanobis distance between the centers of two Gaussians.

void operator+=(const CPosePDFGaussianInf &Ap): Makes: thisPDF = thisPDF + Ap, where “+” is pose composition (both the mean, and the covariance matrix are updated) (see formulas in jacobiansPoseComposition ).

inline void operator-=(const CPosePDFGaussianInf &ref): Makes: thisPDF = thisPDF - Ap, where “-” is pose inverse composition (both the mean, and the covariance matrix are updated)

virtual void printTo(std::ostream &out) const override: Write a human-readable description of this PDF to the given stream. Derived classes must override this method.

Protected Functions

void enforceCovSymmetry(): Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!)