MRPT  1.9.9
nanoflann.hpp
Go to the documentation of this file.
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 
33 /** \mainpage nanoflann C++ API documentation
34  * nanoflann is a C++ header-only library for building KD-Trees, mostly
35  * optimized for 2D or 3D point clouds.
36  *
37  * nanoflann does not require compiling or installing, just an
38  * #include <nanoflann.hpp> in your code.
39  *
40  * See:
41  * - <a href="modules.html" >C++ API organized by modules</a>
42  * - <a href="https://github.com/jlblancoc/nanoflann" >Online README</a>
43  * - <a href="http://jlblancoc.github.io/nanoflann/" >Doxygen documentation</a>
44  */
45 
46 #ifndef NANOFLANN_HPP_
47 #define NANOFLANN_HPP_
48 
49 #include <vector>
50 #include <cassert>
51 #include <algorithm>
52 #include <stdexcept>
53 #include <cstdio> // for fwrite()
54 #include <cmath> // for abs()
55 #include <cstdlib> // for abs()
56 #include <limits>
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 {
69 /** @addtogroup nanoflann_grp nanoflann C++ library for ANN
70  * @{ */
71 
72  /** Library version: 0xMmP (M=Major,m=minor,P=patch) */
73  #define NANOFLANN_VERSION 0x122
74 
75  /** @addtogroup result_sets_grp Result set classes
76  * @{ */
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_) : indices(nullptr), dists(nullptr), capacity(capacity_), count(0)
87  {
88  }
89 
90  inline void init(IndexType* indices_, DistanceType* dists_)
91  {
92  indices = indices_;
93  dists = dists_;
94  count = 0;
95  if (capacity)
96  dists[capacity-1] = (std::numeric_limits<DistanceType>::max)();
97  }
98 
99  inline CountType size() const
100  {
101  return count;
102  }
103 
104  inline bool full() const
105  {
106  return count == capacity;
107  }
108 
109 
110  inline void addPoint(DistanceType dist, IndexType index)
111  {
112  CountType i;
113  for (i=count; i>0; --i) {
114 #ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same distance, the one with the lowest-index will be returned first.
115  if ( (dists[i-1]>dist) || ((dist==dists[i-1])&&(indices[i-1]>index)) ) {
116 #else
117  if (dists[i-1]>dist) {
118 #endif
119  if (i<capacity) {
120  dists[i] = dists[i-1];
121  indices[i] = indices[i-1];
122  }
123  }
124  else break;
125  }
126  if (i<capacity) {
127  dists[i] = dist;
128  indices[i] = index;
129  }
130  if (count<capacity) count++;
131  }
132 
133  inline DistanceType worstDist() const
134  {
135  return dists[capacity-1];
136  }
137  };
138 
139 
140  /**
141  * A result-set class used when performing a radius based search.
142  */
143  template <typename DistanceType, typename IndexType = size_t>
145  {
146  public:
147  const DistanceType radius;
148 
149  std::vector<std::pair<IndexType,DistanceType> >& m_indices_dists;
150 
151  inline RadiusResultSet(DistanceType radius_, std::vector<std::pair<IndexType,DistanceType> >& indices_dists) : radius(radius_), m_indices_dists(indices_dists)
152  {
153  init();
154  }
155 
156  inline ~RadiusResultSet() = default;
157 
158  inline void init() { clear(); }
159  inline void clear() { m_indices_dists.clear(); }
160 
161  inline size_t size() const { return m_indices_dists.size(); }
162 
163  inline bool full() const { return true; }
164 
165  inline void addPoint(DistanceType dist, IndexType index)
166  {
167  if (dist<radius)
168  m_indices_dists.push_back(std::make_pair(index,dist));
169  }
170 
171  inline DistanceType worstDist() const { return radius; }
172 
173  /** Clears the result set and adjusts the search radius. */
174  inline void set_radius_and_clear( const DistanceType r )
175  {
176  radius = r;
177  clear();
178  }
179 
180  /**
181  * Find the worst result (furtherest neighbor) without copying or sorting
182  * Pre-conditions: size() > 0
183  */
184  std::pair<IndexType,DistanceType> worst_item() const
185  {
186  if (m_indices_dists.empty()) throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on an empty list of results.");
187  typedef typename std::vector<std::pair<IndexType,DistanceType> >::const_iterator DistIt;
188  DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end());
189  return *it;
190  }
191  };
192 
193  /** operator "<" for std::sort() */
195  {
196  /** PairType will be typically: std::pair<IndexType,DistanceType> */
197  template <typename PairType>
198  inline bool operator()(const PairType &p1, const PairType &p2) const {
199  return p1.second < p2.second;
200  }
201  };
202 
203  /** @} */
204 
205 
206  /** @addtogroup loadsave_grp Load/save auxiliary functions
207  * @{ */
208  template<typename T>
209  void save_value(FILE* stream, const T& value, size_t count = 1)
210  {
211  fwrite(&value, sizeof(value),count, stream);
212  }
213 
214  template<typename T>
215  void save_value(FILE* stream, const std::vector<T>& value)
216  {
217  size_t size = value.size();
218  fwrite(&size, sizeof(size_t), 1, stream);
219  fwrite(&value[0], sizeof(T), size, stream);
220  }
221 
222  template<typename T>
223  void load_value(FILE* stream, T& value, size_t count = 1)
224  {
225  size_t read_cnt = fread(&value, sizeof(value), count, stream);
226  if (read_cnt != count) {
227  throw std::runtime_error("Cannot read from file");
228  }
229  }
230 
231 
232  template<typename T>
233  void load_value(FILE* stream, std::vector<T>& value)
234  {
235  size_t size;
236  size_t read_cnt = fread(&size, sizeof(size_t), 1, stream);
237  if (read_cnt!=1) {
238  throw std::runtime_error("Cannot read from file");
239  }
240  value.resize(size);
241  read_cnt = fread(&value[0], sizeof(T), size, stream);
242  if (read_cnt!=size) {
243  throw std::runtime_error("Cannot read from file");
244  }
245  }
246  /** @} */
247 
248 
249  /** @addtogroup metric_grp Metric (distance) classes
250  * @{ */
251 
252  /** Manhattan distance functor (generic version, optimized for high-dimensionality data sets).
253  * Corresponding distance traits: nanoflann::metric_L1
254  * \tparam T Type of the elements (e.g. double, float, uint8_t)
255  * \tparam _DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t)
256  */
257  template<class T, class DataSource, typename _DistanceType = T>
258  struct L1_Adaptor
259  {
260  typedef T ElementType;
261  typedef _DistanceType DistanceType;
262 
263  const DataSource &data_source;
264 
265  L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) { }
266 
267  inline DistanceType operator()(const T* a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const
268  {
269  DistanceType result = DistanceType();
270  const T* last = a + size;
271  const T* lastgroup = last - 3;
272  size_t d = 0;
273 
274  /* Process 4 items with each loop for efficiency. */
275  while (a < lastgroup) {
276  const DistanceType diff0 = std::abs(a[0] - data_source.kdtree_get_pt(b_idx,d++));
277  const DistanceType diff1 = std::abs(a[1] - data_source.kdtree_get_pt(b_idx,d++));
278  const DistanceType diff2 = std::abs(a[2] - data_source.kdtree_get_pt(b_idx,d++));
279  const DistanceType diff3 = std::abs(a[3] - data_source.kdtree_get_pt(b_idx,d++));
280  result += diff0 + diff1 + diff2 + diff3;
281  a += 4;
282  if ((worst_dist>0)&&(result>worst_dist)) {
283  return result;
284  }
285  }
286  /* Process last 0-3 components. Not needed for standard vector lengths. */
287  while (a < last) {
288  result += std::abs( *a++ - data_source.kdtree_get_pt(b_idx,d++) );
289  }
290  return result;
291  }
292 
293  template <typename U, typename V>
294  inline DistanceType accum_dist(const U a, const V b, int ) const
295  {
296  return std::abs(a-b);
297  }
298  };
299 
300  /** Squared Euclidean distance functor (generic version, optimized for high-dimensionality data sets).
301  * Corresponding distance traits: nanoflann::metric_L2
302  * \tparam T Type of the elements (e.g. double, float, uint8_t)
303  * \tparam _DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t)
304  */
305  template<class T, class DataSource, typename _DistanceType = T>
306  struct L2_Adaptor
307  {
308  typedef T ElementType;
309  typedef _DistanceType DistanceType;
310 
311  const DataSource &data_source;
312 
313  L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) { }
314 
315  inline DistanceType operator()(const T* a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const
316  {
317  DistanceType result = DistanceType();
318  const T* last = a + size;
319  const T* lastgroup = last - 3;
320  size_t d = 0;
321 
322  /* Process 4 items with each loop for efficiency. */
323  while (a < lastgroup) {
324  const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx,d++);
325  const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx,d++);
326  const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx,d++);
327  const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx,d++);
328  result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
329  a += 4;
330  if ((worst_dist>0)&&(result>worst_dist)) {
331  return result;
332  }
333  }
334  /* Process last 0-3 components. Not needed for standard vector lengths. */
335  while (a < last) {
336  const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx,d++);
337  result += diff0 * diff0;
338  }
339  return result;
340  }
341 
342  template <typename U, typename V>
343  inline DistanceType accum_dist(const U a, const V b, int ) const
344  {
345  return (a-b)*(a-b);
346  }
347  };
348 
349  /** Squared Euclidean (L2) distance functor (suitable for low-dimensionality datasets, like 2D or 3D point clouds)
350  * Corresponding distance traits: nanoflann::metric_L2_Simple
351  * \tparam T Type of the elements (e.g. double, float, uint8_t)
352  * \tparam _DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t)
353  */
354  template<class T, class DataSource, typename _DistanceType = T>
356  {
357  typedef T ElementType;
358  typedef _DistanceType DistanceType;
359 
360  const DataSource &data_source;
361 
362  L2_Simple_Adaptor(const DataSource &_data_source) : data_source(_data_source) { }
363 
364  inline DistanceType operator()(const T* a, const size_t b_idx, size_t size) const {
365  return data_source.kdtree_distance(a,b_idx,size);
366  }
367 
368  template <typename U, typename V>
369  inline DistanceType accum_dist(const U a, const V b, int ) const
370  {
371  return (a-b)*(a-b);
372  }
373  };
374 
375  /** Metaprogramming helper traits class for the L1 (Manhattan) metric */
376  struct metric_L1 {
377  template<class T, class DataSource>
378  struct traits {
380  };
381  };
382  /** Metaprogramming helper traits class for the L2 (Euclidean) metric */
383  struct metric_L2 {
384  template<class T, class DataSource>
385  struct traits {
387  };
388  };
389  /** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */
391  template<class T, class DataSource>
392  struct traits {
394  };
395  };
396 
397  /** @} */
398 
399  /** @addtogroup param_grp Parameter structs
400  * @{ */
401 
402  /** Parameters (see README.md) */
404  {
405  KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10) :
406  leaf_max_size(_leaf_max_size)
407  {}
408 
410  };
411 
412  /** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */
414  {
415  /** Note: The first argument (checks_IGNORED_) is ignored, but kept for compatibility with the FLANN interface */
416  SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true ) :
417  checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {}
418 
419  int checks; //!< Ignored parameter (Kept for compatibility with the FLANN interface).
420  float eps; //!< search for eps-approximate neighbours (default: 0)
421  bool sorted; //!< only for radius search, require neighbours sorted by distance (default: true)
422  };
423  /** @} */
424 
425 
426  /** @addtogroup memalloc_grp Memory allocation
427  * @{ */
428 
429  /**
430  * Allocates (using C's malloc) a generic type T.
431  *
432  * Params:
433  * count = number of instances to allocate.
434  * Returns: pointer (of type T*) to memory buffer
435  */
436  template <typename T>
437  inline T* allocate(size_t count = 1)
438  {
439  T* mem = static_cast<T*>( ::malloc(sizeof(T)*count));
440  return mem;
441  }
442 
443 
444  /**
445  * Pooled storage allocator
446  *
447  * The following routines allow for the efficient allocation of storage in
448  * small chunks from a specified pool. Rather than allowing each structure
449  * to be freed individually, an entire pool of storage is freed at once.
450  * This method has two advantages over just using malloc() and free(). First,
451  * it is far more efficient for allocating small objects, as there is
452  * no overhead for remembering all the information needed to free each
453  * object or consolidating fragmented memory. Second, the decision about
454  * how long to keep an object is made at the time of allocation, and there
455  * is no need to track down all the objects to free them.
456  *
457  */
458 
459  const size_t WORDSIZE=16;
460  const size_t BLOCKSIZE=8192;
461 
463  {
464  /* We maintain memory alignment to word boundaries by requiring that all
465  allocations be in multiples of the machine wordsize. */
466  /* Size of machine word in bytes. Must be power of 2. */
467  /* Minimum number of bytes requested at a time from the system. Must be multiple of WORDSIZE. */
468 
469 
470  size_t remaining; /* Number of bytes left in current block of storage. */
471  void* base; /* Pointer to base of current block of storage. */
472  void* loc; /* Current location in block to next allocate memory. */
473 
475  {
476  remaining = 0;
477  base = nullptr;
478  usedMemory = 0;
479  wastedMemory = 0;
480  }
481 
482  public:
483  size_t usedMemory;
484  size_t wastedMemory;
485 
486  /**
487  Default constructor. Initializes a new pool.
488  */
490  internal_init();
491  }
492 
493  /**
494  * Destructor. Frees all the memory allocated in this pool.
495  */
497  free_all();
498  }
499 
500  /** Frees all allocated memory chunks */
501  void free_all()
502  {
503  while (base != nullptr) {
504  void *prev = *(static_cast<void**>( base)); /* Get pointer to prev block. */
505  ::free(base);
506  base = prev;
507  }
508  internal_init();
509  }
510 
511  /**
512  * Returns a pointer to a piece of new memory of the given size in bytes
513  * allocated from the pool.
514  */
515  void* malloc(const size_t req_size)
516  {
517  /* Round size up to a multiple of wordsize. The following expression
518  only works for WORDSIZE that is a power of 2, by masking last bits of
519  incremented size to zero.
520  */
521  const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1);
522 
523  /* Check whether a new block must be allocated. Note that the first word
524  of a block is reserved for a pointer to the previous block.
525  */
526  if (size > remaining) {
527 
528  wastedMemory += remaining;
529 
530  /* Allocate new storage. */
531  const size_t blocksize = (size + sizeof(void*) + (WORDSIZE-1) > BLOCKSIZE) ?
532  size + sizeof(void*) + (WORDSIZE-1) : BLOCKSIZE;
533 
534  // use the standard C malloc to allocate memory
535  void* m = ::malloc(blocksize);
536  if (!m) {
537  fprintf(stderr,"Failed to allocate memory.\n");
538  return nullptr;
539  }
540 
541  /* Fill first word of new block with pointer to previous block. */
542  static_cast<void**>(m)[0] = base;
543  base = m;
544 
545  size_t shift = 0;
546  //int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & (WORDSIZE-1))) & (WORDSIZE-1);
547 
548  remaining = blocksize - sizeof(void*) - shift;
549  loc = (static_cast<char*>(m) + sizeof(void*) + shift);
550  }
551  void* rloc = loc;
552  loc = static_cast<char*>(loc) + size;
553  remaining -= size;
554 
555  usedMemory += size;
556 
557  return rloc;
558  }
559 
560  /**
561  * Allocates (using this pool) a generic type T.
562  *
563  * Params:
564  * count = number of instances to allocate.
565  * Returns: pointer (of type T*) to memory buffer
566  */
567  template <typename T>
568  T* allocate(const size_t count = 1)
569  {
570  T* mem = static_cast<T*>(this->malloc(sizeof(T)*count));
571  return mem;
572  }
573 
574  };
575  /** @} */
576 
577  /** @addtogroup nanoflann_metaprog_grp Auxiliary metaprogramming stuff
578  * @{ */
579 
580  // ---------------- CArray -------------------------
581  /** A STL container (as wrapper) for arrays of constant size defined at compile time (class imported from the MRPT project)
582  * This code is an adapted version from Boost, modifed for its integration
583  * within MRPT (JLBC, Dec/2009) (Renamed array -> CArray to avoid possible potential conflicts).
584  * See
585  * http://www.josuttis.com/cppcode
586  * for details and the latest version.
587  * See
588  * http://www.boost.org/libs/array for Documentation.
589  * for documentation.
590  *
591  * (C) Copyright Nicolai M. Josuttis 2001.
592  * Permission to copy, use, modify, sell and distribute this software
593  * is granted provided this copyright notice appears in all copies.
594  * This software is provided "as is" without express or implied
595  * warranty, and with no claim as to its suitability for any purpose.
596  *
597  * 29 Jan 2004 - minor fixes (Nico Josuttis)
598  * 04 Dec 2003 - update to synch with library TR1 (Alisdair Meredith)
599  * 23 Aug 2002 - fix for Non-MSVC compilers combined with MSVC libraries.
600  * 05 Aug 2001 - minor update (Nico Josuttis)
601  * 20 Jan 2001 - STLport fix (Beman Dawes)
602  * 29 Sep 2000 - Initial Revision (Nico Josuttis)
603  *
604  * Jan 30, 2004
605  */
606  template <typename T, std::size_t N>
607  class CArray {
608  public:
609  T elems[N]; // fixed-size array of elements of type T
610 
611  public:
612  // type definitions
613  typedef T value_type;
614  typedef T* iterator;
615  typedef const T* const_iterator;
616  typedef T& reference;
617  typedef const T& const_reference;
618  typedef std::size_t size_type;
619  typedef std::ptrdiff_t difference_type;
620 
621  // iterator support
622  inline iterator begin() { return elems; }
623  inline const_iterator begin() const { return elems; }
624  inline iterator end() { return elems+N; }
625  inline const_iterator end() const { return elems+N; }
626 
627  // reverse iterator support
628 #if !defined(BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) && !defined(BOOST_MSVC_STD_ITERATOR) && !defined(BOOST_NO_STD_ITERATOR_TRAITS)
629  typedef std::reverse_iterator<iterator> reverse_iterator;
630  typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
631 #elif defined(_MSC_VER) && (_MSC_VER == 1300) && defined(BOOST_DINKUMWARE_STDLIB) && (BOOST_DINKUMWARE_STDLIB == 310)
632  // workaround for broken reverse_iterator in VC7
633  typedef std::reverse_iterator<std::_Ptrit<value_type, difference_type, iterator,
635  typedef std::reverse_iterator<std::_Ptrit<value_type, difference_type, const_iterator,
637 #else
638  // workaround for broken reverse_iterator implementations
639  typedef std::reverse_iterator<iterator,T> reverse_iterator;
640  typedef std::reverse_iterator<const_iterator,T> const_reverse_iterator;
641 #endif
642 
647  // operator[]
648  inline reference operator[](size_type i) { return elems[i]; }
649  inline const_reference operator[](size_type i) const { return elems[i]; }
650  // at() with range check
651  reference at(size_type i) { rangecheck(i); return elems[i]; }
652  const_reference at(size_type i) const { rangecheck(i); return elems[i]; }
653  // front() and back()
654  reference front() { return elems[0]; }
655  const_reference front() const { return elems[0]; }
656  reference back() { return elems[N-1]; }
657  const_reference back() const { return elems[N-1]; }
658  // size is constant
659  static inline size_type size() { return N; }
660  static bool empty() { return false; }
661  static size_type max_size() { return N; }
662  enum { static_size = N };
663  /** This method has no effects in this class, but raises an exception if the expected size does not match */
664  inline void resize(const size_t nElements) { if (nElements!=N) throw std::logic_error("Try to change the size of a CArray."); }
665  // swap (note: linear complexity in N, constant for given instantiation)
666  void swap (CArray<T,N>& y) { std::swap_ranges(begin(),end(),y.begin()); }
667  // direct access to data (read-only)
668  const T* data() const { return elems; }
669  // use array as C array (direct read/write access to data)
670  T* data() { return elems; }
671  // assignment with type conversion
672  template <typename T2> CArray<T,N>& operator= (const CArray<T2,N>& rhs) {
673  std::copy(rhs.begin(),rhs.end(), begin());
674  return *this;
675  }
676  // assign one value to all elements
677  inline void assign (const T& value) { for (size_t i=0;i<N;i++) elems[i]=value; }
678  // assign (compatible with std::vector's one) (by JLBC for MRPT)
679  void assign (const size_t n, const T& value) { assert(N==n); for (size_t i=0;i<N;i++) elems[i]=value; }
680  private:
681  // check range (may be private because it is static)
682  static void rangecheck (size_type i) { if (i >= size()) { throw std::out_of_range("CArray<>: index out of range"); } }
683  }; // end of CArray
684 
685  /** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors when DIM=-1.
686  * Fixed size version for a generic DIM:
687  */
688  template <int DIM, typename T>
690  {
692  };
693  /** Dynamic size version */
694  template <typename T>
696  typedef std::vector<T> container_t;
697  };
698  /** @} */
699 
700  /** @addtogroup kdtrees_grp KD-tree classes and adaptors
701  * @{ */
702 
703  /** kd-tree index
704  *
705  * Contains the k-d trees and other information for indexing a set of points
706  * for nearest-neighbor matching.
707  *
708  * The class "DatasetAdaptor" must provide the following interface (can be non-virtual, inlined methods):
709  *
710  * \code
711  * // Must return the number of data poins
712  * inline size_t kdtree_get_point_count() const { ... }
713  *
714  * // [Only if using the metric_L2_Simple type] Must return the Euclidean (L2) distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
715  * inline DistanceType kdtree_distance(const T *p1, const size_t idx_p2,size_t size) const { ... }
716  *
717  * // Must return the dim'th component of the idx'th point in the class:
718  * inline T kdtree_get_pt(const size_t idx, int dim) const { ... }
719  *
720  * // Optional bounding-box computation: return false to default to a standard bbox computation loop.
721  * // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
722  * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
723  * template <class BBOX>
724  * bool kdtree_get_bbox(BBOX &bb) const
725  * {
726  * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits
727  * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits
728  * ...
729  * return true;
730  * }
731  *
732  * \endcode
733  *
734  * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
735  * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
736  * \tparam DIM Dimensionality of data points (e.g. 3 for 3D points)
737  * \tparam IndexType Will be typically size_t or int
738  */
739  template <typename Distance, class DatasetAdaptor,int DIM = -1, typename IndexType = size_t>
741  {
742  private:
743  /** Hidden copy constructor, to disallow copying indices (Not implemented) */
745  public:
746  typedef typename Distance::ElementType ElementType;
747  typedef typename Distance::DistanceType DistanceType;
748  protected:
749 
750  /**
751  * Array of indices to vectors in the dataset.
752  */
753  std::vector<IndexType> vind;
754 
756 
757 
758  /**
759  * The dataset used by this index
760  */
761  const DatasetAdaptor &dataset; //!< The source of our data
762 
764 
765  size_t m_size; //!< Number of current poins in the dataset
766  size_t m_size_at_index_build; //!< Number of points in the dataset when the index was built
767  int dim; //!< Dimensionality of each data point
768 
769 
770  /*--------------------- Internal Data Structures --------------------------*/
771  struct Node
772  {
773  /** Union used because a node can be either a LEAF node or a non-leaf node, so both data fields are never used simultaneously */
774  union {
775  struct leaf
776  {
777  IndexType left, right; //!< Indices of points in leaf node
778  } lr;
779  struct nonleaf
780  {
781  int divfeat; //!< Dimension used for subdivision.
782  DistanceType divlow, divhigh; //!< The values used for subdivision.
783  } sub;
784  } node_type;
785  Node* child1, * child2; //!< Child nodes (both=NULL mean its a leaf node)
786  };
787  typedef Node* NodePtr;
788 
789 
790  struct Interval
791  {
793  };
794 
795  /** Define "BoundingBox" as a fixed-size or variable-size container depending on "DIM" */
797 
798  /** Define "distance_vector_t" as a fixed-size or variable-size container depending on "DIM" */
800 
801  /** The KD-tree used to find neighbours */
804 
805  /**
806  * Pooled memory allocator.
807  *
808  * Using a pooled memory allocator is more efficient
809  * than allocating memory directly when there is a large
810  * number small of memory allocations.
811  */
813 
814  public:
815 
816  Distance distance;
817 
818  /**
819  * KDTree constructor
820  *
821  * Refer to docs in README.md or online in https://github.com/jlblancoc/nanoflann
822  *
823  * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 for 3D points)
824  * is determined by means of:
825  * - The \a DIM template parameter if >0 (highest priority)
826  * - Otherwise, the \a dimensionality parameter of this constructor.
827  *
828  * @param inputData Dataset with the input features
829  * @param params Basically, the maximum leaf node size
830  */
831  KDTreeSingleIndexAdaptor(const int dimensionality, const DatasetAdaptor& inputData, const KDTreeSingleIndexAdaptorParams& params = KDTreeSingleIndexAdaptorParams() ) :
832  dataset(inputData), index_params(params), root_node(nullptr), distance(inputData)
833  {
834  m_size = dataset.kdtree_get_point_count();
835  m_size_at_index_build = m_size;
836  dim = dimensionality;
837  if (DIM>0) dim=DIM;
838  m_leaf_max_size = params.leaf_max_size;
839 
840  // Create a permutable array of indices to the input vectors.
841  init_vind();
842  }
843 
844  /** Standard destructor */
845  ~KDTreeSingleIndexAdaptor() = default;
846 
847  /** Frees the previously-built index. Automatically called within buildIndex(). */
848  void freeIndex()
849  {
850  pool.free_all();
851  root_node=nullptr;
852  m_size_at_index_build = 0;
853  }
854 
855  /**
856  * Builds the index
857  */
858  void buildIndex()
859  {
860  init_vind();
861  freeIndex();
862  m_size_at_index_build = m_size;
863  if(m_size == 0) return;
864  computeBoundingBox(root_bbox);
865  root_node = divideTree(0, m_size, root_bbox ); // construct the tree
866  }
867 
868  /** Returns number of points in dataset */
869  size_t size() const { return m_size; }
870 
871  /** Returns the length of each point in the dataset */
872  size_t veclen() const {
873  return static_cast<size_t>(DIM>0 ? DIM : dim);
874  }
875 
876  /**
877  * Computes the inde memory usage
878  * Returns: memory used by the index
879  */
880  size_t usedMemory() const
881  {
882  return pool.usedMemory+pool.wastedMemory+dataset.kdtree_get_point_count()*sizeof(IndexType); // pool memory and vind array memory
883  }
884 
885  /** \name Query methods
886  * @{ */
887 
888  /**
889  * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored inside
890  * the result object.
891  *
892  * Params:
893  * result = the result object in which the indices of the nearest-neighbors are stored
894  * vec = the vector for which to search the nearest neighbors
895  *
896  * \tparam RESULTSET Should be any ResultSet<DistanceType>
897  * \return True if the requested neighbors could be found.
898  * \sa knnSearch, radiusSearch
899  */
900  template <typename RESULTSET>
901  bool findNeighbors(RESULTSET& result, const ElementType* vec, const SearchParams& searchParams) const
902  {
903  assert(vec);
904  if (size() == 0)
905  return false;
906  if (!root_node)
907  throw std::runtime_error("[nanoflann] findNeighbors() called before building the index.");
908  float epsError = 1+searchParams.eps;
909 
910  distance_vector_t dists; // fixed or variable-sized container (depending on DIM)
911  dists.assign((DIM>0 ? DIM : dim) ,0); // Fill it with zeros.
912  DistanceType distsq = computeInitialDistances(vec, dists);
913  searchLevel(result, vec, root_node, distsq, dists, epsError); // "count_leaf" parameter removed since was neither used nor returned to the user.
914  return result.full();
915  }
916 
917  /**
918  * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. Their indices are stored inside
919  * the result object.
920  * \sa radiusSearch, findNeighbors
921  * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
922  * \return Number `N` of valid points in the result set. Only the first `N` entries in `out_indices` and `out_distances_sq` will be valid.
923  * Return may be less than `num_closest` only if the number of elements in the tree is less than `num_closest`.
924  */
925  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
926  {
928  resultSet.init(out_indices, out_distances_sq);
929  this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
930  return resultSet.size();
931  }
932 
933  /**
934  * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius.
935  * The output is given as a vector of pairs, of which the first element is a point index and the second the corresponding distance.
936  * Previous contents of \a IndicesDists are cleared.
937  *
938  * If searchParams.sorted==true, the output list is sorted by ascending distances.
939  *
940  * For a better performance, it is advisable to do a .reserve() on the vector if you have any wild guess about the number of expected matches.
941  *
942  * \sa knnSearch, findNeighbors, radiusSearchCustomCallback
943  * \return The number of points within the given radius (i.e. indices.size() or dists.size() )
944  */
945  size_t radiusSearch(const ElementType *query_point,const DistanceType &radius, std::vector<std::pair<IndexType,DistanceType> >& IndicesDists, const SearchParams& searchParams) const
946  {
947  RadiusResultSet<DistanceType,IndexType> resultSet(radius,IndicesDists);
948  const size_t nFound = radiusSearchCustomCallback(query_point,resultSet,searchParams);
949  if (searchParams.sorted)
950  std::sort(IndicesDists.begin(),IndicesDists.end(), IndexDist_Sorter() );
951  return nFound;
952  }
953 
954  /**
955  * Just like radiusSearch() but with a custom callback class for each point found in the radius of the query.
956  * See the source of RadiusResultSet<> as a start point for your own classes.
957  * \sa radiusSearch
958  */
959  template <class SEARCH_CALLBACK>
960  size_t radiusSearchCustomCallback(const ElementType *query_point,SEARCH_CALLBACK &resultSet, const SearchParams& searchParams = SearchParams() ) const
961  {
962  this->findNeighbors(resultSet, query_point, searchParams);
963  return resultSet.size();
964  }
965 
966  /** @} */
967 
968  private:
969  /** Make sure the auxiliary list \a vind has the same size than the current dataset, and re-generate if size has changed. */
970  void init_vind()
971  {
972  // Create a permutable array of indices to the input vectors.
973  m_size = dataset.kdtree_get_point_count();
974  if (vind.size()!=m_size) vind.resize(m_size);
975  for (size_t i = 0; i < m_size; i++) vind[i] = i;
976  }
977 
978  /// Helper accessor to the dataset points:
979  inline ElementType dataset_get(size_t idx, int component) const {
980  return dataset.kdtree_get_pt(idx,component);
981  }
982 
983 
984  void save_tree(FILE* stream, NodePtr tree)
985  {
986  save_value(stream, *tree);
987  if (tree->child1!=NULL) {
988  save_tree(stream, tree->child1);
989  }
990  if (tree->child2!=NULL) {
991  save_tree(stream, tree->child2);
992  }
993  }
994 
995 
996  void load_tree(FILE* stream, NodePtr& tree)
997  {
998  tree = pool.allocate<Node>();
999  load_value(stream, *tree);
1000  if (tree->child1!=NULL) {
1001  load_tree(stream, tree->child1);
1002  }
1003  if (tree->child2!=NULL) {
1004  load_tree(stream, tree->child2);
1005  }
1006  }
1007 
1008 
1010  {
1011  bbox.resize((DIM>0 ? DIM : dim));
1012  if (dataset.kdtree_get_bbox(bbox))
1013  {
1014  // Done! It was implemented in derived class
1015  }
1016  else
1017  {
1018  const size_t N = dataset.kdtree_get_point_count();
1019  if (!N) throw std::runtime_error("[nanoflann] computeBoundingBox() called but no data points found.");
1020  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1021  bbox[i].low =
1022  bbox[i].high = dataset_get(0,i);
1023  }
1024  for (size_t k=1; k<N; ++k) {
1025  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1026  if (dataset_get(k,i)<bbox[i].low) bbox[i].low = dataset_get(k,i);
1027  if (dataset_get(k,i)>bbox[i].high) bbox[i].high = dataset_get(k,i);
1028  }
1029  }
1030  }
1031  }
1032 
1033 
1034  /**
1035  * Create a tree node that subdivides the list of vecs from vind[first]
1036  * to vind[last]. The routine is called recursively on each sublist.
1037  *
1038  * @param left index of the first vector
1039  * @param right index of the last vector
1040  */
1041  NodePtr divideTree(const IndexType left, const IndexType right, BoundingBox& bbox)
1042  {
1043  auto node = pool.allocate<Node>(); // allocate memory
1044 
1045  /* If too few exemplars remain, then make this a leaf node. */
1046  if ( (right-left) <= static_cast<IndexType>(m_leaf_max_size) ) {
1047  node->child1 = node->child2 = nullptr; /* Mark as leaf node. */
1048  node->node_type.lr.left = left;
1049  node->node_type.lr.right = right;
1050 
1051  // compute bounding-box of leaf points
1052  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1053  bbox[i].low = dataset_get(vind[left],i);
1054  bbox[i].high = dataset_get(vind[left],i);
1055  }
1056  for (IndexType k=left+1; k<right; ++k) {
1057  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1058  if (bbox[i].low>dataset_get(vind[k],i)) bbox[i].low=dataset_get(vind[k],i);
1059  if (bbox[i].high<dataset_get(vind[k],i)) bbox[i].high=dataset_get(vind[k],i);
1060  }
1061  }
1062  }
1063  else {
1064  IndexType idx;
1065  int cutfeat;
1066  DistanceType cutval;
1067  middleSplit_(&vind[0]+left, right-left, idx, cutfeat, cutval, bbox);
1068 
1069  node->node_type.sub.divfeat = cutfeat;
1070 
1071  BoundingBox left_bbox(bbox);
1072  left_bbox[cutfeat].high = cutval;
1073  node->child1 = divideTree(left, left+idx, left_bbox);
1074 
1075  BoundingBox right_bbox(bbox);
1076  right_bbox[cutfeat].low = cutval;
1077  node->child2 = divideTree(left+idx, right, right_bbox);
1078 
1079  node->node_type.sub.divlow = left_bbox[cutfeat].high;
1080  node->node_type.sub.divhigh = right_bbox[cutfeat].low;
1081 
1082  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1083  bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
1084  bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
1085  }
1086  }
1087 
1088  return node;
1089  }
1090 
1091 
1092  void computeMinMax(IndexType* ind, IndexType count, int element, ElementType& min_elem, ElementType& max_elem)
1093  {
1094  min_elem = dataset_get(ind[0],element);
1095  max_elem = dataset_get(ind[0],element);
1096  for (IndexType i=1; i<count; ++i) {
1097  ElementType val = dataset_get(ind[i],element);
1098  if (val<min_elem) min_elem = val;
1099  if (val>max_elem) max_elem = val;
1100  }
1101  }
1102 
1103  void middleSplit_(IndexType* ind, IndexType count, IndexType& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
1104  {
1105  const auto EPS=static_cast<DistanceType>(0.00001);
1106  ElementType max_span = bbox[0].high-bbox[0].low;
1107  for (int i=1; i<(DIM>0 ? DIM : dim); ++i) {
1108  ElementType span = bbox[i].high-bbox[i].low;
1109  if (span>max_span) {
1110  max_span = span;
1111  }
1112  }
1113  ElementType max_spread = -1;
1114  cutfeat = 0;
1115  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1116  ElementType span = bbox[i].high-bbox[i].low;
1117  if (span>(1-EPS)*max_span) {
1118  ElementType min_elem, max_elem;
1119  computeMinMax(ind, count, cutfeat, min_elem, max_elem);
1120  ElementType spread = max_elem-min_elem;;
1121  if (spread>max_spread) {
1122  cutfeat = i;
1123  max_spread = spread;
1124  }
1125  }
1126  }
1127  // split in the middle
1128  DistanceType split_val = (bbox[cutfeat].low+bbox[cutfeat].high)/2;
1129  ElementType min_elem, max_elem;
1130  computeMinMax(ind, count, cutfeat, min_elem, max_elem);
1131 
1132  if (split_val<min_elem) cutval = min_elem;
1133  else if (split_val>max_elem) cutval = max_elem;
1134  else cutval = split_val;
1135 
1136  IndexType lim1, lim2;
1137  planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
1138 
1139  if (lim1>count/2) index = lim1;
1140  else if (lim2<count/2) index = lim2;
1141  else index = count/2;
1142  }
1143 
1144 
1145  /**
1146  * Subdivide the list of points by a plane perpendicular on axe corresponding
1147  * to the 'cutfeat' dimension at 'cutval' position.
1148  *
1149  * On return:
1150  * dataset[ind[0..lim1-1]][cutfeat]<cutval
1151  * dataset[ind[lim1..lim2-1]][cutfeat]==cutval
1152  * dataset[ind[lim2..count]][cutfeat]>cutval
1153  */
1154  void planeSplit(IndexType* ind, const IndexType count, int cutfeat, DistanceType &cutval, IndexType& lim1, IndexType& lim2)
1155  {
1156  /* Move vector indices for left subtree to front of list. */
1157  IndexType left = 0;
1158  IndexType right = count-1;
1159  for (;; ) {
1160  while (left<=right && dataset_get(ind[left],cutfeat)<cutval) ++left;
1161  while (right && left<=right && dataset_get(ind[right],cutfeat)>=cutval) --right;
1162  if (left>right || !right) break; // "!right" was added to support unsigned Index types
1163  std::swap(ind[left], ind[right]);
1164  ++left;
1165  --right;
1166  }
1167  /* If either list is empty, it means that all remaining features
1168  * are identical. Split in the middle to maintain a balanced tree.
1169  */
1170  lim1 = left;
1171  right = count-1;
1172  for (;; ) {
1173  while (left<=right && dataset_get(ind[left],cutfeat)<=cutval) ++left;
1174  while (right && left<=right && dataset_get(ind[right],cutfeat)>cutval) --right;
1175  if (left>right || !right) break; // "!right" was added to support unsigned Index types
1176  std::swap(ind[left], ind[right]);
1177  ++left;
1178  --right;
1179  }
1180  lim2 = left;
1181  }
1182 
1184  {
1185  assert(vec);
1186  DistanceType distsq = DistanceType();
1187 
1188  for (int i = 0; i < (DIM>0 ? DIM : dim); ++i) {
1189  if (vec[i] < root_bbox[i].low) {
1190  dists[i] = distance.accum_dist(vec[i], root_bbox[i].low, i);
1191  distsq += dists[i];
1192  }
1193  if (vec[i] > root_bbox[i].high) {
1194  dists[i] = distance.accum_dist(vec[i], root_bbox[i].high, i);
1195  distsq += dists[i];
1196  }
1197  }
1198 
1199  return distsq;
1200  }
1201 
1202  /**
1203  * Performs an exact search in the tree starting from a node.
1204  * \tparam RESULTSET Should be any ResultSet<DistanceType>
1205  */
1206  template <class RESULTSET>
1207  void searchLevel(RESULTSET& result_set, const ElementType* vec, const NodePtr node, DistanceType mindistsq,
1208  distance_vector_t& dists, const float epsError) const
1209  {
1210  /* If this is a leaf node, then do check and return. */
1211  if ((node->child1 == nullptr)&&(node->child2 == nullptr)) {
1212  //count_leaf += (node->lr.right-node->lr.left); // Removed since was neither used nor returned to the user.
1213  DistanceType worst_dist = result_set.worstDist();
1214  for (IndexType i=node->node_type.lr.left; i<node->node_type.lr.right; ++i) {
1215  const IndexType index = vind[i];// reorder... : i;
1216  DistanceType dist = distance(vec, index, (DIM>0 ? DIM : dim));
1217  if (dist<worst_dist) {
1218  result_set.addPoint(dist,vind[i]);
1219  }
1220  }
1221  return;
1222  }
1223 
1224  /* Which child branch should be taken first? */
1225  int idx = node->node_type.sub.divfeat;
1226  ElementType val = vec[idx];
1227  DistanceType diff1 = val - node->node_type.sub.divlow;
1228  DistanceType diff2 = val - node->node_type.sub.divhigh;
1229 
1230  NodePtr bestChild;
1231  NodePtr otherChild;
1232  DistanceType cut_dist;
1233  if ((diff1+diff2)<0) {
1234  bestChild = node->child1;
1235  otherChild = node->child2;
1236  cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx);
1237  }
1238  else {
1239  bestChild = node->child2;
1240  otherChild = node->child1;
1241  cut_dist = distance.accum_dist( val, node->node_type.sub.divlow, idx);
1242  }
1243 
1244  /* Call recursively to search next level down. */
1245  searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError);
1246 
1247  DistanceType dst = dists[idx];
1248  mindistsq = mindistsq + cut_dist - dst;
1249  dists[idx] = cut_dist;
1250  if (mindistsq*epsError<=result_set.worstDist()) {
1251  searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError);
1252  }
1253  dists[idx] = dst;
1254  }
1255 
1256  public:
1257  /** Stores the index in a binary file.
1258  * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when loading the index object it must be constructed associated to the same source of data points used while building it.
1259  * See the example: examples/saveload_example.cpp
1260  * \sa loadIndex */
1261  void saveIndex(FILE* stream)
1262  {
1263  save_value(stream, m_size);
1264  save_value(stream, dim);
1265  save_value(stream, root_bbox);
1266  save_value(stream, m_leaf_max_size);
1267  save_value(stream, vind);
1268  save_tree(stream, root_node);
1269  }
1270 
1271  /** Loads a previous index from a binary file.
1272  * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the index object must be constructed associated to the same source of data points used while building the index.
1273  * See the example: examples/saveload_example.cpp
1274  * \sa loadIndex */
1275  void loadIndex(FILE* stream)
1276  {
1277  load_value(stream, m_size);
1278  load_value(stream, dim);
1279  load_value(stream, root_bbox);
1280  load_value(stream, m_leaf_max_size);
1281  load_value(stream, vind);
1282  load_tree(stream, root_node);
1283  }
1284 
1285  }; // class KDTree
1286 
1287 
1288  /** An L2-metric KD-tree adaptor for working with data directly stored in an Eigen Matrix, without duplicating the data storage.
1289  * Each row in the matrix represents a point in the state space.
1290  *
1291  * Example of usage:
1292  * \code
1293  * Eigen::Matrix<num_t,Dynamic,Dynamic> mat;
1294  * // Fill out "mat"...
1295  *
1296  * typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix<num_t,Dynamic,Dynamic> > my_kd_tree_t;
1297  * const int max_leaf = 10;
1298  * my_kd_tree_t mat_index(dimdim, mat, max_leaf );
1299  * mat_index.index->buildIndex();
1300  * mat_index.index->...
1301  * \endcode
1302  *
1303  * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations.
1304  * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
1305  */
1306  template <class MatrixType, int DIM = -1, class Distance = nanoflann::metric_L2>
1308  {
1310  typedef typename MatrixType::Scalar num_t;
1311  typedef typename MatrixType::Index IndexType;
1312  typedef typename Distance::template traits<num_t,self_t>::distance_t metric_t;
1314 
1315  index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index.
1316 
1317  /// Constructor: takes a const ref to the matrix object with the data points
1318  KDTreeEigenMatrixAdaptor(const int dimensionality, const MatrixType &mat, const int leaf_max_size = 10) : m_data_matrix(mat)
1319  {
1320  const IndexType dims = mat.cols();
1321  if (dims!=dimensionality) throw std::runtime_error("Error: 'dimensionality' must match column count in data matrix");
1322  if (DIM>0 && static_cast<int>(dims)!=DIM)
1323  throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument");
1324  index = new index_t( dims, *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) );
1325  index->buildIndex();
1326  }
1327  private:
1328  /** Hidden copy constructor, to disallow copying this class (Not implemented) */
1329  KDTreeEigenMatrixAdaptor(const self_t&) = delete;
1330  public:
1331 
1333  delete index;
1334  }
1335 
1336  const MatrixType &m_data_matrix;
1337 
1338  /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]).
1339  * Note that this is a short-cut method for index->findNeighbors().
1340  * The user can also call index->... methods as desired.
1341  * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
1342  */
1343  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
1344  {
1345  nanoflann::KNNResultSet<num_t,IndexType> resultSet(num_closest);
1346  resultSet.init(out_indices, out_distances_sq);
1347  index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
1348  }
1349 
1350  /** @name Interface expected by KDTreeSingleIndexAdaptor
1351  * @{ */
1352 
1353  const self_t & derived() const {
1354  return *this;
1355  }
1357  return *this;
1358  }
1359 
1360  // Must return the number of data points
1361  inline size_t kdtree_get_point_count() const {
1362  return m_data_matrix.rows();
1363  }
1364 
1365  // Returns the L2 distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
1366  inline num_t kdtree_distance(const num_t *p1, const IndexType idx_p2,IndexType size) const
1367  {
1368  num_t s=0;
1369  for (IndexType i=0; i<size; i++) {
1370  const num_t d= p1[i]-m_data_matrix.coeff(idx_p2,i);
1371  s+=d*d;
1372  }
1373  return s;
1374  }
1375 
1376  // Returns the dim'th component of the idx'th point in the class:
1377  inline num_t kdtree_get_pt(const IndexType idx, int dim) const {
1378  return m_data_matrix.coeff(idx,IndexType(dim));
1379  }
1380 
1381  // Optional bounding-box computation: return false to default to a standard bbox computation loop.
1382  // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
1383  // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
1384  template <class BBOX>
1385  bool kdtree_get_bbox(BBOX& /*bb*/) const {
1386  return false;
1387  }
1388 
1389  /** @} */
1390 
1391  }; // end of KDTreeEigenMatrixAdaptor
1392  /** @} */
1393 
1394 /** @} */ // end of grouping
1395 } // end of NS
1396 
1397 
1398 #endif /* NANOFLANN_HPP_ */
An L2-metric KD-tree adaptor for working with data directly stored in an Eigen Matrix, without duplicating the data storage.
Definition: nanoflann.hpp:1307
std::pair< IndexType, DistanceType > worst_item() const
Find the worst result (furtherest neighbor) without copying or sorting Pre-conditions: size() > 0...
Definition: nanoflann.hpp:184
DistanceType accum_dist(const U a, const V b, int) const
Definition: nanoflann.hpp:343
std::reverse_iterator< iterator > reverse_iterator
Definition: nanoflann.hpp:629
size_t size() const
Returns number of points in dataset.
Definition: nanoflann.hpp:869
double Scalar
Definition: KmUtils.h:43
void freeIndex()
Frees the previously-built index.
Definition: nanoflann.hpp:848
void swap(CArray< T, N > &y)
Definition: nanoflann.hpp:666
const DataSource & data_source
Definition: nanoflann.hpp:263
const T * data() const
Definition: nanoflann.hpp:668
Metaprogramming helper traits class for the L2 (Euclidean) metric.
Definition: nanoflann.hpp:383
std::vector< IndexType > vind
Array of indices to vectors in the dataset.
Definition: nanoflann.hpp:753
Manhattan distance functor (generic version, optimized for high-dimensionality data sets)...
Definition: nanoflann.hpp:258
iterator begin()
Definition: nanoflann.hpp:622
void loadIndex(FILE *stream)
Loads a previous index from a binary file.
Definition: nanoflann.hpp:1275
Squared Euclidean (L2) distance functor (suitable for low-dimensionality datasets, like 2D or 3D point clouds) Corresponding distance traits: nanoflann::metric_L2_Simple.
Definition: nanoflann.hpp:355
const DistanceType radius
Definition: nanoflann.hpp:147
size_t usedMemory() const
Computes the inde memory usage Returns: memory used by the index.
Definition: nanoflann.hpp:880
KDTreeEigenMatrixAdaptor< MatrixType, DIM, Distance > self_t
Definition: nanoflann.hpp:1309
~PooledAllocator()
Destructor.
Definition: nanoflann.hpp:496
size_t m_size_at_index_build
Number of points in the dataset when the index was built.
Definition: nanoflann.hpp:766
std::ptrdiff_t difference_type
Definition: nanoflann.hpp:619
RadiusResultSet(DistanceType radius_, std::vector< std::pair< IndexType, DistanceType > > &indices_dists)
Definition: nanoflann.hpp:151
static size_type size()
Definition: nanoflann.hpp:659
const KDTreeSingleIndexAdaptorParams index_params
Definition: nanoflann.hpp:763
static void rangecheck(size_type i)
Definition: nanoflann.hpp:682
reverse_iterator rbegin()
Definition: nanoflann.hpp:643
void init(IndexType *indices_, DistanceType *dists_)
Definition: nanoflann.hpp:90
IndexType right
Indices of points in leaf node.
Definition: nanoflann.hpp:777
const self_t & derived() const
Definition: nanoflann.hpp:1353
const_reference at(size_type i) const
Definition: nanoflann.hpp:652
mrpt::vision::TStereoCalibParams params
DistanceType * dists
Definition: nanoflann.hpp:81
DistanceType accum_dist(const U a, const V b, int) const
Definition: nanoflann.hpp:294
DistanceType operator()(const T *a, const size_t b_idx, size_t size) const
Definition: nanoflann.hpp:364
iterator end()
Definition: nanoflann.hpp:624
L2_Simple_Adaptor< T, DataSource > distance_t
Definition: nanoflann.hpp:393
float eps
search for eps-approximate neighbours (default: 0)
Definition: nanoflann.hpp:420
const_reference front() const
Definition: nanoflann.hpp:655
void planeSplit(IndexType *ind, const IndexType count, int cutfeat, DistanceType &cutval, IndexType &lim1, IndexType &lim2)
Subdivide the list of points by a plane perpendicular on axe corresponding to the &#39;cutfeat&#39; dimension...
Definition: nanoflann.hpp:1154
void resize(const size_t nElements)
This method has no effects in this class, but raises an exception if the expected size does not match...
Definition: nanoflann.hpp:664
L2_Adaptor< T, DataSource > distance_t
Definition: nanoflann.hpp:386
DistanceType computeInitialDistances(const ElementType *vec, distance_vector_t &dists) const
Definition: nanoflann.hpp:1183
DistanceType operator()(const T *a, const size_t b_idx, size_t size, DistanceType worst_dist=-1) const
Definition: nanoflann.hpp:267
L1_Adaptor< T, DataSource > distance_t
Definition: nanoflann.hpp:379
void load_value(FILE *stream, T &value, size_t count=1)
Definition: nanoflann.hpp:223
Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors when DIM=-1.
Definition: nanoflann.hpp:689
const DataSource & data_source
Definition: nanoflann.hpp:311
bool operator()(const PairType &p1, const PairType &p2) const
PairType will be typically: std::pair<IndexType,DistanceType>
Definition: nanoflann.hpp:198
reference back()
Definition: nanoflann.hpp:656
KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size=10)
Definition: nanoflann.hpp:405
void searchLevel(RESULTSET &result_set, const ElementType *vec, const NodePtr node, DistanceType mindistsq, distance_vector_t &dists, const float epsError) const
Performs an exact search in the tree starting from a node.
Definition: nanoflann.hpp:1207
KDTreeEigenMatrixAdaptor(const int dimensionality, const MatrixType &mat, const int leaf_max_size=10)
The kd-tree index for the user to call its methods as usual with any other FLANN index.
Definition: nanoflann.hpp:1318
reverse_iterator rend()
Definition: nanoflann.hpp:645
void init_vind()
Make sure the auxiliary list vind has the same size than the current dataset, and re-generate if size...
Definition: nanoflann.hpp:970
DistanceType accum_dist(const U a, const V b, int) const
Definition: nanoflann.hpp:369
bool findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const
Find set of nearest neighbors to vec[0:dim-1].
Definition: nanoflann.hpp:901
L2_Adaptor(const DataSource &_data_source)
Definition: nanoflann.hpp:313
void addPoint(DistanceType dist, IndexType index)
Definition: nanoflann.hpp:165
const size_t BLOCKSIZE
Definition: nanoflann.hpp:460
void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int=10) const
Query for the num_closest closest points to a given point (entered as query_point[0:dim-1]).
Definition: nanoflann.hpp:1343
array_or_vector_selector< DIM, DistanceType >::container_t distance_vector_t
Define "distance_vector_t" as a fixed-size or variable-size container depending on "DIM"...
Definition: nanoflann.hpp:799
static size_type max_size()
Definition: nanoflann.hpp:661
bool sorted
only for radius search, require neighbours sorted by distance (default: true)
Definition: nanoflann.hpp:421
const DatasetAdaptor & dataset
The dataset used by this index.
Definition: nanoflann.hpp:761
operator "<" for std::sort()
Definition: nanoflann.hpp:194
reference operator[](size_type i)
Definition: nanoflann.hpp:648
int val
Definition: mrpt_jpeglib.h:957
Distance::DistanceType DistanceType
Definition: nanoflann.hpp:747
void middleSplit_(IndexType *ind, IndexType count, IndexType &index, int &cutfeat, DistanceType &cutval, const BoundingBox &bbox)
Definition: nanoflann.hpp:1103
const double eps
const T * const_iterator
Definition: nanoflann.hpp:615
CountType size() const
Definition: nanoflann.hpp:99
DistanceType operator()(const T *a, const size_t b_idx, size_t size, DistanceType worst_dist=-1) const
Definition: nanoflann.hpp:315
std::vector< std::pair< IndexType, DistanceType > > & m_indices_dists
Definition: nanoflann.hpp:149
T * allocate(size_t count=1)
Allocates (using C&#39;s malloc) a generic type T.
Definition: nanoflann.hpp:437
Distance::ElementType ElementType
Definition: nanoflann.hpp:746
num_t kdtree_distance(const num_t *p1, const IndexType idx_p2, IndexType size) const
Definition: nanoflann.hpp:1366
size_t radiusSearch(const ElementType *query_point, const DistanceType &radius, std::vector< std::pair< IndexType, DistanceType > > &IndicesDists, const SearchParams &searchParams) const
Find all the neighbors to query_point[0:dim-1] within a maximum radius.
Definition: nanoflann.hpp:945
reference at(size_type i)
Definition: nanoflann.hpp:651
std::size_t size_type
Definition: nanoflann.hpp:618
KNNResultSet(CountType capacity_)
Definition: nanoflann.hpp:86
size_t radiusSearchCustomCallback(const ElementType *query_point, SEARCH_CALLBACK &resultSet, const SearchParams &searchParams=SearchParams()) const
Just like radiusSearch() but with a custom callback class for each point found in the radius of the q...
Definition: nanoflann.hpp:960
void saveIndex(FILE *stream)
Stores the index in a binary file.
Definition: nanoflann.hpp:1261
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:408
A result-set class used when performing a radius based search.
Definition: nanoflann.hpp:144
ElementType dataset_get(size_t idx, int component) const
Helper accessor to the dataset points:
Definition: nanoflann.hpp:979
DistanceType worstDist() const
Definition: nanoflann.hpp:171
KDTreeSingleIndexAdaptor(const int dimensionality, const DatasetAdaptor &inputData, const KDTreeSingleIndexAdaptorParams &params=KDTreeSingleIndexAdaptorParams())
KDTree constructor.
Definition: nanoflann.hpp:831
const_reverse_iterator rend() const
Definition: nanoflann.hpp:646
T * allocate(const size_t count=1)
Allocates (using this pool) a generic type T.
Definition: nanoflann.hpp:568
A STL container (as wrapper) for arrays of constant size defined at compile time (class imported from...
Definition: nanoflann.hpp:607
int checks
Ignored parameter (Kept for compatibility with the FLANN interface).
Definition: nanoflann.hpp:419
L1_Adaptor(const DataSource &_data_source)
Definition: nanoflann.hpp:265
void addPoint(DistanceType dist, IndexType index)
Definition: nanoflann.hpp:110
const_iterator end() const
Definition: ts_hash_map.h:246
const_reference back() const
Definition: nanoflann.hpp:657
void free_all()
Frees all allocated memory chunks.
Definition: nanoflann.hpp:501
const_iterator begin() const
Definition: ts_hash_map.h:240
size_t knnSearch(const ElementType *query_point, const size_t num_closest, IndexType *out_indices, DistanceType *out_distances_sq, const int=10) const
Find the "num_closest" nearest neighbors to the query_point[0:dim-1].
Definition: nanoflann.hpp:925
Distance::template traits< num_t, self_t >::distance_t metric_t
Definition: nanoflann.hpp:1312
size_t veclen() const
Returns the length of each point in the dataset.
Definition: nanoflann.hpp:872
num_t kdtree_get_pt(const IndexType idx, int dim) const
Definition: nanoflann.hpp:1377
_DistanceType DistanceType
Definition: nanoflann.hpp:261
const_reference operator[](size_type i) const
Definition: nanoflann.hpp:649
array_or_vector_selector< DIM, Interval >::container_t BoundingBox
Define "BoundingBox" as a fixed-size or variable-size container depending on "DIM".
Definition: nanoflann.hpp:796
Squared Euclidean distance functor (generic version, optimized for high-dimensionality data sets)...
Definition: nanoflann.hpp:306
PooledAllocator()
Default constructor.
Definition: nanoflann.hpp:489
NodePtr root_node
The KD-tree used to find neighbours.
Definition: nanoflann.hpp:802
const DataSource & data_source
Definition: nanoflann.hpp:360
void assign(const T &value)
Definition: nanoflann.hpp:677
Parameters (see README.md)
Definition: nanoflann.hpp:403
size_t m_size
Number of current poins in the dataset.
Definition: nanoflann.hpp:765
DistanceType worstDist() const
Definition: nanoflann.hpp:133
static bool empty()
Definition: nanoflann.hpp:660
void computeMinMax(IndexType *ind, IndexType count, int element, ElementType &min_elem, ElementType &max_elem)
Definition: nanoflann.hpp:1092
NodePtr divideTree(const IndexType left, const IndexType right, BoundingBox &bbox)
Create a tree node that subdivides the list of vecs from vind[first] to vind[last].
Definition: nanoflann.hpp:1041
L2_Simple_Adaptor(const DataSource &_data_source)
Definition: nanoflann.hpp:362
void assign(const size_t n, const T &value)
Definition: nanoflann.hpp:679
void save_tree(FILE *stream, NodePtr tree)
Definition: nanoflann.hpp:984
int dim
Dimensionality of each data point.
Definition: nanoflann.hpp:767
struct nanoflann::KDTreeSingleIndexAdaptor::Node::@17::leaf lr
Search options for KDTreeSingleIndexAdaptor::findNeighbors()
Definition: nanoflann.hpp:413
std::reverse_iterator< const_iterator > const_reverse_iterator
Definition: nanoflann.hpp:630
const size_t WORDSIZE
Pooled storage allocator.
Definition: nanoflann.hpp:459
void computeBoundingBox(BoundingBox &bbox)
Definition: nanoflann.hpp:1009
void save_value(FILE *stream, const T &value, size_t count=1)
Definition: nanoflann.hpp:209
void load_tree(FILE *stream, NodePtr &tree)
Definition: nanoflann.hpp:996
PooledAllocator pool
Pooled memory allocator.
Definition: nanoflann.hpp:812
const_iterator begin() const
Definition: nanoflann.hpp:623
int divfeat
Dimension used for subdivision.
Definition: nanoflann.hpp:781
Metaprogramming helper traits class for the L1 (Manhattan) metric.
Definition: nanoflann.hpp:376
union nanoflann::KDTreeSingleIndexAdaptor::Node::@17 node_type
Union used because a node can be either a LEAF node or a non-leaf node, so both data fields are never...
const_reverse_iterator rbegin() const
Definition: nanoflann.hpp:644
KDTreeSingleIndexAdaptor< metric_t, self_t, DIM, IndexType > index_t
Definition: nanoflann.hpp:1313
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
void * malloc(const size_t req_size)
Returns a pointer to a piece of new memory of the given size in bytes allocated from the pool...
Definition: nanoflann.hpp:515
SearchParams(int checks_IGNORED_=32, float eps_=0, bool sorted_=true)
Note: The first argument (checks_IGNORED_) is ignored, but kept for compatibility with the FLANN inte...
Definition: nanoflann.hpp:416
struct nanoflann::KDTreeSingleIndexAdaptor::Node::@17::nonleaf sub
Metaprogramming helper traits class for the L2_simple (Euclidean) metric.
Definition: nanoflann.hpp:390
size_t m_size
Number of elements accessed with write access so far.
Definition: ts_hash_map.h:175
_DistanceType DistanceType
Definition: nanoflann.hpp:309
void set_radius_and_clear(const DistanceType r)
Clears the result set and adjusts the search radius.
Definition: nanoflann.hpp:174
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1807
const_iterator end() const
Definition: nanoflann.hpp:625
reference front()
Definition: nanoflann.hpp:654
void buildIndex()
Builds the index.
Definition: nanoflann.hpp:858
const T & const_reference
Definition: nanoflann.hpp:617
Node * child2
Child nodes (both=NULL mean its a leaf node)
Definition: nanoflann.hpp:785



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020