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