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