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 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 {
59  #define NANOFLANN_VERSION 0x113
60 
63  template <typename DistanceType, typename IndexType = size_t, typename CountType = size_t>
64  class KNNResultSet
65  {
66  IndexType * indices;
67  DistanceType* dists;
68  CountType capacity;
69  CountType count;
70 
71  public:
72  inline KNNResultSet(CountType capacity_) : 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 
128  template <typename DistanceType, typename IndexType = size_t>
129  class RadiusResultSet
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<IndexType,DistanceType>(index,dist));
154  }
155 
156  inline DistanceType worstDist() const { return radius; }
157 
159  inline void set_radius_and_clear( const DistanceType r )
160  {
161  radius = r;
162  clear();
163  }
164 
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 
179  struct IndexDist_Sorter
180  {
182  template <typename PairType>
183  inline bool operator()(const PairType &p1, const PairType &p2) const {
184  return p1.second < p2.second;
185  }
186  };
187 
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  }
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 
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 dim) const
286  {
287  return (a-b)*(a-b);
288  }
289  };
290 
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 dim) const
335  {
336  return (a-b)*(a-b);
337  }
338  };
339 
345  template<class T, class DataSource, typename _DistanceType = T>
346  struct L2_Simple_Adaptor
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 dim) const
361  {
362  return (a-b)*(a-b);
363  }
364  };
365 
367  struct metric_L1 {
368  template<class T, class DataSource>
369  struct traits {
370  typedef L1_Adaptor<T,DataSource> distance_t;
371  };
372  };
374  struct metric_L2 {
375  template<class T, class DataSource>
376  struct traits {
377  typedef L2_Adaptor<T,DataSource> distance_t;
378  };
379  };
381  struct metric_L2_Simple {
382  template<class T, class DataSource>
383  struct traits {
384  typedef L2_Simple_Adaptor<T,DataSource> distance_t;
385  };
386  };
387 
397  struct KDTreeSingleIndexAdaptorParams
398  {
399  KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10, int dim_ = -1) :
400  leaf_max_size(_leaf_max_size), dim(dim_)
401  {}
402 
403  size_t leaf_max_size;
404  int dim;
405  };
406 
408  struct SearchParams
409  {
411  SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true ) :
412  eps(eps_), sorted(sorted_) {}
413 
414  int checks;
415  float eps;
416  bool sorted;
417  };
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 
454  const size_t WORDSIZE=16;
455  const size_t BLOCKSIZE=8192;
456 
457  class PooledAllocator
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 
470 
471  public:
472  size_t usedMemory;
473  size_t wastedMemory;
474 
478  PooledAllocator(const size_t blocksize = BLOCKSIZE)
479  {
480  this->blocksize = blocksize;
481  remaining = 0;
482  base = NULL;
483 
484  usedMemory = 0;
485  wastedMemory = 0;
486  }
487 
491  ~PooledAllocator()
492  {
493  while (base != NULL) {
494  void *prev = *((void**) base); /* Get pointer to prev block. */
495  ::free(base);
496  base = prev;
497  }
498  }
499 
504  void* malloc(const size_t req_size)
505  {
506  /* Round size up to a multiple of wordsize. The following expression
507  only works for WORDSIZE that is a power of 2, by masking last bits of
508  incremented size to zero.
509  */
510  const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1);
511 
512  /* Check whether a new block must be allocated. Note that the first word
513  of a block is reserved for a pointer to the previous block.
514  */
515  if (size > remaining) {
516 
517  wastedMemory += remaining;
518 
519  /* Allocate new storage. */
520  const size_t blocksize = (size + sizeof(void*) + (WORDSIZE-1) > BLOCKSIZE) ?
521  size + sizeof(void*) + (WORDSIZE-1) : BLOCKSIZE;
522 
523  // use the standard C malloc to allocate memory
524  void* m = ::malloc(blocksize);
525  if (!m) {
526  fprintf(stderr,"Failed to allocate memory.\n");
527  return NULL;
528  }
529 
530  /* Fill first word of new block with pointer to previous block. */
531  ((void**) m)[0] = base;
532  base = m;
533 
534  size_t shift = 0;
535  //int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & (WORDSIZE-1))) & (WORDSIZE-1);
536 
537  remaining = blocksize - sizeof(void*) - shift;
538  loc = ((char*)m + sizeof(void*) + shift);
539  }
540  void* rloc = loc;
541  loc = (char*)loc + size;
542  remaining -= size;
543 
544  usedMemory += size;
545 
546  return rloc;
547  }
548 
556  template <typename T>
557  T* allocate(const size_t count = 1)
558  {
559  T* mem = (T*) this->malloc(sizeof(T)*count);
560  return mem;
561  }
562 
563  };
603  template <typename Distance, class DatasetAdaptor,int DIM = -1, typename IndexType = size_t>
604  class KDTreeSingleIndexAdaptor
605  {
606  public:
607  typedef typename Distance::ElementType ElementType;
608  typedef typename Distance::DistanceType DistanceType;
609  protected:
610 
614  std::vector<IndexType> vind;
615 
616  size_t m_leaf_max_size;
617 
618 
622  const DatasetAdaptor &dataset;
623 
624  const KDTreeSingleIndexAdaptorParams index_params;
625 
626  size_t m_size;
627  int dim;
628 
629 
630  /*--------------------- Internal Data Structures --------------------------*/
631  struct Node
632  {
633  union {
634  struct
635  {
639  IndexType left, right;
640  } lr;
641  struct
642  {
646  int divfeat;
650  DistanceType divlow, divhigh;
651  } sub;
652  };
656  Node* child1, * child2;
657  };
658  typedef Node* NodePtr;
659 
660 
661  struct Interval
662  {
663  ElementType low, high;
664  };
665 
666  typedef std::vector<Interval> BoundingBox;
667 
668 
673  template <typename T, typename DistanceType>
674  struct BranchStruct
675  {
676  T node; /* Tree node at which search resumes */
677  DistanceType mindist; /* Minimum distance to query for all nodes below. */
678 
679  BranchStruct() {}
680  BranchStruct(const T& aNode, DistanceType dist) : node(aNode), mindist(dist) {}
681 
682  inline bool operator<(const BranchStruct<T, DistanceType>& rhs) const
683  {
684  return mindist<rhs.mindist;
685  }
686  };
687 
691  NodePtr root_node;
692  typedef BranchStruct<NodePtr, DistanceType> BranchSt;
693  typedef BranchSt* Branch;
694 
695  BoundingBox root_bbox;
696 
704  PooledAllocator pool;
705 
706  public:
707 
708  Distance distance;
709 
717  KDTreeSingleIndexAdaptor(const int dimensionality, const DatasetAdaptor& inputData, const KDTreeSingleIndexAdaptorParams& params = KDTreeSingleIndexAdaptorParams() ) :
718  dataset(inputData), index_params(params), distance(inputData)
719  {
720  m_size = dataset.kdtree_get_point_count();
721  dim = dimensionality;
722  if (DIM>0) dim=DIM;
723  else {
724  if (params.dim>0) dim = params.dim;
725  }
726  m_leaf_max_size = params.leaf_max_size;
727 
728  // Create a permutable array of indices to the input vectors.
729  init_vind();
730  }
731 
735  ~KDTreeSingleIndexAdaptor()
736  {
737  }
738 
742  void buildIndex()
743  {
744  init_vind();
745  computeBoundingBox(root_bbox);
746  root_node = divideTree(0, m_size, root_bbox ); // construct the tree
747  }
748 
752  size_t size() const
753  {
754  return m_size;
755  }
756 
760  size_t veclen() const
761  {
762  return static_cast<size_t>(DIM>0 ? DIM : dim);
763  }
764 
769  size_t usedMemory() const
770  {
771  return pool.usedMemory+pool.wastedMemory+dataset.kdtree_get_point_count()*sizeof(IndexType); // pool memory and vind array memory
772  }
773 
788  template <typename RESULTSET>
789  void findNeighbors(RESULTSET& result, const ElementType* vec, const SearchParams& searchParams) const
790  {
791  assert(vec);
792  float epsError = 1+searchParams.eps;
793 
794  std::vector<DistanceType> dists( (DIM>0 ? DIM : dim) ,0);
795  DistanceType distsq = computeInitialDistances(vec, dists);
796  searchLevel(result, vec, root_node, distsq, dists, epsError); // "count_leaf" parameter removed since was neither used nor returned to the user.
797  }
798 
805  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
806  {
807  nanoflann::KNNResultSet<DistanceType,IndexType> resultSet(num_closest);
808  resultSet.init(out_indices, out_distances_sq);
809  this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
810  }
811 
824  size_t radiusSearch(const ElementType *query_point,const DistanceType radius, std::vector<std::pair<IndexType,DistanceType> >& IndicesDists, const SearchParams& searchParams) const
825  {
826  RadiusResultSet<DistanceType,IndexType> resultSet(radius,IndicesDists);
827  this->findNeighbors(resultSet, query_point, searchParams);
828 
829  if (searchParams.sorted)
830  std::sort(IndicesDists.begin(),IndicesDists.end(), IndexDist_Sorter() );
831 
832  return resultSet.size();
833  }
834 
837  private:
839  void init_vind()
840  {
841  // Create a permutable array of indices to the input vectors.
842  m_size = dataset.kdtree_get_point_count();
843  if (vind.size()!=m_size)
844  {
845  vind.resize(m_size);
846  for (size_t i = 0; i < m_size; i++) vind[i] = i;
847  }
848  }
849 
851  inline ElementType dataset_get(size_t idx, int component) const {
852  return dataset.kdtree_get_pt(idx,component);
853  }
854 
855 
856  void save_tree(FILE* stream, NodePtr tree)
857  {
858  save_value(stream, *tree);
859  if (tree->child1!=NULL) {
860  save_tree(stream, tree->child1);
861  }
862  if (tree->child2!=NULL) {
863  save_tree(stream, tree->child2);
864  }
865  }
866 
867 
868  void load_tree(FILE* stream, NodePtr& tree)
869  {
870  tree = pool.allocate<Node>();
871  load_value(stream, *tree);
872  if (tree->child1!=NULL) {
873  load_tree(stream, tree->child1);
874  }
875  if (tree->child2!=NULL) {
876  load_tree(stream, tree->child2);
877  }
878  }
879 
880 
881  void computeBoundingBox(BoundingBox& bbox)
882  {
883  bbox.resize((DIM>0 ? DIM : dim));
884  if (dataset.kdtree_get_bbox(bbox))
885  {
886  // Done! It was implemented in derived class
887  }
888  else
889  {
890  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
891  bbox[i].low =
892  bbox[i].high = dataset_get(0,i);
893  }
894  const size_t N = dataset.kdtree_get_point_count();
895  for (size_t k=1; k<N; ++k) {
896  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
897  if (dataset_get(k,i)<bbox[i].low) bbox[i].low = dataset_get(k,i);
898  if (dataset_get(k,i)>bbox[i].high) bbox[i].high = dataset_get(k,i);
899  }
900  }
901  }
902  }
903 
904 
914  NodePtr divideTree(const IndexType left, const IndexType right, BoundingBox& bbox)
915  {
916  NodePtr node = pool.allocate<Node>(); // allocate memory
917 
918  /* If too few exemplars remain, then make this a leaf node. */
919  if ( (right-left) <= m_leaf_max_size) {
920  node->child1 = node->child2 = NULL; /* Mark as leaf node. */
921  node->lr.left = left;
922  node->lr.right = right;
923 
924  // compute bounding-box of leaf points
925  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
926  bbox[i].low = dataset_get(vind[left],i);
927  bbox[i].high = dataset_get(vind[left],i);
928  }
929  for (IndexType k=left+1; k<right; ++k) {
930  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
931  if (bbox[i].low>dataset_get(vind[k],i)) bbox[i].low=dataset_get(vind[k],i);
932  if (bbox[i].high<dataset_get(vind[k],i)) bbox[i].high=dataset_get(vind[k],i);
933  }
934  }
935  }
936  else {
937  IndexType idx;
938  int cutfeat;
939  DistanceType cutval;
940  middleSplit_(&vind[0]+left, right-left, idx, cutfeat, cutval, bbox);
941 
942  node->sub.divfeat = cutfeat;
943 
944  BoundingBox left_bbox(bbox);
945  left_bbox[cutfeat].high = cutval;
946  node->child1 = divideTree(left, left+idx, left_bbox);
947 
948  BoundingBox right_bbox(bbox);
949  right_bbox[cutfeat].low = cutval;
950  node->child2 = divideTree(left+idx, right, right_bbox);
951 
952  node->sub.divlow = left_bbox[cutfeat].high;
953  node->sub.divhigh = right_bbox[cutfeat].low;
954 
955  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
956  bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
957  bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
958  }
959  }
960 
961  return node;
962  }
963 
964  void computeMinMax(IndexType* ind, IndexType count, int element, ElementType& min_elem, ElementType& max_elem)
965  {
966  min_elem = dataset_get(ind[0],element);
967  max_elem = dataset_get(ind[0],element);
968  for (IndexType i=1; i<count; ++i) {
969  ElementType val = dataset_get(ind[i],element);
970  if (val<min_elem) min_elem = val;
971  if (val>max_elem) max_elem = val;
972  }
973  }
974 
975  void middleSplit(IndexType* ind, IndexType count, IndexType& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
976  {
977  // find the largest span from the approximate bounding box
978  ElementType max_span = bbox[0].high-bbox[0].low;
979  cutfeat = 0;
980  cutval = (bbox[0].high+bbox[0].low)/2;
981  for (int i=1; i<(DIM>0 ? DIM : dim); ++i) {
982  ElementType span = bbox[i].low-bbox[i].low;
983  if (span>max_span) {
984  max_span = span;
985  cutfeat = i;
986  cutval = (bbox[i].high+bbox[i].low)/2;
987  }
988  }
989 
990  // compute exact span on the found dimension
991  ElementType min_elem, max_elem;
992  computeMinMax(ind, count, cutfeat, min_elem, max_elem);
993  cutval = (min_elem+max_elem)/2;
994  max_span = max_elem - min_elem;
995 
996  // check if a dimension of a largest span exists
997  size_t k = cutfeat;
998  for (size_t i=0; i<(DIM>0 ? DIM : dim); ++i) {
999  if (i==k) continue;
1000  ElementType span = bbox[i].high-bbox[i].low;
1001  if (span>max_span) {
1002  computeMinMax(ind, count, i, min_elem, max_elem);
1003  span = max_elem - min_elem;
1004  if (span>max_span) {
1005  max_span = span;
1006  cutfeat = i;
1007  cutval = (min_elem+max_elem)/2;
1008  }
1009  }
1010  }
1011  IndexType lim1, lim2;
1012  planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
1013 
1014  if (lim1>count/2) index = lim1;
1015  else if (lim2<count/2) index = lim2;
1016  else index = count/2;
1017  }
1018 
1019 
1020  void middleSplit_(IndexType* ind, IndexType count, IndexType& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
1021  {
1022  const DistanceType EPS=static_cast<DistanceType>(0.00001);
1023  ElementType max_span = bbox[0].high-bbox[0].low;
1024  for (int i=1; i<(DIM>0 ? DIM : dim); ++i) {
1025  ElementType span = bbox[i].high-bbox[i].low;
1026  if (span>max_span) {
1027  max_span = span;
1028  }
1029  }
1030  ElementType max_spread = -1;
1031  cutfeat = 0;
1032  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1033  ElementType span = bbox[i].high-bbox[i].low;
1034  if (span>(1-EPS)*max_span) {
1035  ElementType min_elem, max_elem;
1036  computeMinMax(ind, count, cutfeat, min_elem, max_elem);
1037  ElementType spread = max_elem-min_elem;;
1038  if (spread>max_spread) {
1039  cutfeat = i;
1040  max_spread = spread;
1041  }
1042  }
1043  }
1044  // split in the middle
1045  DistanceType split_val = (bbox[cutfeat].low+bbox[cutfeat].high)/2;
1046  ElementType min_elem, max_elem;
1047  computeMinMax(ind, count, cutfeat, min_elem, max_elem);
1048 
1049  if (split_val<min_elem) cutval = min_elem;
1050  else if (split_val>max_elem) cutval = max_elem;
1051  else cutval = split_val;
1052 
1053  IndexType lim1, lim2;
1054  planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
1055 
1056  if (lim1>count/2) index = lim1;
1057  else if (lim2<count/2) index = lim2;
1058  else index = count/2;
1059  }
1060 
1061 
1071  void planeSplit(IndexType* ind, const IndexType count, int cutfeat, DistanceType cutval, IndexType& lim1, IndexType& lim2)
1072  {
1073  /* Move vector indices for left subtree to front of list. */
1074  IndexType left = 0;
1075  IndexType right = count-1;
1076  for (;; ) {
1077  while (left<=right && dataset_get(ind[left],cutfeat)<cutval) ++left;
1078  while (right && left<=right && dataset_get(ind[right],cutfeat)>=cutval) --right;
1079  if (left>right || !right) break; // "!right" was added to support unsigned Index types
1080  std::swap(ind[left], ind[right]);
1081  ++left;
1082  --right;
1083  }
1084  /* If either list is empty, it means that all remaining features
1085  * are identical. Split in the middle to maintain a balanced tree.
1086  */
1087  lim1 = left;
1088  right = count-1;
1089  for (;; ) {
1090  while (left<=right && dataset_get(ind[left],cutfeat)<=cutval) ++left;
1091  while (right && left<=right && dataset_get(ind[right],cutfeat)>cutval) --right;
1092  if (left>right || !right) break; // "!right" was added to support unsigned Index types
1093  std::swap(ind[left], ind[right]);
1094  ++left;
1095  --right;
1096  }
1097  lim2 = left;
1098  }
1099 
1100  DistanceType computeInitialDistances(const ElementType* vec, std::vector<DistanceType>& dists) const
1101  {
1102  assert(vec);
1103  DistanceType distsq = 0.0;
1104 
1105  for (int i = 0; i < (DIM>0 ? DIM : dim); ++i) {
1106  if (vec[i] < root_bbox[i].low) {
1107  dists[i] = distance.accum_dist(vec[i], root_bbox[i].low, i);
1108  distsq += dists[i];
1109  }
1110  if (vec[i] > root_bbox[i].high) {
1111  dists[i] = distance.accum_dist(vec[i], root_bbox[i].high, i);
1112  distsq += dists[i];
1113  }
1114  }
1115 
1116  return distsq;
1117  }
1118 
1123  template <class RESULTSET>
1124  void searchLevel(RESULTSET& result_set, const ElementType* vec, const NodePtr node, DistanceType mindistsq,
1125  std::vector<DistanceType>& dists, const float epsError) const
1126  {
1127  /* If this is a leaf node, then do check and return. */
1128  if ((node->child1 == NULL)&&(node->child2 == NULL)) {
1129  //count_leaf += (node->lr.right-node->lr.left); // Removed since was neither used nor returned to the user.
1130  DistanceType worst_dist = result_set.worstDist();
1131  for (IndexType i=node->lr.left; i<node->lr.right; ++i) {
1132  const IndexType index = vind[i];// reorder... : i;
1133  DistanceType dist = distance(vec, index, (DIM>0 ? DIM : dim));
1134  if (dist<worst_dist) {
1135  result_set.addPoint(dist,vind[i]);
1136  }
1137  }
1138  return;
1139  }
1140 
1141  /* Which child branch should be taken first? */
1142  int idx = node->sub.divfeat;
1143  ElementType val = vec[idx];
1144  DistanceType diff1 = val - node->sub.divlow;
1145  DistanceType diff2 = val - node->sub.divhigh;
1146 
1147  NodePtr bestChild;
1148  NodePtr otherChild;
1149  DistanceType cut_dist;
1150  if ((diff1+diff2)<0) {
1151  bestChild = node->child1;
1152  otherChild = node->child2;
1153  cut_dist = distance.accum_dist(val, node->sub.divhigh, idx);
1154  }
1155  else {
1156  bestChild = node->child2;
1157  otherChild = node->child1;
1158  cut_dist = distance.accum_dist( val, node->sub.divlow, idx);
1159  }
1160 
1161  /* Call recursively to search next level down. */
1162  searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError);
1163 
1164  DistanceType dst = dists[idx];
1165  mindistsq = mindistsq + cut_dist - dst;
1166  dists[idx] = cut_dist;
1167  if (mindistsq*epsError<=result_set.worstDist()) {
1168  searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError);
1169  }
1170  dists[idx] = dst;
1171  }
1172 
1173 
1174  void saveIndex(FILE* stream)
1175  {
1176  save_value(stream, m_size);
1177  save_value(stream, dim);
1178  save_value(stream, root_bbox);
1179  save_value(stream, m_leaf_max_size);
1180  save_value(stream, vind);
1181  save_tree(stream, root_node);
1182  }
1183 
1184  void loadIndex(FILE* stream)
1185  {
1186  load_value(stream, m_size);
1187  load_value(stream, dim);
1188  load_value(stream, root_bbox);
1189  load_value(stream, m_leaf_max_size);
1190  load_value(stream, vind);
1191  load_tree(stream, root_node);
1192  }
1193 
1194  }; // class KDTree
1195 
1196 
1216  template <class MatrixType, int DIM = -1, class Distance = nanoflann::metric_L2, typename IndexType = size_t>
1217  struct KDTreeEigenMatrixAdaptor
1218  {
1219  typedef KDTreeEigenMatrixAdaptor<MatrixType,DIM,Distance> self_t;
1220  typedef typename MatrixType::Scalar num_t;
1221  typedef typename Distance::template traits<num_t,self_t>::distance_t metric_t;
1222  typedef KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t;
1223 
1224  index_t* index;
1225 
1227  KDTreeEigenMatrixAdaptor(const int dimensionality, const MatrixType &mat, const int leaf_max_size = 10) : m_data_matrix(mat)
1228  {
1229  const size_t dims = mat.cols();
1230  if (DIM>0 && static_cast<int>(dims)!=DIM)
1231  throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument");
1232  index = new index_t( dims, *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size, dims ) );
1233  index->buildIndex();
1234  }
1235 
1236  ~KDTreeEigenMatrixAdaptor() {
1237  delete index;
1238  }
1239 
1240  const MatrixType &m_data_matrix;
1241 
1247  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
1248  {
1249  nanoflann::KNNResultSet<typename MatrixType::Scalar,IndexType> resultSet(num_closest);
1250  resultSet.init(out_indices, out_distances_sq);
1251  index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
1252  }
1253 
1257  const self_t & derived() const {
1258  return *this;
1259  }
1260  self_t & derived() {
1261  return *this;
1262  }
1263 
1264  // Must return the number of data points
1265  inline size_t kdtree_get_point_count() const {
1266  return m_data_matrix.rows();
1267  }
1268 
1269  // Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
1270  inline num_t kdtree_distance(const num_t *p1, const size_t idx_p2,size_t size) const
1271  {
1272  num_t s=0;
1273  for (size_t i=0; i<size; i++) {
1274  const num_t d= p1[i]-m_data_matrix.coeff(idx_p2,i);
1275  s+=d*d;
1276  }
1277  return s;
1278  }
1279 
1280  // Returns the dim'th component of the idx'th point in the class:
1281  inline num_t kdtree_get_pt(const size_t idx, int dim) const {
1282  return m_data_matrix.coeff(idx,dim);
1283  }
1284 
1285  // Optional bounding-box computation: return false to default to a standard bbox computation loop.
1286  // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
1287  // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
1288  template <class BBOX>
1289  bool kdtree_get_bbox(BBOX &bb) const {
1290  return false;
1291  }
1292 
1295  }; // end of KDTreeEigenMatrixAdaptor // end of grouping
1299 } // end of NS
1300 
1301 
1302 #endif /* NANOFLANN_HPP_ */
nanoflann::metric_L2
Definition: nanoflann.hpp:404
nanoflann::L2_Simple_Adaptor
Definition: nanoflann.hpp:376
nanoflann::KDTreeSingleIndexAdaptor::BranchStruct::mindist
DistanceType mindist
Definition: nanoflann.hpp:707
nanoflann::KNNResultSet::worstDist
DistanceType worstDist() const
Definition: nanoflann.hpp:148
nanoflann::KDTreeSingleIndexAdaptor::DistanceType
Distance::DistanceType DistanceType
Definition: nanoflann.hpp:638
nanoflann::load_value
void load_value(FILE *stream, T &value, size_t count=1)
Definition: nanoflann.hpp:238
nanoflann::PooledAllocator::wastedMemory
size_t wastedMemory
Definition: nanoflann.hpp:503
nanoflann::abs< float >
float abs< float >(float x)
Definition: nanoflann.hpp:269
nanoflann::KDTreeSingleIndexAdaptor::Interval
Definition: nanoflann.hpp:691
nanoflann::metric_L2_Simple
Definition: nanoflann.hpp:411
nanoflann::SearchParams::eps
float eps
search for eps-approximate neighbours (default: 0)
Definition: nanoflann.hpp:445
nanoflann::L1_Adaptor
Definition: nanoflann.hpp:279
nanoflann::metric_L1
Definition: nanoflann.hpp:397
nanoflann::KDTreeEigenMatrixAdaptor
Definition: nanoflann.hpp:1247
nanoflann::abs< double >
double abs< double >(double x)
Definition: nanoflann.hpp:270
nanoflann
Definition: nanoflann.hpp:53
nanoflann::KDTreeSingleIndexAdaptor::Node
Definition: nanoflann.hpp:661
nanoflann::PooledAllocator::allocate
T * allocate(const size_t count=1)
Definition: nanoflann.hpp:587
nanoflann::L2_Adaptor
Definition: nanoflann.hpp:327
nanoflann::allocate
T * allocate(size_t count=1)
Definition: nanoflann.hpp:462
nanoflann::abs< long double >
long double abs< long double >(long double x)
Definition: nanoflann.hpp:271
NULL
#define NULL
Definition: mydefs.hpp:141
nanoflann::KNNResultSet::size
CountType size() const
Definition: nanoflann.hpp:114
nanoflann::L2_Adaptor::DistanceType
_DistanceType DistanceType
Definition: nanoflann.hpp:330
nanoflann::PooledAllocator::usedMemory
size_t usedMemory
Definition: nanoflann.hpp:502
nanoflann::KDTreeSingleIndexAdaptor::BranchStruct
Definition: nanoflann.hpp:704
nanoflann::KDTreeSingleIndexAdaptorParams
Definition: nanoflann.hpp:427
nanoflann::KDTreeSingleIndexAdaptor::Node::child2
Node * child2
Definition: nanoflann.hpp:686
nanoflann::KDTreeSingleIndexAdaptor
Definition: nanoflann.hpp:634
nanoflann::KNNResultSet::count
CountType count
Definition: nanoflann.hpp:99
nanoflann::KDTreeSingleIndexAdaptor::Node::child1
Node * child1
Definition: nanoflann.hpp:686
nanoflann::BLOCKSIZE
const size_t BLOCKSIZE
Definition: nanoflann.hpp:485
nanoflann::KNNResultSet::init
void init(IndexType *indices_, DistanceType *dists_)
Definition: nanoflann.hpp:106
nanoflann::KNNResultSet::KNNResultSet
KNNResultSet(CountType capacity_)
Definition: nanoflann.hpp:102
nanoflann::IndexDist_Sorter
Definition: nanoflann.hpp:209
nanoflann::SearchParams
Definition: nanoflann.hpp:438
nanoflann::abs< int >
int abs< int >(int x)
Definition: nanoflann.hpp:268
nanoflann::KDTreeEigenMatrixAdaptor::num_t
MatrixType::Scalar num_t
Definition: nanoflann.hpp:1250
nanoflann::KNNResultSet
Definition: nanoflann.hpp:94
kfusion::device::swap
__kf_hdevice__ void swap(T &a, T &b)
Definition: temp_utils.hpp:10
nanoflann::L1_Adaptor::DistanceType
_DistanceType DistanceType
Definition: nanoflann.hpp:282
nanoflann::KNNResultSet::full
bool full() const
Definition: nanoflann.hpp:119
nanoflann::PooledAllocator
Definition: nanoflann.hpp:487
nanoflann::KNNResultSet::dists
DistanceType * dists
Definition: nanoflann.hpp:97
nanoflann::KNNResultSet::capacity
CountType capacity
Definition: nanoflann.hpp:98
nanoflann::KDTreeSingleIndexAdaptor::BoundingBox
std::vector< Interval > BoundingBox
Definition: nanoflann.hpp:696
nanoflann::KNNResultSet::addPoint
void addPoint(DistanceType dist, IndexType index)
Definition: nanoflann.hpp:125
nanoflann::KDTreeSingleIndexAdaptor::ElementType
Distance::ElementType ElementType
Definition: nanoflann.hpp:637
nanoflann::KNNResultSet::indices
IndexType * indices
Definition: nanoflann.hpp:96
nanoflann::abs
T abs(T x)
Definition: nanoflann.hpp:267
nanoflann::save_value
void save_value(FILE *stream, const T &value, size_t count=1)
Definition: nanoflann.hpp:224
nanoflann::WORDSIZE
const size_t WORDSIZE
Definition: nanoflann.hpp:484

lvr2
Author(s): Thomas Wiemann , Sebastian Pütz , Alexander Mock , Lars Kiesow , Lukas Kalbertodt , Tristan Igelbrink , Johan M. von Behren , Dominik Feldschnieders , Alexander Löhr
autogenerated on Wed Mar 2 2022 00:37:24