NearestNeighborsGNAT.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Mark Moll, Bryant Gipson */
36 
37 #ifndef OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_GNAT_
38 #define OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_GNAT_
39 
40 #include "ompl/datastructures/NearestNeighbors.h"
41 #include "ompl/datastructures/GreedyKCenters.h"
42 #ifdef GNAT_SAMPLER
43 #include "ompl/datastructures/PDF.h"
44 #endif
45 #include "ompl/util/Exception.h"
46 #include <unordered_set>
47 #include <queue>
48 #include <algorithm>
49 #include <utility>
50 #include <iostream>
51 
52 namespace ompl
53 {
70  template <typename _T>
72  {
73  protected:
75  // internally, we use a priority queue for nearest neighbors, paired
76  // with their distance to the query point
77  using NearQueue = std::priority_queue<std::pair<double, const _T *>>;
78 
79  // another internal data structure is a priority queue of nodes to
80  // check next for possible nearest neighbors
81  class Node;
82  using NodeDist = std::pair<Node *, double>;
83  struct NodeDistCompare
84  {
85  bool operator()(const NodeDist &n0, const NodeDist &n1) const
86  {
87  return (n0.second - n0.first->maxRadius_) > (n1.second - n1.first->maxRadius_);
88  }
89  };
90  using NodeQueue = std::priority_queue<NodeDist, std::vector<NodeDist>, NodeDistCompare>;
92 
93  public:
94  NearestNeighborsGNAT(unsigned int degree = 8, unsigned int minDegree = 4, unsigned int maxDegree = 12,
95  unsigned int maxNumPtsPerLeaf = 50, unsigned int removedCacheSize = 500,
96  bool rebalancing = false
97 #ifdef GNAT_SAMPLER
98  ,
99  double estimatedDimension = 6.0
100 #endif
101  )
103  , degree_(degree)
104  , minDegree_(std::min(degree, minDegree))
105  , maxDegree_(std::max(maxDegree, degree))
106  , maxNumPtsPerLeaf_(maxNumPtsPerLeaf)
107  , rebuildSize_(rebalancing ? maxNumPtsPerLeaf * degree : std::numeric_limits<std::size_t>::max())
108  , removedCacheSize_(removedCacheSize)
109 #ifdef GNAT_SAMPLER
110  , estimatedDimension_(estimatedDimension)
111 #endif
112  {
113  }
114 
115  ~NearestNeighborsGNAT() override
116  {
117  if (tree_)
118  delete tree_;
119  }
121  void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction &distFun) override
122  {
124  pivotSelector_.setDistanceFunction(distFun);
125  if (tree_)
127  }
128 
129  void clear() override
130  {
131  if (tree_)
132  {
133  delete tree_;
134  tree_ = nullptr;
135  }
136  size_ = 0;
137  removed_.clear();
138  if (rebuildSize_ != std::numeric_limits<std::size_t>::max())
140  }
141 
142  bool reportsSortedResults() const override
143  {
144  return true;
145  }
146 
147  void add(const _T &data) override
148  {
149  if (tree_)
150  {
151  if (isRemoved(data))
153  tree_->add(*this, data);
154  }
155  else
156  {
157  tree_ = new Node(degree_, maxNumPtsPerLeaf_, data);
158  size_ = 1;
159  }
160  }
161  void add(const std::vector<_T> &data) override
162  {
163  if (tree_)
165  else if (!data.empty())
166  {
167  tree_ = new Node(degree_, maxNumPtsPerLeaf_, data[0]);
168 #ifdef GNAT_SAMPLER
169  tree_->subtreeSize_ = data.size();
170 #endif
171  tree_->data_.insert(tree_->data_.end(), data.begin() + 1, data.end());
172  size_ += data.size();
173  if (tree_->needToSplit(*this))
174  tree_->split(*this);
175  }
176  }
179  {
180  std::vector<_T> lst;
181  list(lst);
182  clear();
183  add(lst);
184  }
190  bool remove(const _T &data) override
191  {
192  if (size_ == 0u)
193  return false;
194  NearQueue nbhQueue;
195  // find data in tree
196  bool isPivot = nearestKInternal(data, 1, nbhQueue);
197  const _T *d = nbhQueue.top().second;
198  if (*d != data)
199  return false;
200  removed_.insert(d);
201  size_--;
202  // if we removed a pivot or if the capacity of removed elements
203  // has been reached, we rebuild the entire GNAT
204  if (isPivot || removed_.size() >= removedCacheSize_)
206  return true;
207  }
208 
209  _T nearest(const _T &data) const override
210  {
211  if (size_)
212  {
213  NearQueue nbhQueue;
214  nearestKInternal(data, 1, nbhQueue);
215  if (!nbhQueue.empty())
216  return *nbhQueue.top().second;
217  }
218  throw Exception("No elements found in nearest neighbors data structure");
219  }
220 
222  void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const override
223  {
224  nbh.clear();
225  if (k == 0)
226  return;
227  if (size_)
228  {
229  NearQueue nbhQueue;
230  nearestKInternal(data, k, nbhQueue);
231  postprocessNearest(nbhQueue, nbh);
232  }
233  }
234 
236  void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const override
237  {
238  nbh.clear();
239  if (size_)
240  {
241  NearQueue nbhQueue;
242  nearestRInternal(data, radius, nbhQueue);
243  postprocessNearest(nbhQueue, nbh);
244  }
245  }
246 
247  std::size_t size() const override
248  {
249  return size_;
250  }
251 
252 #ifdef GNAT_SAMPLER
253  const _T &sample(RNG &rng) const
255  {
256  if (!size())
257  throw Exception("Cannot sample from an empty tree");
258  else
259  return tree_->sample(*this, rng);
260  }
261 #endif
262 
263  void list(std::vector<_T> &data) const override
264  {
265  data.clear();
266  data.reserve(size());
267  if (tree_)
268  tree_->list(*this, data);
269  }
270 
272  friend std::ostream &operator<<(std::ostream &out, const NearestNeighborsGNAT<_T> &gnat)
273  {
274  if (gnat.tree_)
275  {
276  out << *gnat.tree_;
277  if (!gnat.removed_.empty())
278  {
279  out << "Elements marked for removal:\n";
280  for (const auto &elt : gnat.removed_)
281  out << *elt << '\t';
282  out << std::endl;
283  }
284  }
285  return out;
286  }
287 
288  // for debugging purposes
289  void integrityCheck()
290  {
291  std::vector<_T> lst;
292  std::unordered_set<const _T *> tmp;
293  // get all elements, including those marked for removal
294  removed_.swap(tmp);
295  list(lst);
296  // check if every element marked for removal is also in the tree
297  for (const auto &elt : tmp)
298  {
299  unsigned int i;
300  for (i = 0; i < lst.size(); ++i)
301  if (lst[i] == *elt)
302  break;
303  if (i == lst.size())
304  {
305  // an element marked for removal is not actually in the tree
306  std::cout << "***** FAIL!! ******\n" << *this << '\n';
307  for (const auto &l : lst)
308  std::cout << l << '\t';
309  std::cout << std::endl;
310  }
311  assert(i != lst.size());
312  }
313  // restore
314  removed_.swap(tmp);
315  // get elements in the tree with elements marked for removal purged from the list
316  list(lst);
317  if (lst.size() != size_)
318  std::cout << "#########################################\n" << *this << std::endl;
319  assert(lst.size() == size_);
320  }
321 
322  protected:
323  using GNAT = NearestNeighborsGNAT<_T>;
324 
326  bool isRemoved(const _T &data) const
327  {
328  return !removed_.empty() && removed_.find(&data) != removed_.end();
329  }
330 
335  bool nearestKInternal(const _T &data, std::size_t k, NearQueue &nbhQueue) const
336  {
337  bool isPivot;
338  double dist;
339  NodeDist nodeDist;
340  NodeQueue nodeQueue;
341 
343  isPivot = tree_->insertNeighborK(nbhQueue, k, tree_->pivot_, data, dist);
344  tree_->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
345  while (!nodeQueue.empty())
346  {
347  dist = nbhQueue.top().first; // note the difference with nearestRInternal
348  nodeDist = nodeQueue.top();
349  nodeQueue.pop();
350  if (nbhQueue.size() == k && (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
351  nodeDist.second < nodeDist.first->minRadius_ - dist))
352  continue;
353  nodeDist.first->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
354  }
355  return isPivot;
356  }
358  void nearestRInternal(const _T &data, double radius, NearQueue &nbhQueue) const
359  {
360  double dist = radius; // note the difference with nearestKInternal
361  NodeQueue nodeQueue;
362  NodeDist nodeDist;
363 
364  tree_->insertNeighborR(nbhQueue, radius, tree_->pivot_,
366  tree_->nearestR(*this, data, radius, nbhQueue, nodeQueue);
367  while (!nodeQueue.empty())
368  {
369  nodeDist = nodeQueue.top();
370  nodeQueue.pop();
371  if (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
372  nodeDist.second < nodeDist.first->minRadius_ - dist)
373  continue;
374  nodeDist.first->nearestR(*this, data, radius, nbhQueue, nodeQueue);
375  }
376  }
379  void postprocessNearest(NearQueue &nbhQueue, std::vector<_T> &nbh) const
380  {
381  typename std::vector<_T>::reverse_iterator it;
382  nbh.resize(nbhQueue.size());
383  for (it = nbh.rbegin(); it != nbh.rend(); it++, nbhQueue.pop())
384  *it = *nbhQueue.top().second;
385  }
386 
388  class Node
389  {
390  public:
393  Node(int degree, int capacity, _T pivot)
394  : degree_(degree)
395  , pivot_(std::move(pivot))
396  , minRadius_(std::numeric_limits<double>::infinity())
398  , minRange_(degree, minRadius_)
399  , maxRange_(degree, maxRadius_)
400 #ifdef GNAT_SAMPLER
401  , subtreeSize_(1)
402  , activity_(0)
403 #endif
404  {
405  // The "+1" is needed because we add an element before we check whether to split
406  data_.reserve(capacity + 1);
407  }
408 
409  ~Node()
410  {
411  for (auto &child : children_)
412  delete child;
413  }
414 
417  void updateRadius(double dist)
418  {
419  if (minRadius_ > dist)
420  minRadius_ = dist;
421 #ifndef GNAT_SAMPLER
422  if (maxRadius_ < dist)
423  maxRadius_ = dist;
424 #else
425  if (maxRadius_ < dist)
426  {
427  maxRadius_ = dist;
428  activity_ = 0;
429  }
430  else
431  activity_ = std::max(-32, activity_ - 1);
432 #endif
433  }
437  void updateRange(unsigned int i, double dist)
438  {
439  if (minRange_[i] > dist)
440  minRange_[i] = dist;
441  if (maxRange_[i] < dist)
442  maxRange_[i] = dist;
443  }
445  void add(GNAT &gnat, const _T &data)
446  {
447 #ifdef GNAT_SAMPLER
448  subtreeSize_++;
449 #endif
450  if (children_.empty())
451  {
452  data_.push_back(data);
453  gnat.size_++;
454  if (needToSplit(gnat))
455  {
456  if (!gnat.removed_.empty())
457  gnat.rebuildDataStructure();
458  else if (gnat.size_ >= gnat.rebuildSize_)
459  {
460  gnat.rebuildSize_ <<= 1;
461  gnat.rebuildDataStructure();
462  }
463  else
464  split(gnat);
465  }
466  }
467  else
468  {
469  std::vector<double> dist(children_.size());
470  double minDist = dist[0] = gnat.distFun_(data, children_[0]->pivot_);
471  int minInd = 0;
472 
473  for (unsigned int i = 1; i < children_.size(); ++i)
474  if ((dist[i] = gnat.distFun_(data, children_[i]->pivot_)) < minDist)
475  {
476  minDist = dist[i];
477  minInd = i;
478  }
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);
483  }
484  }
486  bool needToSplit(const GNAT &gnat) const
487  {
488  unsigned int sz = data_.size();
489  return sz > gnat.maxNumPtsPerLeaf_ && sz > degree_;
490  }
494  void split(GNAT &gnat)
495  {
496  typename GreedyKCenters<_T>::Matrix dists(data_.size(), degree_);
497  std::vector<unsigned int> pivots;
498 
499  children_.reserve(degree_);
500  gnat.pivotSelector_.kcenters(data_, degree_, pivots, dists);
501  for (unsigned int &pivot : pivots)
502  children_.push_back(new Node(degree_, gnat.maxNumPtsPerLeaf_, data_[pivot]));
503  degree_ = pivots.size(); // in case fewer than degree_ pivots were found
504  for (unsigned int j = 0; j < data_.size(); ++j)
505  {
506  unsigned int k = 0;
507  for (unsigned int i = 1; i < degree_; ++i)
508  if (dists(j, i) < dists(j, k))
509  k = i;
510  Node *child = children_[k];
511  if (j != pivots[k])
512  {
513  child->data_.push_back(data_[j]);
514  child->updateRadius(dists(j, k));
515  }
516  for (unsigned int i = 0; i < degree_; ++i)
517  children_[i]->updateRange(k, dists(j, i));
518  }
519 
520  for (auto &child : children_)
521  {
522  // make sure degree lies between minDegree_ and maxDegree_
523  child->degree_ =
524  std::min(std::max((unsigned int)((degree_ * child->data_.size()) / data_.size()),
525  gnat.minDegree_),
526  gnat.maxDegree_);
527  // singleton
528  if (child->minRadius_ >= std::numeric_limits<double>::infinity())
529  child->minRadius_ = child->maxRadius_ = 0.;
530 #ifdef GNAT_SAMPLER
531  // set subtree size
532  child->subtreeSize_ = child->data_.size() + 1;
533 #endif
534  }
535  // this does more than clear(); it also sets capacity to 0 and frees the memory
536  std::vector<_T> tmp;
537  data_.swap(tmp);
538  // check if new leaves need to be split
539  for (auto &child : children_)
540  if (child->needToSplit(gnat))
541  child->split(gnat);
542  }
543 
545  bool insertNeighborK(NearQueue &nbh, std::size_t k, const _T &data, const _T &key, double dist) const
546  {
547  if (nbh.size() < k)
548  {
549  nbh.push(std::make_pair(dist, &data));
550  return true;
551  }
552  if (dist < nbh.top().first || (dist < std::numeric_limits<double>::epsilon() && data == key))
553  {
554  nbh.pop();
555  nbh.push(std::make_pair(dist, &data));
556  return true;
557  }
558  return false;
559  }
560 
566  void nearestK(const GNAT &gnat, const _T &data, std::size_t k, NearQueue &nbh, NodeQueue &nodeQueue,
567  bool &isPivot) const
568  {
569  for (const auto &d : data_)
570  if (!gnat.isRemoved(d))
571  {
572  if (insertNeighborK(nbh, k, d, data, gnat.distFun_(data, d)))
573  isPivot = false;
574  }
575  if (!children_.empty())
576  {
577  double dist;
578  Node *child;
579  std::vector<double> distToPivot(children_.size());
580  std::vector<int> permutation(children_.size());
581  for (unsigned int i = 0; i < permutation.size(); ++i)
582  permutation[i] = i;
583  // for one-time use this is faster than using ompl::Permutation
584  std::random_shuffle(permutation.begin(), permutation.end());
585 
586  for (unsigned int i = 0; i < children_.size(); ++i)
587  if (permutation[i] >= 0)
588  {
589  child = children_[permutation[i]];
590  distToPivot[permutation[i]] = gnat.distFun_(data, child->pivot_);
591  if (insertNeighborK(nbh, k, child->pivot_, data, distToPivot[permutation[i]]))
592  isPivot = true;
593  if (nbh.size() == k)
594  {
595  dist = nbh.top().first; // note difference with nearestR
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]]))
600  permutation[j] = -1;
601  }
602  }
603 
604  dist = nbh.top().first;
605  for (auto p : permutation)
606  if (p >= 0)
607  {
608  child = children_[p];
609  if (nbh.size() < k || (distToPivot[p] - dist <= child->maxRadius_ &&
610  distToPivot[p] + dist >= child->minRadius_))
611  nodeQueue.push(std::make_pair(child, distToPivot[p]));
612  }
613  }
614  }
616  void insertNeighborR(NearQueue &nbh, double r, const _T &data, double dist) const
617  {
618  if (dist <= r)
619  nbh.push(std::make_pair(dist, &data));
620  }
624  void nearestR(const GNAT &gnat, const _T &data, double r, NearQueue &nbh, NodeQueue &nodeQueue) const
625  {
626  double dist = r; // note difference with nearestK
627 
628  for (const auto &d : data_)
629  if (!gnat.isRemoved(d))
630  insertNeighborR(nbh, r, d, gnat.distFun_(data, d));
631  if (!children_.empty())
632  {
633  Node *child;
634  std::vector<double> distToPivot(children_.size());
635  std::vector<int> permutation(children_.size());
636  for (unsigned int i = 0; i < permutation.size(); ++i)
637  permutation[i] = i;
638  // for one-time use this is faster than using ompl::Permutation
639  std::random_shuffle(permutation.begin(), permutation.end());
640 
641  for (unsigned int i = 0; i < children_.size(); ++i)
642  if (permutation[i] >= 0)
643  {
644  child = children_[permutation[i]];
645  distToPivot[i] = gnat.distFun_(data, child->pivot_);
646  insertNeighborR(nbh, r, child->pivot_, distToPivot[i]);
647  for (unsigned int j = 0; j < children_.size(); ++j)
648  if (permutation[j] >= 0 && i != j &&
649  (distToPivot[i] - dist > child->maxRange_[permutation[j]] ||
650  distToPivot[i] + dist < child->minRange_[permutation[j]]))
651  permutation[j] = -1;
652  }
653 
654  for (unsigned int i = 0; i < children_.size(); ++i)
655  if (permutation[i] >= 0)
656  {
657  child = children_[permutation[i]];
658  if (distToPivot[i] - dist <= child->maxRadius_ &&
659  distToPivot[i] + dist >= child->minRadius_)
660  nodeQueue.push(std::make_pair(child, distToPivot[i]));
661  }
662  }
663  }
664 
665 #ifdef GNAT_SAMPLER
666  double getSamplingWeight(const GNAT &gnat) const
667  {
668  double minR = std::numeric_limits<double>::max();
669  for (auto minRange : minRange_)
670  if (minRange < minR && minRange > 0.0)
671  minR = minRange;
672  minR = std::max(minR, maxRadius_);
673  return std::pow(minR, gnat.estimatedDimension_) / (double)subtreeSize_;
674  }
675  const _T &sample(const GNAT &gnat, RNG &rng) const
676  {
677  if (children_.size() != 0)
678  {
679  if (rng.uniform01() < 1. / (double)subtreeSize_)
680  return pivot_;
681  PDF<const Node *> distribution;
682  for (const auto &child : children_)
683  distribution.add(child, child->getSamplingWeight(gnat));
684  return distribution.sample(rng.uniform01())->sample(gnat, rng);
685  }
686  else
687  {
688  unsigned int i = rng.uniformInt(0, data_.size());
689  return (i == data_.size()) ? pivot_ : data_[i];
690  }
691  }
692 #endif
693 
694  void list(const GNAT &gnat, std::vector<_T> &data) const
695  {
696  if (!gnat.isRemoved(pivot_))
697  data.push_back(pivot_);
698  for (const auto &d : data_)
699  if (!gnat.isRemoved(d))
700  data.push_back(d);
701  for (const auto &child : children_)
702  child->list(gnat, data);
703  }
704 
705  friend std::ostream &operator<<(std::ostream &out, const Node &node)
706  {
707  out << "\ndegree:\t" << node.degree_;
708  out << "\nminRadius:\t" << node.minRadius_;
709  out << "\nmaxRadius:\t" << node.maxRadius_;
710  out << "\nminRange:\t";
711  for (auto minR : node.minRange_)
712  out << minR << '\t';
713  out << "\nmaxRange: ";
714  for (auto maxR : node.maxRange_)
715  out << maxR << '\t';
716  out << "\npivot:\t" << node.pivot_;
717  out << "\ndata: ";
718  for (auto &data : node.data_)
719  out << data << '\t';
720  out << "\nthis:\t" << &node;
721 #ifdef GNAT_SAMPLER
722  out << "\nsubtree size:\t" << node.subtreeSize_;
723  out << "\nactivity:\t" << node.activity_;
724 #endif
725  out << "\nchildren:\n";
726  for (auto &child : node.children_)
727  out << child << '\t';
728  out << '\n';
729  for (auto &child : node.children_)
730  out << *child << '\n';
731  return out;
732  }
733 
735  unsigned int degree_;
737  const _T pivot_;
739  double minRadius_;
741  double maxRadius_;
744  std::vector<double> minRange_;
747  std::vector<double> maxRange_;
750  std::vector<_T> data_;
753  std::vector<Node *> children_;
754 #ifdef GNAT_SAMPLER
755  unsigned int subtreeSize_;
761  int activity_;
762 #endif
763  };
764 
766  Node *tree_{nullptr};
768  unsigned int degree_;
773  unsigned int minDegree_;
778  unsigned int maxDegree_;
781  unsigned int maxNumPtsPerLeaf_;
783  std::size_t size_{0};
786  std::size_t rebuildSize_;
790  std::size_t removedCacheSize_;
794  std::unordered_set<const _T *> removed_;
795 #ifdef GNAT_SAMPLER
796  double estimatedDimension_;
798 #endif
799  };
800 }
801 
802 #endif
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...
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...
Node(int degree, int capacity, _T pivot)
Construct a node of given degree with at most capacity data elements and with given 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.
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.
std::vector< Node * > children_
The child nodes of this node. By definition, only internal nodes have child nodes.
STL namespace.
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 nearestR(const _T &data, double radius, std::vector< _T > &nbh) const override
Return the nearest neighbors within distance radius in sorted order.
void rebuildDataStructure()
Rebuild the internal data structure.
void nearestK(const _T &data, std::size_t k, std::vector< _T > &nbh) const override
Return the k nearest neighbors in sorted order.
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.
Definition: PDF.h:48
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:66
unsigned int minDegree_
After splitting a Node, each child Node has degree equal to the default degree times the fraction of ...
void setDistanceFunction(const typename NearestNeighbors< _T >::DistanceFunction &distFun) override
Set the distance function to use.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:56
std::size_t size() const override
Get the number of elements in the datastructure.
virtual void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.
void list(std::vector< _T > &data) const override
Get all the elements in the datastructure.
void add(const _T &data) override
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.
Eigen::MatrixXd Matrix
A matrix type for storing distances between points and centers.
std::size_t rebuildSize_
If size_ exceeds rebuildSize_, the tree will be rebuilt (and automatically rebalanced), and rebuildSize_ will be doubled.
std::unordered_set< const _T * > removed_
Cache of removed elements.
Abstract representation of a container that can perform nearest neighbors queries.
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.
Definition: Exception.h:46
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...
Definition: PDF.h:132
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.
Definition: PDF.h:97
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...
_T nearest(const _T &data) const override
Get the nearest neighbor of a point.
void clear() override
Clear the datastructure.
void add(const std::vector< _T > &data) override
Add a vector of points.
bool reportsSortedResults() const override
Return true if the solutions reported by this data structure are sorted, when calling nearestK / near...
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.
The class used internally to define the GNAT.
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].
Definition: RandomNumbers.h:79
unsigned int degree_
The desired degree of each node.
std::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
std::size_t removedCacheSize_
Maximum number of removed elements that can be stored in the removed_ cache. If the cache is full...