9 #ifndef OCTOMAP_OCTREE_BASE_IMPL_H 10 #define OCTOMAP_OCTREE_BASE_IMPL_H 84 template <
class NODE,
class INTERFACE>
93 #include <mrpt/otherlibs/octomap/OcTreeIterator.hxx> 124 NODE*
search(
double x,
double y,
double z,
unsigned int depth = 0)
const;
130 NODE*
search(
const point3d& value,
unsigned int depth = 0)
const;
143 bool deleteNode(
double x,
double y,
double z,
unsigned int depth = 0);
167 virtual void prune();
192 virtual void getMetricSize(
double& x,
double& y,
double& z)
const;
194 virtual void getMetricMin(
double& x,
double& y,
double& z);
196 void getMetricMin(
double& x,
double& y,
double& z)
const;
198 virtual void getMetricMax(
double& x,
double& y,
double& z);
200 void getMetricMax(
double& x,
double& y,
double& z)
const;
252 std::istream&
readData(std::istream &s);
256 std::ostream&
writeData(std::ostream &s)
const;
264 iterator
begin(
unsigned char maxDepth=0)
const {
return iterator(
this, maxDepth);};
269 leaf_iterator
begin_leafs(
unsigned char maxDepth=0)
const {
return leaf_iterator(
this, maxDepth);};
275 return leaf_bbx_iterator(
this, min, max, maxDepth);
285 tree_iterator
begin_tree(
unsigned char maxDepth=0)
const {
return tree_iterator(
this, maxDepth);}
294 inline unsigned short int coordToKey(
double coordinate)
const{
299 unsigned short int coordToKey(
double coordinate,
unsigned depth)
const;
352 unsigned short int adjustKeyAtDepth(
unsigned short int key,
unsigned int depth)
const;
413 bool coordToKeyChecked(
double coordinate,
unsigned depth,
unsigned short int& key)
const;
417 double keyToCoord(
unsigned short int key,
unsigned depth)
const;
438 DEPRECATED(
bool genKeyValue(
double coordinate,
unsigned short int& keyval)
const) {
448 DEPRECATED(
bool genKeyValueAtDepth(
const unsigned short int keyval,
unsigned int depth,
unsigned short int &out_keyval)
const );
455 DEPRECATED(
bool genCoordFromKey(
const unsigned short int& key,
unsigned depth,
float& coord)
const ){
462 DEPRECATED(
inline bool genCoordFromKey(
const unsigned short int& key,
float& coord,
unsigned depth)
const) {
469 DEPRECATED(
inline bool genCoordFromKey(
const unsigned short int& key,
float& coord)
const) {
475 DEPRECATED(
double genCoordFromKey(
const unsigned short int& key,
unsigned depth)
const) {
480 DEPRECATED(
inline double genCoordFromKey(
const unsigned short int& key)
const) {
511 void pruneRecurs(NODE* node,
unsigned int depth,
unsigned int max_depth,
unsigned int& num_pruned);
514 void expandRecurs(NODE* node,
unsigned int depth,
unsigned int max_depth);
555 #include <mrpt/otherlibs/octomap/OcTreeBaseImpl.hxx> double resolution
in meters
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
OcTreeKey coordToKey(double x, double y, double z) const
Converts from a 3D coordinate into a 3D addressing key.
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.
size_t calcNumNodes() const
Traverses the tree to calculate the total number of nodes.
DEPRECATED(inline bool genCoordFromKey(const unsigned short int &key, float &coord) const)
OctoMap: A probabilistic, flexible, and compact 3D mapping library for robotic systems.
DEPRECATED(inline bool genCoordFromKey(const unsigned short int &key, float &coord, unsigned depth) const)
unsigned char computeChildIdx(const OcTreeKey &key, int depth)
generate child index (between 0 and 7) from key at given tree depth
const tree_iterator tree_iterator_end
std::string getTreeType() const
DEPRECATED(bool genCoordFromKey(const unsigned short int &key, unsigned depth, float &coord) const )
virtual ~OcTreeBaseImpl()
const iterator end() const
Bounding-box leaf iterator.
void expandRecurs(NODE *node, unsigned int depth, unsigned int max_depth)
recursive call of expand()
double resolution_factor
= 1. / resolution
leaf_bbx_iterator begin_leafs_bbx(const OcTreeKey &min, const OcTreeKey &max, unsigned char maxDepth=0) const
virtual size_t size() const
double min_value[3]
min in x, y, z
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 ...
OcTreeBaseImpl(double resolution)
void setResolution(double r)
Change the resolution of the octree, scaling all voxels.
leaf_bbx_iterator begin_leafs_bbx(const point3d &min, const point3d &max, unsigned char maxDepth=0) const
const leaf_bbx_iterator leaf_iterator_bbx_end
const unsigned int tree_max_val
DEPRECATED(bool genKeyValue(double coordinate, unsigned short int &keyval) const)
unsigned int getTreeDepth() 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
Search node at specified depth given a 3d point (depth=0: search full tree depth) ...
iterator begin(unsigned char maxDepth=0) const
std::istream & readData(std::istream &s)
Read all nodes from the input stream (without file header), for this the tree needs to be already cre...
virtual void prune()
Lossless compression of OcTree: merge children to parent when there are eight children with identical...
void pruneRecurs(NODE *node, unsigned int depth, unsigned int max_depth, unsigned int &num_pruned)
recursive call of prune()
virtual void getMetricSize(double &x, double &y, double &z)
Size of OcTree (all known space) in meters for x, y and z dimension.
size_t getNumLeafNodesRecurs(const NODE *parent) const
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 bea...
virtual void getMetricMax(double &x, double &y, double &z)
maximum value of the bounding box of all known space in x, y, z
unsigned short int coordToKey(double coordinate) const
Converts from a single coordinate into a discrete key.
const unsigned int tree_depth
Maximum tree depth is fixed to 16 currently.
const tree_iterator end_tree() const
size_t tree_size
number of nodes in tree
OcTree base class, to be used with with any kind of OcTreeDataNode.
size_t memoryFullGrid() const
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) const
Converts a 3D coordinate into a 3D OcTreeKey, with boundary checking.
NODE NodeType
Make the templated NODE type available from the outside.
const leaf_iterator leaf_iterator_end
void calcNumNodesRecurs(NODE *node, size_t &num_nodes) const
std::list< octomath::Vector3 > point3d_list
leaf_iterator begin_leafs(unsigned char maxDepth=0) const
OcTreeBaseImpl< NODE, INTERFACE > & operator=(const OcTreeBaseImpl< NODE, INTERFACE > &)
Assignment operator is private: don't (re-)assign octrees (const-parameters can't be changed) - use t...
std::vector< double > sizeLookupTable
contains the size of a voxel at level i (0: root node). tree_depth+1 levels (incl. 0)
virtual size_t memoryUsage() const
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
void clear()
Deletes the complete tree structure (only the root node will remain)
DEPRECATED(bool genKey(const point3d &point, OcTreeKey &key) const )
void calcMinMax()
recalculates min and max in x, y, z. Does nothing when tree size didn't change.
OcTreeKey is a container class for internal key addressing.
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) ...
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
virtual void expand()
Expands all pruned nodes (reverse of prune())
std::ostream & writeData(std::ostream &s) const
Write complete state of tree to stream (without file header) unmodified.
bool deleteNode(double x, double y, double z, unsigned int depth=0)
Delete a node (if exists) given a 3d point.
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 ...
OcTreeKey coordToKey(const point3d &coord) const
Converts from a 3D coordinate into a 3D addressing key.
double getNodeSize(unsigned depth) const
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 ...
DEPRECATED(inline bool genCoords(const OcTreeKey &key, unsigned int depth, point3d &point) const)
DEPRECATED(inline double genCoordFromKey(const unsigned short int &key) const)
const leaf_bbx_iterator end_leafs_bbx() const
virtual size_t memoryUsageNode() const
tree_iterator begin_tree(unsigned char maxDepth=0) const
virtual void getMetricMin(double &x, double &y, double &z)
minimum value of the bounding box of all known space in x, y, z
double max_value[3]
max in x, y, z
const leaf_iterator end_leafs() const
bool size_changed
flag to denote whether the octree extent changed (for lazy min/max eval)
DEPRECATED(double genCoordFromKey(const unsigned short int &key, unsigned depth) const)
OcTreeKey coordToKey(const point3d &coord, unsigned depth) const
Converts from a 3D coordinate into a 3D addressing key at a given depth.
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
bool operator==(const OcTreeBaseImpl< NODE, INTERFACE > &rhs) const
double getResolution() const
This class represents a three-dimensional vector.
point3d keyToCoord(const OcTreeKey &key) const
converts from an addressing key at the lowest tree level into a coordinate corresponding to the key's...