template class mrpt::poses::CPose3DGridTemplate

Overview

This is a template class for storing a 6-dimensional grid, with components corresponding to Euler angle parameterization of SE(3) poses.

poses_pdf_grp

#include <mrpt/poses/CPose3DGridTemplate.h>

template <class T>
class CPose3DGridTemplate
{
public:
    // construction

    CPose3DGridTemplate(
        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;
    const std::vector<T>& data() const;
    std::vector<T>& data();

    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;

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

    const T* getByPos(const mrpt::math::TPose3D& p) const;
    T* getByPos(const mrpt::math::TPose3D& p);
    const T* getByIndex(int cx, int cy, int cz, int cY, int cP, int cR) const;

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

    size_t idx2absidx(
        size_t cx,
        size_t cy,
        size_t cz,
        size_t cYaw,
        size_t cPitch,
        size_t cRoll
        ) const;

    std::tuple<size_t, size_t, size_t, size_t, size_t, size_t> absidx2idx(size_t absIdx) 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;
    mrpt::math::TPose3D getMaxBoundingBox() const;
    double getResolutionXYZ() const;
    double getResolutionAngles() const;
    void fill(const T& 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<T>& getData() const;
    std::vector<T>& getData();
};

// direct descendants

class CPose3DPDFGrid;

Construction

CPose3DGridTemplate(
    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)
    )

Default constructor:

Methods

const std::vector<T>& data() const

Read-only access to the underlying flat data array.

std::vector<T>& data()

Read-write access to the underlying flat data array.

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

Changes the limits and size of the grid, erasing previous contents:

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

Reads the contents of a cell.

const T* getByIndex(int cx, int cy, int cz, int cY, int cP, int cR) const

Reads the contents of a cell.

size_t idx2absidx(
    size_t cx,
    size_t cy,
    size_t cz,
    size_t cYaw,
    size_t cPitch,
    size_t cRoll
    ) const

(x,y,z,yaw,pitch,roll) indices to absolute index in raw data container

std::tuple<size_t, size_t, size_t, size_t, size_t, size_t> absidx2idx(size_t absIdx) const

absolute index to (x,y,z,yaw,pitch,roll) indices

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

Returns a XY slice of the grid, for given constant z,yaw, pitch and roll.

mrpt::math::TPose3D getMinBoundingBox() const

Get info about the 6D grid.