All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 <boost/unordered_set.hpp>
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 = 4, unsigned int minDegree = 2,
102  unsigned int maxDegree = 6, unsigned int maxNumPtsPerLeaf = 50,
103  unsigned int removedCacheSize = 50, bool rebalancing = false
104 #ifdef GNAT_SAMPLER
105  , double estimatedDimension = 6.0
106 #endif
107  )
108  : NearestNeighbors<_T>(), tree_(NULL), 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(void)
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  virtual void clear(void)
133  {
134  if (tree_)
135  {
136  delete tree_;
137  tree_ = NULL;
138  }
139  size_ = 0;
140  removed_.clear();
141  if (rebuildSize_ != std::numeric_limits<std::size_t>::max())
143  }
144 
145  virtual void add(const _T &data)
146  {
147  if (tree_)
148  {
149  if (isRemoved(data))
151  tree_->add(*this, data);
152  }
153  else
154  {
155  tree_ = new Node(degree_, maxNumPtsPerLeaf_, data);
156  size_ = 1;
157  }
158  }
159  virtual void add(const std::vector<_T> &data)
160  {
161  if (tree_)
163  else if (data.size()>0)
164  {
165  tree_ = new Node(degree_, maxNumPtsPerLeaf_, data[0]);
166 #ifdef GNAT_SAMPLER
167  tree_->subtreeSize_= data.size();
168 #endif
169  for (unsigned int i=1; i<data.size(); ++i)
170  tree_->data_.push_back(data[i]);
171  if (tree_->needToSplit(*this))
172  tree_->split(*this);
173  }
174  size_ += data.size();
175  }
178  {
179  std::vector<_T> lst;
180  list(lst);
181  clear();
182  add(lst);
183  }
189  virtual bool remove(const _T &data)
190  {
191  if (!size_) return false;
192  NearQueue nbhQueue;
193  // find data in tree
194  bool isPivot = nearestKInternal(data, 1, nbhQueue);
195  if (*nbhQueue.top().first != data)
196  return false;
197  removed_.insert(nbhQueue.top().first);
198  size_--;
199  // if we removed a pivot or if the capacity of removed elements
200  // has been reached, we rebuild the entire GNAT
201  if (isPivot || removed_.size()>=removedCacheSize_)
203  return true;
204  }
205 
206  virtual _T nearest(const _T &data) const
207  {
208  if (size_)
209  {
210  std::vector<_T> nbh;
211  nearestK(data, 1, nbh);
212  if (!nbh.empty()) return nbh[0];
213  }
214  throw Exception("No elements found in nearest neighbors data structure");
215  }
216 
218  virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const
219  {
220  nbh.clear();
221  if (k == 0) return;
222  if (size_)
223  {
224  NearQueue nbhQueue;
225  nearestKInternal(data, k, nbhQueue);
226  postprocessNearest(nbhQueue, nbh);
227  }
228  }
229 
231  virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const
232  {
233  nbh.clear();
234  if (size_)
235  {
236  NearQueue nbhQueue;
237  nearestRInternal(data, radius, nbhQueue);
238  postprocessNearest(nbhQueue, nbh);
239  }
240  }
241 
242  virtual std::size_t size(void) const
243  {
244  return size_;
245  }
246 
247 #ifdef GNAT_SAMPLER
248  const _T& sample(RNG& rng) const
250  {
251  if (!size())
252  throw Exception("Cannot sample from an empty tree");
253  else
254  return tree_->sample(*this, rng);
255  }
256 #endif
257 
258  virtual void list(std::vector<_T> &data) const
259  {
260  data.clear();
261  data.reserve(size());
262  if (tree_)
263  tree_->list(*this, data);
264  }
265 
267  friend std::ostream& operator<<(std::ostream& out, const NearestNeighborsGNAT<_T>& gnat)
268  {
269  if (gnat.tree_)
270  {
271  out << *gnat.tree_;
272  if (!gnat.removed_.empty())
273  {
274  out << "Elements marked for removal:\n";
275  for (typename boost::unordered_set<const _T*>::const_iterator it = gnat.removed_.begin();
276  it != gnat.removed_.end(); it++)
277  out << **it << '\t';
278  out << std::endl;
279  }
280  }
281  return out;
282  }
283 
284  // for debugging purposes
285  void integrityCheck()
286  {
287  std::vector<_T> lst;
288  boost::unordered_set<const _T*> tmp;
289  // get all elements, including those marked for removal
290  removed_.swap(tmp);
291  list(lst);
292  // check if every element marked for removal is also in the tree
293  for (typename boost::unordered_set<const _T*>::iterator it=tmp.begin(); it!=tmp.end(); it++)
294  {
295  unsigned int i;
296  for (i=0; i<lst.size(); ++i)
297  if (lst[i]==**it)
298  break;
299  if (i == lst.size())
300  {
301  // an element marked for removal is not actually in the tree
302  std::cout << "***** FAIL!! ******\n" << *this << '\n';
303  for (unsigned int j=0; j<lst.size(); ++j) std::cout<<lst[j]<<'\t';
304  std::cout<<std::endl;
305  }
306  assert(i != lst.size());
307  }
308  // restore
309  removed_.swap(tmp);
310  // get elements in the tree with elements marked for removal purged from the list
311  list(lst);
312  if (lst.size() != size_)
313  std::cout << "#########################################\n" << *this << std::endl;
314  assert(lst.size() == size_);
315  }
316  protected:
317  typedef NearestNeighborsGNAT<_T> GNAT;
318 
320  bool isRemoved(const _T& data) const
321  {
322  return !removed_.empty() && removed_.find(&data) != removed_.end();
323  }
324 
329  bool nearestKInternal(const _T &data, std::size_t k, NearQueue& nbhQueue) const
330  {
331  bool isPivot;
332  double dist;
333  NodeDist nodeDist;
334  NodeQueue nodeQueue;
335 
336  isPivot = tree_->insertNeighborK(nbhQueue, k, tree_->pivot_, data,
338  tree_->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
339  while (nodeQueue.size() > 0)
340  {
341  dist = nbhQueue.top().second; // note the difference with nearestRInternal
342  nodeDist = nodeQueue.top();
343  nodeQueue.pop();
344  if (nbhQueue.size() == k &&
345  (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
346  nodeDist.second < nodeDist.first->minRadius_ - dist))
347  break;
348  nodeDist.first->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
349  }
350  return isPivot;
351  }
353  void nearestRInternal(const _T &data, double radius, NearQueue& nbhQueue) const
354  {
355  double dist = radius; // note the difference with nearestKInternal
356  NodeQueue nodeQueue;
357  NodeDist nodeDist;
358 
359  tree_->insertNeighborR(nbhQueue, radius, tree_->pivot_,
361  tree_->nearestR(*this, data, radius, nbhQueue, nodeQueue);
362  while (nodeQueue.size() > 0)
363  {
364  nodeDist = nodeQueue.top();
365  nodeQueue.pop();
366  if (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
367  nodeDist.second < nodeDist.first->minRadius_ - dist)
368  break;
369  nodeDist.first->nearestR(*this, data, radius, nbhQueue, nodeQueue);
370  }
371  }
374  void postprocessNearest(NearQueue& nbhQueue, std::vector<_T> &nbh) const
375  {
376  typename std::vector<_T>::reverse_iterator it;
377  nbh.resize(nbhQueue.size());
378  for (it=nbh.rbegin(); it!=nbh.rend(); it++, nbhQueue.pop())
379  *it = *nbhQueue.top().first;
380  }
381 
383  class Node
384  {
385  public:
388  Node(int degree, int capacity, const _T& pivot)
389  : degree_(degree), pivot_(pivot),
390  minRadius_(std::numeric_limits<double>::infinity()),
392  maxRange_(degree, maxRadius_)
393 #ifdef GNAT_SAMPLER
394  , subtreeSize_(1), activity_(0)
395 #endif
396  {
397  // The "+1" is needed because we add an element before we check whether to split
398  data_.reserve(capacity+1);
399  }
400 
401  ~Node()
402  {
403  for (unsigned int i=0; i<children_.size(); ++i)
404  delete children_[i];
405  }
406 
409  void updateRadius(double dist)
410  {
411  if (minRadius_ > dist)
412  minRadius_ = dist;
413 #ifndef GNAT_SAMPLER
414  if (maxRadius_ < dist)
415  maxRadius_ = dist;
416 #else
417  if (maxRadius_ < dist)
418  {
419  maxRadius_ = dist;
420  activity_ = 0;
421  }
422  else
423  activity_ = std::max(-32, activity_ - 1);
424 #endif
425  }
429  void updateRange(unsigned int i, double dist)
430  {
431  if (minRange_[i] > dist)
432  minRange_[i] = dist;
433  if (maxRange_[i] < dist)
434  maxRange_[i] = dist;
435  }
437  void add(GNAT& gnat, const _T& data)
438  {
439 #ifdef GNAT_SAMPLER
440  subtreeSize_++;
441 #endif
442  if (children_.size()==0)
443  {
444  data_.push_back(data);
445  gnat.size_++;
446  if (needToSplit(gnat))
447  {
448  if (gnat.removed_.size() > 0)
449  gnat.rebuildDataStructure();
450  else if (gnat.size_ >= gnat.rebuildSize_)
451  {
452  gnat.rebuildSize_ <<= 1;
453  gnat.rebuildDataStructure();
454  }
455  else
456  split(gnat);
457  }
458  }
459  else
460  {
461  std::vector<double> dist(children_.size());
462  double minDist = dist[0] = gnat.distFun_(data, children_[0]->pivot_);
463  int minInd = 0;
464 
465  for (unsigned int i=1; i<children_.size(); ++i)
466  if ((dist[i] = gnat.distFun_(data, children_[i]->pivot_)) < minDist)
467  {
468  minDist = dist[i];
469  minInd = i;
470  }
471  for (unsigned int i=0; i<children_.size(); ++i)
472  children_[i]->updateRange(minInd, dist[i]);
473  children_[minInd]->updateRadius(minDist);
474  children_[minInd]->add(gnat, data);
475  }
476  }
478  bool needToSplit(const GNAT& gnat) const
479  {
480  unsigned int sz = data_.size();
481  return sz > gnat.maxNumPtsPerLeaf_ && sz > degree_;
482  }
486  void split(GNAT& gnat)
487  {
488  std::vector<std::vector<double> > dists;
489  std::vector<unsigned int> pivots;
490 
491  children_.reserve(degree_);
492  gnat.pivotSelector_.kcenters(data_, degree_, pivots, dists);
493  for(unsigned int i=0; i<pivots.size(); i++)
494  children_.push_back(new Node(degree_, gnat.maxNumPtsPerLeaf_, data_[pivots[i]]));
495  degree_ = pivots.size(); // in case fewer than degree_ pivots were found
496  for (unsigned int j=0; j<data_.size(); ++j)
497  {
498  unsigned int k = 0;
499  for (unsigned int i=1; i<degree_; ++i)
500  if (dists[j][i] < dists[j][k])
501  k = i;
502  Node* child = children_[k];
503  if (j != pivots[k])
504  {
505  child->data_.push_back(data_[j]);
506  child->updateRadius(dists[j][k]);
507  }
508  for (unsigned int i=0; i<degree_; ++i)
509  children_[i]->updateRange(k, dists[j][i]);
510  }
511 
512  for (unsigned int i=0; i<degree_; ++i)
513  {
514  // make sure degree lies between minDegree_ and maxDegree_
515  children_[i]->degree_ = std::min(std::max(
516  degree_ * (unsigned int)(children_[i]->data_.size() / data_.size()),
517  gnat.minDegree_), gnat.maxDegree_);
518  // singleton
519  if (children_[i]->minRadius_ >= std::numeric_limits<double>::infinity())
520  children_[i]->minRadius_ = children_[i]->maxRadius_ = 0.;
521 #ifdef GNAT_SAMPLER
522  // set subtree size
523  children_[i]->subtreeSize_ = children_[i]->data_.size() + 1;
524 #endif
525  }
526  // this does more than clear(); it also sets capacity to 0 and frees the memory
527  std::vector<_T> tmp;
528  data_.swap(tmp);
529  // check if new leaves need to be split
530  for (unsigned int i=0; i<degree_; ++i)
531  if (children_[i]->needToSplit(gnat))
532  children_[i]->split(gnat);
533  }
534 
536  bool insertNeighborK(NearQueue& nbh, std::size_t k, const _T& data, const _T& key, double dist) const
537  {
538  if (nbh.size() < k)
539  {
540  nbh.push(std::make_pair(&data, dist));
541  return true;
542  }
543  else if (dist < nbh.top().second ||
544  (dist < std::numeric_limits<double>::epsilon() && data==key))
545  {
546  nbh.pop();
547  nbh.push(std::make_pair(&data, dist));
548  return true;
549  }
550  return false;
551  }
552 
558  void nearestK(const GNAT& gnat, const _T &data, std::size_t k,
559  NearQueue& nbh, NodeQueue& nodeQueue, bool& isPivot) const
560  {
561  for (unsigned int i=0; i<data_.size(); ++i)
562  if (!gnat.isRemoved(data_[i]))
563  {
564  if (insertNeighborK(nbh, k, data_[i], data, gnat.distFun_(data, data_[i])))
565  isPivot = false;
566  }
567  if (children_.size() > 0)
568  {
569  double dist;
570  Node* child;
571  std::vector<double> distToPivot(children_.size());
572  std::vector<int> permutation(children_.size());
573 
574  for (unsigned int i=0; i<permutation.size(); ++i)
575  permutation[i] = i;
576  std::random_shuffle(permutation.begin(), permutation.end());
577 
578  for (unsigned int i=0; i<children_.size(); ++i)
579  if (permutation[i] >= 0)
580  {
581  child = children_[permutation[i]];
582  distToPivot[permutation[i]] = gnat.distFun_(data, child->pivot_);
583  if (insertNeighborK(nbh, k, child->pivot_, data, distToPivot[permutation[i]]))
584  isPivot = true;
585  if (nbh.size()==k)
586  {
587  dist = nbh.top().second; // note difference with nearestR
588  for (unsigned int j=0; j<children_.size(); ++j)
589  if (permutation[j] >=0 && i != j &&
590  (distToPivot[permutation[i]] - dist > child->maxRange_[permutation[j]] ||
591  distToPivot[permutation[i]] + dist < child->minRange_[permutation[j]]))
592  permutation[j] = -1;
593  }
594  }
595 
596  dist = nbh.top().second;
597  for (unsigned int i=0; i<children_.size(); ++i)
598  if (permutation[i] >= 0)
599  {
600  child = children_[permutation[i]];
601  if (nbh.size()<k ||
602  (distToPivot[permutation[i]] - dist <= child->maxRadius_ &&
603  distToPivot[permutation[i]] + dist >= child->minRadius_))
604  nodeQueue.push(std::make_pair(child, distToPivot[permutation[i]]));
605  }
606  }
607  }
609  void insertNeighborR(NearQueue& nbh, double r, const _T& data, double dist) const
610  {
611  if (dist <= r)
612  nbh.push(std::make_pair(&data, dist));
613  }
617  void nearestR(const GNAT& gnat, const _T &data, double r, NearQueue& nbh, NodeQueue& nodeQueue) const
618  {
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  std::vector<double> distToPivot(children_.size());
628  std::vector<int> permutation(children_.size());
629 
630  for (unsigned int i=0; i<permutation.size(); ++i)
631  permutation[i] = i;
632  std::random_shuffle(permutation.begin(), permutation.end());
633 
634  for (unsigned int i=0; i<children_.size(); ++i)
635  if (permutation[i] >= 0)
636  {
637  child = children_[permutation[i]];
638  distToPivot[i] = gnat.distFun_(data, child->pivot_);
639  insertNeighborR(nbh, r, child->pivot_, distToPivot[i]);
640  for (unsigned int j=0; j<children_.size(); ++j)
641  if (permutation[j] >=0 && i != j &&
642  (distToPivot[i] - dist > child->maxRange_[permutation[j]] ||
643  distToPivot[i] + dist < child->minRange_[permutation[j]]))
644  permutation[j] = -1;
645  }
646 
647  for (unsigned int i=0; i<children_.size(); ++i)
648  if (permutation[i] >= 0)
649  {
650  child = children_[permutation[i]];
651  if (distToPivot[i] - dist <= child->maxRadius_ &&
652  distToPivot[i] + dist >= child->minRadius_)
653  nodeQueue.push(std::make_pair(child, distToPivot[i]));
654  }
655  }
656  }
657 
658 #ifdef GNAT_SAMPLER
659  double getSamplingWeight(const GNAT& gnat) const
660  {
661  double minR = std::numeric_limits<double>::max();
662  for(size_t i = 0; i<minRange_.size(); i++)
663  if(minRange_[i] < minR && minRange_[i] > 0.0)
664  minR = minRange_[i];
665  minR = std::max(minR, maxRadius_);
666  return std::pow(minR, gnat.estimatedDimension_) / (double) subtreeSize_;
667  }
668  const _T& sample(const GNAT& gnat, RNG& rng) const
669  {
670  if (children_.size() != 0)
671  {
672  if (rng.uniform01() < 1./(double) subtreeSize_)
673  return pivot_;
674  PDF<const Node*> distribution;
675  for(unsigned int i = 0; i < children_.size(); ++i)
676  distribution.add(children_[i], children_[i]->getSamplingWeight(gnat));
677  return distribution.sample(rng.uniform01())->sample(gnat, rng);
678  }
679  else
680  {
681  unsigned int i = rng.uniformInt(0, data_.size());
682  return (i==data_.size()) ? pivot_ : data_[i];
683  }
684  }
685 #endif
686 
687  void list(const GNAT& gnat, std::vector<_T> &data) const
688  {
689  if (!gnat.isRemoved(pivot_))
690  data.push_back(pivot_);
691  for (unsigned int i=0; i<data_.size(); ++i)
692  if(!gnat.isRemoved(data_[i]))
693  data.push_back(data_[i]);
694  for (unsigned int i=0; i<children_.size(); ++i)
695  children_[i]->list(gnat, data);
696  }
697 
698  friend std::ostream& operator<<(std::ostream& out, const Node& node)
699  {
700  out << "\ndegree:\t" << node.degree_;
701  out << "\nminRadius:\t" << node.minRadius_;
702  out << "\nmaxRadius:\t" << node.maxRadius_;
703  out << "\nminRange:\t";
704  for (unsigned int i=0; i<node.minRange_.size(); ++i)
705  out << node.minRange_[i] << '\t';
706  out << "\nmaxRange: ";
707  for (unsigned int i=0; i<node.maxRange_.size(); ++i)
708  out << node.maxRange_[i] << '\t';
709  out << "\npivot:\t" << node.pivot_;
710  out << "\ndata: ";
711  for (unsigned int i=0; i<node.data_.size(); ++i)
712  out << node.data_[i] << '\t';
713  out << "\nthis:\t" << &node;
714 #ifdef GNAT_SAMPLER
715  out << "\nsubtree size:\t" << node.subtreeSize_;
716  out << "\nactivity:\t" << node.activity_;
717 #endif
718  out << "\nchildren:\n";
719  for (unsigned int i=0; i<node.children_.size(); ++i)
720  out << node.children_[i] << '\t';
721  out << '\n';
722  for (unsigned int i=0; i<node.children_.size(); ++i)
723  out << *node.children_[i] << '\n';
724  return out;
725  }
726 
728  unsigned int degree_;
730  const _T pivot_;
732  double minRadius_;
734  double maxRadius_;
737  std::vector<double> minRange_;
740  std::vector<double> maxRange_;
743  std::vector<_T> data_;
746  std::vector<Node*> children_;
747 #ifdef GNAT_SAMPLER
748  unsigned int subtreeSize_;
754  int activity_;
755 #endif
756  };
757 
761  unsigned int degree_;
766  unsigned int minDegree_;
771  unsigned int maxDegree_;
774  unsigned int maxNumPtsPerLeaf_;
776  std::size_t size_;
779  std::size_t rebuildSize_;
783  std::size_t removedCacheSize_;
787  boost::unordered_set<const _T*> removed_;
788 #ifdef GNAT_SAMPLER
789  double estimatedDimension_;
791 #endif
792  };
793 
794 }
795 
796 #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 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...
boost::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
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.
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.
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
unsigned int minDegree_
After splitting a Node, each child Node has degree equal to the default degree times the fraction of ...
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:54
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.
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.
Abstract representation of a container that can perform nearest neighbors queries.
virtual void clear(void)
Clear the datastructure.
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.
virtual std::size_t size(void) const
Get the number of elements in the datastructure.
boost::unordered_set< const _T * > removed_
Cache of removed elements.
double uniform01(void)
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:62
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...
Node * tree_
The data structure containing the elements stored in this structure.
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:75
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.