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  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
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:832
ompl::NearestNeighborsGNAT::Node::maxRadius_
double maxRadius_
Maximum distance between the pivot element and the elements stored in data_.
Definition: NearestNeighborsGNAT.h:805
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:854
ompl::NearestNeighborsGNAT::rebuildSize_
std::size_t rebuildSize_
If size_ exceeds rebuildSize_, the tree will be rebuilt (and automatically rebalanced),...
Definition: NearestNeighborsGNAT.h:850
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:837
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:814
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:630
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:803
ompl::NearestNeighborsGNAT::size_
std::size_t size_
Number of elements stored in the tree.
Definition: NearestNeighborsGNAT.h:847
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:830
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:842
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:550
ompl::NearestNeighborsGNAT::pivotSelector_
GreedyKCenters< _T > pivotSelector_
The data structure used to split data into subtrees.
Definition: NearestNeighborsGNAT.h:856
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:808
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:558
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:811
ompl::NearestNeighborsGNAT::Node::degree_
unsigned int degree_
Number of child nodes.
Definition: NearestNeighborsGNAT.h:799
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:817
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:687
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:679
ompl::NearestNeighborsGNAT::Node::pivot_
const _T pivot_
Data element stored in this Node.
Definition: NearestNeighborsGNAT.h:801
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:858
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:845
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:609