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:
    // typedefs

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

    // fields

    static constexpr const char* className = "mrpt::poses" "::" "CPose3DPDFGaussianInf";
    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

    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 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);
    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<CPose3DPDF> Ptr;
    typedef std::shared_ptr<const CPose3DPDF> 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 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;

Typedefs

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

A type for the associated smart pointer.

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

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 necessary (for example, between particles and gaussian representations)

void copyFrom(const CPosePDF& o)

Copy operator, translating if necessary (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.

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.