NearestNeighborsGNAT.h
Go to the documentation of this file.
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 // This file is a slightly modified version of <ompl/datastructures/NearestNeighborsGNAT.h>
38 
39 #ifndef MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_NEAREST_NEIGHBORS_GNAT_
40 #define MOVEIT_ROS_PLANNING_CACHED_IK_KINEMATICS_NEAREST_NEIGHBORS_GNAT_
41 
42 #include "NearestNeighbors.h"
43 #include "GreedyKCenters.h"
45 #include <unordered_set>
46 #include <queue>
47 #include <algorithm>
48 #include <utility>
49 
51 {
68 template <typename _T>
70 {
71 protected:
72  // \cond IGNORE
73  // internally, we use a priority queue for nearest neighbors, paired
74  // with their distance to the query point
75  using DataDist = std::pair<const _T*, double>;
76  struct DataDistCompare
77  {
78  bool operator()(const DataDist& d0, const DataDist& d1)
79  {
80  return d0.second < d1.second;
81  }
82  };
83  using NearQueue = std::priority_queue<DataDist, std::vector<DataDist>, DataDistCompare>;
84 
85  // another internal data structure is a priority queue of nodes to
86  // check next for possible nearest neighbors
87  class Node;
88  using NodeDist = std::pair<Node*, double>;
89  struct NodeDistCompare
90  {
91  bool operator()(const NodeDist& n0, const NodeDist& n1) const
92  {
93  return (n0.second - n0.first->maxRadius_) > (n1.second - n1.first->maxRadius_);
94  }
95  };
96  using NodeQueue = std::priority_queue<NodeDist, std::vector<NodeDist>, NodeDistCompare>;
97  // \endcond
98 
99 public:
100  NearestNeighborsGNAT(unsigned int degree = 8, unsigned int minDegree = 4, unsigned int maxDegree = 12,
101  unsigned int maxNumPtsPerLeaf = 50, unsigned int removedCacheSize = 500,
102  bool rebalancing = false)
103  : NearestNeighbors<_T>()
104  , degree_(degree)
105  , minDegree_(std::min(degree, minDegree))
106  , maxDegree_(std::max(maxDegree, degree))
107  , maxNumPtsPerLeaf_(maxNumPtsPerLeaf)
108  , rebuildSize_(rebalancing ? maxNumPtsPerLeaf * degree : std::numeric_limits<std::size_t>::max())
109  , removedCacheSize_(removedCacheSize)
110  {
111  }
112 
114  {
115  if (tree_)
116  delete tree_;
117  }
118  // \brief Set the distance function to use
119  void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction& distFun) override
120  {
122  pivotSelector_.setDistanceFunction(distFun);
123  if (tree_)
125  }
126 
127  void clear() override
128  {
129  if (tree_)
130  {
131  delete tree_;
132  tree_ = nullptr;
133  }
134  size_ = 0;
135  removed_.clear();
136  if (rebuildSize_ != std::numeric_limits<std::size_t>::max())
138  }
139 
140  bool reportsSortedResults() const override
141  {
142  return true;
143  }
144 
145  void add(const _T& data) override
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  void add(const std::vector<_T>& data) override
160  {
161  if (tree_)
163  else if (!data.empty())
164  {
165  tree_ = new Node(degree_, maxNumPtsPerLeaf_, data[0]);
166  for (unsigned int i = 1; i < data.size(); ++i)
167  tree_->data_.push_back(data[i]);
168  size_ += data.size();
169  if (tree_->needToSplit(*this))
170  tree_->split(*this);
171  }
172  }
173  // \brief Rebuild the internal data structure.
175  {
176  std::vector<_T> lst;
177  list(lst);
178  clear();
179  add(lst);
180  }
181  // \brief Remove data from the tree.
182  // The element won't actually be removed immediately, but just marked
183  // for removal in the removed_ cache. When the cache is full, the tree
184  // will be rebuilt and the elements marked for removal will actually
185  // be removed.
186  bool remove(const _T& data) override
187  {
188  if (size_ == 0u)
189  return false;
190  NearQueue nbhQueue;
191  // find data in tree
192  bool isPivot = nearestKInternal(data, 1, nbhQueue);
193  const _T* d = nbhQueue.top().first;
194  if (*d != data)
195  return false;
196  removed_.insert(d);
197  size_--;
198  // if we removed a pivot or if the capacity of removed elements
199  // has been reached, we rebuild the entire GNAT
200  if (isPivot || removed_.size() >= removedCacheSize_)
202  return true;
203  }
204 
205  _T nearest(const _T& data) const override
206  {
207  if (size_)
208  {
209  NearQueue nbhQueue;
210  nearestKInternal(data, 1, nbhQueue);
211  if (!nbhQueue.empty())
212  return *nbhQueue.top().first;
213  }
214  throw moveit::Exception("No elements found in nearest neighbors data structure");
215  }
216 
217  // Return the k nearest neighbors in sorted order
218  void nearestK(const _T& data, std::size_t k, std::vector<_T>& nbh) const override
219  {
220  nbh.clear();
221  if (k == 0)
222  return;
223  if (size_)
224  {
225  NearQueue nbhQueue;
226  nearestKInternal(data, k, nbhQueue);
227  postprocessNearest(nbhQueue, nbh);
228  }
229  }
230 
231  // Return the nearest neighbors within distance \c radius in sorted order
232  void nearestR(const _T& data, double radius, std::vector<_T>& nbh) const override
233  {
234  nbh.clear();
235  if (size_)
236  {
237  NearQueue nbhQueue;
238  nearestRInternal(data, radius, nbhQueue);
239  postprocessNearest(nbhQueue, nbh);
240  }
241  }
242 
243  std::size_t size() const override
244  {
245  return size_;
246  }
247 
248  void list(std::vector<_T>& data) const override
249  {
250  data.clear();
251  data.reserve(size());
252  if (tree_)
253  tree_->list(*this, data);
254  }
255 
256  // \brief Print a GNAT structure (mostly useful for debugging purposes).
257  friend std::ostream& operator<<(std::ostream& out, const NearestNeighborsGNAT<_T>& gnat)
258  {
259  if (gnat.tree_)
260  {
261  out << *gnat.tree_;
262  if (!gnat.removed_.empty())
263  {
264  out << "Elements marked for removal:\n";
265  for (typename std::unordered_set<const _T*>::const_iterator it = gnat.removed_.begin();
266  it != gnat.removed_.end(); it++)
267  out << **it << '\t';
268  out << std::endl;
269  }
270  }
271  return out;
272  }
273 
274  // for debugging purposes
276  {
277  std::vector<_T> lst;
278  std::unordered_set<const _T*> tmp;
279  // get all elements, including those marked for removal
280  removed_.swap(tmp);
281  list(lst);
282  // check if every element marked for removal is also in the tree
283  for (typename std::unordered_set<const _T*>::iterator it = tmp.begin(); it != tmp.end(); it++)
284  {
285  unsigned int i;
286  for (i = 0; i < lst.size(); ++i)
287  if (lst[i] == **it)
288  break;
289  if (i == lst.size())
290  {
291  // an element marked for removal is not actually in the tree
292  std::cout << "***** FAIL!! ******\n" << *this << '\n';
293  for (unsigned int j = 0; j < lst.size(); ++j)
294  std::cout << lst[j] << '\t';
295  std::cout << std::endl;
296  }
297  assert(i != lst.size());
298  }
299  // restore
300  removed_.swap(tmp);
301  // get elements in the tree with elements marked for removal purged from the list
302  list(lst);
303  if (lst.size() != size_)
304  std::cout << "#########################################\n" << *this << std::endl;
305  assert(lst.size() == size_);
306  }
307 
308 protected:
310 
311  // Return true iff data has been marked for removal.
312  bool isRemoved(const _T& data) const
313  {
314  return !removed_.empty() && removed_.find(&data) != removed_.end();
315  }
316 
317  // \brief Return in nbhQueue the k nearest neighbors of data.
318  // For k=1, return true if the nearest neighbor is a pivot.
319  // (which is important during removal; removing pivots is a
320  // special case).
321  bool nearestKInternal(const _T& data, std::size_t k, NearQueue& nbhQueue) const
322  {
323  bool isPivot;
324  double dist;
325  NodeDist nodeDist;
326  NodeQueue nodeQueue;
327 
329  isPivot = tree_->insertNeighborK(nbhQueue, k, tree_->pivot_, data, dist);
330  tree_->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
331  while (!nodeQueue.empty())
332  {
333  dist = nbhQueue.top().second; // note the difference with nearestRInternal
334  nodeDist = nodeQueue.top();
335  nodeQueue.pop();
336  if (nbhQueue.size() == k &&
337  (nodeDist.second > nodeDist.first->maxRadius_ + dist || nodeDist.second < nodeDist.first->minRadius_ - dist))
338  continue;
339  nodeDist.first->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
340  }
341  return isPivot;
342  }
343  // \brief Return in nbhQueue the elements that are within distance radius of data.
344  void nearestRInternal(const _T& data, double radius, NearQueue& nbhQueue) const
345  {
346  double dist = radius; // note the difference with nearestKInternal
347  NodeQueue nodeQueue;
348  NodeDist nodeDist;
349 
351  tree_->nearestR(*this, data, radius, nbhQueue, nodeQueue);
352  while (!nodeQueue.empty())
353  {
354  nodeDist = nodeQueue.top();
355  nodeQueue.pop();
356  if (nodeDist.second > nodeDist.first->maxRadius_ + dist || nodeDist.second < nodeDist.first->minRadius_ - dist)
357  continue;
358  nodeDist.first->nearestR(*this, data, radius, nbhQueue, nodeQueue);
359  }
360  }
361  // \brief Convert the internal data structure used for storing neighbors
362  // to the vector that NearestNeighbor API requires.
363  void postprocessNearest(NearQueue& nbhQueue, std::vector<_T>& nbh) const
364  {
365  typename std::vector<_T>::reverse_iterator it;
366  nbh.resize(nbhQueue.size());
367  for (it = nbh.rbegin(); it != nbh.rend(); it++, nbhQueue.pop())
368  *it = *nbhQueue.top().first;
369  }
370 
371  // The class used internally to define the GNAT.
372  class Node
373  {
374  public:
375  // \brief Construct a node of given degree with at most
376  // \e capacity data elements and with given pivot.
377  Node(int degree, int capacity, _T pivot)
378  : degree_(degree)
379  , pivot_(std::move(pivot))
380  , minRadius_(std::numeric_limits<double>::infinity())
381  , maxRadius_(-minRadius_)
382  , minRange_(degree, minRadius_)
383  , maxRange_(degree, maxRadius_)
384  {
385  // The "+1" is needed because we add an element before we check whether to split
386  data_.reserve(capacity + 1);
387  }
388 
390  {
391  for (unsigned int i = 0; i < children_.size(); ++i)
392  delete children_[i];
393  }
394 
395  // \brief Update minRadius_ and maxRadius_, given that an element
396  // was added with distance dist to the pivot.
397  void updateRadius(double dist)
398  {
399  if (minRadius_ > dist)
400  minRadius_ = dist;
401 #ifndef GNAT_SAMPLER
402  if (maxRadius_ < dist)
403  maxRadius_ = dist;
404 #else
405  if (maxRadius_ < dist)
406  {
407  maxRadius_ = dist;
408  activity_ = 0;
409  }
410  else
411  activity_ = std::max(-32, activity_ - 1);
412 #endif
413  }
414  // \brief Update minRange_[i] and maxRange_[i], given that an
415  // element was added to the i-th child of the parent that has
416  // distance dist to this Node's pivot.
417  void updateRange(unsigned int i, double dist)
418  {
419  if (minRange_[i] > dist)
420  minRange_[i] = dist;
421  if (maxRange_[i] < dist)
422  maxRange_[i] = dist;
423  }
424  // Add an element to the tree rooted at this node.
425  void add(GNAT& gnat, const _T& data)
426  {
427  if (children_.empty())
428  {
429  data_.push_back(data);
430  gnat.size_++;
431  if (needToSplit(gnat))
432  {
433  if (!gnat.removed_.empty())
434  gnat.rebuildDataStructure();
435  else if (gnat.size_ >= gnat.rebuildSize_)
436  {
437  gnat.rebuildSize_ <<= 1;
438  gnat.rebuildDataStructure();
439  }
440  else
441  split(gnat);
442  }
443  }
444  else
445  {
446  std::vector<double> dist(children_.size());
447  double minDist = dist[0] = gnat.distFun_(data, children_[0]->pivot_);
448  int minInd = 0;
449 
450  for (unsigned int i = 1; i < children_.size(); ++i)
451  if ((dist[i] = gnat.distFun_(data, children_[i]->pivot_)) < minDist)
452  {
453  minDist = dist[i];
454  minInd = i;
455  }
456  for (unsigned int i = 0; i < children_.size(); ++i)
457  children_[i]->updateRange(minInd, dist[i]);
458  children_[minInd]->updateRadius(minDist);
459  children_[minInd]->add(gnat, data);
460  }
461  }
462  // Return true iff the node needs to be split into child nodes.
463  bool needToSplit(const GNAT& gnat) const
464  {
465  unsigned int sz = data_.size();
466  return sz > gnat.maxNumPtsPerLeaf_ && sz > degree_;
467  }
468  // \brief The split operation finds pivot elements for the child
469  // nodes and moves each data element of this node to the appropriate
470  // child node.
471  void split(GNAT& gnat)
472  {
473  typename GreedyKCenters<_T>::Matrix dists(data_.size(), degree_);
474  std::vector<unsigned int> pivots;
475 
476  children_.reserve(degree_);
477  gnat.pivotSelector_.kcenters(data_, degree_, pivots, dists);
478  for (unsigned int& pivot : pivots)
479  children_.push_back(new Node(degree_, gnat.maxNumPtsPerLeaf_, data_[pivot]));
480  degree_ = pivots.size(); // in case fewer than degree_ pivots were found
481  for (unsigned int j = 0; j < data_.size(); ++j)
482  {
483  unsigned int k = 0;
484  for (unsigned int i = 1; i < degree_; ++i)
485  if (dists(j, i) < dists(j, k))
486  k = i;
487  Node* child = children_[k];
488  if (j != pivots[k])
489  {
490  child->data_.push_back(data_[j]);
491  child->updateRadius(dists(j, k));
492  }
493  for (unsigned int i = 0; i < degree_; ++i)
494  children_[i]->updateRange(k, dists(j, i));
495  }
496 
497  for (unsigned int i = 0; i < degree_; ++i)
498  {
499  // make sure degree lies between minDegree_ and maxDegree_
500  children_[i]->degree_ =
501  std::min(std::max((unsigned int)((degree_ * children_[i]->data_.size()) / data_.size()), gnat.minDegree_),
502  gnat.maxDegree_);
503  // singleton
504  if (children_[i]->minRadius_ >= std::numeric_limits<double>::infinity())
505  children_[i]->minRadius_ = children_[i]->maxRadius_ = 0.;
506  }
507  // this does more than clear(); it also sets capacity to 0 and frees the memory
508  std::vector<_T> tmp;
509  data_.swap(tmp);
510  // check if new leaves need to be split
511  for (unsigned int i = 0; i < degree_; ++i)
512  if (children_[i]->needToSplit(gnat))
513  children_[i]->split(gnat);
514  }
515 
516  // Insert data in nbh if it is a near neighbor. Return true iff data was added to nbh.
517  bool insertNeighborK(NearQueue& nbh, std::size_t k, const _T& data, const _T& key, double dist) const
518  {
519  if (nbh.size() < k)
520  {
521  nbh.push(std::make_pair(&data, dist));
522  return true;
523  }
524  if (dist < nbh.top().second || (dist < std::numeric_limits<double>::epsilon() && data == key))
525  {
526  nbh.pop();
527  nbh.push(std::make_pair(&data, dist));
528  return true;
529  }
530  return false;
531  }
532 
533  // \brief Compute the k nearest neighbors of data in the tree.
534  // For k=1, isPivot is true if the nearest neighbor is a pivot
535  // (which is important during removal; removing pivots is a
536  // special case). The nodeQueue, which contains other Nodes
537  // that need to be checked for nearest neighbors, is updated.
538  void nearestK(const GNAT& gnat, const _T& data, std::size_t k, NearQueue& nbh, NodeQueue& nodeQueue,
539  bool& isPivot) const
540  {
541  for (unsigned int i = 0; i < data_.size(); ++i)
542  if (!gnat.isRemoved(data_[i]))
543  {
544  if (insertNeighborK(nbh, k, data_[i], data, gnat.distFun_(data, data_[i])))
545  isPivot = false;
546  }
547  if (!children_.empty())
548  {
549  double dist;
550  Node* child;
551  std::vector<double> distToPivot(children_.size());
552  std::vector<int> permutation(children_.size());
553  for (unsigned int i = 0; i < permutation.size(); ++i)
554  permutation[i] = i;
555  std::random_shuffle(permutation.begin(), permutation.end());
556 
557  for (unsigned int i = 0; i < children_.size(); ++i)
558  if (permutation[i] >= 0)
559  {
560  child = children_[permutation[i]];
561  distToPivot[permutation[i]] = gnat.distFun_(data, child->pivot_);
562  if (insertNeighborK(nbh, k, child->pivot_, data, distToPivot[permutation[i]]))
563  isPivot = true;
564  if (nbh.size() == k)
565  {
566  dist = nbh.top().second; // note difference with nearestR
567  for (unsigned int j = 0; j < children_.size(); ++j)
568  if (permutation[j] >= 0 && i != j &&
569  (distToPivot[permutation[i]] - dist > child->maxRange_[permutation[j]] ||
570  distToPivot[permutation[i]] + dist < child->minRange_[permutation[j]]))
571  permutation[j] = -1;
572  }
573  }
574 
575  dist = nbh.top().second;
576  for (unsigned int i = 0; i < children_.size(); ++i)
577  if (permutation[i] >= 0)
578  {
579  child = children_[permutation[i]];
580  if (nbh.size() < k || (distToPivot[permutation[i]] - dist <= child->maxRadius_ &&
581  distToPivot[permutation[i]] + dist >= child->minRadius_))
582  nodeQueue.push(std::make_pair(child, distToPivot[permutation[i]]));
583  }
584  }
585  }
586  // Insert data in nbh if it is a near neighbor.
587  void insertNeighborR(NearQueue& nbh, double r, const _T& data, double dist) const
588  {
589  if (dist <= r)
590  nbh.push(std::make_pair(&data, dist));
591  }
592  // \brief Return all elements that are within distance r in nbh.
593  // The nodeQueue, which contains other Nodes that need to
594  // be checked for nearest neighbors, is updated.
595  void nearestR(const GNAT& gnat, const _T& data, double r, NearQueue& nbh, NodeQueue& nodeQueue) const
596  {
597  double dist = r; // note difference with nearestK
598 
599  for (unsigned int i = 0; i < data_.size(); ++i)
600  if (!gnat.isRemoved(data_[i]))
601  insertNeighborR(nbh, r, data_[i], gnat.distFun_(data, data_[i]));
602  if (!children_.empty())
603  {
604  Node* child;
605  std::vector<double> distToPivot(children_.size());
606  std::vector<int> permutation(children_.size());
607  for (unsigned int i = 0; i < permutation.size(); ++i)
608  permutation[i] = i;
609  std::random_shuffle(permutation.begin(), permutation.end());
610 
611  for (unsigned int i = 0; i < children_.size(); ++i)
612  if (permutation[i] >= 0)
613  {
614  child = children_[permutation[i]];
615  distToPivot[i] = gnat.distFun_(data, child->pivot_);
616  insertNeighborR(nbh, r, child->pivot_, distToPivot[i]);
617  for (unsigned int j = 0; j < children_.size(); ++j)
618  if (permutation[j] >= 0 && i != j && (distToPivot[i] - dist > child->maxRange_[permutation[j]] ||
619  distToPivot[i] + dist < child->minRange_[permutation[j]]))
620  permutation[j] = -1;
621  }
622 
623  for (unsigned int i = 0; i < children_.size(); ++i)
624  if (permutation[i] >= 0)
625  {
626  child = children_[permutation[i]];
627  if (distToPivot[i] - dist <= child->maxRadius_ && distToPivot[i] + dist >= child->minRadius_)
628  nodeQueue.push(std::make_pair(child, distToPivot[i]));
629  }
630  }
631  }
632 
633  void list(const GNAT& gnat, std::vector<_T>& data) const
634  {
635  if (!gnat.isRemoved(pivot_))
636  data.push_back(pivot_);
637  for (unsigned int i = 0; i < data_.size(); ++i)
638  if (!gnat.isRemoved(data_[i]))
639  data.push_back(data_[i]);
640  for (unsigned int i = 0; i < children_.size(); ++i)
641  children_[i]->list(gnat, data);
642  }
643 
644  friend std::ostream& operator<<(std::ostream& out, const Node& node)
645  {
646  out << "\ndegree:\t" << node.degree_;
647  out << "\nminRadius:\t" << node.minRadius_;
648  out << "\nmaxRadius:\t" << node.maxRadius_;
649  out << "\nminRange:\t";
650  for (unsigned int i = 0; i < node.minRange_.size(); ++i)
651  out << node.minRange_[i] << '\t';
652  out << "\nmaxRange: ";
653  for (unsigned int i = 0; i < node.maxRange_.size(); ++i)
654  out << node.maxRange_[i] << '\t';
655  out << "\npivot:\t" << node.pivot_;
656  out << "\ndata: ";
657  for (unsigned int i = 0; i < node.data_.size(); ++i)
658  out << node.data_[i] << '\t';
659  out << "\nthis:\t" << &node;
660  out << "\nchildren:\n";
661  for (unsigned int i = 0; i < node.children_.size(); ++i)
662  out << node.children_[i] << '\t';
663  out << '\n';
664  for (unsigned int i = 0; i < node.children_.size(); ++i)
665  out << *node.children_[i] << '\n';
666  return out;
667  }
668 
669  // Number of child nodes
670  unsigned int degree_;
671  // Data element stored in this Node
672  const _T pivot_;
673  // Minimum distance between the pivot element and the elements stored in data_
674  double minRadius_;
675  // Maximum distance between the pivot element and the elements stored in data_
676  double maxRadius_;
677  // \brief The i-th element in minRange_ is the minimum distance between the
678  // pivot and any data_ element in the i-th child node of this node's parent.
679  std::vector<double> minRange_;
680  // \brief The i-th element in maxRange_ is the maximum distance between the
681  // pivot and any data_ element in the i-th child node of this node's parent.
682  std::vector<double> maxRange_;
683  // \brief The data elements stored in this node (in addition to the pivot
684  // element). An internal node has no elements stored in data_.
685  std::vector<_T> data_;
686  // \brief The child nodes of this node. By definition, only internal nodes
687  // have child nodes.
688  std::vector<Node*> children_;
689  };
690 
691  // \brief The data structure containing the elements stored in this structure.
692  Node* tree_{ nullptr };
693  // The desired degree of each node.
694  unsigned int degree_;
695  // \brief After splitting a Node, each child Node has degree equal to
696  // the default degree times the fraction of data elements from the
697  // original node that got assigned to that child Node. However, its
698  // degree can be no less than minDegree_.
699  unsigned int minDegree_;
700  // \brief After splitting a Node, each child Node has degree equal to
701  // the default degree times the fraction of data elements from the
702  // original node that got assigned to that child Node. However, its
703  // degree can be no larger than maxDegree_.
704  unsigned int maxDegree_;
705  // \brief Maximum number of elements allowed to be stored in a Node before
706  // it needs to be split into several nodes.
707  unsigned int maxNumPtsPerLeaf_;
708  // \brief Number of elements stored in the tree.
709  std::size_t size_{ 0 };
710  // \brief If size_ exceeds rebuildSize_, the tree will be rebuilt (and
711  // automatically rebalanced), and rebuildSize_ will be doubled.
712  std::size_t rebuildSize_;
713  // \brief Maximum number of removed elements that can be stored in the
714  // removed_ cache. If the cache is full, the tree will be rebuilt with
715  // the elements in removed_ actually removed from the tree.
716  std::size_t removedCacheSize_;
717  // \brief The data structure used to split data into subtrees.
719  // \brief Cache of removed elements.
720  std::unordered_set<const _T*> removed_;
721 };
722 }
723 
724 #endif
d
void clear() override
Clear the datastructure.
_T nearest(const _T &data) const override
Get the nearest neighbor of a point.
void add(const _T &data) override
Add an element to the datastructure.
void list(std::vector< _T > &data) const override
Get all the elements in the datastructure.
friend std::ostream & operator<<(std::ostream &out, const Node &node)
void insertNeighborR(NearQueue &nbh, double r, const _T &data, double dist) const
NearestNeighborsGNAT(unsigned int degree=8, unsigned int minDegree=4, unsigned int maxDegree=12, unsigned int maxNumPtsPerLeaf=50, unsigned int removedCacheSize=500, bool rebalancing=false)
std::size_t size() const override
Get the number of elements in the datastructure.
bool reportsSortedResults() const override
Return true if the solutions reported by this data structure are sorted, when calling nearestK / near...
An instance of this class can be used to greedily select a given number of representatives from a set...
void nearestR(const _T &data, double radius, std::vector< _T > &nbh) const override
Get the nearest neighbors of a point, within a specified radius.
void list(const GNAT &gnat, std::vector< _T > &data) const
virtual void add(const _T &data)=0
Add an element to the datastructure.
void nearestR(const GNAT &gnat, const _T &data, double r, NearQueue &nbh, NodeQueue &nodeQueue) const
bool insertNeighborK(NearQueue &nbh, std::size_t k, const _T &data, const _T &key, double dist) const
virtual void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.
void nearestK(const _T &data, std::size_t k, std::vector< _T > &nbh) const override
Get the k-nearest neighbors of a point.
Geometric Near-neighbor Access Tree (GNAT), a data structure for nearest neighbor search...
double min(double a, double b)
boost::numeric::ublas::matrix< double > Matrix
A matrix type for storing distances between points and centers.
bool nearestKInternal(const _T &data, std::size_t k, NearQueue &nbhQueue) const
Abstract representation of a container that can perform nearest neighbors queries.
void postprocessNearest(NearQueue &nbhQueue, std::vector< _T > &nbh) const
DistanceFunction distFun_
The used distance function.
void add(const std::vector< _T > &data) override
Add a vector of points.
void nearestRInternal(const _T &data, double radius, NearQueue &nbhQueue) const
double max(double a, double b)
void setDistanceFunction(const typename NearestNeighbors< _T >::DistanceFunction &distFun) override
void nearestK(const GNAT &gnat, const _T &data, std::size_t k, NearQueue &nbh, NodeQueue &nodeQueue, bool &isPivot) const
std::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:17:53