39 #ifndef MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_NEAREST_NEIGHBORS_GNAT_ 40 #define MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_NEAREST_NEIGHBORS_GNAT_ 45 #include <unordered_set> 68 template <
typename _T>
75 using DataDist = std::pair<const _T*, double>;
76 struct DataDistCompare
78 bool operator()(
const DataDist& d0,
const DataDist& d1)
80 return d0.second < d1.second;
83 using NearQueue = std::priority_queue<DataDist, std::vector<DataDist>, DataDistCompare>;
88 using NodeDist = std::pair<Node*, double>;
89 struct NodeDistCompare
91 bool operator()(
const NodeDist& n0,
const NodeDist& n1)
const 93 return (n0.second - n0.first->maxRadius_) > (n1.second - n1.first->maxRadius_);
96 using NodeQueue = std::priority_queue<NodeDist, std::vector<NodeDist>, NodeDistCompare>;
101 unsigned int maxNumPtsPerLeaf = 50,
unsigned int removedCacheSize = 500,
102 bool rebalancing =
false)
136 if (
rebuildSize_ != std::numeric_limits<std::size_t>::max())
145 void add(
const _T& data)
override 159 void add(
const std::vector<_T>& data)
override 163 else if (!data.empty())
166 for (
unsigned int i = 1; i < data.size(); ++i)
168 size_ += data.size();
186 bool remove(
const _T& data)
override 193 const _T*
d = nbhQueue.top().first;
211 if (!nbhQueue.empty())
212 return *nbhQueue.top().first;
214 throw moveit::Exception(
"No elements found in nearest neighbors data structure");
218 void nearestK(
const _T& data, std::size_t k, std::vector<_T>& nbh)
const override 232 void nearestR(
const _T& data,
double radius, std::vector<_T>& nbh)
const override 243 std::size_t
size()
const override 248 void list(std::vector<_T>& data)
const override 251 data.reserve(
size());
257 friend std::ostream& operator<<(std::ostream& out, const NearestNeighborsGNAT<_T>& gnat)
262 if (!gnat.removed_.empty())
264 out <<
"Elements marked for removal:\n";
265 for (
typename std::unordered_set<const _T*>::const_iterator it = gnat.removed_.begin();
266 it != gnat.removed_.end(); it++)
278 std::unordered_set<const _T*> tmp;
283 for (
typename std::unordered_set<const _T*>::iterator it = tmp.begin(); it != tmp.end(); it++)
286 for (i = 0; i < lst.size(); ++i)
292 std::cout <<
"***** FAIL!! ******\n" << *
this <<
'\n';
293 for (
unsigned int j = 0; j < lst.size(); ++j)
294 std::cout << lst[j] <<
'\t';
295 std::cout << std::endl;
297 assert(i != lst.size());
303 if (lst.size() !=
size_)
304 std::cout <<
"#########################################\n" << *
this << std::endl;
305 assert(lst.size() ==
size_);
330 tree_->
nearestK(*
this, data, k, nbhQueue, nodeQueue, isPivot);
331 while (!nodeQueue.empty())
333 dist = nbhQueue.top().second;
334 nodeDist = nodeQueue.top();
336 if (nbhQueue.size() == k &&
337 (nodeDist.second > nodeDist.first->maxRadius_ + dist || nodeDist.second < nodeDist.first->minRadius_ - dist))
339 nodeDist.first->nearestK(*
this, data, k, nbhQueue, nodeQueue, isPivot);
346 double dist = radius;
352 while (!nodeQueue.empty())
354 nodeDist = nodeQueue.top();
356 if (nodeDist.second > nodeDist.first->maxRadius_ + dist || nodeDist.second < nodeDist.first->minRadius_ - dist)
358 nodeDist.first->nearestR(*
this, data, radius, nbhQueue, nodeQueue);
365 typename std::vector<_T>::reverse_iterator it;
366 nbh.resize(nbhQueue.size());
367 for (it = nbh.rbegin(); it != nbh.rend(); it++, nbhQueue.pop())
368 *it = *nbhQueue.top().first;
377 Node(
int degree,
int capacity, _T pivot)
379 , pivot_(
std::move(pivot))
380 , minRadius_(
std::numeric_limits<double>::infinity())
381 , maxRadius_(-minRadius_)
382 , minRange_(degree, minRadius_)
383 , maxRange_(degree, maxRadius_)
386 data_.reserve(capacity + 1);
391 for (
unsigned int i = 0; i < children_.size(); ++i)
399 if (minRadius_ > dist)
402 if (maxRadius_ < dist)
405 if (maxRadius_ < dist)
411 activity_ = std::max(-32, activity_ - 1);
419 if (minRange_[i] > dist)
421 if (maxRange_[i] < dist)
427 if (children_.empty())
429 data_.push_back(data);
431 if (needToSplit(gnat))
446 std::vector<double> dist(children_.size());
447 double minDist = dist[0] = gnat.
distFun_(data, children_[0]->pivot_);
450 for (
unsigned int i = 1; i < children_.size(); ++i)
451 if ((dist[i] = gnat.
distFun_(data, children_[i]->pivot_)) < minDist)
456 for (
unsigned int i = 0; i < children_.size(); ++i)
457 children_[i]->updateRange(minInd, dist[i]);
458 children_[minInd]->updateRadius(minDist);
459 children_[minInd]->add(gnat, data);
465 unsigned int sz = data_.size();
474 std::vector<unsigned int> pivots;
478 for (
unsigned int& pivot : pivots)
481 for (
unsigned int j = 0; j < data_.size(); ++j)
484 for (
unsigned int i = 1; i <
degree_; ++i)
485 if (dists(j, i) < dists(j, k))
487 Node* child = children_[k];
490 child->
data_.push_back(data_[j]);
493 for (
unsigned int i = 0; i <
degree_; ++i)
494 children_[i]->updateRange(k, dists(j, i));
497 for (
unsigned int i = 0; i <
degree_; ++i)
500 children_[i]->degree_ =
501 std::min(std::max((
unsigned int)((degree_ * children_[i]->data_.size()) / data_.size()), gnat.
minDegree_),
504 if (children_[i]->minRadius_ >= std::numeric_limits<double>::infinity())
505 children_[i]->minRadius_ = children_[i]->maxRadius_ = 0.;
511 for (
unsigned int i = 0; i <
degree_; ++i)
512 if (children_[i]->needToSplit(gnat))
513 children_[i]->split(gnat);
517 bool insertNeighborK(NearQueue& nbh, std::size_t k,
const _T& data,
const _T& key,
double dist)
const 521 nbh.push(std::make_pair(&data, dist));
524 if (dist < nbh.top().second || (dist < std::numeric_limits<double>::epsilon() && data == key))
527 nbh.push(std::make_pair(&data, dist));
538 void nearestK(
const GNAT& gnat,
const _T& data, std::size_t k, NearQueue& nbh, NodeQueue& nodeQueue,
541 for (
unsigned int i = 0; i < data_.size(); ++i)
544 if (insertNeighborK(nbh, k, data_[i], data, gnat.
distFun_(data, data_[i])))
547 if (!children_.empty())
551 std::vector<double> distToPivot(children_.size());
552 std::vector<int> permutation(children_.size());
553 for (
unsigned int i = 0; i < permutation.size(); ++i)
555 std::random_shuffle(permutation.begin(), permutation.end());
557 for (
unsigned int i = 0; i < children_.size(); ++i)
558 if (permutation[i] >= 0)
560 child = children_[permutation[i]];
562 if (insertNeighborK(nbh, k, child->
pivot_, data, distToPivot[permutation[i]]))
566 dist = nbh.top().second;
567 for (
unsigned int j = 0; j < children_.size(); ++j)
568 if (permutation[j] >= 0 && i != j &&
569 (distToPivot[permutation[i]] - dist > child->
maxRange_[permutation[j]] ||
570 distToPivot[permutation[i]] + dist < child->minRange_[permutation[j]]))
575 dist = nbh.top().second;
576 for (
unsigned int i = 0; i < children_.size(); ++i)
577 if (permutation[i] >= 0)
579 child = children_[permutation[i]];
580 if (nbh.size() < k || (distToPivot[permutation[i]] - dist <= child->
maxRadius_ &&
581 distToPivot[permutation[i]] + dist >= child->
minRadius_))
582 nodeQueue.push(std::make_pair(child, distToPivot[permutation[i]]));
590 nbh.push(std::make_pair(&data, dist));
595 void nearestR(
const GNAT& gnat,
const _T& data,
double r, NearQueue& nbh, NodeQueue& nodeQueue)
const 599 for (
unsigned int i = 0; i < data_.size(); ++i)
601 insertNeighborR(nbh, r, data_[i], gnat.
distFun_(data, data_[i]));
602 if (!children_.empty())
605 std::vector<double> distToPivot(children_.size());
606 std::vector<int> permutation(children_.size());
607 for (
unsigned int i = 0; i < permutation.size(); ++i)
609 std::random_shuffle(permutation.begin(), permutation.end());
611 for (
unsigned int i = 0; i < children_.size(); ++i)
612 if (permutation[i] >= 0)
614 child = children_[permutation[i]];
616 insertNeighborR(nbh, r, child->
pivot_, distToPivot[i]);
617 for (
unsigned int j = 0; j < children_.size(); ++j)
618 if (permutation[j] >= 0 && i != j && (distToPivot[i] - dist > child->
maxRange_[permutation[j]] ||
619 distToPivot[i] + dist < child->minRange_[permutation[j]]))
623 for (
unsigned int i = 0; i < children_.size(); ++i)
624 if (permutation[i] >= 0)
626 child = children_[permutation[i]];
627 if (distToPivot[i] - dist <= child->maxRadius_ && distToPivot[i] + dist >= child->
minRadius_)
628 nodeQueue.push(std::make_pair(child, distToPivot[i]));
633 void list(
const GNAT& gnat, std::vector<_T>& data)
const 636 data.push_back(pivot_);
637 for (
unsigned int i = 0; i < data_.size(); ++i)
639 data.push_back(data_[i]);
640 for (
unsigned int i = 0; i < children_.size(); ++i)
641 children_[i]->
list(gnat, data);
646 out <<
"\ndegree:\t" << node.
degree_;
649 out <<
"\nminRange:\t";
650 for (
unsigned int i = 0; i < node.
minRange_.size(); ++i)
652 out <<
"\nmaxRange: ";
653 for (
unsigned int i = 0; i < node.
maxRange_.size(); ++i)
655 out <<
"\npivot:\t" << node.
pivot_;
657 for (
unsigned int i = 0; i < node.
data_.size(); ++i)
658 out << node.
data_[i] <<
'\t';
659 out <<
"\nthis:\t" << &node;
660 out <<
"\nchildren:\n";
661 for (
unsigned int i = 0; i < node.
children_.size(); ++i)
664 for (
unsigned int i = 0; i < node.
children_.size(); ++i)
void clear() override
Clear the datastructure.
_T nearest(const _T &data) const override
Get the nearest neighbor of a point.
void add(const _T &data) override
Add an element to the datastructure.
void updateRadius(double dist)
void list(std::vector< _T > &data) const override
Get all the elements in the datastructure.
GreedyKCenters< _T > pivotSelector_
friend std::ostream & operator<<(std::ostream &out, const Node &node)
bool needToSplit(const GNAT &gnat) const
std::vector< Node * > children_
void insertNeighborR(NearQueue &nbh, double r, const _T &data, double dist) const
NearestNeighborsGNAT(unsigned int degree=8, unsigned int minDegree=4, unsigned int maxDegree=12, unsigned int maxNumPtsPerLeaf=50, unsigned int removedCacheSize=500, bool rebalancing=false)
std::size_t size() const override
Get the number of elements in the datastructure.
bool reportsSortedResults() const override
Return true if the solutions reported by this data structure are sorted, when calling nearestK / near...
An instance of this class can be used to greedily select a given number of representatives from a set...
std::vector< double > minRange_
void add(GNAT &gnat, const _T &data)
void nearestR(const _T &data, double radius, std::vector< _T > &nbh) const override
Get the nearest neighbors of a point, within a specified radius.
~NearestNeighborsGNAT() override
void list(const GNAT &gnat, std::vector< _T > &data) const
unsigned int maxNumPtsPerLeaf_
virtual void add(const _T &data)=0
Add an element to the datastructure.
void nearestR(const GNAT &gnat, const _T &data, double r, NearQueue &nbh, NodeQueue &nodeQueue) const
bool insertNeighborK(NearQueue &nbh, std::size_t k, const _T &data, const _T &key, double dist) const
virtual void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.
void nearestK(const _T &data, std::size_t k, std::vector< _T > &nbh) const override
Get the k-nearest neighbors of a point.
bool isRemoved(const _T &data) const
Geometric Near-neighbor Access Tree (GNAT), a data structure for nearest neighbor search...
double min(double a, double b)
boost::numeric::ublas::matrix< double > Matrix
A matrix type for storing distances between points and centers.
Node(int degree, int capacity, _T pivot)
std::size_t removedCacheSize_
void rebuildDataStructure()
std::vector< double > maxRange_
void updateRange(unsigned int i, double dist)
bool nearestKInternal(const _T &data, std::size_t k, NearQueue &nbhQueue) const
Abstract representation of a container that can perform nearest neighbors queries.
void postprocessNearest(NearQueue &nbhQueue, std::vector< _T > &nbh) const
DistanceFunction distFun_
The used distance function.
std::unordered_set< const _T * > removed_
void add(const std::vector< _T > &data) override
Add a vector of points.
void nearestRInternal(const _T &data, double radius, NearQueue &nbhQueue) const
double max(double a, double b)
void setDistanceFunction(const typename NearestNeighbors< _T >::DistanceFunction &distFun) override
void nearestK(const GNAT &gnat, const _T &data, std::size_t k, NearQueue &nbh, NodeQueue &nodeQueue, bool &isPivot) const
std::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.