octomap 1.5.0
octomap::OccupancyOcTreeBase< NODE > Class Template Reference

Base implementation for Occupancy Octrees (e.g. More...

Inheritance diagram for octomap::OccupancyOcTreeBase< NODE >:
Collaboration diagram for octomap::OccupancyOcTreeBase< NODE >:

Public Types

typedef leaf_iterator iterator
typedef NODE NodeType
 Make the templated NODE type available from the outside.

Public Member Functions

OcTreeKey adjustKeyAtDepth (const OcTreeKey &key, unsigned int depth) const
 Adjusts a 3D key from the lowest level to correspond to a higher depth (by shifting the key values)
unsigned short int adjustKeyAtDepth (unsigned short int key, unsigned int depth) const
 Adjusts a single key value from the lowest level to correspond to a higher depth (by shifting the key value)
bool bbxSet () const
iterator begin (unsigned char maxDepth=0) const
leaf_iterator begin_leafs (unsigned char maxDepth=0) const
leaf_bbx_iterator begin_leafs_bbx (const OcTreeKey &min, const OcTreeKey &max, unsigned char maxDepth=0) const
leaf_bbx_iterator begin_leafs_bbx (const point3d &min, const point3d &max, unsigned char maxDepth=0) const
tree_iterator begin_tree (unsigned char maxDepth=0) const
size_t calcNumNodes () const
 Traverses the tree to calculate the total number of nodes.
void calcNumThresholdedNodes (unsigned int &num_thresholded, unsigned int &num_other) const
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().
KeyBoolMap::const_iterator changedKeysBegin ()
 Iterator to traverse all keys of changed nodes.
KeyBoolMap::const_iterator changedKeysEnd ()
 Iterator to traverse all keys of changed nodes.
void clear ()
 Deletes the complete tree structure (only the root node will remain)
bool computeRay (const point3d &origin, const point3d &end, std::vector< point3d > &ray)
 Traces a ray from origin to end (excluding), returning the coordinates of all nodes traversed by the beam.
bool computeRayKeys (const point3d &origin, const point3d &end, KeyRay &ray) const
 Traces a ray from origin to end (excluding), returning an OcTreeKey of all nodes traversed by the beam.
void computeUpdate (const Pointcloud &scan, const octomap::point3d &origin, KeySet &free_cells, KeySet &occupied_cells, double maxrange)
 Helper for insertScan.
unsigned short int coordToKey (double coordinate) const
 Converts from a single coordinate into a discrete key.
unsigned short int coordToKey (double coordinate, unsigned depth) const
 Converts from a single coordinate into a discrete key at a given depth.
OcTreeKey coordToKey (const point3d &coord, unsigned depth) const
 Converts from a 3D coordinate into a 3D addressing key at a given depth.
OcTreeKey coordToKey (const point3d &coord) const
 Converts from a 3D coordinate into a 3D addressing key.
bool coordToKeyChecked (const point3d &coord, OcTreeKey &key) const
 Converts a 3D coordinate into a 3D OcTreeKey, with boundary checking.
bool coordToKeyChecked (const point3d &coord, unsigned depth, OcTreeKey &key) const
 Converts a 3D coordinate into a 3D OcTreeKey at a certain depth, with boundary checking.
bool coordToKeyChecked (double coordinate, unsigned short int &key) const
 Converts a single coordinate into a discrete addressing key, with boundary checking.
bool coordToKeyChecked (double coordinate, unsigned depth, unsigned short int &key) const
 Converts a single coordinate into a discrete addressing key, with boundary checking.
virtual AbstractOcTreecreate () const =0
 virtual constructor: creates a new object of same type
OcTreeBaseImpl deepCopy () const
bool deleteNode (const OcTreeKey &key, unsigned int depth=0)
 Delete a node (if exists) given an addressing key.
bool deleteNode (float x, float y, float z, unsigned int depth=0)
 Delete a node (if exists) given a 3d point.
bool deleteNode (const point3d &value, unsigned int depth=0)
 Delete a node (if exists) given a 3d point.
 DEPRECATED (void getOccupied(std::list< OcTreeVolume > &binary_nodes, std::list< OcTreeVolume > &delta_nodes, unsigned int max_depth=0) const)
 Traverses the tree and collects all OcTreeVolumes regarded as occupied.
 DEPRECATED (void getOccupiedLeafsBBX(point3d_list &node_centers, point3d min, point3d max) const)
 returns occupied leafs within a bounding box defined by min and max.
 DEPRECATED (double genCoordFromKey(const unsigned short int &key, unsigned depth) const)
 DEPRECATED (void getFreespace(std::list< OcTreeVolume > &free_volumes, unsigned int max_depth=0) const)
 Convenience function to return all free nodes in the OcTree.
 DEPRECATED (void getOccupied(std::list< OcTreeVolume > &occupied_volumes, unsigned int max_depth=0) const)
 Convenience function to return all occupied nodes in the OcTree.
 DEPRECATED (void getFreespace(std::list< OcTreeVolume > &binary_nodes, std::list< OcTreeVolume > &delta_nodes, unsigned int max_depth=0) const)
 Traverses the tree and collects all OcTreeVolumes regarded as free.
 DEPRECATED (bool genKeyValue(double coordinate, unsigned short int &keyval) const)
 DEPRECATED (bool genKey(const point3d &point, OcTreeKey &key) const )
 DEPRECATED (bool genKeyValueAtDepth(const unsigned short int keyval, unsigned int depth, unsigned short int &out_keyval) const )
 DEPRECATED (bool genKeyAtDepth(const OcTreeKey &key, unsigned int depth, OcTreeKey &out_key) const )
 DEPRECATED (bool genCoordFromKey(const unsigned short int &key, unsigned depth, float &coord) const )
 DEPRECATED (inline bool genCoordFromKey(const unsigned short int &key, float &coord, unsigned depth) const)
 DEPRECATED (inline bool genCoordFromKey(const unsigned short int &key, float &coord) const)
 DEPRECATED (inline double genCoordFromKey(const unsigned short int &key) const)
 DEPRECATED (inline bool genCoords(const OcTreeKey &key, unsigned int depth, point3d &point) const)
 DEPRECATED (inline void genPos(const OcTreeKey &key, int depth, unsigned int &pos) const)
 generate child index (between 0 and 7) from key at given tree depth DEPRECATED
 DEPRECATED (void getOccupied(point3d_list &node_centers, unsigned int max_depth=0) const)
 Convenience function to return all occupied nodes in the OcTree.
void enableChangeDetection (bool enable)
 track or ignore changes while inserting scans (default: ignore)
const iterator end () const
const leaf_iterator end_leafs () const
const leaf_bbx_iterator end_leafs_bbx () const
const tree_iterator end_tree () const
virtual void expand ()
 Expands all pruned nodes (reverse of prune())
point3d getBBXBounds () const
point3d getBBXCenter () const
point3d getBBXMax () const
point3d getBBXMin () const
double getClampingThresMax () const
float getClampingThresMaxLog () const
double getClampingThresMin () const
float getClampingThresMinLog () const
void getMetricMax (double &x, double &y, double &z) const
 maximum value of the bounding box of all known space in x, y, z
virtual void getMetricMax (double &x, double &y, double &z)
 maximum value of the bounding box of all known space in x, y, z
void getMetricMin (double &x, double &y, double &z) const
 minimum value of the bounding box of all known space in x, y, z
virtual void getMetricMin (double &x, double &y, double &z)
 minimum value of the bounding box of all known space in x, y, z
virtual void getMetricSize (double &x, double &y, double &z)
 Size of OcTree (all known space) in meters for x, y and z dimension.
double getNodeSize (unsigned depth) const
size_t getNumLeafNodes () const
 Traverses the tree to calculate the total number of leaf nodes.
double getOccupancyThres () const
float getOccupancyThresLog () const
double getProbHit () const
float getProbHitLog () const
double getProbMiss () const
float getProbMissLog () const
double getResolution () const
NODE * getRoot () const
unsigned int getTreeDepth () const
std::string getTreeType () const
 returns actual class name as string for identification
void getUnknownLeafCenters (point3d_list &node_centers, point3d pmin, point3d pmax) const
 return centers of leafs that do NOT exist (but could) in a given bounding box
bool inBBX (const point3d &p) const
bool inBBX (const OcTreeKey &key) const
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.
virtual void insertScan (const Pointcloud &scan, const point3d &sensor_origin, const pose6d &frame_origin, double maxrange=-1., bool pruning=true, bool lazy_eval=false)
 Integrate a 3d scan, transform scan before tree update.
virtual void insertScan (const ScanNode &scan, double maxrange=-1., bool pruning=true, bool lazy_eval=false)
 Insert a 3d scan (given as a ScanNode) into the tree.
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 insertScanNaive (const Pointcloud &pc, const point3d &origin, double maxrange, bool pruning=true, bool lazy_eval=false)
 for testing only
virtual void integrateHit (NODE *occupancyNode) const
 integrate a "hit" measurement according to the tree's sensor model
virtual void integrateMiss (NODE *occupancyNode) const
 integrate a "miss" measurement according to the tree's sensor model
bool isNodeAtThreshold (const OcTreeNode *occupancyNode) const
 queries whether a node is at the clamping threshold according to the tree's parameter
bool isNodeAtThreshold (const OcTreeNode &occupancyNode) const
 queries whether a node is at the clamping threshold according to the tree's parameter
bool isNodeOccupied (const OcTreeNode *occupancyNode) const
 queries whether a node is occupied according to the tree's parameter for "occupancy"
bool isNodeOccupied (const OcTreeNode &occupancyNode) const
 queries whether a node is occupied according to the tree's parameter for "occupancy"
double keyToCoord (unsigned short int key, unsigned depth) const
 converts from a discrete key at a given depth into a coordinate corresponding to the key's center
point3d keyToCoord (const OcTreeKey &key, unsigned depth) const
 converts from an addressing key at a given depth into a coordinate corresponding to the key's center
double keyToCoord (unsigned short int key) const
 converts from a discrete key at the lowest tree level into a coordinate corresponding to the key's center
point3d keyToCoord (const OcTreeKey &key) const
 converts from an addressing key at the lowest tree level into a coordinate corresponding to the key's center
size_t memoryFullGrid ()
virtual size_t memoryUsage () const
virtual size_t memoryUsageNode () const
virtual void nodeToMaxLikelihood (NODE *occupancyNode) const
 converts the node to the maximum likelihood value according to the tree's parameter for "occupancy"
virtual void nodeToMaxLikelihood (NODE &occupancyNode) const
 converts the node to the maximum likelihood value according to the tree's parameter for "occupancy"
 OccupancyOcTreeBase (double resolution)
 Default constructor, sets resolution of leafs.
 OccupancyOcTreeBase (const OccupancyOcTreeBase< NODE > &rhs)
 Copy constructor.
bool operator== (const OcTreeBaseImpl< NODE, AbstractOccupancyOcTree > &rhs) const
virtual void prune ()
 Lossless compression of OcTree: merge children to parent when there are eight children with identical values.
bool readBinary (const std::string &filename)
 Reads OcTree from a binary file.
bool readBinary (std::istream &s)
 Reads an OcTree from an input stream.
std::istream & readBinaryData (std::istream &s)
 Reads only the data (=tree structure) from the input stream.
std::istream & readBinaryNode (std::istream &s, NODE *node) const
 Read node from binary stream (max-likelihood value), recursively continue with all children.
std::istream & readData (std::istream &s)
 Read all nodes from the input stream (without file header), for this the tree needs to be already created.
void resetChangeDetection ()
 Reset the set of changed keys. Call this after you obtained all changed nodes.
NODE * search (const OcTreeKey &key, unsigned int depth=0) const
 Search a node at specified depth given an addressing key (depth=0: search full tree depth)
NODE * search (float x, float y, float z, unsigned int depth=0) const
 Search node at specified depth given a 3d point (depth=0: search full tree depth)
NODE * search (const point3d &value, unsigned int depth=0) const
 Search node at specified depth given a 3d point (depth=0: search full tree depth)
void setBBXMax (point3d &max)
 sets the maximum for a query bounding box to use
void setBBXMin (point3d &min)
 sets the minimum for a query bounding box to use
void setClampingThresMax (double thresProb)
 sets the maximum threshold for occupancy clamping (sensor model)
void setClampingThresMin (double thresProb)
 sets the minimum threshold for occupancy clamping (sensor model)
void setOccupancyThres (double prob)
 sets the threshold for occupancy (sensor model)
void setProbHit (double prob)
 sets the probablility for a "hit" (will be converted to logodds) - sensor model
void setProbMiss (double prob)
 sets the probablility for a "miss" (will be converted to logodds) - sensor model
void setResolution (double r)
 Change the resolution of the octree, scaling all voxels.
virtual size_t size () const
virtual void toMaxLikelihood ()
 Creates the maximum likelihood map by calling toMaxLikelihood on all tree nodes, setting their occupancy to the corresponding occupancy thresholds.
void updateInnerOccupancy ()
 Updates the occupancy of all inner nodes to reflect their children's occupancy.
virtual NODE * updateNode (const point3d &value, float log_odds_update, bool lazy_eval=false)
 Manipulate log_odds value of voxel directly.
virtual NODE * updateNode (const OcTreeKey &key, bool occupied, bool lazy_eval=false)
 Integrate occupancy measurement.
virtual NODE * updateNode (const point3d &value, bool occupied, bool lazy_eval=false)
 Integrate occupancy measurement.
virtual NODE * updateNode (const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
 Manipulate log_odds value of voxel directly.
virtual void updateNodeLogOdds (NODE *occupancyNode, const float &update) const
void useBBXLimit (bool enable)
 use or ignore BBX limit (default: ignore)
double volume ()
bool write (const std::string &filename) const
 Write file header and complete tree to file (serialization)
bool write (std::ostream &s) const
 Write file header and complete tree to stream (serialization)
bool writeBinary (std::ostream &s)
 Writes compressed maximum likelihood OcTree to a binary stream.
bool writeBinary (const std::string &filename)
 Writes OcTree to a binary file using writeBinary().
bool writeBinaryConst (std::ostream &s) const
 Writes the maximum likelihood OcTree to a binary stream (const variant).
bool writeBinaryConst (const std::string &filename) const
 Writes OcTree to a binary file using writeBinaryConst().
std::ostream & writeBinaryData (std::ostream &s) const
 Writes the data of the tree (without header) to the stream, recursively calling writeBinaryNode (starting with root)
std::ostream & writeBinaryNode (std::ostream &s, const NODE *node) const
 Write node to binary stream (max-likelihood value), recursively continue with all children.
std::ostream & writeData (std::ostream &s) const
 Write complete state of tree to stream (without file header) unmodified.
virtual ~OccupancyOcTreeBase ()

Static Public Member Functions

static AbstractOcTreecreateTree (const std::string id, double res)
 Creates a certain OcTree (factory pattern)
static AbstractOcTreeread (std::istream &s)
 Read the file header, create the appropriate class and deserialize.
static AbstractOcTreeread (const std::string &filename)
 Read the file header, create the appropriate class and deserialize.

Protected Member Functions

void calcMinMax ()
 recalculates min and max in x, y, z. Does nothing when tree size didn't change.
void calcNumNodesRecurs (NODE *node, size_t &num_nodes) const
void calcNumThresholdedNodesRecurs (NODE *node, unsigned int &num_thresholded, unsigned int &num_other) const
bool deleteNodeRecurs (NODE *node, unsigned int depth, unsigned int max_depth, const OcTreeKey &key)
 recursive call of deleteNode()
void expandRecurs (NODE *node, unsigned int depth, unsigned int max_depth)
 recursive call of expand()
size_t getNumLeafNodesRecurs (const NODE *parent) const
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
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.
 OccupancyOcTreeBase (double resolution, unsigned int tree_depth, unsigned int tree_max_val)
 Constructor to enable derived classes to change tree constants.
void pruneRecurs (NODE *node, unsigned int depth, unsigned int max_depth, unsigned int &num_pruned)
 recursive call of prune()
bool readBinaryLegacyHeader (std::istream &s, unsigned int &size, double &res)
 Try to read the old binary format for conversion, will be removed in the future.
void toMaxLikelihoodRecurs (NODE *node, unsigned int depth, unsigned int max_depth)
void updateInnerOccupancyRecurs (NODE *node, unsigned int depth)
NODE * updateNodeRecurs (NODE *node, bool node_just_created, const OcTreeKey &key, unsigned int depth, const float &log_odds_update, bool lazy_eval=false)

Static Protected Member Functions

static bool readHeader (std::istream &s, std::string &id, unsigned &size, double &res)
static void registerTreeType (AbstractOcTree *tree)

Protected Attributes

point3d bbx_max
OcTreeKey bbx_max_key
point3d bbx_min
OcTreeKey bbx_min_key
KeyBoolMap changed_keys
 Set of leaf keys (lowest level) which changed since last resetChangeDetection.
float clamping_thres_max
float clamping_thres_min
KeyRay keyray
const leaf_bbx_iterator leaf_iterator_bbx_end
const leaf_iterator leaf_iterator_end
double max_value [3]
 max in x, y, z
double min_value [3]
 min in x, y, z
float occ_prob_thres_log
float prob_hit_log
float prob_miss_log
double resolution
 in meters
double resolution_factor
 = 1. / resolution
NODE * root
bool size_changed
 flag to denote whether the octree extent changed (for lazy min/max eval)
std::vector< double > sizeLookupTable
 contains the size of a voxel at level i (0: root node). tree_depth+1 levels (incl. 0)
point3d tree_center
const unsigned int tree_depth
 Maximum tree depth is fixed to 16 currently.
const tree_iterator tree_iterator_end
const unsigned int tree_max_val
size_t tree_size
 number of nodes in tree
bool use_bbx_limit
 use bounding box for queries (needs to be set)?
bool use_change_detection

Static Protected Attributes

static const std::string binaryFileHeader = "# Octomap OcTree binary file"
static const std::string fileHeader = "# Octomap OcTree file"

Detailed Description

template<class NODE>
class octomap::OccupancyOcTreeBase< NODE >

Base implementation for Occupancy Octrees (e.g.

for mapping). AbstractOccupancyOcTree serves as a common base interface for all these classes. Each class used as NODE type needs to be derived from OccupancyOcTreeNode.

This tree implementation has a maximum depth of 16. At a resolution of 1 cm, values have to be < +/- 327.68 meters (2^15)

This limitation enables the use of an efficient key generation method which uses the binary representation of the data.

Note:
The tree does not save individual points.
Template Parameters:
NODENode class to be used in tree (usually derived from OcTreeDataNode)

Member Typedef Documentation

typedef NODE octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::NodeType [inherited]

Make the templated NODE type available from the outside.


Constructor & Destructor Documentation

template<class NODE >
octomap::OccupancyOcTreeBase< NODE >::OccupancyOcTreeBase ( double  resolution)

Default constructor, sets resolution of leafs.

template<class NODE >
octomap::OccupancyOcTreeBase< NODE >::~OccupancyOcTreeBase ( ) [virtual]
template<class NODE>
octomap::OccupancyOcTreeBase< NODE >::OccupancyOcTreeBase ( double  resolution,
unsigned int  tree_depth,
unsigned int  tree_max_val 
) [protected]

Constructor to enable derived classes to change tree constants.

This usually requires a re-implementation of some core tree-traversal functions as well!


Member Function Documentation

OcTreeKey octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::adjustKeyAtDepth ( const OcTreeKey key,
unsigned int  depth 
) const [inline, inherited]

Adjusts a 3D key from the lowest level to correspond to a higher depth (by shifting the key values)

Parameters:
keyInput key, at the lowest tree level
depthTarget depth level for the new key
Returns:
Key for the new depth level
unsigned short int octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::adjustKeyAtDepth ( unsigned short int  key,
unsigned int  depth 
) const [inherited]

Adjusts a single key value from the lowest level to correspond to a higher depth (by shifting the key value)

Parameters:
keyInput key, at the lowest tree level
depthTarget depth level for the new key
Returns:
Key for the new depth level
template<class NODE>
bool octomap::OccupancyOcTreeBase< NODE >::bbxSet ( ) const [inline]
iterator octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::begin ( unsigned char  maxDepth = 0) const [inline, inherited]
Returns:
beginning of the tree as leaf iterator
leaf_iterator octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::begin_leafs ( unsigned char  maxDepth = 0) const [inline, inherited]
Returns:
beginning of the tree as leaf iterator
leaf_bbx_iterator octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::begin_leafs_bbx ( const OcTreeKey min,
const OcTreeKey max,
unsigned char  maxDepth = 0 
) const [inline, inherited]
Returns:
beginning of the tree as leaf iterator in a bounding box
leaf_bbx_iterator octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::begin_leafs_bbx ( const point3d min,
const point3d max,
unsigned char  maxDepth = 0 
) const [inline, inherited]
Returns:
beginning of the tree as leaf iterator in a bounding box
tree_iterator octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::begin_tree ( unsigned char  maxDepth = 0) const [inline, inherited]
Returns:
beginning of the tree as iterator to all nodes (incl. inner)
void octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::calcMinMax ( ) [protected, inherited]

recalculates min and max in x, y, z. Does nothing when tree size didn't change.

size_t octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::calcNumNodes ( ) const [inherited]

Traverses the tree to calculate the total number of nodes.

void octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::calcNumNodesRecurs ( NODE *  node,
size_t &  num_nodes 
) const [protected, inherited]
template<class NODE >
void octomap::OccupancyOcTreeBase< NODE >::calcNumThresholdedNodes ( unsigned int &  num_thresholded,
unsigned int &  num_other 
) const

Referenced by main().

template<class NODE>
void octomap::OccupancyOcTreeBase< NODE >::calcNumThresholdedNodesRecurs ( NODE *  node,
unsigned int &  num_thresholded,
unsigned int &  num_other 
) const [protected]
template<class NODE >
bool octomap::OccupancyOcTreeBase< NODE >::castRay ( const point3d origin,
const point3d direction,
point3d end,
bool  ignoreUnknownCells = false,
double  maxRange = -1.0 
) const [virtual]

Performs raycasting in 3d, similar to computeRay().

A ray is cast from origin with a given direction, the first occupied cell is returned (as center coordinate). If the starting coordinate is already occupied in the tree, this coordinate will be returned as a hit.

Parameters:
originstarting coordinate of ray
directionA vector pointing in the direction of the raycast. Does not need to be normalized.
endreturns the center of the cell that was hit by the ray, if successful
ignoreUnknownCellswhether unknown cells are ignored. If false (default), the raycast aborts when an unkown cell is hit.
maxRangeMaximum range after which the raycast is aborted (<= 0: no limit, default)
Returns:
whether or not an occupied cell was hit

---------- see OcTreeBase::computeRayKeys -----------

References octomath::Vector3::normalized(), OCTOMAP_ERROR, OCTOMAP_ERROR_STR, OCTOMAP_WARNING, and OCTOMAP_WARNING_STR.

Referenced by main().

template<class NODE>
KeyBoolMap::const_iterator octomap::OccupancyOcTreeBase< NODE >::changedKeysBegin ( ) [inline]

Iterator to traverse all keys of changed nodes.

you need to enableChangeDetection() first. Here, an OcTreeKey always refers to a node at the lowest tree level (its size is the minimum tree resolution)

Referenced by printChanges().

template<class NODE>
KeyBoolMap::const_iterator octomap::OccupancyOcTreeBase< NODE >::changedKeysEnd ( ) [inline]

Iterator to traverse all keys of changed nodes.

Referenced by printChanges().

void octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::clear ( ) [virtual, inherited]

Deletes the complete tree structure (only the root node will remain)

Implements octomap::AbstractOcTree.

bool octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::computeRay ( const point3d origin,
const point3d end,
std::vector< point3d > &  ray 
) [inherited]

Traces a ray from origin to end (excluding), returning the coordinates of all nodes traversed by the beam.

You still need to check if a node at that coordinate exists (e.g. with search()).

Note:
: use the faster computeRayKeys method if possible.
Parameters:
originstart coordinate of ray
endend coordinate of ray
rayKeyRay structure that holds the keys of all nodes traversed by the ray, excluding "end"
Returns:
Success of operation. Returning false usually means that one of the coordinates is out of the OcTree's range
bool octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::computeRayKeys ( const point3d origin,
const point3d end,
KeyRay ray 
) const [inherited]

Traces a ray from origin to end (excluding), returning an OcTreeKey of all nodes traversed by the beam.

You still need to check if a node at that coordinate exists (e.g. with search()).

Parameters:
originstart coordinate of ray
endend coordinate of ray
rayKeyRay structure that holds the keys of all nodes traversed by the ray, excluding "end"
Returns:
Success of operation. Returning false usually means that one of the coordinates is out of the OcTree's range
template<class NODE >
void octomap::OccupancyOcTreeBase< NODE >::computeUpdate ( const Pointcloud scan,
const octomap::point3d origin,
KeySet free_cells,
KeySet occupied_cells,
double  maxrange 
)

Helper for insertScan.

Computes all octree nodes affected by the point cloud integration at once. Here, occupied nodes have a preference over free ones.

Parameters:
scanpoint cloud measurement to be integrated
originorigin of the sensor for ray casting
free_cellskeys of nodes to be cleared
occupied_cellskeys of nodes to be marked occupied
maxrangemaximum range for raycasting (-1: unlimited)

References octomap::Pointcloud::begin(), and octomap::Pointcloud::end().

unsigned short int octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::coordToKey ( double  coordinate) const [inline, inherited]

Converts from a single coordinate into a discrete key.

unsigned short int octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::coordToKey ( double  coordinate,
unsigned  depth 
) const [inherited]

Converts from a single coordinate into a discrete key at a given depth.

OcTreeKey octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::coordToKey ( const point3d coord) const [inline, inherited]

Converts from a 3D coordinate into a 3D addressing key.

OcTreeKey octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::coordToKey ( const point3d coord,
unsigned  depth 
) const [inline, inherited]

Converts from a 3D coordinate into a 3D addressing key at a given depth.

bool octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::coordToKeyChecked ( const point3d coord,
OcTreeKey key 
) const [inherited]

Converts a 3D coordinate into a 3D OcTreeKey, with boundary checking.

Parameters:
coord3d coordinate of a point
keyvalues that will be computed, an array of fixed size 3.
Returns:
true if point is within the octree (valid), false otherwise
bool octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::coordToKeyChecked ( const point3d coord,
unsigned  depth,
OcTreeKey key 
) const [inherited]

Converts a 3D coordinate into a 3D OcTreeKey at a certain depth, with boundary checking.

Parameters:
coord3d coordinate of a point
depthlevel of the key from the top
keyvalues that will be computed, an array of fixed size 3.
Returns:
true if point is within the octree (valid), false otherwise
bool octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::coordToKeyChecked ( double  coordinate,
unsigned short int &  key 
) const [inherited]

Converts a single coordinate into a discrete addressing key, with boundary checking.

Parameters:
coordinate3d coordinate of a point
keydiscrete 16 bit adressing key, result
Returns:
true if coordinate is within the octree bounds (valid), false otherwise
bool octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::coordToKeyChecked ( double  coordinate,
unsigned  depth,
unsigned short int &  key 
) const [inherited]

Converts a single coordinate into a discrete addressing key, with boundary checking.

Parameters:
coordinate3d coordinate of a point
depthlevel of the key from the top
keydiscrete 16 bit adressing key, result
Returns:
true if coordinate is within the octree bounds (valid), false otherwise
virtual AbstractOcTree* octomap::AbstractOcTree::create ( ) const [pure virtual, inherited]

virtual constructor: creates a new object of same type

Implemented in octomap::OcTreeBase< NODE >.

Referenced by octomap::AbstractOcTree::createTree().

AbstractOcTree * octomap::AbstractOcTree::createTree ( const std::string  id,
double  res 
) [static, inherited]

Creates a certain OcTree (factory pattern)

Parameters:
idunique ID of OcTree
resresolution of OcTree
Returns:
pointer to newly created OcTree (empty). NULL if the ID is unknown!

References octomap::AbstractOcTree::create(), OCTOMAP_ERROR, and octomap::AbstractOcTree::setResolution().

Referenced by octomap::AbstractOcTree::read().

OcTreeBaseImpl octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::deepCopy ( ) const [inherited]
bool octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::deleteNode ( const point3d value,
unsigned int  depth = 0 
) [inherited]

Delete a node (if exists) given a 3d point.

Will always delete at the lowest level unless depth !=0, and expand pruned inner nodes as needed. Pruned nodes at level "depth" will directly be deleted as a whole.

bool octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::deleteNode ( float  x,
float  y,
float  z,
unsigned int  depth = 0 
) [inherited]

Delete a node (if exists) given a 3d point.

Will always delete at the lowest level unless depth !=0, and expand pruned inner nodes as needed. Pruned nodes at level "depth" will directly be deleted as a whole.

bool octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::deleteNode ( const OcTreeKey key,
unsigned int  depth = 0 
) [inherited]

Delete a node (if exists) given an addressing key.

Will always delete at the lowest level unless depth !=0, and expand pruned inner nodes as needed. Pruned nodes at level "depth" will directly be deleted as a whole.

bool octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::deleteNodeRecurs ( NODE *  node,
unsigned int  depth,
unsigned int  max_depth,
const OcTreeKey key 
) [protected, inherited]

recursive call of deleteNode()

template<class NODE>
octomap::OccupancyOcTreeBase< NODE >::DEPRECATED ( void getFreespace(std::list< OcTreeVolume > &free_volumes, unsigned int max_depth=0)  const)

Convenience function to return all free nodes in the OcTree.

Note:
Deprecated, will be removed in the future. Direcly access the nodes with iterators instead!
Parameters:
free_volumeslist of free nodes (as point3d and size of the volume)
max_depthDepth limit of query. 0 (default): no depth limit
template<class NODE>
octomap::OccupancyOcTreeBase< NODE >::DEPRECATED ( void getFreespace(std::list< OcTreeVolume > &binary_nodes, std::list< OcTreeVolume > &delta_nodes, unsigned int max_depth=0)  const)

Traverses the tree and collects all OcTreeVolumes regarded as free.

Inner nodes with both occupied and free children are regarded as occupied.

Note:
Deprecated, will be removed in the future. Direcly access the nodes with iterators instead!
Parameters:
binary_nodeslist of binary OcTreeVolumes which are free
delta_nodeslist of delta OcTreeVolumes which are free
max_depthDepth limit of query. 0 (default): no depth limit
template<class NODE>
octomap::OccupancyOcTreeBase< NODE >::DEPRECATED ( void getOccupied(std::list< OcTreeVolume > &occupied_volumes, unsigned int max_depth=0)  const)

Convenience function to return all occupied nodes in the OcTree.

Note:
Deprecated, will be removed in the future. Direcly access the nodes with iterators instead! *
Parameters:
occupied_volumeslist of occpupied nodes (as point3d and size of the volume)
max_depthDepth limit of query. 0 (default): no depth limit
octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::DEPRECATED ( bool genKeyValue(double coordinate, unsigned short int &keyval)  const) [inline, inherited]
octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::DEPRECATED ( inline bool genCoords(const OcTreeKey &key, unsigned int depth, point3d &point)  const) [inline, inherited]

Will always return true, there is no more boundary check here

octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::DEPRECATED ( bool genKey(const point3d &point, OcTreeKey &key)  const) [inline, inherited]
octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::DEPRECATED ( bool genKeyValueAtDepth(const unsigned short int keyval, unsigned int depth, unsigned short int &out_keyval)  const) [inherited]
octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::DEPRECATED ( bool genKeyAtDepth(const OcTreeKey &key, unsigned int depth, OcTreeKey &out_key)  const) [inherited]
octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::DEPRECATED ( bool genCoordFromKey(const unsigned short int &key, unsigned depth, float &coord)  const) [inline, inherited]
octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::DEPRECATED ( inline bool genCoordFromKey(const unsigned short int &key, float &coord)  const) [inline, inherited]
octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::DEPRECATED ( double genCoordFromKey(const unsigned short int &key, unsigned depth)  const) [inline, inherited]
octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::DEPRECATED ( inline double genCoordFromKey(const unsigned short int &key)  const) [inline, inherited]
octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::DEPRECATED ( inline void genPos(const OcTreeKey &key, int depth, unsigned int &pos)  const) [inline, inherited]

generate child index (between 0 and 7) from key at given tree depth DEPRECATED

octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::DEPRECATED ( inline bool genCoordFromKey(const unsigned short int &key, float &coord, unsigned depth)  const) [inline, inherited]
template<class NODE>
octomap::OccupancyOcTreeBase< NODE >::DEPRECATED ( void getOccupiedLeafsBBX(point3d_list &node_centers, point3d min, point3d max)  const)

returns occupied leafs within a bounding box defined by min and max.

Note:
Deprecated, will be removed in the future. Direcly access the nodes with iterators instead!
template<class NODE>
octomap::OccupancyOcTreeBase< NODE >::DEPRECATED ( void getOccupied(point3d_list &node_centers, unsigned int max_depth=0)  const)

Convenience function to return all occupied nodes in the OcTree.

Note:
Deprecated, will be removed in the future. Direcly access the nodes with iterators instead!
Parameters:
node_centerslist of occpupied nodes (as point3d)
max_depthDepth limit of query. 0 (default): no depth limit
template<class NODE>
octomap::OccupancyOcTreeBase< NODE >::DEPRECATED ( void getOccupied(std::list< OcTreeVolume > &binary_nodes, std::list< OcTreeVolume > &delta_nodes, unsigned int max_depth=0)  const)

Traverses the tree and collects all OcTreeVolumes regarded as occupied.

Inner nodes with both occupied and free children are regarded as occupied. This should be for internal use only, use getOccupied(occupied_volumes) instead.

Note:
Deprecated, will be removed in the future. Direcly access the nodes with iterators instead! *
Parameters:
binary_nodeslist of binary OcTreeVolumes which are occupied
delta_nodeslist of delta OcTreeVolumes which are occupied
max_depthDepth limit of query. 0 (default): no depth limit
template<class NODE>
void octomap::OccupancyOcTreeBase< NODE >::enableChangeDetection ( bool  enable) [inline]

track or ignore changes while inserting scans (default: ignore)

Referenced by main().

const iterator octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::end ( ) const [inline, inherited]
Returns:
end of the tree as leaf iterator
const leaf_iterator octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::end_leafs ( ) const [inline, inherited]
Returns:
end of the tree as leaf iterator
const leaf_bbx_iterator octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::end_leafs_bbx ( ) const [inline, inherited]
Returns:
end of the tree as leaf iterator in a bounding box
const tree_iterator octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::end_tree ( ) const [inline, inherited]
Returns:
end of the tree as iterator to all nodes (incl. inner)
virtual void octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::expand ( ) [virtual, inherited]

Expands all pruned nodes (reverse of prune())

Note:
This is an expensive operation, especially when the tree is nearly empty!

Implements octomap::AbstractOcTree.

void octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::expandRecurs ( NODE *  node,
unsigned int  depth,
unsigned int  max_depth 
) [protected, inherited]

recursive call of expand()

template<class NODE >
point3d octomap::OccupancyOcTreeBase< NODE >::getBBXBounds ( ) const
template<class NODE >
point3d octomap::OccupancyOcTreeBase< NODE >::getBBXCenter ( ) const
template<class NODE>
point3d octomap::OccupancyOcTreeBase< NODE >::getBBXMax ( ) const [inline]
Returns:
the currently set maximum for bounding box queries, if set
template<class NODE>
point3d octomap::OccupancyOcTreeBase< NODE >::getBBXMin ( ) const [inline]
Returns:
the currently set minimum for bounding box queries, if set
double octomap::AbstractOccupancyOcTree::getClampingThresMax ( ) const [inline, inherited]
Returns:
maximum threshold for occupancy clamping in the sensor model (probability)

References octomap::AbstractOccupancyOcTree::clamping_thres_max, and octomap::probability().

float octomap::AbstractOccupancyOcTree::getClampingThresMaxLog ( ) const [inline, inherited]
Returns:
maximum threshold for occupancy clamping in the sensor model (logodds)

References octomap::AbstractOccupancyOcTree::clamping_thres_max.

double octomap::AbstractOccupancyOcTree::getClampingThresMin ( ) const [inline, inherited]
Returns:
minimum threshold for occupancy clamping in the sensor model (probability)

References octomap::AbstractOccupancyOcTree::clamping_thres_min, and octomap::probability().

float octomap::AbstractOccupancyOcTree::getClampingThresMinLog ( ) const [inline, inherited]
Returns:
minimum threshold for occupancy clamping in the sensor model (logodds)

References octomap::AbstractOccupancyOcTree::clamping_thres_min.

virtual void octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::getMetricMax ( double &  x,
double &  y,
double &  z 
) [virtual, inherited]

maximum value of the bounding box of all known space in x, y, z

Implements octomap::AbstractOcTree.

void octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::getMetricMax ( double &  x,
double &  y,
double &  z 
) const [virtual, inherited]

maximum value of the bounding box of all known space in x, y, z

Implements octomap::AbstractOcTree.

virtual void octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::getMetricMin ( double &  x,
double &  y,
double &  z 
) [virtual, inherited]

minimum value of the bounding box of all known space in x, y, z

Implements octomap::AbstractOcTree.

void octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::getMetricMin ( double &  x,
double &  y,
double &  z 
) const [virtual, inherited]

minimum value of the bounding box of all known space in x, y, z

Implements octomap::AbstractOcTree.

virtual void octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::getMetricSize ( double &  x,
double &  y,
double &  z 
) [virtual, inherited]

Size of OcTree (all known space) in meters for x, y and z dimension.

Implements octomap::AbstractOcTree.

double octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::getNodeSize ( unsigned  depth) const [inline, inherited]
size_t octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::getNumLeafNodes ( ) const [inherited]

Traverses the tree to calculate the total number of leaf nodes.

size_t octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::getNumLeafNodesRecurs ( const NODE *  parent) const [protected, inherited]
double octomap::AbstractOccupancyOcTree::getOccupancyThres ( ) const [inline, inherited]
Returns:
threshold (probability) for occupancy - sensor model

References octomap::AbstractOccupancyOcTree::occ_prob_thres_log, and octomap::probability().

float octomap::AbstractOccupancyOcTree::getOccupancyThresLog ( ) const [inline, inherited]
Returns:
threshold (logodds) for occupancy - sensor model

References octomap::AbstractOccupancyOcTree::occ_prob_thres_log.

template<class NODE>
void octomap::OccupancyOcTreeBase< NODE >::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 [protected]
double octomap::AbstractOccupancyOcTree::getProbHit ( ) const [inline, inherited]
Returns:
probablility for a "hit" in the sensor model (probability)

References octomap::AbstractOccupancyOcTree::prob_hit_log, and octomap::probability().

float octomap::AbstractOccupancyOcTree::getProbHitLog ( ) const [inline, inherited]
Returns:
probablility for a "hit" in the sensor model (logodds)

References octomap::AbstractOccupancyOcTree::prob_hit_log.

double octomap::AbstractOccupancyOcTree::getProbMiss ( ) const [inline, inherited]
Returns:
probablility for a "miss" in the sensor model (probability)

References octomap::AbstractOccupancyOcTree::prob_miss_log, and octomap::probability().

float octomap::AbstractOccupancyOcTree::getProbMissLog ( ) const [inline, inherited]
Returns:
probablility for a "miss" in the sensor model (logodds)

References octomap::AbstractOccupancyOcTree::prob_miss_log.

double octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::getResolution ( ) const [inline, virtual, inherited]
NODE* octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::getRoot ( ) const [inline, inherited]
Returns:
Pointer to the root node of the tree. This pointer should not be modified or deleted externally, the OcTree manages its memory itself.
unsigned int octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::getTreeDepth ( ) const [inline, inherited]
std::string octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::getTreeType ( ) const [inline, virtual, inherited]
void octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::getUnknownLeafCenters ( point3d_list node_centers,
point3d  pmin,
point3d  pmax 
) const [inherited]

return centers of leafs that do NOT exist (but could) in a given bounding box

template<class NODE >
bool octomap::OccupancyOcTreeBase< NODE >::inBBX ( const point3d p) const
Returns:
true if point is in the currently set bounding box

References octomath::Vector3::x(), octomath::Vector3::y(), and octomath::Vector3::z().

template<class NODE >
bool octomap::OccupancyOcTreeBase< NODE >::inBBX ( const OcTreeKey key) const
Returns:
true if key is in the currently set bounding box
template<class NODE >
bool octomap::OccupancyOcTreeBase< NODE >::insertRay ( const point3d origin,
const point3d end,
double  maxrange = -1.0,
bool  lazy_eval = false 
) [virtual]

Insert one ray between origin and end into the tree.

integrateMissOnRay() is called for the ray, the end point is updated as occupied.

Parameters:
originorigin of sensor in global coordinates
endendpoint of measurement in global coordinates
maxrangemaximum range after which the raycast should be aborted
lazy_evalwhether update of inner nodes is omitted after the update (default: false). This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
Returns:
success of operation

Referenced by generateSphereTree(), and main().

template<class NODE >
void octomap::OccupancyOcTreeBase< NODE >::insertScan ( const Pointcloud scan,
const point3d sensor_origin,
const pose6d frame_origin,
double  maxrange = -1.,
bool  pruning = true,
bool  lazy_eval = false 
) [virtual]

Integrate a 3d scan, transform scan before tree update.

Parameters:
scanPointcloud (measurement endpoints) relative to frame origin
sensor_originorigin of sensor relative to frame origin
frame_originorigin of reference frame, determines transform to be applied to cloud and sensor origin
maxrangemaximum range for how long individual beams are inserted (default -1: complete beam)
pruningwhether the tree is (losslessly) pruned after insertion (default: true)
lazy_evalwhether update of inner nodes is omitted after the update (default: false). This speeds up the insertion, but you need to call updateInnerOccupancy() when done.

References octomath::Pose6D::transform(), and octomap::Pointcloud::transform().

template<class NODE >
void octomap::OccupancyOcTreeBase< NODE >::insertScan ( const Pointcloud scan,
const octomap::point3d sensor_origin,
double  maxrange = -1.,
bool  pruning = true,
bool  lazy_eval = false 
) [virtual]

Integrate a Pointcloud (in global reference frame)

Parameters:
scanPointcloud (measurement endpoints), in global reference frame
sensor_originmeasurement origin in global reference frame
maxrangemaximum range for how long individual beams are inserted (default -1: complete beam)
pruningwhether the tree is (losslessly) pruned after insertion (default: true)
lazy_evalwhether update of inner nodes is omitted after the update (default: false). This speeds up the insertion, but you need to call updateInnerOccupancy() when done.

Referenced by main().

template<class NODE >
void octomap::OccupancyOcTreeBase< NODE >::insertScan ( const ScanNode scan,
double  maxrange = -1.,
bool  pruning = true,
bool  lazy_eval = false 
) [virtual]

Insert a 3d scan (given as a ScanNode) into the tree.

Parameters:
scanScanNode contains Pointcloud data and frame/sensor origin
maxrangemaximum range for how long individual beams are inserted (default -1: complete beam)
pruningwhether the tree is (losslessly) pruned after insertion (default: true)
lazy_evalwhether the tree is left 'dirty' after the update (default: false). This speeds up the insertion by not updating inner nodes, but you need to call updateInnerOccupancy() when done.

References octomath::Pose6D::inv(), octomap::ScanNode::pose, octomap::ScanNode::scan, and octomath::Pose6D::trans().

template<class NODE >
void octomap::OccupancyOcTreeBase< NODE >::insertScanNaive ( const Pointcloud pc,
const point3d origin,
double  maxrange,
bool  pruning = true,
bool  lazy_eval = false 
) [virtual]
template<class NODE>
void octomap::OccupancyOcTreeBase< NODE >::integrateHit ( NODE *  occupancyNode) const [virtual]

integrate a "hit" measurement according to the tree's sensor model

template<class NODE>
void octomap::OccupancyOcTreeBase< NODE >::integrateMiss ( NODE *  occupancyNode) const [virtual]

integrate a "miss" measurement according to the tree's sensor model

template<class NODE >
bool octomap::OccupancyOcTreeBase< NODE >::integrateMissOnRay ( const point3d origin,
const point3d end,
bool  lazy_eval = false 
) [inline, protected]

Traces a ray from origin to end and updates all voxels on the way as free.

The volume containing "end" is not updated.

bool octomap::AbstractOccupancyOcTree::isNodeAtThreshold ( const OcTreeNode occupancyNode) const [inline, inherited]

queries whether a node is at the clamping threshold according to the tree's parameter

References octomap::AbstractOccupancyOcTree::clamping_thres_max, octomap::AbstractOccupancyOcTree::clamping_thres_min, and octomap::OcTreeNode::getLogOdds().

bool octomap::AbstractOccupancyOcTree::isNodeAtThreshold ( const OcTreeNode occupancyNode) const [inline, inherited]

queries whether a node is at the clamping threshold according to the tree's parameter

References octomap::AbstractOccupancyOcTree::clamping_thres_max, octomap::AbstractOccupancyOcTree::clamping_thres_min, and octomap::OcTreeNode::getLogOdds().

bool octomap::AbstractOccupancyOcTree::isNodeOccupied ( const OcTreeNode occupancyNode) const [inline, inherited]

queries whether a node is occupied according to the tree's parameter for "occupancy"

References octomap::OcTreeNode::getLogOdds(), and octomap::AbstractOccupancyOcTree::occ_prob_thres_log.

bool octomap::AbstractOccupancyOcTree::isNodeOccupied ( const OcTreeNode occupancyNode) const [inline, inherited]

queries whether a node is occupied according to the tree's parameter for "occupancy"

References octomap::OcTreeNode::getLogOdds(), and octomap::AbstractOccupancyOcTree::occ_prob_thres_log.

point3d octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::keyToCoord ( const OcTreeKey key) const [inline, inherited]

converts from an addressing key at the lowest tree level into a coordinate corresponding to the key's center

double octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::keyToCoord ( unsigned short int  key,
unsigned  depth 
) const [inherited]

converts from a discrete key at a given depth into a coordinate corresponding to the key's center

double octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::keyToCoord ( unsigned short int  key) const [inline, inherited]

converts from a discrete key at the lowest tree level into a coordinate corresponding to the key's center

point3d octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::keyToCoord ( const OcTreeKey key,
unsigned  depth 
) const [inline, inherited]

converts from an addressing key at a given depth into a coordinate corresponding to the key's center

size_t octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::memoryFullGrid ( ) [inherited]
Returns:
Memory usage of a full grid of the same size as the OcTree in bytes (for comparison)
virtual size_t octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::memoryUsage ( ) const [virtual, inherited]
Returns:
Memory usage of the complete octree in bytes (may vary between architectures)

Implements octomap::AbstractOcTree.

virtual size_t octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::memoryUsageNode ( ) const [inline, virtual, inherited]
Returns:
Memory usage of the a single octree node

Implements octomap::AbstractOcTree.

template<class NODE>
void octomap::OccupancyOcTreeBase< NODE >::nodeToMaxLikelihood ( NODE *  occupancyNode) const [virtual]

converts the node to the maximum likelihood value according to the tree's parameter for "occupancy"

template<class NODE>
void octomap::OccupancyOcTreeBase< NODE >::nodeToMaxLikelihood ( NODE &  occupancyNode) const [virtual]

converts the node to the maximum likelihood value according to the tree's parameter for "occupancy"

bool octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::operator== ( const OcTreeBaseImpl< NODE, AbstractOccupancyOcTree > &  rhs) const [inherited]
virtual void octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::prune ( ) [virtual, inherited]

Lossless compression of OcTree: merge children to parent when there are eight children with identical values.

Implements octomap::AbstractOcTree.

void octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::pruneRecurs ( NODE *  node,
unsigned int  depth,
unsigned int  max_depth,
unsigned int &  num_pruned 
) [protected, inherited]

recursive call of prune()

AbstractOcTree * octomap::AbstractOcTree::read ( std::istream &  s) [static, inherited]

Read the file header, create the appropriate class and deserialize.

This creates a new octree which you need to delete yourself.

References octomap::AbstractOcTree::createTree(), octomap::AbstractOcTree::fileHeader, OCTOMAP_DEBUG_STR, OCTOMAP_ERROR_STR, octomap::AbstractOcTree::readData(), octomap::AbstractOcTree::readHeader(), and octomap::AbstractOcTree::size().

AbstractOcTree * octomap::AbstractOcTree::read ( const std::string &  filename) [static, inherited]

Read the file header, create the appropriate class and deserialize.

This creates a new octree which you need to delete yourself. If you expect or requre a specific kind of octree, use dynamic_cast afterwards:

 AbstractOcTree* tree = AbstractOcTree::read("filename.ot");
 OcTree* octree = dynamic_cast<OcTree*>(tree);

References OCTOMAP_ERROR_STR.

bool octomap::AbstractOccupancyOcTree::readBinary ( const std::string &  filename) [inherited]

Reads OcTree from a binary file.

Existing nodes of the tree are deleted before the tree is read.

Returns:
success of operation

References OCTOMAP_ERROR_STR, and octomap::AbstractOccupancyOcTree::readBinary().

template<class NODE >
std::istream & octomap::OccupancyOcTreeBase< NODE >::readBinaryData ( std::istream &  s) [virtual]

Reads only the data (=tree structure) from the input stream.

The tree needs to be constructed with the proper header information beforehand, see readBinary().

Implements octomap::AbstractOccupancyOcTree.

References octomap::OcTreeBaseImpl< NODE, INTERFACE >::calcNumNodes().

bool octomap::AbstractOccupancyOcTree::readBinaryLegacyHeader ( std::istream &  s,
unsigned int &  size,
double &  res 
) [protected, inherited]

Try to read the old binary format for conversion, will be removed in the future.

References octomap::AbstractOcTree::clear(), OCTOMAP_ERROR, OCTOMAP_ERROR_STR, and OCTOMAP_WARNING_STR.

Referenced by octomap::AbstractOccupancyOcTree::readBinary().

template<class NODE>
std::istream & octomap::OccupancyOcTreeBase< NODE >::readBinaryNode ( std::istream &  s,
NODE *  node 
) const

Read node from binary stream (max-likelihood value), recursively continue with all children.

This will set the log_odds_occupancy value of all leaves to either free or occupied.

Parameters:
s
Returns:
std::istream& octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::readData ( std::istream &  s) [virtual, inherited]

Read all nodes from the input stream (without file header), for this the tree needs to be already created.

For general file IO, you should probably use AbstractOcTree::read() instead.

Implements octomap::AbstractOcTree.

bool octomap::AbstractOcTree::readHeader ( std::istream &  s,
std::string &  id,
unsigned &  size,
double &  res 
) [static, protected, inherited]
template<class NODE>
void octomap::OccupancyOcTreeBase< NODE >::resetChangeDetection ( ) [inline]

Reset the set of changed keys. Call this after you obtained all changed nodes.

NODE* octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::search ( const point3d value,
unsigned int  depth = 0 
) const [inherited]

Search node at specified depth given a 3d point (depth=0: search full tree depth)

Returns:
pointer to node if found, NULL otherwise
NODE* octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::search ( const OcTreeKey key,
unsigned int  depth = 0 
) const [inherited]

Search a node at specified depth given an addressing key (depth=0: search full tree depth)

Returns:
pointer to node if found, NULL otherwise
NODE* octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::search ( float  x,
float  y,
float  z,
unsigned int  depth = 0 
) const [inherited]

Search node at specified depth given a 3d point (depth=0: search full tree depth)

Returns:
pointer to node if found, NULL otherwise
template<class NODE >
void octomap::OccupancyOcTreeBase< NODE >::setBBXMax ( point3d max)

sets the maximum for a query bounding box to use

References OCTOMAP_ERROR.

template<class NODE >
void octomap::OccupancyOcTreeBase< NODE >::setBBXMin ( point3d min)

sets the minimum for a query bounding box to use

References OCTOMAP_ERROR.

void octomap::AbstractOccupancyOcTree::setClampingThresMax ( double  thresProb) [inline, inherited]

sets the maximum threshold for occupancy clamping (sensor model)

References octomap::AbstractOccupancyOcTree::clamping_thres_max, and octomap::logodds().

Referenced by octomap::AbstractOccupancyOcTree::AbstractOccupancyOcTree().

void octomap::AbstractOccupancyOcTree::setClampingThresMin ( double  thresProb) [inline, inherited]

sets the minimum threshold for occupancy clamping (sensor model)

References octomap::AbstractOccupancyOcTree::clamping_thres_min, and octomap::logodds().

Referenced by octomap::AbstractOccupancyOcTree::AbstractOccupancyOcTree().

void octomap::AbstractOccupancyOcTree::setOccupancyThres ( double  prob) [inline, inherited]
void octomap::AbstractOccupancyOcTree::setProbHit ( double  prob) [inline, inherited]

sets the probablility for a "hit" (will be converted to logodds) - sensor model

References octomap::logodds(), and octomap::AbstractOccupancyOcTree::prob_hit_log.

Referenced by octomap::AbstractOccupancyOcTree::AbstractOccupancyOcTree().

void octomap::AbstractOccupancyOcTree::setProbMiss ( double  prob) [inline, inherited]

sets the probablility for a "miss" (will be converted to logodds) - sensor model

References octomap::logodds(), and octomap::AbstractOccupancyOcTree::prob_miss_log.

Referenced by octomap::AbstractOccupancyOcTree::AbstractOccupancyOcTree().

void octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::setResolution ( double  r) [virtual, inherited]

Change the resolution of the octree, scaling all voxels.

This will not preserve the (metric) scale!

Implements octomap::AbstractOcTree.

virtual size_t octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::size ( ) const [inline, virtual, inherited]
Returns:
The number of nodes in the tree

Implements octomap::AbstractOcTree.

template<class NODE >
void octomap::OccupancyOcTreeBase< NODE >::toMaxLikelihood ( ) [virtual]

Creates the maximum likelihood map by calling toMaxLikelihood on all tree nodes, setting their occupancy to the corresponding occupancy thresholds.

This enables a very efficient compression if you call prune() afterwards.

Implements octomap::AbstractOccupancyOcTree.

Referenced by main().

template<class NODE>
void octomap::OccupancyOcTreeBase< NODE >::toMaxLikelihoodRecurs ( NODE *  node,
unsigned int  depth,
unsigned int  max_depth 
) [protected]
template<class NODE >
void octomap::OccupancyOcTreeBase< NODE >::updateInnerOccupancy ( )

Updates the occupancy of all inner nodes to reflect their children's occupancy.

If you performed batch-updates with lazy evaluation enabled, you must call this before any queries to ensure correct multi-resolution behavior.

Reimplemented in octomap::ColorOcTree.

Referenced by main().

template<class NODE>
void octomap::OccupancyOcTreeBase< NODE >::updateInnerOccupancyRecurs ( NODE *  node,
unsigned int  depth 
) [protected]

Reimplemented in octomap::ColorOcTree.

template<class NODE >
NODE * octomap::OccupancyOcTreeBase< NODE >::updateNode ( const point3d value,
float  log_odds_update,
bool  lazy_eval = false 
) [virtual]

Manipulate log_odds value of voxel directly.

Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it.

Parameters:
value3d coordinate of the NODE that is to be updated
log_odds_updatevalue to be added (+) to log_odds value of node
lazy_evalwhether update of inner nodes is omitted after the update (default: false). This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
Returns:
pointer to the updated NODE

Implements octomap::AbstractOccupancyOcTree.

template<class NODE >
NODE * octomap::OccupancyOcTreeBase< NODE >::updateNode ( const OcTreeKey key,
bool  occupied,
bool  lazy_eval = false 
) [virtual]

Integrate occupancy measurement.

Parameters:
keyOcTreeKey of the NODE that is to be updated
occupiedtrue if the node was measured occupied, else false
lazy_evalwhether update of inner nodes is omitted after the update (default: false). This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
Returns:
pointer to the updated NODE

Implements octomap::AbstractOccupancyOcTree.

template<class NODE >
NODE * octomap::OccupancyOcTreeBase< NODE >::updateNode ( const OcTreeKey key,
float  log_odds_update,
bool  lazy_eval = false 
) [virtual]

Manipulate log_odds value of voxel directly.

Parameters:
keyOcTreeKey of the NODE that is to be updated
log_odds_updatevalue to be added (+) to log_odds value of node
lazy_evalwhether update of inner nodes is omitted after the update (default: false). This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
Returns:
pointer to the updated NODE

Implements octomap::AbstractOccupancyOcTree.

Referenced by main().

template<class NODE >
NODE * octomap::OccupancyOcTreeBase< NODE >::updateNode ( const point3d value,
bool  occupied,
bool  lazy_eval = false 
) [virtual]

Integrate occupancy measurement.

Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it.

Parameters:
value3d coordinate of the NODE that is to be updated
occupiedtrue if the node was measured occupied, else false
lazy_evalwhether update of inner nodes is omitted after the update (default: false). This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
Returns:
pointer to the updated NODE

Implements octomap::AbstractOccupancyOcTree.

template<class NODE>
void octomap::OccupancyOcTreeBase< NODE >::updateNodeLogOdds ( NODE *  occupancyNode,
const float &  update 
) const [virtual]

Reimplemented in octomap::OcTreeStamped.

template<class NODE>
NODE * octomap::OccupancyOcTreeBase< NODE >::updateNodeRecurs ( NODE *  node,
bool  node_just_created,
const OcTreeKey key,
unsigned int  depth,
const float &  log_odds_update,
bool  lazy_eval = false 
) [protected]
template<class NODE>
void octomap::OccupancyOcTreeBase< NODE >::useBBXLimit ( bool  enable) [inline]

use or ignore BBX limit (default: ignore)

double octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::volume ( ) [inherited]
bool octomap::AbstractOcTree::write ( const std::string &  filename) const [inherited]

Write file header and complete tree to file (serialization)

References OCTOMAP_ERROR_STR.

Referenced by main().

bool octomap::AbstractOcTree::write ( std::ostream &  s) const [inherited]
bool octomap::AbstractOccupancyOcTree::writeBinary ( const std::string &  filename) [inherited]

Writes OcTree to a binary file using writeBinary().

The OcTree is first converted to the maximum likelihood estimate and pruned.

Returns:
success of operation

References OCTOMAP_ERROR_STR.

Referenced by main().

bool octomap::AbstractOccupancyOcTree::writeBinary ( std::ostream &  s) [inherited]

Writes compressed maximum likelihood OcTree to a binary stream.

The OcTree is first converted to the maximum likelihood estimate and pruned for maximum compression.

Returns:
success of operation

References octomap::AbstractOcTree::prune(), octomap::AbstractOccupancyOcTree::toMaxLikelihood(), and octomap::AbstractOccupancyOcTree::writeBinaryConst().

bool octomap::AbstractOccupancyOcTree::writeBinaryConst ( std::ostream &  s) const [inherited]

Writes the maximum likelihood OcTree to a binary stream (const variant).

Files will be smaller when the tree is pruned first or by using writeBinary() instead.

Returns:
success of operation

References octomap::AbstractOccupancyOcTree::binaryFileHeader, octomap::AbstractOcTree::getResolution(), octomap::AbstractOcTree::getTreeType(), OCTOMAP_DEBUG, OCTOMAP_WARNING_STR, octomap::AbstractOcTree::size(), and octomap::AbstractOccupancyOcTree::writeBinaryData().

bool octomap::AbstractOccupancyOcTree::writeBinaryConst ( const std::string &  filename) const [inherited]

Writes OcTree to a binary file using writeBinaryConst().

The OcTree is not changed, in particular not pruned first. Files will be smaller when the tree is pruned first or by using writeBinary() instead.

Returns:
success of operation

References OCTOMAP_ERROR_STR.

Referenced by octomap::AbstractOccupancyOcTree::writeBinary().

template<class NODE >
std::ostream & octomap::OccupancyOcTreeBase< NODE >::writeBinaryData ( std::ostream &  s) const [virtual]

Writes the data of the tree (without header) to the stream, recursively calling writeBinaryNode (starting with root)

Implements octomap::AbstractOccupancyOcTree.

References OCTOMAP_DEBUG.

template<class NODE>
std::ostream & octomap::OccupancyOcTreeBase< NODE >::writeBinaryNode ( std::ostream &  s,
const NODE *  node 
) const

Write node to binary stream (max-likelihood value), recursively continue with all children.

This will discard the log_odds_occupancy value, writing all leaves as either free or occupied.

Parameters:
s
nodeOcTreeNode to write out, will recurse to all children
Returns:
std::ostream& octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::writeData ( std::ostream &  s) const [virtual, inherited]

Write complete state of tree to stream (without file header) unmodified.

Pruning the tree first produces smaller files (lossless compression)

Implements octomap::AbstractOcTree.


Field Documentation

template<class NODE>
OcTreeKey octomap::OccupancyOcTreeBase< NODE >::bbx_max_key [protected]
template<class NODE>
OcTreeKey octomap::OccupancyOcTreeBase< NODE >::bbx_min_key [protected]
const std::string octomap::AbstractOcTree::fileHeader = "# Octomap OcTree file" [static, protected, inherited]
double octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::max_value[3] [protected, inherited]

max in x, y, z

double octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::min_value[3] [protected, inherited]

min in x, y, z

double octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::resolution [protected, inherited]

in meters

double octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::resolution_factor [protected, inherited]

= 1. / resolution

NODE* octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::root [protected, inherited]
bool octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::size_changed [protected, inherited]

flag to denote whether the octree extent changed (for lazy min/max eval)

std::vector<double> octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::sizeLookupTable [protected, inherited]

contains the size of a voxel at level i (0: root node). tree_depth+1 levels (incl. 0)

const unsigned int octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::tree_depth [protected, inherited]

Maximum tree depth is fixed to 16 currently.

const unsigned int octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::tree_max_val [protected, inherited]
size_t octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::tree_size [protected, inherited]

number of nodes in tree

template<class NODE>
bool octomap::OccupancyOcTreeBase< NODE >::use_bbx_limit [protected]

The documentation for this class was generated from the following files: