class mrpt::poses::CPose3DPDFSOG

Overview

Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose \(p(\mathbf{x}) = [x ~ y ~ z ~ yaw ~ pitch ~ roll]^t\).

This class implements that PDF as the following multi-modal Gaussian distribution:

\(p(\mathbf{x}) = \sum\limits_{i=1}^N \omega^i \mathcal{N}( \mathbf{x} ; \bar{\mathbf{x}}^i, \mathbf{\Sigma}^i )\)

Where the number of modes N is the size of CPose3DPDFSOG::m_modes. Angles are always in radians.

See mrpt::poses::CPose3DPDF for more details.

See also:

CPose3DPDF

#include <mrpt/poses/CPose3DPDFSOG.h>

class CPose3DPDFSOG: public mrpt::poses::CPose3DPDF
{
public:
    // typedefs

    typedef std::shared_ptr<mrpt::poses ::CPose3DPDFSOG> Ptr;
    typedef std::shared_ptr<const mrpt::poses ::CPose3DPDFSOG> ConstPtr;
    typedef std::unique_ptr<mrpt::poses ::CPose3DPDFSOG> UniquePtr;
    typedef std::unique_ptr<const mrpt::poses ::CPose3DPDFSOG> ConstUniquePtr;
    typedef std::vector<TGaussianMode, mrpt::aligned_allocator_cpp11<TGaussianMode>> TModesList;
    typedef TModesList::const_iterator const_iterator;
    typedef TModesList::iterator iterator;

    // structs

    struct TGaussianMode;

    // fields

    static constexpr const char* className = "mrpt::poses" "::" "CPose3DPDFSOG";

    // construction

    CPose3DPDFSOG(size_t nModes = 1);

    // 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;
    void clear();
    void resize(size_t N);
    size_t size() const;
    bool empty() const;
    iterator begin();
    iterator end();
    const_iterator begin() const;
    const_iterator end() const;
    void getMean(CPose3D& mean_pose) const;
    CPose3D meanVal() const;
    virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const;
    void normalizeWeights();
    void getMostLikelyMode(CPose3DPDFGaussian& outVal) const;
    CPose3DPDFGaussian getMostLikelyMode() const;
    virtual void copyFrom(const CPose3DPDF& o);
    virtual bool saveToTextFile(const std::string& file) const;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase);
    virtual void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2);
    void drawSingleSample(CPose3D& outPart) const;
    virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const;
    virtual void inverse(CPose3DPDF& o) const;
    void appendFrom(const CPose3DPDFSOG& o);
    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::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;
    typedef std::shared_ptr<CPose3DPDF> Ptr;
    typedef std::shared_ptr<const CPose3DPDF> ConstPtr;

    // enums

    enum
    {
        is_3D_val = 1,
    };

    enum
    {
        is_PDF_val = 1,
    };

    // 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;
    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();

Typedefs

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

A type for the associated smart pointer.

Construction

CPose3DPDFSOG(size_t nModes = 1)

Default constructor.

Parameters:

nModes

The initial size of CPose3DPDFSOG::m_modes

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.

void clear()

Clear all the gaussian modes.

void resize(size_t N)

Set the number of SOG modes.

size_t size() const

Return the number of Gaussian modes.

bool empty() const

Return whether there is any Gaussian mode.

CPose3D 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

void normalizeWeights()

Normalize the weights in m_modes such as the maximum log-weight is 0.

void getMostLikelyMode(CPose3DPDFGaussian& outVal) const

Return the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in this SOG).

Deprecated Use getMostLikelyMode() returning by value instead.

CPose3DPDFGaussian getMostLikelyMode() const

Returns the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in this SOG).

virtual 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 the density to a text file, with the following format: There is one row per Gaussian “mode”, and each row contains 10 elements:

  • w (The linear weight)

  • x_mean (gaussian mean value)

  • y_mean (gaussian mean value)

  • x_mean (gaussian mean value)

  • yaw_mean (gaussian mean value, in radians)

  • pitch_mean (gaussian mean value, in radians)

  • roll_mean (gaussian mean value, in radians)

  • C11,C22,C33,C44,C55,C66 (Covariance elements)

  • C12,C13,C14,C15,C16 (Covariance elements)

  • C23,C24,C25,C25 (Covariance elements)

  • C34,C35,C36 (Covariance elements)

  • C45,C46 (Covariance elements)

  • C56 (Covariance elements)

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.

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

Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently p1 must be a mrpt::poses::CPose3DPDFSOG object and p2 a mrpt::poses::CPose3DPDFSOG 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,z,yaw,pitch,roll) datum.

virtual void inverse(CPose3DPDF& o) const

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

void appendFrom(const CPose3DPDFSOG& o)

Append the Gaussian modes from “o” to the current set of modes of “this” density.

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.