class mrpt::poses::CPose3DPDFGrid

Overview

Declares a class that represents a Probability Distribution function (PDF) of a SE(3) pose (x,y,z, yaw, pitch, roll), in the form of a 6-dimensional grid of “voxels”.

See also:

CPose3D, CPose3DPDF, CPose3DGridTemplate

#include <mrpt/poses/CPose3DPDFGrid.h>

class CPose3DPDFGrid:
    public mrpt::poses::CPose3DPDF,
    public mrpt::poses::CPose3DGridTemplate
{
public:
    // construction

    CPose3DPDFGrid(
        const mrpt::math::TPose3D& bb_min = mrpt::math::TPose3D(-1., -1., -1., -M_PI, -.5*M_PI, -.5*M_PI),
        const mrpt::math::TPose3D& bb_max = mrpt::math::TPose3D(1., 1., 1., M_PI,.5*M_PI,.5*M_PI),
        double resolution_XYZ = 0.10,
        double resolution_YPR = mrpt::DEG2RAD(10.0)
        );

    // methods

    int x2idx(double x) const;
    int y2idx(double y) const;
    int z2idx(double z) const;
    int yaw2idx(double yaw) const;
    int pitch2idx(double pitch) const;
    int roll2idx(double roll) const;
    double idx2x(uint32_t cx) const;
    double idx2y(uint32_t cy) const;
    double idx2z(uint32_t cz) const;
    double idx2yaw(uint32_t cY) const;
    double idx2pitch(uint32_t cP) const;
    double idx2roll(uint32_t cR) const;
    virtual void copyFrom(const CPose3DPDF& o);
    void normalize();
    void uniformDistribution();
    void getMean(CPose3D& 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 CPose3DPDF& p1, const CPose3DPDF& p2);
    virtual void inverse(CPose3DPDF& o) const;
    void drawSingleSample(CPose3D& outPart) const;
    virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const;

    double* getByPos(
        double x,
        double y,
        double z,
        double yaw,
        double pitch,
        double roll
        );

    const double* getByPos(const mrpt::math::TPose3D& p) const;
    double* getByPos(const mrpt::math::TPose3D& p);

    double* getByIndex(
        int cx,
        int cy,
        int cz,
        int cY,
        int cP,
        int cR
        );

    mrpt::math::TPose3D getMaxBoundingBox() const;
    double getResolutionXYZ() const;
    double getResolutionAngles() const;
    void fill(const double& val);
    uint32_t getSizeX() const;
    uint32_t getSizeY() const;
    uint32_t getSizeZ() const;
    uint32_t getSizeYaw() const;
    uint32_t getSizePitch() const;
    uint32_t getSizeRoll() const;
    uint32_t getTotalVoxelCount() const;
    const std::vector<double>& getData() const;
    std::vector<double>& getData();
};

Inherited Members

public:
    // typedefs

    typedef CProbabilityDensityFunction<TDATA, STATE_LEN> self_t;

    // methods

    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;

    void setSize(
        const mrpt::math::TPose3D& bb_min,
        const mrpt::math::TPose3D& bb_max,
        double resolution_XYZ,
        double resolution_YPR
        );

    const T* getByPos(double x, double y, double z, double yaw, double pitch, double roll) const;
    const T* getByIndex(int cx, int cy, int cz, int cY, int cP, int cR) const;

    template <class MATRIXLIKE>
    void getAsMatrix(
        MATRIXLIKE& outMat,
        const double z,
        const double yaw,
        const double pitch,
        const double roll
        ) const;

    mrpt::math::TPose3D getMinBoundingBox() const;

Construction

CPose3DPDFGrid(
    const mrpt::math::TPose3D& bb_min = mrpt::math::TPose3D(-1., -1., -1., -M_PI, -.5*M_PI, -.5*M_PI),
    const mrpt::math::TPose3D& bb_max = mrpt::math::TPose3D(1., 1., 1., M_PI,.5*M_PI,.5*M_PI),
    double resolution_XYZ = 0.10,
    double resolution_YPR = mrpt::DEG2RAD(10.0)
    )

Constructor: Initializes a, uniform distribution over the whole given range, given by a “rectangular” (6D) bounding box.

Methods

virtual void copyFrom(const CPose3DPDF& o)

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

void normalize()

Normalizes the PDF, such as all voxels 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 concatenation of (X,Y) slices.

The size in X,Y,and the values for Z,yaw, pitch, roll, PHI are stored in another file named <filename>_dims.txt

Returns:

false on error

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

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

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

void drawSingleSample(CPose3D& outPart) const

Draws a single sample from the distribution.

Precondition: voxel 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 1x6 vectors, where each row contains a (x,y,z,yaw,pitch,roll) datum.