NearestNeighborsGNAT.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Mark Moll, Bryant Gipson */
36 
37 #ifndef OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_GNAT_
38 #define OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_GNAT_
39 
40 #include "ompl/datastructures/NearestNeighbors.h"
41 #include "ompl/datastructures/GreedyKCenters.h"
42 #ifdef GNAT_SAMPLER
43 #include "ompl/datastructures/PDF.h"
44 #endif
45 #include "ompl/util/Exception.h"
46 #include <unordered_set>
47 #include <queue>
48 #include <algorithm>
49 #include <utility>
50 
51 namespace ompl
52 {
69  template <typename _T>
71  {
72  protected:
74  // internally, we use a priority queue for nearest neighbors, paired
75  // with their distance to the query point
76  using DataDist = std::pair<const _T *, double>;
77  struct DataDistCompare
78  {
79  bool operator()(const DataDist &d0, const DataDist &d1)
80  {
81  return d0.second < d1.second;
82  }
83  };
84  using NearQueue = std::priority_queue<DataDist, std::vector<DataDist>, DataDistCompare>;
85 
86  // another internal data structure is a priority queue of nodes to
87  // check next for possible nearest neighbors
88  class Node;
89  using NodeDist = std::pair<Node *, double>;
90  struct NodeDistCompare
91  {
92  bool operator()(const NodeDist &n0, const NodeDist &n1) const
93  {
94  return (n0.second - n0.first->maxRadius_) > (n1.second - n1.first->maxRadius_);
95  }
96  };
97  using NodeQueue = std::priority_queue<NodeDist, std::vector<NodeDist>, NodeDistCompare>;
99 
100  public:
101  NearestNeighborsGNAT(unsigned int degree = 8, unsigned int minDegree = 4, unsigned int maxDegree = 12,
102  unsigned int maxNumPtsPerLeaf = 50, unsigned int removedCacheSize = 500,
103  bool rebalancing = false
104 #ifdef GNAT_SAMPLER
105  ,
106  double estimatedDimension = 6.0
107 #endif
108  )
110  , degree_(degree)
111  , minDegree_(std::min(degree, minDegree))
112  , maxDegree_(std::max(maxDegree, degree))
113  , maxNumPtsPerLeaf_(maxNumPtsPerLeaf)
114  , rebuildSize_(rebalancing ? maxNumPtsPerLeaf * degree : std::numeric_limits<std::size_t>::max())
115  , removedCacheSize_(removedCacheSize)
116 #ifdef GNAT_SAMPLER
117  , estimatedDimension_(estimatedDimension)
118 #endif
119  {
120  }
121 
122  ~NearestNeighborsGNAT() override
123  {
124  if (tree_)
125  delete tree_;
126  }
128  void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction &distFun) override
129  {
131  pivotSelector_.setDistanceFunction(distFun);
132  if (tree_)
134  }
135 
136  void clear() override
137  {
138  if (tree_)
139  {
140  delete tree_;
141  tree_ = nullptr;
142  }
143  size_ = 0;
144  removed_.clear();
145  if (rebuildSize_ != std::numeric_limits<std::size_t>::max())
147  }
148 
149  bool reportsSortedResults() const override
150  {
151  return true;
152  }
153 
154  void add(const _T &data) override
155  {
156  if (tree_)
157  {
158  if (isRemoved(data))
160  tree_->add(*this, data);
161  }
162  else
163  {
164  tree_ = new Node(degree_, maxNumPtsPerLeaf_, data);
165  size_ = 1;
166  }
167  }
168  void add(const std::vector<_T> &data) override
169  {
170  if (tree_)
172  else if (!data.empty())
173  {
174  tree_ = new Node(degree_, maxNumPtsPerLeaf_, data[0]);
175 #ifdef GNAT_SAMPLER
176  tree_->subtreeSize_ = data.size();
177 #endif
178  for (unsigned int i = 1; i < data.size(); ++i)
179  tree_->data_.push_back(data[i]);
180  size_ += data.size();
181  if (tree_->needToSplit(*this))
182  tree_->split(*this);
183  }
184  }
187  {
188  std::vector<_T> lst;
189  list(lst);
190  clear();
191  add(lst);
192  }
198  bool remove(const _T &data) override
199  {
200  if (size_ == 0u)
201  return false;
202  NearQueue nbhQueue;
203  // find data in tree
204  bool isPivot = nearestKInternal(data, 1, nbhQueue);
205  const _T *d = nbhQueue.top().first;
206  if (*d != data)
207  return false;
208  removed_.insert(d);
209  size_--;
210  // if we removed a pivot or if the capacity of removed elements
211  // has been reached, we rebuild the entire GNAT
212  if (isPivot || removed_.size() >= removedCacheSize_)
214  return true;
215  }
216 
217  _T nearest(const _T &data) const override
218  {
219  if (size_)
220  {
221  NearQueue nbhQueue;
222  nearestKInternal(data, 1, nbhQueue);
223  if (!nbhQueue.empty())
224  return *nbhQueue.top().first;
225  }
226  throw Exception("No elements found in nearest neighbors data structure");
227  }
228 
230  void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const override
231  {
232  nbh.clear();
233  if (k == 0)
234  return;
235  if (size_)
236  {
237  NearQueue nbhQueue;
238  nearestKInternal(data, k, nbhQueue);
239  postprocessNearest(nbhQueue, nbh);
240  }
241  }
242 
244  void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const override
245  {
246  nbh.clear();
247  if (size_)
248  {
249  NearQueue nbhQueue;
250  nearestRInternal(data, radius, nbhQueue);
251  postprocessNearest(nbhQueue, nbh);
252  }
253  }
254 
255  std::size_t size() const override
256  {
257  return size_;
258  }
259 
260 #ifdef GNAT_SAMPLER
261  const _T &sample(RNG &rng) const
263  {
264  if (!size())
265  throw Exception("Cannot sample from an empty tree");
266  else
267  return tree_->sample(*this, rng);
268  }
269 #endif
270 
271  void list(std::vector<_T> &data) const override
272  {
273  data.clear();
274  data.reserve(size());
275  if (tree_)
276  tree_->list(*this, data);
277  }
278 
280  friend std::ostream &operator<<(std::ostream &out, const NearestNeighborsGNAT<_T> &gnat)
281  {
282  if (gnat.tree_)
283  {
284  out << *gnat.tree_;
285  if (!gnat.removed_.empty())
286  {
287  out << "Elements marked for removal:\n";
288  for (typename std::unordered_set<const _T *>::const_iterator it = gnat.removed_.begin();
289  it != gnat.removed_.end(); it++)
290  out << **it << '\t';
291  out << std::endl;
292  }
293  }
294  return out;
295  }
296 
297  // for debugging purposes
298  void integrityCheck()
299  {
300  std::vector<_T> lst;
301  std::unordered_set<const _T *> tmp;
302  // get all elements, including those marked for removal
303  removed_.swap(tmp);
304  list(lst);
305  // check if every element marked for removal is also in the tree
306  for (typename std::unordered_set<const _T *>::iterator it = tmp.begin(); it != tmp.end(); it++)
307  {
308  unsigned int i;
309  for (i = 0; i < lst.size(); ++i)
310  if (lst[i] == **it)
311  break;
312  if (i == lst.size())
313  {
314  // an element marked for removal is not actually in the tree
315  std::cout << "***** FAIL!! ******\n" << *this << '\n';
316  for (unsigned int j = 0; j < lst.size(); ++j)
317  std::cout << lst[j] << '\t';
318  std::cout << std::endl;
319  }
320  assert(i != lst.size());
321  }
322  // restore
323  removed_.swap(tmp);
324  // get elements in the tree with elements marked for removal purged from the list
325  list(lst);
326  if (lst.size() != size_)
327  std::cout << "#########################################\n" << *this << std::endl;
328  assert(lst.size() == size_);
329  }
330 
331  protected:
333 
335  bool isRemoved(const _T &data) const
336  {
337  return !removed_.empty() && removed_.find(&data) != removed_.end();
338  }
339 
344  bool nearestKInternal(const _T &data, std::size_t k, NearQueue &nbhQueue) const
345  {
346  bool isPivot;
347  double dist;
348  NodeDist nodeDist;
349  NodeQueue nodeQueue;
350 
352  isPivot = tree_->insertNeighborK(nbhQueue, k, tree_->pivot_, data, dist);
353  tree_->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
354  while (!nodeQueue.empty())
355  {
356  dist = nbhQueue.top().second; // note the difference with nearestRInternal
357  nodeDist = nodeQueue.top();
358  nodeQueue.pop();
359  if (nbhQueue.size() == k && (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
360  nodeDist.second < nodeDist.first->minRadius_ - dist))
361  continue;
362  nodeDist.first->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
363  }
364  return isPivot;
365  }
367  void nearestRInternal(const _T &data, double radius, NearQueue &nbhQueue) const
368  {
369  double dist = radius; // note the difference with nearestKInternal
370  NodeQueue nodeQueue;
371  NodeDist nodeDist;
372 
373  tree_->insertNeighborR(nbhQueue, radius, tree_->pivot_,
375  tree_->nearestR(*this, data, radius, nbhQueue, nodeQueue);
376  while (!nodeQueue.empty())
377  {
378  nodeDist = nodeQueue.top();
379  nodeQueue.pop();
380  if (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
381  nodeDist.second < nodeDist.first->minRadius_ - dist)
382  continue;
383  nodeDist.first->nearestR(*this, data, radius, nbhQueue, nodeQueue);
384  }
385  }
388  void postprocessNearest(NearQueue &nbhQueue, std::vector<_T> &nbh) const
389  {
390  typename std::vector<_T>::reverse_iterator it;
391  nbh.resize(nbhQueue.size());
392  for (it = nbh.rbegin(); it != nbh.rend(); it++, nbhQueue.pop())
393  *it = *nbhQueue.top().first;
394  }
395 
397  class Node
398  {
399  public:
402  Node(int degree, int capacity, _T pivot)
403  : degree_(degree)
404  , pivot_(std::move(pivot))
405  , minRadius_(std::numeric_limits<double>::infinity())
406  , maxRadius_(-minRadius_)
407  , minRange_(degree, minRadius_)
408  , maxRange_(degree, maxRadius_)
409 #ifdef GNAT_SAMPLER
410  , subtreeSize_(1)
411  , activity_(0)
412 #endif
413  {
414  // The "+1" is needed because we add an element before we check whether to split
415  data_.reserve(capacity + 1);
416  }
417 
418  ~Node()
419  {
420  for (unsigned int i = 0; i < children_.size(); ++i)
421  delete children_[i];
422  }
423 
426  void updateRadius(double dist)
427  {
428  if (minRadius_ > dist)
429  minRadius_ = dist;
430 #ifndef GNAT_SAMPLER
431  if (maxRadius_ < dist)
432  maxRadius_ = dist;
433 #else
434  if (maxRadius_ < dist)
435  {
436  maxRadius_ = dist;
437  activity_ = 0;
438  }
439  else
440  activity_ = std::max(-32, activity_ - 1);
441 #endif
442  }
446  void updateRange(unsigned int i, double dist)
447  {
448  if (minRange_[i] > dist)
449  minRange_[i] = dist;
450  if (maxRange_[i] < dist)
451  maxRange_[i] = dist;
452  }
454  void add(GNAT &gnat, const _T &data)
455  {
456 #ifdef GNAT_SAMPLER
457  subtreeSize_++;
458 #endif
459  if (children_.empty())
460  {
461  data_.push_back(data);
462  gnat.size_++;
463  if (needToSplit(gnat))
464  {
465  if (!gnat.removed_.empty())
466  gnat.rebuildDataStructure();
467  else if (gnat.size_ >= gnat.rebuildSize_)
468  {
469  gnat.rebuildSize_ <<= 1;
470  gnat.rebuildDataStructure();
471  }
472  else
473  split(gnat);
474  }
475  }
476  else
477  {
478  std::vector<double> dist(children_.size());
479  double minDist = dist[0] = gnat.distFun_(data, children_[0]->pivot_);
480  int minInd = 0;
481 
482  for (unsigned int i = 1; i < children_.size(); ++i)
483  if ((dist[i] = gnat.distFun_(data, children_[i]->pivot_)) < minDist)
484  {
485  minDist = dist[i];
486  minInd = i;
487  }
488  for (unsigned int i = 0; i < children_.size(); ++i)
489  children_[i]->updateRange(minInd, dist[i]);
490  children_[minInd]->updateRadius(minDist);
491  children_[minInd]->add(gnat, data);
492  }
493  }
495  bool needToSplit(const GNAT &gnat) const
496  {
497  unsigned int sz = data_.size();
498  return sz > gnat.maxNumPtsPerLeaf_ && sz > degree_;
499  }
503  void split(GNAT &gnat)
504  {
505  typename GreedyKCenters<_T>::Matrix dists(data_.size(), degree_);
506  std::vector<unsigned int> pivots;
507 
508  children_.reserve(degree_);
509  gnat.pivotSelector_.kcenters(data_, degree_, pivots, dists);
510  for (unsigned int &pivot : pivots)
511  children_.push_back(new Node(degree_, gnat.maxNumPtsPerLeaf_, data_[pivot]));
512  degree_ = pivots.size(); // in case fewer than degree_ pivots were found
513  for (unsigned int j = 0; j < data_.size(); ++j)
514  {
515  unsigned int k = 0;
516  for (unsigned int i = 1; i < degree_; ++i)
517  if (dists(j, i) < dists(j, k))
518  k = i;
519  Node *child = children_[k];
520  if (j != pivots[k])
521  {
522  child->data_.push_back(data_[j]);
523  child->updateRadius(dists(j, k));
524  }
525  for (unsigned int i = 0; i < degree_; ++i)
526  children_[i]->updateRange(k, dists(j, i));
527  }
528 
529  for (unsigned int i = 0; i < degree_; ++i)
530  {
531  // make sure degree lies between minDegree_ and maxDegree_
532  children_[i]->degree_ =
533  std::min(std::max((unsigned int)((degree_ * children_[i]->data_.size()) / data_.size()),
534  gnat.minDegree_),
535  gnat.maxDegree_);
536  // singleton
537  if (children_[i]->minRadius_ >= std::numeric_limits<double>::infinity())
538  children_[i]->minRadius_ = children_[i]->maxRadius_ = 0.;
539 #ifdef GNAT_SAMPLER
540  // set subtree size
541  children_[i]->subtreeSize_ = children_[i]->data_.size() + 1;
542 #endif
543  }
544  // this does more than clear(); it also sets capacity to 0 and frees the memory
545  std::vector<_T> tmp;
546  data_.swap(tmp);
547  // check if new leaves need to be split
548  for (unsigned int i = 0; i < degree_; ++i)
549  if (children_[i]->needToSplit(gnat))
550  children_[i]->split(gnat);
551  }
552 
554  bool insertNeighborK(NearQueue &nbh, std::size_t k, const _T &data, const _T &key, double dist) const
555  {
556  if (nbh.size() < k)
557  {
558  nbh.push(std::make_pair(&data, dist));
559  return true;
560  }
561  if (dist < nbh.top().second || (dist < std::numeric_limits<double>::epsilon() && data == key))
562  {
563  nbh.pop();
564  nbh.push(std::make_pair(&data, dist));
565  return true;
566  }
567  return false;
568  }
569 
575  void nearestK(const GNAT &gnat, const _T &data, std::size_t k, NearQueue &nbh, NodeQueue &nodeQueue,
576  bool &isPivot) const
577  {
578  for (unsigned int i = 0; i < data_.size(); ++i)
579  if (!gnat.isRemoved(data_[i]))
580  {
581  if (insertNeighborK(nbh, k, data_[i], data, gnat.distFun_(data, data_[i])))
582  isPivot = false;
583  }
584  if (!children_.empty())
585  {
586  double dist;
587  Node *child;
588  std::vector<double> distToPivot(children_.size());
589  std::vector<int> permutation(children_.size());
590  for (unsigned int i = 0; i < permutation.size(); ++i)
591  permutation[i] = i;
592  // for one-time use this is faster than using ompl::Permutation
593  std::random_shuffle(permutation.begin(), permutation.end());
594 
595  for (unsigned int i = 0; i < children_.size(); ++i)
596  if (permutation[i] >= 0)
597  {
598  child = children_[permutation[i]];
599  distToPivot[permutation[i]] = gnat.distFun_(data, child->pivot_);
600  if (insertNeighborK(nbh, k, child->pivot_, data, distToPivot[permutation[i]]))
601  isPivot = true;
602  if (nbh.size() == k)
603  {
604  dist = nbh.top().second; // note difference with nearestR
605  for (unsigned int j = 0; j < children_.size(); ++j)
606  if (permutation[j] >= 0 && i != j &&
607  (distToPivot[permutation[i]] - dist > child->maxRange_[permutation[j]] ||
608  distToPivot[permutation[i]] + dist < child->minRange_[permutation[j]]))
609  permutation[j] = -1;
610  }
611  }
612 
613  dist = nbh.top().second;
614  for (unsigned int i = 0; i < children_.size(); ++i)
615  if (permutation[i] >= 0)
616  {
617  child = children_[permutation[i]];
618  if (nbh.size() < k || (distToPivot[permutation[i]] - dist <= child->maxRadius_ &&
619  distToPivot[permutation[i]] + dist >= child->minRadius_))
620  nodeQueue.push(std::make_pair(child, distToPivot[permutation[i]]));
621  }
622  }
623  }
625  void insertNeighborR(NearQueue &nbh, double r, const _T &data, double dist) const
626  {
627  if (dist <= r)
628  nbh.push(std::make_pair(&data, dist));
629  }
633  void nearestR(const GNAT &gnat, const _T &data, double r, NearQueue &nbh, NodeQueue &nodeQueue) const
634  {
635  double dist = r; // note difference with nearestK
636 
637  for (unsigned int i = 0; i < data_.size(); ++i)
638  if (!gnat.isRemoved(data_[i]))
639  insertNeighborR(nbh, r, data_[i], gnat.distFun_(data, data_[i]));
640  if (!children_.empty())
641  {
642  Node *child;
643  std::vector<double> distToPivot(children_.size());
644  std::vector<int> permutation(children_.size());
645  for (unsigned int i = 0; i < permutation.size(); ++i)
646  permutation[i] = i;
647  // for one-time use this is faster than using ompl::Permutation
648  std::random_shuffle(permutation.begin(), permutation.end());
649 
650  for (unsigned int i = 0; i < children_.size(); ++i)
651  if (permutation[i] >= 0)
652  {
653  child = children_[permutation[i]];
654  distToPivot[i] = gnat.distFun_(data, child->pivot_);
655  insertNeighborR(nbh, r, child->pivot_, distToPivot[i]);
656  for (unsigned int j = 0; j < children_.size(); ++j)
657  if (permutation[j] >= 0 && i != j &&
658  (distToPivot[i] - dist > child->maxRange_[permutation[j]] ||
659  distToPivot[i] + dist < child->minRange_[permutation[j]]))
660  permutation[j] = -1;
661  }
662 
663  for (unsigned int i = 0; i < children_.size(); ++i)
664  if (permutation[i] >= 0)
665  {
666  child = children_[permutation[i]];
667  if (distToPivot[i] - dist <= child->maxRadius_ &&
668  distToPivot[i] + dist >= child->minRadius_)
669  nodeQueue.push(std::make_pair(child, distToPivot[i]));
670  }
671  }
672  }
673 
674 #ifdef GNAT_SAMPLER
675  double getSamplingWeight(const GNAT &gnat) const
676  {
677  double minR = std::numeric_limits<double>::max();
678  for (auto minRange : minRange_)
679  if (minRange < minR && minRange > 0.0)
680  minR = minRange;
681  minR = std::max(minR, maxRadius_);
682  return std::pow(minR, gnat.estimatedDimension_) / (double)subtreeSize_;
683  }
684  const _T &sample(const GNAT &gnat, RNG &rng) const
685  {
686  if (children_.size() != 0)
687  {
688  if (rng.uniform01() < 1. / (double)subtreeSize_)
689  return pivot_;
690  PDF<const Node *> distribution;
691  for (const auto &child : children_)
692  distribution.add(child, child->getSamplingWeight(gnat));
693  return distribution.sample(rng.uniform01())->sample(gnat, rng);
694  }
695  else
696  {
697  unsigned int i = rng.uniformInt(0, data_.size());
698  return (i == data_.size()) ? pivot_ : data_[i];
699  }
700  }
701 #endif
702 
703  void list(const GNAT &gnat, std::vector<_T> &data) const
704  {
705  if (!gnat.isRemoved(pivot_))
706  data.push_back(pivot_);
707  for (unsigned int i = 0; i < data_.size(); ++i)
708  if (!gnat.isRemoved(data_[i]))
709  data.push_back(data_[i]);
710  for (unsigned int i = 0; i < children_.size(); ++i)
711  children_[i]->list(gnat, data);
712  }
713 
714  friend std::ostream &operator<<(std::ostream &out, const Node &node)
715  {
716  out << "\ndegree:\t" << node.degree_;
717  out << "\nminRadius:\t" << node.minRadius_;
718  out << "\nmaxRadius:\t" << node.maxRadius_;
719  out << "\nminRange:\t";
720  for (unsigned int i = 0; i < node.minRange_.size(); ++i)
721  out << node.minRange_[i] << '\t';
722  out << "\nmaxRange: ";
723  for (unsigned int i = 0; i < node.maxRange_.size(); ++i)
724  out << node.maxRange_[i] << '\t';
725  out << "\npivot:\t" << node.pivot_;
726  out << "\ndata: ";
727  for (unsigned int i = 0; i < node.data_.size(); ++i)
728  out << node.data_[i] << '\t';
729  out << "\nthis:\t" << &node;
730 #ifdef GNAT_SAMPLER
731  out << "\nsubtree size:\t" << node.subtreeSize_;
732  out << "\nactivity:\t" << node.activity_;
733 #endif
734  out << "\nchildren:\n";
735  for (unsigned int i = 0; i < node.children_.size(); ++i)
736  out << node.children_[i] << '\t';
737  out << '\n';
738  for (unsigned int i = 0; i < node.children_.size(); ++i)
739  out << *node.children_[i] << '\n';
740  return out;
741  }
742 
744  unsigned int degree_;
746  const _T pivot_;
748  double minRadius_;
750  double maxRadius_;
753  std::vector<double> minRange_;
756  std::vector<double> maxRange_;
759  std::vector<_T> data_;
762  std::vector<Node *> children_;
763 #ifdef GNAT_SAMPLER
764  unsigned int subtreeSize_;
770  int activity_;
771 #endif
772  };
773 
775  Node *tree_{nullptr};
777  unsigned int degree_;
782  unsigned int minDegree_;
787  unsigned int maxDegree_;
790  unsigned int maxNumPtsPerLeaf_;
792  std::size_t size_{0};
795  std::size_t rebuildSize_;
799  std::size_t removedCacheSize_;
803  std::unordered_set<const _T *> removed_;
804 #ifdef GNAT_SAMPLER
805  double estimatedDimension_;
807 #endif
808  };
809 }
810 
811 #endif
std::vector< double > maxRange_
The i-th element in maxRange_ is the maximum distance between the pivot and any data_ element in the ...
std::vector< _T > data_
The data elements stored in this node (in addition to the pivot element). An internal node has no ele...
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...
void updateRadius(double dist)
Update minRadius_ and maxRadius_, given that an element was added with distance dist to the pivot...
Node(int degree, int capacity, _T pivot)
Construct a node of given degree with at most capacity data elements and with given pivot...
An instance of this class can be used to greedily select a given number of representatives from a set...
void add(GNAT &gnat, const _T &data)
Add an element to the tree rooted at this node.
void nearestK(const GNAT &gnat, const _T &data, std::size_t k, NearQueue &nbh, NodeQueue &nodeQueue, bool &isPivot) const
Compute the k nearest neighbors of data in the tree. For k=1, isPivot is true if the nearest neighbor...
const _T pivot_
Data element stored in this Node.
std::vector< Node * > children_
The child nodes of this node. By definition, only internal nodes have child nodes.
STL namespace.
double minRadius_
Minimum distance between the pivot element and the elements stored in data_.
Geometric Near-neighbor Access Tree (GNAT), a data structure for nearest neighbor search...
void nearestR(const _T &data, double radius, std::vector< _T > &nbh) const override
Return the nearest neighbors within distance radius in sorted order.
void rebuildDataStructure()
Rebuild the internal data structure.
void nearestK(const _T &data, std::size_t k, std::vector< _T > &nbh) const override
Return the k nearest neighbors in sorted order.
bool needToSplit(const GNAT &gnat) const
Return true iff the node needs to be split into child nodes.
unsigned int maxDegree_
After splitting a Node, each child Node has degree equal to the default degree times the fraction of ...
void split(GNAT &gnat)
The split operation finds pivot elements for the child nodes and moves each data element of this node...
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:48
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:68
unsigned int minDegree_
After splitting a Node, each child Node has degree equal to the default degree times the fraction of ...
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
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
std::size_t size() const override
Get the number of elements in the datastructure.
virtual void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.
void list(std::vector< _T > &data) const override
Get all the elements in the datastructure.
void add(const _T &data) override
Add an element to the datastructure.
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...
GreedyKCenters< _T > pivotSelector_
The data structure used to split data into subtrees.
void insertNeighborR(NearQueue &nbh, double r, const _T &data, double dist) const
Insert data in nbh if it is a near neighbor.
DistanceFunction distFun_
The used distance function.
std::size_t rebuildSize_
If size_ exceeds rebuildSize_, the tree will be rebuilt (and automatically rebalanced), and rebuildSize_ will be doubled.
std::unordered_set< const _T * > removed_
Cache of removed elements.
friend std::ostream & operator<<(std::ostream &out, const NearestNeighborsGNAT< _T > &gnat)
Print a GNAT structure (mostly useful for debugging purposes).
Abstract representation of a container that can perform nearest neighbors queries.
void nearestR(const GNAT &gnat, const _T &data, double r, NearQueue &nbh, NodeQueue &nodeQueue) const
Return all elements that are within distance r in nbh. The nodeQueue, which contains other Nodes that...
void postprocessNearest(NearQueue &nbhQueue, std::vector< _T > &nbh) const
Convert the internal data structure used for storing neighbors to the vector that NearestNeighbor API...
The exception type for ompl.
Definition: Exception.h:46
void nearestRInternal(const _T &data, double radius, NearQueue &nbhQueue) const
Return in nbhQueue the elements that are within distance radius of data.
_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.
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
bool isRemoved(const _T &data) const
Return true iff data has been marked for removal.
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 degree_
Number of child nodes.
bool nearestKInternal(const _T &data, std::size_t k, NearQueue &nbhQueue) const
Return in nbhQueue the k nearest neighbors of data. For k=1, return true if the nearest neighbor is a...
_T nearest(const _T &data) const override
Get the nearest neighbor of a point.
void clear() override
Clear the datastructure.
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...
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.
Node * tree_
The data structure containing the elements stored in this structure.
The class used internally to define the GNAT.
double maxRadius_
Maximum distance between the pivot element and the elements stored in data_.
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
Definition: RandomNumbers.h:81
unsigned int degree_
The desired degree of each node.
boost::numeric::ublas::matrix< double > Matrix
A matrix type for storing distances between points and centers.
std::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
std::size_t removedCacheSize_
Maximum number of removed elements that can be stored in the removed_ cache. If the cache is full...