class mrpt::poses::CPosePDFGaussianInf

Overview

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:

CPose2D, CPosePDF, CPosePDFParticles

#include <mrpt/poses/CPosePDFGaussianInf.h>

class CPosePDFGaussianInf: public mrpt::poses::CPosePDF
{
public:
    // fields

    CPose2D mean;
    mrpt::math::CMatrixDouble33 cov_inv;

    // construction

    CPosePDFGaussianInf();
    CPosePDFGaussianInf(const CPose2D& init_Mean);
    CPosePDFGaussianInf(const CPose2D& init_Mean, const mrpt::math::CMatrixDouble33& init_CovInv);
    CPosePDFGaussianInf(const CPosePDF& o);
    CPosePDFGaussianInf(const CPose3DPDF& o);

    // methods

    const CPose2D& getPoseMean() const;
    CPose2D& getPoseMean();
    void getMean(CPose2D& mean_pose) const;
    virtual bool isInfType() const;
    virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const;
    void getInformationMatrix(mrpt::math::CMatrixDouble33& inf) const;
    virtual void copyFrom(const CPosePDF& o);
    void copyFrom(const CPose3DPDF& o);
    virtual bool saveToTextFile(const std::string& file) const;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase);
    void changeCoordinatesReference(const CPose2D& newReferenceBase);
    void rotateCov(const double ang);
    void inverseComposition(const CPosePDFGaussianInf& x, const CPosePDFGaussianInf& ref);

    void inverseComposition(
        const CPosePDFGaussianInf& x1,
        const CPosePDFGaussianInf& x0,
        const mrpt::math::CMatrixDouble33& COV_01
        );

    void drawSingleSample(CPose2D& outPart) const;
    virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const;
    virtual void bayesianFusion(const CPosePDF& p1, const CPosePDF& p2, const double minMahalanobisDistToDrop = 0);
    virtual void inverse(CPosePDF& o) const;
    void operator += (const CPose2D& Ap);
    double evaluatePDF(const CPose2D& x) const;
    double evaluateNormalizedPDF(const CPose2D& x) const;
    double mahalanobisDistanceTo(const CPosePDFGaussianInf& theOther);
    void operator += (const CPosePDFGaussianInf& Ap);
    void operator -= (const CPosePDFGaussianInf& ref);
};

Inherited Members

public:
    // typedefs

    typedef CProbabilityDensityFunction<TDATA, STATE_LEN> self_t;

    // methods

    virtual void copyFrom(const CPosePDF& o) = 0;
    virtual void bayesianFusion(const CPosePDF& p1, const CPosePDF& p2, const double minMahalanobisDistToDrop = 0) = 0;
    virtual void inverse(CPosePDF& o) const = 0;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase) = 0;

Fields

CPose2D mean

The mean value.

mrpt::math::CMatrixDouble33 cov_inv

The inverse of the 3x3 covariance matrix (the “information” matrix)

Construction

CPosePDFGaussianInf()

Default constructor (mean=all zeros, inverse covariance=all zeros -> so be careful!)

CPosePDFGaussianInf(const CPose2D& init_Mean)

Constructor with a mean value (inverse covariance=all zeros -> so be careful!)

CPosePDFGaussianInf(const CPose2D& init_Mean, const mrpt::math::CMatrixDouble33& init_CovInv)

Constructor.

CPosePDFGaussianInf(const CPosePDF& o)

Copy constructor, including transformations between other PDFs.

CPosePDFGaussianInf(const CPose3DPDF& o)

Copy constructor, including transformations between other PDFs.

Methods

void getMean(CPose2D& mean_pose) const

Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).

See also:

getCovariance

virtual bool isInfType() const

Returns whether the class instance holds the uncertainty in covariance or information form.

By default this is going to be covariance form. *Inf classes (e.g. CPosePDFGaussianInf) store it in information form.

See also:

mrpt::traits::is_inf_type

virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const

Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once.

See also:

getMean, getInformationMatrix

void getInformationMatrix(mrpt::math::CMatrixDouble33& inf) const

Returns the information (inverse covariance) matrix (a STATE_LEN x STATE_LEN matrix)

See also:

getMean, getCovarianceAndMean

virtual void copyFrom(const CPosePDF& o)

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)

virtual bool saveToTextFile(const std::string& file) const

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)

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

Draws a single sample from the distribution.

virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const

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
    )

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

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

void operator -= (const CPosePDFGaussianInf& ref)

Makes: thisPDF = thisPDF - Ap, where “-” is pose inverse composition (both the mean, and the covariance matrix are updated)