00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #ifndef OCTOMAP_OCTREE_BASE_IMPL_H
00035 #define OCTOMAP_OCTREE_BASE_IMPL_H
00036
00037
00038 #include <list>
00039 #include <limits>
00040 #include <iterator>
00041 #include <stack>
00042
00043
00044 #include "octomap_types.h"
00045 #include "OcTreeKey.h"
00046 #include "ScanGraph.h"
00047
00048
00049 namespace octomap {
00050
00051
00071 template <class NODE,class INTERFACE>
00072 class OcTreeBaseImpl : public INTERFACE {
00073
00074 public:
00076 typedef NODE NodeType;
00077
00078
00079
00080 #include <octomap/OcTreeIterator.hxx>
00081
00082 OcTreeBaseImpl(double resolution);
00083 virtual ~OcTreeBaseImpl();
00084
00086 OcTreeBaseImpl(const OcTreeBaseImpl<NODE,INTERFACE>& rhs);
00087
00088
00095 void swapContent(OcTreeBaseImpl<NODE,INTERFACE>& rhs);
00096
00099 bool operator== (const OcTreeBaseImpl<NODE,INTERFACE>& rhs) const;
00100
00101 std::string getTreeType() const {return "OcTreeBaseImpl";}
00102
00105 void setResolution(double r);
00106 inline double getResolution() const { return resolution; }
00107
00108 inline unsigned int getTreeDepth () const { return tree_depth; }
00109
00110 inline double getNodeSize(unsigned depth) const {assert(depth <= tree_depth); return sizeLookupTable[depth];}
00111
00117 inline NODE* getRoot() const { return root; }
00118
00124 NODE* search(double x, double y, double z, unsigned int depth = 0) const;
00125
00131 NODE* search(const point3d& value, unsigned int depth = 0) const;
00132
00138 NODE* search(const OcTreeKey& key, unsigned int depth = 0) const;
00139
00145 bool deleteNode(double x, double y, double z, unsigned int depth = 0);
00146
00152 bool deleteNode(const point3d& value, unsigned int depth = 0);
00153
00159 bool deleteNode(const OcTreeKey& key, unsigned int depth = 0);
00160
00162 void clear();
00163
00170 virtual void prune();
00171
00174 virtual void expand();
00175
00176
00177
00179 virtual inline size_t size() const { return tree_size; }
00180
00182 virtual size_t memoryUsage() const;
00183
00185 virtual inline size_t memoryUsageNode() const {return sizeof(NODE); };
00186
00189 unsigned long long memoryFullGrid() const;
00190
00191 double volume();
00192
00194 virtual void getMetricSize(double& x, double& y, double& z);
00196 virtual void getMetricSize(double& x, double& y, double& z) const;
00198 virtual void getMetricMin(double& x, double& y, double& z);
00200 void getMetricMin(double& x, double& y, double& z) const;
00202 virtual void getMetricMax(double& x, double& y, double& z);
00204 void getMetricMax(double& x, double& y, double& z) const;
00205
00207 size_t calcNumNodes() const;
00208
00210 size_t getNumLeafNodes() const;
00211
00212
00213
00214
00216 void getUnknownLeafCenters(point3d_list& node_centers, point3d pmin, point3d pmax) const;
00217
00218
00219
00220
00231 bool computeRayKeys(const point3d& origin, const point3d& end, KeyRay& ray) const;
00232
00233
00245 bool computeRay(const point3d& origin, const point3d& end, std::vector<point3d>& ray);
00246
00247
00248
00249
00256 std::istream& readData(std::istream &s);
00257
00260 std::ostream& writeData(std::ostream &s) const;
00261
00262 class leaf_iterator;
00263 class tree_iterator;
00264 class leaf_bbx_iterator;
00265 typedef leaf_iterator iterator;
00266
00268 iterator begin(unsigned char maxDepth=0) const {return iterator(this, maxDepth);};
00270 const iterator end() const {return leaf_iterator_end;};
00271
00273 leaf_iterator begin_leafs(unsigned char maxDepth=0) const {return leaf_iterator(this, maxDepth);};
00275 const leaf_iterator end_leafs() const {return leaf_iterator_end;}
00276
00278 leaf_bbx_iterator begin_leafs_bbx(const OcTreeKey& min, const OcTreeKey& max, unsigned char maxDepth=0) const {
00279 return leaf_bbx_iterator(this, min, max, maxDepth);
00280 }
00282 leaf_bbx_iterator begin_leafs_bbx(const point3d& min, const point3d& max, unsigned char maxDepth=0) const {
00283 return leaf_bbx_iterator(this, min, max, maxDepth);
00284 }
00286 const leaf_bbx_iterator end_leafs_bbx() const {return leaf_iterator_bbx_end;}
00287
00289 tree_iterator begin_tree(unsigned char maxDepth=0) const {return tree_iterator(this, maxDepth);}
00291 const tree_iterator end_tree() const {return tree_iterator_end;}
00292
00293
00294
00295
00296
00298 inline unsigned short int coordToKey(double coordinate) const{
00299 return ((int) floor(resolution_factor * coordinate)) + tree_max_val;
00300 }
00301
00303 unsigned short int coordToKey(double coordinate, unsigned depth) const;
00304
00305
00307 inline OcTreeKey coordToKey(const point3d& coord) const{
00308 return OcTreeKey(coordToKey(coord(0)), coordToKey(coord(1)), coordToKey(coord(2)));
00309 }
00310
00312 inline OcTreeKey coordToKey(double x, double y, double z) const{
00313 return OcTreeKey(coordToKey(x), coordToKey(y), coordToKey(z));
00314 }
00315
00317 inline OcTreeKey coordToKey(const point3d& coord, unsigned depth) const{
00318 if (depth == tree_depth)
00319 return coordToKey(coord);
00320 else
00321 return OcTreeKey(coordToKey(coord(0), depth), coordToKey(coord(1), depth), coordToKey(coord(2), depth));
00322 }
00323
00325 inline OcTreeKey coordToKey(double x, double y, double z, unsigned depth) const{
00326 if (depth == tree_depth)
00327 return coordToKey(x,y,z);
00328 else
00329 return OcTreeKey(coordToKey(x, depth), coordToKey(y, depth), coordToKey(z, depth));
00330 }
00331
00340 inline OcTreeKey adjustKeyAtDepth(const OcTreeKey& key, unsigned int depth) const{
00341 if (depth == tree_depth)
00342 return key;
00343
00344 assert(depth <= tree_depth);
00345 return OcTreeKey(adjustKeyAtDepth(key[0], depth), adjustKeyAtDepth(key[1], depth), adjustKeyAtDepth(key[2], depth));
00346 }
00347
00356 unsigned short int adjustKeyAtDepth(unsigned short int key, unsigned int depth) const;
00357
00365 bool coordToKeyChecked(const point3d& coord, OcTreeKey& key) const;
00366
00375 bool coordToKeyChecked(const point3d& coord, unsigned depth, OcTreeKey& key) const;
00376
00386 bool coordToKeyChecked(double x, double y, double z, OcTreeKey& key) const;
00387
00398 bool coordToKeyChecked(double x, double y, double z, unsigned depth, OcTreeKey& key) const;
00399
00407 bool coordToKeyChecked(double coordinate, unsigned short int& key) const;
00408
00417 bool coordToKeyChecked(double coordinate, unsigned depth, unsigned short int& key) const;
00418
00421 double keyToCoord(unsigned short int key, unsigned depth) const;
00422
00425 inline double keyToCoord(unsigned short int key) const{
00426 return (double( (int) key - (int) this->tree_max_val ) +0.5) * this->resolution;
00427 }
00428
00431 inline point3d keyToCoord(const OcTreeKey& key) const{
00432 return point3d(float(keyToCoord(key[0])), float(keyToCoord(key[1])), float(keyToCoord(key[2])));
00433 }
00434
00437 inline point3d keyToCoord(const OcTreeKey& key, unsigned depth) const{
00438 return point3d(float(keyToCoord(key[0], depth)), float(keyToCoord(key[1], depth)), float(keyToCoord(key[2], depth)));
00439 }
00440
00441 protected:
00444 OcTreeBaseImpl(double resolution, unsigned int tree_depth, unsigned int tree_max_val);
00445
00447 void init();
00448
00450 void calcMinMax();
00451
00452 void calcNumNodesRecurs(NODE* node, size_t& num_nodes) const;
00453
00455 bool deleteNodeRecurs(NODE* node, unsigned int depth, unsigned int max_depth, const OcTreeKey& key);
00456
00458 void pruneRecurs(NODE* node, unsigned int depth, unsigned int max_depth, unsigned int& num_pruned);
00459
00461 void expandRecurs(NODE* node, unsigned int depth, unsigned int max_depth);
00462
00463 size_t getNumLeafNodesRecurs(const NODE* parent) const;
00464
00465 private:
00468 OcTreeBaseImpl<NODE,INTERFACE>& operator=(const OcTreeBaseImpl<NODE,INTERFACE>&);
00469
00470 protected:
00471
00472 NODE* root;
00473
00474
00475 const unsigned int tree_depth;
00476 const unsigned int tree_max_val;
00477 double resolution;
00478 double resolution_factor;
00479
00480 size_t tree_size;
00481
00482 bool size_changed;
00483
00484 point3d tree_center;
00485
00486 double max_value[3];
00487 double min_value[3];
00488
00489 std::vector<double> sizeLookupTable;
00490
00492 std::vector<KeyRay> keyrays;
00493
00494 const leaf_iterator leaf_iterator_end;
00495 const leaf_bbx_iterator leaf_iterator_bbx_end;
00496 const tree_iterator tree_iterator_end;
00497
00498
00499 };
00500
00501 }
00502
00503 #include <octomap/OcTreeBaseImpl.hxx>
00504
00505 #endif