34 #ifndef OCTOMAP_OCTREE_BASE_IMPL_H 35 #define OCTOMAP_OCTREE_BASE_IMPL_H 71 template <
class NODE,
class INTERFACE>
80 #include <octomap/OcTreeIterator.hxx> 124 NODE*
search(
double x,
double y,
double z,
unsigned int depth = 0)
const;
131 NODE*
search(
const point3d& value,
unsigned int depth = 0)
const;
145 bool deleteNode(
double x,
double y,
double z,
unsigned int depth = 0);
170 virtual void prune();
196 virtual void getMetricSize(
double& x,
double& y,
double& z)
const;
198 virtual void getMetricMin(
double& x,
double& y,
double& z);
200 void getMetricMin(
double& x,
double& y,
double& z)
const;
202 virtual void getMetricMax(
double& x,
double& y,
double& z);
204 void getMetricMax(
double& x,
double& y,
double& z)
const;
256 std::istream&
readData(std::istream &s);
260 std::ostream&
writeData(std::ostream &s)
const;
265 iterator
begin(
unsigned char maxDepth=0)
const {
return iterator(
this, maxDepth);};
270 leaf_iterator
begin_leafs(
unsigned char maxDepth=0)
const {
return leaf_iterator(
this, maxDepth);};
276 return leaf_bbx_iterator(
this, min, max, maxDepth);
280 return leaf_bbx_iterator(
this, min, max, maxDepth);
286 tree_iterator
begin_tree(
unsigned char maxDepth=0)
const {
return tree_iterator(
this, maxDepth);}
295 inline unsigned short int coordToKey(
double coordinate)
const{
300 unsigned short int coordToKey(
double coordinate,
unsigned depth)
const;
353 unsigned short int adjustKeyAtDepth(
unsigned short int key,
unsigned int depth)
const;
414 bool coordToKeyChecked(
double coordinate,
unsigned depth,
unsigned short int& key)
const;
418 double keyToCoord(
unsigned short int key,
unsigned depth)
const;
455 void pruneRecurs(NODE* node,
unsigned int depth,
unsigned int max_depth,
unsigned int& num_pruned);
458 void expandRecurs(NODE* node,
unsigned int depth,
unsigned int max_depth);
500 #include <octomap/OcTreeBaseImpl.hxx> unsigned long long memoryFullGrid() const
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
void setResolution(double r)
size_t getNumLeafNodesRecurs(const NODE *parent) const
std::vector< double > sizeLookupTable
contains the size of a voxel at level i (0: root node). tree_depth+1 levels (incl. 0)
iterator begin(unsigned char maxDepth=0) const
virtual void getMetricSize(double &x, double &y, double &z)
Size of OcTree (all known space) in meters for x, y and z dimension.
const tree_iterator end_tree() const
void calcMinMax()
recalculates min and max in x, y, z. Does nothing when tree size didn't change.
unsigned short int coordToKey(double coordinate) const
Converts from a single coordinate into a discrete key.
virtual ~OcTreeBaseImpl()
virtual size_t memoryUsage() const
OcTreeKey coordToKey(double x, double y, double z) const
Converts from a 3D coordinate into a 3D addressing key.
const unsigned int tree_depth
Maximum tree depth is fixed to 16 currently.
std::ostream & writeData(std::ostream &s) const
const iterator end() const
const leaf_iterator end_leafs() const
bool deleteNode(double x, double y, double z, unsigned int depth=0)
leaf_bbx_iterator begin_leafs_bbx(const OcTreeKey &min, const OcTreeKey &max, unsigned char maxDepth=0) const
bool deleteNodeRecurs(NODE *node, unsigned int depth, unsigned int max_depth, const OcTreeKey &key)
recursive call of deleteNode()
NODE * search(double x, double y, double z, unsigned int depth=0) const
double resolution
in meters
unsigned int getTreeDepth() const
point3d keyToCoord(const OcTreeKey &key, unsigned depth) const
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) const
OcTreeBaseImpl(double resolution)
point3d keyToCoord(const OcTreeKey &key) 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
void swapContent(OcTreeBaseImpl< NODE, INTERFACE > &rhs)
double keyToCoord(unsigned short int key, unsigned depth) const
bool computeRayKeys(const point3d &origin, const point3d &end, KeyRay &ray) const
const leaf_bbx_iterator leaf_iterator_bbx_end
bool size_changed
flag to denote whether the octree extent changed (for lazy min/max eval)
std::string getTreeType() const
virtual void getMetricMin(double &x, double &y, double &z)
minimum value of the bounding box of all known space in x, y, z
virtual size_t memoryUsageNode() const
OcTreeBaseImpl< NODE, INTERFACE > & operator=(const OcTreeBaseImpl< NODE, INTERFACE > &)
leaf_bbx_iterator begin_leafs_bbx(const point3d &min, const point3d &max, unsigned char maxDepth=0) const
std::istream & readData(std::istream &s)
const tree_iterator tree_iterator_end
std::list< octomath::Vector3 > point3d_list
This class represents a three-dimensional vector.
OcTreeKey coordToKey(const point3d &coord) const
Converts from a 3D coordinate into a 3D addressing key.
void expandRecurs(NODE *node, unsigned int depth, unsigned int max_depth)
recursive call of expand()
void calcNumNodesRecurs(NODE *node, size_t &num_nodes) const
NODE NodeType
Make the templated NODE type available from the outside.
tree_iterator begin_tree(unsigned char maxDepth=0) const
virtual void getMetricMax(double &x, double &y, double &z)
maximum value of the bounding box of all known space in x, y, z
double max_value[3]
max in x, y, z
void clear()
Deletes the complete tree structure.
bool computeRay(const point3d &origin, const point3d &end, std::vector< point3d > &ray)
double getResolution() const
void init()
initialize non-trivial members, helper for constructors
bool operator==(const OcTreeBaseImpl< NODE, INTERFACE > &rhs) const
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
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.
NODE * root
Pointer to the root NODE, NULL for empty tree.
double resolution_factor
= 1. / resolution
leaf_iterator begin_leafs(unsigned char maxDepth=0) const
OcTreeKey adjustKeyAtDepth(const OcTreeKey &key, unsigned int depth) const
void pruneRecurs(NODE *node, unsigned int depth, unsigned int max_depth, unsigned int &num_pruned)
recursive call of prune()
OcTreeKey coordToKey(const point3d &coord, unsigned depth) const
Converts from a 3D coordinate into a 3D addressing key at a given depth.
double getNodeSize(unsigned depth) const
std::vector< KeyRay > keyrays
data structure for ray casting, array for multithreading
const unsigned int tree_max_val
const leaf_bbx_iterator end_leafs_bbx() const
double keyToCoord(unsigned short int key) const
size_t calcNumNodes() const
Traverses the tree to calculate the total number of nodes.
virtual size_t size() const
const leaf_iterator leaf_iterator_end