Template Class CVoxelMapOccupancyBase

Inheritance Relationships

Base Types

Class Documentation

template<typename voxel_node_t, typename occupancy_t = int8_t>
class CVoxelMapOccupancyBase : public mrpt::maps::CVoxelMapBase<voxel_node_t>, public mrpt::maps::detail::logoddscell_traits<int8_t>, public mrpt::maps::NearestNeighborsCapable

Base class for log-odds sparse voxel map for cells containing occupancy, and possibly other information, for each voxel.

See also

Use derived classes CVoxelMap, CVoxelMapRGB

API of the NearestNeighborsCapable virtual interface

inline virtual void nn_prepare_for_2d_queries() const override

Must be called before calls to nn_*_search() to ensure the required data structures are ready for queries (e.g. KD-trees). Useful in multithreading applications.

inline virtual void nn_prepare_for_3d_queries() const override

Must be called before calls to nn_*_search() to ensure the required data structures are ready for queries (e.g. KD-trees). Useful in multithreading applications.

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.

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

inline virtual bool nn_single_search(const mrpt::math::TPoint2Df &query, mrpt::math::TPoint2Df &result, float &out_dist_sqr, uint64_t &resultIndexOrID) const override
inline 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.

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

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

inline CVoxelMapOccupancyBase(double resolution = 0.05, uint8_t inner_bits = 2, uint8_t leaf_bits = 3)
virtual ~CVoxelMapOccupancyBase() = default
inline bool isEmpty() const override
virtual void getAsOctoMapVoxels(mrpt::viz::COctoMapVoxels &gl_obj) const override

Builds a renderizable representation of the octomap as a mrpt::viz::COctoMapVoxels object. Implementation defined for each children class.

See also

renderingOptions

void updateVoxel(const double x, const double y, const double z, bool occupied)

Manually updates the occupancy of the voxel at (x,y,z) as being occupied (true) or free (false), using the log-odds parameters in insertionOptions

bool getPointOccupancy(const double x, const double y, const double z, double &prob_occupancy) const

Get the occupancy probability [0,1] of a point

Returns:

false if the point is not mapped, in which case the returned “prob” is undefined.

void insertPointCloudAsRays(const mrpt::maps::CPointsMap &pts, const mrpt::math::TPoint3D &sensorPt, const std::optional<const mrpt::poses::CPose3D> &sensorPose = std::nullopt)
void insertPointCloudAsEndPoints(const mrpt::maps::CPointsMap &pts, const mrpt::math::TPoint3D &sensorPt, const std::optional<const mrpt::poses::CPose3D> &sensorPose = std::nullopt)
mrpt::maps::CSimplePointsMap::Ptr getOccupiedVoxels() const

Returns all occupied voxels as a point cloud. The shared_ptr is also hold and updated internally, so it is not safe to read it while also updating the voxel map in another thread.

The point cloud is cached, and invalidated upon map updates.

A voxel is considered occupied if its occupancy is larger than likelihoodOptions.occupiedThreshold (Range: [0,1], default: 0.6)

mrpt::math::TBoundingBoxf boundingBox() const override

This visits all cells to calculate a bounding box, caching the result so subsequent calls are cheap until the voxelmap is changed in some way.

inline void updateCell_fast_occupied(voxel_node_t *theCell, const occupancy_t logodd_obs, const occupancy_t thres)

Performs Bayesian fusion of a new observation of a cell. This method increases the “occupancy-ness” of a cell, managing possible saturation.

See also

updateCell, updateCell_fast_free

Parameters:

  • theCell – The cell to modify

  • logodd_obs – Observation of the cell, in log-odd form as transformed by p2l.

  • thres – This must be CELLTYPE_MIN+logodd_obs

inline void updateCell_fast_occupied(const Bonxai::CoordT &coord, const occupancy_t logodd_obs, const occupancy_t thres)

Performs Bayesian fusion of a new observation of a cell. This method increases the “occupancy-ness” of a cell, managing possible saturation.

See also

updateCell, updateCell_fast_free

Parameters:

  • coord – Cell indexes.

  • logodd_obs – Observation of the cell, in log-odd form as transformed by p2l.

  • thres – This must be CELLTYPE_MIN+logodd_obs

inline void updateCell_fast_free(voxel_node_t *theCell, const occupancy_t logodd_obs, const occupancy_t thres)

Performs Bayesian fusion of a new observation of a cell. This method increases the “free-ness” of a cell, managing possible saturation.

See also

updateCell_fast_occupied

Parameters:

  • logodd_obs – Observation of the cell, in log-odd form as transformed by p2l.

  • thres – This must be CELLTYPE_MAX-logodd_obs

inline void updateCell_fast_free(const Bonxai::CoordT &coord, const occupancy_t logodd_obs, const occupancy_t thres)

Performs the Bayesian fusion of a new observation of a cell. This method increases the “free-ness” of a cell, managing possible saturation.

See also

updateCell_fast_occupied

Parameters:

  • coord – Cell indexes.

  • logodd_obs – Observation of the cell, in log-odd form as transformed by p2l.

  • thres – This must be CELLTYPE_MAX-logodd_obs

Public Members

TVoxelMap_InsertionOptions insertionOptions

The options used when inserting observations in the map:

TVoxelMap_LikelihoodOptions likelihoodOptions
TVoxelMap_RenderingOptions renderingOptions

Public Static Functions

static inline CLogOddsGridMapLUT<occupancy_value_t> &get_logodd_lut()

Lookup tables for log-odds

static inline float l2p(const occupancy_value_t 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 occupancy_value_t l)

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

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

Protected Types

using occupancy_value_t = occupancy_t
using traits_t = detail::logoddscell_traits<occupancy_t>
using base_t = CVoxelMapBase<voxel_node_t>

Protected Functions

void internal_clear() override
inline void markAsChanged()
void updateCachedProperties() const

Protected Attributes

mutable mrpt::maps::CSimplePointsMap::Ptr m_cachedOccupied
mutable mrpt::math::TBoundingBox m_bbox