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  std::size_t rebuildSize = gnat.rebuildSize_ << 1;
460  gnat.rebuildDataStructure();
461  gnat.rebuildSize_ = rebuildSize;
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.emplace(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.emplace(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::size_t sz = children_.size(), offset = gnat.offset_++;
580  std::vector<double> distToPivot(sz);
581  std::vector<int> permutation(sz);
582  for (unsigned int i = 0; i < sz; ++i)
583  permutation[i] = (i + offset) % sz;
584 
585  for (unsigned int i = 0; i < sz; ++i)
586  if (permutation[i] >= 0)
587  {
588  child = children_[permutation[i]];
589  distToPivot[permutation[i]] = gnat.distFun_(data, child->pivot_);
590  if (insertNeighborK(nbh, k, child->pivot_, data, distToPivot[permutation[i]]))
591  isPivot = true;
592  if (nbh.size() == k)
593  {
594  dist = nbh.top().first; // note difference with nearestR
595  for (unsigned int j = 0; j < sz; ++j)
596  if (permutation[j] >= 0 && i != j &&
597  (distToPivot[permutation[i]] - dist > child->maxRange_[permutation[j]] ||
598  distToPivot[permutation[i]] + dist < child->minRange_[permutation[j]]))
599  permutation[j] = -1;
600  }
601  }
602 
603  dist = nbh.top().first;
604  for (auto p : permutation)
605  if (p >= 0)
606  {
607  child = children_[p];
608  if (nbh.size() < k || (distToPivot[p] - dist <= child->maxRadius_ &&
609  distToPivot[p] + dist >= child->minRadius_))
610  nodeQueue.emplace(child, distToPivot[p]);
611  }
612  }
613  }
615  void insertNeighborR(NearQueue &nbh, double r, const _T &data, double dist) const
616  {
617  if (dist <= r)
618  nbh.emplace(dist, &data);
619  }
623  void nearestR(const GNAT &gnat, const _T &data, double r, NearQueue &nbh, NodeQueue &nodeQueue) const
624  {
625  double dist = r; // note difference with nearestK
626 
627  for (const auto &d : data_)
628  if (!gnat.isRemoved(d))
629  insertNeighborR(nbh, r, d, gnat.distFun_(data, d));
630  if (!children_.empty())
631  {
632  Node *child;
633  std::size_t sz = children_.size(), offset = gnat.offset_++;
634  std::vector<double> distToPivot(sz);
635  std::vector<int> permutation(sz);
636  // Not a random permutation, but processing the children in slightly different order is
637  // "good enough" to get a performance boost. A call to std::shuffle takes too long.
638  for (unsigned int i = 0; i < sz; ++i)
639  permutation[i] = (i + offset) % sz;
640 
641  for (unsigned int i = 0; i < sz; ++i)
642  if (permutation[i] >= 0)
643  {
644  child = children_[permutation[i]];
645  distToPivot[permutation[i]] = gnat.distFun_(data, child->pivot_);
646  insertNeighborR(nbh, r, child->pivot_, distToPivot[permutation[i]]);
647  for (unsigned int j = 0; j < sz; ++j)
648  if (permutation[j] >= 0 && i != j &&
649  (distToPivot[permutation[i]] - dist > child->maxRange_[permutation[j]] ||
650  distToPivot[permutation[i]] + dist < child->minRange_[permutation[j]]))
651  permutation[j] = -1;
652  }
653 
654  for (auto p : permutation)
655  if (p >= 0)
656  {
657  child = children_[p];
658  if (distToPivot[p] - dist <= child->maxRadius_ &&
659  distToPivot[p] + dist >= child->minRadius_)
660  nodeQueue.emplace(child, distToPivot[p]);
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_;
792  GreedyKCenters<_T> pivotSelector_;
794  std::unordered_set<const _T *> removed_;
795 #ifdef GNAT_SAMPLER
796  double estimatedDimension_;
798 #endif
799 
801  // used to cycle through children of a node in different orders
802  mutable std::size_t offset_{0};
804  };
805 }
806 
807 #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.
Definition: AppBase.h:21
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.