class mrpt::poses::CPointPDFSOG

Overview

Declares a class that represents a Probability Density function (PDF) of a 3D point \(p(\mathbf{x}) = [x ~ y ~ z ]^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 CPointPDFSOG::m_modes

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

See also:

CPointPDF, CPosePDF,

#include <mrpt/poses/CPointPDFSOG.h>

class CPointPDFSOG: public mrpt::poses::CPointPDF
{
public:
    // typedefs

    typedef std::shared_ptr<mrpt::poses ::CPointPDFSOG> Ptr;
    typedef std::shared_ptr<const mrpt::poses ::CPointPDFSOG> ConstPtr;
    typedef std::unique_ptr<mrpt::poses ::CPointPDFSOG> UniquePtr;
    typedef std::unique_ptr<const mrpt::poses ::CPointPDFSOG> ConstUniquePtr;
    typedef std::deque<TGaussianMode> CListGaussianModes;
    typedef std::deque<TGaussianMode>::const_iterator const_iterator;
    typedef std::deque<TGaussianMode>::iterator iterator;

    // enums

    enum
    {
        is_3D_val = 1,
    };

    enum
    {
        is_PDF_val = 1,
    };

    // structs

    struct TGaussianMode;

    // fields

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

    // construction

    CPointPDFSOG(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;
    static constexpr bool is_3D();
    static constexpr bool is_PDF();
    void clear();
    const TGaussianMode& operator [] (size_t i) const;
    TGaussianMode& operator [] (size_t i);
    const TGaussianMode& get(size_t i) const;
    TGaussianMode& get(size_t i);
    void push_back(const TGaussianMode& m);
    iterator begin();
    iterator end();
    const_iterator begin() const;
    const_iterator end() const;
    iterator erase(iterator i);
    void resize(size_t N);
    size_t size() const;
    bool empty() const;
    void getMean(CPoint3D& mean_point) const;
    virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const;
    void normalizeWeights();
    void getMostLikelyMode(CPointPDFGaussian& outVal) const;
    double ESS() const;
    virtual void copyFrom(const CPointPDF& o);
    virtual bool saveToTextFile(const std::string& file) const;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase);
    void drawSingleSample(CPoint3D& outSample) const;
    virtual void bayesianFusion(const CPointPDF& p1, const CPointPDF& p2, const double minMahalanobisDistToDrop = 0);

    void evaluatePDFInArea(
        float x_min,
        float x_max,
        float y_min,
        float y_max,
        float resolutionXY,
        float z,
        mrpt::math::CMatrixD& outMatrix,
        bool sumOverAllZs = false
        );

    double evaluatePDF(const CPoint3D& x, bool sumOverAllZs) 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<CPointPDF> Ptr;
    typedef std::shared_ptr<const CPointPDF> 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 copyFrom(const CPointPDF& o) = 0;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase) = 0;

Typedefs

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

A type for the associated smart pointer.

Construction

CPointPDFSOG(size_t nModes = 1)

Default constructor.

Parameters:

nModes

The initial size of CPointPDFSOG::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.

const TGaussianMode& operator [] (size_t i) const

Access to individual beacons.

TGaussianMode& operator [] (size_t i)

Access to individual beacons.

const TGaussianMode& get(size_t i) const

Access to individual beacons.

TGaussianMode& get(size_t i)

Access to individual beacons.

void push_back(const TGaussianMode& m)

Inserts a copy of the given mode into the SOG.

void resize(size_t N)

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

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(CPointPDFGaussian& outVal) const

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

double ESS() const

Computes the “Effective sample size” (typical measure for Particle Filters), applied to the weights of the individual Gaussian modes, as a measure of the equality of the modes (in the range [0,total # of modes]).

virtual void copyFrom(const CPointPDF& 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 weight)

• x_mean (gaussian mean value)

• y_mean (gaussian mean value)

• x_mean (gaussian mean value)

• C11 (Covariance elements)

• C22 (Covariance elements)

• C33 (Covariance elements)

• C12 (Covariance elements)

• C13 (Covariance elements)

• C23 (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.

void drawSingleSample(CPoint3D& outSample) const

Draw a sample from the pdf.

virtual void bayesianFusion(
    const CPointPDF& p1,
    const CPointPDF& p2,
    const double minMahalanobisDistToDrop = 0
    )

Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)

Parameters:

p1	The first distribution to fuse
p2	The second distribution to fuse
minMahalanobisDistToDrop	If set to different of 0, the result of very separate Gaussian modes (that will result in negligible components) in SOGs will be dropped to reduce the number of modes in the output.

void evaluatePDFInArea(
    float x_min,
    float x_max,
    float y_min,
    float y_max,
    float resolutionXY,
    float z,
    mrpt::math::CMatrixD& outMatrix,
    bool sumOverAllZs = false
    )

Evaluates the PDF within a rectangular grid and saves the result in a matrix (each row contains values for a fixed y-coordinate value).

double evaluatePDF(const CPoint3D& x, bool sumOverAllZs) const

Evaluates the PDF at a given point.