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  {
123  NearestNeighbors<_T>::setDistanceFunction(distFun);
124  pivotSelector_.setDistanceFunction(distFun);
125  if (tree_)
126  rebuildDataStructure();
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())
139  rebuildSize_ = maxNumPtsPerLeaf_ * degree_;
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))
152  rebuildDataStructure();
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_)
164  NearestNeighbors<_T>::add(data);
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_)
205  rebuildDataStructure();
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 
342  dist = NearestNeighbors<_T>::distFun_(data, tree_->pivot_);
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_,
365  NearestNeighbors<_T>::distFun_(data, 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())
396  , maxRadius_(-minRadius_)
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
ompl::PDF::sample
_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
ompl::GreedyKCenters::Matrix
Eigen::MatrixXd Matrix
A matrix type for storing distances between points and centers.
Definition: GreedyKCenters.h:120
ompl::PDF::add
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
ompl::NearestNeighborsGNAT::Node::updateRange
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...
Definition: NearestNeighborsGNAT.h:500
ompl::NearestNeighbors::add
virtual void add(const _T &data)=0
Add an element to the datastructure.
ompl::NearestNeighborsGNAT::degree_
unsigned int degree_
The desired degree of each node.
Definition: NearestNeighborsGNAT.h:831
ompl::NearestNeighborsGNAT::Node::maxRadius_
double maxRadius_
Maximum distance between the pivot element and the elements stored in data_.
Definition: NearestNeighborsGNAT.h:804
ompl::NearestNeighborsGNAT
Geometric Near-neighbor Access Tree (GNAT), a data structure for nearest neighbor search.
Definition: NearestNeighborsGNAT.h:104
ompl::NearestNeighborsGNAT::removedCacheSize_
std::size_t removedCacheSize_
Maximum number of removed elements that can be stored in the removed_ cache. If the cache is full,...
Definition: NearestNeighborsGNAT.h:853
ompl::NearestNeighborsGNAT::rebuildSize_
std::size_t rebuildSize_
If size_ exceeds rebuildSize_, the tree will be rebuilt (and automatically rebalanced),...
Definition: NearestNeighborsGNAT.h:849
ompl::NearestNeighborsGNAT::minDegree_
unsigned int minDegree_
After splitting a Node, each child Node has degree equal to the default degree times the fraction of ...
Definition: NearestNeighborsGNAT.h:836
ompl::NearestNeighborsGNAT::rebuildDataStructure
void rebuildDataStructure()
Rebuild the internal data structure.
Definition: NearestNeighborsGNAT.h:242
ompl::NearestNeighborsGNAT::Node::data_
std::vector< _T > data_
The data elements stored in this node (in addition to the pivot element). An internal node has no ele...
Definition: NearestNeighborsGNAT.h:813
ompl::NearestNeighborsGNAT::nearestK
void nearestK(const _T &data, std::size_t k, std::vector< _T > &nbh) const override
Return the k nearest neighbors in sorted order.
Definition: NearestNeighborsGNAT.h:286
ompl::NearestNeighborsGNAT::Node::nearestK
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...
Definition: NearestNeighborsGNAT.h:629
ompl::NearestNeighborsGNAT::nearestR
void nearestR(const _T &data, double radius, std::vector< _T > &nbh) const override
Return the nearest neighbors within distance radius in sorted order.
Definition: NearestNeighborsGNAT.h:300
ompl::NearestNeighborsGNAT::Node::Node
Node(int degree, int capacity, _T pivot)
Construct a node of given degree with at most capacity data elements and with given pivot.
Definition: NearestNeighborsGNAT.h:456
ompl::NearestNeighborsGNAT::reportsSortedResults
bool reportsSortedResults() const override
Return true if the solutions reported by this data structure are sorted, when calling nearestK / near...
Definition: NearestNeighborsGNAT.h:206
ompl::NearestNeighborsGNAT::Node::minRadius_
double minRadius_
Minimum distance between the pivot element and the elements stored in data_.
Definition: NearestNeighborsGNAT.h:802
ompl::NearestNeighborsGNAT::size_
std::size_t size_
Number of elements stored in the tree.
Definition: NearestNeighborsGNAT.h:846
ompl::PDF
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:80
ompl::NearestNeighbors::DistanceFunction
std::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
Definition: NearestNeighbors.h:115
ompl::NearestNeighborsGNAT::tree_
Node * tree_
The data structure containing the elements stored in this structure.
Definition: NearestNeighborsGNAT.h:829
ompl::NearestNeighborsGNAT::maxDegree_
unsigned int maxDegree_
After splitting a Node, each child Node has degree equal to the default degree times the fraction of ...
Definition: NearestNeighborsGNAT.h:841
ompl::NearestNeighborsGNAT::Node::needToSplit
bool needToSplit(const GNAT &gnat) const
Return true iff the node needs to be split into child nodes.
Definition: NearestNeighborsGNAT.h:549
ompl::NearestNeighborsGNAT::pivotSelector_
GreedyKCenters< _T > pivotSelector_
The data structure used to split data into subtrees.
Definition: NearestNeighborsGNAT.h:855
ompl::NearestNeighborsGNAT::add
void add(const _T &data) override
Add an element to the datastructure.
Definition: NearestNeighborsGNAT.h:211
ompl::NearestNeighborsGNAT::isRemoved
bool isRemoved(const _T &data) const
Return true iff data has been marked for removal.
Definition: NearestNeighborsGNAT.h:390
ompl::NearestNeighborsGNAT::Node::minRange_
std::vector< double > minRange_
The i-th element in minRange_ is the minimum distance between the pivot and any data_ element in the ...
Definition: NearestNeighborsGNAT.h:807
ompl::NearestNeighbors
Abstract representation of a container that can perform nearest neighbors queries.
Definition: NearestNeighbors.h:79
ompl::NearestNeighborsGNAT::Node::split
void split(GNAT &gnat)
The split operation finds pivot elements for the child nodes and moves each data element of this node...
Definition: NearestNeighborsGNAT.h:557
ompl::NearestNeighborsGNAT::Node::maxRange_
std::vector< double > maxRange_
The i-th element in maxRange_ is the maximum distance between the pivot and any data_ element in the ...
Definition: NearestNeighborsGNAT.h:810
ompl::NearestNeighborsGNAT::Node::degree_
unsigned int degree_
Number of child nodes.
Definition: NearestNeighborsGNAT.h:798
ompl::NearestNeighborsGNAT::size
std::size_t size() const override
Get the number of elements in the datastructure.
Definition: NearestNeighborsGNAT.h:311
ompl::NearestNeighborsGNAT::Node::children_
std::vector< Node * > children_
The child nodes of this node. By definition, only internal nodes have child nodes.
Definition: NearestNeighborsGNAT.h:816
ompl::NearestNeighborsGNAT::Node::nearestR
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...
Definition: NearestNeighborsGNAT.h:686
ompl::NearestNeighborsGNAT::setDistanceFunction
void setDistanceFunction(const typename NearestNeighbors< _T >::DistanceFunction &distFun) override
Set the distance function to use.
Definition: NearestNeighborsGNAT.h:185
ompl::NearestNeighbors::distFun_
DistanceFunction distFun_
The used distance function.
Definition: NearestNeighbors.h:178
ompl::NearestNeighborsGNAT::Node::insertNeighborR
void insertNeighborR(NearQueue &nbh, double r, const _T &data, double dist) const
Insert data in nbh if it is a near neighbor.
Definition: NearestNeighborsGNAT.h:678
ompl::NearestNeighborsGNAT::Node::pivot_
const _T pivot_
Data element stored in this Node.
Definition: NearestNeighborsGNAT.h:800
ompl::NearestNeighborsGNAT::Node::updateRadius
void updateRadius(double dist)
Update minRadius_ and maxRadius_, given that an element was added with distance dist to the pivot.
Definition: NearestNeighborsGNAT.h:480
ompl::NearestNeighborsGNAT::list
void list(std::vector< _T > &data) const override
Get all the elements in the datastructure.
Definition: NearestNeighborsGNAT.h:327
ompl::NearestNeighborsGNAT::Node::add
void add(GNAT &gnat, const _T &data)
Add an element to the tree rooted at this node.
Definition: NearestNeighborsGNAT.h:508
ompl::NearestNeighborsGNAT::remove
bool remove(const _T &data) override
Remove data from the tree. The element won't actually be removed immediately, but just marked for rem...
Definition: NearestNeighborsGNAT.h:254
ompl::NearestNeighborsGNAT::removed_
std::unordered_set< const _T * > removed_
Cache of removed elements.
Definition: NearestNeighborsGNAT.h:857
ompl::NearestNeighborsGNAT::postprocessNearest
void postprocessNearest(NearQueue &nbhQueue, std::vector< _T > &nbh) const
Convert the internal data structure used for storing neighbors to the vector that NearestNeighbor API...
Definition: NearestNeighborsGNAT.h:443
ompl::NearestNeighborsGNAT::clear
void clear() override
Clear the datastructure.
Definition: NearestNeighborsGNAT.h:193
ompl::NearestNeighborsGNAT::maxNumPtsPerLeaf_
unsigned int maxNumPtsPerLeaf_
Maximum number of elements allowed to be stored in a Node before it needs to be split into several no...
Definition: NearestNeighborsGNAT.h:844
ompl::NearestNeighborsGNAT::operator<<
friend std::ostream & operator<<(std::ostream &out, const NearestNeighborsGNAT< _T > &gnat)
Print a GNAT structure (mostly useful for debugging purposes).
Definition: NearestNeighborsGNAT.h:336
ompl::NearestNeighborsGNAT::nearestKInternal
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...
Definition: NearestNeighborsGNAT.h:399
ompl::Exception
The exception type for ompl.
Definition: Exception.h:78
ompl::NearestNeighborsGNAT::nearestRInternal
void nearestRInternal(const _T &data, double radius, NearQueue &nbhQueue) const
Return in nbhQueue the elements that are within distance radius of data.
Definition: NearestNeighborsGNAT.h:422
ompl::NearestNeighborsGNAT::nearest
_T nearest(const _T &data) const override
Get the nearest neighbor of a point.
Definition: NearestNeighborsGNAT.h:273
ompl::NearestNeighborsGNAT::Node
The class used internally to define the GNAT.
Definition: NearestNeighborsGNAT.h:451
ompl
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
ompl::NearestNeighbors::setDistanceFunction
virtual void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.
Definition: NearestNeighbors.h:122
ompl::NearestNeighborsGNAT::Node::insertNeighborK
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.
Definition: NearestNeighborsGNAT.h:608