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)
141 if (
rebuildSize_ != std::numeric_limits<std::size_t>::max())
145 virtual void add(
const _T &data)
159 virtual void add(
const std::vector<_T> &data)
163 else if (data.size()>0)
167 tree_->subtreeSize_= data.size();
169 for (
unsigned int i=1; i<data.size(); ++i)
174 size_ += data.size();
189 virtual bool remove(
const _T &data)
191 if (!
size_)
return false;
195 if (*nbhQueue.top().first != data)
197 removed_.insert(nbhQueue.top().first);
212 if (!nbh.empty())
return nbh[0];
214 throw Exception(
"No elements found in nearest neighbors data structure");
218 virtual void nearestK(
const _T &data, std::size_t k, std::vector<_T> &nbh)
const
231 virtual void nearestR(
const _T &data,
double radius, std::vector<_T> &nbh)
const
242 virtual std::size_t
size(
void)
const
248 const _T& sample(
RNG& rng)
const
252 throw Exception(
"Cannot sample from an empty tree");
254 return tree_->sample(*
this, rng);
258 virtual void list(std::vector<_T> &data)
const
261 data.reserve(
size());
263 tree_->list(*
this, data);
267 friend std::ostream& operator<<(std::ostream& out, const NearestNeighborsGNAT<_T>& gnat)
272 if (!gnat.removed_.empty())
274 out <<
"Elements marked for removal:\n";
275 for (
typename boost::unordered_set<const _T*>::const_iterator it = gnat.removed_.begin();
276 it != gnat.removed_.end(); it++)
285 void integrityCheck()
288 boost::unordered_set<const _T*> tmp;
293 for (
typename boost::unordered_set<const _T*>::iterator it=tmp.begin(); it!=tmp.end(); it++)
296 for (i=0; i<lst.size(); ++i)
302 std::cout <<
"***** FAIL!! ******\n" << *
this <<
'\n';
303 for (
unsigned int j=0; j<lst.size(); ++j) std::cout<<lst[j]<<
'\t';
304 std::cout<<std::endl;
306 assert(i != lst.size());
312 if (lst.size() !=
size_)
313 std::cout <<
"#########################################\n" << *
this << std::endl;
314 assert(lst.size() ==
size_);
317 typedef NearestNeighborsGNAT<_T> GNAT;
338 tree_->
nearestK(*
this, data, k, nbhQueue, nodeQueue, isPivot);
339 while (nodeQueue.size() > 0)
341 dist = nbhQueue.top().second;
342 nodeDist = nodeQueue.top();
344 if (nbhQueue.size() == k &&
345 (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
346 nodeDist.second < nodeDist.first->minRadius_ - dist))
348 nodeDist.first->nearestK(*
this, data, k, nbhQueue, nodeQueue, isPivot);
355 double dist = radius;
362 while (nodeQueue.size() > 0)
364 nodeDist = nodeQueue.top();
366 if (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
367 nodeDist.second < nodeDist.first->minRadius_ - dist)
369 nodeDist.first->nearestR(*
this, data, radius, nbhQueue, nodeQueue);
376 typename std::vector<_T>::reverse_iterator it;
377 nbh.resize(nbhQueue.size());
378 for (it=nbh.rbegin(); it!=nbh.rend(); it++, nbhQueue.pop())
379 *it = *nbhQueue.top().first;
388 Node(
int degree,
int capacity,
const _T& pivot)
390 minRadius_(std::numeric_limits<double>::infinity()),
394 , subtreeSize_(1), activity_(0)
398 data_.reserve(capacity+1);
403 for (
unsigned int i=0; i<
children_.size(); ++i)
423 activity_ = std::max(-32, activity_ - 1);
444 data_.push_back(data);
461 std::vector<double> dist(
children_.size());
465 for (
unsigned int i=1; i<
children_.size(); ++i)
471 for (
unsigned int i=0; i<
children_.size(); ++i)
473 children_[minInd]->updateRadius(minDist);
480 unsigned int sz =
data_.size();
488 std::vector<std::vector<double> > dists;
489 std::vector<unsigned int> pivots;
493 for(
unsigned int i=0; i<pivots.size(); i++)
496 for (
unsigned int j=0; j<
data_.size(); ++j)
499 for (
unsigned int i=1; i<
degree_; ++i)
500 if (dists[j][i] < dists[j][k])
508 for (
unsigned int i=0; i<
degree_; ++i)
512 for (
unsigned int i=0; i<
degree_; ++i)
515 children_[i]->degree_ = std::min(std::max(
530 for (
unsigned int i=0; i<
degree_; ++i)
536 bool insertNeighborK(NearQueue& nbh, std::size_t k,
const _T& data,
const _T& key,
double dist)
const
540 nbh.push(std::make_pair(&data, dist));
543 else if (dist < nbh.top().second ||
544 (dist < std::numeric_limits<double>::epsilon() && data==key))
547 nbh.push(std::make_pair(&data, dist));
559 NearQueue& nbh, NodeQueue& nodeQueue,
bool& isPivot)
const
561 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)
587 dist = nbh.top().second;
588 for (
unsigned int j=0; j<
children_.size(); ++j)
589 if (permutation[j] >=0 && i != j &&
590 (distToPivot[permutation[i]] - dist > child->
maxRange_[permutation[j]] ||
591 distToPivot[permutation[i]] + dist < child->
minRange_[permutation[j]]))
596 dist = nbh.top().second;
597 for (
unsigned int i=0; i<
children_.size(); ++i)
598 if (permutation[i] >= 0)
602 (distToPivot[permutation[i]] - dist <= child->
maxRadius_ &&
603 distToPivot[permutation[i]] + dist >= child->
minRadius_))
604 nodeQueue.push(std::make_pair(child, distToPivot[permutation[i]]));
612 nbh.push(std::make_pair(&data, dist));
617 void nearestR(
const GNAT& gnat,
const _T &data,
double r, NearQueue& nbh, NodeQueue& nodeQueue)
const
621 for (
unsigned int i=0; i<
data_.size(); ++i)
627 std::vector<double> distToPivot(
children_.size());
628 std::vector<int> permutation(
children_.size());
630 for (
unsigned int i=0; i<permutation.size(); ++i)
632 std::random_shuffle(permutation.begin(), permutation.end());
634 for (
unsigned int i=0; i<
children_.size(); ++i)
635 if (permutation[i] >= 0)
640 for (
unsigned int j=0; j<
children_.size(); ++j)
641 if (permutation[j] >=0 && i != j &&
642 (distToPivot[i] - dist > child->
maxRange_[permutation[j]] ||
643 distToPivot[i] + dist < child->
minRange_[permutation[j]]))
647 for (
unsigned int i=0; i<
children_.size(); ++i)
648 if (permutation[i] >= 0)
651 if (distToPivot[i] - dist <= child->
maxRadius_ &&
653 nodeQueue.push(std::make_pair(child, distToPivot[i]));
659 double getSamplingWeight(
const GNAT& gnat)
const
661 double minR = std::numeric_limits<double>::max();
662 for(
size_t i = 0; i<
minRange_.size(); i++)
666 return std::pow(minR, gnat.estimatedDimension_) / (double) subtreeSize_;
668 const _T& sample(
const GNAT& gnat,
RNG& rng)
const
672 if (rng.
uniform01() < 1./(double) subtreeSize_)
675 for(
unsigned int i = 0; i <
children_.size(); ++i)
687 void list(
const GNAT& gnat, std::vector<_T> &data)
const
689 if (!gnat.isRemoved(
pivot_))
691 for (
unsigned int i=0; i<
data_.size(); ++i)
692 if(!gnat.isRemoved(
data_[i]))
693 data.push_back(
data_[i]);
694 for (
unsigned int i=0; i<
children_.size(); ++i)
698 friend std::ostream& operator<<(std::ostream& out,
const Node& node)
700 out <<
"\ndegree:\t" << node.degree_;
701 out <<
"\nminRadius:\t" << node.minRadius_;
702 out <<
"\nmaxRadius:\t" << node.maxRadius_;
703 out <<
"\nminRange:\t";
704 for (
unsigned int i=0; i<node.minRange_.size(); ++i)
705 out << node.minRange_[i] <<
'\t';
706 out <<
"\nmaxRange: ";
707 for (
unsigned int i=0; i<node.maxRange_.size(); ++i)
708 out << node.maxRange_[i] <<
'\t';
709 out <<
"\npivot:\t" << node.pivot_;
711 for (
unsigned int i=0; i<node.data_.size(); ++i)
712 out << node.data_[i] <<
'\t';
713 out <<
"\nthis:\t" << &node;
715 out <<
"\nsubtree size:\t" << node.subtreeSize_;
716 out <<
"\nactivity:\t" << node.activity_;
718 out <<
"\nchildren:\n";
719 for (
unsigned int i=0; i<node.children_.size(); ++i)
720 out << node.children_[i] <<
'\t';
722 for (
unsigned int i=0; i<node.children_.size(); ++i)
723 out << *node.children_[i] <<
'\n';
748 unsigned int subtreeSize_;
789 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 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...
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.
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.
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.
unsigned int minDegree_
After splitting a Node, each child Node has degree equal to the default degree times the fraction of ...
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.
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.
Abstract representation of a container that can perform nearest neighbors queries.
virtual void clear(void)
Clear the datastructure.
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.
virtual std::size_t size(void) const
Get the number of elements in the datastructure.
boost::unordered_set< const _T * > removed_
Cache of removed elements.
double uniform01(void)
Generate a random real between 0 and 1.
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...
Node * tree_
The data structure containing the elements stored in this structure.
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.