face_contour_detector: KdTree.h Source File

Go to the documentation of this file.
00001 #ifndef FACE_CONTOUR_DETECTOR_KDTREE_H_
00002 #define FACE_CONTOUR_DETECTOR_KDTREE_H_
00003 
00004 #include <vector>
00005 
00006 namespace face_contour_detector {
00007 
00010         template <class T, class V>
00011         class KdTree {
00012         public:
00015                 KdTree(int dimensions);
00017                 virtual ~KdTree();
00021                 void Insert(const std::vector<T>& point, const V& value);
00025                 const V* Find(const std::vector<T>& point);
00026 
00027         private:
00028                 class Node;
00029                 int m_dimensions;
00030                 Node* m_root;
00031 
00032                 class Node {
00033                 public:
00034                         Node(const std::vector<T>& point, const V& value, int dimension);
00035                         virtual ~Node();
00036                         std::vector<T> point;
00037                         V value;
00038                         int dimension;
00039                         T dimensionValue;
00040                         Node* smallerNode;
00041                         Node* biggerAndEqNode;
00042                 };
00043 
00044         };
00045 
00046 
00048         template<class T, class V>
00049         KdTree<T,V>::KdTree(int dimensions) : m_dimensions(dimensions), m_root(0) {}
00050 
00051         template<class T, class V>
00052         KdTree<T,V>::~KdTree() { if (m_root != 0) delete m_root; }
00053 
00054         template<class T, class V>
00055         void KdTree<T,V>::Insert(const std::vector<T>& point, const V& value) {
00056                 ROS_ASSERT(int(point.size()) == m_dimensions);
00057                 if (m_root == 0) {
00058                         m_root = new Node(point, value, 0);
00059                 } else {
00060                         Node* currNode = m_root;
00061                         while (currNode != 0) {
00062                                 if (currNode->dimensionValue > point[currNode->dimension]) {
00063                                         if (currNode->smallerNode == 0) {
00064                                                 currNode->smallerNode = new Node(point, value, (currNode->dimension)%m_dimensions);
00065                                                 return;
00066                                         }
00067                                         currNode = currNode->smallerNode;
00068                                 } else {
00069                                         if (currNode->biggerAndEqNode == 0) {
00070                                                 currNode->biggerAndEqNode = new Node(point, value, (currNode->dimension)%m_dimensions);
00071                                                 return;
00072                                         }
00073                                         currNode = currNode->biggerAndEqNode;
00074                                 }
00075                         }
00076                 }
00077 
00078         }
00079 
00080         template<class T, class V>
00081         const V* KdTree<T,V>::Find(const std::vector<T>& point) {
00082                 Node* currNode = m_root;
00083                 while(currNode != 0) {
00084                         bool eq = true;
00085                         typename std::vector<T>::iterator it;
00086                         typename std::vector<T>::const_iterator itP = point.begin();
00087                         for (it = currNode->point.begin(); it != currNode->point.end(); it++) {
00088                                 if (*it != *itP) {
00089                                         eq = false;
00090                                         break;
00091                                 }
00092                                 itP++;
00093                         }
00094                         if (eq) {
00095                                 return &(currNode->value);
00096                         }
00097                         if (point[currNode->dimension] < currNode->dimensionValue) {
00098                                 currNode = currNode->smallerNode;
00099                         } else {
00100                                 currNode = currNode->biggerAndEqNode;
00101                         }
00102                 }
00103                 return 0;
00104         }
00105 
00106         template<class T, class V>
00107         KdTree<T,V>::Node::Node(const std::vector<T>& point, const V& value, int dimension) : point(point), value(value), dimension(dimension), smallerNode(0), biggerAndEqNode(0) {
00108                 assert(dimension < point.size());
00109                 dimensionValue = point[dimension];
00110         }
00111 
00112         template<class T, class V>
00113         KdTree<T,V>::Node::~Node() {
00114                 if (smallerNode != 0) delete smallerNode;
00115                 if (biggerAndEqNode != 0) delete biggerAndEqNode;
00116         }
00117 
00118 }//namespace face_contour_detector
00119 
00120 
00121 #endif /* FACE_CONTOUR_DETECTOR_KDTREE_H_ */