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" 43 #include "ompl/datastructures/PDF.h" 45 #include "ompl/util/Exception.h" 46 #include <boost/unordered_set.hpp> 76 typedef std::pair<const _T*,double> DataDist;
77 struct DataDistCompare
79 bool operator()(
const DataDist& d0,
const DataDist& d1)
81 return d0.second < d1.second;
84 typedef std::priority_queue<DataDist, std::vector<DataDist>, DataDistCompare> NearQueue;
89 typedef std::pair<Node*,double> NodeDist;
90 struct NodeDistCompare
92 bool operator()(
const NodeDist& n0,
const NodeDist& n1)
const 94 return (n0.second - n0.first->maxRadius_) > (n1.second - n1.first->maxRadius_);
97 typedef std::priority_queue<NodeDist, std::vector<NodeDist>, NodeDistCompare> NodeQueue;
102 unsigned int maxDegree = 6,
unsigned int maxNumPtsPerLeaf = 50,
103 unsigned int removedCacheSize = 50,
bool rebalancing =
false 105 ,
double estimatedDimension = 6.0
111 rebuildSize_(rebalancing ? maxNumPtsPerLeaf*degree : std::numeric_limits<std::size_t>::max()),
114 , estimatedDimension_(estimatedDimension)
142 if (
rebuildSize_ != std::numeric_limits<std::size_t>::max())
151 virtual void add(
const _T &data)
165 virtual void add(
const std::vector<_T> &data)
169 else if (data.size()>0)
173 tree_->subtreeSize_= data.size();
175 for (
unsigned int i=1; i<data.size(); ++i)
180 size_ += data.size();
195 virtual bool remove(
const _T &data)
197 if (!
size_)
return false;
201 if (*nbhQueue.top().first != data)
203 removed_.insert(nbhQueue.top().first);
218 if (!nbh.empty())
return nbh[0];
220 throw Exception(
"No elements found in nearest neighbors data structure");
224 virtual void nearestK(
const _T &data, std::size_t k, std::vector<_T> &nbh)
const 237 virtual void nearestR(
const _T &data,
double radius, std::vector<_T> &nbh)
const 248 virtual std::size_t
size()
const 254 const _T& sample(
RNG &rng)
const 258 throw Exception(
"Cannot sample from an empty tree");
260 return tree_->sample(*
this, rng);
264 virtual void list(std::vector<_T> &data)
const 267 data.reserve(
size());
269 tree_->list(*
this, data);
273 friend std::ostream& operator<<(std::ostream& out, const NearestNeighborsGNAT<_T>& gnat)
278 if (!gnat.removed_.empty())
280 out <<
"Elements marked for removal:\n";
281 for (
typename boost::unordered_set<const _T*>::const_iterator it = gnat.removed_.begin();
282 it != gnat.removed_.end(); it++)
291 void integrityCheck()
294 boost::unordered_set<const _T*> tmp;
299 for (
typename boost::unordered_set<const _T*>::iterator it=tmp.begin(); it!=tmp.end(); it++)
302 for (i=0; i<lst.size(); ++i)
308 std::cout <<
"***** FAIL!! ******\n" << *
this <<
'\n';
309 for (
unsigned int j=0; j<lst.size(); ++j) std::cout<<lst[j]<<
'\t';
310 std::cout<<std::endl;
312 assert(i != lst.size());
318 if (lst.size() !=
size_)
319 std::cout <<
"#########################################\n" << *
this << std::endl;
320 assert(lst.size() ==
size_);
344 tree_->
nearestK(*
this, data, k, nbhQueue, nodeQueue, isPivot);
345 while (nodeQueue.size() > 0)
347 dist = nbhQueue.top().second;
348 nodeDist = nodeQueue.top();
350 if (nbhQueue.size() == k &&
351 (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
352 nodeDist.second < nodeDist.first->minRadius_ - dist))
354 nodeDist.first->nearestK(*
this, data, k, nbhQueue, nodeQueue, isPivot);
361 double dist = radius;
368 while (nodeQueue.size() > 0)
370 nodeDist = nodeQueue.top();
372 if (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
373 nodeDist.second < nodeDist.first->minRadius_ - dist)
375 nodeDist.first->nearestR(*
this, data, radius, nbhQueue, nodeQueue);
382 typename std::vector<_T>::reverse_iterator it;
383 nbh.resize(nbhQueue.size());
384 for (it=nbh.rbegin(); it!=nbh.rend(); it++, nbhQueue.pop())
385 *it = *nbhQueue.top().first;
394 Node(
int degree,
int capacity,
const _T& pivot)
395 :
degree_(degree), pivot_(pivot),
396 minRadius_(
std::numeric_limits<double>::infinity()),
397 maxRadius_(-minRadius_), minRange_(degree, minRadius_),
398 maxRange_(degree, maxRadius_)
400 , subtreeSize_(1), activity_(0)
404 data_.reserve(capacity+1);
409 for (
unsigned int i=0; i<children_.size(); ++i)
417 if (minRadius_ > dist)
420 if (maxRadius_ < dist)
423 if (maxRadius_ < dist)
429 activity_ = std::max(-32, activity_ - 1);
437 if (minRange_[i] > dist)
439 if (maxRange_[i] < dist)
443 void add(GNAT& gnat,
const _T& data)
448 if (children_.size()==0)
450 data_.push_back(data);
452 if (needToSplit(gnat))
467 std::vector<double> dist(children_.size());
468 double minDist = dist[0] = gnat.
distFun_(data, children_[0]->pivot_);
471 for (
unsigned int i=1; i<children_.size(); ++i)
472 if ((dist[i] = gnat.
distFun_(data, children_[i]->pivot_)) < minDist)
477 for (
unsigned int i=0; i<children_.size(); ++i)
478 children_[i]->updateRange(minInd, dist[i]);
479 children_[minInd]->updateRadius(minDist);
480 children_[minInd]->add(gnat, data);
486 unsigned int sz = data_.size();
494 std::vector<std::vector<double> > dists;
495 std::vector<unsigned int> pivots;
499 for(
unsigned int i=0; i<pivots.size(); i++)
502 for (
unsigned int j=0; j<data_.size(); ++j)
505 for (
unsigned int i=1; i<
degree_; ++i)
506 if (dists[j][i] < dists[j][k])
508 Node *child = children_[k];
511 child->
data_.push_back(data_[j]);
514 for (
unsigned int i=0; i<
degree_; ++i)
515 children_[i]->updateRange(k, dists[j][i]);
518 for (
unsigned int i=0; i<
degree_; ++i)
521 children_[i]->degree_ = std::min(std::max(
522 degree_ * (
unsigned int)(children_[i]->data_.size() / data_.size()),
525 if (children_[i]->minRadius_ >= std::numeric_limits<double>::infinity())
526 children_[i]->minRadius_ = children_[i]->maxRadius_ = 0.;
529 children_[i]->subtreeSize_ = children_[i]->data_.size() + 1;
536 for (
unsigned int i=0; i<
degree_; ++i)
537 if (children_[i]->needToSplit(gnat))
538 children_[i]->split(gnat);
542 bool insertNeighborK(NearQueue& nbh, std::size_t k,
const _T& data,
const _T& key,
double dist)
const 546 nbh.push(std::make_pair(&data, dist));
549 else if (dist < nbh.top().second ||
550 (dist < std::numeric_limits<double>::epsilon() && data==key))
553 nbh.push(std::make_pair(&data, dist));
564 void nearestK(
const GNAT& gnat,
const _T &data, std::size_t k,
565 NearQueue& nbh, NodeQueue& nodeQueue,
bool &isPivot)
const 567 for (
unsigned int i=0; i<data_.size(); ++i)
570 if (insertNeighborK(nbh, k, data_[i], data, gnat.
distFun_(data, data_[i])))
573 if (children_.size() > 0)
577 std::vector<double> distToPivot(children_.size());
578 std::vector<int> permutation(children_.size());
580 for (
unsigned int i=0; i<permutation.size(); ++i)
582 std::random_shuffle(permutation.begin(), permutation.end());
584 for (
unsigned int i=0; i<children_.size(); ++i)
585 if (permutation[i] >= 0)
587 child = children_[permutation[i]];
589 if (insertNeighborK(nbh, k, child->
pivot_, data, distToPivot[permutation[i]]))
593 dist = nbh.top().second;
594 for (
unsigned int j=0; j<children_.size(); ++j)
595 if (permutation[j] >=0 && i != j &&
596 (distToPivot[permutation[i]] - dist > child->
maxRange_[permutation[j]] ||
597 distToPivot[permutation[i]] + dist < child->minRange_[permutation[j]]))
602 dist = nbh.top().second;
603 for (
unsigned int i=0; i<children_.size(); ++i)
604 if (permutation[i] >= 0)
606 child = children_[permutation[i]];
608 (distToPivot[permutation[i]] - dist <= child->
maxRadius_ &&
609 distToPivot[permutation[i]] + dist >= child->
minRadius_))
610 nodeQueue.push(std::make_pair(child, distToPivot[permutation[i]]));
618 nbh.push(std::make_pair(&data, dist));
623 void nearestR(
const GNAT& gnat,
const _T &data,
double r, NearQueue& nbh, NodeQueue& nodeQueue)
const 627 for (
unsigned int i=0; i<data_.size(); ++i)
629 insertNeighborR(nbh, r, data_[i], gnat.
distFun_(data, data_[i]));
630 if (children_.size() > 0)
633 std::vector<double> distToPivot(children_.size());
634 std::vector<int> permutation(children_.size());
636 for (
unsigned int i=0; i<permutation.size(); ++i)
638 std::random_shuffle(permutation.begin(), permutation.end());
640 for (
unsigned int i=0; i<children_.size(); ++i)
641 if (permutation[i] >= 0)
643 child = children_[permutation[i]];
645 insertNeighborR(nbh, r, child->
pivot_, distToPivot[i]);
646 for (
unsigned int j=0; j<children_.size(); ++j)
647 if (permutation[j] >=0 && i != j &&
648 (distToPivot[i] - dist > child->
maxRange_[permutation[j]] ||
649 distToPivot[i] + dist < child->minRange_[permutation[j]]))
653 for (
unsigned int i=0; i<children_.size(); ++i)
654 if (permutation[i] >= 0)
656 child = children_[permutation[i]];
657 if (distToPivot[i] - dist <= child->maxRadius_ &&
659 nodeQueue.push(std::make_pair(child, distToPivot[i]));
665 double getSamplingWeight(
const GNAT& gnat)
const 667 double minR = std::numeric_limits<double>::max();
668 for(
size_t i = 0; i<minRange_.size(); i++)
669 if(minRange_[i] < minR && minRange_[i] > 0.0)
671 minR = std::max(minR, maxRadius_);
672 return std::pow(minR, gnat.estimatedDimension_) / (double) subtreeSize_;
674 const _T& sample(
const GNAT& gnat,
RNG &rng)
const 676 if (children_.size() != 0)
678 if (rng.
uniform01() < 1./(double) subtreeSize_)
681 for(
unsigned int i = 0; i < children_.size(); ++i)
682 distribution.
add(children_[i], children_[i]->getSamplingWeight(gnat));
687 unsigned int i = rng.
uniformInt(0, data_.size());
688 return (i==data_.size()) ? pivot_ : data_[i];
693 void list(
const GNAT& gnat, std::vector<_T> &data)
const 696 data.push_back(pivot_);
697 for (
unsigned int i=0; i<data_.size(); ++i)
699 data.push_back(data_[i]);
700 for (
unsigned int i=0; i<children_.size(); ++i)
701 children_[i]->
list(gnat, data);
704 friend std::ostream&
operator<<(std::ostream& out,
const Node &node)
706 out <<
"\ndegree:\t" << node.
degree_;
709 out <<
"\nminRange:\t";
710 for (
unsigned int i=0; i<node.
minRange_.size(); ++i)
712 out <<
"\nmaxRange: ";
713 for (
unsigned int i=0; i<node.
maxRange_.size(); ++i)
715 out <<
"\npivot:\t" << node.
pivot_;
717 for (
unsigned int i=0; i<node.
data_.size(); ++i)
718 out << node.
data_[i] <<
'\t';
719 out <<
"\nthis:\t" << &node;
721 out <<
"\nsubtree size:\t" << node.subtreeSize_;
722 out <<
"\nactivity:\t" << node.activity_;
724 out <<
"\nchildren:\n";
725 for (
unsigned int i=0; i<node.
children_.size(); ++i)
728 for (
unsigned int i=0; i<node.
children_.size(); ++i)
754 unsigned int subtreeSize_;
795 double estimatedDimension_;
std::vector< double > maxRange_
The i-th element in maxRange_ is the maximum distance between the pivot and any data_ element in the ...
std::vector< _T > data_
The data elements stored in this node (in addition to the pivot element). An internal node has no ele...
virtual void nearestK(const _T &data, std::size_t k, std::vector< _T > &nbh) const
Return the k nearest neighbors in sorted order.
std::size_t size_
Number of elements stored in the tree.
unsigned int maxNumPtsPerLeaf_
Maximum number of elements allowed to be stored in a Node before it needs to be split into several no...
void updateRadius(double dist)
Update minRadius_ and maxRadius_, given that an element was added with distance dist to the pivot...
An instance of this class can be used to greedily select a given number of representatives from a set...
boost::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
void add(GNAT &gnat, const _T &data)
Add an element to the tree rooted at this node.
void nearestK(const GNAT &gnat, const _T &data, std::size_t k, NearQueue &nbh, NodeQueue &nodeQueue, bool &isPivot) const
Compute the k nearest neighbors of data in the tree. For k=1, isPivot is true if the nearest neighbor...
const _T pivot_
Data element stored in this Node.
virtual void setDistanceFunction(const typename NearestNeighbors< _T >::DistanceFunction &distFun)
Set the distance function to use.
double minRadius_
Minimum distance between the pivot element and the elements stored in data_.
Geometric Near-neighbor Access Tree (GNAT), a data structure for nearest neighbor search...
void rebuildDataStructure()
Rebuild the internal data structure.
bool needToSplit(const GNAT &gnat) const
Return true iff the node needs to be split into child nodes.
unsigned int maxDegree_
After splitting a Node, each child Node has degree equal to the default degree times the fraction of ...
void split(GNAT &gnat)
The split operation finds pivot elements for the child nodes and moves each data element of this node...
A container that supports probabilistic sampling over weighted data.
double uniform01()
Generate a random real between 0 and 1.
unsigned int minDegree_
After splitting a Node, each child Node has degree equal to the default degree times the fraction of ...
Main namespace. Contains everything in this library.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
virtual bool reportsSortedResults() const
Return true if the solutions reported by this data structure are sorted, when calling nearestK / near...
virtual void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.
virtual void add(const _T &data)
Add an element to the datastructure.
void updateRange(unsigned int i, double dist)
Update minRange_[i] and maxRange_[i], given that an element was added to the i-th child of the parent...
GreedyKCenters< _T > pivotSelector_
The data structure used to split data into subtrees.
void insertNeighborR(NearQueue &nbh, double r, const _T &data, double dist) const
Insert data in nbh if it is a near neighbor.
DistanceFunction distFun_
The used distance function.
std::size_t rebuildSize_
If size_ exceeds rebuildSize_, the tree will be rebuilt (and automatically rebalanced), and rebuildSize_ will be doubled.
friend std::ostream & operator<<(std::ostream &out, const NearestNeighborsGNAT< _T > &gnat)
Print a GNAT structure (mostly useful for debugging purposes).
Abstract representation of a container that can perform nearest neighbors queries.
virtual std::size_t size() const
Get the number of elements in the datastructure.
void nearestR(const GNAT &gnat, const _T &data, double r, NearQueue &nbh, NodeQueue &nodeQueue) const
Return all elements that are within distance r in nbh. The nodeQueue, which contains other Nodes that...
void postprocessNearest(NearQueue &nbhQueue, std::vector< _T > &nbh) const
Convert the internal data structure used for storing neighbors to the vector that NearestNeighbor API...
The exception type for ompl.
void nearestRInternal(const _T &data, double radius, NearQueue &nbhQueue) const
Return in nbhQueue the elements that are within distance radius of data.
_T & sample(double r) const
Returns a piece of data from the PDF according to the input sampling value, which must be between 0 a...
virtual void add(const _T &data)=0
Add an element to the datastructure.
Element * add(const _T &d, const double w)
Adds a piece of data with a given weight to the PDF. Returns a corresponding Element, which can be used to subsequently update or remove the data from the PDF.
bool isRemoved(const _T &data) const
Return true iff data has been marked for removal.
std::vector< double > minRange_
The i-th element in minRange_ is the minimum distance between the pivot and any data_ element in the ...
unsigned int degree_
Number of child nodes.
bool nearestKInternal(const _T &data, std::size_t k, NearQueue &nbhQueue) const
Return in nbhQueue the k nearest neighbors of data. For k=1, return true if the nearest neighbor is a...
boost::unordered_set< const _T * > removed_
Cache of removed elements.
virtual _T nearest(const _T &data) const
Get the nearest neighbor of a point.
bool insertNeighborK(NearQueue &nbh, std::size_t k, const _T &data, const _T &key, double dist) const
Insert data in nbh if it is a near neighbor. Return true iff data was added to nbh.
Node * tree_
The data structure containing the elements stored in this structure.
virtual void clear()
Clear the datastructure.
The class used internally to define the GNAT.
std::vector< Node * > children_
The child nodes of this node. By definition, only internal nodes have child nodes.
double maxRadius_
Maximum distance between the pivot element and the elements stored in data_.
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
unsigned int degree_
The desired degree of each node.
virtual void add(const std::vector< _T > &data)
Add a vector of points.
virtual void nearestR(const _T &data, double radius, std::vector< _T > &nbh) const
Return the nearest neighbors within distance radius in sorted order.
std::size_t removedCacheSize_
Maximum number of removed elements that can be stored in the removed_ cache. If the cache is full...
virtual void list(std::vector< _T > &data) const
Get all the elements in the datastructure.
Node(int degree, int capacity, const _T &pivot)
Construct a node of given degree with at most capacity data elements and with given pivot...