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()),
125 virtual void add(
const _T &data)
139 virtual void add(
const std::vector<_T> &data)
143 else if (data.size()>0)
146 for (
unsigned int i=1; i<data.size(); ++i)
151 size_ += data.size();
166 virtual bool remove(
const _T &data)
168 if (!
size_)
return false;
172 if (*nbhQueue.top().first != data)
174 removed_.insert(nbhQueue.top().first);
189 if (!nbh.empty())
return nbh[0];
191 throw Exception(
"No elements found in nearest neighbors data structure");
194 virtual void nearestK(
const _T &data, std::size_t k, std::vector<_T> &nbh)
const
206 virtual void nearestR(
const _T &data,
double radius, std::vector<_T> &nbh)
const
217 virtual std::size_t
size(
void)
const
222 virtual void list(std::vector<_T> &data)
const
225 data.reserve(
size());
227 tree_->list(*
this, data);
231 friend std::ostream& operator<<(std::ostream& out, const NearestNeighborsGNAT<_T>& gnat)
236 if (!gnat.removed_.empty())
238 out <<
"Elements marked for removal:\n";
239 for (
typename boost::unordered_set<const _T*>::const_iterator it = gnat.removed_.begin();
240 it != gnat.removed_.end(); it++)
249 void integrityCheck()
252 boost::unordered_set<const _T*> tmp;
257 for (
typename boost::unordered_set<const _T*>::iterator it=tmp.begin(); it!=tmp.end(); it++)
260 for (i=0; i<lst.size(); ++i)
266 std::cout <<
"***** FAIL!! ******\n" << *
this <<
'\n';
267 for (
unsigned int j=0; j<lst.size(); ++j) std::cout<<lst[j]<<
'\t';
268 std::cout<<std::endl;
270 assert(i != lst.size());
276 if (lst.size() !=
size_)
277 std::cout <<
"#########################################\n" << *
this << std::endl;
278 assert(lst.size() ==
size_);
281 typedef NearestNeighborsGNAT<_T> GNAT;
302 tree_->
nearestK(*
this, data, k, nbhQueue, nodeQueue, isPivot);
303 while (nodeQueue.size() > 0)
305 dist = nbhQueue.top().second;
306 nodeDist = nodeQueue.top();
308 if (nbhQueue.size() == k &&
309 (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
310 nodeDist.second < nodeDist.first->minRadius_ - dist))
312 nodeDist.first->nearestK(*
this, data, k, nbhQueue, nodeQueue, isPivot);
319 double dist = radius;
326 while (nodeQueue.size() > 0)
328 nodeDist = nodeQueue.top();
330 if (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
331 nodeDist.second < nodeDist.first->minRadius_ - dist)
333 nodeDist.first->nearestR(*
this, data, radius, nbhQueue, nodeQueue);
340 typename std::vector<_T>::reverse_iterator it;
341 nbh.resize(nbhQueue.size());
342 for (it=nbh.rbegin(); it!=nbh.rend(); it++, nbhQueue.pop())
343 *it = *nbhQueue.top().first;
352 Node(
int degree,
int capacity,
const _T& pivot)
354 minRadius_(std::numeric_limits<double>::infinity()),
359 data_.reserve(capacity+1);
364 for (
unsigned int i=0; i<
children_.size(); ++i)
392 data_.push_back(data);
409 std::vector<double> dist(
children_.size());
413 for (
unsigned int i=1; i<
children_.size(); ++i)
419 for (
unsigned int i=0; i<
children_.size(); ++i)
421 children_[minInd]->updateRadius(minDist);
428 unsigned int sz =
data_.size();
436 std::vector<std::vector<double> > dists;
437 std::vector<unsigned int> pivots;
441 for(
unsigned int i=0; i<pivots.size(); i++)
444 for (
unsigned int j=0; j<
data_.size(); ++j)
447 for (
unsigned int i=1; i<
degree_; ++i)
448 if (dists[j][i] < dists[j][k])
456 for (
unsigned int i=0; i<
degree_; ++i)
460 for (
unsigned int i=0; i<
degree_; ++i)
463 children_[i]->degree_ = std::min(std::max(
474 for (
unsigned int i=0; i<
degree_; ++i)
480 bool insertNeighborK(NearQueue& nbh, std::size_t k,
const _T& data,
const _T& key,
double dist)
const
484 nbh.push(std::make_pair(&data, dist));
487 else if (dist < nbh.top().second ||
488 (dist < std::numeric_limits<double>::epsilon() && data==key))
491 nbh.push(std::make_pair(&data, dist));
503 NearQueue& nbh, NodeQueue& nodeQueue,
bool& isPivot)
const
505 for (
unsigned int i=0; i<
data_.size(); ++i)
515 std::vector<double> distToPivot(
children_.size());
516 std::vector<int> permutation(
children_.size());
518 for (
unsigned int i=0; i<permutation.size(); ++i)
520 std::random_shuffle(permutation.begin(), permutation.end());
522 for (
unsigned int i=0; i<
children_.size(); ++i)
523 if (permutation[i] >= 0)
531 dist = nbh.top().second;
532 for (
unsigned int j=0; j<
children_.size(); ++j)
533 if (permutation[j] >=0 && i != j &&
534 (distToPivot[permutation[i]] - dist > child->
maxRange_[permutation[j]] ||
535 distToPivot[permutation[i]] + dist < child->
minRange_[permutation[j]]))
540 dist = nbh.top().second;
541 for (
unsigned int i=0; i<
children_.size(); ++i)
542 if (permutation[i] >= 0)
546 (distToPivot[permutation[i]] - dist <= child->
maxRadius_ &&
547 distToPivot[permutation[i]] + dist >= child->
minRadius_))
548 nodeQueue.push(std::make_pair(child, distToPivot[permutation[i]]));
556 nbh.push(std::make_pair(&data, dist));
561 void nearestR(
const GNAT& gnat,
const _T &data,
double r, NearQueue& nbh, NodeQueue& nodeQueue)
const
565 for (
unsigned int i=0; i<
data_.size(); ++i)
571 std::vector<double> distToPivot(
children_.size());
572 std::vector<int> permutation(
children_.size());
574 for (
unsigned int i=0; i<permutation.size(); ++i)
576 std::random_shuffle(permutation.begin(), permutation.end());
578 for (
unsigned int i=0; i<
children_.size(); ++i)
579 if (permutation[i] >= 0)
584 for (
unsigned int j=0; j<
children_.size(); ++j)
585 if (permutation[j] >=0 && i != j &&
586 (distToPivot[i] - dist > child->
maxRange_[permutation[j]] ||
587 distToPivot[i] + dist < child->
minRange_[permutation[j]]))
591 for (
unsigned int i=0; i<
children_.size(); ++i)
592 if (permutation[i] >= 0)
595 if (distToPivot[i] - dist <= child->
maxRadius_ &&
597 nodeQueue.push(std::make_pair(child, distToPivot[i]));
602 void list(
const GNAT& gnat, std::vector<_T> &data)
const
606 for (
unsigned int i=0; i<
data_.size(); ++i)
608 data.push_back(
data_[i]);
609 for (
unsigned int i=0; i<
children_.size(); ++i)
613 friend std::ostream& operator<<(std::ostream& out,
const Node& node)
615 out <<
"\ndegree:\t" << node.degree_;
616 out <<
"\nminRadius:\t" << node.minRadius_;
617 out <<
"\nmaxRadius:\t" << node.maxRadius_;
618 out <<
"\nminRange:\t";
619 for (
unsigned int i=0; i<node.minRange_.size(); ++i)
620 out << node.minRange_[i] <<
'\t';
621 out <<
"\nmaxRange: ";
622 for (
unsigned int i=0; i<node.maxRange_.size(); ++i)
623 out << node.maxRange_[i] <<
'\t';
624 out <<
"\npivot:\t" << node.pivot_;
626 for (
unsigned int i=0; i<node.data_.size(); ++i)
627 out << node.data_[i] <<
'\t';
628 out <<
"\nthis:\t" << &node;
629 out <<
"\nchildren:\n";
630 for (
unsigned int i=0; i<node.children_.size(); ++i)
631 out << node.children_[i] <<
'\t';
633 for (
unsigned int i=0; i<node.children_.size(); ++i)
634 out << *node.children_[i] <<
'\n';