37 #ifndef OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_GNAT_
38 #define OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_GNAT_
40 #include "ompl/datastructures/NearestNeighbors.h"
41 #include "ompl/datastructures/GreedyKCenters.h"
42 #include "ompl/util/Exception.h"
43 #include <boost/unordered_set.hpp>
65 typedef std::pair<const _T*,double> DataDist;
66 struct DataDistCompare
68 bool operator()(
const DataDist& d0,
const DataDist& d1)
70 return d0.second < d1.second;
73 typedef std::priority_queue<DataDist, std::vector<DataDist>, DataDistCompare> NearQueue;
78 typedef std::pair<Node*,double> NodeDist;
79 struct NodeDistCompare
81 bool operator()(
const NodeDist& n0,
const NodeDist& n1)
const
83 return (n0.second - n0.first->maxRadius_) > (n1.second - n1.first->maxRadius_);
86 typedef std::priority_queue<NodeDist, std::vector<NodeDist>, NodeDistCompare> NodeQueue;
91 unsigned int maxDegree = 6,
unsigned int maxNumPtsPerLeaf = 50,
92 unsigned int removedCacheSize = 50,
bool rebalancing =
false)
96 rebuildSize_(rebalancing ? maxNumPtsPerLeaf*degree : std::numeric_limits<std::size_t>::max()),
123 virtual void add(
const _T &data)
133 virtual void add(
const std::vector<_T> &data)
137 else if (data.size()>0)
140 for (
unsigned int i=1; i<data.size(); ++i)
145 size_ += data.size();
160 virtual bool remove(
const _T &data)
162 if (!
tree_)
return false;
166 if (*nbhQueue.top().first != data)
168 removed_.insert(nbhQueue.top().first);
183 if (!nbh.empty())
return nbh[0];
185 throw Exception(
"No elements found in nearest neighbors data structure");
188 virtual void nearestK(
const _T &data, std::size_t k, std::vector<_T> &nbh)
const
200 virtual void nearestR(
const _T &data,
double radius, std::vector<_T> &nbh)
const
211 virtual std::size_t
size(
void)
const
216 virtual void list(std::vector<_T> &data)
const
219 data.reserve(
size());
221 tree_->list(*
this, data);
225 friend std::ostream& operator<<(std::ostream& out, const NearestNeighborsGNAT<_T>& gnat)
230 if (!gnat.removed_.empty())
232 out <<
"Elements marked for removal:\n";
233 for (
typename boost::unordered_set<const _T*>::const_iterator it = gnat.removed_.begin();
234 it != gnat.removed_.end(); it++)
243 void integrityCheck()
246 boost::unordered_set<const _T*> tmp;
251 for (
typename boost::unordered_set<const _T*>::iterator it=tmp.begin(); it!=tmp.end(); it++)
254 for (i=0; i<lst.size(); ++i)
260 std::cout <<
"***** FAIL!! ******\n" << *
this <<
'\n';
261 for (
unsigned int j=0; j<lst.size(); ++j) std::cout<<lst[j]<<
'\t';
262 std::cout<<std::endl;
264 assert(i != lst.size());
270 if (lst.size() !=
size_)
271 std::cout <<
"#########################################\n" << *
this << std::endl;
272 assert(lst.size() ==
size_);
275 typedef NearestNeighborsGNAT<_T> GNAT;
296 tree_->
nearestK(*
this, data, k, nbhQueue, nodeQueue, isPivot);
297 while (nodeQueue.size() > 0)
299 dist = nbhQueue.top().second;
300 nodeDist = nodeQueue.top();
302 if (nbhQueue.size() == k &&
303 (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
304 nodeDist.second < nodeDist.first->minRadius_ - dist))
306 nodeDist.first->nearestK(*
this, data, k, nbhQueue, nodeQueue, isPivot);
313 double dist = radius;
320 while (nodeQueue.size() > 0)
322 nodeDist = nodeQueue.top();
324 if (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
325 nodeDist.second < nodeDist.first->minRadius_ - dist)
327 nodeDist.first->nearestR(*
this, data, radius, nbhQueue, nodeQueue);
334 typename std::vector<_T>::reverse_iterator it;
335 nbh.resize(nbhQueue.size());
336 for (it=nbh.rbegin(); it!=nbh.rend(); it++, nbhQueue.pop())
337 *it = *nbhQueue.top().first;
346 Node(
int degree,
int capacity,
const _T& pivot)
348 minRadius_(std::numeric_limits<double>::infinity()),
353 data_.reserve(capacity+1);
358 for (
unsigned int i=0; i<
children_.size(); ++i)
386 data_.push_back(data);
403 std::vector<double> dist(
children_.size());
407 for (
unsigned int i=1; i<
children_.size(); ++i)
413 for (
unsigned int i=0; i<
children_.size(); ++i)
415 children_[minInd]->updateRadius(minDist);
422 unsigned int sz =
data_.size();
430 std::vector<std::vector<double> > dists;
431 std::vector<unsigned int> pivots;
435 for(
unsigned int i=0; i<pivots.size(); i++)
438 for (
unsigned int j=0; j<
data_.size(); ++j)
441 for (
unsigned int i=1; i<
degree_; ++i)
442 if (dists[j][i] < dists[j][k])
450 for (
unsigned int i=0; i<
degree_; ++i)
454 for (
unsigned int i=0; i<
degree_; ++i)
457 children_[i]->degree_ = std::min(std::max(
468 for (
unsigned int i=0; i<
degree_; ++i)
474 bool insertNeighborK(NearQueue& nbh, std::size_t k,
const _T& data,
const _T& key,
double dist)
const
478 nbh.push(std::make_pair(&data, dist));
481 else if (dist < nbh.top().second ||
482 (dist < std::numeric_limits<double>::epsilon() && data==key))
485 nbh.push(std::make_pair(&data, dist));
497 NearQueue& nbh, NodeQueue& nodeQueue,
bool& isPivot)
const
499 for (
unsigned int i=0; i<
data_.size(); ++i)
509 std::vector<double> distToPivot(
children_.size());
510 std::vector<int> permutation(
children_.size());
512 for (
unsigned int i=0; i<permutation.size(); ++i)
514 std::random_shuffle(permutation.begin(), permutation.end());
516 for (
unsigned int i=0; i<
children_.size(); ++i)
517 if (permutation[i] >= 0)
525 dist = nbh.top().second;
526 for (
unsigned int j=0; j<
children_.size(); ++j)
527 if (permutation[j] >=0 && i != j &&
528 (distToPivot[permutation[i]] - dist > child->
maxRange_[permutation[j]] ||
529 distToPivot[permutation[i]] + dist < child->
minRange_[permutation[j]]))
534 dist = nbh.top().second;
535 for (
unsigned int i=0; i<
children_.size(); ++i)
536 if (permutation[i] >= 0)
540 (distToPivot[permutation[i]] - dist <= child->
maxRadius_ &&
541 distToPivot[permutation[i]] + dist >= child->
minRadius_))
542 nodeQueue.push(std::make_pair(child, distToPivot[permutation[i]]));
550 nbh.push(std::make_pair(&data, dist));
555 void nearestR(
const GNAT& gnat,
const _T &data,
double r, NearQueue& nbh, NodeQueue& nodeQueue)
const
559 for (
unsigned int i=0; i<
data_.size(); ++i)
565 std::vector<double> distToPivot(
children_.size());
566 std::vector<int> permutation(
children_.size());
568 for (
unsigned int i=0; i<permutation.size(); ++i)
570 std::random_shuffle(permutation.begin(), permutation.end());
572 for (
unsigned int i=0; i<
children_.size(); ++i)
573 if (permutation[i] >= 0)
578 for (
unsigned int j=0; j<
children_.size(); ++j)
579 if (permutation[j] >=0 && i != j &&
580 (distToPivot[i] - dist > child->
maxRange_[permutation[j]] ||
581 distToPivot[i] + dist < child->
minRange_[permutation[j]]))
585 for (
unsigned int i=0; i<
children_.size(); ++i)
586 if (permutation[i] >= 0)
589 if (distToPivot[i] - dist <= child->
maxRadius_ &&
591 nodeQueue.push(std::make_pair(child, distToPivot[i]));
596 void list(
const GNAT& gnat, std::vector<_T> &data)
const
600 for (
unsigned int i=0; i<
data_.size(); ++i)
602 data.push_back(
data_[i]);
603 for (
unsigned int i=0; i<
children_.size(); ++i)
607 friend std::ostream& operator<<(std::ostream& out,
const Node& node)
609 out <<
"\ndegree:\t" << node.degree_;
610 out <<
"\nminRadius:\t" << node.minRadius_;
611 out <<
"\nmaxRadius:\t" << node.maxRadius_;
612 out <<
"\nminRange:\t";
613 for (
unsigned int i=0; i<node.minRange_.size(); ++i)
614 out << node.minRange_[i] <<
'\t';
615 out <<
"\nmaxRange: ";
616 for (
unsigned int i=0; i<node.maxRange_.size(); ++i)
617 out << node.maxRange_[i] <<
'\t';
618 out <<
"\npivot:\t" << node.pivot_;
620 for (
unsigned int i=0; i<node.data_.size(); ++i)
621 out << node.data_[i] <<
'\t';
622 out <<
"\nthis:\t" << &node;
623 out <<
"\nchildren:\n";
624 for (
unsigned int i=0; i<node.children_.size(); ++i)
625 out << node.children_[i] <<
'\t';
627 for (
unsigned int i=0; i<node.children_.size(); ++i)
628 out << *node.children_[i] <<
'\n';