class mrpt::poses::CPosePDFGaussian

Overview

Declares a class that represents a Probability Density function (PDF) of a 2D pose \(p(\mathbf{x}) = [x ~ y ~ \phi ]^t\).

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

See also:

CPose2D, CPosePDF, CPosePDFParticles

#include <mrpt/poses/CPosePDFGaussian.h>

class CPosePDFGaussian: public mrpt::poses::CPosePDF
{
public:
    // typedefs

    typedef std::shared_ptr<mrpt::poses ::CPosePDFGaussian> Ptr;
    typedef std::shared_ptr<const mrpt::poses ::CPosePDFGaussian> ConstPtr;
    typedef std::unique_ptr<mrpt::poses ::CPosePDFGaussian> UniquePtr;
    typedef std::unique_ptr<const mrpt::poses ::CPosePDFGaussian> ConstUniquePtr;

    // fields

    static constexpr const char* className = "mrpt::poses" "::" "CPosePDFGaussian";
    CPose2D mean;
    mrpt::math::CMatrixDouble33 cov;

    // construction

    CPosePDFGaussian();
    CPosePDFGaussian(const CPose2D& init_Mean);
    CPosePDFGaussian(const CPose2D& init_Mean, const mrpt::math::CMatrixDouble33& init_Cov);
    CPosePDFGaussian(const CPosePDF& o);
    CPosePDFGaussian(const CPose3DPDF& o);

    // methods

    static constexpr auto getClassName();
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    static std::shared_ptr<CObject> CreateObject();

    template <typename... Args>
    static Ptr Create(Args&&... args);

    template <typename Alloc, typename... Args>
    static Ptr CreateAlloc(
        const Alloc& alloc,
        Args&&... args
        );

    template <typename... Args>
    static UniquePtr CreateUnique(Args&&... args);

    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    virtual mrpt::rtti::CObject* clone() const;
    const CPose2D& getPoseMean() const;
    CPose2D& getPoseMean();
    void getMean(CPose2D& mean_pose) const;
    CPose2D meanVal() const;
    virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() 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 CPosePDFGaussian& x, const CPosePDFGaussian& ref);
    void inverseComposition(const CPosePDFGaussian& x1, const CPosePDFGaussian& 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 CPosePDFGaussian& theOther);
    void assureMinCovariance(double minStdXY, double minStdPhi);
    void operator += (const CPosePDFGaussian& Ap);
    void operator -= (const CPosePDFGaussian& ref);
    void composePoint(const mrpt::math::TPoint2D& l, CPoint2DPDFGaussian& g) const;
    virtual void printTo(std::ostream& out) const;
};

Inherited Members

public:
    // typedefs

    typedef std::shared_ptr<CObject> Ptr;
    typedef std::shared_ptr<const CObject> ConstPtr;
    typedef std::shared_ptr<CSerializable> Ptr;
    typedef std::shared_ptr<const CSerializable> ConstPtr;
    typedef CProbabilityDensityFunction<TDATA, STATE_LEN> self_t;
    typedef std::shared_ptr<CPosePDF> Ptr;
    typedef std::shared_ptr<const CPosePDF> ConstPtr;

    // methods

    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    CProbabilityDensityFunction& operator = (const CProbabilityDensityFunction&);
    CProbabilityDensityFunction& operator = (CProbabilityDensityFunction&&);
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual void printTo(std::ostream& out) const = 0;
    virtual void copyFrom(const CPosePDF& o) = 0;
    virtual void bayesianFusion(const CPosePDF& p1, const CPosePDF& p2, double minMahalanobisDistToDrop = 0) = 0;
    virtual void inverse(CPosePDF& o) const = 0;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase) = 0;

Typedefs

typedef std::shared_ptr<mrpt::poses ::CPosePDFGaussian> Ptr

A type for the associated smart pointer.

Fields

CPose2D mean

The mean value.

mrpt::math::CMatrixDouble33 cov

The 3x3 covariance matrix.

Construction

CPosePDFGaussian()

Default constructor.

CPosePDFGaussian(const CPose2D& init_Mean)

Constructor.

CPosePDFGaussian(const CPose2D& init_Mean, const mrpt::math::CMatrixDouble33& init_Cov)

Constructor.

CPosePDFGaussian(const CPosePDF& o)

Copy constructor, including transformations between other PDFs.

CPosePDFGaussian(const CPose3DPDF& o)

Copy constructor, including transformations between other PDFs.

Methods

virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const

Returns information about the class of an object in runtime.

virtual mrpt::rtti::CObject* clone() const

Returns a deep copy (clone) of the object, indepently of its class.

CPose2D meanVal() const

Returns the mean pose by value (convenience non-virtual overload).

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

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 CPosePDFGaussian& x, const CPosePDFGaussian& 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 CPosePDFGaussian& x1,
    const CPosePDFGaussian& 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 CPosePDFGaussian& theOther)

Computes the Mahalanobis distance between the centers of two Gaussians.

void assureMinCovariance(double minStdXY, double minStdPhi)

Substitutes the diagonal elements if (square) they are below some given minimum values (Use this before bayesianFusion, for example, to avoid inversion of singular matrixes, etc…)

void operator += (const CPosePDFGaussian& 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 CPosePDFGaussian& ref)

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

void composePoint(const mrpt::math::TPoint2D& l, CPoint2DPDFGaussian& g) const

Returns the PDF of the 2D point \(g = q \oplus l\) with “q”=this pose and “l” a point without uncertainty.

virtual void printTo(std::ostream& out) const

Write a human-readable description of this PDF to the given stream.

Derived classes must override this method.