00001 #ifndef NAVLIB_PATHPLANNER_VPTREE
00002 #define NAVLIB_PATHPLANNER_VPTREE
00003 #include <boost/bind.hpp>
00004 #include <boost/function.hpp>
00005 #undef min
00006 #undef max
00007 #include <limits>
00008 #include <list>
00009 #include <queue>
00010 #include <vector>
00011
00012 namespace navlib
00013 {
00014
00017 template<class T>
00018 class VpTree
00019 {
00021 typedef boost::function<double (const T&, const T&)> VpDistance;
00022
00023
00025 typedef std::pair<T, std::vector<double> > VpInitElement;
00026
00028 struct VpNode
00029 {
00030 VpNode(const T& datai) : data(datai) {}
00031 T data;
00032 std::vector<VpNode*> children;
00033 std::vector<double> lowerBounds;
00034 std::vector<double> upperBounds;
00035 };
00036
00038 struct VpSearchElement
00039 {
00041 VpSearchElement(double distancei, bool isVantagei, VpNode* nodei,
00042 std::vector<double> distancesi=std::vector<double>()) :
00043 distance(distancei), isVantage(isVantagei), node(nodei),
00044 distances(distancesi) {}
00045
00047 bool operator <(const VpSearchElement& e) const
00048 {
00049 if (distance < e.distance) return false;
00050 if (distance > e.distance) return true;
00051 return false;
00052 }
00053
00054 double distance;
00055 bool isVantage;
00056 VpNode* node;
00057 std::vector<double> distances;
00058 };
00059
00060 public:
00061 VpDistance _distance;
00062 VpNode* _root;
00063 double _tau;
00064 size_t _maxChildren;
00065
00067 VpTree(std::vector<T> items, VpDistance distance, size_t maxChildren=2) :
00068 _distance(distance), _root(NULL), _maxChildren(maxChildren)
00069 {
00070 if (items.size() != 0)
00071 {
00072 std::vector<VpInitElement> S;
00073 for (size_t k=0; k<items.size(); k++)
00074 {
00075 S.push_back(VpInitElement(items[k], std::vector<double>()));
00076 }
00077 _root = this->recurseInit(S);
00078 }
00079 }
00080
00082 ~VpTree()
00083 {
00084 this->recurseDelete(_root);
00085 }
00086
00088 std::vector<T> search(const T& query, size_t k)
00089 {
00090 std::vector<T> result;
00091
00092
00093 if (_root == NULL)
00094 {
00095 return result;
00096 }
00097
00098
00099 std::priority_queue<VpSearchElement> heap;
00100 heap.push(VpSearchElement(0.0, true, _root));
00101
00102 while (!heap.empty())
00103 {
00104 VpSearchElement top = heap.top(); heap.pop();
00105 if (top.isVantage)
00106 {
00107 this->recurseSearch(top.node, query, top.distances, heap);
00108 }
00109 else
00110 {
00111 result.push_back(top.node->data);
00112 if (result.size() >= k)
00113 {
00114 return result;
00115 }
00116 }
00117 }
00118 return result;
00119 }
00120
00121 private:
00123 void recurseSearch(VpNode* node, const T& query,
00124 std::vector<double> distances, std::priority_queue<VpSearchElement>& heap)
00125 {
00126 double d = _distance(node->data, query);
00127 distances.push_back(d);
00128 heap.push(VpSearchElement(d, false, node));
00129
00130 for (size_t k=0; k<node->children.size(); k++)
00131 {
00132
00133 VpNode* child = node->children[k];
00134
00135
00136 double minimumDistance = 0.0;
00137 for (size_t k=0; k<distances.size(); k++)
00138 {
00139 if (distances[k] < child->lowerBounds[k])
00140 {
00141 double diff = child->lowerBounds[k] - distances[k];
00142 minimumDistance = std::max(minimumDistance, diff);
00143 }
00144 else if (distances[k] > child->upperBounds[k])
00145 {
00146 double diff = distances[k] - child->lowerBounds[k];
00147 minimumDistance = std::max(minimumDistance, diff);
00148 }
00149 }
00150
00151
00152 heap.push(VpSearchElement(minimumDistance, true, child, distances));
00153 }
00154 }
00155
00157 void recurseDelete(VpNode* node)
00158 {
00159 for (size_t k=0; k<node->children.size(); k++)
00160 {
00161 this->recurseDelete(node->children[k]);
00162 }
00163 delete node;
00164 }
00165
00167 VpNode* recurseInit(std::vector<VpInitElement> S)
00168 {
00169
00170 VpNode* node = new VpNode(S[0].first);
00171
00172
00173 for (size_t i=0; i<S[0].second.size(); i++)
00174 {
00175
00176 double minDistance = std::numeric_limits<double>::infinity();
00177 double maxDistance =-std::numeric_limits<double>::infinity();
00178 for (size_t j=0; j<S.size(); j++)
00179 {
00180 minDistance = std::min(minDistance, S[j].second[i]);
00181 maxDistance = std::max(maxDistance, S[j].second[i]);
00182 }
00183
00184
00185 node->lowerBounds.push_back(minDistance);
00186 node->upperBounds.push_back(maxDistance);
00187 }
00188
00189
00190 if (S.size() == 1) return node;
00191
00192
00193 std::list<double> distances;
00194 for (size_t k=1; k<S.size(); k++)
00195 {
00196
00197 double d = _distance(node->data, S[k].first);
00198
00199
00200
00201 S[k].second.push_back(d);
00202
00203
00204 distances.push_back(d);
00205 }
00206
00207
00208 distances.sort();
00209 distances.unique();
00210 std::vector<double> distanceList(distances.begin(), distances.end());
00211
00212
00213 size_t numChildren = std::min(_maxChildren, distanceList.size());
00214 std::vector<double> splitPoints;
00215 splitPoints.push_back(-std::numeric_limits<double>::infinity());
00216 for (size_t k=0; k<numChildren; k++)
00217 {
00218 size_t point = (k+1)*(distanceList.size()-1)/numChildren;
00219 splitPoints.push_back(distanceList[point]);
00220 }
00221 for (size_t i=0; i<numChildren; i++)
00222 {
00223 std::vector<VpInitElement> children;
00224 for (size_t j=1; j<S.size(); j++)
00225 {
00226 if (splitPoints[i] < S[j].second[S[j].second.size()-1] &&
00227 S[j].second[S[j].second.size()-1] <= splitPoints[i+1])
00228 {
00229 children.push_back(S[j]);
00230 }
00231 }
00232 if (children.empty())
00233 continue;
00234 VpNode* child = this->recurseInit(children);
00235 node->children.push_back(child);
00236 }
00237
00238
00239 return node;
00240 }
00241 };
00242
00243 }
00244 #endif
00245