Main MRPT website > C++ reference for MRPT 1.4.0
OcTreeBaseImpl.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef OCTOMAP_OCTREE_BASE_IMPL_H
10 #define OCTOMAP_OCTREE_BASE_IMPL_H
11 
12 // $Id: OcTreeBaseImpl.h 444 2012-11-16 16:04:12Z ahornung $
13 
14 /**
15 * OctoMap:
16 * A probabilistic, flexible, and compact 3D mapping library for robotic systems.
17 * @author K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009-2011.
18 * @see http://octomap.sourceforge.net/
19 * License: New BSD License
20 */
21 
22 /*
23  * Copyright (c) 2009-2011, K. M. Wurm, A. Hornung, University of Freiburg
24  * All rights reserved.
25  *
26  * Redistribution and use in source and binary forms, with or without
27  * modification, are permitted provided that the following conditions are met:
28  *
29  * * Redistributions of source code must retain the above copyright
30  * notice, this list of conditions and the following disclaimer.
31  * * Redistributions in binary form must reproduce the above copyright
32  * notice, this list of conditions and the following disclaimer in the
33  * documentation and/or other materials provided with the distribution.
34  * * Neither the name of the University of Freiburg nor the names of its
35  * contributors may be used to endorse or promote products derived from
36  * this software without specific prior written permission.
37  *
38  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
39  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
40  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
41  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
42  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
43  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
44  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
45  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
46  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
47  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
48  * POSSIBILITY OF SUCH DAMAGE.
49  */
50 
51 #include <list>
52 #include <limits>
53 #include <iterator>
54 #include <stack>
55 
56 
57 #include "octomap_types.h"
58 #include "OcTreeKey.h"
59 #include "ScanGraph.h"
60 
61 
62 namespace octomap {
63 
64 
65  /**
66  * OcTree base class, to be used with with any kind of OcTreeDataNode.
67  *
68  * This tree implementation currently has a maximum depth of 16
69  * nodes. For this reason, coordinates values have to be, e.g.,
70  * below +/- 327.68 meters (2^15) at a maximum resolution of 0.01m.
71  *
72  * This limitation enables the use of an efficient key generation
73  * method which uses the binary representation of the data point
74  * coordinates.
75  *
76  * \note You should probably not use this class directly, but
77  * OcTreeBase or OccupancyOcTreeBase instead
78  *
79  * \tparam NODE Node class to be used in tree (usually derived from
80  * OcTreeDataNode)
81  * \tparam INTERFACE Interface to be derived from, should be either
82  * AbstractOcTree or AbstractOccupancyOcTree
83  */
84  template <class NODE,class INTERFACE>
85  class OcTreeBaseImpl : public INTERFACE {
86 
87  public:
88  /// Make the templated NODE type available from the outside
89  typedef NODE NodeType;
90 
91  // the actual iterator implementation is included here
92  // as a member from this file
93  #include <mrpt/otherlibs/octomap/OcTreeIterator.hxx>
94 
95  OcTreeBaseImpl(double resolution);
96  virtual ~OcTreeBaseImpl();
97 
99 
100  bool operator== (const OcTreeBaseImpl<NODE,INTERFACE>& rhs) const;
101 
102  std::string getTreeType() const {return "OcTreeBaseImpl";}
104  /// Change the resolution of the octree, scaling all voxels.
105  /// This will not preserve the (metric) scale!
106  void setResolution(double r);
107  inline double getResolution() const { return resolution; }
108 
109  inline unsigned int getTreeDepth () const { return tree_depth; }
110 
111  inline double getNodeSize(unsigned depth) const {assert(depth <= tree_depth); return sizeLookupTable[depth];}
112 
113  /**
114  * \return Pointer to the root node of the tree. This pointer
115  * should not be modified or deleted externally, the OcTree
116  * manages its memory itself.
117  */
118  inline NODE* getRoot() const { return root; }
119 
120  /**
121  * Search node at specified depth given a 3d point (depth=0: search full tree depth)
122  * @return pointer to node if found, NULL otherwise
123  */
124  NODE* search(double x, double y, double z, unsigned int depth = 0) const;
125 
126  /**
127  * Search node at specified depth given a 3d point (depth=0: search full tree depth)
128  * @return pointer to node if found, NULL otherwise
129  */
130  NODE* search(const point3d& value, unsigned int depth = 0) const;
132  /**
133  * Search a node at specified depth given an addressing key (depth=0: search full tree depth)
134  * @return pointer to node if found, NULL otherwise
135  */
136  NODE* search(const OcTreeKey& key, unsigned int depth = 0) const;
137 
138  /**
139  * Delete a node (if exists) given a 3d point. Will always
140  * delete at the lowest level unless depth !=0, and expand pruned inner nodes as needed.
141  * Pruned nodes at level "depth" will directly be deleted as a whole.
142  */
143  bool deleteNode(double x, double y, double z, unsigned int depth = 0);
144 
145  /**
146  * Delete a node (if exists) given a 3d point. Will always
147  * delete at the lowest level unless depth !=0, and expand pruned inner nodes as needed.
148  * Pruned nodes at level "depth" will directly be deleted as a whole.
149  */
150  bool deleteNode(const point3d& value, unsigned int depth = 0);
152  /**
153  * Delete a node (if exists) given an addressing key. Will always
154  * delete at the lowest level unless depth !=0, and expand pruned inner nodes as needed.
155  * Pruned nodes at level "depth" will directly be deleted as a whole.
156  */
157  bool deleteNode(const OcTreeKey& key, unsigned int depth = 0);
158 
159  /// Deletes the complete tree structure (only the root node will remain)
160  void clear();
162  //OcTreeBaseImpl deepCopy() const;
163 
164 
165  /// Lossless compression of OcTree: merge children to parent when there are
166  /// eight children with identical values
167  virtual void prune();
168 
169  /// Expands all pruned nodes (reverse of prune())
170  /// \note This is an expensive operation, especially when the tree is nearly empty!
171  virtual void expand();
172 
173  // -- statistics ----------------------
174 
175  /// \return The number of nodes in the tree
176  virtual inline size_t size() const { return tree_size; }
177 
178  /// \return Memory usage of the complete octree in bytes (may vary between architectures)
179  virtual size_t memoryUsage() const;
180 
181  /// \return Memory usage of the a single octree node
182  virtual inline size_t memoryUsageNode() const {return sizeof(NODE); };
184  /// \return Memory usage of a full grid of the same size as the OcTree in bytes (for comparison)
185  size_t memoryFullGrid() const;
186 
187  double volume() const;
188 
189  /// Size of OcTree (all known space) in meters for x, y and z dimension
190  virtual void getMetricSize(double& x, double& y, double& z);
191  /// Size of OcTree (all known space) in meters for x, y and z dimension
192  virtual void getMetricSize(double& x, double& y, double& z) const;
193  /// minimum value of the bounding box of all known space in x, y, z
194  virtual void getMetricMin(double& x, double& y, double& z);
195  /// minimum value of the bounding box of all known space in x, y, z
196  void getMetricMin(double& x, double& y, double& z) const;
197  /// maximum value of the bounding box of all known space in x, y, z
198  virtual void getMetricMax(double& x, double& y, double& z);
199  /// maximum value of the bounding box of all known space in x, y, z
200  void getMetricMax(double& x, double& y, double& z) const;
201 
202  /// Traverses the tree to calculate the total number of nodes
203  size_t calcNumNodes() const;
204 
205  /// Traverses the tree to calculate the total number of leaf nodes
206  size_t getNumLeafNodes() const;
207 
209  // -- access tree nodes ------------------
211  /// return centers of leafs that do NOT exist (but could) in a given bounding box
212  void getUnknownLeafCenters(point3d_list& node_centers, point3d pmin, point3d pmax) const;
213 
214 
215  // -- raytracing -----------------------
216 
217  /**
218  * Traces a ray from origin to end (excluding), returning an
219  * OcTreeKey of all nodes traversed by the beam. You still need to check
220  * if a node at that coordinate exists (e.g. with search()).
221  *
222  * @param origin start coordinate of ray
223  * @param end end coordinate of ray
224  * @param ray KeyRay structure that holds the keys of all nodes traversed by the ray, excluding "end"
225  * @return Success of operation. Returning false usually means that one of the coordinates is out of the OcTree's range
226  */
227  bool computeRayKeys(const point3d& origin, const point3d& end, KeyRay& ray) const;
228 
229 
230  /**
231  * Traces a ray from origin to end (excluding), returning the
232  * coordinates of all nodes traversed by the beam. You still need to check
233  * if a node at that coordinate exists (e.g. with search()).
234  * @note: use the faster computeRayKeys method if possible.
235  *
236  * @param origin start coordinate of ray
237  * @param end end coordinate of ray
238  * @param ray KeyRay structure that holds the keys of all nodes traversed by the ray, excluding "end"
239  * @return Success of operation. Returning false usually means that one of the coordinates is out of the OcTree's range
240  */
241  bool computeRay(const point3d& origin, const point3d& end, std::vector<point3d>& ray);
242 
243 
244  // file IO
245 
246  /**
247  * Read all nodes from the input stream (without file header),
248  * for this the tree needs to be already created.
249  * For general file IO, you
250  * should probably use AbstractOcTree::read() instead.
251  */
252  std::istream& readData(std::istream &s);
253 
254  /// Write complete state of tree to stream (without file header) unmodified.
255  /// Pruning the tree first produces smaller files (lossless compression)
256  std::ostream& writeData(std::ostream &s) const;
257 
258 // class leaf_iterator; // JLBC
259 // class tree_iterator; // JLBC
260 // class leaf_bbx_iterator; // JLBC
261  typedef leaf_iterator iterator;
263  /// @return beginning of the tree as leaf iterator
264  iterator begin(unsigned char maxDepth=0) const {return iterator(this, maxDepth);};
265  /// @return end of the tree as leaf iterator
266  const iterator end() const {return leaf_iterator_end;}; // TODO: RVE?
267 
268  /// @return beginning of the tree as leaf iterator
269  leaf_iterator begin_leafs(unsigned char maxDepth=0) const {return leaf_iterator(this, maxDepth);};
270  /// @return end of the tree as leaf iterator
271  const leaf_iterator end_leafs() const {return leaf_iterator_end;}
273  /// @return beginning of the tree as leaf iterator in a bounding box
274  leaf_bbx_iterator begin_leafs_bbx(const OcTreeKey& min, const OcTreeKey& max, unsigned char maxDepth=0) const {
275  return leaf_bbx_iterator(this, min, max, maxDepth);
276  }
277  /// @return beginning of the tree as leaf iterator in a bounding box
278  leaf_bbx_iterator begin_leafs_bbx(const point3d& min, const point3d& max, unsigned char maxDepth=0) const {
279  return leaf_bbx_iterator(this, min, max, maxDepth);
280  }
281  /// @return end of the tree as leaf iterator in a bounding box
283 
284  /// @return beginning of the tree as iterator to all nodes (incl. inner)
285  tree_iterator begin_tree(unsigned char maxDepth=0) const {return tree_iterator(this, maxDepth);}
286  /// @return end of the tree as iterator to all nodes (incl. inner)
287  const tree_iterator end_tree() const {return tree_iterator_end;}
288 
289  //
290  // Key / coordinate conversion functions
291  //
292 
293  /// Converts from a single coordinate into a discrete key
294  inline unsigned short int coordToKey(double coordinate) const{
295  return ((int) floor(resolution_factor * coordinate)) + tree_max_val;
296  }
297 
298  /// Converts from a single coordinate into a discrete key at a given depth
299  unsigned short int coordToKey(double coordinate, unsigned depth) const;
300 
301 
302  /// Converts from a 3D coordinate into a 3D addressing key
303  inline OcTreeKey coordToKey(const point3d& coord) const{
304  return OcTreeKey(coordToKey(coord(0)), coordToKey(coord(1)), coordToKey(coord(2)));
305  }
306 
307  /// Converts from a 3D coordinate into a 3D addressing key
308  inline OcTreeKey coordToKey(double x, double y, double z) const{
309  return OcTreeKey(coordToKey(x), coordToKey(y), coordToKey(z));
310  }
311 
312  /// Converts from a 3D coordinate into a 3D addressing key at a given depth
313  inline OcTreeKey coordToKey(const point3d& coord, unsigned depth) const{
314  if (depth == tree_depth)
315  return coordToKey(coord);
316  else
317  return OcTreeKey(coordToKey(coord(0), depth), coordToKey(coord(1), depth), coordToKey(coord(2), depth));
318  }
319 
320  /// Converts from a 3D coordinate into a 3D addressing key at a given depth
321  inline OcTreeKey coordToKey(double x, double y, double z, unsigned depth) const{
322  if (depth == tree_depth)
323  return coordToKey(x,y,z);
324  else
325  return OcTreeKey(coordToKey(x, depth), coordToKey(y, depth), coordToKey(z, depth));
326  }
327 
328  /**
329  * Adjusts a 3D key from the lowest level to correspond to a higher depth (by
330  * shifting the key values)
331  *
332  * @param key Input key, at the lowest tree level
333  * @param depth Target depth level for the new key
334  * @return Key for the new depth level
335  */
336  inline OcTreeKey adjustKeyAtDepth(const OcTreeKey& key, unsigned int depth) const{
337  if (depth == tree_depth)
338  return key;
339 
340  assert(depth <= tree_depth);
341  return OcTreeKey(adjustKeyAtDepth(key[0], depth), adjustKeyAtDepth(key[1], depth), adjustKeyAtDepth(key[2], depth));
342  }
343 
344  /**
345  * Adjusts a single key value from the lowest level to correspond to a higher depth (by
346  * shifting the key value)
347  *
348  * @param key Input key, at the lowest tree level
349  * @param depth Target depth level for the new key
350  * @return Key for the new depth level
351  */
352  unsigned short int adjustKeyAtDepth(unsigned short int key, unsigned int depth) const;
353 
354  /**
355  * Converts a 3D coordinate into a 3D OcTreeKey, with boundary checking.
356  *
357  * @param coord 3d coordinate of a point
358  * @param key values that will be computed, an array of fixed size 3.
359  * @return true if point is within the octree (valid), false otherwise
360  */
361  bool coordToKeyChecked(const point3d& coord, OcTreeKey& key) const;
362 
363  /**
364  * Converts a 3D coordinate into a 3D OcTreeKey at a certain depth, with boundary checking.
365  *
366  * @param coord 3d coordinate of a point
367  * @param depth level of the key from the top
368  * @param key values that will be computed, an array of fixed size 3.
369  * @return true if point is within the octree (valid), false otherwise
370  */
371  bool coordToKeyChecked(const point3d& coord, unsigned depth, OcTreeKey& key) const;
372 
373  /**
374  * Converts a 3D coordinate into a 3D OcTreeKey, with boundary checking.
375  *
376  * @param x
377  * @param y
378  * @param z
379  * @param key values that will be computed, an array of fixed size 3.
380  * @return true if point is within the octree (valid), false otherwise
381  */
382  bool coordToKeyChecked(double x, double y, double z, OcTreeKey& key) const;
383 
384  /**
385  * Converts a 3D coordinate into a 3D OcTreeKey at a certain depth, with boundary checking.
386  *
387  * @param x
388  * @param y
389  * @param z
390  * @param depth level of the key from the top
391  * @param key values that will be computed, an array of fixed size 3.
392  * @return true if point is within the octree (valid), false otherwise
393  */
394  bool coordToKeyChecked(double x, double y, double z, unsigned depth, OcTreeKey& key) const;
395 
396  /**
397  * Converts a single coordinate into a discrete addressing key, with boundary checking.
398  *
399  * @param coordinate 3d coordinate of a point
400  * @param key discrete 16 bit adressing key, result
401  * @return true if coordinate is within the octree bounds (valid), false otherwise
402  */
403  bool coordToKeyChecked(double coordinate, unsigned short int& key) const;
404 
405  /**
406  * Converts a single coordinate into a discrete addressing key, with boundary checking.
407  *
408  * @param coordinate 3d coordinate of a point
409  * @param depth level of the key from the top
410  * @param key discrete 16 bit adressing key, result
411  * @return true if coordinate is within the octree bounds (valid), false otherwise
412  */
413  bool coordToKeyChecked(double coordinate, unsigned depth, unsigned short int& key) const;
414 
415  /// converts from a discrete key at a given depth into a coordinate
416  /// corresponding to the key's center
417  double keyToCoord(unsigned short int key, unsigned depth) const;
418 
419  /// converts from a discrete key at the lowest tree level into a coordinate
420  /// corresponding to the key's center
421  inline double keyToCoord(unsigned short int key) const{
422  return (double( (int) key - (int) this->tree_max_val ) +0.5) * this->resolution;
423  }
424 
425  /// converts from an addressing key at the lowest tree level into a coordinate
426  /// corresponding to the key's center
427  inline point3d keyToCoord(const OcTreeKey& key) const{
428  return point3d(float(keyToCoord(key[0])), float(keyToCoord(key[1])), float(keyToCoord(key[2])));
429  }
430 
431  /// converts from an addressing key at a given depth into a coordinate
432  /// corresponding to the key's center
433  inline point3d keyToCoord(const OcTreeKey& key, unsigned depth) const{
434  return point3d(float(keyToCoord(key[0], depth)), float(keyToCoord(key[1], depth)), float(keyToCoord(key[2], depth)));
435  }
436 
437  /// @deprecated, replaced with coordToKeyChecked()
438  DEPRECATED( bool genKeyValue(double coordinate, unsigned short int& keyval) const) {
439  return coordToKeyChecked(coordinate, keyval);
440  }
441 
442  /// @deprecated, replaced with coordToKeyChecked()
443  DEPRECATED( bool genKey(const point3d& point, OcTreeKey& key) const ) {
444  return coordToKeyChecked(point, key);
445  }
446 
447  /// @deprecated, replaced by adjustKeyAtDepth() or coordToKey() with depth parameter
448  DEPRECATED( bool genKeyValueAtDepth(const unsigned short int keyval, unsigned int depth, unsigned short int &out_keyval) const );
449 
450  /// @deprecated, replaced by adjustKeyAtDepth() or coordToKey() with depth parameter
451  DEPRECATED( bool genKeyAtDepth(const OcTreeKey& key, unsigned int depth, OcTreeKey& out_key) const );
452 
453  /// @deprecated, replaced by keyToCoord()
454  /// Will always return true, there is no more boundary check here
455  DEPRECATED( bool genCoordFromKey(const unsigned short int& key, unsigned depth, float& coord) const ){
456  coord = float(keyToCoord(key, depth));
457  return true;
458  }
459 
460  /// @deprecated, replaced by keyToCoord()
461  /// Will always return true, there is no more boundary check here
462  DEPRECATED( inline bool genCoordFromKey(const unsigned short int& key, float& coord, unsigned depth) const) {
463  coord = float(keyToCoord(key, depth));
464  return true;
465  }
466 
467  /// @deprecated, replaced by keyToCoord()
468  /// Will always return true, there is no more boundary check here
469  DEPRECATED( inline bool genCoordFromKey(const unsigned short int& key, float& coord) const) {
470  coord = float(keyToCoord(key));
471  return true;
472  }
473 
474  /// @deprecated, replaced by keyToCoord()
475  DEPRECATED( double genCoordFromKey(const unsigned short int& key, unsigned depth) const) {
476  return keyToCoord(key, depth);
477  }
478 
479  /// @deprecated, replaced by keyToCoord()
480  DEPRECATED( inline double genCoordFromKey(const unsigned short int& key) const) {
481  return keyToCoord(key);
482  }
483 
484  /// @deprecated, replaced by keyToCoord().
485  /// Will always return true, there is no more boundary check here
486  DEPRECATED( inline bool genCoords(const OcTreeKey& key, unsigned int depth, point3d& point) const){
487  point = keyToCoord(key, depth);
488  return true;
489  }
490 
491  /// generate child index (between 0 and 7) from key at given tree depth
492  /// DEPRECATED
493  DEPRECATED( inline void genPos(const OcTreeKey& key, int depth, unsigned int& pos) const) {
494  pos = computeChildIdx(key, depth);
495  }
496 
497  protected:
498  /// Constructor to enable derived classes to change tree constants.
499  /// This usually requires a re-implementation of some core tree-traversal functions as well!
500  OcTreeBaseImpl(double resolution, unsigned int tree_depth, unsigned int tree_max_val);
501 
502  /// recalculates min and max in x, y, z. Does nothing when tree size didn't change.
503  void calcMinMax();
504 
505  void calcNumNodesRecurs(NODE* node, size_t& num_nodes) const;
506 
507  /// recursive call of deleteNode()
508  bool deleteNodeRecurs(NODE* node, unsigned int depth, unsigned int max_depth, const OcTreeKey& key);
509 
510  /// recursive call of prune()
511  void pruneRecurs(NODE* node, unsigned int depth, unsigned int max_depth, unsigned int& num_pruned);
512 
513  /// recursive call of expand()
514  void expandRecurs(NODE* node, unsigned int depth, unsigned int max_depth);
515 
516  size_t getNumLeafNodesRecurs(const NODE* parent) const;
517 
518  private:
519  /// Assignment operator is private: don't (re-)assign octrees
520  /// (const-parameters can't be changed) - use the copy constructor instead.
522 
523  protected:
524 
525  NODE* root;
526 
527  // constants of the tree
528  const unsigned int tree_depth; ///< Maximum tree depth is fixed to 16 currently
529  const unsigned int tree_max_val;
530  double resolution; ///< in meters
531  double resolution_factor; ///< = 1. / resolution
532 
533  size_t tree_size; ///< number of nodes in tree
534  /// flag to denote whether the octree extent changed (for lazy min/max eval)
536 
537  point3d tree_center; // coordinate offset of tree
538 
539  double max_value[3]; ///< max in x, y, z
540  double min_value[3]; ///< min in x, y, z
541  /// contains the size of a voxel at level i (0: root node). tree_depth+1 levels (incl. 0)
542  std::vector<double> sizeLookupTable;
543 
544  KeyRay keyray; // data structure for ray casting
545 
546  const leaf_iterator leaf_iterator_end;
547  const leaf_bbx_iterator leaf_iterator_bbx_end;
548  const tree_iterator tree_iterator_end;
549 
550 
551  };
552 
553 }
554 
555 #include <mrpt/otherlibs/octomap/OcTreeBaseImpl.hxx>
556 
557 #endif
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
Definition: OcTreeKey.h:190
const tree_iterator tree_iterator_end
std::string getTreeType() const
DEPRECATED(bool genCoordFromKey(const unsigned short int &key, unsigned depth, float &coord) const )
const iterator end() const
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
double volume() const
void calcNumNodesRecurs(NODE *node, size_t &num_nodes) const
std::list< octomath::Vector3 > point3d_list
Definition: octomap_types.h:68
leaf_iterator begin_leafs(unsigned char maxDepth=0) const
OcTreeBaseImpl< NODE, INTERFACE > & operator=(const OcTreeBaseImpl< NODE, INTERFACE > &)
Assignment operator is private: don&#39;t (re-)assign octrees (const-parameters can&#39;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&#39;t change.
OcTreeKey is a container class for internal key addressing.
Definition: OcTreeKey.h:68
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.
Definition: octomap_types.h:63
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&#39;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&#39;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
NODE * getRoot() const
bool operator==(const OcTreeBaseImpl< NODE, INTERFACE > &rhs) const
double getResolution() const
This class represents a three-dimensional vector.
Definition: Vector3.h:65
point3d keyToCoord(const OcTreeKey &key) const
converts from an addressing key at the lowest tree level into a coordinate corresponding to the key&#39;s...



Page generated by Doxygen 1.8.11 for MRPT 1.4.0 SVN:Unversioned directory at Mon May 30 18:20:32 UTC 2016