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 <unordered_set> 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 = 12,
unsigned int maxNumPtsPerLeaf = 50,
103 unsigned int removedCacheSize = 500,
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)
177 size_ += data.size();
195 virtual bool remove(
const _T &data)
197 if (!
size_)
return false;
201 const _T *d = nbhQueue.top().first;
220 return *nbhQueue.top().first;
222 throw Exception(
"No elements found in nearest neighbors data structure");
226 virtual void nearestK(
const _T &data, std::size_t k, std::vector<_T> &nbh)
const 239 virtual void nearestR(
const _T &data,
double radius, std::vector<_T> &nbh)
const 250 virtual std::size_t
size()
const 256 const _T& sample(
RNG &rng)
const 260 throw Exception(
"Cannot sample from an empty tree");
262 return tree_->sample(*
this, rng);
266 virtual void list(std::vector<_T> &data)
const 269 data.reserve(
size());
271 tree_->list(*
this, data);
275 friend std::ostream& operator<<(std::ostream &out, const NearestNeighborsGNAT<_T> &gnat)
280 if (!gnat.removed_.empty())
282 out <<
"Elements marked for removal:\n";
283 for (
typename std::unordered_set<const _T*>::const_iterator it = gnat.removed_.begin();
284 it != gnat.removed_.end(); it++)
293 void integrityCheck()
296 std::unordered_set<const _T*> tmp;
301 for (
typename std::unordered_set<const _T*>::iterator it=tmp.begin(); it!=tmp.end(); it++)
304 for (i=0; i<lst.size(); ++i)
310 std::cout <<
"***** FAIL!! ******\n" << *
this <<
'\n';
311 for (
unsigned int j=0; j<lst.size(); ++j) std::cout<<lst[j]<<
'\t';
312 std::cout<<std::endl;
314 assert(i != lst.size());
320 if (lst.size() !=
size_)
321 std::cout <<
"#########################################\n" << *
this << std::endl;
322 assert(lst.size() ==
size_);
346 tree_->
nearestK(*
this, data, k, nbhQueue, nodeQueue, isPivot);
347 while (nodeQueue.size() > 0)
349 dist = nbhQueue.top().second;
350 nodeDist = nodeQueue.top();
352 if (nbhQueue.size() == k &&
353 (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
354 nodeDist.second < nodeDist.first->minRadius_ - dist))
356 nodeDist.first->nearestK(*
this, data, k, nbhQueue, nodeQueue, isPivot);
363 double dist = radius;
370 while (nodeQueue.size() > 0)
372 nodeDist = nodeQueue.top();
374 if (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
375 nodeDist.second < nodeDist.first->minRadius_ - dist)
377 nodeDist.first->nearestR(*
this, data, radius, nbhQueue, nodeQueue);
384 typename std::vector<_T>::reverse_iterator it;
385 nbh.resize(nbhQueue.size());
386 for (it=nbh.rbegin(); it!=nbh.rend(); it++, nbhQueue.pop())
387 *it = *nbhQueue.top().first;
396 Node(
int degree,
int capacity,
const _T& pivot)
397 :
degree_(degree), pivot_(pivot),
398 minRadius_(
std::numeric_limits<double>::infinity()),
399 maxRadius_(-minRadius_), minRange_(degree, minRadius_),
400 maxRange_(degree, maxRadius_)
402 , subtreeSize_(1), activity_(0)
406 data_.reserve(capacity+1);
411 for (
unsigned int i=0; i<children_.size(); ++i)
419 if (minRadius_ > dist)
422 if (maxRadius_ < dist)
425 if (maxRadius_ < dist)
431 activity_ = std::max(-32, activity_ - 1);
439 if (minRange_[i] > dist)
441 if (maxRange_[i] < dist)
445 void add(GNAT &gnat,
const _T &data)
450 if (children_.size()==0)
452 data_.push_back(data);
454 if (needToSplit(gnat))
469 std::vector<double> dist(children_.size());
470 double minDist = dist[0] = gnat.
distFun_(data, children_[0]->pivot_);
473 for (
unsigned int i=1; i<children_.size(); ++i)
474 if ((dist[i] = gnat.
distFun_(data, children_[i]->pivot_)) < minDist)
479 for (
unsigned int i=0; i<children_.size(); ++i)
480 children_[i]->updateRange(minInd, dist[i]);
481 children_[minInd]->updateRadius(minDist);
482 children_[minInd]->add(gnat, data);
488 unsigned int sz = data_.size();
497 std::vector<unsigned int> pivots;
501 for(
unsigned int i=0; i<pivots.size(); i++)
504 for (
unsigned int j=0; j<data_.size(); ++j)
507 for (
unsigned int i=1; i<
degree_; ++i)
508 if (dists(j, i) < dists(j, k))
510 Node *child = children_[k];
513 child->
data_.push_back(data_[j]);
516 for (
unsigned int i=0; i<
degree_; ++i)
517 children_[i]->updateRange(k, dists(j, i));
520 for (
unsigned int i=0; i<
degree_; ++i)
523 children_[i]->degree_ = std::min(std::max(
524 (
unsigned int) ((degree_ * children_[i]->data_.size()) / data_.size()),
527 if (children_[i]->minRadius_ >= std::numeric_limits<double>::infinity())
528 children_[i]->minRadius_ = children_[i]->maxRadius_ = 0.;
531 children_[i]->subtreeSize_ = children_[i]->data_.size() + 1;
538 for (
unsigned int i=0; i<
degree_; ++i)
539 if (children_[i]->needToSplit(gnat))
540 children_[i]->split(gnat);
544 bool insertNeighborK(NearQueue &nbh, std::size_t k,
const _T &data,
const _T &key,
double dist)
const 548 nbh.push(std::make_pair(&data, dist));
551 else if (dist < nbh.top().second ||
552 (dist < std::numeric_limits<double>::epsilon() && data==key))
555 nbh.push(std::make_pair(&data, dist));
566 void nearestK(
const GNAT &gnat,
const _T &data, std::size_t k,
567 NearQueue &nbh, NodeQueue &nodeQueue,
bool &isPivot)
const 569 for (
unsigned int i=0; i<data_.size(); ++i)
572 if (insertNeighborK(nbh, k, data_[i], data, gnat.
distFun_(data, data_[i])))
575 if (children_.size() > 0)
579 std::vector<double> distToPivot(children_.size());
580 std::vector<int> permutation(children_.size());
581 for (
unsigned int i=0; i<permutation.size(); ++i)
584 std::random_shuffle(permutation.begin(), permutation.end());
586 for (
unsigned int i=0; i<children_.size(); ++i)
587 if (permutation[i] >= 0)
589 child = children_[permutation[i]];
591 if (insertNeighborK(nbh, k, child->
pivot_, data, distToPivot[permutation[i]]))
595 dist = nbh.top().second;
596 for (
unsigned int j=0; j<children_.size(); ++j)
597 if (permutation[j] >=0 && i != j &&
598 (distToPivot[permutation[i]] - dist > child->
maxRange_[permutation[j]] ||
599 distToPivot[permutation[i]] + dist < child->minRange_[permutation[j]]))
604 dist = nbh.top().second;
605 for (
unsigned int i=0; i<children_.size(); ++i)
606 if (permutation[i] >= 0)
608 child = children_[permutation[i]];
610 (distToPivot[permutation[i]] - dist <= child->
maxRadius_ &&
611 distToPivot[permutation[i]] + dist >= child->
minRadius_))
612 nodeQueue.push(std::make_pair(child, distToPivot[permutation[i]]));
620 nbh.push(std::make_pair(&data, dist));
625 void nearestR(
const GNAT &gnat,
const _T &data,
double r, NearQueue &nbh, NodeQueue &nodeQueue)
const 629 for (
unsigned int i=0; i<data_.size(); ++i)
631 insertNeighborR(nbh, r, data_[i], gnat.
distFun_(data, data_[i]));
632 if (children_.size() > 0)
635 std::vector<double> distToPivot(children_.size());
636 std::vector<int> permutation(children_.size());
637 for (
unsigned int i=0; i<permutation.size(); ++i)
640 std::random_shuffle(permutation.begin(), permutation.end());
642 for (
unsigned int i=0; i<children_.size(); ++i)
643 if (permutation[i] >= 0)
645 child = children_[permutation[i]];
647 insertNeighborR(nbh, r, child->
pivot_, distToPivot[i]);
648 for (
unsigned int j=0; j<children_.size(); ++j)
649 if (permutation[j] >=0 && i != j &&
650 (distToPivot[i] - dist > child->
maxRange_[permutation[j]] ||
651 distToPivot[i] + dist < child->minRange_[permutation[j]]))
655 for (
unsigned int i=0; i<children_.size(); ++i)
656 if (permutation[i] >= 0)
658 child = children_[permutation[i]];
659 if (distToPivot[i] - dist <= child->maxRadius_ &&
661 nodeQueue.push(std::make_pair(child, distToPivot[i]));
667 double getSamplingWeight(
const GNAT &gnat)
const 669 double minR = std::numeric_limits<double>::max();
670 for(
size_t i = 0; i<minRange_.size(); i++)
671 if(minRange_[i] < minR && minRange_[i] > 0.0)
673 minR = std::max(minR, maxRadius_);
674 return std::pow(minR, gnat.estimatedDimension_) / (double) subtreeSize_;
676 const _T& sample(
const GNAT &gnat,
RNG &rng)
const 678 if (children_.size() != 0)
680 if (rng.
uniform01() < 1./(double) subtreeSize_)
683 for(
unsigned int i = 0; i < children_.size(); ++i)
684 distribution.
add(children_[i], children_[i]->getSamplingWeight(gnat));
689 unsigned int i = rng.
uniformInt(0, data_.size());
690 return (i==data_.size()) ? pivot_ : data_[i];
695 void list(
const GNAT &gnat, std::vector<_T> &data)
const 698 data.push_back(pivot_);
699 for (
unsigned int i=0; i<data_.size(); ++i)
701 data.push_back(data_[i]);
702 for (
unsigned int i=0; i<children_.size(); ++i)
703 children_[i]->
list(gnat, data);
706 friend std::ostream&
operator<<(std::ostream &out,
const Node &node)
708 out <<
"\ndegree:\t" << node.
degree_;
711 out <<
"\nminRange:\t";
712 for (
unsigned int i=0; i<node.
minRange_.size(); ++i)
714 out <<
"\nmaxRange: ";
715 for (
unsigned int i=0; i<node.
maxRange_.size(); ++i)
717 out <<
"\npivot:\t" << node.
pivot_;
719 for (
unsigned int i=0; i<node.
data_.size(); ++i)
720 out << node.
data_[i] <<
'\t';
721 out <<
"\nthis:\t" << &node;
723 out <<
"\nsubtree size:\t" << node.subtreeSize_;
724 out <<
"\nactivity:\t" << node.activity_;
726 out <<
"\nchildren:\n";
727 for (
unsigned int i=0; i<node.
children_.size(); ++i)
730 for (
unsigned int i=0; i<node.
children_.size(); ++i)
756 unsigned int subtreeSize_;
797 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 std::size_t size() const
Get the number of elements in the datastructure.
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 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...
void add(GNAT &gnat, const _T &data)
Add an element to the tree rooted at this node.
const _T pivot_
Data element stored in this Node.
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...
virtual void setDistanceFunction(const typename NearestNeighbors< _T >::DistanceFunction &distFun)
Set the distance function to use.
void insertNeighborR(NearQueue &nbh, double r, const _T &data, double dist) const
Insert data in nbh if it is a near neighbor.
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.
std::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
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...
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 ...
std::unordered_set< const _T * > removed_
Cache of removed elements.
Main namespace. Contains everything in this library.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
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...
virtual _T nearest(const _T &data) const
Get the nearest neighbor of a point.
virtual void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.
virtual void nearestK(const _T &data, std::size_t k, std::vector< _T > &nbh) const
Return the k nearest neighbors in sorted order.
virtual void list(std::vector< _T > &data) const
Get all the elements in the datastructure.
void nearestRInternal(const _T &data, double radius, NearQueue &nbhQueue) const
Return in nbhQueue the elements that are within distance radius of data.
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.
virtual bool reportsSortedResults() const
Return true if the solutions reported by this data structure are sorted, when calling nearestK / near...
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.
The exception type for ompl.
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.
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 isRemoved(const _T &data) const
Return true iff data has been marked for removal.
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.
void postprocessNearest(NearQueue &nbhQueue, std::vector< _T > &nbh) const
Convert the internal data structure used for storing neighbors to the vector that NearestNeighbor API...
boost::numeric::ublas::matrix< double > Matrix
A matrix type for storing distances between points and centers.
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.
_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...
std::size_t removedCacheSize_
Maximum number of removed elements that can be stored in the removed_ cache. If the cache is full...
Node(int degree, int capacity, const _T &pivot)
Construct a node of given degree with at most capacity data elements and with given pivot...
bool needToSplit(const GNAT &gnat) const
Return true iff the node needs to be split into child nodes.