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 <algorithm>
46 #include <iostream>
47 #include <queue>
48 #include <random>
49 #include <unordered_set>
50 #include <utility>
51 #include "ompl/util/Exception.h"
52 
53 namespace ompl
54 {
71  template <typename _T>
72  class NearestNeighborsGNAT : public NearestNeighbors<_T>
73  {
74  protected:
76  // internally, we use a priority queue for nearest neighbors, paired
77  // with their distance to the query point
78  using NearQueue = std::priority_queue<std::pair<double, const _T *>>;
79 
80  // another internal data structure is a priority queue of nodes to
81  // check next for possible nearest neighbors
82  class Node;
83  using NodeDist = std::pair<Node *, double>;
84  struct NodeDistCompare
85  {
86  bool operator()(const NodeDist &n0, const NodeDist &n1) const
87  {
88  return (n0.second - n0.first->maxRadius_) > (n1.second - n1.first->maxRadius_);
89  }
90  };
91  using NodeQueue = std::priority_queue<NodeDist, std::vector<NodeDist>, NodeDistCompare>;
93 
94  public:
95  NearestNeighborsGNAT(unsigned int degree = 8, unsigned int minDegree = 4, unsigned int maxDegree = 12,
96  unsigned int maxNumPtsPerLeaf = 50, unsigned int removedCacheSize = 500,
97  bool rebalancing = false
98 #ifdef GNAT_SAMPLER
99  ,
100  double estimatedDimension = 6.0
101 #endif
102  )
103  : NearestNeighbors<_T>()
104  , degree_(degree)
105  , minDegree_(std::min(degree, minDegree))
106  , maxDegree_(std::max(maxDegree, degree))
107  , maxNumPtsPerLeaf_(maxNumPtsPerLeaf)
108  , rebuildSize_(rebalancing ? maxNumPtsPerLeaf * degree : std::numeric_limits<std::size_t>::max())
109  , removedCacheSize_(removedCacheSize)
110 #ifdef GNAT_SAMPLER
111  , estimatedDimension_(estimatedDimension)
112 #endif
113  {
114  }
115 
116  ~NearestNeighborsGNAT() override
117  {
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  }
178  void rebuildDataStructure()
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  nbh.resize(nbhQueue.size());
382  for (auto it = nbh.rbegin(); it != nbh.rend(); it++, nbhQueue.pop())
383  *it = *nbhQueue.top().second;
384  }
385 
387  class Node
388  {
389  public:
392  Node(int degree, int capacity, _T pivot)
393  : degree_(degree)
394  , pivot_(std::move(pivot))
395  , minRadius_(std::numeric_limits<double>::infinity())
397  , minRange_(degree, minRadius_)
398  , maxRange_(degree, maxRadius_)
399 #ifdef GNAT_SAMPLER
400  , subtreeSize_(1)
401  , activity_(0)
402 #endif
403  {
404  // The "+1" is needed because we add an element before we check whether to split
405  data_.reserve(capacity + 1);
406  }
407 
408  ~Node()
409  {
410  for (auto &child : children_)
411  delete child;
412  }
413 
416  void updateRadius(double dist)
417  {
418  if (minRadius_ > dist)
419  minRadius_ = dist;
420 #ifndef GNAT_SAMPLER
421  if (maxRadius_ < dist)
422  maxRadius_ = dist;
423 #else
424  if (maxRadius_ < dist)
425  {
426  maxRadius_ = dist;
427  activity_ = 0;
428  }
429  else
430  activity_ = std::max(-32, activity_ - 1);
431 #endif
432  }
436  void updateRange(unsigned int i, double dist)
437  {
438  if (minRange_[i] > dist)
439  minRange_[i] = dist;
440  if (maxRange_[i] < dist)
441  maxRange_[i] = dist;
442  }
444  void add(GNAT &gnat, const _T &data)
445  {
446 #ifdef GNAT_SAMPLER
447  subtreeSize_++;
448 #endif
449  if (children_.empty())
450  {
451  data_.push_back(data);
452  gnat.size_++;
453  if (needToSplit(gnat))
454  {
455  if (!gnat.removed_.empty())
456  gnat.rebuildDataStructure();
457  else if (gnat.size_ >= gnat.rebuildSize_)
458  {
459  gnat.rebuildSize_ <<= 1;
460  gnat.rebuildDataStructure();
461  }
462  else
463  split(gnat);
464  }
465  }
466  else
467  {
468  std::vector<double> dist(children_.size());
469  double minDist = dist[0] = gnat.distFun_(data, children_[0]->pivot_);
470  int minInd = 0;
471 
472  for (unsigned int i = 1; i < children_.size(); ++i)
473  if ((dist[i] = gnat.distFun_(data, children_[i]->pivot_)) < minDist)
474  {
475  minDist = dist[i];
476  minInd = i;
477  }
478  for (unsigned int i = 0; i < children_.size(); ++i)
479  children_[i]->updateRange(minInd, dist[i]);
480  children_[minInd]->updateRadius(minDist);
481  children_[minInd]->add(gnat, data);
482  }
483  }
485  bool needToSplit(const GNAT &gnat) const
486  {
487  unsigned int sz = data_.size();
488  return sz > gnat.maxNumPtsPerLeaf_ && sz > degree_;
489  }
493  void split(GNAT &gnat)
494  {
495  typename GreedyKCenters<_T>::Matrix dists(data_.size(), degree_);
496  std::vector<unsigned int> pivots;
497 
498  children_.reserve(degree_);
499  gnat.pivotSelector_.kcenters(data_, degree_, pivots, dists);
500  for (unsigned int &pivot : pivots)
501  children_.push_back(new Node(degree_, gnat.maxNumPtsPerLeaf_, data_[pivot]));
502  degree_ = pivots.size(); // in case fewer than degree_ pivots were found
503  for (unsigned int j = 0; j < data_.size(); ++j)
504  {
505  unsigned int k = 0;
506  for (unsigned int i = 1; i < degree_; ++i)
507  if (dists(j, i) < dists(j, k))
508  k = i;
509  Node *child = children_[k];
510  if (j != pivots[k])
511  {
512  child->data_.push_back(data_[j]);
513  child->updateRadius(dists(j, k));
514  }
515  for (unsigned int i = 0; i < degree_; ++i)
516  children_[i]->updateRange(k, dists(j, i));
517  }
518 
519  for (auto &child : children_)
520  {
521  // make sure degree lies between minDegree_ and maxDegree_
522  child->degree_ =
523  std::min(std::max((unsigned int)((degree_ * child->data_.size()) / data_.size()),
524  gnat.minDegree_),
525  gnat.maxDegree_);
526  // singleton
527  if (child->minRadius_ >= std::numeric_limits<double>::infinity())
528  child->minRadius_ = child->maxRadius_ = 0.;
529 #ifdef GNAT_SAMPLER
530  // set subtree size
531  child->subtreeSize_ = child->data_.size() + 1;
532 #endif
533  }
534  // this does more than clear(); it also sets capacity to 0 and frees the memory
535  std::vector<_T> tmp;
536  data_.swap(tmp);
537  // check if new leaves need to be split
538  for (auto &child : children_)
539  if (child->needToSplit(gnat))
540  child->split(gnat);
541  }
542 
544  bool insertNeighborK(NearQueue &nbh, std::size_t k, const _T &data, const _T &key, double dist) const
545  {
546  if (nbh.size() < k)
547  {
548  nbh.emplace(dist, &data);
549  return true;
550  }
551  if (dist < nbh.top().first || (dist < std::numeric_limits<double>::epsilon() && data == key))
552  {
553  nbh.pop();
554  nbh.emplace(dist, &data);
555  return true;
556  }
557  return false;
558  }
559 
565  void nearestK(const GNAT &gnat, const _T &data, std::size_t k, NearQueue &nbh, NodeQueue &nodeQueue,
566  bool &isPivot) const
567  {
568  for (const auto &d : data_)
569  if (!gnat.isRemoved(d))
570  {
571  if (insertNeighborK(nbh, k, d, data, gnat.distFun_(data, d)))
572  isPivot = false;
573  }
574  if (!children_.empty())
575  {
576  double dist;
577  Node *child;
578  std::size_t sz = children_.size(), offset = gnat.offset_++;
579  std::vector<double> distToPivot(sz);
580  std::vector<int> permutation(sz);
581  for (unsigned int i = 0; i < sz; ++i)
582  permutation[i] = (i + offset) % sz;
583 
584  for (unsigned int i = 0; i < sz; ++i)
585  if (permutation[i] >= 0)
586  {
587  child = children_[permutation[i]];
588  distToPivot[permutation[i]] = gnat.distFun_(data, child->pivot_);
589  if (insertNeighborK(nbh, k, child->pivot_, data, distToPivot[permutation[i]]))
590  isPivot = true;
591  if (nbh.size() == k)
592  {
593  dist = nbh.top().first; // note difference with nearestR
594  for (unsigned int j = 0; j < sz; ++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]]))
598  permutation[j] = -1;
599  }
600  }
601 
602  dist = nbh.top().first;
603  for (auto p : permutation)
604  if (p >= 0)
605  {
606  child = children_[p];
607  if (nbh.size() < k || (distToPivot[p] - dist <= child->maxRadius_ &&
608  distToPivot[p] + dist >= child->minRadius_))
609  nodeQueue.emplace(child, distToPivot[p]);
610  }
611  }
612  }
614  void insertNeighborR(NearQueue &nbh, double r, const _T &data, double dist) const
615  {
616  if (dist <= r)
617  nbh.emplace(dist, &data);
618  }
622  void nearestR(const GNAT &gnat, const _T &data, double r, NearQueue &nbh, NodeQueue &nodeQueue) const
623  {
624  double dist = r; // note difference with nearestK
625 
626  for (const auto &d : data_)
627  if (!gnat.isRemoved(d))
628  insertNeighborR(nbh, r, d, gnat.distFun_(data, d));
629  if (!children_.empty())
630  {
631  Node *child;
632  std::size_t sz = children_.size(), offset = gnat.offset_++;
633  std::vector<double> distToPivot(sz);
634  std::vector<int> permutation(sz);
635  // Not a random permutation, but processing the children in slightly different order is
636  // "good enough" to get a performance boost. A call to std::shuffle takes too long.
637  for (unsigned int i = 0; i < sz; ++i)
638  permutation[i] = (i + offset) % sz;
639 
640  for (unsigned int i = 0; i < sz; ++i)
641  if (permutation[i] >= 0)
642  {
643  child = children_[permutation[i]];
644  distToPivot[permutation[i]] = gnat.distFun_(data, child->pivot_);
645  insertNeighborR(nbh, r, child->pivot_, distToPivot[permutation[i]]);
646  for (unsigned int j = 0; j < sz; ++j)
647  if (permutation[j] >= 0 && i != j &&
648  (distToPivot[permutation[i]] - dist > child->maxRange_[permutation[j]] ||
649  distToPivot[permutation[i]] + dist < child->minRange_[permutation[j]]))
650  permutation[j] = -1;
651  }
652 
653  for (auto p : permutation)
654  if (p >= 0)
655  {
656  child = children_[p];
657  if (distToPivot[p] - dist <= child->maxRadius_ &&
658  distToPivot[p] + dist >= child->minRadius_)
659  nodeQueue.emplace(child, distToPivot[p]);
660  }
661  }
662  }
663 
664 #ifdef GNAT_SAMPLER
665  double getSamplingWeight(const GNAT &gnat) const
666  {
667  double minR = std::numeric_limits<double>::max();
668  for (auto minRange : minRange_)
669  if (minRange < minR && minRange > 0.0)
670  minR = minRange;
671  minR = std::max(minR, maxRadius_);
672  return std::pow(minR, gnat.estimatedDimension_) / (double)subtreeSize_;
673  }
674  const _T &sample(const GNAT &gnat, RNG &rng) const
675  {
676  if (children_.size() != 0)
677  {
678  if (rng.uniform01() < 1. / (double)subtreeSize_)
679  return pivot_;
680  PDF<const Node *> distribution;
681  for (const auto &child : children_)
682  distribution.add(child, child->getSamplingWeight(gnat));
683  return distribution.sample(rng.uniform01())->sample(gnat, rng);
684  }
685  else
686  {
687  unsigned int i = rng.uniformInt(0, data_.size());
688  return (i == data_.size()) ? pivot_ : data_[i];
689  }
690  }
691 #endif
692 
693  void list(const GNAT &gnat, std::vector<_T> &data) const
694  {
695  if (!gnat.isRemoved(pivot_))
696  data.push_back(pivot_);
697  for (const auto &d : data_)
698  if (!gnat.isRemoved(d))
699  data.push_back(d);
700  for (const auto &child : children_)
701  child->list(gnat, data);
702  }
703 
704  friend std::ostream &operator<<(std::ostream &out, const Node &node)
705  {
706  out << "\ndegree:\t" << node.degree_;
707  out << "\nminRadius:\t" << node.minRadius_;
708  out << "\nmaxRadius:\t" << node.maxRadius_;
709  out << "\nminRange:\t";
710  for (auto minR : node.minRange_)
711  out << minR << '\t';
712  out << "\nmaxRange: ";
713  for (auto maxR : node.maxRange_)
714  out << maxR << '\t';
715  out << "\npivot:\t" << node.pivot_;
716  out << "\ndata: ";
717  for (auto &data : node.data_)
718  out << data << '\t';
719  out << "\nthis:\t" << &node;
720 #ifdef GNAT_SAMPLER
721  out << "\nsubtree size:\t" << node.subtreeSize_;
722  out << "\nactivity:\t" << node.activity_;
723 #endif
724  out << "\nchildren:\n";
725  for (auto &child : node.children_)
726  out << child << '\t';
727  out << '\n';
728  for (auto &child : node.children_)
729  out << *child << '\n';
730  return out;
731  }
732 
734  unsigned int degree_;
736  const _T pivot_;
738  double minRadius_;
740  double maxRadius_;
743  std::vector<double> minRange_;
746  std::vector<double> maxRange_;
749  std::vector<_T> data_;
752  std::vector<Node *> children_;
753 #ifdef GNAT_SAMPLER
754  unsigned int subtreeSize_;
760  int activity_;
761 #endif
762  };
763 
765  Node *tree_{nullptr};
767  unsigned int degree_;
772  unsigned int minDegree_;
777  unsigned int maxDegree_;
780  unsigned int maxNumPtsPerLeaf_;
782  std::size_t size_{0};
785  std::size_t rebuildSize_;
789  std::size_t removedCacheSize_;
791  GreedyKCenters<_T> pivotSelector_;
793  std::unordered_set<const _T *> removed_;
794 #ifdef GNAT_SAMPLER
795  double estimatedDimension_;
797 #endif
798 
800  // used to cycle through children of a node in different orders
801  mutable std::size_t offset_{0};
803  };
804 }
805 
806 #endif
_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:196
Eigen::MatrixXd Matrix
A matrix type for storing distances between points and centers.
Element * add(const _T &d, const double w)
Adds a piece of data with a given weight to the PDF. Returns a corresponding Element,...
Definition: PDF.h:161
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...
virtual void add(const _T &data)=0
Add an element to the datastructure.
unsigned int degree_
The desired degree of each node.
double maxRadius_
Maximum distance between the pivot element and the elements stored in data_.
Geometric Near-neighbor Access Tree (GNAT), a data structure for nearest neighbor search.
std::size_t removedCacheSize_
Maximum number of removed elements that can be stored in the removed_ cache. If the cache is full,...
std::size_t rebuildSize_
If size_ exceeds rebuildSize_, the tree will be rebuilt (and automatically rebalanced),...
unsigned int minDegree_
After splitting a Node, each child Node has degree equal to the default degree times the fraction of ...
void rebuildDataStructure()
Rebuild the internal data structure.
std::vector< _T > data_
The data elements stored in this node (in addition to the pivot element). An internal node has no ele...
void nearestK(const _T &data, std::size_t k, std::vector< _T > &nbh) const override
Return the k nearest neighbors in sorted order.
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...
void nearestR(const _T &data, double radius, std::vector< _T > &nbh) const override
Return the nearest neighbors within distance radius in sorted order.
Node(int degree, int capacity, _T pivot)
Construct a node of given degree with at most capacity data elements and with given pivot.
bool reportsSortedResults() const override
Return true if the solutions reported by this data structure are sorted, when calling nearestK / near...
double minRadius_
Minimum distance between the pivot element and the elements stored in data_.
std::size_t size_
Number of elements stored in the tree.
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:80
std::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
Node * tree_
The data structure containing the elements stored in this structure.
unsigned int maxDegree_
After splitting a Node, each child Node has degree equal to the default degree times the fraction of ...
bool needToSplit(const GNAT &gnat) const
Return true iff the node needs to be split into child nodes.
GreedyKCenters< _T > pivotSelector_
The data structure used to split data into subtrees.
void add(const _T &data) override
Add an element to the datastructure.
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 ...
Abstract representation of a container that can perform nearest neighbors queries.
void split(GNAT &gnat)
The split operation finds pivot elements for the child nodes and moves each data element of this node...
std::vector< double > maxRange_
The i-th element in maxRange_ is the maximum distance between the pivot and any data_ element in the ...
unsigned int degree_
Number of child nodes.
std::size_t size() const override
Get the number of elements in the datastructure.
std::vector< Node * > children_
The child nodes of this node. By definition, only internal nodes have child nodes.
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 setDistanceFunction(const typename NearestNeighbors< _T >::DistanceFunction &distFun) override
Set the distance function to use.
DistanceFunction distFun_
The used distance function.
void insertNeighborR(NearQueue &nbh, double r, const _T &data, double dist) const
Insert data in nbh if it is a near neighbor.
const _T pivot_
Data element stored in this Node.
void updateRadius(double dist)
Update minRadius_ and maxRadius_, given that an element was added with distance dist to the pivot.
void list(std::vector< _T > &data) const override
Get all the elements in the datastructure.
void add(GNAT &gnat, const _T &data)
Add an element to the tree rooted at this node.
bool remove(const _T &data) override
Remove data from the tree. The element won't actually be removed immediately, but just marked for rem...
std::unordered_set< const _T * > removed_
Cache of removed elements.
void postprocessNearest(NearQueue &nbhQueue, std::vector< _T > &nbh) const
Convert the internal data structure used for storing neighbors to the vector that NearestNeighbor API...
void clear() override
Clear the datastructure.
unsigned int maxNumPtsPerLeaf_
Maximum number of elements allowed to be stored in a Node before it needs to be split into several no...
friend std::ostream & operator<<(std::ostream &out, const NearestNeighborsGNAT< _T > &gnat)
Print a GNAT structure (mostly useful for debugging purposes).
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...
The exception type for ompl.
Definition: Exception.h:78
void nearestRInternal(const _T &data, double radius, NearQueue &nbhQueue) const
Return in nbhQueue the elements that are within distance radius of data.
_T nearest(const _T &data) const override
Get the nearest neighbor of a point.
The class used internally to define the GNAT.
Main namespace. Contains everything in this library.
virtual void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.
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.