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 DataDist = std::pair<const _T *, double>;
78  struct DataDistCompare
79  {
80  bool operator()(const DataDist &d0, const DataDist &d1)
81  {
82  return d0.second < d1.second;
83  }
84  };
85  using NearQueue = std::priority_queue<DataDist, std::vector<DataDist>, DataDistCompare>;
87 
88  public:
89  NearestNeighborsGNATNoThreadSafety(unsigned int degree = 8, unsigned int minDegree = 4,
90  unsigned int maxDegree = 12, unsigned int maxNumPtsPerLeaf = 50,
91  unsigned int removedCacheSize = 500, bool rebalancing = false
92 #ifdef GNAT_SAMPLER
93  ,
94  double estimatedDimension = 6.0
95 #endif
96  )
98  , degree_(degree)
99  , minDegree_(std::min(degree, minDegree))
100  , maxDegree_(std::max(maxDegree, degree))
101  , maxNumPtsPerLeaf_(maxNumPtsPerLeaf)
102  , rebuildSize_(rebalancing ? maxNumPtsPerLeaf * degree : std::numeric_limits<std::size_t>::max())
103  , removedCacheSize_(removedCacheSize)
106 #ifdef GNAT_SAMPLER
107  , estimatedDimension_(estimatedDimension)
108 #endif
109  {
110  }
111 
113  {
114  if (tree_)
115  delete tree_;
116  }
118  void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction &distFun) override
119  {
121  pivotSelector_.setDistanceFunction(distFun);
122  if (tree_)
124  }
125 
126  void clear() override
127  {
128  if (tree_)
129  {
130  delete tree_;
131  tree_ = nullptr;
132  }
133  size_ = 0;
134  removed_.clear();
135  if (rebuildSize_ != std::numeric_limits<std::size_t>::max())
137  }
138 
139  bool reportsSortedResults() const override
140  {
141  return true;
142  }
143 
144  void add(const _T &data) override
145  {
146  if (tree_)
147  {
148  if (isRemoved(data))
150  tree_->add(*this, data);
151  }
152  else
153  {
154  tree_ = new Node(degree_, maxNumPtsPerLeaf_, data);
155  size_ = 1;
156  }
157  }
158  void add(const std::vector<_T> &data) override
159  {
160  if (tree_)
162  else if (!data.empty())
163  {
164  tree_ = new Node(degree_, maxNumPtsPerLeaf_, data[0]);
165 #ifdef GNAT_SAMPLER
166  tree_->subtreeSize_ = data.size();
167 #endif
168  for (unsigned int i = 1; i < data.size(); ++i)
169  tree_->data_.push_back(data[i]);
170  size_ += data.size();
171  if (tree_->needToSplit(*this))
172  tree_->split(*this);
173  }
174  }
177  {
178  std::vector<_T> lst;
179  list(lst);
180  clear();
181  add(lst);
182  }
188  bool remove(const _T &data) override
189  {
190  if (size_ == 0u)
191  return false;
192  // find data in tree
193  bool isPivot = nearestKInternal(data, 1);
194  const _T *d = nearQueue_.top().first;
195  nearQueue_.pop();
196  if (*d != data)
197  return false;
198  removed_.insert(d);
199  size_--;
200  // if we removed a pivot or if the capacity of removed elements
201  // has been reached, we rebuild the entire GNAT
202  if (isPivot || removed_.size() >= removedCacheSize_)
204  return true;
205  }
206 
207  _T nearest(const _T &data) const override
208  {
209  if (size_)
210  {
211  nearestKInternal(data, 1);
212  if (!nearQueue_.empty())
213  {
214  _T result = *nearQueue_.top().first;
215  nearQueue_.pop();
216  return result;
217  }
218  }
219  throw Exception("No elements found in nearest neighbors data structure");
220  }
221 
223  void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const override
224  {
225  nbh.clear();
226  if (k == 0)
227  return;
228  if (size_)
229  {
230  nearestKInternal(data, k);
231  postprocessNearest(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  nearestRInternal(data, radius);
242  postprocessNearest(nbh);
243  }
244  assert(nearQueue_.empty());
245  assert(nodeQueue_.empty());
246  }
247 
248  std::size_t size() const override
249  {
250  return size_;
251  }
252 
253 #ifdef GNAT_SAMPLER
254  const _T &sample(RNG &rng) const
256  {
257  if (!size())
258  throw Exception("Cannot sample from an empty tree");
259  else
260  return tree_->sample(*this, rng);
261  }
262 #endif
263 
264  void list(std::vector<_T> &data) const override
265  {
266  data.clear();
267  data.reserve(size());
268  if (tree_)
269  tree_->list(*this, data);
270  }
271 
273  friend std::ostream &operator<<(std::ostream &out, const NearestNeighborsGNATNoThreadSafety<_T> &gnat)
274  {
275  if (gnat.tree_)
276  {
277  out << *gnat.tree_;
278  if (!gnat.removed_.empty())
279  {
280  out << "Elements marked for removal:\n";
281  for (typename std::unordered_set<const _T *>::const_iterator it = gnat.removed_.begin();
282  it != gnat.removed_.end(); it++)
283  out << **it << '\t';
284  out << std::endl;
285  }
286  }
287  return out;
288  }
289 
290  // for debugging purposes
291  void integrityCheck()
292  {
293  std::vector<_T> lst;
294  std::unordered_set<const _T *> tmp;
295  // get all elements, including those marked for removal
296  removed_.swap(tmp);
297  list(lst);
298  // check if every element marked for removal is also in the tree
299  for (typename std::unordered_set<const _T *>::iterator it = tmp.begin(); it != tmp.end(); it++)
300  {
301  unsigned int i;
302  for (i = 0; i < lst.size(); ++i)
303  if (lst[i] == **it)
304  break;
305  if (i == lst.size())
306  {
307  // an element marked for removal is not actually in the tree
308  std::cout << "***** FAIL!! ******\n" << *this << '\n';
309  for (unsigned int j = 0; j < lst.size(); ++j)
310  std::cout << lst[j] << '\t';
311  std::cout << std::endl;
312  }
313  assert(i != lst.size());
314  }
315  // restore
316  removed_.swap(tmp);
317  // get elements in the tree with elements marked for removal purged from the list
318  list(lst);
319  if (lst.size() != size_)
320  std::cout << "#########################################\n" << *this << std::endl;
321  assert(lst.size() == size_);
322  }
323 
324  protected:
326 
328  bool isRemoved(const _T &data) const
329  {
330  return !removed_.empty() && removed_.find(&data) != removed_.end();
331  }
336  bool nearestKInternal(const _T &data, std::size_t k) const
337  {
338  bool isPivot;
339  double dist;
340  Node *node;
341 
343  isPivot = tree_->insertNeighborK(nearQueue_, k, tree_->pivot_, data, tree_->distToPivot_);
344  tree_->nearestK(*this, data, k, isPivot);
345  while (!nodeQueue_.empty())
346  {
347  dist = nearQueue_.top().second; // note the difference with nearestRInternal
348  node = nodeQueue_.top();
349  nodeQueue_.pop();
350  if (nearQueue_.size() == k &&
351  (node->distToPivot_ > node->maxRadius_ + dist || node->distToPivot_ < node->minRadius_ - dist))
352  continue;
353  node->nearestK(*this, data, k, isPivot);
354  }
355  return isPivot;
356  }
358  void nearestRInternal(const _T &data, double radius) const
359  {
360  double dist = radius; // note the difference with nearestKInternal
361  Node *node;
362 
363  tree_->insertNeighborR(nearQueue_, radius, tree_->pivot_,
365  tree_->nearestR(*this, data, radius);
366  while (!nodeQueue_.empty())
367  {
368  node = nodeQueue_.top();
369  nodeQueue_.pop();
370  if (node->distToPivot_ > node->maxRadius_ + dist || node->distToPivot_ < node->minRadius_ - dist)
371  continue;
372  node->nearestR(*this, data, radius);
373  }
374  }
377  void postprocessNearest(std::vector<_T> &nbh) const
378  {
379  typename std::vector<_T>::reverse_iterator it;
380  nbh.resize(nearQueue_.size());
381  for (it = nbh.rbegin(); it != nbh.rend(); it++, nearQueue_.pop())
382  *it = *nearQueue_.top().first;
383  }
384 
386  class Node
387  {
388  public:
391  Node(int degree, int capacity, _T pivot)
392  : degree_(degree)
393  , pivot_(std::move(pivot))
394  , minRadius_(std::numeric_limits<double>::infinity())
395  , maxRadius_(-minRadius_)
396  , minRange_(degree, minRadius_)
397  , maxRange_(degree, maxRadius_)
398 #ifdef GNAT_SAMPLER
399  , subtreeSize_(1)
400  , activity_(0)
401 #endif
402  {
403  // The "+1" is needed because we add an element before we check whether to split
404  data_.reserve(capacity + 1);
405  }
406 
407  ~Node()
408  {
409  for (unsigned int i = 0; i < children_.size(); ++i)
410  delete children_[i];
411  }
412 
415  void updateRadius(double dist)
416  {
417  if (minRadius_ > dist)
418  minRadius_ = dist;
419 #ifndef GNAT_SAMPLER
420  if (maxRadius_ < dist)
421  maxRadius_ = dist;
422 #else
423  if (maxRadius_ < dist)
424  {
425  maxRadius_ = dist;
426  activity_ = 0;
427  }
428  else
429  activity_ = std::max(-32, activity_ - 1);
430 #endif
431  }
435  void updateRange(unsigned int i, double dist)
436  {
437  if (minRange_[i] > dist)
438  minRange_[i] = dist;
439  if (maxRange_[i] < dist)
440  maxRange_[i] = dist;
441  }
443  void add(GNAT &gnat, const _T &data)
444  {
445 #ifdef GNAT_SAMPLER
446  subtreeSize_++;
447 #endif
448  if (children_.empty())
449  {
450  data_.push_back(data);
451  gnat.size_++;
452  if (needToSplit(gnat))
453  {
454  if (!gnat.removed_.empty())
455  gnat.rebuildDataStructure();
456  else if (gnat.size_ >= gnat.rebuildSize_)
457  {
458  gnat.rebuildSize_ <<= 1;
459  gnat.rebuildDataStructure();
460  }
461  else
462  split(gnat);
463  }
464  }
465  else
466  {
467  double minDist = children_[0]->distToPivot_ = gnat.distFun_(data, children_[0]->pivot_);
468  int minInd = 0;
469 
470  for (unsigned int i = 1; i < children_.size(); ++i)
471  if ((children_[i]->distToPivot_ = gnat.distFun_(data, children_[i]->pivot_)) < minDist)
472  {
473  minDist = children_[i]->distToPivot_;
474  minInd = i;
475  }
476  for (unsigned int i = 0; i < children_.size(); ++i)
477  children_[i]->updateRange(minInd, children_[i]->distToPivot_);
478  children_[minInd]->updateRadius(minDist);
479  children_[minInd]->add(gnat, data);
480  }
481  }
483  bool needToSplit(const GNAT &gnat) const
484  {
485  unsigned int sz = data_.size();
486  return sz > gnat.maxNumPtsPerLeaf_ && sz > degree_;
487  }
491  void split(GNAT &gnat)
492  {
493  typename GreedyKCenters<_T>::Matrix &dists = gnat.distances_;
494  std::vector<unsigned int> &pivots = gnat.pivots_;
495 
496  children_.reserve(degree_);
497  gnat.pivotSelector_.kcenters(data_, degree_, pivots, dists);
498  for (unsigned int &pivot : pivots)
499  children_.push_back(new Node(degree_, gnat.maxNumPtsPerLeaf_, data_[pivot]));
500  degree_ = pivots.size(); // in case fewer than degree_ pivots were found
501  for (unsigned int j = 0; j < data_.size(); ++j)
502  {
503  unsigned int k = 0;
504  for (unsigned int i = 1; i < degree_; ++i)
505  if (dists(j, i) < dists(j, k))
506  k = i;
507  Node *child = children_[k];
508  if (j != pivots[k])
509  {
510  child->data_.push_back(data_[j]);
511  child->updateRadius(dists(j, k));
512  }
513  for (unsigned int i = 0; i < degree_; ++i)
514  children_[i]->updateRange(k, dists(j, i));
515  }
516 
517  for (unsigned int i = 0; i < degree_; ++i)
518  {
519  // make sure degree lies between minDegree_ and maxDegree_
520  children_[i]->degree_ =
521  std::min(std::max((unsigned int)((degree_ * children_[i]->data_.size()) / data_.size()),
522  gnat.minDegree_),
523  gnat.maxDegree_);
524  // singleton
525  if (children_[i]->minRadius_ >= std::numeric_limits<double>::infinity())
526  children_[i]->minRadius_ = children_[i]->maxRadius_ = 0.;
527 #ifdef GNAT_SAMPLER
528  // set subtree size
529  children_[i]->subtreeSize_ = children_[i]->data_.size() + 1;
530 #endif
531  }
532  // this does more than clear(); it also sets capacity to 0 and frees the memory
533  std::vector<_T> tmp;
534  data_.swap(tmp);
535  // check if new leaves need to be split
536  for (unsigned int i = 0; i < degree_; ++i)
537  if (children_[i]->needToSplit(gnat))
538  children_[i]->split(gnat);
539  }
540 
542  bool insertNeighborK(NearQueue &nbh, std::size_t k, const _T &data, const _T &key, double dist) const
543  {
544  if (nbh.size() < k)
545  {
546  nbh.push(std::make_pair(&data, dist));
547  return true;
548  }
549  if (dist < nbh.top().second || (dist < std::numeric_limits<double>::epsilon() && data == key))
550  {
551  nbh.pop();
552  nbh.push(std::make_pair(&data, dist));
553  return true;
554  }
555  return false;
556  }
557 
562  void nearestK(const GNAT &gnat, const _T &data, std::size_t k, bool &isPivot) const
563  {
564  NearQueue &nbh = gnat.nearQueue_;
565  for (unsigned int i = 0; i < data_.size(); ++i)
566  if (!gnat.isRemoved(data_[i]))
567  {
568  if (insertNeighborK(nbh, k, data_[i], data, gnat.distFun_(data, data_[i])))
569  isPivot = false;
570  }
571  if (!children_.empty())
572  {
573  double dist;
574  Node *child;
575  Permutation &permutation = gnat.permutation_;
576  permutation.permute(children_.size());
577 
578  for (unsigned int i = 0; i < children_.size(); ++i)
579  if (permutation[i] >= 0)
580  {
581  child = children_[permutation[i]];
582  child->distToPivot_ = gnat.distFun_(data, child->pivot_);
583  if (insertNeighborK(nbh, k, child->pivot_, data, child->distToPivot_))
584  isPivot = true;
585  if (nbh.size() == k)
586  {
587  dist = nbh.top().second; // note difference with nearestR
588  for (unsigned int j = 0; j < children_.size(); ++j)
589  if (permutation[j] >= 0 && i != j &&
590  (child->distToPivot_ - dist > child->maxRange_[permutation[j]] ||
591  child->distToPivot_ + dist < child->minRange_[permutation[j]]))
592  permutation[j] = -1;
593  }
594  }
595 
596  dist = nbh.top().second;
597  for (unsigned int i = 0; i < children_.size(); ++i)
598  if (permutation[i] >= 0)
599  {
600  child = children_[permutation[i]];
601  if (nbh.size() < k || (child->distToPivot_ - dist <= child->maxRadius_ &&
602  child->distToPivot_ + dist >= child->minRadius_))
603  gnat.nodeQueue_.push(child);
604  }
605  }
606  }
608  void insertNeighborR(NearQueue &nbh, double r, const _T &data, double dist) const
609  {
610  if (dist <= r)
611  nbh.push(std::make_pair(&data, dist));
612  }
614  void nearestR(const GNAT &gnat, const _T &data, double r) const
615  {
616  NearQueue &nbh = gnat.nearQueue_;
617  double dist = r; // note difference with nearestK
618 
619  for (unsigned int i = 0; i < data_.size(); ++i)
620  if (!gnat.isRemoved(data_[i]))
621  insertNeighborR(nbh, r, data_[i], gnat.distFun_(data, data_[i]));
622  if (!children_.empty())
623  {
624  Node *child;
625  Permutation &permutation = gnat.permutation_;
626  permutation.permute(children_.size());
627 
628  for (unsigned int i = 0; i < children_.size(); ++i)
629  if (permutation[i] >= 0)
630  {
631  child = children_[permutation[i]];
632  child->distToPivot_ = gnat.distFun_(data, child->pivot_);
633  insertNeighborR(nbh, r, child->pivot_, child->distToPivot_);
634  for (unsigned int j = 0; j < children_.size(); ++j)
635  if (permutation[j] >= 0 && i != j &&
636  (child->distToPivot_ - dist > child->maxRange_[permutation[j]] ||
637  child->distToPivot_ + dist < child->minRange_[permutation[j]]))
638  permutation[j] = -1;
639  }
640 
641  for (unsigned int i = 0; i < children_.size(); ++i)
642  if (permutation[i] >= 0)
643  {
644  child = children_[permutation[i]];
645  if (child->distToPivot_ - dist <= child->maxRadius_ &&
646  child->distToPivot_ + dist >= child->minRadius_)
647  gnat.nodeQueue_.push(child);
648  }
649  }
650  }
651 
652 #ifdef GNAT_SAMPLER
653  double getSamplingWeight(const GNAT &gnat) const
654  {
655  double minR = std::numeric_limits<double>::max();
656  for (auto minRange : minRange_)
657  if (minRange < minR && minRange > 0.0)
658  minR = minRange;
659  minR = std::max(minR, maxRadius_);
660  return std::pow(minR, gnat.estimatedDimension_) / (double)subtreeSize_;
661  }
662  const _T &sample(const GNAT &gnat, RNG &rng) const
663  {
664  if (children_.size() != 0)
665  {
666  if (rng.uniform01() < 1. / (double)subtreeSize_)
667  return pivot_;
668  PDF<const Node *> distribution;
669  for (const auto &child : children_)
670  distribution.add(child, child->getSamplingWeight(gnat));
671  return distribution.sample(rng.uniform01())->sample(gnat, rng);
672  }
673  else
674  {
675  unsigned int i = rng.uniformInt(0, data_.size());
676  return (i == data_.size()) ? pivot_ : data_[i];
677  }
678  }
679 #endif
680 
681  void list(const GNAT &gnat, std::vector<_T> &data) const
682  {
683  if (!gnat.isRemoved(pivot_))
684  data.push_back(pivot_);
685  for (unsigned int i = 0; i < data_.size(); ++i)
686  if (!gnat.isRemoved(data_[i]))
687  data.push_back(data_[i]);
688  for (unsigned int i = 0; i < children_.size(); ++i)
689  children_[i]->list(gnat, data);
690  }
691 
692  friend std::ostream &operator<<(std::ostream &out, const Node &node)
693  {
694  out << "\ndegree:\t" << node.degree_;
695  out << "\nminRadius:\t" << node.minRadius_;
696  out << "\nmaxRadius:\t" << node.maxRadius_;
697  out << "\nminRange:\t";
698  for (unsigned int i = 0; i < node.minRange_.size(); ++i)
699  out << node.minRange_[i] << '\t';
700  out << "\nmaxRange: ";
701  for (unsigned int i = 0; i < node.maxRange_.size(); ++i)
702  out << node.maxRange_[i] << '\t';
703  out << "\npivot:\t" << node.pivot_;
704  out << "\ndata: ";
705  for (unsigned int i = 0; i < node.data_.size(); ++i)
706  out << node.data_[i] << '\t';
707  out << "\nthis:\t" << &node;
708 #ifdef GNAT_SAMPLER
709  out << "\nsubtree size:\t" << node.subtreeSize_;
710  out << "\nactivity:\t" << node.activity_;
711 #endif
712  out << "\nchildren:\n";
713  for (unsigned int i = 0; i < node.children_.size(); ++i)
714  out << node.children_[i] << '\t';
715  out << '\n';
716  for (unsigned int i = 0; i < node.children_.size(); ++i)
717  out << *node.children_[i] << '\n';
718  return out;
719  }
720 
722  unsigned int degree_;
724  const _T pivot_;
726  double minRadius_;
728  double maxRadius_;
731  std::vector<double> minRange_;
734  std::vector<double> maxRange_;
737  std::vector<_T> data_;
740  std::vector<Node *> children_;
741 
743  mutable double distToPivot_;
744 
745 #ifdef GNAT_SAMPLER
746  unsigned int subtreeSize_;
752  int activity_;
753 #endif
754  };
755 
757  // another internal data structure is a priority queue of nodes to
758  // check next for possible nearest neighbors
759  struct NodeCompare
760  {
761  bool operator()(const Node *n0, const Node *n1) const
762  {
763  return (n0->distToPivot_ - n0->maxRadius_) > (n1->distToPivot_ - n1->maxRadius_);
764  }
765  };
766  using NodeQueue = std::priority_queue<Node *, std::vector<Node *>, NodeCompare>;
768 
770  Node *tree_{nullptr};
772  unsigned int degree_;
777  unsigned int minDegree_;
782  unsigned int maxDegree_;
785  unsigned int maxNumPtsPerLeaf_;
787  std::size_t size_{0};
790  std::size_t rebuildSize_;
794  std::size_t removedCacheSize_;
798  std::unordered_set<const _T *> removed_;
799 
803  mutable NearQueue nearQueue_;
805  mutable NodeQueue nodeQueue_;
809  mutable std::vector<unsigned int> pivots_;
813 
814 #ifdef GNAT_SAMPLER
815  double estimatedDimension_;
817 #endif
818  };
819 }
820 
821 #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:68
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:58
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...
friend std::ostream & operator<<(std::ostream &out, const NearestNeighborsGNATNoThreadSafety< _T > &gnat)
Print a GNAT structure (mostly useful for debugging purposes).
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.
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:81
boost::numeric::ublas::matrix< double > Matrix
A matrix type for storing distances between points and centers.
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.