OcTreeBaseImpl.h
Go to the documentation of this file.
00001 /*
00002  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
00003  * http://octomap.github.com/
00004  *
00005  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
00006  * All rights reserved.
00007  * License: New BSD
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of the University of Freiburg nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
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     // the actual iterator implementation is included here
00079     // as a member from this file
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     // -- statistics  ----------------------
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     // -- access tree nodes  ------------------
00214 
00216     void getUnknownLeafCenters(point3d_list& node_centers, point3d pmin, point3d pmax) const;
00217 
00218 
00219     // -- raytracing  -----------------------
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     // file IO
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;}; // TODO: RVE?
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     // Key / coordinate conversion functions
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     // constants of the tree
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;  // coordinate offset of tree
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


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Feb 11 2016 23:50:59