ECCE @ EIC Software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
nanoflann.hpp
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file 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-2016 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 
46 #ifndef NANOFLANN_HPP_
47 #define NANOFLANN_HPP_
48 
49 #include <algorithm>
50 #include <cassert>
51 #include <cmath> // for abs()
52 #include <cstdio> // for fwrite()
53 #include <cstdlib> // for abs()
54 #include <limits>
55 #include <stdexcept>
56 #include <vector>
57 
58 // Avoid conflicting declaration of min/max macros in windows headers
59 #if !defined(NOMINMAX) && (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64))
60 #define NOMINMAX
61 #ifdef max
62 #undef max
63 #undef min
64 #endif
65 #endif
66 
67 namespace nanoflann
68 {
73 #define NANOFLANN_VERSION 0x123
74 
77  template <typename DistanceType, typename IndexType = size_t, typename CountType = size_t>
79  {
80  IndexType* indices;
81  DistanceType* dists;
82  CountType capacity;
83  CountType count;
84 
85  public:
86  inline KNNResultSet(CountType capacity_)
87  : indices(0)
88  , dists(0)
89  , capacity(capacity_)
90  , count(0)
91  {
92  }
93 
94  inline void init(IndexType* indices_, DistanceType* dists_)
95  {
96  indices = indices_;
97  dists = dists_;
98  count = 0;
99  if (capacity)
101  }
102 
103  inline CountType size() const
104  {
105  return count;
106  }
107 
108  inline bool full() const
109  {
110  return count == capacity;
111  }
112 
113  inline void addPoint(DistanceType dist, IndexType index)
114  {
115  CountType i;
116  for (i = count; i > 0; --i)
117  {
118 #ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same distance, the one with the lowest-index will be returned first.
119  if ((dists[i - 1] > dist) || ((dist == dists[i - 1]) && (indices[i - 1] > index)))
120  {
121 #else
122  if (dists[i - 1] > dist)
123  {
124 #endif
125  if (i < capacity)
126  {
127  dists[i] = dists[i - 1];
128  indices[i] = indices[i - 1];
129  }
130  }
131  else
132  break;
133  }
134  if (i < capacity)
135  {
136  dists[i] = dist;
137  indices[i] = index;
138  }
139  if (count < capacity) count++;
140  }
141 
142  inline DistanceType worstDist() const
143  {
144  return dists[capacity - 1];
145  }
146  }; // namespace nanoflann
147 
151  template <typename DistanceType, typename IndexType = size_t>
153  {
154  public:
155  const DistanceType radius;
156 
157  std::vector<std::pair<IndexType, DistanceType> >& m_indices_dists;
158 
159  inline RadiusResultSet(DistanceType radius_, std::vector<std::pair<IndexType, DistanceType> >& indices_dists)
160  : radius(radius_)
161  , m_indices_dists(indices_dists)
162  {
163  init();
164  }
165 
166  inline ~RadiusResultSet() {}
167 
168  inline void init() { clear(); }
169  inline void clear() { m_indices_dists.clear(); }
170 
171  inline size_t size() const { return m_indices_dists.size(); }
172 
173  inline bool full() const { return true; }
174 
175  inline void addPoint(DistanceType dist, IndexType index)
176  {
177  if (dist < radius)
178  m_indices_dists.push_back(std::make_pair(index, dist));
179  }
180 
181  inline DistanceType worstDist() const { return radius; }
182 
184  inline void set_radius_and_clear(const DistanceType r)
185  {
186  radius = r;
187  clear();
188  }
189 
194  std::pair<IndexType, DistanceType> worst_item() const
195  {
196  if (m_indices_dists.empty()) throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on an empty list of results.");
197  typedef typename std::vector<std::pair<IndexType, DistanceType> >::const_iterator DistIt;
198  DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end());
199  return *it;
200  }
201  };
202 
205  {
207  template <typename PairType>
208  inline bool operator()(const PairType& p1, const PairType& p2) const
209  {
210  return p1.second < p2.second;
211  }
212  };
213 
218  template <typename T>
219  void save_value(FILE* stream, const T& value, size_t count = 1)
220  {
221  fwrite(&value, sizeof(value), count, stream);
222  }
223 
224  template <typename T>
225  void save_value(FILE* stream, const std::vector<T>& value)
226  {
227  size_t size = value.size();
228  fwrite(&size, sizeof(size_t), 1, stream);
229  fwrite(&value[0], sizeof(T), size, stream);
230  }
231 
232  template <typename T>
233  void load_value(FILE* stream, T& value, size_t count = 1)
234  {
235  size_t read_cnt = fread(&value, sizeof(value), count, stream);
236  if (read_cnt != count)
237  {
238  throw std::runtime_error("Cannot read from file");
239  }
240  }
241 
242  template <typename T>
243  void load_value(FILE* stream, std::vector<T>& value)
244  {
245  size_t size;
246  size_t read_cnt = fread(&size, sizeof(size_t), 1, stream);
247  if (read_cnt != 1)
248  {
249  throw std::runtime_error("Cannot read from file");
250  }
251  value.resize(size);
252  read_cnt = fread(&value[0], sizeof(T), size, stream);
253  if (read_cnt != size)
254  {
255  throw std::runtime_error("Cannot read from file");
256  }
257  }
268  template <class T, class DataSource, typename _DistanceType = T>
269  struct L1_Adaptor
270  {
271  typedef T ElementType;
272  typedef _DistanceType DistanceType;
273 
274  const DataSource& data_source;
275 
276  L1_Adaptor(const DataSource& _data_source)
277  : data_source(_data_source)
278  {
279  }
280 
281  inline DistanceType operator()(const T* a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const
282  {
283  DistanceType result = DistanceType();
284  const T* last = a + size;
285  const T* lastgroup = last - 3;
286  size_t d = 0;
287 
288  /* Process 4 items with each loop for efficiency. */
289  while (a < lastgroup)
290  {
291  const DistanceType diff0 = std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++));
292  const DistanceType diff1 = std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++));
293  const DistanceType diff2 = std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++));
294  const DistanceType diff3 = std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++));
295  result += diff0 + diff1 + diff2 + diff3;
296  a += 4;
297  if ((worst_dist > 0) && (result > worst_dist))
298  {
299  return result;
300  }
301  }
302  /* Process last 0-3 components. Not needed for standard vector lengths. */
303  while (a < last)
304  {
305  result += std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++));
306  }
307  return result;
308  }
309 
310  template <typename U, typename V>
311  inline DistanceType accum_dist(const U a, const V b, int) const
312  {
313  return std::abs(a - b);
314  }
315  };
316 
322  template <class T, class DataSource, typename _DistanceType = T>
323  struct L2_Adaptor
324  {
325  typedef T ElementType;
326  typedef _DistanceType DistanceType;
327 
328  const DataSource& data_source;
329 
330  L2_Adaptor(const DataSource& _data_source)
331  : data_source(_data_source)
332  {
333  }
334 
335  inline DistanceType operator()(const T* a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const
336  {
337  DistanceType result = DistanceType();
338  const T* last = a + size;
339  const T* lastgroup = last - 3;
340  size_t d = 0;
341 
342  /* Process 4 items with each loop for efficiency. */
343  while (a < lastgroup)
344  {
345  const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx, d++);
346  const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx, d++);
347  const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx, d++);
348  const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx, d++);
349  result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
350  a += 4;
351  if ((worst_dist > 0) && (result > worst_dist))
352  {
353  return result;
354  }
355  }
356  /* Process last 0-3 components. Not needed for standard vector lengths. */
357  while (a < last)
358  {
359  const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx, d++);
360  result += diff0 * diff0;
361  }
362  return result;
363  }
364 
365  template <typename U, typename V>
366  inline DistanceType accum_dist(const U a, const V b, int) const
367  {
368  return (a - b) * (a - b);
369  }
370  };
371 
377  template <class T, class DataSource, typename _DistanceType = T>
379  {
380  typedef T ElementType;
381  typedef _DistanceType DistanceType;
382 
383  const DataSource& data_source;
384 
385  L2_Simple_Adaptor(const DataSource& _data_source)
386  : data_source(_data_source)
387  {
388  }
389 
390  inline DistanceType operator()(const T* a, const size_t b_idx, size_t size) const
391  {
392  return data_source.kdtree_distance(a, b_idx, size);
393  }
394 
395  template <typename U, typename V>
396  inline DistanceType accum_dist(const U a, const V b, int) const
397  {
398  return (a - b) * (a - b);
399  }
400  };
401 
403  struct metric_L1
404  {
405  template <class T, class DataSource>
406  struct traits
407  {
409  };
410  };
412  struct metric_L2
413  {
414  template <class T, class DataSource>
415  struct traits
416  {
418  };
419  };
422  {
423  template <class T, class DataSource>
424  struct traits
425  {
427  };
428  };
429 
437  {
438  KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10)
439  : leaf_max_size(_leaf_max_size)
440  {
441  }
442 
444  };
445 
448  {
450  SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true)
451  : checks(checks_IGNORED_)
452  , eps(eps_)
453  , sorted(sorted_)
454  {
455  }
456 
457  int checks;
458  float eps;
459  bool sorted;
460  };
473  template <typename T>
474  inline T* allocate(size_t count = 1)
475  {
476  T* mem = static_cast<T*>(::malloc(sizeof(T) * count));
477  return mem;
478  }
479 
495  const size_t WORDSIZE = 16;
496  const size_t BLOCKSIZE = 8192;
497 
499  {
500  /* We maintain memory alignment to word boundaries by requiring that all
501  allocations be in multiples of the machine wordsize. */
502  /* Size of machine word in bytes. Must be power of 2. */
503  /* Minimum number of bytes requested at a time from the system. Must be multiple of WORDSIZE. */
504 
505  size_t remaining; /* Number of bytes left in current block of storage. */
506  void* base; /* Pointer to base of current block of storage. */
507  void* loc; /* Current location in block to next allocate memory. */
508 
509  void internal_init()
510  {
511  remaining = 0;
512  base = NULL;
513  usedMemory = 0;
514  wastedMemory = 0;
515  }
516 
517  public:
518  size_t usedMemory;
519  size_t wastedMemory;
520 
524 // cppcheck-suppress uninitMemberVar
526  {
527  internal_init();
528  }
529 
534  {
535  free_all();
536  }
537 
539  void free_all()
540  {
541  while (base != NULL)
542  {
543  void* prev = *(static_cast<void**>(base)); /* Get pointer to prev block. */
544  ::free(base);
545  base = prev;
546  }
547  internal_init();
548  }
549 
554  void* malloc(const size_t req_size)
555  {
556  /* Round size up to a multiple of wordsize. The following expression
557  only works for WORDSIZE that is a power of 2, by masking last bits of
558  incremented size to zero.
559  */
560  const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1);
561 
562  /* Check whether a new block must be allocated. Note that the first word
563  of a block is reserved for a pointer to the previous block.
564  */
565  if (size > remaining)
566  {
567  wastedMemory += remaining;
568 
569  /* Allocate new storage. */
570  const size_t blocksize = (size + sizeof(void*) + (WORDSIZE - 1) > BLOCKSIZE) ? size + sizeof(void*) + (WORDSIZE - 1) : BLOCKSIZE;
571 
572  // use the standard C malloc to allocate memory
573  void* m = ::malloc(blocksize);
574  if (!m)
575  {
576  fprintf(stderr, "Failed to allocate memory.\n");
577  return NULL;
578  }
579 
580  /* Fill first word of new block with pointer to previous block. */
581  static_cast<void**>(m)[0] = base;
582  base = m;
583 
584  size_t shift = 0;
585  //int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & (WORDSIZE-1))) & (WORDSIZE-1);
586 
587  remaining = blocksize - sizeof(void*) - shift;
588  loc = (static_cast<char*>(m) + sizeof(void*) + shift);
589  }
590  void* rloc = loc;
591  loc = static_cast<char*>(loc) + size;
592  remaining -= size;
593 
594  usedMemory += size;
595 
596  return rloc;
597  }
598 
606  template <typename T>
607  T* allocate(const size_t count = 1)
608  {
609  T* mem = static_cast<T*>(this->malloc(sizeof(T) * count));
610  return mem;
611  }
612  };
618  // ---------------- CArray -------------------------
644  template <typename T, std::size_t N>
645  class CArray
646  {
647  public:
648  T elems[N]; // fixed-size array of elements of type T
649 
650  public:
651  // type definitions
652  typedef T value_type;
653  typedef T* iterator;
654  typedef const T* const_iterator;
655  typedef T& reference;
656  typedef const T& const_reference;
657  typedef std::size_t size_type;
658  typedef std::ptrdiff_t difference_type;
659 
660  // iterator support
661  inline iterator begin() { return elems; }
662  inline const_iterator begin() const { return elems; }
663  inline iterator end() { return elems + N; }
664  inline const_iterator end() const { return elems + N; }
665 
666  // reverse iterator support
667 #if !defined(BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) && !defined(BOOST_MSVC_STD_ITERATOR) && !defined(BOOST_NO_STD_ITERATOR_TRAITS)
668  typedef std::reverse_iterator<iterator> reverse_iterator;
669  typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
670 #elif defined(_MSC_VER) && (_MSC_VER == 1300) && defined(BOOST_DINKUMWARE_STDLIB) && (BOOST_DINKUMWARE_STDLIB == 310)
671  // workaround for broken reverse_iterator in VC7
672  typedef std::reverse_iterator<std::_Ptrit<value_type, difference_type, iterator,
675  typedef std::reverse_iterator<std::_Ptrit<value_type, difference_type, const_iterator,
678 #else
679  // workaround for broken reverse_iterator implementations
680  typedef std::reverse_iterator<iterator, T> reverse_iterator;
681  typedef std::reverse_iterator<const_iterator, T> const_reverse_iterator;
682 #endif
683 
685  {
686  return reverse_iterator(end());
687  }
689  reverse_iterator rend() { return reverse_iterator(begin()); }
691  // operator[]
692  inline reference operator[](size_type i) { return elems[i]; }
693  inline const_reference operator[](size_type i) const { return elems[i]; }
694  // at() with range check
696  {
697  rangecheck(i);
698  return elems[i];
699  }
701  {
702  rangecheck(i);
703  return elems[i];
704  }
705  // front() and back()
706  reference front() { return elems[0]; }
707  const_reference front() const { return elems[0]; }
708  reference back() { return elems[N - 1]; }
709  const_reference back() const { return elems[N - 1]; }
710  // size is constant
711  static inline size_type size() { return N; }
712  static bool empty() { return false; }
713  static size_type max_size() { return N; }
714  enum
715  {
716  static_size = N
717  };
719  inline void resize(const size_t nElements)
720  {
721  if (nElements != N) throw std::logic_error("Try to change the size of a CArray.");
722  }
723  // swap (note: linear complexity in N, constant for given instantiation)
724  void swap(CArray<T, N>& y) { std::swap_ranges(begin(), end(), y.begin()); }
725  // direct access to data (read-only)
726  const T* data() const { return elems; }
727  // use array as C array (direct read/write access to data)
728  T* data() { return elems; }
729  // assignment with type conversion
730  template <typename T2>
731 // cppcheck-suppress *
732  CArray<T, N>& operator=(const CArray<T2, N>& rhs)
733  {
734  std::copy(rhs.begin(), rhs.end(), begin());
735  return *this;
736  }
737  // assign one value to all elements
738  inline void assign(const T& value)
739  {
740  for (size_t i = 0; i < N; i++) elems[i] = value;
741  }
742  // assign (compatible with std::vector's one) (by JLBC for MRPT)
743  void assign(const size_t n, const T& value)
744  {
745  assert(N == n);
746  for (size_t i = 0; i < N; i++) elems[i] = value;
747  }
748 
749  private:
750  // check range (may be private because it is static)
751  static void rangecheck(size_type i)
752  {
753  if (i >= size())
754  {
755  throw std::out_of_range("CArray<>: index out of range");
756  }
757  }
758  }; // end of CArray
759 
763  template <int DIM, typename T>
765  {
767  };
769  template <typename T>
771  {
772  typedef std::vector<T> container_t;
773  };
815  template <typename Distance, class DatasetAdaptor, int DIM = -1, typename IndexType = size_t>
817  {
818  private:
821 
822  public:
823  typedef typename Distance::ElementType ElementType;
824  typedef typename Distance::DistanceType DistanceType;
825 
826  protected:
830  std::vector<IndexType> vind;
831 
833 
837  const DatasetAdaptor& dataset;
838 
840 
841  size_t m_size;
843  int dim;
844 
845  /*--------------------- Internal Data Structures --------------------------*/
846  struct Node
847  {
849  union {
850  struct leaf
851  {
852  IndexType left, right;
853  } lr;
854  struct nonleaf
855  {
856  int divfeat;
857  DistanceType divlow, divhigh;
858  } sub;
859  } node_type;
860  Node *child1, *child2;
861  };
862  typedef Node* NodePtr;
863 
864  struct Interval
865  {
867  };
868 
871 
874 
878 
887 
888  public:
889  Distance distance;
890 
904  KDTreeSingleIndexAdaptor(const int dimensionality, const DatasetAdaptor& inputData, const KDTreeSingleIndexAdaptorParams& params = KDTreeSingleIndexAdaptorParams())
905  : dataset(inputData)
906  , index_params(params)
907  , root_node(NULL)
908  , distance(inputData)
909  {
910  m_size = dataset.kdtree_get_point_count();
911  m_size_at_index_build = m_size;
912  dim = dimensionality;
913  if (DIM > 0) dim = DIM;
914  m_leaf_max_size = params.leaf_max_size;
915 
916  // Create a permutable array of indices to the input vectors.
917  init_vind();
918  }
919 
922 
924  void freeIndex()
925  {
926  pool.free_all();
927  root_node = NULL;
928  m_size_at_index_build = 0;
929  }
930 
934  void buildIndex()
935  {
936  init_vind();
937  freeIndex();
938  m_size_at_index_build = m_size;
939  if (m_size == 0) return;
940  computeBoundingBox(root_bbox);
941  root_node = divideTree(0, m_size, root_bbox); // construct the tree
942  }
943 
945  size_t size() const { return m_size; }
946 
948  size_t veclen() const
949  {
950  return static_cast<size_t>(DIM > 0 ? DIM : dim);
951  }
952 
957  size_t usedMemory() const
958  {
959  return pool.usedMemory + pool.wastedMemory + dataset.kdtree_get_point_count() * sizeof(IndexType); // pool memory and vind array memory
960  }
961 
977  template <typename RESULTSET>
978  bool findNeighbors(RESULTSET& result, const ElementType* vec, const SearchParams& searchParams) const
979  {
980  assert(vec);
981  if (size() == 0)
982  return false;
983  if (!root_node)
984  throw std::runtime_error("[nanoflann] findNeighbors() called before building the index.");
985  float epsError = 1 + searchParams.eps;
986 
987  distance_vector_t dists; // fixed or variable-sized container (depending on DIM)
988  dists.assign((DIM > 0 ? DIM : dim), 0); // Fill it with zeros.
989  DistanceType distsq = computeInitialDistances(vec, dists);
990  searchLevel(result, vec, root_node, distsq, dists, epsError); // "count_leaf" parameter removed since was neither used nor returned to the user.
991  return result.full();
992  }
993 
1002  size_t knnSearch(const ElementType* query_point, const size_t num_closest, IndexType* out_indices, DistanceType* out_distances_sq, const int /* nChecks_IGNORED */ = 10) const
1003  {
1004  nanoflann::KNNResultSet<DistanceType, IndexType> resultSet(num_closest);
1005  resultSet.init(out_indices, out_distances_sq);
1006  this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
1007  return resultSet.size();
1008  }
1009 
1022  size_t radiusSearch(const ElementType* query_point, const DistanceType& radius, std::vector<std::pair<IndexType, DistanceType> >& IndicesDists, const SearchParams& searchParams) const
1023  {
1024  RadiusResultSet<DistanceType, IndexType> resultSet(radius, IndicesDists);
1025  const size_t nFound = radiusSearchCustomCallback(query_point, resultSet, searchParams);
1026  if (searchParams.sorted)
1027  std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter());
1028  return nFound;
1029  }
1030 
1036  template <class SEARCH_CALLBACK>
1037  size_t radiusSearchCustomCallback(const ElementType* query_point, SEARCH_CALLBACK& resultSet, const SearchParams& searchParams = SearchParams()) const
1038  {
1039  this->findNeighbors(resultSet, query_point, searchParams);
1040  return resultSet.size();
1041  }
1042 
1045  private:
1047  void init_vind()
1048  {
1049  // Create a permutable array of indices to the input vectors.
1050  m_size = dataset.kdtree_get_point_count();
1051  if (vind.size() != m_size) vind.resize(m_size);
1052  for (size_t i = 0; i < m_size; i++) vind[i] = i;
1053  }
1054 
1056  inline ElementType dataset_get(size_t idx, int component) const
1057  {
1058  return dataset.kdtree_get_pt(idx, component);
1059  }
1060 
1061  void save_tree(FILE* stream, NodePtr tree)
1062  {
1063  save_value(stream, *tree);
1064  if (tree->child1 != NULL)
1065  {
1066  save_tree(stream, tree->child1);
1067  }
1068  if (tree->child2 != NULL)
1069  {
1070  save_tree(stream, tree->child2);
1071  }
1072  }
1073 
1074  void load_tree(FILE* stream, NodePtr& tree)
1075  {
1076  tree = pool.allocate<Node>();
1077  load_value(stream, *tree);
1078  if (tree->child1 != NULL)
1079  {
1080  load_tree(stream, tree->child1);
1081  }
1082  if (tree->child2 != NULL)
1083  {
1084  load_tree(stream, tree->child2);
1085  }
1086  }
1087 
1088  void computeBoundingBox(BoundingBox& bbox)
1089  {
1090  bbox.resize((DIM > 0 ? DIM : dim));
1091  if (dataset.kdtree_get_bbox(bbox))
1092  {
1093  // Done! It was implemented in derived class
1094  }
1095  else
1096  {
1097  const size_t N = dataset.kdtree_get_point_count();
1098  if (!N) throw std::runtime_error("[nanoflann] computeBoundingBox() called but no data points found.");
1099  for (int i = 0; i < (DIM > 0 ? DIM : dim); ++i)
1100  {
1101  bbox[i].low =
1102  bbox[i].high = dataset_get(0, i);
1103  }
1104  for (size_t k = 1; k < N; ++k)
1105  {
1106  for (int i = 0; i < (DIM > 0 ? DIM : dim); ++i)
1107  {
1108  if (dataset_get(k, i) < bbox[i].low) bbox[i].low = dataset_get(k, i);
1109  if (dataset_get(k, i) > bbox[i].high) bbox[i].high = dataset_get(k, i);
1110  }
1111  }
1112  }
1113  }
1114 
1122  NodePtr divideTree(const IndexType left, const IndexType right, BoundingBox& bbox)
1123  {
1124  NodePtr node = pool.allocate<Node>(); // allocate memory
1125 
1126  /* If too few exemplars remain, then make this a leaf node. */
1127  if ((right - left) <= static_cast<IndexType>(m_leaf_max_size))
1128  {
1129  node->child1 = node->child2 = NULL; /* Mark as leaf node. */
1130  node->node_type.lr.left = left;
1131  node->node_type.lr.right = right;
1132 
1133  // compute bounding-box of leaf points
1134  for (int i = 0; i < (DIM > 0 ? DIM : dim); ++i)
1135  {
1136  bbox[i].low = dataset_get(vind[left], i);
1137  bbox[i].high = dataset_get(vind[left], i);
1138  }
1139  for (IndexType k = left + 1; k < right; ++k)
1140  {
1141  for (int i = 0; i < (DIM > 0 ? DIM : dim); ++i)
1142  {
1143  if (bbox[i].low > dataset_get(vind[k], i)) bbox[i].low = dataset_get(vind[k], i);
1144  if (bbox[i].high < dataset_get(vind[k], i)) bbox[i].high = dataset_get(vind[k], i);
1145  }
1146  }
1147  }
1148  else
1149  {
1150  IndexType idx;
1151  int cutfeat;
1152  DistanceType cutval;
1153  middleSplit_(&vind[0] + left, right - left, idx, cutfeat, cutval, bbox);
1154 
1155  node->node_type.sub.divfeat = cutfeat;
1156 
1157  BoundingBox left_bbox(bbox);
1158  left_bbox[cutfeat].high = cutval;
1159  node->child1 = divideTree(left, left + idx, left_bbox);
1160 
1161  BoundingBox right_bbox(bbox);
1162  right_bbox[cutfeat].low = cutval;
1163  node->child2 = divideTree(left + idx, right, right_bbox);
1164 
1165  node->node_type.sub.divlow = left_bbox[cutfeat].high;
1166  node->node_type.sub.divhigh = right_bbox[cutfeat].low;
1167 
1168  for (int i = 0; i < (DIM > 0 ? DIM : dim); ++i)
1169  {
1170  bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
1171  bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
1172  }
1173  }
1174 
1175  return node;
1176  }
1177 
1178  void computeMinMax(IndexType* ind, IndexType count, int element, ElementType& min_elem, ElementType& max_elem)
1179  {
1180  min_elem = dataset_get(ind[0], element);
1181  max_elem = dataset_get(ind[0], element);
1182  for (IndexType i = 1; i < count; ++i)
1183  {
1184  ElementType val = dataset_get(ind[i], element);
1185  if (val < min_elem) min_elem = val;
1186  if (val > max_elem) max_elem = val;
1187  }
1188  }
1189 
1190  void middleSplit_(IndexType* ind, IndexType count, IndexType& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
1191  {
1192  const DistanceType EPS = static_cast<DistanceType>(0.00001);
1193  ElementType max_span = bbox[0].high - bbox[0].low;
1194  for (int i = 1; i < (DIM > 0 ? DIM : dim); ++i)
1195  {
1196  ElementType span = bbox[i].high - bbox[i].low;
1197  if (span > max_span)
1198  {
1199  max_span = span;
1200  }
1201  }
1202  ElementType max_spread = -1;
1203  cutfeat = 0;
1204  for (int i = 0; i < (DIM > 0 ? DIM : dim); ++i)
1205  {
1206  ElementType span = bbox[i].high - bbox[i].low;
1207  if (span > (1 - EPS) * max_span)
1208  {
1209  ElementType min_elem, max_elem;
1210  computeMinMax(ind, count, i, min_elem, max_elem);
1211  ElementType spread = max_elem - min_elem;
1212  ;
1213  if (spread > max_spread)
1214  {
1215  cutfeat = i;
1216  max_spread = spread;
1217  }
1218  }
1219  }
1220  // split in the middle
1221  DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2;
1222  ElementType min_elem, max_elem;
1223  computeMinMax(ind, count, cutfeat, min_elem, max_elem);
1224 
1225  if (split_val < min_elem)
1226  cutval = min_elem;
1227  else if (split_val > max_elem)
1228  cutval = max_elem;
1229  else
1230  cutval = split_val;
1231 
1232  IndexType lim1, lim2;
1233  planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
1234 
1235  if (lim1 > count / 2)
1236  index = lim1;
1237  else if (lim2 < count / 2)
1238  index = lim2;
1239  else
1240  index = count / 2;
1241  }
1242 
1252  void planeSplit(IndexType* ind, const IndexType count, int cutfeat, DistanceType& cutval, IndexType& lim1, IndexType& lim2)
1253  {
1254  /* Move vector indices for left subtree to front of list. */
1255  IndexType left = 0;
1256  IndexType right = count - 1;
1257  for (;;)
1258  {
1259  while (left <= right && dataset_get(ind[left], cutfeat) < cutval) ++left;
1260  while (right && left <= right && dataset_get(ind[right], cutfeat) >= cutval) --right;
1261  if (left > right || !right) break; // "!right" was added to support unsigned Index types
1262  std::swap(ind[left], ind[right]);
1263  ++left;
1264  --right;
1265  }
1266  /* If either list is empty, it means that all remaining features
1267  * are identical. Split in the middle to maintain a balanced tree.
1268  */
1269  lim1 = left;
1270  right = count - 1;
1271  for (;;)
1272  {
1273  while (left <= right && dataset_get(ind[left], cutfeat) <= cutval) ++left;
1274  while (right && left <= right && dataset_get(ind[right], cutfeat) > cutval) --right;
1275  if (left > right || !right) break; // "!right" was added to support unsigned Index types
1276  std::swap(ind[left], ind[right]);
1277  ++left;
1278  --right;
1279  }
1280  lim2 = left;
1281  }
1282 
1283  DistanceType computeInitialDistances(const ElementType* vec, distance_vector_t& dists) const
1284  {
1285  assert(vec);
1286  DistanceType distsq = DistanceType();
1287 
1288  for (int i = 0; i < (DIM > 0 ? DIM : dim); ++i)
1289  {
1290  if (vec[i] < root_bbox[i].low)
1291  {
1292  dists[i] = distance.accum_dist(vec[i], root_bbox[i].low, i);
1293  distsq += dists[i];
1294  }
1295  if (vec[i] > root_bbox[i].high)
1296  {
1297  dists[i] = distance.accum_dist(vec[i], root_bbox[i].high, i);
1298  distsq += dists[i];
1299  }
1300  }
1301 
1302  return distsq;
1303  }
1304 
1309  template <class RESULTSET>
1310  void searchLevel(RESULTSET& result_set, const ElementType* vec, const NodePtr node, DistanceType mindistsq,
1311  distance_vector_t& dists, const float epsError) const
1312  {
1313  /* If this is a leaf node, then do check and return. */
1314  if ((node->child1 == NULL) && (node->child2 == NULL))
1315  {
1316  //count_leaf += (node->lr.right-node->lr.left); // Removed since was neither used nor returned to the user.
1317  DistanceType worst_dist = result_set.worstDist();
1318  for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; ++i)
1319  {
1320  const IndexType index = vind[i]; // reorder... : i;
1321  DistanceType dist = distance(vec, index, (DIM > 0 ? DIM : dim));
1322  if (dist < worst_dist)
1323  {
1324  result_set.addPoint(dist, vind[i]);
1325  }
1326  }
1327  return;
1328  }
1329 
1330  /* Which child branch should be taken first? */
1331  int idx = node->node_type.sub.divfeat;
1332  ElementType val = vec[idx];
1333  DistanceType diff1 = val - node->node_type.sub.divlow;
1334  DistanceType diff2 = val - node->node_type.sub.divhigh;
1335 
1336  NodePtr bestChild;
1337  NodePtr otherChild;
1338  DistanceType cut_dist;
1339  if ((diff1 + diff2) < 0)
1340  {
1341  bestChild = node->child1;
1342  otherChild = node->child2;
1343  cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx);
1344  }
1345  else
1346  {
1347  bestChild = node->child2;
1348  otherChild = node->child1;
1349  cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx);
1350  }
1351 
1352  /* Call recursively to search next level down. */
1353  searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError);
1354 
1355  DistanceType dst = dists[idx];
1356  mindistsq = mindistsq + cut_dist - dst;
1357  dists[idx] = cut_dist;
1358  if (mindistsq * epsError <= result_set.worstDist())
1359  {
1360  searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError);
1361  }
1362  dists[idx] = dst;
1363  }
1364 
1365  public:
1370  void saveIndex(FILE* stream)
1371  {
1372  save_value(stream, m_size);
1373  save_value(stream, dim);
1374  save_value(stream, root_bbox);
1375  save_value(stream, m_leaf_max_size);
1376  save_value(stream, vind);
1377  save_tree(stream, root_node);
1378  }
1379 
1384  void loadIndex(FILE* stream)
1385  {
1386  load_value(stream, m_size);
1387  load_value(stream, dim);
1388  load_value(stream, root_bbox);
1389  load_value(stream, m_leaf_max_size);
1390  load_value(stream, vind);
1391  load_tree(stream, root_node);
1392  }
1393 
1394  }; // class KDTree
1395 
1414  template <class MatrixType, int DIM = -1, class Distance = nanoflann::metric_L2>
1416  {
1418  typedef typename MatrixType::Scalar num_t;
1419  typedef typename MatrixType::Index IndexType;
1420  typedef typename Distance::template traits<num_t, self_t>::distance_t metric_t;
1422 
1424 
1426  KDTreeEigenMatrixAdaptor(const int dimensionality, const MatrixType& mat, const int leaf_max_size = 10)
1427  : m_data_matrix(mat)
1428  {
1429  const IndexType dims = mat.cols();
1430  if (dims != dimensionality) throw std::runtime_error("Error: 'dimensionality' must match column count in data matrix");
1431  if (DIM > 0 && static_cast<int>(dims) != DIM)
1432  throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument");
1433  index = new index_t(dims, *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size));
1434  index->buildIndex();
1435  }
1436 
1437  private:
1439  KDTreeEigenMatrixAdaptor(const self_t&);
1440 
1441  public:
1443  {
1444  delete index;
1445  }
1446 
1447  const MatrixType& m_data_matrix;
1448 
1454  inline void query(const num_t* query_point, const size_t num_closest, IndexType* out_indices, num_t* out_distances_sq, const int /* nChecks_IGNORED */ = 10) const
1455  {
1456  nanoflann::KNNResultSet<num_t, IndexType> resultSet(num_closest);
1457  resultSet.init(out_indices, out_distances_sq);
1458  index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
1459  }
1460 
1464  const self_t& derived() const
1465  {
1466  return *this;
1467  }
1468  self_t& derived()
1469  {
1470  return *this;
1471  }
1472 
1473  // Must return the number of data points
1474  inline size_t kdtree_get_point_count() const
1475  {
1476  return m_data_matrix.rows();
1477  }
1478 
1479  // Returns the L2 distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
1480  inline num_t kdtree_distance(const num_t* p1, const IndexType idx_p2, IndexType size) const
1481  {
1482  num_t s = 0;
1483  for (IndexType i = 0; i < size; i++)
1484  {
1485  const num_t d = p1[i] - m_data_matrix.coeff(idx_p2, i);
1486  s += d * d;
1487  }
1488  return s;
1489  }
1490 
1491  // Returns the dim'th component of the idx'th point in the class:
1492  inline num_t kdtree_get_pt(const IndexType idx, int dim) const
1493  {
1494  return m_data_matrix.coeff(idx, IndexType(dim));
1495  }
1496 
1497  // Optional bounding-box computation: return false to default to a standard bbox computation loop.
1498  // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
1499  // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
1500  template <class BBOX>
1501  bool kdtree_get_bbox(BBOX& /*bb*/) const
1502  {
1503  return false;
1504  }
1505 
1508  }; // end of KDTreeEigenMatrixAdaptor // end of grouping
1512 } // namespace nanoflann
1513 
1514 #endif /* NANOFLANN_HPP_ */