9 #ifndef OCTOMAP_OCCUPANCY_OCTREE_BASE_H
10 #define OCTOMAP_OCCUPANCY_OCTREE_BASE_H
102 double maxrange=-1.,
bool pruning=
true,
bool lazy_eval =
false);
104 template <
class SCAN>
106 double maxrange=-1.,
bool pruning=
true,
bool lazy_eval =
false);
120 double maxrange=-1.,
bool pruning =
true,
bool lazy_eval =
false);
131 virtual void insertScan(
const ScanNode& scan,
double maxrange=-1.,
bool pruning =
true,
bool lazy_eval =
false);
145 virtual NODE*
updateNode(
const OcTreeKey& key,
float log_odds_update,
bool lazy_eval =
false);
157 virtual NODE*
updateNode(
const point3d& value,
float log_odds_update,
bool lazy_eval =
false);
171 virtual NODE*
updateNode(
double x,
double y,
double z,
float log_odds_update,
bool lazy_eval =
false);
194 virtual NODE*
updateNode(
const point3d& value,
bool occupied,
bool lazy_eval =
false);
208 virtual NODE*
updateNode(
double x,
double y,
double z,
bool occupied,
bool lazy_eval =
false);
246 bool ignoreUnknownCells=
false,
double maxRange=-1.0)
const;
266 DEPRECATED(
void getOccupied(std::list<OcTreeVolume>& occupied_volumes,
unsigned int max_depth = 0)
const);
279 DEPRECATED(
void getOccupied(std::list<OcTreeVolume>& binary_nodes, std::list<OcTreeVolume>& delta_nodes,
280 unsigned int max_depth = 0)
const);
298 DEPRECATED(
void getFreespace(std::list<OcTreeVolume>& free_volumes,
unsigned int max_depth = 0)
const);
310 DEPRECATED(
void getFreespace(std::list<OcTreeVolume>& binary_nodes, std::list<OcTreeVolume>& delta_nodes,
311 unsigned int max_depth = 0)
const);
368 template <
class POINTCLOUD>
412 std::ostream&
writeBinaryNode(std::ostream &s,
const NODE* node)
const;
458 unsigned int depth,
const float& log_odds_update,
bool lazy_eval =
false);
463 unsigned int depth,
const OcTreeKey& parent_key,
469 unsigned int& num_thresholded,
470 unsigned int& num_other)
const;
488 #include "mrpt/otherlibs/octomap/OccupancyOcTreeBase.hxx"
double resolution
in meters
Base implementation for Occupancy Octrees (e.g.
virtual bool castRay(const point3d &origin, const point3d &direction, point3d &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
Performs raycasting in 3d, similar to computeRay().
std::ostream & writeBinaryNode(std::ostream &s, const NODE *node) const
Write node to binary stream (max-likelihood value), recursively continue with all children...
This class represents a tree-dimensional pose of an object.
OctoMap: A probabilistic, flexible, and compact 3D mapping library for robotic systems.
point3d getBBXBounds() const
NODE * updateNodeRecurs(NODE *node, bool node_just_created, const OcTreeKey &key, unsigned int depth, const float &log_odds_update, bool lazy_eval=false)
virtual void insertScanNaive(const Pointcloud &pc, const point3d &origin, double maxrange, bool pruning=true, bool lazy_eval=false)
for testing only
KeyBoolMap changed_keys
Set of leaf keys (lowest level) which changed since last resetChangeDetection.
const iterator end() const
void computeUpdate(const Pointcloud &scan, const octomap::point3d &origin, KeySet &free_cells, KeySet &occupied_cells, double maxrange)
Helper for insertScan.
void setBBXMax(point3d &max)
sets the maximum for a query bounding box to use
void useBBXLimit(bool enable)
use or ignore BBX limit (default: ignore)
virtual ~OccupancyOcTreeBase()
std::unordered_map< OcTreeKey, bool, OcTreeKey::KeyHash > KeyBoolMap
Data structrure to efficiently track changed nodes as a combination of OcTreeKeys and a bool flag (to...
void resetChangeDetection()
Reset the set of changed keys. Call this after you obtained all changed nodes.
bool integrateMissOnRay(const point3d &origin, const point3d &end, bool lazy_eval=false)
Traces a ray from origin to end and updates all voxels on the way as free.
point3d getBBXMax() const
void enableChangeDetection(bool enable)
track or ignore changes while inserting scans (default: ignore)
void calcNumThresholdedNodesRecurs(NODE *node, unsigned int &num_thresholded, unsigned int &num_other) const
const Scalar * const_iterator
void calcNumThresholdedNodes(unsigned int &num_thresholded, unsigned int &num_other) const
bool inBBX(const point3d &p) const
const unsigned int tree_max_val
void insertScan_templ(const SCAN &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool pruning=true, bool lazy_eval=false)
point3d getBBXMin() const
void updateInnerOccupancy()
Updates the occupancy of all inner nodes to reflect their children's occupancy.
std::unordered_set< OcTreeKey, OcTreeKey::KeyHash > KeySet
Data structure to efficiently compute the nodes to update from a scan insertion using a hash set...
std::ostream & writeBinaryData(std::ostream &s) const
Writes the data of the tree (without header) to the stream, recursively calling writeBinaryNode (star...
virtual void integrateMiss(NODE *occupancyNode) const
integrate a "miss" measurement according to the tree's sensor model
DEPRECATED(void getOccupied(point3d_list &node_centers, unsigned int max_depth=0) const)
Convenience function to return all occupied nodes in the OcTree.
void computeUpdate_templ(const POINTCLOUD &scan, const octomap::point3d &origin, KeySet &free_cells, KeySet &occupied_cells, double maxrange)
bool use_change_detection
const unsigned int tree_depth
Maximum tree depth is fixed to 16 currently.
KeyBoolMap::const_iterator changedKeysEnd()
Iterator to traverse all keys of changed nodes.
OccupancyOcTreeBase(double resolution)
Default constructor, sets resolution of leafs.
void updateInnerOccupancyRecurs(NODE *node, unsigned int depth)
A collection of 3D coordinates (point3d), which are regarded as endpoints of a 3D laser scan...
OcTree base class, to be used with with any kind of OcTreeDataNode.
virtual void nodeToMaxLikelihood(NODE *occupancyNode) const
converts the node to the maximum likelihood value according to the tree's parameter for "occupancy" ...
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
Manipulate log_odds value of voxel directly.
void toMaxLikelihoodRecurs(NODE *node, unsigned int depth, unsigned int max_depth)
virtual void updateNodeLogOdds(NODE *occupancyNode, const float &update) const
bool use_bbx_limit
use bounding box for queries (needs to be set)?
void computeUpdate_onePoint(const point3d &p, const octomap::point3d &origin, KeySet &free_cells, KeySet &occupied_cells, double maxrange)
std::list< octomath::Vector3 > point3d_list
virtual void toMaxLikelihood()
Creates the maximum likelihood map by calling toMaxLikelihood on all tree nodes, setting their occupa...
virtual bool insertRay(const point3d &origin, const point3d &end, double maxrange=-1.0, bool lazy_eval=false)
Insert one ray between origin and end into the tree.
OcTreeKey is a container class for internal key addressing.
point3d getBBXCenter() const
virtual void insertScan(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool pruning=true, bool lazy_eval=false)
Integrate a Pointcloud (in global reference frame)
virtual void integrateHit(NODE *occupancyNode) const
integrate a "hit" measurement according to the tree's sensor model
A 3D scan as Pointcloud, performed from a Pose6D.
void getOccupiedLeafsBBXRecurs(point3d_list &node_centers, unsigned int max_depth, NODE *node, unsigned int depth, const OcTreeKey &parent_key, const OcTreeKey &min, const OcTreeKey &max) const
std::istream & readBinaryData(std::istream &s)
Reads only the data (=tree structure) from the input stream.
void setBBXMin(point3d &min)
sets the minimum for a query bounding box to use
std::istream & readBinaryNode(std::istream &s, NODE *node) const
Read node from binary stream (max-likelihood value), recursively continue with all children...
KeyBoolMap::const_iterator changedKeysBegin()
Iterator to traverse all keys of changed nodes.
This class represents a three-dimensional vector.