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 
50 namespace ompl
51 {
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  typedef std::pair<const _T*,double> DataDist;
77  struct DataDistCompare
78  {
79  bool operator()(const DataDist& d0, const DataDist& d1)
80  {
81  return d0.second < d1.second;
82  }
83  };
84  typedef std::priority_queue<DataDist, std::vector<DataDist>, DataDistCompare> NearQueue;
85 
86  // another internal data structure is a priority queue of nodes to
87  // check next for possible nearest neighbors
88  class Node;
89  typedef std::pair<Node*,double> NodeDist;
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  typedef std::priority_queue<NodeDist, std::vector<NodeDist>, NodeDistCompare> NodeQueue;
99 
100  public:
101  NearestNeighborsGNAT(unsigned int degree = 8, unsigned int minDegree = 4,
102  unsigned int maxDegree = 12, unsigned int maxNumPtsPerLeaf = 50,
103  unsigned int removedCacheSize = 500, bool rebalancing = false
104 #ifdef GNAT_SAMPLER
105  , double estimatedDimension = 6.0
106 #endif
107  )
108  : NearestNeighbors<_T>(), tree_(nullptr), degree_(degree),
109  minDegree_(std::min(degree,minDegree)), maxDegree_(std::max(maxDegree,degree)),
110  maxNumPtsPerLeaf_(maxNumPtsPerLeaf), size_(0),
111  rebuildSize_(rebalancing ? maxNumPtsPerLeaf*degree : std::numeric_limits<std::size_t>::max()),
112  removedCacheSize_(removedCacheSize)
113 #ifdef GNAT_SAMPLER
114  , estimatedDimension_(estimatedDimension)
115 #endif
116  {
117  }
118 
119  virtual ~NearestNeighborsGNAT()
120  {
121  if (tree_)
122  delete tree_;
123  }
125  virtual void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction &distFun)
126  {
128  pivotSelector_.setDistanceFunction(distFun);
129  if (tree_)
131  }
132 
133  virtual void clear()
134  {
135  if (tree_)
136  {
137  delete tree_;
138  tree_ = nullptr;
139  }
140  size_ = 0;
141  removed_.clear();
142  if (rebuildSize_ != std::numeric_limits<std::size_t>::max())
144  }
145 
146  virtual bool reportsSortedResults() const
147  {
148  return true;
149  }
150 
151  virtual void add(const _T &data)
152  {
153  if (tree_)
154  {
155  if (isRemoved(data))
157  tree_->add(*this, data);
158  }
159  else
160  {
161  tree_ = new Node(degree_, maxNumPtsPerLeaf_, data);
162  size_ = 1;
163  }
164  }
165  virtual void add(const std::vector<_T> &data)
166  {
167  if (tree_)
169  else if (data.size()>0)
170  {
171  tree_ = new Node(degree_, maxNumPtsPerLeaf_, data[0]);
172 #ifdef GNAT_SAMPLER
173  tree_->subtreeSize_= data.size();
174 #endif
175  for (unsigned int i=1; i<data.size(); ++i)
176  tree_->data_.push_back(data[i]);
177  size_ += data.size();
178  if (tree_->needToSplit(*this))
179  tree_->split(*this);
180  }
181  }
184  {
185  std::vector<_T> lst;
186  list(lst);
187  clear();
188  add(lst);
189  }
195  virtual bool remove(const _T &data)
196  {
197  if (!size_) return false;
198  NearQueue nbhQueue;
199  // find data in tree
200  bool isPivot = nearestKInternal(data, 1, nbhQueue);
201  const _T *d = nbhQueue.top().first;
202  if (*d != data)
203  return false;
204  removed_.insert(d);
205  size_--;
206  // if we removed a pivot or if the capacity of removed elements
207  // has been reached, we rebuild the entire GNAT
208  if (isPivot || removed_.size() >= removedCacheSize_)
210  return true;
211  }
212 
213  virtual _T nearest(const _T &data) const
214  {
215  if (size_)
216  {
217  NearQueue nbhQueue;
218  nearestKInternal(data, 1, nbhQueue);
219  if (nbhQueue.size())
220  return *nbhQueue.top().first;
221  }
222  throw Exception("No elements found in nearest neighbors data structure");
223  }
224 
226  virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const
227  {
228  nbh.clear();
229  if (k == 0) return;
230  if (size_)
231  {
232  NearQueue nbhQueue;
233  nearestKInternal(data, k, nbhQueue);
234  postprocessNearest(nbhQueue, nbh);
235  }
236  }
237 
239  virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const
240  {
241  nbh.clear();
242  if (size_)
243  {
244  NearQueue nbhQueue;
245  nearestRInternal(data, radius, nbhQueue);
246  postprocessNearest(nbhQueue, nbh);
247  }
248  }
249 
250  virtual std::size_t size() const
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  virtual void list(std::vector<_T> &data) const
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 NearestNeighborsGNAT<_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) std::cout<<lst[j]<<'\t';
312  std::cout<<std::endl;
313  }
314  assert(i != lst.size());
315  }
316  // restore
317  removed_.swap(tmp);
318  // get elements in the tree with elements marked for removal purged from the list
319  list(lst);
320  if (lst.size() != size_)
321  std::cout << "#########################################\n" << *this << std::endl;
322  assert(lst.size() == size_);
323  }
324  protected:
326 
328  bool isRemoved(const _T &data) const
329  {
330  return !removed_.empty() && removed_.find(&data) != removed_.end();
331  }
332 
337  bool nearestKInternal(const _T &data, std::size_t k, NearQueue &nbhQueue) const
338  {
339  bool isPivot;
340  double dist;
341  NodeDist nodeDist;
342  NodeQueue nodeQueue;
343 
345  isPivot = tree_->insertNeighborK(nbhQueue, k, tree_->pivot_, data, dist);
346  tree_->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
347  while (nodeQueue.size() > 0)
348  {
349  dist = nbhQueue.top().second; // note the difference with nearestRInternal
350  nodeDist = nodeQueue.top();
351  nodeQueue.pop();
352  if (nbhQueue.size() == k &&
353  (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
354  nodeDist.second < nodeDist.first->minRadius_ - dist))
355  continue;
356  nodeDist.first->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
357  }
358  return isPivot;
359  }
361  void nearestRInternal(const _T &data, double radius, NearQueue &nbhQueue) const
362  {
363  double dist = radius; // note the difference with nearestKInternal
364  NodeQueue nodeQueue;
365  NodeDist nodeDist;
366 
367  tree_->insertNeighborR(nbhQueue, radius, tree_->pivot_,
369  tree_->nearestR(*this, data, radius, nbhQueue, nodeQueue);
370  while (nodeQueue.size() > 0)
371  {
372  nodeDist = nodeQueue.top();
373  nodeQueue.pop();
374  if (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
375  nodeDist.second < nodeDist.first->minRadius_ - dist)
376  continue;
377  nodeDist.first->nearestR(*this, data, radius, nbhQueue, nodeQueue);
378  }
379  }
382  void postprocessNearest(NearQueue& nbhQueue, std::vector<_T> &nbh) const
383  {
384  typename std::vector<_T>::reverse_iterator it;
385  nbh.resize(nbhQueue.size());
386  for (it=nbh.rbegin(); it!=nbh.rend(); it++, nbhQueue.pop())
387  *it = *nbhQueue.top().first;
388  }
389 
391  class Node
392  {
393  public:
396  Node(int degree, int capacity, const _T& pivot)
397  : degree_(degree), pivot_(pivot),
398  minRadius_(std::numeric_limits<double>::infinity()),
399  maxRadius_(-minRadius_), minRange_(degree, minRadius_),
400  maxRange_(degree, maxRadius_)
401 #ifdef GNAT_SAMPLER
402  , subtreeSize_(1), 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  std::vector<double> dist(children_.size());
470  double minDist = dist[0] = gnat.distFun_(data, children_[0]->pivot_);
471  int minInd = 0;
472 
473  for (unsigned int i=1; i<children_.size(); ++i)
474  if ((dist[i] = gnat.distFun_(data, children_[i]->pivot_)) < minDist)
475  {
476  minDist = dist[i];
477  minInd = i;
478  }
479  for (unsigned int i=0; i<children_.size(); ++i)
480  children_[i]->updateRange(minInd, dist[i]);
481  children_[minInd]->updateRadius(minDist);
482  children_[minInd]->add(gnat, data);
483  }
484  }
486  bool needToSplit(const GNAT &gnat) const
487  {
488  unsigned int sz = data_.size();
489  return sz > gnat.maxNumPtsPerLeaf_ && sz > degree_;
490  }
494  void split(GNAT &gnat)
495  {
496  typename GreedyKCenters<_T>::Matrix dists(data_.size(), degree_);
497  std::vector<unsigned int> pivots;
498 
499  children_.reserve(degree_);
500  gnat.pivotSelector_.kcenters(data_, degree_, pivots, dists);
501  for(unsigned int i=0; i<pivots.size(); i++)
502  children_.push_back(new Node(degree_, gnat.maxNumPtsPerLeaf_, data_[pivots[i]]));
503  degree_ = pivots.size(); // in case fewer than degree_ pivots were found
504  for (unsigned int j=0; j<data_.size(); ++j)
505  {
506  unsigned int k = 0;
507  for (unsigned int i=1; i<degree_; ++i)
508  if (dists(j, i) < dists(j, k))
509  k = i;
510  Node *child = children_[k];
511  if (j != pivots[k])
512  {
513  child->data_.push_back(data_[j]);
514  child->updateRadius(dists(j, k));
515  }
516  for (unsigned int i=0; i<degree_; ++i)
517  children_[i]->updateRange(k, dists(j, i));
518  }
519 
520  for (unsigned int i=0; i<degree_; ++i)
521  {
522  // make sure degree lies between minDegree_ and maxDegree_
523  children_[i]->degree_ = std::min(std::max(
524  (unsigned int) ((degree_ * children_[i]->data_.size()) / data_.size()),
525  gnat.minDegree_), 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 ||
552  (dist < std::numeric_limits<double>::epsilon() && data==key))
553  {
554  nbh.pop();
555  nbh.push(std::make_pair(&data, dist));
556  return true;
557  }
558  return false;
559  }
560 
566  void nearestK(const GNAT &gnat, const _T &data, std::size_t k,
567  NearQueue &nbh, NodeQueue &nodeQueue, bool &isPivot) const
568  {
569  for (unsigned int i=0; i<data_.size(); ++i)
570  if (!gnat.isRemoved(data_[i]))
571  {
572  if (insertNeighborK(nbh, k, data_[i], data, gnat.distFun_(data, data_[i])))
573  isPivot = false;
574  }
575  if (children_.size() > 0)
576  {
577  double dist;
578  Node *child;
579  std::vector<double> distToPivot(children_.size());
580  std::vector<int> permutation(children_.size());
581  for (unsigned int i=0; i<permutation.size(); ++i)
582  permutation[i] = i;
583  // for one-time use this is faster than using ompl::Permutation
584  std::random_shuffle(permutation.begin(), permutation.end());
585 
586  for (unsigned int i=0; i<children_.size(); ++i)
587  if (permutation[i] >= 0)
588  {
589  child = children_[permutation[i]];
590  distToPivot[permutation[i]] = gnat.distFun_(data, child->pivot_);
591  if (insertNeighborK(nbh, k, child->pivot_, data, distToPivot[permutation[i]]))
592  isPivot = true;
593  if (nbh.size()==k)
594  {
595  dist = nbh.top().second; // note difference with nearestR
596  for (unsigned int j=0; j<children_.size(); ++j)
597  if (permutation[j] >=0 && i != j &&
598  (distToPivot[permutation[i]] - dist > child->maxRange_[permutation[j]] ||
599  distToPivot[permutation[i]] + dist < child->minRange_[permutation[j]]))
600  permutation[j] = -1;
601  }
602  }
603 
604  dist = nbh.top().second;
605  for (unsigned int i=0; i<children_.size(); ++i)
606  if (permutation[i] >= 0)
607  {
608  child = children_[permutation[i]];
609  if (nbh.size()<k ||
610  (distToPivot[permutation[i]] - dist <= child->maxRadius_ &&
611  distToPivot[permutation[i]] + dist >= child->minRadius_))
612  nodeQueue.push(std::make_pair(child, distToPivot[permutation[i]]));
613  }
614  }
615  }
617  void insertNeighborR(NearQueue &nbh, double r, const _T &data, double dist) const
618  {
619  if (dist <= r)
620  nbh.push(std::make_pair(&data, dist));
621  }
625  void nearestR(const GNAT &gnat, const _T &data, double r, NearQueue &nbh, NodeQueue &nodeQueue) const
626  {
627  double dist = r; //note difference with nearestK
628 
629  for (unsigned int i=0; i<data_.size(); ++i)
630  if (!gnat.isRemoved(data_[i]))
631  insertNeighborR(nbh, r, data_[i], gnat.distFun_(data, data_[i]));
632  if (children_.size() > 0)
633  {
634  Node *child;
635  std::vector<double> distToPivot(children_.size());
636  std::vector<int> permutation(children_.size());
637  for (unsigned int i=0; i<permutation.size(); ++i)
638  permutation[i] = i;
639  // for one-time use this is faster than using ompl::Permutation
640  std::random_shuffle(permutation.begin(), permutation.end());
641 
642  for (unsigned int i=0; i<children_.size(); ++i)
643  if (permutation[i] >= 0)
644  {
645  child = children_[permutation[i]];
646  distToPivot[i] = gnat.distFun_(data, child->pivot_);
647  insertNeighborR(nbh, r, child->pivot_, distToPivot[i]);
648  for (unsigned int j=0; j<children_.size(); ++j)
649  if (permutation[j] >=0 && i != j &&
650  (distToPivot[i] - dist > child->maxRange_[permutation[j]] ||
651  distToPivot[i] + dist < child->minRange_[permutation[j]]))
652  permutation[j] = -1;
653  }
654 
655  for (unsigned int i=0; i<children_.size(); ++i)
656  if (permutation[i] >= 0)
657  {
658  child = children_[permutation[i]];
659  if (distToPivot[i] - dist <= child->maxRadius_ &&
660  distToPivot[i] + dist >= child->minRadius_)
661  nodeQueue.push(std::make_pair(child, distToPivot[i]));
662  }
663  }
664  }
665 
666 #ifdef GNAT_SAMPLER
667  double getSamplingWeight(const GNAT &gnat) const
668  {
669  double minR = std::numeric_limits<double>::max();
670  for(size_t i = 0; i<minRange_.size(); i++)
671  if(minRange_[i] < minR && minRange_[i] > 0.0)
672  minR = minRange_[i];
673  minR = std::max(minR, maxRadius_);
674  return std::pow(minR, gnat.estimatedDimension_) / (double) subtreeSize_;
675  }
676  const _T& sample(const GNAT &gnat, RNG &rng) const
677  {
678  if (children_.size() != 0)
679  {
680  if (rng.uniform01() < 1./(double) subtreeSize_)
681  return pivot_;
682  PDF<const Node*> distribution;
683  for(unsigned int i = 0; i < children_.size(); ++i)
684  distribution.add(children_[i], children_[i]->getSamplingWeight(gnat));
685  return distribution.sample(rng.uniform01())->sample(gnat, rng);
686  }
687  else
688  {
689  unsigned int i = rng.uniformInt(0, data_.size());
690  return (i==data_.size()) ? pivot_ : data_[i];
691  }
692  }
693 #endif
694 
695  void list(const GNAT &gnat, std::vector<_T> &data) const
696  {
697  if (!gnat.isRemoved(pivot_))
698  data.push_back(pivot_);
699  for (unsigned int i=0; i<data_.size(); ++i)
700  if(!gnat.isRemoved(data_[i]))
701  data.push_back(data_[i]);
702  for (unsigned int i=0; i<children_.size(); ++i)
703  children_[i]->list(gnat, data);
704  }
705 
706  friend std::ostream& operator<<(std::ostream &out, const Node &node)
707  {
708  out << "\ndegree:\t" << node.degree_;
709  out << "\nminRadius:\t" << node.minRadius_;
710  out << "\nmaxRadius:\t" << node.maxRadius_;
711  out << "\nminRange:\t";
712  for (unsigned int i=0; i<node.minRange_.size(); ++i)
713  out << node.minRange_[i] << '\t';
714  out << "\nmaxRange: ";
715  for (unsigned int i=0; i<node.maxRange_.size(); ++i)
716  out << node.maxRange_[i] << '\t';
717  out << "\npivot:\t" << node.pivot_;
718  out << "\ndata: ";
719  for (unsigned int i=0; i<node.data_.size(); ++i)
720  out << node.data_[i] << '\t';
721  out << "\nthis:\t" << &node;
722 #ifdef GNAT_SAMPLER
723  out << "\nsubtree size:\t" << node.subtreeSize_;
724  out << "\nactivity:\t" << node.activity_;
725 #endif
726  out << "\nchildren:\n";
727  for (unsigned int i=0; i<node.children_.size(); ++i)
728  out << node.children_[i] << '\t';
729  out << '\n';
730  for (unsigned int i=0; i<node.children_.size(); ++i)
731  out << *node.children_[i] << '\n';
732  return out;
733  }
734 
736  unsigned int degree_;
738  const _T pivot_;
740  double minRadius_;
742  double maxRadius_;
745  std::vector<double> minRange_;
748  std::vector<double> maxRange_;
751  std::vector<_T> data_;
754  std::vector<Node*> children_;
755 #ifdef GNAT_SAMPLER
756  unsigned int subtreeSize_;
762  int activity_;
763 #endif
764  };
765 
769  unsigned int degree_;
774  unsigned int minDegree_;
779  unsigned int maxDegree_;
782  unsigned int maxNumPtsPerLeaf_;
784  std::size_t size_;
787  std::size_t rebuildSize_;
791  std::size_t removedCacheSize_;
795  std::unordered_set<const _T*> removed_;
796 #ifdef GNAT_SAMPLER
797  double estimatedDimension_;
799 #endif
800  };
801 
802 }
803 
804 #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...
virtual std::size_t size() const
Get the number of elements in the datastructure.
virtual void nearestR(const _T &data, double radius, std::vector< _T > &nbh) const
Return the nearest neighbors within distance radius in sorted order.
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...
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.
const _T pivot_
Data element stored in this Node.
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...
virtual void setDistanceFunction(const typename NearestNeighbors< _T >::DistanceFunction &distFun)
Set the distance function to use.
void insertNeighborR(NearQueue &nbh, double r, const _T &data, double dist) const
Insert data in nbh if it is a near neighbor.
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 rebuildDataStructure()
Rebuild the internal data structure.
std::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
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...
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:69
unsigned int minDegree_
After splitting a Node, each child Node has degree equal to the default degree times the fraction of ...
std::unordered_set< const _T * > removed_
Cache of removed elements.
Main namespace. Contains everything in this library.
Definition: Cost.h:42
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
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...
virtual _T nearest(const _T &data) const
Get the nearest neighbor of a point.
virtual void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.
virtual void nearestK(const _T &data, std::size_t k, std::vector< _T > &nbh) const
Return the k nearest neighbors in sorted order.
virtual void list(std::vector< _T > &data) const
Get all the elements in the datastructure.
void nearestRInternal(const _T &data, double radius, NearQueue &nbhQueue) const
Return in nbhQueue the elements that are within distance radius of data.
virtual void add(const _T &data)
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.
virtual bool reportsSortedResults() const
Return true if the solutions reported by this data structure are sorted, when calling nearestK / near...
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.
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.
The exception type for ompl.
Definition: Exception.h:47
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
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 isRemoved(const _T &data) const
Return true iff data has been marked for removal.
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.
void postprocessNearest(NearQueue &nbhQueue, std::vector< _T > &nbh) const
Convert the internal data structure used for storing neighbors to the vector that NearestNeighbor API...
boost::numeric::ublas::matrix< double > Matrix
A matrix type for storing distances between points and centers.
Node * tree_
The data structure containing the elements stored in this structure.
virtual void clear()
Clear the datastructure.
The class used internally to define the GNAT.
std::vector< Node * > children_
The child nodes of this node. By definition, only internal nodes have child nodes.
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:82
unsigned int degree_
The desired degree of each node.
virtual void add(const std::vector< _T > &data)
Add a vector of points.
_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
std::size_t removedCacheSize_
Maximum number of removed elements that can be stored in the removed_ cache. If the cache is full...
Node(int degree, int capacity, const _T &pivot)
Construct a node of given degree with at most capacity data elements and with given pivot...
bool needToSplit(const GNAT &gnat) const
Return true iff the node needs to be split into child nodes.