Class ProbabilisticMap

Nested Relationships

Nested Types

Class Documentation

class ProbabilisticMap

The ProbabilisticMap class is meant to behave as much as possible as octomap::Octree, given the same voxel size.

Insert a point cloud to update the current probability

Public Types

using Vector3D = Eigen::Vector3d

Public Functions

ProbabilisticMap(double resolution)
VoxelGrid<CellT> &grid()
const VoxelGrid<CellT> &grid() const
const Options &options() const
void setOptions(const Options &options)
template<typename PointT, typename Allocator>
void insertPointCloud(const std::vector<PointT, Allocator> &points, const PointT &origin, double max_range)

insertPointCloud will update the probability map with a new set of detections. The template function can accept points of different types, such as pcl:Point, Eigen::Vector or Bonxai::Point3d

Both origin and points must be in world coordinates

Parameters:
  • points – a vector of points which represent detected obstacles

  • origin – origin of the point cloud

  • max_range – max range of the ray, if exceeded, we will use that to compute a free space

void addHitPoint(const Vector3D &point)
void addMissPoint(const Vector3D &point)
bool isOccupied(const Bonxai::CoordT &coord) const
bool isUnknown(const Bonxai::CoordT &coord) const
bool isFree(const Bonxai::CoordT &coord) const
float queryProbability(const Eigen::Vector3f &p_world) const
float queryProbability(const Bonxai::CoordT &coord) const
void getOccupiedVoxels(std::vector<Bonxai::CoordT> &coords)
void getFreeVoxels(std::vector<Bonxai::CoordT> &coords)
template<typename PointT>
inline void getOccupiedVoxels(std::vector<PointT> &points)
template<typename PointT, typename Alloc>
inline void insertPointCloud(const std::vector<PointT, Alloc> &points, const PointT &origin, double max_range)

Public Static Functions

static inline constexpr int32_t logods(float prob)

Compute the logds, but return the result as an integer, The real number is represented as a fixed precision integer (6 decimals after the comma)

static inline constexpr float prob(int32_t logods_fixed)

Expect the fixed comma value returned by logods()

Public Static Attributes

static const int32_t UnknownProbability
struct CellT

Public Functions

inline CellT()

Public Members

int32_t update_id
int32_t probability_log
struct Options

These default values are the same as OctoMap.

Public Members

int32_t prob_miss_log = logods(0.4f)
int32_t prob_hit_log = logods(0.7f)
int32_t clamp_min_log = logods(0.12f)
int32_t clamp_max_log = logods(0.97f)
int32_t occupancy_threshold_log = logods(0.5)