experimental/kdtree_cpu.cpp
Go to the documentation of this file.
1 /*
2 
3 Copyright (c) 2010--2011, Stephane Magnenat, ASL, ETHZ, Switzerland
4 You can contact the author at <stephane at magnenat dot net>
5 
6 All rights reserved.
7 
8 Redistribution and use in source and binary forms, with or without
9 modification, are permitted provided that the following conditions are met:
10  * Redistributions of source code must retain the above copyright
11  notice, this list of conditions and the following disclaimer.
12  * Redistributions in binary form must reproduce the above copyright
13  notice, this list of conditions and the following disclaimer in the
14  documentation and/or other materials provided with the distribution.
15  * Neither the name of the <organization> nor the
16  names of its contributors may be used to endorse or promote products
17  derived from this software without specific prior written permission.
18 
19 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
20 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
21 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
23 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
24 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
26 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
28 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29 
30 */
31 
32 #include "nabo_experimental.h"
33 #include "../nabo/index_heap.h"
34 #include <iostream>
35 #include <stdexcept>
36 #include <limits>
37 #include <queue>
38 #include <algorithm>
39 
40 namespace Nabo
41 {
42  using namespace std;
43 
44  template<typename T, typename CloudType>
45  size_t argMax(const typename NearestNeighbourSearch<T, CloudType>::Vector& v)
46  {
47  T maxVal(0);
48  size_t maxIdx(0);
49  for (int i = 0; i < v.size(); ++i)
50  {
51  if (v[i] > maxVal)
52  {
53  maxVal = v[i];
54  maxIdx = i;
55  }
56  }
57  return maxIdx;
58  }
59 
60  template<typename T, typename CloudType>
61  size_t KDTreeBalancedPtInNodes<T, CloudType>::getTreeSize(size_t elCount) const
62  {
63  // FIXME: 64 bits safe stuff, only work for 2^32 elements right now
64  size_t count = 0;
65  int i = 31;
66  for (; i >= 0; --i)
67  {
68  if (elCount & (1 << i))
69  break;
70  }
71  for (int j = 0; j <= i; ++j)
72  count |= (1 << j);
73  //cerr << "tree size " << count << " (" << elCount << " elements)\n";
74  return count;
75  }
76 
77  template<typename T, typename CloudType>
78  typename KDTreeBalancedPtInNodes<T, CloudType>::IndexVector KDTreeBalancedPtInNodes<T, CloudType>::cloudIndexesFromNodesIndexes(const IndexVector& indexes) const
79  {
80  IndexVector cloudIndexes(indexes.size());
81  for (int i = 0; i < indexes.size(); ++i)
82  cloudIndexes.coeffRef(i) = nodes[indexes[i]].index;
83  return cloudIndexes;
84  }
85 
86  template<typename T, typename CloudType>
87  void KDTreeBalancedPtInNodes<T, CloudType>::buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos)
88  {
89  const size_t count(last - first);
90  //cerr << count << endl;
91  if (count == 1)
92  {
93  nodes[pos] = Node(first->pos, -1, first->index);
94  return;
95  }
96 
97  // estimate variance
98  // get mean
99  Vector mean(Vector::Zero(this->dim));
100  for (BuildPointsCstIt it(first); it != last; ++it)
101  mean += it->pos;
102  mean /= last - first;
103  // get sum of variance
104  Vector var(Vector::Zero(this->dim));
105  for (BuildPointsCstIt it(first); it != last; ++it)
106  var += (it->pos - mean).cwise() * (it->pos - mean);
107  // get dimension of maxmial variance
108  const size_t cutDim = argMax<T>(var);
109 
110  // sort
111  sort(first, last, CompareDim(cutDim));
112 
113  // set node
114  const size_t recurseCount(count-1);
115  const size_t rightCount(recurseCount/2);
116  const size_t leftCount(recurseCount-rightCount);
117  assert(last - rightCount == first + leftCount + 1);
118 
119  nodes[pos] = Node((first+leftCount)->pos, cutDim, (first+leftCount)->index);
120 
121  //cerr << pos << " cutting on " << cutDim << " at " << (first+leftCount)->pos[cutDim] << endl;
122 
123  // recurse
124  if (count > 2)
125  {
126  buildNodes(first, first + leftCount, childLeft(pos));
127  buildNodes(first + leftCount + 1, last, childRight(pos));
128  }
129  else
130  {
131  nodes[childLeft(pos)] = Node(first->pos, -1, first->index);
132  nodes[childRight(pos)] = Node(Vector(), -2, 0);
133  }
134  }
135 
136  template<typename T, typename CloudType>
137  void KDTreeBalancedPtInNodes<T, CloudType>::dump(const Vector minValues, const Vector maxValues, const size_t pos) const
138  {
139  const Node& node(nodes[pos]);
140 
141  if (node.dim >= -1)
142  {
143  if (this->dim == 2)
144  cout << "<circle cx=\"" << 100*node.pos(0) << "\" cy=\"" << 100*node.pos(1) << "\" r=\"1\" stroke=\"black\" stroke-width=\"0.2\" fill=\"red\"/>" << endl;
145  else
146  cout << "pt at\n" << node.pos << endl;
147  }
148  if (node.dim >= 0)
149  {
150  //cerr << "in bounds:\n" << minValues << "\nto\n" << maxValues << endl;
151 
152  // update bounds for left
153  Vector leftMaxValues(maxValues);
154  leftMaxValues[node.dim] = node.pos[node.dim];
155  // update bounds for right
156  Vector rightMinValues(minValues);
157  rightMinValues[node.dim] = node.pos[node.dim];
158 
159  // print line
160  if (this->dim == 2)
161  cout << "<line x1=\"" << 100*rightMinValues(0) << "\" y1=\"" << 100*rightMinValues(1) << "\" x2=\"" << 100*leftMaxValues(0) << "\" y2=\"" << 100*leftMaxValues(1) << "\" style=\"stroke:rgb(0,0,0);stroke-width:0.2\"/>" << endl;
162  else
163  cout << "cut from\n" << rightMinValues << "\nto\n" << leftMaxValues << endl;
164  // recurs
165  dump(minValues, leftMaxValues, childLeft(pos));
166  dump(rightMinValues, maxValues, childRight(pos));
167  }
168  }
169 
170  template<typename T, typename CloudType>
171  KDTreeBalancedPtInNodes<T, CloudType>::KDTreeBalancedPtInNodes(const CloudType& cloud):
172  NearestNeighbourSearch<T, CloudType>::NearestNeighbourSearch(cloud)
173  {
174  // build point vector and compute bounds
175  BuildPoints buildPoints;
176  buildPoints.reserve(cloud.cols());
177  for (int i = 0; i < cloud.cols(); ++i)
178  {
179  const Vector& v(cloud.col(i));
180  buildPoints.push_back(BuildPoint(v, i));
181  const_cast<Vector&>(this->minBound) = this->minBound.cwise().min(v);
182  const_cast<Vector&>(this->maxBound) = this->maxBound.cwise().max(v);
183  }
184 
185  // create nodes
186  nodes.resize(getTreeSize(cloud.cols()));
187  buildNodes(buildPoints.begin(), buildPoints.end(), 0);
188 
189  // dump nodes
190  //dump(minBound, maxBound, 0);
191  }
192 
193  // points in nodes, priority queue
194 
195  template<typename T, typename CloudType>
196  KDTreeBalancedPtInNodesPQ<T, CloudType>::KDTreeBalancedPtInNodesPQ(const CloudType& cloud):
197  KDTreeBalancedPtInNodes<T, CloudType>::KDTreeBalancedPtInNodes(cloud)
198  {
199  }
200 
201  template<typename T, typename CloudType>
202  typename KDTreeBalancedPtInNodesPQ<T, CloudType>::IndexVector KDTreeBalancedPtInNodesPQ<T, CloudType>::knn(const Vector& query, const Index k, const T epsilon, const unsigned optionFlags)
203  {
204  typedef priority_queue<SearchElement> Queue;
205 
206  const T maxError(1 + epsilon);
207  const bool allowSelfMatch(optionFlags & NearestNeighbourSearch<T>::ALLOW_SELF_MATCH);
208 
209  Queue queue;
210  queue.push(SearchElement(0, 0));
211  IndexHeapSTL<Index, T> heap(k);
212  statistics.lastQueryVisitCount = 0;
213 
214  while (!queue.empty())
215  {
216  SearchElement el(queue.top());
217  queue.pop();
218 
219  // nothing is closer, we found best
220  if (el.minDist * maxError > heap.headValue())
221  break;
222 
223  size_t n(el.index);
224  while (1)
225  {
226  const Node& node(nodes[n]);
227  assert (node.dim != -2);
228 
229  // TODO: optimise dist while going down
230  const T dist(dist2<T>(node.pos, query));
231  if ((dist < heap.headValue()) &&
232  (allowSelfMatch || (dist > numeric_limits<T>::epsilon())))
233  heap.replaceHead(n, dist);
234 
235  // if we are at leaf, stop
236  if (node.dim < 0)
237  break;
238 
239  const T offset(query.coeff(node.dim) - node.pos.coeff(node.dim));
240  const T offset2(offset * offset);
241  const T bestDist(heap.headValue());
242  if (offset > 0)
243  {
244  // enqueue offside ?
245  if (offset2 < bestDist && nodes[childLeft(n)].dim != -2)
246  queue.push(SearchElement(childLeft(n), offset2));
247  // continue onside
248  if (nodes[childRight(n)].dim != -2)
249  n = childRight(n);
250  else
251  break;
252  }
253  else
254  {
255  // enqueue offside ?
256  if (offset2 < bestDist && nodes[childRight(n)].dim != -2)
257  queue.push(SearchElement(childRight(n), offset2));
258  // continue onside
259  if (nodes[childLeft(n)].dim != -2)
260  n = childLeft(n);
261  else
262  break;
263  }
264  ++statistics.lastQueryVisitCount;
265  }
266  }
267  statistics.totalVisitCount += statistics.lastQueryVisitCount;
268 
269  if (optionFlags & NearestNeighbourSearch<T>::SORT_RESULTS)
270  heap.sort();
271 
272  return cloudIndexesFromNodesIndexes(heap.getIndexes());
273  }
274 
275  template struct KDTreeBalancedPtInNodesPQ<float>;
276  template struct KDTreeBalancedPtInNodesPQ<double>;
277  template struct KDTreeBalancedPtInNodesPQ<float, Eigen::Matrix3Xf>;
278  template struct KDTreeBalancedPtInNodesPQ<double, Eigen::Matrix3Xd>;
279  template struct KDTreeBalancedPtInNodesPQ<float, Eigen::Map<const Eigen::Matrix3Xf, Eigen::Aligned> >;
280  template struct KDTreeBalancedPtInNodesPQ<double, Eigen::Map<const Eigen::Matrix3Xd, Eigen::Aligned> >;
281 
282  // points in nodes, stack
283 
284  template<typename T, typename CloudType>
285  KDTreeBalancedPtInNodesStack<T, CloudType>::KDTreeBalancedPtInNodesStack(const CloudType& cloud):
286  KDTreeBalancedPtInNodes<T, CloudType>::KDTreeBalancedPtInNodes(cloud)
287  {
288  }
289 
290  template<typename T, typename CloudType>
291  typename KDTreeBalancedPtInNodesStack<T, CloudType>::IndexVector KDTreeBalancedPtInNodesStack<T, CloudType>::knn(const Vector& query, const Index k, const T epsilon, const unsigned optionFlags)
292  {
293  const bool allowSelfMatch(optionFlags & NearestNeighbourSearch<T>::ALLOW_SELF_MATCH);
294 
295  assert(nodes.size() > 0);
296  assert(nodes[0].pos.size() == query.size());
297  Heap heap(k);
298  Vector off(Vector::Zero(nodes[0].pos.size()));
299 
300  statistics.lastQueryVisitCount = 0;
301 
302  recurseKnn(query, 0, 0, heap, off, 1 + epsilon, allowSelfMatch);
303 
304  if (optionFlags & NearestNeighbourSearch<T>::SORT_RESULTS)
305  heap.sort();
306 
307  statistics.totalVisitCount += statistics.lastQueryVisitCount;
308 
309  return cloudIndexesFromNodesIndexes(heap.getIndexes());
310  }
311 
312  template<typename T, typename CloudType>
313  void KDTreeBalancedPtInNodesStack<T, CloudType>::recurseKnn(const Vector& query, const size_t n, T rd, Heap& heap, Vector& off, const T maxError, const bool allowSelfMatch)
314  {
315  const Node& node(nodes[n]);
316  const int cd(node.dim);
317 
318  ++statistics.lastQueryVisitCount;
319 
320  if (cd == -2)
321  return;
322 
323  const T dist(dist2<T, CloudType>(node.pos, query));
324  if ((dist < heap.headValue()) &&
325  (allowSelfMatch || (dist > numeric_limits<T>::epsilon()))
326  )
327  heap.replaceHead(n, dist);
328 
329  if (cd != -1)
330  {
331  const T old_off(off.coeff(cd));
332  const T new_off(query.coeff(cd) - node.pos.coeff(cd));
333  if (new_off > 0)
334  {
335  recurseKnn(query, childRight(n), rd, heap, off, maxError, allowSelfMatch);
336  rd += - old_off*old_off + new_off*new_off;
337  if (rd * maxError < heap.headValue())
338  {
339  off.coeffRef(cd) = new_off;
340  recurseKnn(query, childLeft(n), rd, heap, off, maxError, allowSelfMatch);
341  off.coeffRef(cd) = old_off;
342  }
343  }
344  else
345  {
346  recurseKnn(query, childLeft(n), rd, heap, off, maxError, allowSelfMatch);
347  rd += - old_off*old_off + new_off*new_off;
348  if (rd * maxError < heap.headValue())
349  {
350  off.coeffRef(cd) = new_off;
351  recurseKnn(query, childRight(n), rd, heap, off, maxError, allowSelfMatch);
352  off.coeffRef(cd) = old_off;
353  }
354  }
355  }
356  }
357 
358  template struct KDTreeBalancedPtInNodesStack<float>;
359  template struct KDTreeBalancedPtInNodesStack<double>;
360  template struct KDTreeBalancedPtInNodesStack<float, Eigen::Matrix3Xf>;
361  template struct KDTreeBalancedPtInNodesStack<double, Eigen::Matrix3Xd>;
362  template struct KDTreeBalancedPtInNodesStack<float, Eigen::Map<const Eigen::Matrix3Xf, Eigen::Aligned> >;
363  template struct KDTreeBalancedPtInNodesStack<double, Eigen::Map<const Eigen::Matrix3Xd, Eigen::Aligned> >;
364 
365  // NEW:
366 
367  template<typename T, typename CloudType>
368  size_t KDTreeBalancedPtInLeavesStack<T, CloudType>::getTreeSize(size_t elCount) const
369  {
370  // FIXME: 64 bits safe stuff, only work for 2^32 elements right now
371  assert(elCount > 0);
372  elCount --;
373  size_t count = 0;
374  int i = 31;
375  for (; i >= 0; --i)
376  {
377  if (elCount & (1 << i))
378  break;
379  }
380  for (int j = 0; j <= i; ++j)
381  count |= (1 << j);
382  count <<= 1;
383  count |= 1;
384  return count;
385  }
386 
387  template<typename T, typename CloudType>
388  void KDTreeBalancedPtInLeavesStack<T, CloudType>::buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos, const Vector minValues, const Vector maxValues, const bool balanceVariance)
389  {
390  const size_t count(last - first);
391  //cerr << count << endl;
392  if (count == 1)
393  {
394  const int dim = -2-(first->index);
395  assert(pos < nodes.size());
396  nodes[pos] = Node(dim);
397  return;
398  }
399 
400  size_t cutDim;
401  if (balanceVariance)
402  {
403  // estimate variance
404  // get mean
405  Vector mean(Vector::Zero(this->dim));
406  for (BuildPointsCstIt it(first); it != last; ++it)
407  mean += it->pos;
408  mean /= last - first;
409  // get sum of variance
410  Vector var(Vector::Zero(this->dim));
411  for (BuildPointsCstIt it(first); it != last; ++it)
412  var += (it->pos - mean).cwise() * (it->pos - mean);
413  // get dimension of maxmial variance
414  cutDim = argMax<T>(var);
415  }
416  else
417  {
418  // find the largest dimension of the box
419  cutDim = argMax<T>(maxValues - minValues);
420  }
421 
422  // compute number of elements
423  const size_t rightCount(count/2);
424  const size_t leftCount(count - rightCount);
425  assert(last - rightCount == first + leftCount);
426 
427  // sort
428  //sort(first, last, CompareDim(cutDim));
429  nth_element(first, first + leftCount, last, CompareDim(cutDim));
430 
431  // set node
432  const T cutVal((first+leftCount)->pos.coeff(cutDim));
433  nodes[pos] = Node(cutDim, cutVal);
434 
435  //cerr << pos << " cutting on " << cutDim << " at " << (first+leftCount)->pos[cutDim] << endl;
436 
437  // update bounds for left
438  Vector leftMaxValues(maxValues);
439  leftMaxValues[cutDim] = cutVal;
440  // update bounds for right
441  Vector rightMinValues(minValues);
442  rightMinValues[cutDim] = cutVal;
443 
444  // recurse
445  buildNodes(first, first + leftCount, childLeft(pos), minValues, leftMaxValues, balanceVariance);
446  buildNodes(first + leftCount, last, childRight(pos), rightMinValues, maxValues, balanceVariance);
447  }
448 
449  template<typename T, typename CloudType>
450  KDTreeBalancedPtInLeavesStack<T, CloudType>::KDTreeBalancedPtInLeavesStack(const CloudType& cloud, const bool balanceVariance):
451  NearestNeighbourSearch<T, CloudType>::NearestNeighbourSearch(cloud)
452  {
453  // build point vector and compute bounds
454  BuildPoints buildPoints;
455  buildPoints.reserve(cloud.cols());
456  for (int i = 0; i < cloud.cols(); ++i)
457  {
458  const Vector& v(cloud.col(i));
459  buildPoints.push_back(BuildPoint(v, i));
460  const_cast<Vector&>(minBound) = minBound.cwise().min(v);
461  const_cast<Vector&>(maxBound) = maxBound.cwise().max(v);
462  }
463 
464  // create nodes
465  nodes.resize(getTreeSize(cloud.cols()));
466  buildNodes(buildPoints.begin(), buildPoints.end(), 0, minBound, maxBound, balanceVariance);
467  //for (size_t i = 0; i < nodes.size(); ++i)
468  // cout << i << ": " << nodes[i].dim << " " << nodes[i].cutVal << endl;
469  }
470 
471  template<typename T, typename CloudType>
472  typename KDTreeBalancedPtInLeavesStack<T, CloudType>::IndexVector KDTreeBalancedPtInLeavesStack<T, CloudType>::knn(const Vector& query, const Index k, const T epsilon, const unsigned optionFlags)
473  {
474  const bool allowSelfMatch(optionFlags & NearestNeighbourSearch<T, CloudType>::ALLOW_SELF_MATCH);
475 
476  assert(nodes.size() > 0);
477  Heap heap(k);
478  Vector off(Vector::Zero(query.size()));
479 
480  statistics.lastQueryVisitCount = 0;
481 
482  recurseKnn(query, 0, 0, heap, off, 1 + epsilon, allowSelfMatch);
483 
484  if (optionFlags & NearestNeighbourSearch<T>::SORT_RESULTS)
485  heap.sort();
486 
487  statistics.totalVisitCount += statistics.lastQueryVisitCount;
488 
489  return heap.getIndexes();
490  }
491 
492  template<typename T, typename CloudType>
493  void KDTreeBalancedPtInLeavesStack<T, CloudType>::recurseKnn(const Vector& query, const size_t n, T rd, Heap& heap, Vector& off, const T maxError, const bool allowSelfMatch)
494  {
495  const Node& node(nodes[n]);
496  const int cd(node.dim);
497 
498  ++statistics.lastQueryVisitCount;
499 
500  if (cd < 0)
501  {
502  if (cd == -1)
503  return;
504  const int index(-(cd + 2));
505  const T dist(dist2<T>(query, cloud.col(index)));
506  if ((dist < heap.headValue()) &&
507  (allowSelfMatch || (dist > numeric_limits<T>::epsilon()))
508  )
509  heap.replaceHead(index, dist);
510  }
511  else
512  {
513  const T old_off(off.coeff(cd));
514  const T new_off(query.coeff(cd) - node.cutVal);
515  if (new_off > 0)
516  {
517  recurseKnn(query, childRight(n), rd, heap, off, maxError, allowSelfMatch);
518  rd += - old_off*old_off + new_off*new_off;
519  if (rd * maxError < heap.headValue())
520  {
521  off.coeffRef(cd) = new_off;
522  recurseKnn(query, childLeft(n), rd, heap, off, maxError, allowSelfMatch);
523  off.coeffRef(cd) = old_off;
524  }
525  }
526  else
527  {
528  recurseKnn(query, childLeft(n), rd, heap, off, maxError, allowSelfMatch);
529  rd += - old_off*old_off + new_off*new_off;
530  if (rd * maxError < heap.headValue())
531  {
532  off.coeffRef(cd) = new_off;
533  recurseKnn(query, childRight(n), rd, heap, off, maxError, allowSelfMatch);
534  off.coeffRef(cd) = old_off;
535  }
536  }
537  }
538  }
539 
540  template struct KDTreeBalancedPtInLeavesStack<float>;
541  template struct KDTreeBalancedPtInLeavesStack<double>;
542  template struct KDTreeBalancedPtInLeavesStack<float, Eigen::Matrix3Xf>;
543  template struct KDTreeBalancedPtInLeavesStack<double, Eigen::Matrix3Xd>;
544  template struct KDTreeBalancedPtInLeavesStack<float, Eigen::Map<const Eigen::Matrix3Xf, Eigen::Aligned> >;
545  template struct KDTreeBalancedPtInLeavesStack<double, Eigen::Map<const Eigen::Matrix3Xd, Eigen::Aligned> >;
546 
547 
548 
549  template<typename T, typename Heap, typename CloudType>
550  unsigned KDTreeUnbalancedPtInLeavesImplicitBoundsStack<T, Heap, CloudType>::buildNodes(const BuildPointsIt first, const BuildPointsIt last, const Vector minValues, const Vector maxValues)
551  {
552  const size_t count(last - first);
553  const unsigned pos(nodes.size());
554 
555  //cerr << count << endl;
556  if (count == 1)
557  {
558  nodes.push_back(Node(first->index));
559  return pos;
560  }
561 
562  // find the largest dimension of the box
563  const unsigned cutDim = argMax<T>(maxValues - minValues);
564  T cutVal((maxValues(cutDim) + minValues(cutDim))/2);
565 
566  // TODO: do only sort once
567  // sort
568  sort(first, last, CompareDim(cutDim));
569 
570  // TODO: optimise using binary search
571  size_t rightStart(0);
572  while (rightStart < count && (first+rightStart)->pos.coeff(cutDim) < cutVal)
573  ++rightStart;
574 
575  // prevent trivial splits
576  if (rightStart == 0)
577  {
578  cutVal = first->pos.coeff(cutDim);
579  rightStart = 1;
580  }
581  else if (rightStart == count)
582  {
583  rightStart = count - 1;
584  cutVal = (first + rightStart)->pos.coeff(cutDim);
585  }
586 
587  // update bounds for left
588  Vector leftMaxValues(maxValues);
589  leftMaxValues[cutDim] = cutVal;
590  // update bounds for right
591  Vector rightMinValues(minValues);
592  rightMinValues[cutDim] = cutVal;
593 
594  // count for recursion
595  const size_t rightCount(count - rightStart);
596  const size_t leftCount(count - rightCount);
597 
598  // add this
599  nodes.push_back(Node(cutDim, cutVal, 0));
600 
601  // recurse
602  const unsigned __attribute__ ((unused)) leftChild = buildNodes(first, first + leftCount, minValues, leftMaxValues);
603  assert(leftChild == pos + 1);
604  const unsigned rightChild = buildNodes(first + leftCount, last, rightMinValues, maxValues);
605 
606  // write right child index and return
607  nodes[pos].rightChild = rightChild;
608  return pos;
609  }
610 
611  template<typename T, typename Heap, typename CloudType>
612  KDTreeUnbalancedPtInLeavesImplicitBoundsStack<T, Heap, CloudType>::KDTreeUnbalancedPtInLeavesImplicitBoundsStack(const CloudType& cloud):
613  NearestNeighbourSearch<T, CloudType>::NearestNeighbourSearch(cloud)
614  {
615  // build point vector and compute bounds
616  BuildPoints buildPoints;
617  buildPoints.reserve(cloud.cols());
618  for (int i = 0; i < cloud.cols(); ++i)
619  {
620  const Vector& v(cloud.col(i));
621  buildPoints.push_back(BuildPoint(v, i));
622  const_cast<Vector&>(minBound) = minBound.cwise().min(v);
623  const_cast<Vector&>(maxBound) = maxBound.cwise().max(v);
624  }
625 
626  // create nodes
627  //nodes.resize(getTreeSize(cloud.cols()));
628  buildNodes(buildPoints.begin(), buildPoints.end(), minBound, maxBound);
629  //for (size_t i = 0; i < nodes.size(); ++i)
630  // cout << i << ": " << nodes[i].dim << " " << nodes[i].cutVal << " " << nodes[i].rightChild << endl;
631  }
632 
633  template<typename T, typename Heap, typename CloudType>
634  typename KDTreeUnbalancedPtInLeavesImplicitBoundsStack<T, Heap, CloudType>::IndexVector KDTreeUnbalancedPtInLeavesImplicitBoundsStack<T, Heap, CloudType>::knn(const Vector& query, const Index k, const T epsilon, const unsigned optionFlags)
635  {
636  const bool allowSelfMatch(optionFlags & NearestNeighbourSearch<T, CloudType>::ALLOW_SELF_MATCH);
637 
638  assert(nodes.size() > 0);
639  Heap heap(k);
640  Vector off(Vector::Zero(query.size()));
641 
642  statistics.lastQueryVisitCount = 0;
643 
644  recurseKnn(query, 0, 0, heap, off, 1+epsilon, allowSelfMatch);
645 
646  if (optionFlags & NearestNeighbourSearch<T, CloudType>::SORT_RESULTS)
647  heap.sort();
648 
649  statistics.totalVisitCount += statistics.lastQueryVisitCount;
650 
651  return heap.getIndexes();
652  }
653 
654  template<typename T, typename Heap, typename CloudType>
655  typename KDTreeUnbalancedPtInLeavesImplicitBoundsStack<T, Heap, CloudType>::IndexMatrix KDTreeUnbalancedPtInLeavesImplicitBoundsStack<T, Heap, CloudType>::knnM(const Matrix& query, const Index k, const T epsilon, const unsigned optionFlags)
656  {
657  const bool allowSelfMatch(optionFlags & NearestNeighbourSearch<T, CloudType>::ALLOW_SELF_MATCH);
658  assert(nodes.size() > 0);
659 
660  assert(nodes.size() > 0);
661  Heap heap(k);
662  Vector off(query.rows());
663 
664  IndexMatrix result(k, query.cols());
665  const int colCount(query.cols());
666 
667  for (int i = 0; i < colCount; ++i)
668  {
669  const Vector& q(query.col(i));
670 
671  off.setZero();
672  heap.reset();
673 
674  statistics.lastQueryVisitCount = 0;
675 
676  recurseKnn(q, 0, 0, heap, off, 1+epsilon, allowSelfMatch);
677 
678  if (optionFlags & NearestNeighbourSearch<T>::SORT_RESULTS)
679  heap.sort();
680 
681  result.col(i) = heap.getIndexes();
682 
683  statistics.totalVisitCount += statistics.lastQueryVisitCount;
684  }
685 
686  return result;
687  }
688 
689  template<typename T, typename Heap, typename CloudType>
690  void KDTreeUnbalancedPtInLeavesImplicitBoundsStack<T, Heap, CloudType>::recurseKnn(const Vector& query, const unsigned n, T rd, Heap& heap, Vector& off, const T maxError, const bool allowSelfMatch)
691  {
692  const Node& node(nodes[n]);
693  //++statistics.lastQueryVisitCount;
694 
695  if (node.rightChild == Node::INVALID_CHILD)
696  {
697  const unsigned index(node.ptIndex);
698  //const T dist(dist2<T>(query, cloud.col(index)));
699  //const T dist((query - cloud.col(index)).squaredNorm());
700  T dist(0);
701  const T* qPtr(&query.coeff(0));
702  const T* dPtr(&cloud.coeff(0, index));
703  const int dim(query.size());
704  for (int i = 0; i < dim; ++i)
705  {
706  const T diff(*qPtr - *dPtr);
707  dist += diff*diff;
708  qPtr++; dPtr++;
709  }
710  if ((dist < heap.headValue()) &&
711  (allowSelfMatch || (dist > numeric_limits<T>::epsilon()))
712  )
713  heap.replaceHead(index, dist);
714  }
715  else
716  {
717  const unsigned cd(node.dim);
718  const T old_off(off.coeff(cd));
719  const T new_off(query.coeff(cd) - node.cutVal);
720  if (new_off > 0)
721  {
722  recurseKnn(query, node.rightChild, rd, heap, off, maxError, allowSelfMatch);
723  rd += - old_off*old_off + new_off*new_off;
724  if (rd * maxError < heap.headValue())
725  {
726  off.coeffRef(cd) = new_off;
727  recurseKnn(query, n + 1, rd, heap, off, maxError, allowSelfMatch);
728  off.coeffRef(cd) = old_off;
729  }
730  }
731  else
732  {
733  recurseKnn(query, n+1, rd, heap, off, maxError, allowSelfMatch);
734  rd += - old_off*old_off + new_off*new_off;
735  if (rd * maxError < heap.headValue())
736  {
737  off.coeffRef(cd) = new_off;
738  recurseKnn(query, node.rightChild, rd, heap, off, maxError, allowSelfMatch);
739  off.coeffRef(cd) = old_off;
740  }
741  }
742  }
743  }
744 
745  template struct KDTreeUnbalancedPtInLeavesImplicitBoundsStack<float,IndexHeapSTL<int,float>>;
746  template struct KDTreeUnbalancedPtInLeavesImplicitBoundsStack<float,IndexHeapBruteForceVector<int,float>>;
747  template struct KDTreeUnbalancedPtInLeavesImplicitBoundsStack<double,IndexHeapSTL<int,double>>;
748  template struct KDTreeUnbalancedPtInLeavesImplicitBoundsStack<double,IndexHeapBruteForceVector<int,double>>;
749 
750  template struct KDTreeUnbalancedPtInLeavesImplicitBoundsStack<float,IndexHeapSTL<int,float>,Eigen::Matrix3Xf>;
751  template struct KDTreeUnbalancedPtInLeavesImplicitBoundsStack<float,IndexHeapBruteForceVector<int,float>,Eigen::Matrix3Xf>;
752  template struct KDTreeUnbalancedPtInLeavesImplicitBoundsStack<double,IndexHeapSTL<int,double>,Eigen::Matrix3Xd>;
753  template struct KDTreeUnbalancedPtInLeavesImplicitBoundsStack<double,IndexHeapBruteForceVector<int,double>,Eigen::Matrix3Xd>;
754 
755  template struct KDTreeUnbalancedPtInLeavesImplicitBoundsStack<float,IndexHeapSTL<int,float>,Eigen::Map<const Eigen::Matrix3Xf, Eigen::Aligned> >;
756  template struct KDTreeUnbalancedPtInLeavesImplicitBoundsStack<float,IndexHeapBruteForceVector<int,float>,Eigen::Map<const Eigen::Matrix3Xf, Eigen::Aligned> >;
757  template struct KDTreeUnbalancedPtInLeavesImplicitBoundsStack<double,IndexHeapSTL<int,double>,Eigen::Map<const Eigen::Matrix3Xd, Eigen::Aligned> >;
758  template struct KDTreeUnbalancedPtInLeavesImplicitBoundsStack<double,IndexHeapBruteForceVector<int,double>,Eigen::Map<const Eigen::Matrix3Xd, Eigen::Aligned> >;
759 
760  template<typename T, typename CloudType>
761  unsigned KDTreeUnbalancedPtInLeavesExplicitBoundsStack<T, CloudType>::buildNodes(const BuildPointsIt first, const BuildPointsIt last, const Vector minValues, const Vector maxValues)
762  {
763  const size_t count(last - first);
764  const unsigned pos(nodes.size());
765 
766  //cerr << count << endl;
767  if (count == 1)
768  {
769  const int dim = -1-(first->index);
770  nodes.push_back(Node(dim));
771  return pos;
772  }
773 
774  // find the largest dimension of the box
775  const int cutDim = argMax<T>(maxValues - minValues);
776  T cutVal((maxValues(cutDim) + minValues(cutDim))/2);
777 
778  // TODO: do only sort once
779  // sort
780  sort(first, last, CompareDim(cutDim));
781 
782  // TODO: optimise using binary search
783  size_t rightStart(0);
784  while (rightStart < count && (first+rightStart)->pos.coeff(cutDim) < cutVal)
785  ++rightStart;
786 
787  // prevent trivial splits
788  if (rightStart == 0)
789  {
790  cutVal = first->pos.coeff(cutDim);
791  rightStart = 1;
792  }
793  else if (rightStart == count)
794  {
795  rightStart = count - 1;
796  cutVal = (first + rightStart)->pos.coeff(cutDim);
797  }
798 
799  // update bounds for left
800  Vector leftMaxValues(maxValues);
801  leftMaxValues[cutDim] = cutVal;
802  // update bounds for right
803  Vector rightMinValues(minValues);
804  rightMinValues[cutDim] = cutVal;
805 
806  // count for recursion
807  const size_t rightCount(count - rightStart);
808  const size_t leftCount(count - rightCount);
809 
810  // add this
811  nodes.push_back(Node(cutDim, cutVal, minValues.coeff(cutDim), maxValues.coeff(cutDim)));
812 
813  // recurse
814  const unsigned __attribute__ ((unused)) leftChild = buildNodes(first, first + leftCount, minValues, leftMaxValues);
815  assert(leftChild == pos + 1);
816  const unsigned rightChild = buildNodes(first + leftCount, last, rightMinValues, maxValues);
817 
818  // write right child index and return
819  nodes[pos].rightChild = rightChild;
820  return pos;
821  }
822 
823  template<typename T, typename CloudType>
824  KDTreeUnbalancedPtInLeavesExplicitBoundsStack<T, CloudType>::KDTreeUnbalancedPtInLeavesExplicitBoundsStack(const CloudType& cloud):
825  NearestNeighbourSearch<T, CloudType>::NearestNeighbourSearch(cloud)
826  {
827  // build point vector and compute bounds
828  BuildPoints buildPoints;
829  buildPoints.reserve(cloud.cols());
830  for (int i = 0; i < cloud.cols(); ++i)
831  {
832  const Vector& v(cloud.col(i));
833  buildPoints.push_back(BuildPoint(v, i));
834  const_cast<Vector&>(minBound) = minBound.cwise().min(v);
835  const_cast<Vector&>(maxBound) = maxBound.cwise().max(v);
836  }
837 
838  // create nodes
839  //nodes.resize(getTreeSize(cloud.cols()));
840  buildNodes(buildPoints.begin(), buildPoints.end(), minBound, maxBound);
841  //for (size_t i = 0; i < nodes.size(); ++i)
842  // cout << i << ": " << nodes[i].dim << " " << nodes[i].cutVal << " " << nodes[i].rightChild << endl;
843  }
844 
845  template<typename T, typename CloudType>
846  typename KDTreeUnbalancedPtInLeavesExplicitBoundsStack<T, CloudType>::IndexVector KDTreeUnbalancedPtInLeavesExplicitBoundsStack<T, CloudType>::knn(const Vector& query, const Index k, const T epsilon, const unsigned optionFlags)
847  {
848  const bool allowSelfMatch(optionFlags & NearestNeighbourSearch<T, CloudType>::ALLOW_SELF_MATCH);
849 
850  assert(nodes.size() > 0);
851  Heap heap(k);
852 
853  statistics.lastQueryVisitCount = 0;
854 
855  recurseKnn(query, 0, 0, heap, 1+epsilon, allowSelfMatch);
856 
857  if (optionFlags & NearestNeighbourSearch<T>::SORT_RESULTS)
858  heap.sort();
859 
860  statistics.totalVisitCount += statistics.lastQueryVisitCount;
861 
862  return heap.getIndexes();
863  }
864 
865  template<typename T, typename CloudType>
866  void KDTreeUnbalancedPtInLeavesExplicitBoundsStack<T, CloudType>::recurseKnn(const Vector& query, const size_t n, T rd, Heap& heap, const T maxError, const bool allowSelfMatch)
867  {
868  const Node& node(nodes[n]);
869  const int cd(node.dim);
870 
871  ++statistics.lastQueryVisitCount;
872 
873  if (cd < 0)
874  {
875  const int index(-(cd + 1));
876  const T dist(dist2<T>(query, cloud.col(index)));
877  if ((dist < heap.headValue()) &&
878  (allowSelfMatch || (dist > numeric_limits<T>::epsilon()))
879  )
880  heap.replaceHead(index, dist);
881  }
882  else
883  {
884  const T q_val(query.coeff(cd));
885  const T cut_diff(q_val - node.cutVal);
886  if (cut_diff < 0)
887  {
888  recurseKnn(query, n+1, rd, heap, maxError, allowSelfMatch);
889 
890  T box_diff = node.lowBound - q_val;
891  if (box_diff < 0)
892  box_diff = 0;
893 
894  rd += cut_diff*cut_diff - box_diff*box_diff;
895 
896  if (rd * maxError < heap.headValue())
897  recurseKnn(query, node.rightChild, rd, heap, maxError, allowSelfMatch);
898  }
899  else
900  {
901  recurseKnn(query, node.rightChild, rd, heap, maxError, allowSelfMatch);
902 
903  T box_diff = q_val - node.highBound;
904  if (box_diff < 0)
905  box_diff = 0;
906 
907  rd += cut_diff*cut_diff - box_diff*box_diff;
908 
909  if (rd * maxError < heap.headValue())
910  recurseKnn(query, n + 1, rd, heap, maxError, allowSelfMatch);
911  }
912  }
913  }
914 
915  template struct KDTreeUnbalancedPtInLeavesExplicitBoundsStack<float>;
916  template struct KDTreeUnbalancedPtInLeavesExplicitBoundsStack<double>;
917  template struct KDTreeUnbalancedPtInLeavesExplicitBoundsStack<float, Eigen::Matrix3Xf>;
918  template struct KDTreeUnbalancedPtInLeavesExplicitBoundsStack<double, Eigen::Matrix3Xd>;
919  template struct KDTreeUnbalancedPtInLeavesExplicitBoundsStack<float, Eigen::Map<const Eigen::Matrix3Xf, Eigen::Aligned> >;
920  template struct KDTreeUnbalancedPtInLeavesExplicitBoundsStack<double, Eigen::Map<const Eigen::Matrix3Xd, Eigen::Aligned> >;
921 }
Nabo::KDTreeBalancedPtInLeavesStack::Vector
NearestNeighbourSearch< T, CloudType >::Vector Vector
Definition: nabo_experimental.h:153
Nabo::KDTreeBalancedPtInNodes::childRight
size_t childRight(size_t pos) const
Definition: nabo_experimental.h:79
Nabo::KDTreeBalancedPtInNodesPQ::KDTreeBalancedPtInNodesPQ
KDTreeBalancedPtInNodesPQ(const CloudType &cloud)
Definition: experimental/kdtree_cpu.cpp:196
Nabo::IndexHeapSTL::sort
void sort()
sort the entries, from the smallest to the largest
Definition: index_heap.h:123
Nabo::KDTreeBalancedPtInNodesPQ::IndexVector
NearestNeighbourSearch< T, CloudType >::IndexVector IndexVector
Definition: nabo_experimental.h:97
Nabo::KDTreeBalancedPtInLeavesStack::Index
NearestNeighbourSearch< T, CloudType >::Index Index
Definition: nabo_experimental.h:155
Nabo::KDTreeBalancedPtInNodes::BuildPointsIt
BuildPoints::iterator BuildPointsIt
Definition: nabo_experimental.h:57
Nabo::KDTreeBalancedPtInLeavesStack::KDTreeBalancedPtInLeavesStack
KDTreeBalancedPtInLeavesStack(const CloudType &cloud, const bool balanceVariance)
Definition: experimental/kdtree_cpu.cpp:450
Nabo::NearestNeighbourSearch
Nearest neighbour search interface, templatized on scalar type.
Definition: nabo.h:258
Nabo::KDTreeBalancedPtInNodesStack::Node
KDTreeBalancedPtInNodes< T, CloudType >::Node Node
Definition: nabo_experimental.h:130
Nabo::IndexHeapSTL::headValue
const VT & headValue() const
get the largest value of the heap
Definition: index_heap.h:101
Nabo::KDTreeUnbalancedPtInLeavesExplicitBoundsStack::buildNodes
unsigned buildNodes(const BuildPointsIt first, const BuildPointsIt last, const Vector minValues, const Vector maxValues)
Definition: experimental/kdtree_cpu.cpp:761
std
Definition: any.hpp:450
Nabo::KDTreeUnbalancedPtInLeavesExplicitBoundsStack::IndexVector
NearestNeighbourSearch< T, CloudType >::IndexVector IndexVector
Definition: nabo_experimental.h:279
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStack::IndexMatrix
NearestNeighbourSearch< T, CloudType >::IndexMatrix IndexMatrix
Definition: nabo_experimental.h:214
Nabo::KDTreeBalancedPtInNodesStack::knn
virtual IndexVector knn(const Vector &query, const Index k, const T epsilon, const unsigned optionFlags)
Definition: experimental/kdtree_cpu.cpp:291
Nabo::KDTreeBalancedPtInNodes::getTreeSize
size_t getTreeSize(size_t size) const
Definition: experimental/kdtree_cpu.cpp:61
Nabo::KDTreeBalancedPtInLeavesStack::recurseKnn
void recurseKnn(const Vector &query, const size_t n, T rd, Heap &heap, Vector &off, const T maxError, const bool allowSelfMatch)
Definition: experimental/kdtree_cpu.cpp:493
Nabo::NearestNeighbourSearch< T, CloudType >::cloud
const CloudType & cloud
the reference to the data-point cloud, which must remain valid during the lifetime of the NearestNeig...
Definition: nabo.h:274
Nabo::KDTreeBalancedPtInLeavesStack::knn
virtual IndexVector knn(const Vector &query, const Index k, const T epsilon, const unsigned optionFlags)
Definition: experimental/kdtree_cpu.cpp:472
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStack::buildNodes
unsigned buildNodes(const BuildPointsIt first, const BuildPointsIt last, const Vector minValues, const Vector maxValues)
Definition: experimental/kdtree_cpu.cpp:550
Nabo::KDTreeBalancedPtInLeavesStack::childRight
size_t childRight(size_t pos) const
Definition: nabo_experimental.h:195
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStack::IndexVector
NearestNeighbourSearch< T, CloudType >::IndexVector IndexVector
Definition: nabo_experimental.h:213
Nabo::KDTreeBalancedPtInNodes::childLeft
size_t childLeft(size_t pos) const
Definition: nabo_experimental.h:78
Nabo::KDTreeBalancedPtInLeavesStack::childLeft
size_t childLeft(size_t pos) const
Definition: nabo_experimental.h:194
Nabo::KDTreeUnbalancedPtInLeavesExplicitBoundsStack::recurseKnn
void recurseKnn(const Vector &query, const size_t n, T rd, Heap &heap, const T maxError, const bool allowSelfMatch)
Definition: experimental/kdtree_cpu.cpp:866
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStack::Matrix
NearestNeighbourSearch< T, CloudType >::Matrix Matrix
Definition: nabo_experimental.h:211
Nabo::KDTreeUnbalancedPtInLeavesExplicitBoundsStack::Vector
NearestNeighbourSearch< T, CloudType >::Vector Vector
Definition: nabo_experimental.h:276
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStack::knn
virtual IndexVector knn(const Vector &query, const Index k, const T epsilon, const unsigned optionFlags)
Definition: experimental/kdtree_cpu.cpp:634
Nabo::KDTreeBalancedPtInNodes::buildNodes
void buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos)
Definition: experimental/kdtree_cpu.cpp:87
Nabo::argMax
size_t argMax(const typename NearestNeighbourSearch< T, CloudType >::Vector &v)
Return the index of the maximum value of a vector of positive values.
Definition: experimental/kdtree_cpu.cpp:45
Nabo::IndexHeapSTL::replaceHead
void replaceHead(const Index index, const Value value)
put value into heap, replace the largest value if full
Definition: index_heap.h:106
nabo_experimental.h
Nabo::NearestNeighbourSearch< T, CloudType >::maxBound
const Vector maxBound
the high bound of the search space (axis-aligned bounding box)
Definition: nabo.h:282
Nabo::KDTreeBalancedPtInNodesStack::IndexVector
NearestNeighbourSearch< T, CloudType >::IndexVector IndexVector
Definition: nabo_experimental.h:129
Nabo::IndexHeapSTL
balanced-tree implementation of heap
Definition: index_heap.h:52
Nabo::KDTreeBalancedPtInNodes::Vector
NearestNeighbourSearch< T, CloudType >::Vector Vector
Definition: nabo_experimental.h:44
Nabo::KDTreeBalancedPtInNodesPQ::knn
virtual IndexVector knn(const Vector &query, const Index k, const T epsilon, const unsigned optionFlags)
Definition: experimental/kdtree_cpu.cpp:202
Nabo::KDTreeBalancedPtInNodes::KDTreeBalancedPtInNodes
KDTreeBalancedPtInNodes(const CloudType &cloud)
Definition: experimental/kdtree_cpu.cpp:171
Nabo::NearestNeighbourSearch< T, CloudType >::minBound
const Vector minBound
the low bound of the search space (axis-aligned bounding box)
Definition: nabo.h:280
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStack::Index
NearestNeighbourSearch< T, CloudType >::Index Index
Definition: nabo_experimental.h:212
Nabo
Namespace for Nabo.
Definition: experimental/kdtree_cpu.cpp:40
Nabo::KDTreeBalancedPtInLeavesStack::IndexVector
NearestNeighbourSearch< T, CloudType >::IndexVector IndexVector
Definition: nabo_experimental.h:156
Nabo::KDTreeBalancedPtInNodes::IndexVector
NearestNeighbourSearch< T, CloudType >::IndexVector IndexVector
Definition: nabo_experimental.h:47
Nabo::KDTreeBalancedPtInLeavesStack::buildNodes
void buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos, const Vector minValues, const Vector maxValues, const bool balanceVariance)
Definition: experimental/kdtree_cpu.cpp:388
Nabo::KDTreeUnbalancedPtInLeavesExplicitBoundsStack::knn
virtual IndexVector knn(const Vector &query, const Index k, const T epsilon, const unsigned optionFlags)
Definition: experimental/kdtree_cpu.cpp:846
Nabo::NearestNeighbourSearch< T, CloudType >::dim
const Index dim
the dimensionality of the data-point cloud
Definition: nabo.h:276
Nabo::KDTreeBalancedPtInNodes::BuildPoints
std::vector< BuildPoint > BuildPoints
Definition: nabo_experimental.h:56
Nabo::KDTreeBalancedPtInNodes::Index
NearestNeighbourSearch< T, CloudType >::Index Index
Definition: nabo_experimental.h:46
Nabo::KDTreeBalancedPtInNodes::dump
void dump(const Vector minValues, const Vector maxValues, const size_t pos) const
Definition: experimental/kdtree_cpu.cpp:137
Nabo::KDTreeBalancedPtInLeavesStack::BuildPointsCstIt
BuildPoints::const_iterator BuildPointsCstIt
Definition: nabo_experimental.h:172
Nabo::KDTreeBalancedPtInNodes::cloudIndexesFromNodesIndexes
IndexVector cloudIndexesFromNodesIndexes(const IndexVector &indexes) const
Definition: experimental/kdtree_cpu.cpp:78
Nabo::KDTreeBalancedPtInLeavesStack::BuildPoints
std::vector< BuildPoint > BuildPoints
Definition: nabo_experimental.h:170
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStack::knnM
virtual IndexMatrix knnM(const Matrix &query, const Index k, const T epsilon, const unsigned optionFlags)
Definition: experimental/kdtree_cpu.cpp:655
Nabo::NearestNeighbourSearch< T, CloudType >::CloudType
CloudType CloudType
a column-major Eigen matrix in which each column is a point; this matrix has dim rows ...
Definition: nabo.h:265
Nabo::KDTreeBalancedPtInNodesStack::KDTreeBalancedPtInNodesStack
KDTreeBalancedPtInNodesStack(const CloudType &cloud)
Definition: experimental/kdtree_cpu.cpp:285
Nabo::KDTreeUnbalancedPtInLeavesExplicitBoundsStack::Index
NearestNeighbourSearch< T, CloudType >::Index Index
Definition: nabo_experimental.h:278
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStack::recurseKnn
void recurseKnn(const Vector &query, const unsigned n, T rd, Heap &heap, Vector &off, const T maxError, const bool allowSelfMatch)
Definition: experimental/kdtree_cpu.cpp:690
Nabo::KDTreeBalancedPtInNodes::BuildPointsCstIt
BuildPoints::const_iterator BuildPointsCstIt
Definition: nabo_experimental.h:58
Nabo::KDTreeBalancedPtInNodesStack::recurseKnn
void recurseKnn(const Vector &query, const size_t n, T rd, Heap &heap, Vector &off, const T maxError, const bool allowSelfMatch)
Definition: experimental/kdtree_cpu.cpp:313
test.q
q
Definition: test.py:8
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStack::Vector
NearestNeighbourSearch< T, CloudType >::Vector Vector
Definition: nabo_experimental.h:210

libnabo
Author(s): Stéphane Magnenat
autogenerated on Mon Feb 28 2022 22:41:38