class mrpt::poses::CPosePDFGrid

Overview

Declares a class that represents a Probability Distribution function (PDF) of a 2D pose (x,y,phi).

This class implements that PDF using a 3D grid.

See also:

#include <mrpt/poses/CPosePDFGrid.h>

class CPosePDFGrid:
    public mrpt::poses::CPosePDF,
    public mrpt::poses::CPose2DGridTemplate
{
public:
    // construction

    CPosePDFGrid(
        double xMin = -1.0f,
        double xMax = 1.0f,
        double yMin = -1.0f,
        double yMax = 1.0f,
        double resolutionXY = 0.5f,
        double resolutionPhi = mrpt::DEG2RAD(180.0),
        double phiMin = -M_PI,
        double phiMax = M_PI
        );

    // methods

    virtual void copyFrom(const CPosePDF& o);
    void normalize();
    void uniformDistribution();
    void getMean(CPose2D& mean_pose) const;
    virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const;
    virtual bool saveToTextFile(const std::string& dataFile) const;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase);
    virtual void bayesianFusion(const CPosePDF& p1, const CPosePDF& p2, const double minMahalanobisDistToDrop = 0);
    virtual void inverse(CPosePDF& o) const;
    void drawSingleSample(CPose2D& outPart) const;
    virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const;
    const std::vector<double>& data() const;
    double getXMax() const;
    double getYMin() const;
    double getYMax() const;
    double getPhiMin() const;
    double getPhiMax() const;
    double getResolutionXY() const;
    double getResolutionPhi() const;
    size_t getSizeX() const;
    size_t getSizeY() const;
    size_t getSizePhi() const;
};

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;
    size_t x2idx(double x) const;
    size_t y2idx(double y) const;
    size_t phi2idx(double phi) const;
    double idx2x(size_t x) const;
    double idx2y(size_t y) const;
    double idx2phi(size_t phi) const;

    void setSize(
        double xMin,
        double xMax,
        double yMin,
        double yMax,
        double resolutionXY,
        double resolutionPhi,
        double phiMin = -M_PI,
        double phiMax = M_PI
        );

    const T* getByPos(double x, double y, double phi) const;
    T* getByPos(double x, double y, double phi);
    size_t idx2absidx(size_t cx, size_t cy, size_t cPhi) const;
    std::tuple<size_t, size_t, size_t> absidx2idx(size_t absIdx) const;
    const T* getByIndex(size_t x, size_t y, size_t phi) const;
    T* getByIndex(size_t x, size_t y, size_t phi);

    template <class MATRIXLIKE>
    void getAsMatrix(double phi, MATRIXLIKE& outMat) const;

    double getXMin() const;

Construction

CPosePDFGrid(
    double xMin = -1.0f,
    double xMax = 1.0f,
    double yMin = -1.0f,
    double yMax = 1.0f,
    double resolutionXY = 0.5f,
    double resolutionPhi = mrpt::DEG2RAD(180.0),
    double phiMin = -M_PI,
    double phiMax = M_PI
    )

Constructor: Initializes a, uniform distribution over the whole given range.

Methods

virtual void copyFrom(const CPosePDF& o)

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

void normalize()

Normalizes the PDF, such as all cells sum the unity.

void uniformDistribution()

Assigns the same value to all the cells in the grid, so the sum 1.

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

virtual bool saveToTextFile(const std::string& dataFile) const

Save the contents of the 3D grid in one file, as a vertical concatenation of rectangular matrix for the different “PHI” discrete levels, and the size in X,Y,and PHI in another file named “<filename>_dims.txt”.

Returns:

false on error

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 CPosePDF& p1,
    const CPosePDF& p2,
    const double minMahalanobisDistToDrop = 0
    )

Bayesian fusion of 2 densities (In the grid representation this becomes a pointwise multiplication)

virtual void inverse(CPosePDF& o) const

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

void drawSingleSample(CPose2D& outPart) const

Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!)

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.