NearestNeighborsGNATNoThreadSafety.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_NO_THREAD_SAFETY_
38 #define OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_GNAT_NO_THREAD_SAFETY_
39 
40 #include "ompl/datastructures/NearestNeighbors.h"
41 #include "ompl/datastructures/GreedyKCenters.h"
42 #include "ompl/datastructures/Permutation.h"
43 #ifdef GNAT_SAMPLER
44 #include "ompl/datastructures/PDF.h"
45 #endif
46 #include "ompl/util/Exception.h"
47 #include <unordered_set>
48 #include <queue>
49 #include <algorithm>
50 #include <utility>
51 
52 namespace ompl
53 {
70  template <typename _T>
71  class NearestNeighborsGNATNoThreadSafety : public NearestNeighbors<_T>
72  {
73  protected:
75  // internally, we use a priority queue for nearest neighbors, paired
76  // with their distance to the query point
77  using NearQueue = std::priority_queue<std::pair<double, const _T *>>;
79 
80  public:
81  NearestNeighborsGNATNoThreadSafety(unsigned int degree = 8, unsigned int minDegree = 4,
82  unsigned int maxDegree = 12, unsigned int maxNumPtsPerLeaf = 50,
83  unsigned int removedCacheSize = 500, bool rebalancing = false
84 #ifdef GNAT_SAMPLER
85  ,
86  double estimatedDimension = 6.0
87 #endif
88  )
89  : NearestNeighbors<_T>()
90  , degree_(degree)
91  , minDegree_(std::min(degree, minDegree))
92  , maxDegree_(std::max(maxDegree, degree))
93  , maxNumPtsPerLeaf_(maxNumPtsPerLeaf)
94  , rebuildSize_(rebalancing ? maxNumPtsPerLeaf * degree : std::numeric_limits<std::size_t>::max())
95  , removedCacheSize_(removedCacheSize)
98 #ifdef GNAT_SAMPLER
99  , estimatedDimension_(estimatedDimension)
100 #endif
101  {
102  }
103 
105  {
106  delete tree_;
107  }
109  void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction &distFun) override
110  {
112  pivotSelector_.setDistanceFunction(distFun);
113  if (tree_)
115  }
116 
117  void clear() override
118  {
119  if (tree_)
120  {
121  delete tree_;
122  tree_ = nullptr;
123  }
124  size_ = 0;
125  removed_.clear();
126  if (rebuildSize_ != std::numeric_limits<std::size_t>::max())
128  }
129 
130  bool reportsSortedResults() const override
131  {
132  return true;
133  }
134 
135  void add(const _T &data) override
136  {
137  if (tree_)
138  {
139  if (isRemoved(data))
141  tree_->add(*this, data);
142  }
143  else
144  {
145  tree_ = new Node(degree_, maxNumPtsPerLeaf_, data);
146  size_ = 1;
147  }
148  }
149  void add(const std::vector<_T> &data) override
150  {
151  if (tree_)
153  else if (!data.empty())
154  {
155  tree_ = new Node(degree_, maxNumPtsPerLeaf_, data[0]);
156 #ifdef GNAT_SAMPLER
157  tree_->subtreeSize_ = data.size();
158 #endif
159  tree_->data_.insert(tree_->data_.end(), data.begin() + 1, data.end());
160  size_ += data.size();
161  if (tree_->needToSplit(*this))
162  tree_->split(*this);
163  }
164  }
166  void rebuildDataStructure()
167  {
168  std::vector<_T> lst;
169  list(lst);
170  clear();
171  add(lst);
172  }
178  bool remove(const _T &data) override
179  {
180  if (size_ == 0u)
181  return false;
182  // find data in tree
183  bool isPivot = nearestKInternal(data, 1);
184  const _T *d = nearQueue_.top().second;
185  nearQueue_.pop();
186  if (*d != data)
187  return false;
188  removed_.insert(d);
189  size_--;
190  // if we removed a pivot or if the capacity of removed elements
191  // has been reached, we rebuild the entire GNAT
192  if (isPivot || removed_.size() >= removedCacheSize_)
194  return true;
195  }
196 
197  _T nearest(const _T &data) const override
198  {
199  if (size_)
200  {
201  nearestKInternal(data, 1);
202  if (!nearQueue_.empty())
203  {
204  _T result = *nearQueue_.top().second;
205  nearQueue_.pop();
206  return result;
207  }
208  }
209  throw Exception("No elements found in nearest neighbors data structure");
210  }
211 
213  void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const override
214  {
215  nbh.clear();
216  if (k == 0)
217  return;
218  if (size_)
219  {
220  nearestKInternal(data, k);
221  postprocessNearest(nbh);
222  }
223  }
224 
226  void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const override
227  {
228  nbh.clear();
229  if (size_)
230  {
231  nearestRInternal(data, radius);
232  postprocessNearest(nbh);
233  }
234  assert(nearQueue_.empty());
235  assert(nodeQueue_.empty());
236  }
237 
238  std::size_t size() const override
239  {
240  return size_;
241  }
242 
243 #ifdef GNAT_SAMPLER
244  const _T &sample(RNG &rng) const
246  {
247  if (!size())
248  throw Exception("Cannot sample from an empty tree");
249  else
250  return tree_->sample(*this, rng);
251  }
252 #endif
253 
254  void list(std::vector<_T> &data) const override
255  {
256  data.clear();
257  data.reserve(size());
258  if (tree_)
259  tree_->list(*this, data);
260  }
261 
263  friend std::ostream &operator<<(std::ostream &out, const NearestNeighborsGNATNoThreadSafety<_T> &gnat)
264  {
265  if (gnat.tree_)
266  {
267  out << *gnat.tree_;
268  if (!gnat.removed_.empty())
269  {
270  out << "Elements marked for removal:\n";
271  for (const auto &elt : gnat.removed_)
272  out << *elt << '\t';
273  out << std::endl;
274  }
275  }
276  return out;
277  }
278 
279  // for debugging purposes
280  void integrityCheck()
281  {
282  std::vector<_T> lst;
283  std::unordered_set<const _T *> tmp;
284  // get all elements, including those marked for removal
285  removed_.swap(tmp);
286  list(lst);
287  // check if every element marked for removal is also in the tree
288  for (const auto &elt : tmp)
289  {
290  unsigned int i;
291  for (i = 0; i < lst.size(); ++i)
292  if (lst[i] == *elt)
293  break;
294  if (i == lst.size())
295  {
296  // an element marked for removal is not actually in the tree
297  std::cout << "***** FAIL!! ******\n" << *this << '\n';
298  for (const auto &l : lst)
299  std::cout << l << '\t';
300  std::cout << std::endl;
301  }
302  assert(i != lst.size());
303  }
304  // restore
305  removed_.swap(tmp);
306  // get elements in the tree with elements marked for removal purged from the list
307  list(lst);
308  if (lst.size() != size_)
309  std::cout << "#########################################\n" << *this << std::endl;
310  assert(lst.size() == size_);
311  }
312 
313  protected:
314  using GNAT = NearestNeighborsGNATNoThreadSafety<_T>;
315 
317  bool isRemoved(const _T &data) const
318  {
319  return !removed_.empty() && removed_.find(&data) != removed_.end();
320  }
325  bool nearestKInternal(const _T &data, std::size_t k) const
326  {
327  bool isPivot;
328  double dist;
329  Node *node;
330 
332  isPivot = tree_->insertNeighborK(nearQueue_, k, tree_->pivot_, data, tree_->distToPivot_);
333  tree_->nearestK(*this, data, k, isPivot);
334  while (!nodeQueue_.empty())
335  {
336  dist = nearQueue_.top().first; // note the difference with nearestRInternal
337  node = nodeQueue_.top();
338  nodeQueue_.pop();
339  if (nearQueue_.size() == k &&
340  (node->distToPivot_ > node->maxRadius_ + dist || node->distToPivot_ < node->minRadius_ - dist))
341  continue;
342  node->nearestK(*this, data, k, isPivot);
343  }
344  return isPivot;
345  }
347  void nearestRInternal(const _T &data, double radius) const
348  {
349  double dist = radius; // note the difference with nearestKInternal
350  Node *node;
351 
352  tree_->insertNeighborR(nearQueue_, radius, tree_->pivot_,
354  tree_->nearestR(*this, data, radius);
355  while (!nodeQueue_.empty())
356  {
357  node = nodeQueue_.top();
358  nodeQueue_.pop();
359  if (node->distToPivot_ > node->maxRadius_ + dist || node->distToPivot_ < node->minRadius_ - dist)
360  continue;
361  node->nearestR(*this, data, radius);
362  }
363  }
366  void postprocessNearest(std::vector<_T> &nbh) const
367  {
368  nbh.resize(nearQueue_.size());
369  for (auto it = nbh.rbegin(); it != nbh.rend(); it++, nearQueue_.pop())
370  *it = *nearQueue_.top().second;
371  }
372 
374  class Node
375  {
376  public:
379  Node(int degree, int capacity, _T pivot)
380  : degree_(degree)
381  , pivot_(std::move(pivot))
382  , minRadius_(std::numeric_limits<double>::infinity())
384  , minRange_(degree, minRadius_)
385  , maxRange_(degree, maxRadius_)
386 #ifdef GNAT_SAMPLER
387  , subtreeSize_(1)
388  , activity_(0)
389 #endif
390  {
391  // The "+1" is needed because we add an element before we check whether to split
392  data_.reserve(capacity + 1);
393  }
394 
395  ~Node()
396  {
397  for (auto &child : children_)
398  delete child;
399  }
400 
403  void updateRadius(double dist)
404  {
405  if (minRadius_ > dist)
406  minRadius_ = dist;
407 #ifndef GNAT_SAMPLER
408  if (maxRadius_ < dist)
409  maxRadius_ = dist;
410 #else
411  if (maxRadius_ < dist)
412  {
413  maxRadius_ = dist;
414  activity_ = 0;
415  }
416  else
417  activity_ = std::max(-32, activity_ - 1);
418 #endif
419  }
423  void updateRange(unsigned int i, double dist)
424  {
425  if (minRange_[i] > dist)
426  minRange_[i] = dist;
427  if (maxRange_[i] < dist)
428  maxRange_[i] = dist;
429  }
431  void add(GNAT &gnat, const _T &data)
432  {
433 #ifdef GNAT_SAMPLER
434  subtreeSize_++;
435 #endif
436  if (children_.empty())
437  {
438  data_.push_back(data);
439  gnat.size_++;
440  if (needToSplit(gnat))
441  {
442  if (!gnat.removed_.empty())
443  gnat.rebuildDataStructure();
444  else if (gnat.size_ >= gnat.rebuildSize_)
445  {
446  std::size_t rebuildSize = gnat.rebuildSize_ << 1;
447  gnat.rebuildDataStructure();
448  gnat.rebuildSize_ = rebuildSize;
449  }
450  else
451  split(gnat);
452  }
453  }
454  else
455  {
456  double minDist = children_[0]->distToPivot_ = gnat.distFun_(data, children_[0]->pivot_);
457  int minInd = 0;
458 
459  for (unsigned int i = 1; i < children_.size(); ++i)
460  if ((children_[i]->distToPivot_ = gnat.distFun_(data, children_[i]->pivot_)) < minDist)
461  {
462  minDist = children_[i]->distToPivot_;
463  minInd = i;
464  }
465  for (unsigned int i = 0; i < children_.size(); ++i)
466  children_[i]->updateRange(minInd, children_[i]->distToPivot_);
467  children_[minInd]->updateRadius(minDist);
468  children_[minInd]->add(gnat, data);
469  }
470  }
472  bool needToSplit(const GNAT &gnat) const
473  {
474  unsigned int sz = data_.size();
475  return sz > gnat.maxNumPtsPerLeaf_ && sz > degree_;
476  }
480  void split(GNAT &gnat)
481  {
482  typename GreedyKCenters<_T>::Matrix &dists = gnat.distances_;
483  std::vector<unsigned int> &pivots = gnat.pivots_;
484 
485  children_.reserve(degree_);
486  gnat.pivotSelector_.kcenters(data_, degree_, pivots, dists);
487  for (unsigned int &pivot : pivots)
488  children_.push_back(new Node(degree_, gnat.maxNumPtsPerLeaf_, data_[pivot]));
489  degree_ = pivots.size(); // in case fewer than degree_ pivots were found
490  for (unsigned int j = 0; j < data_.size(); ++j)
491  {
492  unsigned int k = 0;
493  for (unsigned int i = 1; i < degree_; ++i)
494  if (dists(j, i) < dists(j, k))
495  k = i;
496  Node *child = children_[k];
497  if (j != pivots[k])
498  {
499  child->data_.push_back(data_[j]);
500  child->updateRadius(dists(j, k));
501  }
502  for (unsigned int i = 0; i < degree_; ++i)
503  children_[i]->updateRange(k, dists(j, i));
504  }
505 
506  for (auto &child : children_)
507  {
508  // make sure degree lies between minDegree_ and maxDegree_
509  child->degree_ =
510  std::min(std::max((unsigned int)((degree_ * child->data_.size()) / data_.size()),
511  gnat.minDegree_),
512  gnat.maxDegree_);
513  // singleton
514  if (child->minRadius_ >= std::numeric_limits<double>::infinity())
515  child->minRadius_ = child->maxRadius_ = 0.;
516 #ifdef GNAT_SAMPLER
517  // set subtree size
518  child->subtreeSize_ = child->data_.size() + 1;
519 #endif
520  }
521  // this does more than clear(); it also sets capacity to 0 and frees the memory
522  std::vector<_T> tmp;
523  data_.swap(tmp);
524  // check if new leaves need to be split
525  for (auto &child : children_)
526  if (child->needToSplit(gnat))
527  child->split(gnat);
528  }
529 
531  bool insertNeighborK(NearQueue &nbh, std::size_t k, const _T &data, const _T &key, double dist) const
532  {
533  if (nbh.size() < k)
534  {
535  nbh.emplace(dist, &data);
536  return true;
537  }
538  if (dist < nbh.top().first || (dist < std::numeric_limits<double>::epsilon() && data == key))
539  {
540  nbh.pop();
541  nbh.emplace(dist, &data);
542  return true;
543  }
544  return false;
545  }
546 
551  void nearestK(const GNAT &gnat, const _T &data, std::size_t k, bool &isPivot) const
552  {
553  NearQueue &nbh = gnat.nearQueue_;
554  for (const auto &d : data_)
555  if (!gnat.isRemoved(d))
556  {
557  if (insertNeighborK(nbh, k, d, data, gnat.distFun_(data, d)))
558  isPivot = false;
559  }
560  if (!children_.empty())
561  {
562  double dist;
563  Node *child;
564  Permutation &permutation = gnat.permutation_;
565  permutation.permute(children_.size());
566 
567  for (unsigned int i = 0; i < children_.size(); ++i)
568  if (permutation[i] >= 0)
569  {
570  child = children_[permutation[i]];
571  child->distToPivot_ = gnat.distFun_(data, child->pivot_);
572  if (insertNeighborK(nbh, k, child->pivot_, data, child->distToPivot_))
573  isPivot = true;
574  if (nbh.size() == k)
575  {
576  dist = nbh.top().first; // note difference with nearestR
577  for (unsigned int j = 0; j < children_.size(); ++j)
578  if (permutation[j] >= 0 && i != j &&
579  (child->distToPivot_ - dist > child->maxRange_[permutation[j]] ||
580  child->distToPivot_ + dist < child->minRange_[permutation[j]]))
581  permutation[j] = -1;
582  }
583  }
584 
585  dist = nbh.top().first;
586  for (unsigned int i = 0; i < children_.size(); ++i)
587  if (permutation[i] >= 0)
588  {
589  child = children_[permutation[i]];
590  if (nbh.size() < k || (child->distToPivot_ - dist <= child->maxRadius_ &&
591  child->distToPivot_ + dist >= child->minRadius_))
592  gnat.nodeQueue_.push(child);
593  }
594  }
595  }
597  void insertNeighborR(NearQueue &nbh, double r, const _T &data, double dist) const
598  {
599  if (dist <= r)
600  nbh.emplace(dist, &data);
601  }
603  void nearestR(const GNAT &gnat, const _T &data, double r) const
604  {
605  NearQueue &nbh = gnat.nearQueue_;
606  double dist = r; // note difference with nearestK
607 
608  for (const auto &d : data_)
609  if (!gnat.isRemoved(d))
610  insertNeighborR(nbh, r, d, gnat.distFun_(data, d));
611  if (!children_.empty())
612  {
613  Node *child;
614  Permutation &permutation = gnat.permutation_;
615  permutation.permute(children_.size());
616 
617  for (unsigned int i = 0; i < children_.size(); ++i)
618  if (permutation[i] >= 0)
619  {
620  child = children_[permutation[i]];
621  child->distToPivot_ = gnat.distFun_(data, child->pivot_);
622  insertNeighborR(nbh, r, child->pivot_, child->distToPivot_);
623  for (unsigned int j = 0; j < children_.size(); ++j)
624  if (permutation[j] >= 0 && i != j &&
625  (child->distToPivot_ - dist > child->maxRange_[permutation[j]] ||
626  child->distToPivot_ + dist < child->minRange_[permutation[j]]))
627  permutation[j] = -1;
628  }
629 
630  for (unsigned int i = 0; i < children_.size(); ++i)
631  if (permutation[i] >= 0)
632  {
633  child = children_[permutation[i]];
634  if (child->distToPivot_ - dist <= child->maxRadius_ &&
635  child->distToPivot_ + dist >= child->minRadius_)
636  gnat.nodeQueue_.push(child);
637  }
638  }
639  }
640 
641 #ifdef GNAT_SAMPLER
642  double getSamplingWeight(const GNAT &gnat) const
643  {
644  double minR = std::numeric_limits<double>::max();
645  for (auto minRange : minRange_)
646  if (minRange < minR && minRange > 0.0)
647  minR = minRange;
648  minR = std::max(minR, maxRadius_);
649  return std::pow(minR, gnat.estimatedDimension_) / (double)subtreeSize_;
650  }
651  const _T &sample(const GNAT &gnat, RNG &rng) const
652  {
653  if (children_.size() != 0)
654  {
655  if (rng.uniform01() < 1. / (double)subtreeSize_)
656  return pivot_;
657  PDF<const Node *> distribution;
658  for (const auto &child : children_)
659  distribution.add(child, child->getSamplingWeight(gnat));
660  return distribution.sample(rng.uniform01())->sample(gnat, rng);
661  }
662  else
663  {
664  unsigned int i = rng.uniformInt(0, data_.size());
665  return (i == data_.size()) ? pivot_ : data_[i];
666  }
667  }
668 #endif
669 
670  void list(const GNAT &gnat, std::vector<_T> &data) const
671  {
672  if (!gnat.isRemoved(pivot_))
673  data.push_back(pivot_);
674  for (const auto &d : data_)
675  if (!gnat.isRemoved(d))
676  data.push_back(d);
677  for (const auto &child : children_)
678  child->list(gnat, data);
679  }
680 
681  friend std::ostream &operator<<(std::ostream &out, const Node &node)
682  {
683  out << "\ndegree:\t" << node.degree_;
684  out << "\nminRadius:\t" << node.minRadius_;
685  out << "\nmaxRadius:\t" << node.maxRadius_;
686  out << "\nminRange:\t";
687  for (auto minR : node.minRange_)
688  out << minR << '\t';
689  out << "\nmaxRange: ";
690  for (auto maxR : node.maxRange_)
691  out << maxR << '\t';
692  out << "\npivot:\t" << node.pivot_;
693  out << "\ndata: ";
694  for (auto &data : node.data_)
695  out << data << '\t';
696  out << "\nthis:\t" << &node;
697 #ifdef GNAT_SAMPLER
698  out << "\nsubtree size:\t" << node.subtreeSize_;
699  out << "\nactivity:\t" << node.activity_;
700 #endif
701  out << "\nchildren:\n";
702  for (auto &child : node.children_)
703  out << child << '\t';
704  out << '\n';
705  for (auto &child : node.children_)
706  out << *child << '\n';
707  return out;
708  }
709 
711  unsigned int degree_;
713  const _T pivot_;
715  double minRadius_;
717  double maxRadius_;
720  std::vector<double> minRange_;
723  std::vector<double> maxRange_;
726  std::vector<_T> data_;
729  std::vector<Node *> children_;
730 
732  mutable double distToPivot_;
733 
734 #ifdef GNAT_SAMPLER
735  unsigned int subtreeSize_;
741  int activity_;
742 #endif
743  };
744 
746  // another internal data structure is a priority queue of nodes to
747  // check next for possible nearest neighbors
748  struct NodeCompare
749  {
750  bool operator()(const Node *n0, const Node *n1) const
751  {
752  return (n0->distToPivot_ - n0->maxRadius_) > (n1->distToPivot_ - n1->maxRadius_);
753  }
754  };
755  using NodeQueue = std::priority_queue<Node *, std::vector<Node *>, NodeCompare>;
757 
759  Node *tree_{nullptr};
761  unsigned int degree_;
766  unsigned int minDegree_;
771  unsigned int maxDegree_;
774  unsigned int maxNumPtsPerLeaf_;
776  std::size_t size_{0};
779  std::size_t rebuildSize_;
783  std::size_t removedCacheSize_;
787  std::unordered_set<const _T *> removed_;
788 
792  mutable NearQueue nearQueue_;
794  mutable NodeQueue nodeQueue_;
798  mutable std::vector<unsigned int> pivots_;
800  mutable typename GreedyKCenters<_T>::Matrix distances_;
802 
803 #ifdef GNAT_SAMPLER
804  double estimatedDimension_;
806 #endif
807  };
808 }
809 
810 #endif
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:88
NodeQueue nodeQueue_
Nodes yet to be processed for possible nearest neighbors.
Eigen::MatrixXd Matrix
A matrix type for storing distances between points and centers.
void insertNeighborR(NearQueue &nbh, double r, const _T &data, double dist) const
Insert data in nbh if it is a near neighbor.
std::unordered_set< const _T * > removed_
Cache of removed elements.
virtual void add(const _T &data)=0
Add an element to the datastructure.
bool needToSplit(const GNAT &gnat) const
Return true iff the node needs to be split into child nodes.
friend std::ostream & operator<<(std::ostream &out, const NearestNeighborsGNATNoThreadSafety< _T > &gnat)
Print a GNAT structure (mostly useful for debugging purposes).
std::size_t removedCacheSize_
Maximum number of removed elements that can be stored in the removed_ cache. If the cache is full,...
bool remove(const _T &data) override
Remove data from the tree. The element won't actually be removed immediately, but just marked for rem...
void updateRadius(double dist)
Update minRadius_ and maxRadius_, given that an element was added with distance dist to the pivot.
void permute(unsigned int n)
Create a permutation of the numbers 0, ..., n - 1.
Definition: Permutation.h:122
void nearestR(const GNAT &gnat, const _T &data, double r) const
Return all elements that are within distance r in nbh.
std::vector< double > maxRange_
The i-th element in maxRange_ is the maximum distance between the pivot and any data_ element in the ...
Geometric Near-neighbor Access Tree (GNAT), a data structure for nearest neighbor search.
void add(GNAT &gnat, const _T &data)
Add an element to the tree rooted at this node.
void setDistanceFunction(const typename NearestNeighbors< _T >::DistanceFunction &distFun) override
Set the distance function to use.
An instance of this class can be used to greedily select a given number of representatives from a set...
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...
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.
A permutation of indices into an array.
Definition: Permutation.h:81
void nearestK(const _T &data, std::size_t k, std::vector< _T > &nbh) const override
Return the k nearest neighbors in sorted order.
GreedyKCenters< _T > pivotSelector_
The data structure used to split data into subtrees.
void nearestR(const _T &data, double radius, std::vector< _T > &nbh) const override
Return the nearest neighbors within distance radius in sorted order.
std::vector< double > minRange_
The i-th element in minRange_ is the minimum distance between the pivot and any data_ element in the ...
unsigned int maxDegree_
After splitting a Node, each child Node has degree equal to the default degree times the fraction of ...
double maxRadius_
Maximum distance between the pivot element and the elements stored in data_.
std::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
std::size_t size_
Number of elements stored in the tree.
unsigned int minDegree_
After splitting a Node, each child Node has degree equal to the default degree times the fraction of ...
Node(int degree, int capacity, _T pivot)
Construct a node of given degree with at most capacity data elements and with given pivot.
std::vector< _T > data_
The data elements stored in this node (in addition to the pivot element). An internal node has no ele...
bool nearestKInternal(const _T &data, std::size_t k) const
Return in nearQueue_ the k nearest neighbors of data. For k=1, return true if the nearest neighbor is...
Node * tree_
The data structure containing the elements stored in this structure.
void add(const _T &data) override
Add an element to the datastructure.
DistanceFunction distFun_
The used distance function.
void nearestRInternal(const _T &data, double radius) const
Return in nearQueue_ the elements that are within distance radius of data.
std::size_t rebuildSize_
If size_ exceeds rebuildSize_, the tree will be rebuilt (and automatically rebalanced),...
unsigned int maxNumPtsPerLeaf_
Maximum number of elements allowed to be stored in a Node before it needs to be split into several no...
unsigned int degree_
The desired degree of each node.
double minRadius_
Minimum distance between the pivot element and the elements stored in data_.
bool reportsSortedResults() const override
Return true if the solutions reported by this data structure are sorted, when calling nearestK / near...
void nearestK(const GNAT &gnat, const _T &data, std::size_t k, bool &isPivot) const
Compute the k nearest neighbors of data in the tree. For k=1, isPivot is true if the nearest neighbor...
Permutation permutation_
Permutation of indices to process children of a node in random order.
void postprocessNearest(std::vector< _T > &nbh) const
Convert the internal data structure used for storing neighbors to the vector that NearestNeighbor API...
void split(GNAT &gnat)
The split operation finds pivot elements for the child nodes and moves each data element of this node...
std::size_t size() const override
Get the number of elements in the datastructure.
bool isRemoved(const _T &data) const
Return true iff data has been marked for removal.
double distToPivot_
Scratch space to store distance to pivot during nearest neighbor queries.
void rebuildDataStructure()
Rebuild the internal data structure.
std::vector< Node * > children_
The child nodes of this node. By definition, only internal nodes have child nodes.
The exception type for ompl.
Definition: Exception.h:78
GreedyKCenters< _T >::Matrix distances_
Matrix of distances to pivots.
_T nearest(const _T &data) const override
Get the nearest neighbor of a point.
std::vector< unsigned int > pivots_
Pivot indices within a vector of elements as selected by GreedyKCenters.
void list(std::vector< _T > &data) const override
Get all the elements in the datastructure.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
virtual void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.