Class COccupancyGridMap3D

Nested Relationships

Nested Types

Inheritance Relationships

Base Types

Class Documentation

class COccupancyGridMap3D : public mrpt::maps::CMetricMap, public mrpt::maps::CLogOddsGridMap3D<OccGridCellTraits::cellType>, public mrpt::maps::NearestNeighborsCapable

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.

An alternative, sparse representation of a 3D map is provided via mrpt::maps::CVoxelMap and mrpt::maps::CVoxelMapRGB

API of the NearestNeighborsCapable virtual interface

inline virtual bool nn_has_indices_or_ids() const override

Returns true if the rest of nn_* methods will populate the output indices values with 0-based contiguous indices. Returns false if indices are actually sparse ID numbers without any expectation of they be contiguous or start near zero.

inline virtual size_t nn_index_count() const override

If nn_has_indices_or_ids() returns true, this must return the number of “points” (or whatever entity) the indices correspond to. Otherwise, the return value should be ignored.

virtual bool nn_single_search(const mrpt::math::TPoint3Df &query, mrpt::math::TPoint3Df &result, float &out_dist_sqr, uint64_t &resultIndexOrID) const override

Search for the closest 3D point to a given one.

Parameters:

  • query – [in] The query input point.

  • result – [out] The found closest point.

  • out_dist_sqr – [out] The square Euclidean distance between the query and the returned point.

  • resultIndexOrID – [out] The index or ID of the result point in the map.

Returns:

True if successful, false if no point was found.

virtual bool nn_single_search(const mrpt::math::TPoint2Df &query, mrpt::math::TPoint2Df &result, float &out_dist_sqr, uint64_t &resultIndexOrID) const override
virtual void nn_multiple_search(const mrpt::math::TPoint3Df &query, const size_t N, std::vector<mrpt::math::TPoint3Df> &results, std::vector<float> &out_dists_sqr, std::vector<uint64_t> &resultIndicesOrIDs) const override

Search for the N closest 3D points to a given one.

Parameters:

  • query – [in] The query input point.

  • results – [out] The found closest points.

  • out_dists_sqr – [out] The square Euclidean distances between the query and the returned point.

  • resultIndicesOrIDs – [out] The indices or IDs of the result points.

virtual void nn_multiple_search(const mrpt::math::TPoint2Df &query, const size_t N, std::vector<mrpt::math::TPoint2Df> &results, std::vector<float> &out_dists_sqr, std::vector<uint64_t> &resultIndicesOrIDs) const override
virtual void nn_radius_search(const mrpt::math::TPoint3Df &query, const float search_radius_sqr, std::vector<mrpt::math::TPoint3Df> &results, std::vector<float> &out_dists_sqr, std::vector<uint64_t> &resultIndicesOrIDs, size_t maxPoints) const override

Radius search for closest 3D points to a given one.

Parameters:

  • query – [in] The query input point.

  • search_radius_sqr – [in] The search radius, squared.

  • results – [out] The found closest points.

  • out_dists_sqr – [out] The square Euclidean distances between the query and the returned point.

  • resultIndicesOrIDs – [out] The indices or IDs of the result points.

  • maxPoints – [in] If !=0, the maximum number of neigbors to return.

virtual void nn_radius_search(const mrpt::math::TPoint2Df &query, const float search_radius_sqr, std::vector<mrpt::math::TPoint2Df> &results, std::vector<float> &out_dists_sqr, std::vector<uint64_t> &resultIndicesOrIDs, size_t maxPoints) const override

Public Types

enum TLikelihoodMethod

The type for selecting a likelihood computation method

Values:

enumerator lmLikelihoodField_Thrun
enumerator lmRayTracing
using voxelType = OccGridCellTraits::cellType

The type of the map voxels:

using voxelTypeUnsigned = OccGridCellTraits::cellTypeUnsigned

Public Functions

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

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.

See also

ResizeGrid

Parameters:

  • resolution – The new size of voxels.

  • default_value – The value of voxels, typically 0.5.

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.

See also

setSize()

Parameters:

new_voxels_default_value – Value of new voxels, typically 0.5

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

inline 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.

inline 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

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

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

inline 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(), const std::optional<mrpt::poses::CPose3D> &robotPose = std::nullopt)

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

See also

insertionOptions parameters are observed in this method.

Parameters:

maxValidRange – [in] 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.

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

See also

renderingOptions

void getVisualizationInto(mrpt::viz::CSetOfObjects &outObj) const override

Returns a 3D object representing the map.

See also

renderingOptions

inline mrpt::math::TBoundingBoxf boundingBox() const override
bool isEmpty() const override

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.

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

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

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

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

void saveMetricMapRepresentationToFile(const std::string &f) const override
inline std::string asString() const override

Returns a short description of the map.

Public Members

TInsertionOptions insertionOptions

With this struct options are provided to the observation insertion process

See also

CObservation::insertIntoGridMap

TLikelihoodOptions likelihoodOptions
TRenderingOptions renderingOptions

Public Static Functions

static inline 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 inline 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 inline 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

Protected Functions

void OnPostSuccesfulInsertObs(const mrpt::obs::CObservation&) override

See base class

void internal_clear() override

Clear the map: It set all voxels to their default occupancy value (0.5), without changing the resolution (the grid extension is reset to the default values).

bool internal_insertObservation(const mrpt::obs::CObservation &obs, const std::optional<const mrpt::poses::CPose3D> &robotPose) override

Insert the observation information into this map.

See also

insertionOptions, CObservation::insertObservationInto

Parameters:

  • obs – The observation

  • robotPose – The 3D pose of the robot mobile base in the map reference system, or nullptr (default) if you want to use CPose2D(0,0,deg)

void internal_insertObservationScan2D(const mrpt::obs::CObservation2DRangeScan &o, const mrpt::poses::CPose3D &robotPose)
void internal_insertObservationScan3D(const mrpt::obs::CObservation3DRangeScan &o, const mrpt::poses::CPose3D &robotPose)
void internal_insertObservationPointCloud(const mrpt::obs::CObservationPointCloud &o, const mrpt::poses::CPose3D &robotPose)

Protected Attributes

bool m_is_empty = {true}

True upon construction; used by isEmpty()

Protected Static Functions

static CLogOddsGridMapLUT<voxelType> &get_logodd_lut()

Lookup tables for log-odds

class TInsertionOptions : public mrpt::config::CLoadableOptions

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

See also

CObservation::insertIntoGridMap

Public Functions

TInsertionOptions() = default
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override

This method load the options from a “.ini” file. Only those parameters found in the given “section” and having the same name that the variable are loaded. Those not found in the file will stay with their previous values (usually the default values loaded at initialization). An example of an “.ini” file: 

[section]
resolution=0.10     ; blah blah...
modeSelection=1     ; 0=blah, 1=blah,...

void saveToConfigFile(mrpt::config::CConfigFileBase &target, const std::string &section) const override

Public Members

float maxDistanceInsertion = {15.0f}

Largest distance at which voxels are updated (Default: 15 meters)

float maxOccupancyUpdateCertainty = {0.65f}

A value in the range [0.5,1] used for updating voxel with a Bayesian approach (default 0.65)

float maxFreenessUpdateCertainty = {.0f}

A value in the range [0.5,1] for updating a free voxel. (default=0 means use the same than maxOccupancyUpdateCertainty)

uint16_t decimation_3d_range = {8}

Specify the decimation of 3D range scans. “N” means keeping the minimum range of each block of “NxN” range pixels. (default=8)

uint16_t decimation = {1}

Decimation for insertPointCloud() or 2D range scans (Default: 1)

bool raytraceEmptyCells = true

If true, raytrace and fill empty cells. If false, only the final (end point) of each ray will be marked as occupied.

class TLikelihoodOptions : public mrpt::config::CLoadableOptions

With this struct options are provided to the observation likelihood computation process

Public Functions

TLikelihoodOptions() = default
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override

This method load the options from a “.ini” file. Only those parameters found in the given “section” and having the same name that the variable are loaded. Those not found in the file will stay with their previous values (usually the default values loaded at initialization). An example of an “.ini” file: 

[section]
resolution=0.10     ; blah blah...
modeSelection=1     ; 0=blah, 1=blah,...

void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string &s) const override

Public Members

TLikelihoodMethod likelihoodMethod = {lmLikelihoodField_Thrun}

The selected method to compute an observation likelihood

float LF_stdHit = {0.35f}

[LikelihoodField] The laser range “sigma” used in computations; Default value = 0.35

float LF_zHit = {0.95f}

[LikelihoodField]

float LF_zRandom = {0.05f}
float LF_maxRange = {20.0f}

[LikelihoodField] The max. range of the sensor (Default= 81 m)

uint32_t LF_decimation = {1}

[LikelihoodField] The decimation of the points in a scan, default=1 == no decimation

float LF_maxCorrsDistance = {0.3f}

[LikelihoodField] The max. distance for searching correspondences around each sensed point

bool LF_useSquareDist = {false}

[LikelihoodField] (Default:false) Use exp(dist^2/std^2) instead of exp(dist^2/std^2)

int32_t rayTracing_decimation = {10}

[rayTracing] One out of “rayTracing_decimation” rays will be simulated and compared only: set to 1 to use all the sensed ranges.

float rayTracing_stdHit = {1.0f}

[rayTracing] The laser range sigma.

struct TRenderingOptions

Options for the conversion of a mrpt::maps::COccupancyGridMap3D into a mrpt::viz::COctoMapVoxels

Public Functions

TRenderingOptions() = default
void writeToStream(mrpt::serialization::CArchive &out) const

Binary dump to stream

void readFromStream(mrpt::serialization::CArchive &in)

Binary dump to stream

Public Members

bool generateGridLines = {false}

Generate grid lines for all octree nodes, useful to draw the “structure” of the octree, but computationally costly (Default: false)

bool generateOccupiedVoxels = {true}

Generate voxels for the occupied volumes (Default=true)

bool visibleOccupiedVoxels = {true}

Set occupied voxels visible (requires generateOccupiedVoxels=true) (Default=true)

bool generateFreeVoxels = {true}

Generate voxels for the freespace (Default=true)

bool visibleFreeVoxels = {true}

Set free voxels visible (requires generateFreeVoxels=true) (Default=true)