nanoflann
C++ header-only ANN library
nanoflann.hpp
1 /***********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
5  * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
6  * Copyright 2011-2022 Jose Luis Blanco (joseluisblancoc@gmail.com).
7  * All rights reserved.
8  *
9  * THE BSD LICENSE
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * 1. Redistributions of source code must retain the above copyright
16  * notice, this list of conditions and the following disclaimer.
17  * 2. Redistributions in binary form must reproduce the above copyright
18  * notice, this list of conditions and the following disclaimer in the
19  * documentation and/or other materials provided with the distribution.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
22  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
23  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
24  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
26  * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
27  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
28  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
29  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
30  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  *************************************************************************/
32 
45 #pragma once
46 
47 #include <algorithm>
48 #include <array>
49 #include <cassert>
50 #include <cmath> // for abs()
51 #include <cstdlib> // for abs()
52 #include <functional>
53 #include <istream>
54 #include <limits> // std::reference_wrapper
55 #include <ostream>
56 #include <stdexcept>
57 #include <vector>
58 
60 #define NANOFLANN_VERSION 0x142
61 
62 // Avoid conflicting declaration of min/max macros in windows headers
63 #if !defined(NOMINMAX) && \
64  (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64))
65 #define NOMINMAX
66 #ifdef max
67 #undef max
68 #undef min
69 #endif
70 #endif
71 
72 namespace nanoflann
73 {
78 template <typename T>
80 {
81  return static_cast<T>(3.14159265358979323846);
82 }
83 
88 template <typename T, typename = int>
89 struct has_resize : std::false_type
90 {
91 };
92 
93 template <typename T>
94 struct has_resize<T, decltype((void)std::declval<T>().resize(1), 0)>
95  : std::true_type
96 {
97 };
98 
99 template <typename T, typename = int>
100 struct has_assign : std::false_type
101 {
102 };
103 
104 template <typename T>
105 struct has_assign<T, decltype((void)std::declval<T>().assign(1, 0), 0)>
106  : std::true_type
107 {
108 };
109 
113 template <typename Container>
114 inline typename std::enable_if<has_resize<Container>::value, void>::type resize(
115  Container& c, const size_t nElements)
116 {
117  c.resize(nElements);
118 }
119 
124 template <typename Container>
125 inline typename std::enable_if<!has_resize<Container>::value, void>::type
126  resize(Container& c, const size_t nElements)
127 {
128  if (nElements != c.size())
129  throw std::logic_error("Try to change the size of a std::array.");
130 }
131 
135 template <typename Container, typename T>
136 inline typename std::enable_if<has_assign<Container>::value, void>::type assign(
137  Container& c, const size_t nElements, const T& value)
138 {
139  c.assign(nElements, value);
140 }
141 
145 template <typename Container, typename T>
146 inline typename std::enable_if<!has_assign<Container>::value, void>::type
147  assign(Container& c, const size_t nElements, const T& value)
148 {
149  for (size_t i = 0; i < nElements; i++) c[i] = value;
150 }
151 
154 template <
155  typename _DistanceType, typename _IndexType = size_t,
156  typename _CountType = size_t>
158 {
159  public:
160  using DistanceType = _DistanceType;
161  using IndexType = _IndexType;
162  using CountType = _CountType;
163 
164  private:
165  IndexType* indices;
166  DistanceType* dists;
167  CountType capacity;
168  CountType count;
169 
170  public:
171  inline KNNResultSet(CountType capacity_)
172  : indices(0), dists(0), capacity(capacity_), count(0)
173  {
174  }
175 
176  inline void init(IndexType* indices_, DistanceType* dists_)
177  {
178  indices = indices_;
179  dists = dists_;
180  count = 0;
181  if (capacity)
182  dists[capacity - 1] = (std::numeric_limits<DistanceType>::max)();
183  }
184 
185  inline CountType size() const { return count; }
186 
187  inline bool full() const { return count == capacity; }
188 
194  inline bool addPoint(DistanceType dist, IndexType index)
195  {
196  CountType i;
197  for (i = count; i > 0; --i)
198  {
199 #ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same
200  // distance, the one with the lowest-index will be
201  // returned first.
202  if ((dists[i - 1] > dist) ||
203  ((dist == dists[i - 1]) && (indices[i - 1] > index)))
204  {
205 #else
206  if (dists[i - 1] > dist)
207  {
208 #endif
209  if (i < capacity)
210  {
211  dists[i] = dists[i - 1];
212  indices[i] = indices[i - 1];
213  }
214  }
215  else
216  break;
217  }
218  if (i < capacity)
219  {
220  dists[i] = dist;
221  indices[i] = index;
222  }
223  if (count < capacity) count++;
224 
225  // tell caller that the search shall continue
226  return true;
227  }
228 
229  inline DistanceType worstDist() const { return dists[capacity - 1]; }
230 };
231 
234 {
236  template <typename PairType>
237  inline bool operator()(const PairType& p1, const PairType& p2) const
238  {
239  return p1.second < p2.second;
240  }
241 };
242 
246 template <typename _DistanceType, typename _IndexType = size_t>
248 {
249  public:
250  using DistanceType = _DistanceType;
251  using IndexType = _IndexType;
252 
253  public:
254  const DistanceType radius;
255 
256  std::vector<std::pair<IndexType, DistanceType>>& m_indices_dists;
257 
258  inline RadiusResultSet(
259  DistanceType radius_,
260  std::vector<std::pair<IndexType, DistanceType>>& indices_dists)
261  : radius(radius_), m_indices_dists(indices_dists)
262  {
263  init();
264  }
265 
266  inline void init() { clear(); }
267  inline void clear() { m_indices_dists.clear(); }
268 
269  inline size_t size() const { return m_indices_dists.size(); }
270 
271  inline bool full() const { return true; }
272 
278  inline bool addPoint(DistanceType dist, IndexType index)
279  {
280  if (dist < radius)
281  m_indices_dists.push_back(std::make_pair(index, dist));
282  return true;
283  }
284 
285  inline DistanceType worstDist() const { return radius; }
286 
291  std::pair<IndexType, DistanceType> worst_item() const
292  {
293  if (m_indices_dists.empty())
294  throw std::runtime_error(
295  "Cannot invoke RadiusResultSet::worst_item() on "
296  "an empty list of results.");
297  using DistIt = typename std::vector<
298  std::pair<IndexType, DistanceType>>::const_iterator;
299  DistIt it = std::max_element(
300  m_indices_dists.begin(), m_indices_dists.end(), IndexDist_Sorter());
301  return *it;
302  }
303 };
304 
309 template <typename T>
310 void save_value(std::ostream& stream, const T& value)
311 {
312  stream.write(reinterpret_cast<const char*>(&value), sizeof(T));
313 }
314 
315 template <typename T>
316 void save_value(std::ostream& stream, const std::vector<T>& value)
317 {
318  size_t size = value.size();
319  stream.write(reinterpret_cast<const char*>(&size), sizeof(size_t));
320  stream.write(reinterpret_cast<const char*>(value.data()), sizeof(T) * size);
321 }
322 
323 template <typename T>
324 void load_value(std::istream& stream, T& value)
325 {
326  stream.read(reinterpret_cast<char*>(&value), sizeof(T));
327 }
328 
329 template <typename T>
330 void load_value(std::istream& stream, std::vector<T>& value)
331 {
332  size_t size;
333  stream.read(reinterpret_cast<char*>(&size), sizeof(size_t));
334  value.resize(size);
335  stream.read(reinterpret_cast<char*>(value.data()), sizeof(T) * size);
336 }
342 struct Metric
343 {
344 };
345 
356 template <
357  class T, class DataSource, typename _DistanceType = T,
358  typename AccessorType = uint32_t>
360 {
361  using ElementType = T;
362  using DistanceType = _DistanceType;
363 
364  const DataSource& data_source;
365 
366  L1_Adaptor(const DataSource& _data_source) : data_source(_data_source) {}
367 
368  inline DistanceType evalMetric(
369  const T* a, const AccessorType b_idx, size_t size,
370  DistanceType worst_dist = -1) const
371  {
372  DistanceType result = DistanceType();
373  const T* last = a + size;
374  const T* lastgroup = last - 3;
375  size_t d = 0;
376 
377  /* Process 4 items with each loop for efficiency. */
378  while (a < lastgroup)
379  {
380  const DistanceType diff0 =
381  std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++));
382  const DistanceType diff1 =
383  std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++));
384  const DistanceType diff2 =
385  std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++));
386  const DistanceType diff3 =
387  std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++));
388  result += diff0 + diff1 + diff2 + diff3;
389  a += 4;
390  if ((worst_dist > 0) && (result > worst_dist)) { return result; }
391  }
392  /* Process last 0-3 components. Not needed for standard vector lengths.
393  */
394  while (a < last)
395  { result += std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++)); }
396  return result;
397  }
398 
399  template <typename U, typename V>
400  inline DistanceType accum_dist(const U a, const V b, const size_t) const
401  {
402  return std::abs(a - b);
403  }
404 };
405 
416 template <
417  class T, class DataSource, typename _DistanceType = T,
418  typename AccessorType = uint32_t>
420 {
421  using ElementType = T;
422  using DistanceType = _DistanceType;
423 
424  const DataSource& data_source;
425 
426  L2_Adaptor(const DataSource& _data_source) : data_source(_data_source) {}
427 
428  inline DistanceType evalMetric(
429  const T* a, const AccessorType b_idx, size_t size,
430  DistanceType worst_dist = -1) const
431  {
432  DistanceType result = DistanceType();
433  const T* last = a + size;
434  const T* lastgroup = last - 3;
435  size_t d = 0;
436 
437  /* Process 4 items with each loop for efficiency. */
438  while (a < lastgroup)
439  {
440  const DistanceType diff0 =
441  a[0] - data_source.kdtree_get_pt(b_idx, d++);
442  const DistanceType diff1 =
443  a[1] - data_source.kdtree_get_pt(b_idx, d++);
444  const DistanceType diff2 =
445  a[2] - data_source.kdtree_get_pt(b_idx, d++);
446  const DistanceType diff3 =
447  a[3] - data_source.kdtree_get_pt(b_idx, d++);
448  result +=
449  diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
450  a += 4;
451  if ((worst_dist > 0) && (result > worst_dist)) { return result; }
452  }
453  /* Process last 0-3 components. Not needed for standard vector lengths.
454  */
455  while (a < last)
456  {
457  const DistanceType diff0 =
458  *a++ - data_source.kdtree_get_pt(b_idx, d++);
459  result += diff0 * diff0;
460  }
461  return result;
462  }
463 
464  template <typename U, typename V>
465  inline DistanceType accum_dist(const U a, const V b, const size_t) const
466  {
467  return (a - b) * (a - b);
468  }
469 };
470 
481 template <
482  class T, class DataSource, typename _DistanceType = T,
483  typename AccessorType = uint32_t>
485 {
486  using ElementType = T;
487  using DistanceType = _DistanceType;
488 
489  const DataSource& data_source;
490 
491  L2_Simple_Adaptor(const DataSource& _data_source)
492  : data_source(_data_source)
493  {
494  }
495 
496  inline DistanceType evalMetric(
497  const T* a, const AccessorType b_idx, size_t size) const
498  {
499  DistanceType result = DistanceType();
500  for (size_t i = 0; i < size; ++i)
501  {
502  const DistanceType diff =
503  a[i] - data_source.kdtree_get_pt(b_idx, i);
504  result += diff * diff;
505  }
506  return result;
507  }
508 
509  template <typename U, typename V>
510  inline DistanceType accum_dist(const U a, const V b, const size_t) const
511  {
512  return (a - b) * (a - b);
513  }
514 };
515 
526 template <
527  class T, class DataSource, typename _DistanceType = T,
528  typename AccessorType = uint32_t>
530 {
531  using ElementType = T;
532  using DistanceType = _DistanceType;
533 
534  const DataSource& data_source;
535 
536  SO2_Adaptor(const DataSource& _data_source) : data_source(_data_source) {}
537 
538  inline DistanceType evalMetric(
539  const T* a, const AccessorType b_idx, size_t size) const
540  {
541  return accum_dist(
542  a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1), size - 1);
543  }
544 
547  template <typename U, typename V>
548  inline DistanceType accum_dist(const U a, const V b, const size_t) const
549  {
550  DistanceType result = DistanceType();
551  DistanceType PI = pi_const<DistanceType>();
552  result = b - a;
553  if (result > PI)
554  result -= 2 * PI;
555  else if (result < -PI)
556  result += 2 * PI;
557  return result;
558  }
559 };
560 
571 template <
572  class T, class DataSource, typename _DistanceType = T,
573  typename AccessorType = uint32_t>
575 {
576  using ElementType = T;
577  using DistanceType = _DistanceType;
578 
580  distance_L2_Simple;
581 
582  SO3_Adaptor(const DataSource& _data_source)
583  : distance_L2_Simple(_data_source)
584  {
585  }
586 
587  inline DistanceType evalMetric(
588  const T* a, const AccessorType b_idx, size_t size) const
589  {
590  return distance_L2_Simple.evalMetric(a, b_idx, size);
591  }
592 
593  template <typename U, typename V>
594  inline DistanceType accum_dist(const U a, const V b, const size_t idx) const
595  {
596  return distance_L2_Simple.accum_dist(a, b, idx);
597  }
598 };
599 
601 struct metric_L1 : public Metric
602 {
603  template <class T, class DataSource, typename AccessorType = uint32_t>
604  struct traits
605  {
607  };
608 };
610 struct metric_L2 : public Metric
611 {
612  template <class T, class DataSource, typename AccessorType = uint32_t>
613  struct traits
614  {
616  };
617 };
619 struct metric_L2_Simple : public Metric
620 {
621  template <class T, class DataSource, typename AccessorType = uint32_t>
622  struct traits
623  {
625  };
626 };
628 struct metric_SO2 : public Metric
629 {
630  template <class T, class DataSource, typename AccessorType = uint32_t>
631  struct traits
632  {
634  };
635 };
637 struct metric_SO3 : public Metric
638 {
639  template <class T, class DataSource, typename AccessorType = uint32_t>
640  struct traits
641  {
643  };
644 };
645 
653 {
654  KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10)
655  : leaf_max_size(_leaf_max_size)
656  {
657  }
658 
659  size_t leaf_max_size;
660 };
661 
664 {
667  SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true)
668  : checks(checks_IGNORED_), eps(eps_), sorted(sorted_)
669  {
670  }
671 
672  int checks;
674  float eps;
675  bool sorted;
677 };
690 template <typename T>
691 inline T* allocate(size_t count = 1)
692 {
693  T* mem = static_cast<T*>(::malloc(sizeof(T) * count));
694  return mem;
695 }
696 
712 const size_t WORDSIZE = 16;
713 const size_t BLOCKSIZE = 8192;
714 
716 {
717  /* We maintain memory alignment to word boundaries by requiring that all
718  allocations be in multiples of the machine wordsize. */
719  /* Size of machine word in bytes. Must be power of 2. */
720  /* Minimum number of bytes requested at a time from the system. Must be
721  * multiple of WORDSIZE. */
722 
723  using Offset = uint32_t;
724  using Size = uint32_t;
725  using Dimension = int32_t;
726 
727  Size remaining; /* Number of bytes left in current block of storage. */
728  void* base; /* Pointer to base of current block of storage. */
729  void* loc; /* Current location in block to next allocate memory. */
730 
731  void internal_init()
732  {
733  remaining = 0;
734  base = nullptr;
735  usedMemory = 0;
736  wastedMemory = 0;
737  }
738 
739  public:
740  Size usedMemory;
741  Size wastedMemory;
742 
746  PooledAllocator() { internal_init(); }
747 
751  ~PooledAllocator() { free_all(); }
752 
754  void free_all()
755  {
756  while (base != nullptr)
757  {
758  void* prev =
759  *(static_cast<void**>(base)); /* Get pointer to prev block. */
760  ::free(base);
761  base = prev;
762  }
763  internal_init();
764  }
765 
770  void* malloc(const size_t req_size)
771  {
772  /* Round size up to a multiple of wordsize. The following expression
773  only works for WORDSIZE that is a power of 2, by masking last bits
774  of incremented size to zero.
775  */
776  const Size size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1);
777 
778  /* Check whether a new block must be allocated. Note that the first
779  word of a block is reserved for a pointer to the previous block.
780  */
781  if (size > remaining)
782  {
783  wastedMemory += remaining;
784 
785  /* Allocate new storage. */
786  const Size blocksize =
787  (size + sizeof(void*) + (WORDSIZE - 1) > BLOCKSIZE)
788  ? size + sizeof(void*) + (WORDSIZE - 1)
789  : BLOCKSIZE;
790 
791  // use the standard C malloc to allocate memory
792  void* m = ::malloc(blocksize);
793  if (!m)
794  {
795  fprintf(stderr, "Failed to allocate memory.\n");
796  throw std::bad_alloc();
797  }
798 
799  /* Fill first word of new block with pointer to previous block. */
800  static_cast<void**>(m)[0] = base;
801  base = m;
802 
803  Size shift = 0;
804  // int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) &
805  // (WORDSIZE-1))) & (WORDSIZE-1);
806 
807  remaining = blocksize - sizeof(void*) - shift;
808  loc = (static_cast<char*>(m) + sizeof(void*) + shift);
809  }
810  void* rloc = loc;
811  loc = static_cast<char*>(loc) + size;
812  remaining -= size;
813 
814  usedMemory += size;
815 
816  return rloc;
817  }
818 
826  template <typename T>
827  T* allocate(const size_t count = 1)
828  {
829  T* mem = static_cast<T*>(this->malloc(sizeof(T) * count));
830  return mem;
831  }
832 };
841 template <int32_t DIM, typename T>
843 {
844  using container_t = std::array<T, DIM>;
845 };
847 template <typename T>
849 {
850  using container_t = std::vector<T>;
851 };
852 
867 template <
868  class Derived, typename Distance, class DatasetAdaptor, int32_t DIM = -1,
869  typename AccessorType = uint32_t>
871 {
872  public:
875  void freeIndex(Derived& obj)
876  {
877  obj.pool.free_all();
878  obj.root_node = nullptr;
879  obj.m_size_at_index_build = 0;
880  }
881 
882  using ElementType = typename Distance::ElementType;
883  using DistanceType = typename Distance::DistanceType;
884 
888  std::vector<AccessorType> vAcc;
889 
890  using Offset = typename decltype(vAcc)::size_type;
891  using Size = typename decltype(vAcc)::size_type;
892  using Dimension = int32_t;
893 
894  /*--------------------- Internal Data Structures
895  * --------------------------*/
896  struct Node
897  {
900  union
901  {
902  struct leaf
903  {
904  Offset left, right;
905  } lr;
906  struct nonleaf
907  {
908  Dimension divfeat;
909  DistanceType divlow,
911  } sub;
912  } node_type;
914  Node *child1, *child2;
915  };
916 
917  using NodePtr = Node*;
918 
919  struct Interval
920  {
921  ElementType low, high;
922  };
923 
924  NodePtr root_node;
925 
926  Size m_leaf_max_size;
927 
928  Size m_size;
931  Dimension dim;
932 
935  using BoundingBox =
937 
941  typename array_or_vector_selector<DIM, DistanceType>::container_t;
942 
946 
955 
957  Size size(const Derived& obj) const { return obj.m_size; }
958 
960  Size veclen(const Derived& obj) { return DIM > 0 ? DIM : obj.dim; }
961 
963  inline ElementType dataset_get(
964  const Derived& obj, AccessorType element, Dimension component) const
965  {
966  return obj.dataset.kdtree_get_pt(element, component);
967  }
968 
973  Size usedMemory(Derived& obj)
974  {
975  return obj.pool.usedMemory + obj.pool.wastedMemory +
976  obj.dataset.kdtree_get_point_count() *
977  sizeof(AccessorType); // pool memory and vind array memory
978  }
979 
980  void computeMinMax(
981  const Derived& obj, Offset ind, Size count, Dimension element,
982  ElementType& min_elem, ElementType& max_elem)
983  {
984  min_elem = dataset_get(obj, vAcc[ind], element);
985  max_elem = min_elem;
986  for (Offset i = 1; i < count; ++i)
987  {
988  ElementType val = dataset_get(obj, vAcc[ind + i], element);
989  if (val < min_elem) min_elem = val;
990  if (val > max_elem) max_elem = val;
991  }
992  }
993 
1002  Derived& obj, const Offset left, const Offset right, BoundingBox& bbox)
1003  {
1004  NodePtr node = obj.pool.template allocate<Node>(); // allocate memory
1005 
1006  /* If too few exemplars remain, then make this a leaf node. */
1007  if ((right - left) <= static_cast<Offset>(obj.m_leaf_max_size))
1008  {
1009  node->child1 = node->child2 = nullptr; /* Mark as leaf node. */
1010  node->node_type.lr.left = left;
1011  node->node_type.lr.right = right;
1012 
1013  // compute bounding-box of leaf points
1014  for (Dimension i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i)
1015  {
1016  bbox[i].low = dataset_get(obj, obj.vAcc[left], i);
1017  bbox[i].high = dataset_get(obj, obj.vAcc[left], i);
1018  }
1019  for (Offset k = left + 1; k < right; ++k)
1020  {
1021  for (Dimension i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i)
1022  {
1023  if (bbox[i].low > dataset_get(obj, obj.vAcc[k], i))
1024  bbox[i].low = dataset_get(obj, obj.vAcc[k], i);
1025  if (bbox[i].high < dataset_get(obj, obj.vAcc[k], i))
1026  bbox[i].high = dataset_get(obj, obj.vAcc[k], i);
1027  }
1028  }
1029  }
1030  else
1031  {
1032  Offset idx;
1033  Dimension cutfeat;
1034  DistanceType cutval;
1035  middleSplit_(obj, left, right - left, idx, cutfeat, cutval, bbox);
1036 
1037  node->node_type.sub.divfeat = cutfeat;
1038 
1039  BoundingBox left_bbox(bbox);
1040  left_bbox[cutfeat].high = cutval;
1041  node->child1 = divideTree(obj, left, left + idx, left_bbox);
1042 
1043  BoundingBox right_bbox(bbox);
1044  right_bbox[cutfeat].low = cutval;
1045  node->child2 = divideTree(obj, left + idx, right, right_bbox);
1046 
1047  node->node_type.sub.divlow = left_bbox[cutfeat].high;
1048  node->node_type.sub.divhigh = right_bbox[cutfeat].low;
1049 
1050  for (Dimension i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i)
1051  {
1052  bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
1053  bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
1054  }
1055  }
1056 
1057  return node;
1058  }
1059 
1060  void middleSplit_(
1061  Derived& obj, Offset ind, Size count, Offset& index, Dimension& cutfeat,
1062  DistanceType& cutval, const BoundingBox& bbox)
1063  {
1064  const auto EPS = static_cast<DistanceType>(0.00001);
1065  ElementType max_span = bbox[0].high - bbox[0].low;
1066  for (Dimension i = 1; i < (DIM > 0 ? DIM : obj.dim); ++i)
1067  {
1068  ElementType span = bbox[i].high - bbox[i].low;
1069  if (span > max_span) { max_span = span; }
1070  }
1071  ElementType max_spread = -1;
1072  cutfeat = 0;
1073  for (Dimension i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i)
1074  {
1075  ElementType span = bbox[i].high - bbox[i].low;
1076  if (span > (1 - EPS) * max_span)
1077  {
1078  ElementType min_elem, max_elem;
1079  computeMinMax(obj, ind, count, i, min_elem, max_elem);
1080  ElementType spread = max_elem - min_elem;
1081  if (spread > max_spread)
1082  {
1083  cutfeat = i;
1084  max_spread = spread;
1085  }
1086  }
1087  }
1088  // split in the middle
1089  DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2;
1090  ElementType min_elem, max_elem;
1091  computeMinMax(obj, ind, count, cutfeat, min_elem, max_elem);
1092 
1093  if (split_val < min_elem)
1094  cutval = min_elem;
1095  else if (split_val > max_elem)
1096  cutval = max_elem;
1097  else
1098  cutval = split_val;
1099 
1100  Offset lim1, lim2;
1101  planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2);
1102 
1103  if (lim1 > count / 2)
1104  index = lim1;
1105  else if (lim2 < count / 2)
1106  index = lim2;
1107  else
1108  index = count / 2;
1109  }
1110 
1121  Derived& obj, Offset ind, const Size count, Dimension cutfeat,
1122  DistanceType& cutval, Offset& lim1, Offset& lim2)
1123  {
1124  /* Move vector indices for left subtree to front of list. */
1125  Offset left = 0;
1126  Offset right = count - 1;
1127  for (;;)
1128  {
1129  while (left <= right &&
1130  dataset_get(obj, vAcc[ind + left], cutfeat) < cutval)
1131  ++left;
1132  while (right && left <= right &&
1133  dataset_get(obj, vAcc[ind + right], cutfeat) >= cutval)
1134  --right;
1135  if (left > right || !right)
1136  break; // "!right" was added to support unsigned Index types
1137  std::swap(vAcc[ind + left], vAcc[ind + right]);
1138  ++left;
1139  --right;
1140  }
1141  /* If either list is empty, it means that all remaining features
1142  * are identical. Split in the middle to maintain a balanced tree.
1143  */
1144  lim1 = left;
1145  right = count - 1;
1146  for (;;)
1147  {
1148  while (left <= right &&
1149  dataset_get(obj, vAcc[ind + left], cutfeat) <= cutval)
1150  ++left;
1151  while (right && left <= right &&
1152  dataset_get(obj, vAcc[ind + right], cutfeat) > cutval)
1153  --right;
1154  if (left > right || !right)
1155  break; // "!right" was added to support unsigned Index types
1156  std::swap(vAcc[ind + left], vAcc[ind + right]);
1157  ++left;
1158  --right;
1159  }
1160  lim2 = left;
1161  }
1162 
1163  DistanceType computeInitialDistances(
1164  const Derived& obj, const ElementType* vec,
1165  distance_vector_t& dists) const
1166  {
1167  assert(vec);
1168  DistanceType distsq = DistanceType();
1169 
1170  for (Dimension i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i)
1171  {
1172  if (vec[i] < obj.root_bbox[i].low)
1173  {
1174  dists[i] =
1175  obj.distance.accum_dist(vec[i], obj.root_bbox[i].low, i);
1176  distsq += dists[i];
1177  }
1178  if (vec[i] > obj.root_bbox[i].high)
1179  {
1180  dists[i] =
1181  obj.distance.accum_dist(vec[i], obj.root_bbox[i].high, i);
1182  distsq += dists[i];
1183  }
1184  }
1185  return distsq;
1186  }
1187 
1188  void save_tree(Derived& obj, std::ostream& stream, NodePtr tree)
1189  {
1190  save_value(stream, *tree);
1191  if (tree->child1 != nullptr) { save_tree(obj, stream, tree->child1); }
1192  if (tree->child2 != nullptr) { save_tree(obj, stream, tree->child2); }
1193  }
1194 
1195  void load_tree(Derived& obj, std::istream& stream, NodePtr& tree)
1196  {
1197  tree = obj.pool.template allocate<Node>();
1198  load_value(stream, *tree);
1199  if (tree->child1 != nullptr) { load_tree(obj, stream, tree->child1); }
1200  if (tree->child2 != nullptr) { load_tree(obj, stream, tree->child2); }
1201  }
1202 
1208  void saveIndex_(Derived& obj, std::ostream& stream)
1209  {
1210  save_value(stream, obj.m_size);
1211  save_value(stream, obj.dim);
1212  save_value(stream, obj.root_bbox);
1213  save_value(stream, obj.m_leaf_max_size);
1214  save_value(stream, obj.vAcc);
1215  save_tree(obj, stream, obj.root_node);
1216  }
1217 
1223  void loadIndex_(Derived& obj, std::istream& stream)
1224  {
1225  load_value(stream, obj.m_size);
1226  load_value(stream, obj.dim);
1227  load_value(stream, obj.root_bbox);
1228  load_value(stream, obj.m_leaf_max_size);
1229  load_value(stream, obj.vAcc);
1230  load_tree(obj, stream, obj.root_node);
1231  }
1232 };
1233 
1274 template <
1275  typename Distance, class DatasetAdaptor, int32_t DIM = -1,
1276  typename AccessorType = uint32_t>
1278  : public KDTreeBaseClass<
1279  KDTreeSingleIndexAdaptor<Distance, DatasetAdaptor, DIM, AccessorType>,
1280  Distance, DatasetAdaptor, DIM, AccessorType>
1281 {
1282  public:
1285  Distance, DatasetAdaptor, DIM, AccessorType>&) =
1286  delete;
1287 
1291  const DatasetAdaptor& dataset;
1292 
1293  const KDTreeSingleIndexAdaptorParams index_params;
1294 
1295  Distance distance;
1296 
1297  using BaseClassRef = typename nanoflann::KDTreeBaseClass<
1299  Distance, DatasetAdaptor, DIM, AccessorType>,
1300  Distance, DatasetAdaptor, DIM, AccessorType>;
1301 
1302  using Offset = typename BaseClassRef::Offset;
1303  using Size = typename BaseClassRef::Size;
1304  using Dimension = typename BaseClassRef::Dimension;
1305 
1306  using ElementType = typename BaseClassRef::ElementType;
1307  using DistanceType = typename BaseClassRef::DistanceType;
1308 
1309  using Node = typename BaseClassRef::Node;
1310  using NodePtr = Node*;
1311 
1312  using Interval = typename BaseClassRef::Interval;
1313 
1316  using BoundingBox = typename BaseClassRef::BoundingBox;
1317 
1320  using distance_vector_t = typename BaseClassRef::distance_vector_t;
1321 
1341  template <class... Args>
1343  const Dimension dimensionality, const DatasetAdaptor& inputData,
1344  const KDTreeSingleIndexAdaptorParams& params = {}, Args&&... args)
1345  : dataset(inputData),
1346  index_params(params),
1347  distance(inputData, std::forward<Args>(args)...)
1348  {
1349  BaseClassRef::root_node = nullptr;
1350  BaseClassRef::m_size = dataset.kdtree_get_point_count();
1351  BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
1352  BaseClassRef::dim = dimensionality;
1353  if (DIM > 0) BaseClassRef::dim = DIM;
1354  BaseClassRef::m_leaf_max_size = params.leaf_max_size;
1355 
1356  buildIndex();
1357  }
1358 
1362  void buildIndex()
1363  {
1364  BaseClassRef::m_size = dataset.kdtree_get_point_count();
1365  BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
1366  init_vind();
1367  this->freeIndex(*this);
1368  BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
1369  if (BaseClassRef::m_size == 0) return;
1370  computeBoundingBox(BaseClassRef::root_bbox);
1371  BaseClassRef::root_node = this->divideTree(
1372  *this, 0, BaseClassRef::m_size,
1373  BaseClassRef::root_bbox); // construct the tree
1374  }
1375 
1392  template <typename RESULTSET>
1394  RESULTSET& result, const ElementType* vec,
1395  const SearchParams& searchParams) const
1396  {
1397  assert(vec);
1398  if (this->size(*this) == 0) return false;
1399  if (!BaseClassRef::root_node)
1400  throw std::runtime_error(
1401  "[nanoflann] findNeighbors() called before building the "
1402  "index.");
1403  float epsError = 1 + searchParams.eps;
1404 
1406  dists; // fixed or variable-sized container (depending on DIM)
1407  auto zero = static_cast<decltype(result.worstDist())>(0);
1408  assign(
1409  dists, (DIM > 0 ? DIM : BaseClassRef::dim),
1410  zero); // Fill it with zeros.
1411  DistanceType distsq = this->computeInitialDistances(*this, vec, dists);
1412  searchLevel(
1413  result, vec, BaseClassRef::root_node, distsq, dists,
1414  epsError); // "count_leaf" parameter removed since was neither
1415  // used nor returned to the user.
1416  return result.full();
1417  }
1418 
1429  const ElementType* query_point, const Size num_closest,
1430  AccessorType* out_indices, DistanceType* out_distances_sq,
1431  const int /* nChecks_IGNORED */ = 10) const
1432  {
1434  num_closest);
1435  resultSet.init(out_indices, out_distances_sq);
1436  this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
1437  return resultSet.size();
1438  }
1439 
1457  const ElementType* query_point, const DistanceType& radius,
1458  std::vector<std::pair<AccessorType, DistanceType>>& IndicesDists,
1459  const SearchParams& searchParams) const
1460  {
1462  radius, IndicesDists);
1463  const Size nFound =
1464  radiusSearchCustomCallback(query_point, resultSet, searchParams);
1465  if (searchParams.sorted)
1466  std::sort(
1467  IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter());
1468  return nFound;
1469  }
1470 
1476  template <class SEARCH_CALLBACK>
1478  const ElementType* query_point, SEARCH_CALLBACK& resultSet,
1479  const SearchParams& searchParams = SearchParams()) const
1480  {
1481  this->findNeighbors(resultSet, query_point, searchParams);
1482  return resultSet.size();
1483  }
1484 
1487  public:
1490  void init_vind()
1491  {
1492  // Create a permutable array of indices to the input vectors.
1493  BaseClassRef::m_size = dataset.kdtree_get_point_count();
1494  if (BaseClassRef::vAcc.size() != BaseClassRef::m_size)
1495  BaseClassRef::vAcc.resize(BaseClassRef::m_size);
1496  for (Size i = 0; i < BaseClassRef::m_size; i++)
1497  BaseClassRef::vAcc[i] = i;
1498  }
1499 
1500  void computeBoundingBox(BoundingBox& bbox)
1501  {
1502  resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim));
1503  if (dataset.kdtree_get_bbox(bbox))
1504  {
1505  // Done! It was implemented in derived class
1506  }
1507  else
1508  {
1509  const Size N = dataset.kdtree_get_point_count();
1510  if (!N)
1511  throw std::runtime_error(
1512  "[nanoflann] computeBoundingBox() called but "
1513  "no data points found.");
1514  for (Dimension i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i)
1515  {
1516  bbox[i].low = bbox[i].high =
1517  this->dataset_get(*this, BaseClassRef::vAcc[0], i);
1518  }
1519  for (Offset k = 1; k < N; ++k)
1520  {
1521  for (Dimension i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim);
1522  ++i)
1523  {
1524  if (this->dataset_get(*this, BaseClassRef::vAcc[k], i) <
1525  bbox[i].low)
1526  bbox[i].low =
1527  this->dataset_get(*this, BaseClassRef::vAcc[k], i);
1528  if (this->dataset_get(*this, BaseClassRef::vAcc[k], i) >
1529  bbox[i].high)
1530  bbox[i].high =
1531  this->dataset_get(*this, BaseClassRef::vAcc[k], i);
1532  }
1533  }
1534  }
1535  }
1536 
1543  template <class RESULTSET>
1545  RESULTSET& result_set, const ElementType* vec, const NodePtr node,
1546  DistanceType mindistsq, distance_vector_t& dists,
1547  const float epsError) const
1548  {
1549  /* If this is a leaf node, then do check and return. */
1550  if ((node->child1 == nullptr) && (node->child2 == nullptr))
1551  {
1552  // count_leaf += (node->lr.right-node->lr.left); // Removed since
1553  // was neither used nor returned to the user.
1554  DistanceType worst_dist = result_set.worstDist();
1555  for (Offset i = node->node_type.lr.left;
1556  i < node->node_type.lr.right; ++i)
1557  {
1558  const AccessorType accessor =
1559  BaseClassRef::vAcc[i]; // reorder... : i;
1560  DistanceType dist = distance.evalMetric(
1561  vec, accessor, (DIM > 0 ? DIM : BaseClassRef::dim));
1562  if (dist < worst_dist)
1563  {
1564  if (!result_set.addPoint(dist, BaseClassRef::vAcc[i]))
1565  {
1566  // the resultset doesn't want to receive any more
1567  // points, we're done searching!
1568  return false;
1569  }
1570  }
1571  }
1572  return true;
1573  }
1574 
1575  /* Which child branch should be taken first? */
1576  Dimension idx = node->node_type.sub.divfeat;
1577  ElementType val = vec[idx];
1578  DistanceType diff1 = val - node->node_type.sub.divlow;
1579  DistanceType diff2 = val - node->node_type.sub.divhigh;
1580 
1581  NodePtr bestChild;
1582  NodePtr otherChild;
1583  DistanceType cut_dist;
1584  if ((diff1 + diff2) < 0)
1585  {
1586  bestChild = node->child1;
1587  otherChild = node->child2;
1588  cut_dist =
1589  distance.accum_dist(val, node->node_type.sub.divhigh, idx);
1590  }
1591  else
1592  {
1593  bestChild = node->child2;
1594  otherChild = node->child1;
1595  cut_dist =
1596  distance.accum_dist(val, node->node_type.sub.divlow, idx);
1597  }
1598 
1599  /* Call recursively to search next level down. */
1600  if (!searchLevel(
1601  result_set, vec, bestChild, mindistsq, dists, epsError))
1602  {
1603  // the resultset doesn't want to receive any more points, we're done
1604  // searching!
1605  return false;
1606  }
1607 
1608  DistanceType dst = dists[idx];
1609  mindistsq = mindistsq + cut_dist - dst;
1610  dists[idx] = cut_dist;
1611  if (mindistsq * epsError <= result_set.worstDist())
1612  {
1613  if (!searchLevel(
1614  result_set, vec, otherChild, mindistsq, dists, epsError))
1615  {
1616  // the resultset doesn't want to receive any more points, we're
1617  // done searching!
1618  return false;
1619  }
1620  }
1621  dists[idx] = dst;
1622  return true;
1623  }
1624 
1625  public:
1631  void saveIndex(std::ostream& stream) { this->saveIndex_(*this, stream); }
1632 
1638  void loadIndex(std::istream& stream) { this->loadIndex_(*this, stream); }
1639 
1640 }; // class KDTree
1641 
1678 template <
1679  typename Distance, class DatasetAdaptor, int32_t DIM = -1,
1680  typename AccessorType = uint32_t>
1682  : public KDTreeBaseClass<
1683  KDTreeSingleIndexDynamicAdaptor_<
1684  Distance, DatasetAdaptor, DIM, AccessorType>,
1685  Distance, DatasetAdaptor, DIM, AccessorType>
1686 {
1687  public:
1691  const DatasetAdaptor& dataset;
1692 
1693  KDTreeSingleIndexAdaptorParams index_params;
1694 
1695  std::vector<int>& treeIndex;
1696 
1697  Distance distance;
1698 
1699  using BaseClassRef = typename nanoflann::KDTreeBaseClass<
1701  Distance, DatasetAdaptor, DIM, AccessorType>,
1702  Distance, DatasetAdaptor, DIM, AccessorType>;
1703 
1704  using ElementType = typename BaseClassRef::ElementType;
1705  using DistanceType = typename BaseClassRef::DistanceType;
1706 
1707  using Offset = typename BaseClassRef::Offset;
1708  using Size = typename BaseClassRef::Size;
1709  using Dimension = typename BaseClassRef::Dimension;
1710 
1711  using Node = typename BaseClassRef::Node;
1712  using NodePtr = Node*;
1713 
1714  using Interval = typename BaseClassRef::Interval;
1717  using BoundingBox = typename BaseClassRef::BoundingBox;
1718 
1721  using distance_vector_t = typename BaseClassRef::distance_vector_t;
1722 
1738  const Dimension dimensionality, const DatasetAdaptor& inputData,
1739  std::vector<int>& treeIndex_,
1740  const KDTreeSingleIndexAdaptorParams& params =
1742  : dataset(inputData),
1743  index_params(params),
1744  treeIndex(treeIndex_),
1745  distance(inputData)
1746  {
1747  BaseClassRef::root_node = nullptr;
1748  BaseClassRef::m_size = 0;
1749  BaseClassRef::m_size_at_index_build = 0;
1750  BaseClassRef::dim = dimensionality;
1751  if (DIM > 0) BaseClassRef::dim = DIM;
1752  BaseClassRef::m_leaf_max_size = params.leaf_max_size;
1753  }
1754 
1757  const KDTreeSingleIndexDynamicAdaptor_& rhs) = default;
1758 
1762  {
1764  std::swap(BaseClassRef::vAcc, tmp.BaseClassRef::vAcc);
1765  std::swap(
1766  BaseClassRef::m_leaf_max_size, tmp.BaseClassRef::m_leaf_max_size);
1767  std::swap(index_params, tmp.index_params);
1768  std::swap(treeIndex, tmp.treeIndex);
1769  std::swap(BaseClassRef::m_size, tmp.BaseClassRef::m_size);
1770  std::swap(
1771  BaseClassRef::m_size_at_index_build,
1772  tmp.BaseClassRef::m_size_at_index_build);
1773  std::swap(BaseClassRef::root_node, tmp.BaseClassRef::root_node);
1774  std::swap(BaseClassRef::root_bbox, tmp.BaseClassRef::root_bbox);
1775  std::swap(BaseClassRef::pool, tmp.BaseClassRef::pool);
1776  return *this;
1777  }
1778 
1782  void buildIndex()
1783  {
1784  BaseClassRef::m_size = BaseClassRef::vAcc.size();
1785  this->freeIndex(*this);
1786  BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
1787  if (BaseClassRef::m_size == 0) return;
1788  computeBoundingBox(BaseClassRef::root_bbox);
1789  BaseClassRef::root_node = this->divideTree(
1790  *this, 0, BaseClassRef::m_size,
1791  BaseClassRef::root_bbox); // construct the tree
1792  }
1793 
1810  template <typename RESULTSET>
1812  RESULTSET& result, const ElementType* vec,
1813  const SearchParams& searchParams) const
1814  {
1815  assert(vec);
1816  if (this->size(*this) == 0) return false;
1817  if (!BaseClassRef::root_node) return false;
1818  float epsError = 1 + searchParams.eps;
1819 
1820  // fixed or variable-sized container (depending on DIM)
1821  distance_vector_t dists;
1822  // Fill it with zeros.
1823  assign(
1824  dists, (DIM > 0 ? DIM : BaseClassRef::dim),
1825  static_cast<typename distance_vector_t::value_type>(0));
1826  DistanceType distsq = this->computeInitialDistances(*this, vec, dists);
1827  searchLevel(
1828  result, vec, BaseClassRef::root_node, distsq, dists,
1829  epsError); // "count_leaf" parameter removed since was neither
1830  // used nor returned to the user.
1831  return result.full();
1832  }
1833 
1844  const ElementType* query_point, const Size num_closest,
1845  AccessorType* out_indices, DistanceType* out_distances_sq,
1846  const int /* nChecks_IGNORED */ = 10) const
1847  {
1849  num_closest);
1850  resultSet.init(out_indices, out_distances_sq);
1851  this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
1852  return resultSet.size();
1853  }
1854 
1872  const ElementType* query_point, const DistanceType& radius,
1873  std::vector<std::pair<AccessorType, DistanceType>>& IndicesDists,
1874  const SearchParams& searchParams) const
1875  {
1877  radius, IndicesDists);
1878  const size_t nFound =
1879  radiusSearchCustomCallback(query_point, resultSet, searchParams);
1880  if (searchParams.sorted)
1881  std::sort(
1882  IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter());
1883  return nFound;
1884  }
1885 
1891  template <class SEARCH_CALLBACK>
1893  const ElementType* query_point, SEARCH_CALLBACK& resultSet,
1894  const SearchParams& searchParams = SearchParams()) const
1895  {
1896  this->findNeighbors(resultSet, query_point, searchParams);
1897  return resultSet.size();
1898  }
1899 
1902  public:
1903  void computeBoundingBox(BoundingBox& bbox)
1904  {
1905  resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim));
1906 
1907  if (dataset.kdtree_get_bbox(bbox))
1908  {
1909  // Done! It was implemented in derived class
1910  }
1911  else
1912  {
1913  const Size N = BaseClassRef::m_size;
1914  if (!N)
1915  throw std::runtime_error(
1916  "[nanoflann] computeBoundingBox() called but "
1917  "no data points found.");
1918  for (Dimension i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i)
1919  {
1920  bbox[i].low = bbox[i].high =
1921  this->dataset_get(*this, BaseClassRef::vAcc[0], i);
1922  }
1923  for (Offset k = 1; k < N; ++k)
1924  {
1925  for (Dimension i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim);
1926  ++i)
1927  {
1928  if (this->dataset_get(*this, BaseClassRef::vAcc[k], i) <
1929  bbox[i].low)
1930  bbox[i].low =
1931  this->dataset_get(*this, BaseClassRef::vAcc[k], i);
1932  if (this->dataset_get(*this, BaseClassRef::vAcc[k], i) >
1933  bbox[i].high)
1934  bbox[i].high =
1935  this->dataset_get(*this, BaseClassRef::vAcc[k], i);
1936  }
1937  }
1938  }
1939  }
1940 
1945  template <class RESULTSET>
1947  RESULTSET& result_set, const ElementType* vec, const NodePtr node,
1948  DistanceType mindistsq, distance_vector_t& dists,
1949  const float epsError) const
1950  {
1951  /* If this is a leaf node, then do check and return. */
1952  if ((node->child1 == nullptr) && (node->child2 == nullptr))
1953  {
1954  // count_leaf += (node->lr.right-node->lr.left); // Removed since
1955  // was neither used nor returned to the user.
1956  DistanceType worst_dist = result_set.worstDist();
1957  for (Offset i = node->node_type.lr.left;
1958  i < node->node_type.lr.right; ++i)
1959  {
1960  const AccessorType index =
1961  BaseClassRef::vAcc[i]; // reorder... : i;
1962  if (treeIndex[index] == -1) continue;
1963  DistanceType dist = distance.evalMetric(
1964  vec, index, (DIM > 0 ? DIM : BaseClassRef::dim));
1965  if (dist < worst_dist)
1966  {
1967  if (!result_set.addPoint(
1968  static_cast<typename RESULTSET::DistanceType>(dist),
1969  static_cast<typename RESULTSET::IndexType>(
1970  BaseClassRef::vAcc[i])))
1971  {
1972  // the resultset doesn't want to receive any more
1973  // points, we're done searching!
1974  return; // false;
1975  }
1976  }
1977  }
1978  return;
1979  }
1980 
1981  /* Which child branch should be taken first? */
1982  Dimension idx = node->node_type.sub.divfeat;
1983  ElementType val = vec[idx];
1984  DistanceType diff1 = val - node->node_type.sub.divlow;
1985  DistanceType diff2 = val - node->node_type.sub.divhigh;
1986 
1987  NodePtr bestChild;
1988  NodePtr otherChild;
1989  DistanceType cut_dist;
1990  if ((diff1 + diff2) < 0)
1991  {
1992  bestChild = node->child1;
1993  otherChild = node->child2;
1994  cut_dist =
1995  distance.accum_dist(val, node->node_type.sub.divhigh, idx);
1996  }
1997  else
1998  {
1999  bestChild = node->child2;
2000  otherChild = node->child1;
2001  cut_dist =
2002  distance.accum_dist(val, node->node_type.sub.divlow, idx);
2003  }
2004 
2005  /* Call recursively to search next level down. */
2006  searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError);
2007 
2008  DistanceType dst = dists[idx];
2009  mindistsq = mindistsq + cut_dist - dst;
2010  dists[idx] = cut_dist;
2011  if (mindistsq * epsError <= result_set.worstDist())
2012  {
2013  searchLevel(
2014  result_set, vec, otherChild, mindistsq, dists, epsError);
2015  }
2016  dists[idx] = dst;
2017  }
2018 
2019  public:
2025  void saveIndex(std::ostream& stream) { this->saveIndex_(*this, stream); }
2026 
2032  void loadIndex(std::istream& stream) { this->loadIndex_(*this, stream); }
2033 };
2034 
2049 template <
2050  typename Distance, class DatasetAdaptor, int32_t DIM = -1,
2051  typename AccessorType = uint32_t>
2053 {
2054  public:
2055  using ElementType = typename Distance::ElementType;
2056  using DistanceType = typename Distance::DistanceType;
2057 
2058  using Offset = typename KDTreeSingleIndexDynamicAdaptor_<
2059  Distance, DatasetAdaptor, DIM>::Offset;
2060  using Size = typename KDTreeSingleIndexDynamicAdaptor_<
2061  Distance, DatasetAdaptor, DIM>::Size;
2062  using Dimension = typename KDTreeSingleIndexDynamicAdaptor_<
2063  Distance, DatasetAdaptor, DIM>::Dimension;
2064 
2065  protected:
2066  Size m_leaf_max_size;
2067  Size treeCount;
2068  Size pointCount;
2069 
2073  const DatasetAdaptor& dataset;
2074 
2075  std::vector<int>
2079 
2080  KDTreeSingleIndexAdaptorParams index_params;
2081 
2082  Dimension dim;
2083 
2084  using index_container_t =
2086  std::vector<index_container_t> index;
2087 
2088  public:
2091  const std::vector<index_container_t>& getAllIndices() const
2092  {
2093  return index;
2094  }
2095 
2096  private:
2098  int First0Bit(AccessorType num)
2099  {
2100  int pos = 0;
2101  while (num & 1)
2102  {
2103  num = num >> 1;
2104  pos++;
2105  }
2106  return pos;
2107  }
2108 
2110  void init()
2111  {
2112  using my_kd_tree_t =
2113  KDTreeSingleIndexDynamicAdaptor_<Distance, DatasetAdaptor, DIM>;
2114  std::vector<my_kd_tree_t> index_(
2115  treeCount,
2116  my_kd_tree_t(dim /*dim*/, dataset, treeIndex, index_params));
2117  index = index_;
2118  }
2119 
2120  public:
2121  Distance distance;
2122 
2138  const int dimensionality, const DatasetAdaptor& inputData,
2139  const KDTreeSingleIndexAdaptorParams& params =
2141  const size_t maximumPointCount = 1000000000U)
2142  : dataset(inputData), index_params(params), distance(inputData)
2143  {
2144  treeCount = static_cast<size_t>(std::log2(maximumPointCount));
2145  pointCount = 0U;
2146  dim = dimensionality;
2147  treeIndex.clear();
2148  if (DIM > 0) dim = DIM;
2149  m_leaf_max_size = params.leaf_max_size;
2150  init();
2151  const size_t num_initial_points = dataset.kdtree_get_point_count();
2152  if (num_initial_points > 0) { addPoints(0, num_initial_points - 1); }
2153  }
2154 
2158  Distance, DatasetAdaptor, DIM, AccessorType>&) = delete;
2159 
2161  void addPoints(AccessorType start, AccessorType end)
2162  {
2163  Size count = end - start + 1;
2164  treeIndex.resize(treeIndex.size() + count);
2165  for (AccessorType idx = start; idx <= end; idx++)
2166  {
2167  int pos = First0Bit(pointCount);
2168  index[pos].vAcc.clear();
2169  treeIndex[pointCount] = pos;
2170  for (int i = 0; i < pos; i++)
2171  {
2172  for (int j = 0; j < static_cast<int>(index[i].vAcc.size()); j++)
2173  {
2174  index[pos].vAcc.push_back(index[i].vAcc[j]);
2175  if (treeIndex[index[i].vAcc[j]] != -1)
2176  treeIndex[index[i].vAcc[j]] = pos;
2177  }
2178  index[i].vAcc.clear();
2179  index[i].freeIndex(index[i]);
2180  }
2181  index[pos].vAcc.push_back(idx);
2182  index[pos].buildIndex();
2183  pointCount++;
2184  }
2185  }
2186 
2188  void removePoint(size_t idx)
2189  {
2190  if (idx >= pointCount) return;
2191  treeIndex[idx] = -1;
2192  }
2193 
2207  template <typename RESULTSET>
2209  RESULTSET& result, const ElementType* vec,
2210  const SearchParams& searchParams) const
2211  {
2212  for (size_t i = 0; i < treeCount; i++)
2213  { index[i].findNeighbors(result, &vec[0], searchParams); }
2214  return result.full();
2215  }
2216 };
2217 
2243 template <
2244  class MatrixType, int32_t DIM = -1, class Distance = nanoflann::metric_L2,
2245  bool row_major = true>
2247 {
2248  using self_t =
2250  using num_t = typename MatrixType::Scalar;
2251  using IndexType = typename MatrixType::Index;
2252  using metric_t = typename Distance::template traits<
2253  num_t, self_t, IndexType>::distance_t;
2254 
2256  metric_t, self_t,
2257  row_major ? MatrixType::ColsAtCompileTime
2258  : MatrixType::RowsAtCompileTime,
2259  IndexType>;
2260 
2261  index_t* index;
2263 
2264  using Offset = typename index_t::Offset;
2265  using Size = typename index_t::Size;
2266  using Dimension = typename index_t::Dimension;
2267 
2270  const Dimension dimensionality,
2271  const std::reference_wrapper<const MatrixType>& mat,
2272  const int leaf_max_size = 10)
2273  : m_data_matrix(mat)
2274  {
2275  const auto dims = row_major ? mat.get().cols() : mat.get().rows();
2276  if (static_cast<Dimension>(dims) != dimensionality)
2277  throw std::runtime_error(
2278  "Error: 'dimensionality' must match column count in data "
2279  "matrix");
2280  if (DIM > 0 && static_cast<int32_t>(dims) != DIM)
2281  throw std::runtime_error(
2282  "Data set dimensionality does not match the 'DIM' template "
2283  "argument");
2284  index = new index_t(
2285  dims, *this /* adaptor */,
2287  index->buildIndex();
2288  }
2289 
2290  public:
2293 
2294  ~KDTreeEigenMatrixAdaptor() { delete index; }
2295 
2296  const std::reference_wrapper<const MatrixType> m_data_matrix;
2297 
2304  inline void query(
2305  const num_t* query_point, const Size num_closest,
2306  IndexType* out_indices, num_t* out_distances_sq,
2307  const int /* nChecks_IGNORED */ = 10) const
2308  {
2309  nanoflann::KNNResultSet<num_t, IndexType> resultSet(num_closest);
2310  resultSet.init(out_indices, out_distances_sq);
2311  index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
2312  }
2313 
2317  const self_t& derived() const { return *this; }
2318  self_t& derived() { return *this; }
2319 
2320  // Must return the number of data points
2321  inline Size kdtree_get_point_count() const
2322  {
2323  if (row_major)
2324  return m_data_matrix.get().rows();
2325  else
2326  return m_data_matrix.get().cols();
2327  }
2328 
2329  // Returns the dim'th component of the idx'th point in the class:
2330  inline num_t kdtree_get_pt(const IndexType idx, size_t dim) const
2331  {
2332  if (row_major)
2333  return m_data_matrix.get().coeff(idx, IndexType(dim));
2334  else
2335  return m_data_matrix.get().coeff(IndexType(dim), idx);
2336  }
2337 
2338  // Optional bounding-box computation: return false to default to a standard
2339  // bbox computation loop.
2340  // Return true if the BBOX was already computed by the class and returned
2341  // in "bb" so it can be avoided to redo it again. Look at bb.size() to
2342  // find out the expected dimensionality (e.g. 2 or 3 for point clouds)
2343  template <class BBOX>
2344  bool kdtree_get_bbox(BBOX& /*bb*/) const
2345  {
2346  return false;
2347  }
2348 
2351 }; // end of KDTreeEigenMatrixAdaptor // end of grouping
2355 } // namespace nanoflann
Definition: nanoflann.hpp:871
Size veclen(const Derived &obj)
Definition: nanoflann.hpp:960
NodePtr divideTree(Derived &obj, const Offset left, const Offset right, BoundingBox &bbox)
Definition: nanoflann.hpp:1001
PooledAllocator pool
Definition: nanoflann.hpp:954
Dimension dim
Dimensionality of each data point.
Definition: nanoflann.hpp:931
Size m_size_at_index_build
Definition: nanoflann.hpp:929
ElementType dataset_get(const Derived &obj, AccessorType element, Dimension component) const
Helper accessor to the dataset points:
Definition: nanoflann.hpp:963
Size size(const Derived &obj) const
Definition: nanoflann.hpp:957
BoundingBox root_bbox
Definition: nanoflann.hpp:945
Size usedMemory(Derived &obj)
Definition: nanoflann.hpp:973
void saveIndex_(Derived &obj, std::ostream &stream)
Definition: nanoflann.hpp:1208
std::vector< AccessorType > vAcc
Definition: nanoflann.hpp:888
void planeSplit(Derived &obj, Offset ind, const Size count, Dimension cutfeat, DistanceType &cutval, Offset &lim1, Offset &lim2)
Definition: nanoflann.hpp:1120
void freeIndex(Derived &obj)
Definition: nanoflann.hpp:875
void loadIndex_(Derived &obj, std::istream &stream)
Definition: nanoflann.hpp:1223
typename array_or_vector_selector< DIM, Interval >::container_t BoundingBox
Definition: nanoflann.hpp:936
Size m_size
Number of current points in the dataset.
Definition: nanoflann.hpp:928
typename array_or_vector_selector< DIM, DistanceType >::container_t distance_vector_t
Definition: nanoflann.hpp:941
Definition: nanoflann.hpp:1281
void saveIndex(std::ostream &stream)
Definition: nanoflann.hpp:1631
Size radiusSearch(const ElementType *query_point, const DistanceType &radius, std::vector< std::pair< AccessorType, DistanceType >> &IndicesDists, const SearchParams &searchParams) const
Definition: nanoflann.hpp:1456
void buildIndex()
Definition: nanoflann.hpp:1362
bool searchLevel(RESULTSET &result_set, const ElementType *vec, const NodePtr node, DistanceType mindistsq, distance_vector_t &dists, const float epsError) const
Definition: nanoflann.hpp:1544
KDTreeSingleIndexAdaptor(const KDTreeSingleIndexAdaptor< Distance, DatasetAdaptor, DIM, AccessorType > &)=delete
Size radiusSearchCustomCallback(const ElementType *query_point, SEARCH_CALLBACK &resultSet, const SearchParams &searchParams=SearchParams()) const
Definition: nanoflann.hpp:1477
KDTreeSingleIndexAdaptor(const Dimension dimensionality, const DatasetAdaptor &inputData, const KDTreeSingleIndexAdaptorParams &params={}, Args &&... args)
Definition: nanoflann.hpp:1342
bool findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const
Definition: nanoflann.hpp:1393
typename BaseClassRef::distance_vector_t distance_vector_t
Definition: nanoflann.hpp:1320
Size knnSearch(const ElementType *query_point, const Size num_closest, AccessorType *out_indices, DistanceType *out_distances_sq, const int=10) const
Definition: nanoflann.hpp:1428
void init_vind()
Definition: nanoflann.hpp:1490
void loadIndex(std::istream &stream)
Definition: nanoflann.hpp:1638
const DatasetAdaptor & dataset
The source of our data.
Definition: nanoflann.hpp:1291
typename BaseClassRef::BoundingBox BoundingBox
Definition: nanoflann.hpp:1316
Definition: nanoflann.hpp:1686
const DatasetAdaptor & dataset
The source of our data.
Definition: nanoflann.hpp:1691
void searchLevel(RESULTSET &result_set, const ElementType *vec, const NodePtr node, DistanceType mindistsq, distance_vector_t &dists, const float epsError) const
Definition: nanoflann.hpp:1946
Size radiusSearchCustomCallback(const ElementType *query_point, SEARCH_CALLBACK &resultSet, const SearchParams &searchParams=SearchParams()) const
Definition: nanoflann.hpp:1892
typename BaseClassRef::distance_vector_t distance_vector_t
Definition: nanoflann.hpp:1721
Size radiusSearch(const ElementType *query_point, const DistanceType &radius, std::vector< std::pair< AccessorType, DistanceType >> &IndicesDists, const SearchParams &searchParams) const
Definition: nanoflann.hpp:1871
void buildIndex()
Definition: nanoflann.hpp:1782
void loadIndex(std::istream &stream)
Definition: nanoflann.hpp:2032
KDTreeSingleIndexDynamicAdaptor_(const KDTreeSingleIndexDynamicAdaptor_ &rhs)=default
typename BaseClassRef::BoundingBox BoundingBox
Definition: nanoflann.hpp:1717
KDTreeSingleIndexDynamicAdaptor_ operator=(const KDTreeSingleIndexDynamicAdaptor_ &rhs)
Definition: nanoflann.hpp:1760
bool findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const
Definition: nanoflann.hpp:1811
KDTreeSingleIndexDynamicAdaptor_(const Dimension dimensionality, const DatasetAdaptor &inputData, std::vector< int > &treeIndex_, const KDTreeSingleIndexAdaptorParams &params=KDTreeSingleIndexAdaptorParams())
Definition: nanoflann.hpp:1737
Size knnSearch(const ElementType *query_point, const Size num_closest, AccessorType *out_indices, DistanceType *out_distances_sq, const int=10) const
Definition: nanoflann.hpp:1843
void saveIndex(std::ostream &stream)
Definition: nanoflann.hpp:2025
Definition: nanoflann.hpp:2053
void removePoint(size_t idx)
Definition: nanoflann.hpp:2188
const DatasetAdaptor & dataset
The source of our data.
Definition: nanoflann.hpp:2073
std::vector< int > treeIndex
Definition: nanoflann.hpp:2076
void addPoints(AccessorType start, AccessorType end)
Definition: nanoflann.hpp:2161
Dimension dim
Dimensionality of each data point.
Definition: nanoflann.hpp:2082
KDTreeSingleIndexDynamicAdaptor(const KDTreeSingleIndexDynamicAdaptor< Distance, DatasetAdaptor, DIM, AccessorType > &)=delete
const std::vector< index_container_t > & getAllIndices() const
Definition: nanoflann.hpp:2091
KDTreeSingleIndexDynamicAdaptor(const int dimensionality, const DatasetAdaptor &inputData, const KDTreeSingleIndexAdaptorParams &params=KDTreeSingleIndexAdaptorParams(), const size_t maximumPointCount=1000000000U)
Definition: nanoflann.hpp:2137
bool findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const
Definition: nanoflann.hpp:2208
Definition: nanoflann.hpp:158
bool addPoint(DistanceType dist, IndexType index)
Definition: nanoflann.hpp:194
Definition: nanoflann.hpp:716
T * allocate(const size_t count=1)
Definition: nanoflann.hpp:827
~PooledAllocator()
Definition: nanoflann.hpp:751
void free_all()
Definition: nanoflann.hpp:754
PooledAllocator()
Definition: nanoflann.hpp:746
void * malloc(const size_t req_size)
Definition: nanoflann.hpp:770
Definition: nanoflann.hpp:248
std::pair< IndexType, DistanceType > worst_item() const
Definition: nanoflann.hpp:291
bool addPoint(DistanceType dist, IndexType index)
Definition: nanoflann.hpp:278
const size_t WORDSIZE
Definition: nanoflann.hpp:712
T * allocate(size_t count=1)
Definition: nanoflann.hpp:691
T pi_const()
Definition: nanoflann.hpp:79
std::enable_if< has_assign< Container >::value, void >::type assign(Container &c, const size_t nElements, const T &value)
Definition: nanoflann.hpp:136
std::enable_if< has_resize< Container >::value, void >::type resize(Container &c, const size_t nElements)
Definition: nanoflann.hpp:114
Definition: nanoflann.hpp:234
bool operator()(const PairType &p1, const PairType &p2) const
Definition: nanoflann.hpp:237
Definition: nanoflann.hpp:920
Definition: nanoflann.hpp:897
DistanceType divhigh
The values used for subdivision.
Definition: nanoflann.hpp:910
Dimension divfeat
Dimension used for subdivision.
Definition: nanoflann.hpp:908
Offset right
Indices of points in leaf node.
Definition: nanoflann.hpp:904
union nanoflann::KDTreeBaseClass::Node::@0 node_type
Node * child1
Definition: nanoflann.hpp:914
Definition: nanoflann.hpp:2247
void query(const num_t *query_point, const Size num_closest, IndexType *out_indices, num_t *out_distances_sq, const int=10) const
Definition: nanoflann.hpp:2304
KDTreeEigenMatrixAdaptor(const Dimension dimensionality, const std::reference_wrapper< const MatrixType > &mat, const int leaf_max_size=10)
Constructor: takes a const ref to the matrix object with the data points.
Definition: nanoflann.hpp:2269
KDTreeEigenMatrixAdaptor(const self_t &)=delete
typename index_t::Offset Offset
Definition: nanoflann.hpp:2264
Definition: nanoflann.hpp:653
Definition: nanoflann.hpp:360
Definition: nanoflann.hpp:420
Definition: nanoflann.hpp:485
Definition: nanoflann.hpp:343
Definition: nanoflann.hpp:530
DistanceType accum_dist(const U a, const V b, const size_t) const
Definition: nanoflann.hpp:548
Definition: nanoflann.hpp:575
Definition: nanoflann.hpp:664
bool sorted
distance (default: true)
Definition: nanoflann.hpp:675
SearchParams(int checks_IGNORED_=32, float eps_=0, bool sorted_=true)
Definition: nanoflann.hpp:667
float eps
search for eps-approximate neighbours (default: 0)
Definition: nanoflann.hpp:674
int checks
Definition: nanoflann.hpp:672
Definition: nanoflann.hpp:843
Definition: nanoflann.hpp:101
Definition: nanoflann.hpp:90
Definition: nanoflann.hpp:605
Definition: nanoflann.hpp:602
Definition: nanoflann.hpp:614
Definition: nanoflann.hpp:623
Definition: nanoflann.hpp:620
Definition: nanoflann.hpp:611
Definition: nanoflann.hpp:632
Definition: nanoflann.hpp:629
Definition: nanoflann.hpp:641
Definition: nanoflann.hpp:638