class mrpt::maps::COccupancyGridMap3D

A 3D occupancy grid map with a regular, even distribution of voxels.

This is a faster alternative to COctoMap, but use with caution with limited map extensions, since it could easily exaust available memory.

Each voxel follows a Bernoulli probability distribution: a value of 0 means certainly occupied, 1 means a certainly empty voxel. Initially 0.5 means uncertainty.

#include <mrpt/maps/COccupancyGridMap3D.h>

class COccupancyGridMap3D:
    public mrpt::maps::CMetricMap,
    public mrpt::maps::CLogOddsGridMap3D
{
public:
    // typedefs

    typedef OccGridCellTraits::cellType voxelType;
    typedef OccGridCellTraits::cellTypeUnsigned voxelTypeUnsigned;
    typedef detail::logoddscell_traits<OccGridCellTraits::cellType> traits_t;
    typedef mrpt::containers::CDynamicGrid3D<OccGridCellTraits::cellType> grid_t;

    // enums

    enum TLikelihoodMethod;

    // structs

    struct TRenderingOptions;

    // classes

    class TInsertionOptions;
    class TLikelihoodOptions;

    //
fields

    TInsertionOptions insertionOptions;
    TLikelihoodOptions likelihoodOptions;
    TRenderingOptions renderingOptions;

    // construction

    COccupancyGridMap3D(
        const mrpt::math::TPoint3D& corner_min = {-5.0f, -5.0f, -5.0f},
        const mrpt::math::TPoint3D& corner_max = {5.0f, 5.0f, 5.0f},
        float resolution = 0.25f
        );

    //
methods

    void fill(float default_value = 0.5f);

    void setSize(
        const mrpt::math::TPoint3D& corner_min,
        const mrpt::math::TPoint3D& corner_max,
        double resolution,
        float default_value = 0.5f
        );

    void resizeGrid(
        const mrpt::math::TPoint3D& corner_min,
        const mrpt::math::TPoint3D& corner_max,
        float new_voxels_default_value = 0.5f
        );

    void updateCell(int cx_idx, int cy_idx, int cz_idx, float v);
    void setCellFreeness(unsigned int cx, unsigned int cy, unsigned int cz, float value);
    float getCellFreeness(unsigned int cx, unsigned int cy, unsigned int cz) const;
    void setFreenessByPos(float x, float y, float z, float value);
    float getFreenessByPos(float x, float y, float z) const;
    void insertRay(const mrpt::math::TPoint3D& sensor, const mrpt::math::TPoint3D& end, bool endIsOccupied = true);

    void insertPointCloud(
        const mrpt::math::TPoint3D& sensorCenter,
        const mrpt::maps::CPointsMap& pts,
        const float maxValidRange = std::numeric_limits<float>::max()
        );

    void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels& gl_obj) const;
    virtual void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const;

    void simulateScanRay(
        const double x,
        const double y,
        const double angle_direction,
        float& out_range,
        bool& out_valid,
        const double max_range_meters,
        const float threshold_free = 0.4f,
        const double noiseStd = .0,
        const double angleNoiseStd = .0
        ) const;

    double computeLikelihoodField_Thrun(const CPointsMap& pm, const mrpt::poses::CPose3D& relativePose = mrpt::poses::CPose3D());
    virtual bool isEmpty() const;

    virtual void determineMatching2D(
        const mrpt::maps::CMetricMap* otherMap,
        const mrpt::poses::CPose2D& otherMapPose,
        mrpt::tfest::TMatchingPairList& correspondences,
        const TMatchingParams& params,
        TMatchingExtraResults& extraResults
        ) const;

    virtual float compute3DMatchingRatio(const mrpt::maps::CMetricMap* otherMap, const mrpt::poses::CPose3D& otherMapPose, const TMatchingRatioParams& params) const;
    virtual void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const;
    static float l2p(const voxelType l);
    static uint8_t l2p_255(const voxelType l);
    static voxelType p2l(const float p);
};

Inherited Members

public:
    // typedefs

    typedef TCELL cell_t;

    //
fields

    grid_t m_grid;

    //
methods

    virtual bool isEmpty() const = 0;
    virtual void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const = 0;
    virtual void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const = 0;
    static void updateCell_fast_occupied(cell_t* theCell, const cell_t logodd_obs, const cell_t thres);
    static void updateCell_fast_free(cell_t* theCell, const cell_t logodd_obs, const cell_t thres);

    void updateCell_fast_occupied(
        const unsigned x,
        const unsigned y,
        const unsigned z,
        const cell_t logodd_obs,
        const cell_t thres
        );

    void updateCell_fast_free(
        const unsigned x,
        const unsigned y,
        const unsigned z,
        const cell_t logodd_obs,
        const cell_t thres
        );

Typedefs

typedef OccGridCellTraits::cellType voxelType

The type of the map voxels:

Fields

TInsertionOptions insertionOptions

With this struct options are provided to the observation insertion process.

See also:

CObservation::insertIntoGridMap

Construction

COccupancyGridMap3D(
    const mrpt::math::TPoint3D& corner_min = {-5.0f, -5.0f, -5.0f},
    const mrpt::math::TPoint3D& corner_max = {5.0f, 5.0f, 5.0f},
    float resolution = 0.25f
    )

Constructor.

Methods

void fill(float default_value = 0.5f)

Fills all the voxels with a default value.

void setSize(
    const mrpt::math::TPoint3D& corner_min,
    const mrpt::math::TPoint3D& corner_max,
    double resolution,
    float default_value = 0.5f
    )

Change the size of gridmap, erasing all its previous contents.

Parameters:

resolution	The new size of voxels.
default_value	The value of voxels, tipically 0.5.

See also:

ResizeGrid

void resizeGrid(
    const mrpt::math::TPoint3D& corner_min,
    const mrpt::math::TPoint3D& corner_max,
    float new_voxels_default_value = 0.5f
    )

Change the size of gridmap, maintaining previous contents.

Parameters:

new_voxels_default_value

Value of new voxels, tipically 0.5

See also:

setSize()

void updateCell(int cx_idx, int cy_idx, int cz_idx, float v)

Performs the Bayesian fusion of a new observation of a cell.

See also:

updateInfoChangeOnly, updateCell_fast_occupied, updateCell_fast_free

void setCellFreeness(
    unsigned int cx,
    unsigned int cy,
    unsigned int cz,
    float value
    )

Change the contents [0,1] (0:occupied, 1:free) of a voxel, given its index.

float getCellFreeness(unsigned int cx, unsigned int cy, unsigned int cz) const

Read the real valued [0,1] (0:occupied, 1:free) contents of a voxel, given its index.

void setFreenessByPos(float x, float y, float z, float value)

Change the contents [0,1] of a voxel, given its coordinates.

float getFreenessByPos(float x, float y, float z) const

Read the real valued [0,1] contents of a voxel, given its coordinates.

void insertRay(const mrpt::math::TPoint3D& sensor, const mrpt::math::TPoint3D& end, bool endIsOccupied = true)

Increases the freeness of a ray segment, and the occupancy of the voxel at its end point (unless endIsOccupied=false).

Normally, users would prefer the higher-level method CMetricMap::insertObservation()

void insertPointCloud(
    const mrpt::math::TPoint3D& sensorCenter,
    const mrpt::maps::CPointsMap& pts,
    const float maxValidRange = std::numeric_limits<float>::max()
    )

Calls insertRay() for each point in the point cloud, using as sensor central point (the origin of all rays), the given sensorCenter.

Parameters:

maxValidRange

If a point has larger distance from sensorCenter than maxValidRange, it will be considered a non-echo, and NO occupied voxel will be created at the end of the segment.

See also:

insertionOptions parameters are observed in this method.

void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels& gl_obj) const

See also:

renderingOptions

virtual void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const

Returns a 3D object representing the map.

See also:

renderingOptions

void simulateScanRay(
    const double x,
    const double y,
    const double angle_direction,
    float& out_range,
    bool& out_valid,
    const double max_range_meters,
    const float threshold_free = 0.4f,
    const double noiseStd = .0,
    const double angleNoiseStd = .0
    ) const

Simulate just one “ray” in the grid map.

This method is used internally to sonarSimulator and laserScanSimulator.

See also:

COccupancyGridMap3D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS

double computeLikelihoodField_Thrun(const CPointsMap& pm, const mrpt::poses::CPose3D& relativePose = mrpt::poses::CPose3D())

Computes the likelihood [0,1] of a set of points, given the current grid map as reference.

Parameters:

pm	The points map
relativePose	The relative pose of the points map in this map’s coordinates, or nullptr for (0,0,0). See “likelihoodOptions” for configuration parameters.

virtual bool isEmpty() const

Returns true upon map construction or after calling clear(), the return changes to false upon successful insertObservation() or any other method to load data in the map.

virtual void determineMatching2D(
    const mrpt::maps::CMetricMap* otherMap,
    const mrpt::poses::CPose2D& otherMapPose,
    mrpt::tfest::TMatchingPairList& correspondences,
    const TMatchingParams& params,
    TMatchingExtraResults& extraResults
    ) const

See docs in base class: in this class this always returns 0.

virtual float compute3DMatchingRatio(
    const mrpt::maps::CMetricMap* otherMap,
    const mrpt::poses::CPose3D& otherMapPose,
    const TMatchingRatioParams& params
    ) const

See docs in base class: in this class this always returns 0.

virtual void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const

This virtual method saves the map to a file “filNamePrefix”+< some_file_extension >, as an image or in any other applicable way (Notice that other methods to save the map may be implemented in classes implementing this virtual interface).

static float l2p(const voxelType l)

Scales an integer representation of the log-odd into a real valued probability in [0,1], using p=exp(l)/(1+exp(l))

static uint8_t l2p_255(const voxelType l)

Scales an integer representation of the log-odd into a linear scale [0,255], using p=exp(l)/(1+exp(l))

static voxelType p2l(const float p)

Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the valid range of voxelType.