class mrpt::poses::CPosePDFSOG

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 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 CPosePDFSOG::m_modes

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

See also:

CPose2D, CPosePDF, CPosePDFParticles

#include <mrpt/poses/CPosePDFSOG.h>

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

    typedef std::vector<TGaussianMode> CListGaussianModes;
    typedef CListGaussianModes::const_iterator const_iterator;
    typedef CListGaussianModes::iterator iterator;

    // enums

    enum
    {
        is_3D_val = 0,
    };

    enum
    {
        is_PDF_val = 1,
    };

    // structs

    struct TGaussianMode;

    // construction

    CPosePDFSOG(size_t nModes = 1);

    // methods

    const CListGaussianModes& getSOGModes() const;
    size_t size() const;
    bool empty() const;
    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);
    void mergeModes(double max_KLd = 0.5, bool verbose = false);
    void getMean(CPose2D& mean_pose) const;
    virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const;
    void getMostLikelyCovarianceAndMean(mrpt::math::CMatrixDouble33& cov, CPose2D& mean_point) const;
    void normalizeWeights();
    virtual void copyFrom(const CPosePDF& o);
    virtual bool saveToTextFile(const std::string& file) const;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase);
    void rotateAllCovariances(double ang);
    void drawSingleSample(CPose2D& outPart) const;
    virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const;
    virtual void inverse(CPosePDF& o) const;
    void operator += (const mrpt::poses::CPose2D& Ap);
    double evaluatePDF(const mrpt::poses::CPose2D& x, bool sumOverAllPhis = false) const;
    double evaluateNormalizedPDF(const mrpt::poses::CPose2D& x) const;

    void evaluatePDFInArea(
        double x_min,
        double x_max,
        double y_min,
        double y_max,
        double resolutionXY,
        double phi,
        mrpt::math::CMatrixDouble& outMatrix,
        bool sumOverAllPhis = false
        );

    virtual void bayesianFusion(const CPosePDF& p1, const CPosePDF& p2, const double minMahalanobisDistToDrop = 0);
    static constexpr bool is_3D();
    static constexpr bool is_PDF();
};

Inherited Members

public:
    // typedefs

    typedef CProbabilityDensityFunction<TDATA, STATE_LEN> self_t;

    // methods

    virtual void copyFrom(const CPosePDF& o) = 0;
    virtual void bayesianFusion(const CPosePDF& p1, const CPosePDF& p2, const double minMahalanobisDistToDrop = 0) = 0;
    virtual void inverse(CPosePDF& o) const = 0;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase) = 0;

Construction

CPosePDFSOG(size_t nModes = 1)

Default constructor.

Parameters:

nModes

The initial size of CPosePDFSOG::m_modes

Methods

size_t size() const

Return the number of Gaussian modes.

bool empty() const

Return whether there is any Gaussian mode.

void clear()

Clear the list of 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.

void mergeModes(double max_KLd = 0.5, bool verbose = false)

Merge very close modes so the overall number of modes is reduced while preserving the total distribution.

This method uses the approach described in the paper:

“Kullback-Leibler Approach to Gaussian Mixture Reduction” AR Runnalls. IEEE Transactions on Aerospace and Electronic Systems, 2007.

Parameters:

max_KLd

The maximum KL-divergence to consider the merge of two nodes (and then stops the process).

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

void normalizeWeights()

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

virtual void copyFrom(const CPosePDF& o)

Copy operator, translating if necesary (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)
phi_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 rotateAllCovariances(double ang)

Rotate all the covariance matrixes by replacing them 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 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 inverse(CPosePDF& o) const

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

void operator += (const mrpt::poses::CPose2D& Ap)

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

double evaluatePDF(const mrpt::poses::CPose2D& x, bool sumOverAllPhis = false) const

Evaluates the PDF at a given point.

double evaluateNormalizedPDF(const mrpt::poses::CPose2D& x) const

Evaluates the ratio PDF(x) / max_PDF(x*), that is, the normalized PDF in the range [0,1].

void evaluatePDFInArea(
    double x_min,
    double x_max,
    double y_min,
    double y_max,
    double resolutionXY,
    double phi,
    mrpt::math::CMatrixDouble& outMatrix,
    bool sumOverAllPhis = false
    )

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

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

Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently p1 must be a mrpt::poses::CPosePDFSOG object and p2 a mrpt::poses::CPosePDFGaussian object)