class mrpt::poses::CPose3DPDFGaussianInf

Overview

Declares a class that represents a Probability Density function (PDF) of a 3D pose \(p(\mathbf{x}) = [x ~ y ~ z ~ yaw ~ pitch ~ roll]^t\) as a Gaussian described by its mean and its inverse covariance matrix.

This class implements that PDF using a mono-modal Gaussian distribution in “information” form (inverse covariance matrix).

Uncertainty of pose composition operations (\(y = x \oplus u\)) is implemented in the method “CPose3DPDFGaussianInf::operator+=”.

Read also: “A tutorial on SE(3) transformation parameterizations and on-manifold optimization”, in [4]

See also:

CPose3D, CPose3DPDF, CPose3DPDFParticles, CPose3DPDFGaussian

#include <mrpt/poses/CPose3DPDFGaussianInf.h>

class CPose3DPDFGaussianInf: public mrpt::poses::CPose3DPDF
{
public:
    // fields

    CPose3D mean;
    mrpt::math::CMatrixDouble66 cov_inv;

    // construction

    CPose3DPDFGaussianInf();
    CPose3DPDFGaussianInf(const CPose3D& init_Mean);
    CPose3DPDFGaussianInf(TConstructorFlags_Poses constructor_dummy_param);
    CPose3DPDFGaussianInf(const CPose3D& init_Mean, const mrpt::math::CMatrixDouble66& init_CovInv);
    CPose3DPDFGaussianInf(const CPose3DQuatPDFGaussian& o);

    // methods

    const CPose3D& getPoseMean() const;
    CPose3D& getPoseMean();
    void getMean(CPose3D& mean_pose) const;
    virtual bool isInfType() const;
    virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const;
    void getInformationMatrix(mrpt::math::CMatrixDouble66& inf) const;
    virtual void copyFrom(const CPose3DPDF& o);
    void copyFrom(const CPosePDF& o);
    void copyFrom(const CPose3DQuatPDFGaussian& o);
    virtual bool saveToTextFile(const std::string& file) const;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase);
    void drawSingleSample(CPose3D& outPart) const;
    virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const;
    virtual void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2);
    virtual void inverse(CPose3DPDF& o) const;
    CPose3DPDFGaussianInf operator - () const;
    void operator += (const CPose3D& Ap);
    void operator += (const CPose3DPDFGaussianInf& Ap);
    void operator -= (const CPose3DPDFGaussianInf& Ap);
    double evaluatePDF(const CPose3D& x) const;
    double evaluateNormalizedPDF(const CPose3D& x) const;
    void getInvCovSubmatrix2D(mrpt::math::CMatrixDouble& out_cov) const;
    double mahalanobisDistanceTo(const CPose3DPDFGaussianInf& theOther);
};

Inherited Members

public:
    // typedefs

    typedef CProbabilityDensityFunction<TDATA, STATE_LEN> self_t;

    // methods

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

Fields

CPose3D mean

The mean value.

mrpt::math::CMatrixDouble66 cov_inv

The inverse of the 6x6 covariance matrix.

Construction

CPose3DPDFGaussianInf()

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

CPose3DPDFGaussianInf(const CPose3D& init_Mean)

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

CPose3DPDFGaussianInf(TConstructorFlags_Poses constructor_dummy_param)

Uninitialized constructor: leave all fields uninitialized - Call with UNINITIALIZED_POSE as argument.

CPose3DPDFGaussianInf(const CPose3D& init_Mean, const mrpt::math::CMatrixDouble66& init_CovInv)

Constructor with mean and inv cov.

CPose3DPDFGaussianInf(const CPose3DQuatPDFGaussian& o)

Constructor from a 6D pose PDF described as a Quaternion.

Methods

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::CMatrixDouble66& inf) const

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

See also:

getMean, getCovarianceAndMean

virtual void copyFrom(const CPose3DPDF& o)

Copy operator, translating if necesary (for example, between particles and gaussian representations)

void copyFrom(const CPosePDF& o)

Copy operator, translating if necesary (for example, between particles and gaussian representations)

void copyFrom(const CPose3DQuatPDFGaussian& o)

Copy from a 6D pose PDF described as a Quaternion.

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

Save the PDF to a text file, containing the 3D 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 drawSingleSample(CPose3D& 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 1x6 vectors, where each row contains a (x,y,phi) datum.

virtual void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2)

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(CPose3DPDF& o) const

Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.

CPose3DPDFGaussianInf operator - () const

Unary - operator, returns the PDF of the inverse pose.

void operator += (const CPose3D& Ap)

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

void operator += (const CPose3DPDFGaussianInf& Ap)

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

void operator -= (const CPose3DPDFGaussianInf& Ap)

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

double evaluatePDF(const CPose3D& x) const

Evaluates the PDF at a given point.

double evaluateNormalizedPDF(const CPose3D& x) const

Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].

void getInvCovSubmatrix2D(mrpt::math::CMatrixDouble& out_cov) const

Returns a 3x3 matrix with submatrix of the inverse covariance for the variables (x,y,yaw) only.

double mahalanobisDistanceTo(const CPose3DPDFGaussianInf& theOther)

Computes the Mahalanobis distance between the centers of two Gaussians.

The variables with a variance exactly equal to 0 are not taken into account in the process, but “infinity” is returned if the corresponding elements are not exactly equal.