class mrpt::poses::CPose3DPDFGaussian

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

This class implements that PDF using a mono-modal Gaussian distribution. See mrpt::poses::CPose3DPDF for more details.

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

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

#include <mrpt/poses/CPose3DPDFGaussian.h>

class CPose3DPDFGaussian:
    public mrpt::poses::CPose3DPDF,
    public mrpt::Stringifyable
{
public:
    // fields

    CPose3D mean;
    mrpt::math::CMatrixDouble66 cov;

    // construction

    CPose3DPDFGaussian();
    CPose3DPDFGaussian(const CPose3D& init_Mean);
    CPose3DPDFGaussian(TConstructorFlags_Poses constructor_dummy_param);
    CPose3DPDFGaussian(const CPose3D& init_Mean, const mrpt::math::CMatrixDouble66& init_Cov);
    CPose3DPDFGaussian(const CPosePDFGaussian& o);
    CPose3DPDFGaussian(const CPose3DQuatPDFGaussian& o);

    // methods

    const CPose3D& getPoseMean() const;
    CPose3D& getPoseMean();
    void getMean(CPose3D& mean_pose) const;
    virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const;
    virtual std::string asString() 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;
    CPose3DPDFGaussian operator - () const;
    void operator += (const CPose3D& Ap);
    void operator += (const CPose3DPDFGaussian& Ap);
    void operator -= (const CPose3DPDFGaussian& Ap);
    double evaluatePDF(const CPose3D& x) const;
    double evaluateNormalizedPDF(const CPose3D& x) const;
    double mahalanobisDistanceTo(const CPose3DPDFGaussian& theOther);
    void getCovSubmatrix2D(mrpt::math::CMatrixDouble& out_cov) const;
};

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

The 6x6 covariance matrix.

Construction

CPose3DPDFGaussian()

Default constructor.

CPose3DPDFGaussian(const CPose3D& init_Mean)

Constructor.

CPose3DPDFGaussian(TConstructorFlags_Poses constructor_dummy_param)

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

CPose3DPDFGaussian(const CPose3D& init_Mean, const mrpt::math::CMatrixDouble66& init_Cov)

Constructor

CPose3DPDFGaussian(const CPosePDFGaussian& o)

Constructor from a Gaussian 2D pose PDF (sets to 0 the missing variables z,pitch, and roll).

CPose3DPDFGaussian(const CPose3DQuatPDFGaussian& o)

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

Methods

void getMean(CPose3D& mean_pose) const

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

See also:

getMean

virtual std::string asString() const

Returns a human-friendly textual description of the object.

For classes with a large/complex internal state, only a summary should be returned instead of the exhaustive enumeration of all data members.

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.

CPose3DPDFGaussian 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 CPose3DPDFGaussian& Ap)

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

void operator -= (const CPose3DPDFGaussian& 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].

double mahalanobisDistanceTo(const CPose3DPDFGaussian& 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.

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

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