octomap  1.9.7
octomap::OcTreeBase< NODE > Class Template Referenceabstract
+ Inheritance diagram for octomap::OcTreeBase< NODE >:
+ Collaboration diagram for octomap::OcTreeBase< NODE >:

Public Types

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

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) More...
 
key_type adjustKeyAtDepth (key_type 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) More...
 
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. More...
 
void clear ()
 Deletes the complete tree structure. More...
 
void clearKeyRays ()
 Clear KeyRay vector to minimize unneeded memory. More...
 
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. More...
 
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. More...
 
OcTreeKey coordToKey (const point3d &coord) const
 Converts from a 3D coordinate into a 3D addressing key. More...
 
OcTreeKey coordToKey (const point3d &coord, unsigned depth) const
 Converts from a 3D coordinate into a 3D addressing key at a given depth. More...
 
key_type coordToKey (double coordinate) const
 Converts from a single coordinate into a discrete key. More...
 
key_type coordToKey (double coordinate, unsigned depth) const
 Converts from a single coordinate into a discrete key at a given depth. More...
 
OcTreeKey coordToKey (double x, double y, double z) const
 Converts from a 3D coordinate into a 3D addressing key. More...
 
OcTreeKey coordToKey (double x, double y, double z, unsigned depth) const
 Converts from a 3D coordinate into a 3D addressing key at a given depth. More...
 
bool coordToKeyChecked (const point3d &coord, OcTreeKey &key) const
 Converts a 3D coordinate into a 3D OcTreeKey, with boundary checking. More...
 
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. More...
 
bool coordToKeyChecked (double coordinate, key_type &key) const
 Converts a single coordinate into a discrete addressing key, with boundary checking. More...
 
bool coordToKeyChecked (double coordinate, unsigned depth, key_type &key) const
 Converts a single coordinate into a discrete addressing key, with boundary checking. More...
 
bool coordToKeyChecked (double x, double y, double z, OcTreeKey &key) const
 Converts a 3D coordinate into a 3D OcTreeKey, with boundary checking. More...
 
bool coordToKeyChecked (double x, double y, double z, unsigned depth, OcTreeKey &key) const
 Converts a 3D coordinate into a 3D OcTreeKey at a certain depth, with boundary checking. More...
 
OcTreeBase< NODE > * create () const
 virtual constructor: creates a new object of same type (Covariant return type requires an up-to-date compiler) More...
 
NODE * createNodeChild (NODE *node, unsigned int childIdx)
 Creates (allocates) the i-th child of the node. More...
 
bool deleteNode (const OcTreeKey &key, unsigned int depth=0)
 Delete a node (if exists) given an addressing key. More...
 
bool deleteNode (const point3d &value, unsigned int depth=0)
 Delete a node (if exists) given a 3d point. More...
 
bool deleteNode (double x, double y, double z, unsigned int depth=0)
 Delete a node (if exists) given a 3d point. More...
 
void deleteNodeChild (NODE *node, unsigned int childIdx)
 Deletes the i-th child of the node. More...
 
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()) More...
 
virtual void expandNode (NODE *node)
 Expands a node (reverse of pruning): All children are created and their occupancy probability is set to the node's value. More...
 
virtual void getMetricMax (double &x, double &y, double &z)
 maximum value of the bounding box of all known space in x, y, z More...
 
void getMetricMax (double &x, double &y, double &z) const
 maximum value of the bounding box of all known space in x, y, z More...
 
virtual void getMetricMin (double &x, double &y, double &z)
 minimum value of the bounding box of all known space in x, y, z More...
 
void getMetricMin (double &x, double &y, double &z) const
 minimum value of the bounding box of all known space in x, y, z More...
 
virtual void getMetricSize (double &x, double &y, double &z)
 Size of OcTree (all known space) in meters for x, y and z dimension. More...
 
virtual void getMetricSize (double &x, double &y, double &z) const
 Size of OcTree (all known space) in meters for x, y and z dimension. More...
 
const NODE * getNodeChild (const NODE *node, unsigned int childIdx) const
 
NODE * getNodeChild (NODE *node, unsigned int childIdx) const
 
double getNodeSize (unsigned depth) const
 
size_t getNumLeafNodes () const
 Traverses the tree to calculate the total number of leaf nodes. More...
 
double getResolution () const
 
virtual double getResolution () const =0
 
NODE * getRoot () const
 
unsigned int getTreeDepth () const
 
std::string getTreeType () const
 returns actual class name as string for identification More...
 
std::string getTreeType () const
 
void getUnknownLeafCenters (point3d_list &node_centers, point3d pmin, point3d pmax, unsigned int depth=0) const
 return centers of leafs that do NOT exist (but could) in a given bounding box More...
 
virtual bool isNodeCollapsible (const NODE *node) const
 A node is collapsible if all children exist, don't have children of their own and have the same occupancy value. More...
 
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 More...
 
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 More...
 
double keyToCoord (key_type key) const
 converts from a discrete key at the lowest tree level into a coordinate corresponding to the key's center More...
 
double keyToCoord (key_type key, unsigned depth) const
 converts from a discrete key at a given depth into a coordinate corresponding to the key's center More...
 
unsigned long long memoryFullGrid () const
 
virtual size_t memoryUsage () const
 
virtual size_t memoryUsage () const =0
 
virtual size_t memoryUsageNode () const
 
virtual size_t memoryUsageNode () const =0
 
bool nodeChildExists (const NODE *node, unsigned int childIdx) const
 Safe test if node has a child at index childIdx. More...
 
bool nodeHasChildren (const NODE *node) const
 Safe test if node has any children. More...
 
 OcTreeBase (double res)
 
bool operator== (const OcTreeBaseImpl< NODE, AbstractOcTree > &rhs) const
 Comparison between two octrees, all meta data, all nodes, and the structure must be identical. More...
 
virtual void prune ()
 Lossless compression of the octree: A node will replace all of its eight children if they have identical values. More...
 
virtual bool pruneNode (NODE *node)
 Prunes a node when it is collapsible. More...
 
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. More...
 
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) You need to check if the returned node is NULL, since it can be in unknown space. More...
 
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) You need to check if the returned node is NULL, since it can be in unknown space. More...
 
NODE * search (double x, double y, double z, unsigned int depth=0) const
 Search node at specified depth given a 3d point (depth=0: search full tree depth). More...
 
void setResolution (double r)
 Change the resolution of the octree, scaling all voxels. This will not preserve the (metric) scale! More...
 
virtual size_t size () const
 
virtual size_t size () const =0
 
void swapContent (OcTreeBaseImpl< NODE, AbstractOcTree > &rhs)
 Swap contents of two octrees, i.e., only the underlying pointer / tree structure. More...
 
double volume ()
 
bool write (const std::string &filename) const
 Write file header and complete tree to file (serialization) More...
 
bool write (std::ostream &s) const
 Write file header and complete tree to stream (serialization) More...
 
std::ostream & writeData (std::ostream &s) const
 Write complete state of tree to stream (without file header) unmodified. Pruning the tree first produces smaller files (lossless compression) More...
 

Static Public Member Functions

static AbstractOcTreecreateTree (const std::string id, double res)
 Creates a certain OcTree (factory pattern) More...
 
static AbstractOcTreeread (const std::string &filename)
 Read the file header, create the appropriate class and deserialize. More...
 
static AbstractOcTreeread (std::istream &s)
 Read the file header, create the appropriate class and deserialize. This creates a new octree which you need to delete yourself. More...
 

Protected Member Functions

void allocNodeChildren (NODE *node)
 
void calcMinMax ()
 recalculates min and max in x, y, z. Does nothing when tree size didn't change. More...
 
void calcNumNodesRecurs (NODE *node, size_t &num_nodes) const
 
void deleteNodeRecurs (NODE *node)
 Recursively delete a node and all children. Deallocates memory but does NOT set the node ptr to NULL nor updates tree size. More...
 
bool deleteNodeRecurs (NODE *node, unsigned int depth, unsigned int max_depth, const OcTreeKey &key)
 recursive call of deleteNode() More...
 
void expandRecurs (NODE *node, unsigned int depth, unsigned int max_depth)
 recursive call of expand() More...
 
size_t getNumLeafNodesRecurs (const NODE *parent) const
 
void init ()
 initialize non-trivial members, helper for constructors More...
 
void pruneRecurs (NODE *node, unsigned int depth, unsigned int max_depth, unsigned int &num_pruned)
 recursive call of prune() More...
 
std::istream & readNodesRecurs (NODE *, std::istream &s)
 recursive call of readData() More...
 
std::ostream & writeNodesRecurs (const NODE *, std::ostream &s) const
 recursive call of writeData() More...
 

Static Protected Member Functions

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

Protected Attributes

std::vector< KeyRaykeyrays
 data structure for ray casting, array for multithreading More...
 
const leaf_bbx_iterator leaf_iterator_bbx_end
 
const leaf_iterator leaf_iterator_end
 
double max_value [3]
 max in x, y, z More...
 
double min_value [3]
 min in x, y, z contains the size of a voxel at level i (0: root node). tree_depth+1 levels (incl. 0) More...
 
double resolution
 in meters More...
 
double resolution_factor
 = 1. / resolution More...
 
NODE * root
 Pointer to the root NODE, NULL for empty tree. More...
 
bool size_changed
 
std::vector< double > sizeLookupTable
 
point3d tree_center
 
const unsigned int tree_depth
 Maximum tree depth is fixed to 16 currently. More...
 
const tree_iterator tree_iterator_end
 
const unsigned int tree_max_val
 
size_t tree_size
 number of nodes in tree flag to denote whether the octree extent changed (for lazy min/max eval) More...
 

Static Protected Attributes

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

Member Typedef Documentation

◆ iterator

◆ NodeType

typedef NODE octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::NodeType
inherited

Make the templated NODE type available from the outside.

Constructor & Destructor Documentation

◆ OcTreeBase()

template<class NODE >
octomap::OcTreeBase< NODE >::OcTreeBase ( double  res)
inline

Member Function Documentation

◆ adjustKeyAtDepth() [1/2]

OcTreeKey octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::adjustKeyAtDepth ( const OcTreeKey key,
unsigned int  depth 
) const
inlineinherited

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

◆ adjustKeyAtDepth() [2/2]

key_type octomap::OcTreeBaseImpl< NODE, I >::adjustKeyAtDepth ( key_type  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

◆ allocNodeChildren()

void octomap::OcTreeBaseImpl< NODE, I >::allocNodeChildren ( NODE *  node)
protectedinherited

◆ begin()

iterator octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::begin ( unsigned char  maxDepth = 0) const
inlineinherited
Returns
beginning of the tree as leaf iterator

◆ begin_leafs()

leaf_iterator octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::begin_leafs ( unsigned char  maxDepth = 0) const
inlineinherited
Returns
beginning of the tree as leaf iterator

◆ begin_leafs_bbx() [1/2]

leaf_bbx_iterator octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::begin_leafs_bbx ( const OcTreeKey min,
const OcTreeKey max,
unsigned char  maxDepth = 0 
) const
inlineinherited
Returns
beginning of the tree as leaf iterator in a bounding box

◆ begin_leafs_bbx() [2/2]

leaf_bbx_iterator octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::begin_leafs_bbx ( const point3d min,
const point3d max,
unsigned char  maxDepth = 0 
) const
inlineinherited
Returns
beginning of the tree as leaf iterator in a bounding box

◆ begin_tree()

tree_iterator octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::begin_tree ( unsigned char  maxDepth = 0) const
inlineinherited
Returns
beginning of the tree as iterator to all nodes (incl. inner)

◆ calcMinMax()

void octomap::OcTreeBaseImpl< NODE, I >::calcMinMax
protectedinherited

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

◆ calcNumNodes()

size_t octomap::OcTreeBaseImpl< NODE, I >::calcNumNodes
inherited

Traverses the tree to calculate the total number of nodes.

◆ calcNumNodesRecurs()

void octomap::OcTreeBaseImpl< NODE, I >::calcNumNodesRecurs ( NODE *  node,
size_t &  num_nodes 
) const
protectedinherited

◆ clear()

void octomap::OcTreeBaseImpl< NODE, I >::clear
virtualinherited

Deletes the complete tree structure.

Implements octomap::AbstractOcTree.

◆ clearKeyRays()

void octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::clearKeyRays
inlineinherited

Clear KeyRay vector to minimize unneeded memory.

This is only useful for the StaticMemberInitializer classes, don't call it for an octree that is actually used.

◆ computeRay()

bool octomap::OcTreeBaseImpl< NODE, I >::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

◆ computeRayKeys()

bool octomap::OcTreeBaseImpl< NODE, I >::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

◆ coordToKey() [1/6]

OcTreeKey octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::coordToKey ( const point3d coord) const
inlineinherited

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

◆ coordToKey() [2/6]

OcTreeKey octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::coordToKey ( const point3d coord,
unsigned  depth 
) const
inlineinherited

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

◆ coordToKey() [3/6]

key_type octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::coordToKey ( double  coordinate) const
inlineinherited

Converts from a single coordinate into a discrete key.

◆ coordToKey() [4/6]

key_type octomap::OcTreeBaseImpl< NODE, I >::coordToKey ( double  coordinate,
unsigned  depth 
) const
inlineinherited

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

◆ coordToKey() [5/6]

OcTreeKey octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::coordToKey ( double  x,
double  y,
double  z 
) const
inlineinherited

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

◆ coordToKey() [6/6]

OcTreeKey octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::coordToKey ( double  x,
double  y,
double  z,
unsigned  depth 
) const
inlineinherited

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

◆ coordToKeyChecked() [1/6]

bool octomap::OcTreeBaseImpl< NODE, I >::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

◆ coordToKeyChecked() [2/6]

bool octomap::OcTreeBaseImpl< NODE, I >::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

◆ coordToKeyChecked() [3/6]

bool octomap::OcTreeBaseImpl< NODE, I >::coordToKeyChecked ( double  coordinate,
key_type 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

◆ coordToKeyChecked() [4/6]

bool octomap::OcTreeBaseImpl< NODE, I >::coordToKeyChecked ( double  coordinate,
unsigned  depth,
key_type 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

◆ coordToKeyChecked() [5/6]

bool octomap::OcTreeBaseImpl< NODE, I >::coordToKeyChecked ( double  x,
double  y,
double  z,
OcTreeKey key 
) const
inherited

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

Parameters
x
y
z
keyvalues that will be computed, an array of fixed size 3.
Returns
true if point is within the octree (valid), false otherwise

◆ coordToKeyChecked() [6/6]

bool octomap::OcTreeBaseImpl< NODE, I >::coordToKeyChecked ( double  x,
double  y,
double  z,
unsigned  depth,
OcTreeKey key 
) const
inherited

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

Parameters
x
y
z
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

◆ create()

template<class NODE >
OcTreeBase<NODE>* octomap::OcTreeBase< NODE >::create ( ) const
inlinevirtual

virtual constructor: creates a new object of same type (Covariant return type requires an up-to-date compiler)

Implements octomap::AbstractOcTree.

◆ createNodeChild()

NODE * octomap::OcTreeBaseImpl< NODE, I >::createNodeChild ( NODE *  node,
unsigned int  childIdx 
)
inherited

Creates (allocates) the i-th child of the node.

Returns
ptr to newly create NODE

◆ createTree()

AbstractOcTree * octomap::AbstractOcTree::createTree ( const std::string  id,
double  res 
)
staticinherited

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_ERROR, and octomap::AbstractOcTree::setResolution().

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

◆ deleteNode() [1/3]

bool octomap::OcTreeBaseImpl< NODE, I >::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.

◆ deleteNode() [2/3]

bool octomap::OcTreeBaseImpl< NODE, I >::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.

◆ deleteNode() [3/3]

bool octomap::OcTreeBaseImpl< NODE, I >::deleteNode ( double  x,
double  y,
double  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.

◆ deleteNodeChild()

void octomap::OcTreeBaseImpl< NODE, I >::deleteNodeChild ( NODE *  node,
unsigned int  childIdx 
)
inherited

Deletes the i-th child of the node.

◆ deleteNodeRecurs() [1/2]

void octomap::OcTreeBaseImpl< NODE, I >::deleteNodeRecurs ( NODE *  node)
protectedinherited

Recursively delete a node and all children. Deallocates memory but does NOT set the node ptr to NULL nor updates tree size.

◆ deleteNodeRecurs() [2/2]

bool octomap::OcTreeBaseImpl< NODE, I >::deleteNodeRecurs ( NODE *  node,
unsigned int  depth,
unsigned int  max_depth,
const OcTreeKey key 
)
protectedinherited

recursive call of deleteNode()

◆ end()

const iterator octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::end
inlineinherited
Returns
end of the tree as leaf iterator

◆ end_leafs()

const leaf_iterator octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::end_leafs
inlineinherited
Returns
end of the tree as leaf iterator

◆ end_leafs_bbx()

const leaf_bbx_iterator octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::end_leafs_bbx
inlineinherited
Returns
end of the tree as leaf iterator in a bounding box

◆ end_tree()

const tree_iterator octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::end_tree
inlineinherited
Returns
end of the tree as iterator to all nodes (incl. inner)

◆ expand()

void octomap::OcTreeBaseImpl< NODE, I >::expand
virtualinherited

Expands all pruned nodes (reverse of prune())

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

Implements octomap::AbstractOcTree.

◆ expandNode()

void octomap::OcTreeBaseImpl< NODE, I >::expandNode ( NODE *  node)
virtualinherited

Expands a node (reverse of pruning): All children are created and their occupancy probability is set to the node's value.

You need to verify that this is indeed a pruned node (i.e. not a leaf at the lowest level)

◆ expandRecurs()

void octomap::OcTreeBaseImpl< NODE, I >::expandRecurs ( NODE *  node,
unsigned int  depth,
unsigned int  max_depth 
)
protectedinherited

recursive call of expand()

◆ getMetricMax() [1/2]

void octomap::OcTreeBaseImpl< NODE, I >::getMetricMax ( double &  x,
double &  y,
double &  z 
)
virtualinherited

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

Implements octomap::AbstractOcTree.

◆ getMetricMax() [2/2]

void octomap::OcTreeBaseImpl< NODE, I >::getMetricMax ( double &  x,
double &  y,
double &  z 
) const
virtualinherited

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

Implements octomap::AbstractOcTree.

◆ getMetricMin() [1/2]

void octomap::OcTreeBaseImpl< NODE, I >::getMetricMin ( double &  x,
double &  y,
double &  z 
)
virtualinherited

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

Implements octomap::AbstractOcTree.

◆ getMetricMin() [2/2]

void octomap::OcTreeBaseImpl< NODE, I >::getMetricMin ( double &  x,
double &  y,
double &  z 
) const
virtualinherited

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

Implements octomap::AbstractOcTree.

◆ getMetricSize() [1/2]

void octomap::OcTreeBaseImpl< NODE, I >::getMetricSize ( double &  x,
double &  y,
double &  z 
)
virtualinherited

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

Implements octomap::AbstractOcTree.

◆ getMetricSize() [2/2]

void octomap::OcTreeBaseImpl< NODE, I >::getMetricSize ( double &  x,
double &  y,
double &  z 
) const
virtualinherited

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

◆ getNodeChild() [1/2]

const NODE * octomap::OcTreeBaseImpl< NODE, I >::getNodeChild ( const NODE *  node,
unsigned int  childIdx 
) const
inherited
Returns
const ptr to child number childIdx of node

◆ getNodeChild() [2/2]

NODE * octomap::OcTreeBaseImpl< NODE, I >::getNodeChild ( NODE *  node,
unsigned int  childIdx 
) const
inherited
Returns
ptr to child number childIdx of node

◆ getNodeSize()

double octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::getNodeSize ( unsigned  depth) const
inlineinherited

◆ getNumLeafNodes()

size_t octomap::OcTreeBaseImpl< NODE, I >::getNumLeafNodes
inherited

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

◆ getNumLeafNodesRecurs()

size_t octomap::OcTreeBaseImpl< NODE, I >::getNumLeafNodesRecurs ( const NODE *  parent) const
protectedinherited

◆ getResolution() [1/2]

double octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::getResolution
inlineinherited

◆ getResolution() [2/2]

virtual double octomap::AbstractOcTree::getResolution ( ) const
pure virtualinherited

◆ getRoot()

NODE* octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::getRoot
inlineinherited
Returns
Pointer to the root node of the tree. This pointer should not be modified or deleted externally, the OcTree manages its memory itself. In an empty tree, root is NULL.

◆ getTreeDepth()

unsigned int octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::getTreeDepth
inlineinherited

◆ getTreeType() [1/2]

template<class NODE >
std::string octomap::OcTreeBase< NODE >::getTreeType ( ) const
inlinevirtual

returns actual class name as string for identification

Implements octomap::AbstractOcTree.

◆ getTreeType() [2/2]

std::string octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::getTreeType
inlineinherited

◆ getUnknownLeafCenters()

void octomap::OcTreeBaseImpl< NODE, I >::getUnknownLeafCenters ( point3d_list node_centers,
point3d  pmin,
point3d  pmax,
unsigned int  depth = 0 
) const
inherited

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

◆ init()

void octomap::OcTreeBaseImpl< NODE, I >::init
protectedinherited

initialize non-trivial members, helper for constructors

◆ isNodeCollapsible()

bool octomap::OcTreeBaseImpl< NODE, I >::isNodeCollapsible ( const NODE *  node) const
virtualinherited

A node is collapsible if all children exist, don't have children of their own and have the same occupancy value.

◆ keyToCoord() [1/4]

point3d octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::keyToCoord ( const OcTreeKey key) const
inlineinherited

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

◆ keyToCoord() [2/4]

point3d octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::keyToCoord ( const OcTreeKey key,
unsigned  depth 
) const
inlineinherited

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

◆ keyToCoord() [3/4]

double octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::keyToCoord ( key_type  key) const
inlineinherited

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

◆ keyToCoord() [4/4]

double octomap::OcTreeBaseImpl< NODE, I >::keyToCoord ( key_type  key,
unsigned  depth 
) const
inherited

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

◆ memoryFullGrid()

unsigned long long octomap::OcTreeBaseImpl< NODE, I >::memoryFullGrid
inherited
Returns
Memory usage of a full grid of the same size as the OcTree in bytes (for comparison)
Note
this can be larger than the adressable memory - size_t may not be enough to hold it!

◆ memoryUsage() [1/2]

size_t octomap::OcTreeBaseImpl< NODE, I >::memoryUsage
virtualinherited
Returns
Memory usage of the complete octree in bytes (may vary between architectures)

◆ memoryUsage() [2/2]

virtual size_t octomap::AbstractOcTree::memoryUsage ( ) const
pure virtualinherited

◆ memoryUsageNode() [1/2]

virtual size_t octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::memoryUsageNode
inlinevirtualinherited
Returns
Memory usage of a single octree node

◆ memoryUsageNode() [2/2]

virtual size_t octomap::AbstractOcTree::memoryUsageNode ( ) const
pure virtualinherited

◆ nodeChildExists()

bool octomap::OcTreeBaseImpl< NODE, I >::nodeChildExists ( const NODE *  node,
unsigned int  childIdx 
) const
inherited

Safe test if node has a child at index childIdx.

First tests if there are any children. Replaces node->childExists(...)

Returns
true if the child at childIdx exists

◆ nodeHasChildren()

bool octomap::OcTreeBaseImpl< NODE, I >::nodeHasChildren ( const NODE *  node) const
inherited

Safe test if node has any children.

Replaces node->hasChildren(...)

Returns
true if node has at least one child

◆ operator==()

bool octomap::OcTreeBaseImpl< NODE, I >::operator== ( const OcTreeBaseImpl< NODE, AbstractOcTree > &  rhs) const
inherited

Comparison between two octrees, all meta data, all nodes, and the structure must be identical.

◆ prune()

void octomap::OcTreeBaseImpl< NODE, I >::prune
virtualinherited

Lossless compression of the octree: A node will replace all of its eight children if they have identical values.

You usually don't have to call prune() after a regular occupancy update, updateNode() incrementally prunes all affected nodes.

Implements octomap::AbstractOcTree.

◆ pruneNode()

bool octomap::OcTreeBaseImpl< NODE, I >::pruneNode ( NODE *  node)
virtualinherited

Prunes a node when it is collapsible.

Returns
true if pruning was successful

◆ pruneRecurs()

void octomap::OcTreeBaseImpl< NODE, I >::pruneRecurs ( NODE *  node,
unsigned int  depth,
unsigned int  max_depth,
unsigned int &  num_pruned 
)
protectedinherited

recursive call of prune()

◆ read() [1/2]

AbstractOcTree * octomap::AbstractOcTree::read ( const std::string &  filename)
staticinherited

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.

◆ read() [2/2]

AbstractOcTree * octomap::AbstractOcTree::read ( std::istream &  s)
staticinherited

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().

◆ readData()

std::istream & octomap::OcTreeBaseImpl< NODE, I >::readData ( std::istream &  s)
virtualinherited

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.

◆ readHeader()

bool octomap::AbstractOcTree::readHeader ( std::istream &  s,
std::string &  id,
unsigned &  size,
double &  res 
)
staticprotectedinherited

◆ readNodesRecurs()

std::istream & octomap::OcTreeBaseImpl< NODE, I >::readNodesRecurs ( NODE *  node,
std::istream &  s 
)
protectedinherited

recursive call of readData()

◆ registerTreeType()

◆ search() [1/3]

NODE * octomap::OcTreeBaseImpl< NODE, I >::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) You need to check if the returned node is NULL, since it can be in unknown space.

Returns
pointer to node if found, NULL otherwise

◆ search() [2/3]

NODE * octomap::OcTreeBaseImpl< NODE, I >::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) You need to check if the returned node is NULL, since it can be in unknown space.

Returns
pointer to node if found, NULL otherwise

◆ search() [3/3]

NODE * octomap::OcTreeBaseImpl< NODE, I >::search ( double  x,
double  y,
double  z,
unsigned int  depth = 0 
) const
inherited

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

You need to check if the returned node is NULL, since it can be in unknown space.

Returns
pointer to node if found, NULL otherwise

◆ setResolution()

void octomap::OcTreeBaseImpl< NODE, I >::setResolution ( double  r)
virtualinherited

Change the resolution of the octree, scaling all voxels. This will not preserve the (metric) scale!

Implements octomap::AbstractOcTree.

◆ size() [1/2]

virtual size_t octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::size
inlinevirtualinherited
Returns
The number of nodes in the tree

◆ size() [2/2]

◆ swapContent()

void octomap::OcTreeBaseImpl< NODE, I >::swapContent ( OcTreeBaseImpl< NODE, AbstractOcTree > &  rhs)
inherited

Swap contents of two octrees, i.e., only the underlying pointer / tree structure.

You have to ensure yourself that the metadata (resolution etc) matches. No memory is cleared in this function

◆ volume()

double octomap::OcTreeBaseImpl< NODE, I >::volume
inherited

◆ write() [1/2]

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().

◆ write() [2/2]

bool octomap::AbstractOcTree::write ( std::ostream &  s) const
inherited

◆ writeData()

std::ostream & octomap::OcTreeBaseImpl< NODE, I >::writeData ( std::ostream &  s) const
virtualinherited

Write complete state of tree to stream (without file header) unmodified. Pruning the tree first produces smaller files (lossless compression)

Implements octomap::AbstractOcTree.

◆ writeNodesRecurs()

std::ostream & octomap::OcTreeBaseImpl< NODE, I >::writeNodesRecurs ( const NODE *  node,
std::ostream &  s 
) const
protectedinherited

recursive call of writeData()

Field Documentation

◆ fileHeader

const std::string octomap::AbstractOcTree::fileHeader = "# Octomap OcTree file"
staticprotectedinherited

◆ keyrays

std::vector<KeyRay> octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::keyrays
protectedinherited

data structure for ray casting, array for multithreading

◆ leaf_iterator_bbx_end

const leaf_bbx_iterator octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::leaf_iterator_bbx_end
protectedinherited

◆ leaf_iterator_end

const leaf_iterator octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::leaf_iterator_end
protectedinherited

◆ max_value

double octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::max_value[3]
protectedinherited

max in x, y, z

◆ min_value

double octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::min_value[3]
protectedinherited

min in x, y, z contains the size of a voxel at level i (0: root node). tree_depth+1 levels (incl. 0)

◆ resolution

double octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::resolution
protectedinherited

in meters

◆ resolution_factor

double octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::resolution_factor
protectedinherited

= 1. / resolution

◆ root

NODE* octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::root
protectedinherited

Pointer to the root NODE, NULL for empty tree.

◆ size_changed

bool octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::size_changed
protectedinherited

◆ sizeLookupTable

std::vector<double> octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::sizeLookupTable
protectedinherited

◆ tree_center

point3d octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::tree_center
protectedinherited

◆ tree_depth

const unsigned int octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::tree_depth
protectedinherited

Maximum tree depth is fixed to 16 currently.

◆ tree_iterator_end

const tree_iterator octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::tree_iterator_end
protectedinherited

◆ tree_max_val

const unsigned int octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::tree_max_val
protectedinherited

◆ tree_size

size_t octomap::OcTreeBaseImpl< NODE, AbstractOcTree >::tree_size
protectedinherited

number of nodes in tree flag to denote whether the octree extent changed (for lazy min/max eval)


The documentation for this class was generated from the following file:
octomap::AbstractOcTree::AbstractOcTree
AbstractOcTree()
Definition: AbstractOcTree.cpp:41
octomap::AbstractOcTree::read
static AbstractOcTree * read(const std::string &filename)
Read the file header, create the appropriate class and deserialize.
Definition: AbstractOcTree.cpp:74