class mrpt::poses::CPose3DPDF

Python API: mrpt.poses.CPose3DPDF

Overview

A Probability Density Function (PDF) of a SE(3) pose.

This class is just the base class for unifying many different ways this PDF can be implemented.

For convenience, a pose composition is also defined for any PDF derived class, changeCoordinatesReference, in the form of a method rather than an operator.

For a similar class for 3D points (without attitude), see CPointPDF

See also: probabilistic spatial representations

See also:

CPose3D, CPosePDF, CPointPDF

#include <mrpt/poses/CPose3DPDF.h>

class CPose3DPDF:
    public mrpt::serialization::CSerializable,
    public mrpt::math::CProbabilityDensityFunction,
    public mrpt::Stringifyable
{
public:
    // typedefs

    typedef std::shared_ptr<CPose3DPDF> Ptr;
    typedef std::shared_ptr<const CPose3DPDF> ConstPtr;

    // enums

    enum
    {
        is_3D_val = 1,
    };

    enum
    {
        is_PDF_val = 1,
    };

    // methods

    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual void printTo(std::ostream& out) const = 0;
    virtual std::string asString() const;
    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;

    template <class OPENGL_SETOFOBJECTSPTR>
    void getAs3DObject(OPENGL_SETOFOBJECTSPTR& out_obj) const;

    template <class OPENGL_SETOFOBJECTSPTR>
    OPENGL_SETOFOBJECTSPTR getAs3DObject() const;

    static CPose3DPDF* createFrom2D(const CPosePDF& o);
    static void jacobiansPoseComposition(const CPose3D& x, const CPose3D& u, mrpt::math::CMatrixDouble66& df_dx, mrpt::math::CMatrixDouble66& df_du);
    static constexpr bool is_3D();
    static constexpr bool is_PDF();
};

// direct descendants

class CPose3DPDFGaussian;
class CPose3DPDFGaussianInf;
class CPose3DPDFGrid;
class CPose3DPDFParticles;
class CPose3DPDFSOG;

Inherited Members

public:
    // typedefs

    typedef std::shared_ptr<CObject> Ptr;
    typedef std::shared_ptr<const CObject> ConstPtr;
    typedef std::unique_ptr<CObject> UniquePtr;
    typedef std::unique_ptr<const CObject> ConstUniquePtr;
    typedef std::shared_ptr<CSerializable> Ptr;
    typedef std::shared_ptr<const CSerializable> ConstPtr;
    typedef TDATA type_value;
    typedef CProbabilityDensityFunction<TDATA, STATE_LEN> self_t;
    typedef mrpt::math::CMatrixFixed<double, STATE_LEN, STATE_LEN> cov_mat_t;
    typedef cov_mat_t inf_mat_t;

    // fields

    static constexpr size_t state_length = STATE_LEN;

    // methods

    mrpt::rtti::CObject::Ptr duplicateGetSmartPtr() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    virtual CObject* clone() const = 0;
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    CProbabilityDensityFunction& operator = (const CProbabilityDensityFunction&);
    CProbabilityDensityFunction& operator = (CProbabilityDensityFunction&&);
    virtual void getMean(type_value& mean_point) const = 0;
    virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const = 0;
    virtual void getCovarianceAndMean(cov_mat_t& c, TDATA& mean) const;
    void getCovarianceDynAndMean(mrpt::math::CMatrixDouble& cov, type_value& mean_point) const;
    type_value getMeanVal() const;
    void getCovariance(mrpt::math::CMatrixDouble& cov) const;
    void getCovariance(cov_mat_t& cov) const;
    cov_mat_t getCovariance() const;
    virtual bool isInfType() const;
    virtual void getInformationMatrix(inf_mat_t& inf) const;
    virtual bool saveToTextFile(const std::string& file) const = 0;
    virtual void drawSingleSample(TDATA& outPart) const = 0;
    virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const;
    double getCovarianceEntropy() const;
    virtual std::string asString() const = 0;

Methods

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

Returns information about the class of an object in runtime.

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

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

Derived classes must override this method.

virtual std::string asString() const

Returns a human-readable string representation of this PDF.

virtual void copyFrom(const CPose3DPDF& o) = 0

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

See also:

createFrom2D

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

Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently only distributions of the same class can be fused! eg, gaussian with gaussian,etc)

virtual void inverse(CPose3DPDF& o) const = 0

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

template <class OPENGL_SETOFOBJECTSPTR>
void getAs3DObject(OPENGL_SETOFOBJECTSPTR& out_obj) const

Returns a 3D representation of this PDF (it doesn’t clear the current contents of out_obj, but append new OpenGL objects to that list)

Needs the mrpt-opengl library, and using mrpt::viz::CSetOfObjects::Ptr as template argument.

By default, ellipsoids for the confidence intervals of “q=3” are drawn; for more mathematical details, see CGeneralizedEllipsoidTemplate::setQuantiles()

See also:

mrpt::viz::CSetOfObjects::posePDF2opengl() for details on pose-to-opengl conversion

template <class OPENGL_SETOFOBJECTSPTR>
OPENGL_SETOFOBJECTSPTR getAs3DObject() const

Returns a 3D representation of this PDF.

Needs the mrpt-opengl library, and using mrpt::viz::CSetOfObjects::Ptr as template argument.

static CPose3DPDF* createFrom2D(const CPosePDF& o)

This is a static transformation method from 2D poses to 3D PDFs, preserving the representation type (particles->particles, Gaussians->Gaussians,etc)

It returns a new object of any of the derived classes of CPose3DPDF. This object must be deleted by the user when not required anymore.

Gaussians get zero covariance for (z,pitch,roll), i.e. these are known exactly, whereas information-form Gaussians get zero information for them (unknown), which gives a singular information matrix.

See also:

copyFrom

static void jacobiansPoseComposition(const CPose3D& x, const CPose3D& u, mrpt::math::CMatrixDouble66& df_dx, mrpt::math::CMatrixDouble66& df_du)

This static method computes the pose composition Jacobians.

See [2] (https://arxiv.org/abs/2103.15980), section 5.1.2, Eqs. (5.1)-(5.4).

Direct equations (for the covariances) in yaw-pitch-roll are too complex. Make a way around them and consider instead this path:

X(6D)       U(6D)
  |           |
  v           v
X(7D)       U(7D)
  |           |
  +--- (+) ---+
        |
        v
      RES(7D)
        |
        v
      RES(6D)