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



Page generated by Doxygen 1.9.1 for MRPT 1.4.0 SVN: at Sat Jan 30 21:34:41 UTC 2021