Robotics Library  0.7.0
KdtreeNearestNeighbors.h
Go to the documentation of this file.
1 //
2 // Copyright (c) 2009, Markus Rickert
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright notice,
11 // this list of conditions and the following disclaimer in the documentation
12 // and/or other materials provided with the distribution.
13 //
14 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
15 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
18 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
19 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
20 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
21 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
22 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
23 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
24 // POSSIBILITY OF SUCH DAMAGE.
25 //
26 
27 #ifndef RL_MATH_KDTREENEARESTNEIGHBORS_H
28 #define RL_MATH_KDTREENEARESTNEIGHBORS_H
29 
30 #include <algorithm>
31 #include <array>
32 #include <cmath>
33 #include <iterator>
34 #include <memory>
35 #include <numeric>
36 #include <type_traits>
37 #include <utility>
38 #include <vector>
39 #include <boost/optional.hpp>
40 
41 namespace rl
42 {
43  namespace math
44  {
54  template<typename MetricT>
56  {
57  private:
58  struct Node;
59 
60  public:
61  typedef const typename MetricT::Value& const_reference;
62 
63  typedef ::std::ptrdiff_t difference_type;
64 
65  typedef typename MetricT::Value& reference;
66 
67  typedef ::std::size_t size_type;
68 
69  typedef typename MetricT::Value value_type;
70 
71  typedef typename MetricT::Distance Distance;
72 
73  typedef MetricT Metric;
74 
75  typedef typename MetricT::Size Size;
76 
77  typedef typename MetricT::Value Value;
78 
79  typedef ::std::pair<Distance, Value> Neighbor;
80 
82  checks(),
83  mean(),
84  metric(metric),
85  root(),
86  samples(100),
87  values(0),
88  var()
89  {
90  }
91 
93  checks(),
94  mean(),
95  metric(::std::move(metric)),
96  root(),
97  samples(100),
98  values(0),
99  var()
100  {
101  }
102 
103  template<typename InputIterator>
104  KdtreeNearestNeighbors(InputIterator first, InputIterator last, const Metric& metric) :
105  checks(),
106  mean(),
107  metric(metric),
108  root(),
109  samples(100),
110  values(0),
111  var()
112  {
113  this->insert(first, last);
114  }
115 
116  template<typename InputIterator>
117  KdtreeNearestNeighbors(InputIterator first, InputIterator last, Metric&& metric = Metric()) :
118  checks(),
119  mean(),
120  metric(::std::move(metric)),
121  root(),
122  samples(100),
123  values(0),
124  var()
125  {
126  this->insert(first, last);
127  }
128 
130  {
131  }
132 
133  void clear()
134  {
135  this->root.children[0].reset(nullptr);
136  this->root.children[1].reset(nullptr);
137  this->root.data.reset();
138  this->values = 0;
139  }
140 
141  ::std::vector<Value> data() const
142  {
143  ::std::vector<Value> data;
144  data.reserve(this->values);
145  this->data(this->root, data);
146  return data;
147  }
148 
149  bool empty() const
150  {
151  return !this->root.data && nullptr == this->root.children[0] && nullptr == this->root.children[1];
152  }
153 
154  ::boost::optional< ::std::size_t> getChecks() const
155  {
156  return this->checks;
157  }
158 
159  ::std::size_t getSamples() const
160  {
161  return this->samples;
162  }
163 
164  template<typename InputIterator>
165  void insert(InputIterator first, InputIterator last)
166  {
167  if (this->empty())
168  {
169  ::std::size_t size = ::std::distance(first, last);
170 
171  if (size > 1)
172  {
173  this->divide(this->root, first, last);
174  }
175  else
176  {
177  this->root.data = *first;
178  }
179 
180  this->values += ::std::distance(first, last);
181  }
182  else
183  {
184  for (InputIterator i = first; i != last; ++i)
185  {
186  this->push(*i);
187  }
188  }
189  }
190 
191  ::std::vector<Neighbor> nearest(const Value& query, const ::std::size_t& k, const bool& sorted = true) const
192  {
193  return this->search(query, &k, nullptr, sorted);
194  }
195 
196  void push(const Value& value)
197  {
198  this->push(this->root, value);
199  ++this->values;
200  }
201 
202  ::std::vector<Neighbor> radius(const Value& query, const Distance& radius, const bool& sorted = true) const
203  {
204  return this->search(query, nullptr, &radius, sorted);
205  }
206 
207  void setChecks(const ::boost::optional< ::std::size_t>& checks)
208  {
209  this->checks = checks;
210  }
211 
212  void setSamples(const ::std::size_t& samples)
213  {
214  this->samples = samples;
215  }
216 
217  ::std::size_t size() const
218  {
219  return this->values;
220  }
221 
223  {
225  swap(this->mean, other.mean);
226  swap(this->metric, other.metric);
227  swap(this->samples, other.samples);
228  swap(this->root, other.root);
229  swap(this->values, other.values);
230  swap(this->var, other.var);
231  }
232 
234  {
235  lhs.swap(rhs);
236  }
237 
238  protected:
239 
240  private:
241  struct Branch
242  {
243  Branch(Distance& dist, ::std::vector<Distance>& sidedist, const Node* node) :
244  dist(dist),
245  node(node),
247  {
248  }
249 
251 
252  const Node* node;
253 
254  ::std::vector<Distance> sidedist;
255  };
256 
258  {
259  bool operator()(const Branch& lhs, const Branch& rhs) const
260  {
261  return lhs.dist > rhs.dist;
262  }
263  };
264 
265  struct Cut
266  {
267  bool operator()(const Value& other) const
268  {
269  using ::std::begin;
270  return *(begin(other) + this->index) < this->value;
271  }
272 
273  Size index;
274 
276  };
277 
279  {
280  bool operator()(const Neighbor& lhs, const Neighbor& rhs) const
281  {
282  return lhs.first < rhs.first;
283  }
284  };
285 
286  struct Node
287  {
288  Node() :
289  children(),
290  cut(),
291  data()
292  {
293  }
294 
296  {
297  }
298 
299  void swap(Node& other)
300  {
302  swap(this->children, other.children);
303  swap(this->cut, other.cut);
304  swap(this->data, other.data);
305  }
306 
307  friend void swap(Node& lhs, Node& rhs)
308  {
309  lhs.swap(rhs);
310  }
311 
312  ::std::array< ::std::unique_ptr<Node>, 2> children;
313 
315 
316  ::boost::optional<Value> data;
317  };
318 
319  void data(const Node& node, ::std::vector<Value>& data) const
320  {
321  data.push_back(*node.data);
322 
323  for (::std::size_t i = 0; i < node.children.size(); ++i)
324  {
325  this->data(*node.children[i], data);
326  }
327  }
328 
329  template<typename InputIterator>
330  void divide(Node& node, InputIterator first, InputIterator last)
331  {
332  node.cut = this->select(first, last);
333  InputIterator split = ::std::partition(first, last, node.cut);
334 
335  for (::std::size_t i = 0; i < 2; ++i)
336  {
337 #if __cplusplus > 201103L || _MSC_VER >= 1800
338  node.children[i] = ::std::make_unique<Node>();
339 #else
340  node.children[i].reset(new Node());
341 #endif
342 
343  InputIterator begin = 0 == i ? first : split;
344  InputIterator end = 0 == i ? split : last;
345 
346  if (::std::distance(begin, end) > 1)
347  {
348  this->divide(*node.children[i], begin, end);
349  }
350  else
351  {
352  node.children[i]->data = *begin;
353  }
354  }
355  }
356 
357  void push(Node& node, const Value& value)
358  {
359  using ::std::begin;
360  using ::std::size;
361 
362  if (nullptr == node.children[0] && nullptr == node.children[1] && !node.data)
363  {
364  node.data = value;
365  }
366  else if (nullptr == node.children[0] && nullptr == node.children[1])
367  {
368  ::std::size_t dim = size(value);
369  Distance max = Distance();
370 
371  for (::std::size_t i = 0; i < dim; ++i)
372  {
373  Distance span = ::std::abs(*(begin(value) + i) - *(begin(*node.data) + i));
374 
375  if (span > max)
376  {
377  max = span;
378  node.cut.index = i;
379  }
380  }
381 
382  for (::std::size_t i = 0; i < 2; ++i)
383  {
384 #if __cplusplus > 201103L || _MSC_VER >= 1800
385  node.children[i] = ::std::make_unique<Node>();
386 #else
387  node.children[i].reset(new Node());
388 #endif
389  }
390 
391  bool less = *(begin(value) + node.cut.index) < *(begin(*node.data) + node.cut.index);
392  node.cut.value = (*(begin(value) + node.cut.index) + *(begin(*node.data) + node.cut.index)) / 2;
393 
394  node.children[0]->cut.index = 0;
395  node.children[0]->data = less ? value : ::std::move(node.data);
396  node.children[1]->cut.index = 0;
397  node.children[1]->data = less ? ::std::move(node.data) : value;
398 
399  node.data.reset();
400  }
401  else
402  {
403  if (*(begin(value) + node.cut.index) < node.cut.value)
404  {
405  this->push(*node.children[0], value);
406  }
407  else
408  {
409  this->push(*node.children[1], value);
410  }
411  }
412  }
413 
414  ::std::vector<Neighbor> search(const Value& query, const ::std::size_t* k, const Distance* radius, const bool& sorted) const
415  {
416  using ::std::size;
417 
418  ::std::vector<Neighbor> neighbors;
419  neighbors.reserve(nullptr != k ? *k : this->values);
420 
421  ::std::size_t checks = 0;
422 
423  ::std::vector<Branch> branches;
424  ::std::vector<Distance> sidedist(size(query), Distance());
425  this->search(this->root, query, k, radius, branches, neighbors, checks, Distance(), sidedist);
426 
427  while (!branches.empty() && (!this->checks || checks < this->checks))
428  {
429  Branch branch = branches.front();
430  ::std::pop_heap(branches.begin(), branches.end(), BranchCompare());
431  branches.pop_back();
432  this->search(*branch.node, query, k, radius, branches, neighbors, checks, branch.dist, branch.sidedist);
433  }
434 
435  if (sorted)
436  {
437  ::std::sort_heap(neighbors.begin(), neighbors.end(), NeighborCompare());
438  }
439 
440  if (nullptr == k)
441  {
442  neighbors.shrink_to_fit();
443  }
444 
445  return neighbors;
446  }
447 
448  void search(const Node& node, const Value& query, const ::std::size_t* k, const Distance* radius, ::std::vector<Branch>& branches, ::std::vector<Neighbor>& neighbors, ::std::size_t& checks, const Distance& mindist, const ::std::vector<Distance>& sidedist) const
449  {
450  using ::std::begin;
451 
452  if (nullptr == node.children[0] && nullptr == node.children[1])
453  {
454  if (node.data)
455  {
456  Distance distance = this->metric(query, *node.data);
457 
458  if (nullptr == k || neighbors.size() < *k || distance < neighbors.front().first)
459  {
460  if (nullptr == radius || distance < *radius)
461  {
462  if (nullptr != k && *k == neighbors.size())
463  {
464  ::std::pop_heap(neighbors.begin(), neighbors.end(), NeighborCompare());
465  neighbors.pop_back();
466  }
467 
468 #if (defined(_MSC_VER) && _MSC_VER < 1800) || (defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ < 8)
469  neighbors.push_back(::std::make_pair(distance, *node.data));
470 #else
471  neighbors.emplace_back(::std::piecewise_construct, ::std::forward_as_tuple(distance), ::std::forward_as_tuple(*node.data));
472 #endif
473  ::std::push_heap(neighbors.begin(), neighbors.end(), NeighborCompare());
474  }
475  }
476 
477  ++checks;
478  }
479  }
480  else
481  {
482  Distance value = *(begin(query) + node.cut.index);
483  Distance diff = value - node.cut.value;
484 
485  ::std::size_t best = diff < 0 ? 0 : 1;
486  ::std::size_t worst = diff < 0 ? 1 : 0;
487 
488  this->search(*node.children[best], query, k, radius, branches, neighbors, checks, mindist, sidedist);
489 
490  Distance cutdist = this->metric(value, node.cut.value, node.cut.index);
491  Distance newdist = mindist - sidedist[node.cut.index] + cutdist;
492 
493  if (nullptr == k || neighbors.size() < *k || newdist <= neighbors.front().first)
494  {
495  ::std::vector<Distance> newsidedist(sidedist);
496  newsidedist[node.cut.index] = cutdist;
497 
498  if (!this->checks)
499  {
500  this->search(*node.children[worst], query, k, radius, branches, neighbors, checks, newdist, newsidedist);
501  }
502  else
503  {
504 #if defined(_MSC_VER) && _MSC_VER < 1800
505  branches.push_back(Branch(newdist, newsidedist, node.children[worst].get()));
506 #else
507  branches.emplace_back(newdist, newsidedist, node.children[worst].get());
508 #endif
509  ::std::push_heap(branches.begin(), branches.end(), BranchCompare());
510  }
511  }
512  }
513  }
514 
515  template<typename InputIterator>
516  Cut select(InputIterator first, InputIterator last)
517  {
518  using ::std::begin;
519  using ::std::size;
520 
521  ::std::size_t distance = ::std::distance(first, last);
522  assert(distance > 0 || "mean expects at least one element");
523  ::std::size_t samples = ::std::min(this->samples, distance);
524  ::std::size_t dim = size(*first);
525 
526  this->mean.resize(dim);
527  ::std::fill(this->mean.begin(), this->mean.end(), Distance());
528 
529  this->var.resize(dim);
530  ::std::fill(this->var.begin(), this->var.end(), Distance());
531 
532  for (InputIterator i = first; i < first + samples; ++i)
533  {
534  for (::std::size_t j = 0; j < dim; ++j)
535  {
536  this->mean[j] += *(begin(*i) + j);
537  }
538  }
539 
540  for (::std::size_t i = 0; i < dim; ++i)
541  {
542  this->mean[i] /= samples;
543  }
544 
545  for (InputIterator i = first; i < first + samples; ++i)
546  {
547  for (::std::size_t j = 0; j < dim; ++j)
548  {
549  Distance d = *(begin(*i) + j) - this->mean[j];
550  this->var[j] += d * d;
551  }
552  }
553 
554  typename ::std::vector<Distance>::iterator max = ::std::max_element(this->var.begin(), this->var.end());
555 
556  Cut cut;
557  cut.index = ::std::distance(this->var.begin(), max);
558  cut.value = this->mean[cut.index];
559  return cut;
560  }
561 
562  ::boost::optional< ::std::size_t> checks;
563 
564  ::std::vector<Distance> mean;
565 
567 
568  Node root;
569 
570  ::std::size_t samples;
571 
572  ::std::size_t values;
573 
574  ::std::vector<Distance> var;
575  };
576  }
577 }
578 
579 #endif // RL_MATH_KDTREENEARESTNEIGHBORS_H
rl::math::KdtreeNearestNeighbors::setSamples
void setSamples(const ::std::size_t &samples)
Definition: KdtreeNearestNeighbors.h:212
rl::math::KdtreeNearestNeighbors::value_type
MetricT::Value value_type
Definition: KdtreeNearestNeighbors.h:69
rl::math::KdtreeNearestNeighbors::setChecks
void setChecks(const ::boost::optional< ::std::size_t > &checks)
Definition: KdtreeNearestNeighbors.h:207
rl::math::KdtreeNearestNeighbors::Distance
MetricT::Distance Distance
Definition: KdtreeNearestNeighbors.h:71
rl::math::KdtreeNearestNeighbors::Branch::sidedist
::std::vector< Distance > sidedist
Definition: KdtreeNearestNeighbors.h:254
rl::math::KdtreeNearestNeighbors::swap
friend void swap(KdtreeNearestNeighbors &lhs, KdtreeNearestNeighbors &rhs)
Definition: KdtreeNearestNeighbors.h:233
rl::math::KdtreeNearestNeighbors::checks
::boost::optional< ::std::size_t > checks
Definition: KdtreeNearestNeighbors.h:562
rl::math::KdtreeNearestNeighbors::BranchCompare::operator()
bool operator()(const Branch &lhs, const Branch &rhs) const
Definition: KdtreeNearestNeighbors.h:259
rl::math::KdtreeNearestNeighbors::Size
MetricT::Size Size
Definition: KdtreeNearestNeighbors.h:75
rl::math::KdtreeNearestNeighbors::Node::~Node
~Node()
Definition: KdtreeNearestNeighbors.h:295
rl::math::KdtreeNearestNeighbors::nearest
::std::vector< Neighbor > nearest(const Value &query, const ::std::size_t &k, const bool &sorted=true) const
Definition: KdtreeNearestNeighbors.h:191
rl::math::KdtreeNearestNeighbors::getChecks
::boost::optional< ::std::size_t > getChecks() const
Definition: KdtreeNearestNeighbors.h:154
rl::math::KdtreeNearestNeighbors::KdtreeNearestNeighbors
KdtreeNearestNeighbors(const Metric &metric)
Definition: KdtreeNearestNeighbors.h:81
rl::math::KdtreeNearestNeighbors::Node::children
::std::array< ::std::unique_ptr< Node >, 2 > children
Definition: KdtreeNearestNeighbors.h:312
rl::math::KdtreeNearestNeighbors::size
::std::size_t size() const
Definition: KdtreeNearestNeighbors.h:217
rl::math::KdtreeNearestNeighbors::Node::Node
Node()
Definition: KdtreeNearestNeighbors.h:288
rl::math::KdtreeNearestNeighbors::values
::std::size_t values
Definition: KdtreeNearestNeighbors.h:572
rl::math::KdtreeNearestNeighbors::mean
::std::vector< Distance > mean
Definition: KdtreeNearestNeighbors.h:564
rl::math::KdtreeNearestNeighbors::Cut::operator()
bool operator()(const Value &other) const
Definition: KdtreeNearestNeighbors.h:267
rl::math::KdtreeNearestNeighbors::Branch::Branch
Branch(Distance &dist, ::std::vector< Distance > &sidedist, const Node *node)
Definition: KdtreeNearestNeighbors.h:243
rl::math::KdtreeNearestNeighbors::reference
MetricT::Value & reference
Definition: KdtreeNearestNeighbors.h:65
rl::math::KdtreeNearestNeighbors::empty
bool empty() const
Definition: KdtreeNearestNeighbors.h:149
rl::math::KdtreeNearestNeighbors::Branch::dist
Distance dist
Definition: KdtreeNearestNeighbors.h:250
rl::math::KdtreeNearestNeighbors::Node
Definition: KdtreeNearestNeighbors.h:287
rl::math::KdtreeNearestNeighbors
k-d tree.
Definition: KdtreeNearestNeighbors.h:56
rl::math::KdtreeNearestNeighbors::Branch::node
const Node * node
Definition: KdtreeNearestNeighbors.h:252
rl::math::KdtreeNearestNeighbors::const_reference
const MetricT::Value & const_reference
Definition: KdtreeNearestNeighbors.h:58
rl::math::KdtreeNearestNeighbors::Neighbor
::std::pair< Distance, Value > Neighbor
Definition: KdtreeNearestNeighbors.h:79
rl::math::KdtreeNearestNeighbors::samples
::std::size_t samples
Definition: KdtreeNearestNeighbors.h:570
rl::math::KdtreeNearestNeighbors::radius
::std::vector< Neighbor > radius(const Value &query, const Distance &radius, const bool &sorted=true) const
Definition: KdtreeNearestNeighbors.h:202
rl::math::KdtreeNearestNeighbors::Metric
MetricT Metric
Definition: KdtreeNearestNeighbors.h:73
rl::math::KdtreeNearestNeighbors::Branch
Definition: KdtreeNearestNeighbors.h:242
rl::math::KdtreeNearestNeighbors::KdtreeNearestNeighbors
KdtreeNearestNeighbors(InputIterator first, InputIterator last, Metric &&metric=Metric())
Definition: KdtreeNearestNeighbors.h:117
std::swap
void swap(::rl::util::rtai::thread &x, ::rl::util::rtai::thread &y)
Definition: thread.h:492
rl::math::KdtreeNearestNeighbors::~KdtreeNearestNeighbors
~KdtreeNearestNeighbors()
Definition: KdtreeNearestNeighbors.h:129
rl::math::KdtreeNearestNeighbors::Cut
Definition: KdtreeNearestNeighbors.h:266
rl::math::KdtreeNearestNeighbors::Node::data
::boost::optional< Value > data
Definition: KdtreeNearestNeighbors.h:316
rl::math::KdtreeNearestNeighbors::Node::swap
void swap(Node &other)
Definition: KdtreeNearestNeighbors.h:299
rl::math::KdtreeNearestNeighbors::NeighborCompare::operator()
bool operator()(const Neighbor &lhs, const Neighbor &rhs) const
Definition: KdtreeNearestNeighbors.h:280
rl::math::KdtreeNearestNeighbors::insert
void insert(InputIterator first, InputIterator last)
Definition: KdtreeNearestNeighbors.h:165
rl::math::KdtreeNearestNeighbors::size_type
::std::size_t size_type
Definition: KdtreeNearestNeighbors.h:67
rl::plan::Metric
Definition: Metric.h:40
rl::math::KdtreeNearestNeighbors::NeighborCompare
Definition: KdtreeNearestNeighbors.h:279
rl::math::KdtreeNearestNeighbors::push
void push(const Value &value)
Definition: KdtreeNearestNeighbors.h:196
std
Definition: thread.h:482
rl::math::KdtreeNearestNeighbors::root
Node root
Definition: KdtreeNearestNeighbors.h:568
rl::math::KdtreeNearestNeighbors::Cut::index
Size index
Definition: KdtreeNearestNeighbors.h:273
rl::math::KdtreeNearestNeighbors::divide
void divide(Node &node, InputIterator first, InputIterator last)
Definition: KdtreeNearestNeighbors.h:330
rl::math::KdtreeNearestNeighbors::search
void search(const Node &node, const Value &query, const ::std::size_t *k, const Distance *radius, ::std::vector< Branch > &branches, ::std::vector< Neighbor > &neighbors, ::std::size_t &checks, const Distance &mindist, const ::std::vector< Distance > &sidedist) const
Definition: KdtreeNearestNeighbors.h:448
rl::math::KdtreeNearestNeighbors::metric
Metric metric
Definition: KdtreeNearestNeighbors.h:566
rl::math::KdtreeNearestNeighbors::swap
void swap(KdtreeNearestNeighbors &other)
Definition: KdtreeNearestNeighbors.h:222
rl::math::KdtreeNearestNeighbors::select
Cut select(InputIterator first, InputIterator last)
Definition: KdtreeNearestNeighbors.h:516
rl::math::KdtreeNearestNeighbors::push
void push(Node &node, const Value &value)
Definition: KdtreeNearestNeighbors.h:357
rl::math::KdtreeNearestNeighbors::Value
MetricT::Value Value
Definition: KdtreeNearestNeighbors.h:77
rl::math::KdtreeNearestNeighbors::getSamples
::std::size_t getSamples() const
Definition: KdtreeNearestNeighbors.h:159
rl::math::KdtreeNearestNeighbors::data
::std::vector< Value > data() const
Definition: KdtreeNearestNeighbors.h:141
distance
Scalar distance(const Transform< Scalar, Dim, Mode, Options > &other, const Scalar &weight=1) const
Definition: TransformAddons.h:33
rl::math::KdtreeNearestNeighbors::difference_type
::std::ptrdiff_t difference_type
Definition: KdtreeNearestNeighbors.h:63
rl::math::KdtreeNearestNeighbors::Cut::value
Distance value
Definition: KdtreeNearestNeighbors.h:275
rl::math::KdtreeNearestNeighbors::var
::std::vector< Distance > var
Definition: KdtreeNearestNeighbors.h:574
rl::math::KdtreeNearestNeighbors::KdtreeNearestNeighbors
KdtreeNearestNeighbors(Metric &&metric=Metric())
Definition: KdtreeNearestNeighbors.h:92
rl::math::KdtreeNearestNeighbors::Node::cut
Cut cut
Definition: KdtreeNearestNeighbors.h:314
rl::math::KdtreeNearestNeighbors::BranchCompare
Definition: KdtreeNearestNeighbors.h:258
rl::math::KdtreeNearestNeighbors::KdtreeNearestNeighbors
KdtreeNearestNeighbors(InputIterator first, InputIterator last, const Metric &metric)
Definition: KdtreeNearestNeighbors.h:104
rl::math::KdtreeNearestNeighbors::Node::swap
friend void swap(Node &lhs, Node &rhs)
Definition: KdtreeNearestNeighbors.h:307
rl::math::KdtreeNearestNeighbors::clear
void clear()
Definition: KdtreeNearestNeighbors.h:133
rl::math::KdtreeNearestNeighbors::search
::std::vector< Neighbor > search(const Value &query, const ::std::size_t *k, const Distance *radius, const bool &sorted) const
Definition: KdtreeNearestNeighbors.h:414
rl::math::KdtreeNearestNeighbors::data
void data(const Node &node, ::std::vector< Value > &data) const
Definition: KdtreeNearestNeighbors.h:319
rl
Robotics Library.
Definition: AnalogInput.cpp:30