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  , tree_(nullptr)
111  , degree_(degree)
112  , minDegree_(std::min(degree, minDegree))
113  , maxDegree_(std::max(maxDegree, degree))
114  , maxNumPtsPerLeaf_(maxNumPtsPerLeaf)
115  , size_(0)
116  , rebuildSize_(rebalancing ? maxNumPtsPerLeaf * degree : std::numeric_limits<std::size_t>::max())
117  , removedCacheSize_(removedCacheSize)
118 #ifdef GNAT_SAMPLER
119  , estimatedDimension_(estimatedDimension)
120 #endif
121  {
122  }
123 
124  ~NearestNeighborsGNAT() override
125  {
126  if (tree_)
127  delete tree_;
128  }
130  void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction &distFun) override
131  {
133  pivotSelector_.setDistanceFunction(distFun);
134  if (tree_)
136  }
137 
138  void clear() override
139  {
140  if (tree_)
141  {
142  delete tree_;
143  tree_ = nullptr;
144  }
145  size_ = 0;
146  removed_.clear();
147  if (rebuildSize_ != std::numeric_limits<std::size_t>::max())
149  }
150 
151  bool reportsSortedResults() const override
152  {
153  return true;
154  }
155 
156  void add(const _T &data) override
157  {
158  if (tree_)
159  {
160  if (isRemoved(data))
162  tree_->add(*this, data);
163  }
164  else
165  {
166  tree_ = new Node(degree_, maxNumPtsPerLeaf_, data);
167  size_ = 1;
168  }
169  }
170  void add(const std::vector<_T> &data) override
171  {
172  if (tree_)
174  else if (data.size() > 0)
175  {
176  tree_ = new Node(degree_, maxNumPtsPerLeaf_, data[0]);
177 #ifdef GNAT_SAMPLER
178  tree_->subtreeSize_ = data.size();
179 #endif
180  for (unsigned int i = 1; i < data.size(); ++i)
181  tree_->data_.push_back(data[i]);
182  size_ += data.size();
183  if (tree_->needToSplit(*this))
184  tree_->split(*this);
185  }
186  }
189  {
190  std::vector<_T> lst;
191  list(lst);
192  clear();
193  add(lst);
194  }
200  bool remove(const _T &data) override
201  {
202  if (!size_)
203  return false;
204  NearQueue nbhQueue;
205  // find data in tree
206  bool isPivot = nearestKInternal(data, 1, nbhQueue);
207  const _T *d = nbhQueue.top().first;
208  if (*d != data)
209  return false;
210  removed_.insert(d);
211  size_--;
212  // if we removed a pivot or if the capacity of removed elements
213  // has been reached, we rebuild the entire GNAT
214  if (isPivot || removed_.size() >= removedCacheSize_)
216  return true;
217  }
218 
219  _T nearest(const _T &data) const override
220  {
221  if (size_)
222  {
223  NearQueue nbhQueue;
224  nearestKInternal(data, 1, nbhQueue);
225  if (nbhQueue.size())
226  return *nbhQueue.top().first;
227  }
228  throw Exception("No elements found in nearest neighbors data structure");
229  }
230 
232  void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const override
233  {
234  nbh.clear();
235  if (k == 0)
236  return;
237  if (size_)
238  {
239  NearQueue nbhQueue;
240  nearestKInternal(data, k, nbhQueue);
241  postprocessNearest(nbhQueue, nbh);
242  }
243  }
244 
246  void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const override
247  {
248  nbh.clear();
249  if (size_)
250  {
251  NearQueue nbhQueue;
252  nearestRInternal(data, radius, nbhQueue);
253  postprocessNearest(nbhQueue, nbh);
254  }
255  }
256 
257  std::size_t size() const override
258  {
259  return size_;
260  }
261 
262 #ifdef GNAT_SAMPLER
263  const _T &sample(RNG &rng) const
265  {
266  if (!size())
267  throw Exception("Cannot sample from an empty tree");
268  else
269  return tree_->sample(*this, rng);
270  }
271 #endif
272 
273  void list(std::vector<_T> &data) const override
274  {
275  data.clear();
276  data.reserve(size());
277  if (tree_)
278  tree_->list(*this, data);
279  }
280 
282  friend std::ostream &operator<<(std::ostream &out, const NearestNeighborsGNAT<_T> &gnat)
283  {
284  if (gnat.tree_)
285  {
286  out << *gnat.tree_;
287  if (!gnat.removed_.empty())
288  {
289  out << "Elements marked for removal:\n";
290  for (typename std::unordered_set<const _T *>::const_iterator it = gnat.removed_.begin();
291  it != gnat.removed_.end(); it++)
292  out << **it << '\t';
293  out << std::endl;
294  }
295  }
296  return out;
297  }
298 
299  // for debugging purposes
300  void integrityCheck()
301  {
302  std::vector<_T> lst;
303  std::unordered_set<const _T *> tmp;
304  // get all elements, including those marked for removal
305  removed_.swap(tmp);
306  list(lst);
307  // check if every element marked for removal is also in the tree
308  for (typename std::unordered_set<const _T *>::iterator it = tmp.begin(); it != tmp.end(); it++)
309  {
310  unsigned int i;
311  for (i = 0; i < lst.size(); ++i)
312  if (lst[i] == **it)
313  break;
314  if (i == lst.size())
315  {
316  // an element marked for removal is not actually in the tree
317  std::cout << "***** FAIL!! ******\n" << *this << '\n';
318  for (unsigned int j = 0; j < lst.size(); ++j)
319  std::cout << lst[j] << '\t';
320  std::cout << std::endl;
321  }
322  assert(i != lst.size());
323  }
324  // restore
325  removed_.swap(tmp);
326  // get elements in the tree with elements marked for removal purged from the list
327  list(lst);
328  if (lst.size() != size_)
329  std::cout << "#########################################\n" << *this << std::endl;
330  assert(lst.size() == size_);
331  }
332 
333  protected:
335 
337  bool isRemoved(const _T &data) const
338  {
339  return !removed_.empty() && removed_.find(&data) != removed_.end();
340  }
341 
346  bool nearestKInternal(const _T &data, std::size_t k, NearQueue &nbhQueue) const
347  {
348  bool isPivot;
349  double dist;
350  NodeDist nodeDist;
351  NodeQueue nodeQueue;
352 
354  isPivot = tree_->insertNeighborK(nbhQueue, k, tree_->pivot_, data, dist);
355  tree_->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
356  while (nodeQueue.size() > 0)
357  {
358  dist = nbhQueue.top().second; // note the difference with nearestRInternal
359  nodeDist = nodeQueue.top();
360  nodeQueue.pop();
361  if (nbhQueue.size() == k && (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
362  nodeDist.second < nodeDist.first->minRadius_ - dist))
363  continue;
364  nodeDist.first->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
365  }
366  return isPivot;
367  }
369  void nearestRInternal(const _T &data, double radius, NearQueue &nbhQueue) const
370  {
371  double dist = radius; // note the difference with nearestKInternal
372  NodeQueue nodeQueue;
373  NodeDist nodeDist;
374 
375  tree_->insertNeighborR(nbhQueue, radius, tree_->pivot_,
377  tree_->nearestR(*this, data, radius, nbhQueue, nodeQueue);
378  while (nodeQueue.size() > 0)
379  {
380  nodeDist = nodeQueue.top();
381  nodeQueue.pop();
382  if (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
383  nodeDist.second < nodeDist.first->minRadius_ - dist)
384  continue;
385  nodeDist.first->nearestR(*this, data, radius, nbhQueue, nodeQueue);
386  }
387  }
390  void postprocessNearest(NearQueue &nbhQueue, std::vector<_T> &nbh) const
391  {
392  typename std::vector<_T>::reverse_iterator it;
393  nbh.resize(nbhQueue.size());
394  for (it = nbh.rbegin(); it != nbh.rend(); it++, nbhQueue.pop())
395  *it = *nbhQueue.top().first;
396  }
397 
399  class Node
400  {
401  public:
404  Node(int degree, int capacity, _T pivot)
405  : degree_(degree)
406  , pivot_(std::move(pivot))
407  , minRadius_(std::numeric_limits<double>::infinity())
408  , maxRadius_(-minRadius_)
409  , minRange_(degree, minRadius_)
410  , maxRange_(degree, maxRadius_)
411 #ifdef GNAT_SAMPLER
412  , subtreeSize_(1)
413  , activity_(0)
414 #endif
415  {
416  // The "+1" is needed because we add an element before we check whether to split
417  data_.reserve(capacity + 1);
418  }
419 
420  ~Node()
421  {
422  for (unsigned int i = 0; i < children_.size(); ++i)
423  delete children_[i];
424  }
425 
428  void updateRadius(double dist)
429  {
430  if (minRadius_ > dist)
431  minRadius_ = dist;
432 #ifndef GNAT_SAMPLER
433  if (maxRadius_ < dist)
434  maxRadius_ = dist;
435 #else
436  if (maxRadius_ < dist)
437  {
438  maxRadius_ = dist;
439  activity_ = 0;
440  }
441  else
442  activity_ = std::max(-32, activity_ - 1);
443 #endif
444  }
448  void updateRange(unsigned int i, double dist)
449  {
450  if (minRange_[i] > dist)
451  minRange_[i] = dist;
452  if (maxRange_[i] < dist)
453  maxRange_[i] = dist;
454  }
456  void add(GNAT &gnat, const _T &data)
457  {
458 #ifdef GNAT_SAMPLER
459  subtreeSize_++;
460 #endif
461  if (children_.size() == 0)
462  {
463  data_.push_back(data);
464  gnat.size_++;
465  if (needToSplit(gnat))
466  {
467  if (gnat.removed_.size() > 0)
468  gnat.rebuildDataStructure();
469  else if (gnat.size_ >= gnat.rebuildSize_)
470  {
471  gnat.rebuildSize_ <<= 1;
472  gnat.rebuildDataStructure();
473  }
474  else
475  split(gnat);
476  }
477  }
478  else
479  {
480  std::vector<double> dist(children_.size());
481  double minDist = dist[0] = gnat.distFun_(data, children_[0]->pivot_);
482  int minInd = 0;
483 
484  for (unsigned int i = 1; i < children_.size(); ++i)
485  if ((dist[i] = gnat.distFun_(data, children_[i]->pivot_)) < minDist)
486  {
487  minDist = dist[i];
488  minInd = i;
489  }
490  for (unsigned int i = 0; i < children_.size(); ++i)
491  children_[i]->updateRange(minInd, dist[i]);
492  children_[minInd]->updateRadius(minDist);
493  children_[minInd]->add(gnat, data);
494  }
495  }
497  bool needToSplit(const GNAT &gnat) const
498  {
499  unsigned int sz = data_.size();
500  return sz > gnat.maxNumPtsPerLeaf_ && sz > degree_;
501  }
505  void split(GNAT &gnat)
506  {
507  typename GreedyKCenters<_T>::Matrix dists(data_.size(), degree_);
508  std::vector<unsigned int> pivots;
509 
510  children_.reserve(degree_);
511  gnat.pivotSelector_.kcenters(data_, degree_, pivots, dists);
512  for (unsigned int &pivot : pivots)
513  children_.push_back(new Node(degree_, gnat.maxNumPtsPerLeaf_, data_[pivot]));
514  degree_ = pivots.size(); // in case fewer than degree_ pivots were found
515  for (unsigned int j = 0; j < data_.size(); ++j)
516  {
517  unsigned int k = 0;
518  for (unsigned int i = 1; i < degree_; ++i)
519  if (dists(j, i) < dists(j, k))
520  k = i;
521  Node *child = children_[k];
522  if (j != pivots[k])
523  {
524  child->data_.push_back(data_[j]);
525  child->updateRadius(dists(j, k));
526  }
527  for (unsigned int i = 0; i < degree_; ++i)
528  children_[i]->updateRange(k, dists(j, i));
529  }
530 
531  for (unsigned int i = 0; i < degree_; ++i)
532  {
533  // make sure degree lies between minDegree_ and maxDegree_
534  children_[i]->degree_ =
535  std::min(std::max((unsigned int)((degree_ * children_[i]->data_.size()) / data_.size()),
536  gnat.minDegree_),
537  gnat.maxDegree_);
538  // singleton
539  if (children_[i]->minRadius_ >= std::numeric_limits<double>::infinity())
540  children_[i]->minRadius_ = children_[i]->maxRadius_ = 0.;
541 #ifdef GNAT_SAMPLER
542  // set subtree size
543  children_[i]->subtreeSize_ = children_[i]->data_.size() + 1;
544 #endif
545  }
546  // this does more than clear(); it also sets capacity to 0 and frees the memory
547  std::vector<_T> tmp;
548  data_.swap(tmp);
549  // check if new leaves need to be split
550  for (unsigned int i = 0; i < degree_; ++i)
551  if (children_[i]->needToSplit(gnat))
552  children_[i]->split(gnat);
553  }
554 
556  bool insertNeighborK(NearQueue &nbh, std::size_t k, const _T &data, const _T &key, double dist) const
557  {
558  if (nbh.size() < k)
559  {
560  nbh.push(std::make_pair(&data, dist));
561  return true;
562  }
563  else if (dist < nbh.top().second || (dist < std::numeric_limits<double>::epsilon() && data == key))
564  {
565  nbh.pop();
566  nbh.push(std::make_pair(&data, dist));
567  return true;
568  }
569  return false;
570  }
571 
577  void nearestK(const GNAT &gnat, const _T &data, std::size_t k, NearQueue &nbh, NodeQueue &nodeQueue,
578  bool &isPivot) const
579  {
580  for (unsigned int i = 0; i < data_.size(); ++i)
581  if (!gnat.isRemoved(data_[i]))
582  {
583  if (insertNeighborK(nbh, k, data_[i], data, gnat.distFun_(data, data_[i])))
584  isPivot = false;
585  }
586  if (children_.size() > 0)
587  {
588  double dist;
589  Node *child;
590  std::vector<double> distToPivot(children_.size());
591  std::vector<int> permutation(children_.size());
592  for (unsigned int i = 0; i < permutation.size(); ++i)
593  permutation[i] = i;
594  // for one-time use this is faster than using ompl::Permutation
595  std::random_shuffle(permutation.begin(), permutation.end());
596 
597  for (unsigned int i = 0; i < children_.size(); ++i)
598  if (permutation[i] >= 0)
599  {
600  child = children_[permutation[i]];
601  distToPivot[permutation[i]] = gnat.distFun_(data, child->pivot_);
602  if (insertNeighborK(nbh, k, child->pivot_, data, distToPivot[permutation[i]]))
603  isPivot = true;
604  if (nbh.size() == k)
605  {
606  dist = nbh.top().second; // note difference with nearestR
607  for (unsigned int j = 0; j < children_.size(); ++j)
608  if (permutation[j] >= 0 && i != j &&
609  (distToPivot[permutation[i]] - dist > child->maxRange_[permutation[j]] ||
610  distToPivot[permutation[i]] + dist < child->minRange_[permutation[j]]))
611  permutation[j] = -1;
612  }
613  }
614 
615  dist = nbh.top().second;
616  for (unsigned int i = 0; i < children_.size(); ++i)
617  if (permutation[i] >= 0)
618  {
619  child = children_[permutation[i]];
620  if (nbh.size() < k || (distToPivot[permutation[i]] - dist <= child->maxRadius_ &&
621  distToPivot[permutation[i]] + dist >= child->minRadius_))
622  nodeQueue.push(std::make_pair(child, distToPivot[permutation[i]]));
623  }
624  }
625  }
627  void insertNeighborR(NearQueue &nbh, double r, const _T &data, double dist) const
628  {
629  if (dist <= r)
630  nbh.push(std::make_pair(&data, dist));
631  }
635  void nearestR(const GNAT &gnat, const _T &data, double r, NearQueue &nbh, NodeQueue &nodeQueue) const
636  {
637  double dist = r; // note difference with nearestK
638 
639  for (unsigned int i = 0; i < data_.size(); ++i)
640  if (!gnat.isRemoved(data_[i]))
641  insertNeighborR(nbh, r, data_[i], gnat.distFun_(data, data_[i]));
642  if (children_.size() > 0)
643  {
644  Node *child;
645  std::vector<double> distToPivot(children_.size());
646  std::vector<int> permutation(children_.size());
647  for (unsigned int i = 0; i < permutation.size(); ++i)
648  permutation[i] = i;
649  // for one-time use this is faster than using ompl::Permutation
650  std::random_shuffle(permutation.begin(), permutation.end());
651 
652  for (unsigned int i = 0; i < children_.size(); ++i)
653  if (permutation[i] >= 0)
654  {
655  child = children_[permutation[i]];
656  distToPivot[i] = gnat.distFun_(data, child->pivot_);
657  insertNeighborR(nbh, r, child->pivot_, distToPivot[i]);
658  for (unsigned int j = 0; j < children_.size(); ++j)
659  if (permutation[j] >= 0 && i != j &&
660  (distToPivot[i] - dist > child->maxRange_[permutation[j]] ||
661  distToPivot[i] + dist < child->minRange_[permutation[j]]))
662  permutation[j] = -1;
663  }
664 
665  for (unsigned int i = 0; i < children_.size(); ++i)
666  if (permutation[i] >= 0)
667  {
668  child = children_[permutation[i]];
669  if (distToPivot[i] - dist <= child->maxRadius_ &&
670  distToPivot[i] + dist >= child->minRadius_)
671  nodeQueue.push(std::make_pair(child, distToPivot[i]));
672  }
673  }
674  }
675 
676 #ifdef GNAT_SAMPLER
677  double getSamplingWeight(const GNAT &gnat) const
678  {
679  double minR = std::numeric_limits<double>::max();
680  for (auto minRange : minRange_)
681  if (minRange < minR && minRange > 0.0)
682  minR = minRange;
683  minR = std::max(minR, maxRadius_);
684  return std::pow(minR, gnat.estimatedDimension_) / (double)subtreeSize_;
685  }
686  const _T &sample(const GNAT &gnat, RNG &rng) const
687  {
688  if (children_.size() != 0)
689  {
690  if (rng.uniform01() < 1. / (double)subtreeSize_)
691  return pivot_;
692  PDF<const Node *> distribution;
693  for (const auto &child : children_)
694  distribution.add(child, child->getSamplingWeight(gnat));
695  return distribution.sample(rng.uniform01())->sample(gnat, rng);
696  }
697  else
698  {
699  unsigned int i = rng.uniformInt(0, data_.size());
700  return (i == data_.size()) ? pivot_ : data_[i];
701  }
702  }
703 #endif
704 
705  void list(const GNAT &gnat, std::vector<_T> &data) const
706  {
707  if (!gnat.isRemoved(pivot_))
708  data.push_back(pivot_);
709  for (unsigned int i = 0; i < data_.size(); ++i)
710  if (!gnat.isRemoved(data_[i]))
711  data.push_back(data_[i]);
712  for (unsigned int i = 0; i < children_.size(); ++i)
713  children_[i]->list(gnat, data);
714  }
715 
716  friend std::ostream &operator<<(std::ostream &out, const Node &node)
717  {
718  out << "\ndegree:\t" << node.degree_;
719  out << "\nminRadius:\t" << node.minRadius_;
720  out << "\nmaxRadius:\t" << node.maxRadius_;
721  out << "\nminRange:\t";
722  for (unsigned int i = 0; i < node.minRange_.size(); ++i)
723  out << node.minRange_[i] << '\t';
724  out << "\nmaxRange: ";
725  for (unsigned int i = 0; i < node.maxRange_.size(); ++i)
726  out << node.maxRange_[i] << '\t';
727  out << "\npivot:\t" << node.pivot_;
728  out << "\ndata: ";
729  for (unsigned int i = 0; i < node.data_.size(); ++i)
730  out << node.data_[i] << '\t';
731  out << "\nthis:\t" << &node;
732 #ifdef GNAT_SAMPLER
733  out << "\nsubtree size:\t" << node.subtreeSize_;
734  out << "\nactivity:\t" << node.activity_;
735 #endif
736  out << "\nchildren:\n";
737  for (unsigned int i = 0; i < node.children_.size(); ++i)
738  out << node.children_[i] << '\t';
739  out << '\n';
740  for (unsigned int i = 0; i < node.children_.size(); ++i)
741  out << *node.children_[i] << '\n';
742  return out;
743  }
744 
746  unsigned int degree_;
748  const _T pivot_;
750  double minRadius_;
752  double maxRadius_;
755  std::vector<double> minRange_;
758  std::vector<double> maxRange_;
761  std::vector<_T> data_;
764  std::vector<Node *> children_;
765 #ifdef GNAT_SAMPLER
766  unsigned int subtreeSize_;
772  int activity_;
773 #endif
774  };
775 
779  unsigned int degree_;
784  unsigned int minDegree_;
789  unsigned int maxDegree_;
792  unsigned int maxNumPtsPerLeaf_;
794  std::size_t size_;
797  std::size_t rebuildSize_;
801  std::size_t removedCacheSize_;
805  std::unordered_set<const _T *> removed_;
806 #ifdef GNAT_SAMPLER
807  double estimatedDimension_;
809 #endif
810  };
811 }
812 
813 #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...