Go to the documentation of this file.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
00035 #ifndef OCT_TREE_HH
00036 #define OCT_TREE_HH
00037
00038 #include <ndt_map/spatial_index.h>
00039 #include <ndt_map/ndt_cell.h>
00040
00041 #include <vector>
00042 #include <cstdio>
00043 #include <Eigen/Core>
00044
00045 namespace lslgeneric
00046 {
00047
00048 template <typename PointT>
00049 class AdaptiveOctTree;
00057 template <typename PointT>
00058 class OctTree : public SpatialIndex<PointT>
00059 {
00060 protected:
00061 OctTree* parent_;
00062 OctTree *children_[8];
00063 NDTCell<PointT> *myCell_;
00064
00065 std::vector< Cell<PointT>* > myLeafs_;
00066 unsigned int depth_;
00067 bool leaf_;
00068 bool leafsCached_;
00069
00071 virtual size_t getIndexForPoint(const PointT &pt) const;
00072
00074 virtual void computeLeafCells();
00075
00076
00077 public:
00078
00080 int MAX_DEPTH;
00082 int MAX_POINTS;
00084 double SMALL_CELL_SIZE;
00086 double BIG_CELL_SIZE;
00087 bool parametersSet_;
00088
00090 OctTree();
00092 OctTree(PointT center, double xsize, double ysize,
00093 double zsize, NDTCell<PointT>* type, OctTree<PointT> *_parent=NULL, unsigned int _depth=0);
00094 virtual ~OctTree();
00095
00097 void setParameters(double _BIG_CELL_SIZE =4,
00098 double _SMALL_CELL_SIZE =0.5 ,
00099 int _MAX_POINTS =10,
00100 int _MAX_DEPTH =20
00101 );
00102
00104 virtual Cell<PointT>* addPoint(const PointT &point);
00105
00107 virtual Cell<PointT>* getCellForPoint(const PointT &point);
00108 inline virtual Cell<PointT>* getMyCell()
00109 {
00110 return myCell_;
00111 }
00112 virtual OctTree<PointT>* getLeafForPoint(const PointT &point);
00113
00115 virtual void setCellType(Cell<PointT> *type);
00116
00118 virtual typename SpatialIndex<PointT>::CellVectorItr begin();
00120 virtual typename SpatialIndex<PointT>::CellVectorItr end();
00121
00123 void print();
00124
00126 void writeToVRML(const char* filename);
00127 void writeToVRML(FILE* fout);
00128
00130 inline OctTree<PointT>* getChild(int idx)
00131 {
00132 if(idx<8 && idx>=0)
00133 {
00134 return children_[idx];
00135 }
00136 return NULL;
00137 }
00138 inline bool isLeaf()
00139 {
00140 return leaf_;
00141 }
00142
00144 virtual SpatialIndex<PointT>* clone() const;
00145 virtual SpatialIndex<PointT>* copy() const;
00146
00147 virtual void setCenter(const double &cx, const double &cy, const double &cz);
00148 virtual void setSize(const double &sx, const double &sy, const double &sz);
00149
00150 virtual void getNeighbors(const PointT &point, const double &radius, std::vector<Cell<PointT>*> &cells);
00151 virtual bool intersectSphere(const PointT center, const double &radius) const;
00152
00153 virtual Cell<PointT>* getClosestLeafCell(const PointT &pt);
00154 virtual NDTCell<PointT>* getClosestNDTCell(const PointT &pt);
00155
00156 friend class AdaptiveOctTree<PointT>;
00157 public:
00158 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00159 };
00160
00161 }
00162
00163 #include<ndt_map/impl/oc_tree.hpp>
00164
00165 #endif