kdtree_index.h
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  *
7  * THE BSD LICENSE
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * 1. Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * 2. Redistributions in binary form must reproduce the above copyright
16  * notice, this list of conditions and the following disclaimer in the
17  * documentation and/or other materials provided with the distribution.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
20  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
21  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
22  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
23  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
24  * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
26  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
28  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29  *************************************************************************/
30 
31 #ifndef RTABMAP_FLANN_KDTREE_INDEX_H_
32 #define RTABMAP_FLANN_KDTREE_INDEX_H_
33 
34 #include <algorithm>
35 #include <map>
36 #include <cassert>
37 #include <cstring>
38 #include <stdarg.h>
39 #include <cmath>
40 
41 #include "rtflann/general.h"
42 #include "rtflann/algorithms/nn_index.h"
43 #include "rtflann/util/dynamic_bitset.h"
44 #include "rtflann/util/matrix.h"
45 #include "rtflann/util/result_set.h"
46 #include "rtflann/util/heap.h"
47 #include "rtflann/util/allocator.h"
48 #include "rtflann/util/random.h"
49 #include "rtflann/util/saving.h"
50 
51 
52 namespace rtflann
53 {
54 
55 struct KDTreeIndexParams : public IndexParams
56 {
57  KDTreeIndexParams(int trees = 4)
58  {
59  (*this)["algorithm"] = FLANN_INDEX_KDTREE;
60  (*this)["trees"] = trees;
61  }
62 };
63 
64 
71 template <typename Distance>
72 class KDTreeIndex : public NNIndex<Distance>
73 {
74 public:
75  typedef typename Distance::ElementType ElementType;
76  typedef typename Distance::ResultType DistanceType;
77 
78  typedef NNIndex<Distance> BaseClass;
79 
80  typedef bool needs_kdtree_distance;
81 
82 private:
83  /*--------------------- Internal Data Structures --------------------------*/
84  struct Node
85  {
89  int divfeat;
93  DistanceType divval;
97  ElementType* point;
101  Node* child1, *child2;
102  Node(){
103  child1 = NULL;
104  child2 = NULL;
105  }
106  ~Node() {
107  if (child1 != NULL) { child1->~Node(); child1 = NULL; }
108 
109  if (child2 != NULL) { child2->~Node(); child2 = NULL; }
110  }
111 
112  private:
113  template<typename Archive>
114  void serialize(Archive& ar)
115  {
116  typedef KDTreeIndex<Distance> Index;
117  Index* obj = static_cast<Index*>(ar.getObject());
118 
119  ar & divfeat;
120  ar & divval;
121 
122  bool leaf_node = false;
123  if (Archive::is_saving::value) {
124  leaf_node = ((child1==NULL) && (child2==NULL));
125  }
126  ar & leaf_node;
127 
128  if (leaf_node) {
129  if (Archive::is_loading::value) {
130  point = obj->points_[divfeat];
131  }
132  }
133 
134  if (!leaf_node) {
135  if (Archive::is_loading::value) {
136  child1 = new(obj->pool_) Node();
137  child2 = new(obj->pool_) Node();
138  }
139  ar & *child1;
140  ar & *child2;
141  }
142  }
143  friend struct serialization::access;
144  };
145 
146  typedef Node* NodePtr;
147  typedef BranchStruct<NodePtr, DistanceType> BranchSt;
148  typedef BranchSt* Branch;
149 
150 public:
151 
159  KDTreeIndex(const IndexParams& params = KDTreeIndexParams(), Distance d = Distance() ) :
160  BaseClass(params, d), mean_(NULL), var_(NULL)
161  {
162  trees_ = get_param(index_params_,"trees",4);
163  }
164 
165 
173  KDTreeIndex(const Matrix<ElementType>& dataset, const IndexParams& params = KDTreeIndexParams(),
174  Distance d = Distance() ) : BaseClass(params,d ), mean_(NULL), var_(NULL)
175  {
176  trees_ = get_param(index_params_,"trees",4);
177 
178  setDataset(dataset);
179  }
180 
181  KDTreeIndex(const KDTreeIndex& other) : BaseClass(other),
182  trees_(other.trees_)
183  {
184  tree_roots_.resize(other.tree_roots_.size());
185  for (size_t i=0;i<tree_roots_.size();++i) {
186  copyTree(tree_roots_[i], other.tree_roots_[i]);
187  }
188  }
189 
190  KDTreeIndex& operator=(KDTreeIndex other)
191  {
192  this->swap(other);
193  return *this;
194  }
195 
199  virtual ~KDTreeIndex()
200  {
201  freeIndex();
202  }
203 
204  BaseClass* clone() const
205  {
206  return new KDTreeIndex(*this);
207  }
208 
209  using BaseClass::buildIndex;
210 
211  void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
212  {
213  assert(points.cols==veclen_);
214 
215  size_t old_size = size_;
216  extendDataset(points);
217 
218  if (rebuild_threshold>1 && size_at_build_*rebuild_threshold<size_) {
219  buildIndex();
220  }
221  else {
222  for (size_t i=old_size;i<size_;++i) {
223  for (int j = 0; j < trees_; j++) {
224  addPointToTree(tree_roots_[j], i);
225  }
226  }
227  }
228  }
229 
230  flann_algorithm_t getType() const
231  {
232  return FLANN_INDEX_KDTREE;
233  }
234 
235 
236  template<typename Archive>
237  void serialize(Archive& ar)
238  {
239  ar.setObject(this);
240 
241  ar & *static_cast<NNIndex<Distance>*>(this);
242 
243  ar & trees_;
244 
245  if (Archive::is_loading::value) {
246  tree_roots_.resize(trees_);
247  }
248  for (size_t i=0;i<tree_roots_.size();++i) {
249  if (Archive::is_loading::value) {
250  tree_roots_[i] = new(pool_) Node();
251  }
252  ar & *tree_roots_[i];
253  }
254 
255  if (Archive::is_loading::value) {
256  index_params_["algorithm"] = getType();
257  index_params_["trees"] = trees_;
258  }
259  }
260 
261 
262  void saveIndex(FILE* stream)
263  {
264  serialization::SaveArchive sa(stream);
265  sa & *this;
266  }
267 
268 
269  void loadIndex(FILE* stream)
270  {
271  freeIndex();
272  serialization::LoadArchive la(stream);
273  la & *this;
274  }
275 
280  int usedMemory() const
281  {
282  return int(pool_.usedMemory+pool_.wastedMemory+size_*sizeof(int)); // pool memory and vind array memory
283  }
284 
294  void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
295  {
296  int maxChecks = searchParams.checks;
297  float epsError = 1+searchParams.eps;
298 
299  if (maxChecks==FLANN_CHECKS_UNLIMITED) {
300  if (removed_) {
301  getExactNeighbors<true>(result, vec, epsError);
302  }
303  else {
304  getExactNeighbors<false>(result, vec, epsError);
305  }
306  }
307  else {
308  if (removed_) {
309  getNeighbors<true>(result, vec, maxChecks, epsError);
310  }
311  else {
312  getNeighbors<false>(result, vec, maxChecks, epsError);
313  }
314  }
315  }
316 
317 #ifdef FLANN_KDTREE_MEM_OPT
318 
328  void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams, Heap<BranchSt>* heap) const
329  {
330  int maxChecks = searchParams.checks;
331  float epsError = 1+searchParams.eps;
332 
333  if (maxChecks==FLANN_CHECKS_UNLIMITED) {
334  if (removed_) {
335  getExactNeighbors<true>(result, vec, epsError);
336  }
337  else {
338  getExactNeighbors<false>(result, vec, epsError);
339  }
340  }
341  else {
342  if (removed_) {
343  getNeighbors<true>(result, vec, maxChecks, epsError, heap);
344  }
345  else {
346  getNeighbors<false>(result, vec, maxChecks, epsError, heap);
347  }
348  }
349  }
350 
359  virtual int knnSearch(const Matrix<ElementType>& queries,
360  Matrix<size_t>& indices,
361  Matrix<DistanceType>& dists,
362  size_t knn,
363  const SearchParams& params) const
364  {
365  assert(queries.cols == veclen_);
366  assert(indices.rows >= queries.rows);
367  assert(dists.rows >= queries.rows);
368  assert(indices.cols >= knn);
369  assert(dists.cols >= knn);
370  bool use_heap;
371 
372  if (params.use_heap==FLANN_Undefined) {
373  use_heap = (knn>KNN_HEAP_THRESHOLD)?true:false;
374  }
375  else {
376  use_heap = (params.use_heap==FLANN_True)?true:false;
377  }
378  int count = 0;
379 
380  Heap<BranchSt>* heap = new Heap<BranchSt>((int)size_);
381 
382  if (use_heap) {
383  //#pragma omp parallel num_threads(params.cores)
384  {
385  KNNResultSet2<DistanceType> resultSet(knn);
386  //#pragma omp for schedule(static) reduction(+:count)
387  for (int i = 0; i < (int)queries.rows; i++) {
388  resultSet.clear();
389  findNeighbors(resultSet, queries[i], params, heap);
390  size_t n = std::min(resultSet.size(), knn);
391  resultSet.copy(indices[i], dists[i], n, params.sorted);
392  indices_to_ids(indices[i], indices[i], n);
393  count += n;
394  }
395  }
396  }
397  else {
398  std::vector<double> times(queries.rows);
399  //#pragma omp parallel num_threads(params.cores)
400  {
401  KNNSimpleResultSet<DistanceType> resultSet(knn);
402  //#pragma omp for schedule(static) reduction(+:count)
403  for (int i = 0; i < (int)queries.rows; i++) {
404  resultSet.clear();
405  findNeighbors(resultSet, queries[i], params, heap);
406  size_t n = std::min(resultSet.size(), knn);
407  resultSet.copy(indices[i], dists[i], n, params.sorted);
408  indices_to_ids(indices[i], indices[i], n);
409  count += n;
410  }
411  }
412  std::sort(times.begin(), times.end());
413  }
414  delete heap;
415  return count;
416  }
417 
418 
427  virtual int knnSearch(const Matrix<ElementType>& queries,
428  std::vector< std::vector<size_t> >& indices,
429  std::vector<std::vector<DistanceType> >& dists,
430  size_t knn,
431  const SearchParams& params) const
432  {
433  assert(queries.cols == veclen_);
434  bool use_heap;
435  if (params.use_heap==FLANN_Undefined) {
436  use_heap = (knn>KNN_HEAP_THRESHOLD)?true:false;
437  }
438  else {
439  use_heap = (params.use_heap==FLANN_True)?true:false;
440  }
441 
442  if (indices.size() < queries.rows ) indices.resize(queries.rows);
443  if (dists.size() < queries.rows ) dists.resize(queries.rows);
444 
445  Heap<BranchSt>* heap = new Heap<BranchSt>((int)size_);
446 
447  int count = 0;
448  if (use_heap) {
449  //#pragma omp parallel num_threads(params.cores)
450  {
451  KNNResultSet2<DistanceType> resultSet(knn);
452  //#pragma omp for schedule(static) reduction(+:count)
453  for (int i = 0; i < (int)queries.rows; i++) {
454  resultSet.clear();
455  findNeighbors(resultSet, queries[i], params, heap);
456  size_t n = std::min(resultSet.size(), knn);
457  indices[i].resize(n);
458  dists[i].resize(n);
459  if (n>0) {
460  resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
461  indices_to_ids(&indices[i][0], &indices[i][0], n);
462  }
463  count += n;
464  }
465  }
466  }
467  else {
468  //#pragma omp parallel num_threads(params.cores)
469  {
470  KNNSimpleResultSet<DistanceType> resultSet(knn);
471  //#pragma omp for schedule(static) reduction(+:count)
472  for (int i = 0; i < (int)queries.rows; i++) {
473  resultSet.clear();
474  findNeighbors(resultSet, queries[i], params, heap);
475  size_t n = std::min(resultSet.size(), knn);
476  indices[i].resize(n);
477  dists[i].resize(n);
478  if (n>0) {
479  resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
480  indices_to_ids(&indices[i][0], &indices[i][0], n);
481  }
482  count += n;
483  }
484  }
485  }
486  delete heap;
487 
488  return count;
489  }
490 
500  virtual int radiusSearch(const Matrix<ElementType>& queries,
501  Matrix<size_t>& indices,
502  Matrix<DistanceType>& dists,
503  float radius,
504  const SearchParams& params) const
505  {
506  assert(queries.cols == veclen);
507  int count = 0;
508  size_t num_neighbors = std::min(indices.cols, dists.cols);
509  int max_neighbors = params.max_neighbors;
510  if (max_neighbors<0) max_neighbors = num_neighbors;
511  else max_neighbors = std::min(max_neighbors,(int)num_neighbors);
512 
513  Heap<BranchSt>* heap = new Heap<BranchSt>((int)size_);
514 
515  if (max_neighbors==0) {
516  //#pragma omp parallel num_threads(params.cores)
517  {
518  CountRadiusResultSet<DistanceType> resultSet(radius);
519  //#pragma omp for schedule(static) reduction(+:count)
520  for (int i = 0; i < (int)queries.rows; i++) {
521  resultSet.clear();
522  findNeighbors(resultSet, queries[i], params, heap);
523  count += resultSet.size();
524  }
525  }
526  }
527  else {
528  // explicitly indicated to use unbounded radius result set
529  // and we know there'll be enough room for resulting indices and dists
530  if (params.max_neighbors<0 && (num_neighbors>=this->size())) {
531  //#pragma omp parallel num_threads(params.cores)
532  {
533  RadiusResultSet<DistanceType> resultSet(radius);
534  //#pragma omp for schedule(static) reduction(+:count)
535  for (int i = 0; i < (int)queries.rows; i++) {
536  resultSet.clear();
537  findNeighbors(resultSet, queries[i], params, heap);
538  size_t n = resultSet.size();
539  count += n;
540  if (n>num_neighbors) n = num_neighbors;
541  resultSet.copy(indices[i], dists[i], n, params.sorted);
542 
543  // mark the next element in the output buffers as unused
544  if (n<indices.cols) indices[i][n] = size_t(-1);
545  if (n<dists.cols) dists[i][n] = std::numeric_limits<DistanceType>::infinity();
546  indices_to_ids(indices[i], indices[i], n);
547  }
548  }
549  }
550  else {
551  // number of neighbors limited to max_neighbors
552  //#pragma omp parallel num_threads(params.cores)
553  {
554  KNNRadiusResultSet<DistanceType> resultSet(radius, max_neighbors);
555  //#pragma omp for schedule(static) reduction(+:count)
556  for (int i = 0; i < (int)queries.rows; i++) {
557  resultSet.clear();
558  findNeighbors(resultSet, queries[i], params, heap);
559  size_t n = resultSet.size();
560  count += n;
561  if ((int)n>max_neighbors) n = max_neighbors;
562  resultSet.copy(indices[i], dists[i], n, params.sorted);
563 
564  // mark the next element in the output buffers as unused
565  if (n<indices.cols) indices[i][n] = size_t(-1);
566  if (n<dists.cols) dists[i][n] = std::numeric_limits<DistanceType>::infinity();
567  indices_to_ids(indices[i], indices[i], n);
568  }
569  }
570  }
571  }
572  delete heap;
573  return count;
574  }
575 
585  virtual int radiusSearch(const Matrix<ElementType>& queries,
586  std::vector< std::vector<size_t> >& indices,
587  std::vector<std::vector<DistanceType> >& dists,
588  float radius,
589  const SearchParams& params) const
590  {
591  assert(queries.cols == veclen_);
592  int count = 0;
593 
594  Heap<BranchSt>* heap = new Heap<BranchSt>((int)size_);
595 
596  // just count neighbors
597  if (params.max_neighbors==0) {
598  //#pragma omp parallel num_threads(params.cores)
599  {
600  CountRadiusResultSet<DistanceType> resultSet(radius);
601  //#pragma omp for schedule(static) reduction(+:count)
602  for (int i = 0; i < (int)queries.rows; i++) {
603  resultSet.clear();
604  findNeighbors(resultSet, queries[i], params, heap);
605  count += resultSet.size();
606  }
607  }
608  }
609  else {
610  if (indices.size() < queries.rows ) indices.resize(queries.rows);
611  if (dists.size() < queries.rows ) dists.resize(queries.rows);
612 
613  if (params.max_neighbors<0) {
614  // search for all neighbors
615  //#pragma omp parallel num_threads(params.cores)
616  {
617  RadiusResultSet<DistanceType> resultSet(radius);
618  //#pragma omp for schedule(static) reduction(+:count)
619  for (int i = 0; i < (int)queries.rows; i++) {
620  resultSet.clear();
621  findNeighbors(resultSet, queries[i], params, heap);
622  size_t n = resultSet.size();
623  count += n;
624  indices[i].resize(n);
625  dists[i].resize(n);
626  if (n > 0) {
627  resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
628  indices_to_ids(&indices[i][0], &indices[i][0], n);
629  }
630  }
631  }
632  }
633  else {
634  // number of neighbors limited to max_neighbors
635  //#pragma omp parallel num_threads(params.cores)
636  {
637  KNNRadiusResultSet<DistanceType> resultSet(radius, params.max_neighbors);
638  //#pragma omp for schedule(static) reduction(+:count)
639  for (int i = 0; i < (int)queries.rows; i++) {
640  resultSet.clear();
641  findNeighbors(resultSet, queries[i], params, heap);
642  size_t n = resultSet.size();
643  count += n;
644  if ((int)n>params.max_neighbors) n = params.max_neighbors;
645  indices[i].resize(n);
646  dists[i].resize(n);
647  if (n > 0) {
648  resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
649  indices_to_ids(&indices[i][0], &indices[i][0], n);
650  }
651  }
652  }
653  }
654  }
655  delete heap;
656  return count;
657  }
658 #endif
659 
660 protected:
661 
665  void buildIndexImpl()
666  {
667  // Create a permutable array of indices to the input vectors.
668  std::vector<int> ind(size_);
669  for (size_t i = 0; i < size_; ++i) {
670  ind[i] = int(i);
671  }
672 
673  mean_ = new DistanceType[veclen_];
674  var_ = new DistanceType[veclen_];
675 
676  tree_roots_.resize(trees_);
677  /* Construct the randomized trees. */
678  for (int i = 0; i < trees_; i++) {
679  /* Randomize the order of vectors to allow for unbiased sampling. */
680  std::random_shuffle(ind.begin(), ind.end());
681  tree_roots_[i] = divideTree(&ind[0], int(size_) );
682  }
683  delete[] mean_;
684  delete[] var_;
685  }
686 
687  void freeIndex()
688  {
689  for (size_t i=0;i<tree_roots_.size();++i) {
690  // using placement new, so call destructor explicitly
691  if (tree_roots_[i]!=NULL) tree_roots_[i]->~Node();
692  }
693  pool_.free();
694  }
695 
696 
697 private:
698 
699  void copyTree(NodePtr& dst, const NodePtr& src)
700  {
701  dst = new(pool_) Node();
702  dst->divfeat = src->divfeat;
703  dst->divval = src->divval;
704  if (src->child1==NULL && src->child2==NULL) {
705  dst->point = points_[dst->divfeat];
706  dst->child1 = NULL;
707  dst->child2 = NULL;
708  }
709  else {
710  copyTree(dst->child1, src->child1);
711  copyTree(dst->child2, src->child2);
712  }
713  }
714 
724  NodePtr divideTree(int* ind, int count)
725  {
726  NodePtr node = new(pool_) Node(); // allocate memory
727 
728  /* If too few exemplars remain, then make this a leaf node. */
729  if (count == 1) {
730  node->child1 = node->child2 = NULL; /* Mark as leaf node. */
731  node->divfeat = *ind; /* Store index of this vec. */
732  node->point = points_[*ind];
733  }
734  else {
735  int idx;
736  int cutfeat;
737  DistanceType cutval;
738  meanSplit(ind, count, idx, cutfeat, cutval);
739 
740  node->divfeat = cutfeat;
741  node->divval = cutval;
742  node->child1 = divideTree(ind, idx);
743  node->child2 = divideTree(ind+idx, count-idx);
744  }
745 
746  return node;
747  }
748 
749 
755  void meanSplit(int* ind, int count, int& index, int& cutfeat, DistanceType& cutval)
756  {
757  memset(mean_,0,veclen_*sizeof(DistanceType));
758  memset(var_,0,veclen_*sizeof(DistanceType));
759 
760  /* Compute mean values. Only the first SAMPLE_MEAN values need to be
761  sampled to get a good estimate.
762  */
763  int cnt = std::min((int)SAMPLE_MEAN+1, count);
764  for (int j = 0; j < cnt; ++j) {
765  ElementType* v = points_[ind[j]];
766  for (size_t k=0; k<veclen_; ++k) {
767  mean_[k] += v[k];
768  }
769  }
770  DistanceType div_factor = DistanceType(1)/cnt;
771  for (size_t k=0; k<veclen_; ++k) {
772  mean_[k] *= div_factor;
773  }
774 
775  /* Compute variances (no need to divide by count). */
776  for (int j = 0; j < cnt; ++j) {
777  ElementType* v = points_[ind[j]];
778  for (size_t k=0; k<veclen_; ++k) {
779  DistanceType dist = v[k] - mean_[k];
780  var_[k] += dist * dist;
781  }
782  }
783  /* Select one of the highest variance indices at random. */
784  cutfeat = selectDivision(var_);
785  cutval = mean_[cutfeat];
786 
787  int lim1, lim2;
788  planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
789 
790  if (lim1>count/2) index = lim1;
791  else if (lim2<count/2) index = lim2;
792  else index = count/2;
793 
794  /* If either list is empty, it means that all remaining features
795  * are identical. Split in the middle to maintain a balanced tree.
796  */
797  if ((lim1==count)||(lim2==0)) index = count/2;
798  }
799 
800 
805  int selectDivision(DistanceType* v)
806  {
807  int num = 0;
808  size_t topind[RAND_DIM];
809 
810  /* Create a list of the indices of the top RAND_DIM values. */
811  for (size_t i = 0; i < veclen_; ++i) {
812  if ((num < RAND_DIM)||(v[i] > v[topind[num-1]])) {
813  /* Put this element at end of topind. */
814  if (num < RAND_DIM) {
815  topind[num++] = i; /* Add to list. */
816  }
817  else {
818  topind[num-1] = i; /* Replace last element. */
819  }
820  /* Bubble end value down to right location by repeated swapping. */
821  int j = num - 1;
822  while (j > 0 && v[topind[j]] > v[topind[j-1]]) {
823  std::swap(topind[j], topind[j-1]);
824  --j;
825  }
826  }
827  }
828  /* Select a random integer in range [0,num-1], and return that index. */
829  int rnd = rand_int(num);
830  return (int)topind[rnd];
831  }
832 
833 
843  void planeSplit(int* ind, int count, int cutfeat, DistanceType cutval, int& lim1, int& lim2)
844  {
845  /* Move vector indices for left subtree to front of list. */
846  int left = 0;
847  int right = count-1;
848  for (;; ) {
849  while (left<=right && points_[ind[left]][cutfeat]<cutval) ++left;
850  while (left<=right && points_[ind[right]][cutfeat]>=cutval) --right;
851  if (left>right) break;
852  std::swap(ind[left], ind[right]); ++left; --right;
853  }
854  lim1 = left;
855  right = count-1;
856  for (;; ) {
857  while (left<=right && points_[ind[left]][cutfeat]<=cutval) ++left;
858  while (left<=right && points_[ind[right]][cutfeat]>cutval) --right;
859  if (left>right) break;
860  std::swap(ind[left], ind[right]); ++left; --right;
861  }
862  lim2 = left;
863  }
864 
869  template<bool with_removed>
870  void getExactNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, float epsError) const
871  {
872  // checkID -= 1; /* Set a different unique ID for each search. */
873 
874  if (trees_ > 1) {
875  fprintf(stderr,"It doesn't make any sense to use more than one tree for exact search");
876  }
877  if (trees_>0) {
878  searchLevelExact<with_removed>(result, vec, tree_roots_[0], 0.0, epsError);
879  }
880  }
881 
887  template<bool with_removed>
888  void getNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, int maxCheck, float epsError) const
889  {
890  int i;
891  BranchSt branch;
892 
893  int checkCount = 0;
894  Heap<BranchSt>* heap = new Heap<BranchSt>((int)size_);
895  DynamicBitset checked(size_);
896 
897  /* Search once through each tree down to root. */
898  for (i = 0; i < trees_; ++i) {
899  searchLevel<with_removed>(result, vec, tree_roots_[i], 0, checkCount, maxCheck, epsError, heap, checked);
900  }
901 
902  /* Keep searching other branches from heap until finished. */
903  while ( heap->popMin(branch) && (checkCount < maxCheck || !result.full() )) {
904  searchLevel<with_removed>(result, vec, branch.node, branch.mindist, checkCount, maxCheck, epsError, heap, checked);
905  }
906 
907  delete heap;
908 
909  }
910 
911 #ifdef FLANN_KDTREE_MEM_OPT
912 
917  template<bool with_removed>
918  void getNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, int maxCheck, float epsError, Heap<BranchSt>* heap) const
919  {
920  int i;
921  BranchSt branch;
922 
923  int checkCount = 0;
924  DynamicBitset checked(size_);
925  heap->clear();
926 
927  /* Search once through each tree down to root. */
928  for (i = 0; i < trees_; ++i) {
929  searchLevel<with_removed>(result, vec, tree_roots_[i], 0, checkCount, maxCheck, epsError, heap, checked);
930  }
931 
932  /* Keep searching other branches from heap until finished. */
933  while ( heap->popMin(branch) && (checkCount < maxCheck || !result.full() )) {
934  searchLevel<with_removed>(result, vec, branch.node, branch.mindist, checkCount, maxCheck, epsError, heap, checked);
935  }
936  }
937 #endif
938 
939 
945  template<bool with_removed>
946  void searchLevel(ResultSet<DistanceType>& result_set, const ElementType* vec, NodePtr node, DistanceType mindist, int& checkCount, int maxCheck,
947  float epsError, Heap<BranchSt>* heap, DynamicBitset& checked) const
948  {
949  if (result_set.worstDist()<mindist) {
950  // printf("Ignoring branch, too far\n");
951  return;
952  }
953 
954  /* If this is a leaf node, then do check and return. */
955  if ((node->child1 == NULL)&&(node->child2 == NULL)) {
956  int index = node->divfeat;
957  if (with_removed) {
958  if (removed_points_.test(index)) return;
959  }
960  /* Do not check same node more than once when searching multiple trees. */
961  if ( checked.test(index) || ((checkCount>=maxCheck)&& result_set.full()) ) return;
962  checked.set(index);
963  checkCount++;
964 
965  DistanceType dist = distance_(node->point, vec, veclen_);
966  result_set.addPoint(dist,index);
967  return;
968  }
969 
970  /* Which child branch should be taken first? */
971  ElementType val = vec[node->divfeat];
972  DistanceType diff = val - node->divval;
973  NodePtr bestChild = (diff < 0) ? node->child1 : node->child2;
974  NodePtr otherChild = (diff < 0) ? node->child2 : node->child1;
975 
976  /* Create a branch record for the branch not taken. Add distance
977  of this feature boundary (we don't attempt to correct for any
978  use of this feature in a parent node, which is unlikely to
979  happen and would have only a small effect). Don't bother
980  adding more branches to heap after halfway point, as cost of
981  adding exceeds their value.
982  */
983 
984  DistanceType new_distsq = mindist + distance_.accum_dist(val, node->divval, node->divfeat);
985  // if (2 * checkCount < maxCheck || !result.full()) {
986  if ((new_distsq*epsError < result_set.worstDist())|| !result_set.full()) {
987  heap->insert( BranchSt(otherChild, new_distsq) );
988  }
989 
990  /* Call recursively to search next level down. */
991  searchLevel<with_removed>(result_set, vec, bestChild, mindist, checkCount, maxCheck, epsError, heap, checked);
992  }
993 
997  template<bool with_removed>
998  void searchLevelExact(ResultSet<DistanceType>& result_set, const ElementType* vec, const NodePtr node, DistanceType mindist, const float epsError) const
999  {
1000  /* If this is a leaf node, then do check and return. */
1001  if ((node->child1 == NULL)&&(node->child2 == NULL)) {
1002  int index = node->divfeat;
1003  if (with_removed) {
1004  if (removed_points_.test(index)) return; // ignore removed points
1005  }
1006  DistanceType dist = distance_(node->point, vec, veclen_);
1007  result_set.addPoint(dist,index);
1008 
1009  return;
1010  }
1011 
1012  /* Which child branch should be taken first? */
1013  ElementType val = vec[node->divfeat];
1014  DistanceType diff = val - node->divval;
1015  NodePtr bestChild = (diff < 0) ? node->child1 : node->child2;
1016  NodePtr otherChild = (diff < 0) ? node->child2 : node->child1;
1017 
1018  /* Create a branch record for the branch not taken. Add distance
1019  of this feature boundary (we don't attempt to correct for any
1020  use of this feature in a parent node, which is unlikely to
1021  happen and would have only a small effect). Don't bother
1022  adding more branches to heap after halfway point, as cost of
1023  adding exceeds their value.
1024  */
1025 
1026  DistanceType new_distsq = mindist + distance_.accum_dist(val, node->divval, node->divfeat);
1027 
1028  /* Call recursively to search next level down. */
1029  searchLevelExact<with_removed>(result_set, vec, bestChild, mindist, epsError);
1030 
1031  if (mindist*epsError<=result_set.worstDist()) {
1032  searchLevelExact<with_removed>(result_set, vec, otherChild, new_distsq, epsError);
1033  }
1034  }
1035 
1036  void addPointToTree(NodePtr node, int ind)
1037  {
1038  ElementType* point = points_[ind];
1039 
1040  if ((node->child1==NULL) && (node->child2==NULL)) {
1041  ElementType* leaf_point = node->point;
1042  ElementType max_span = 0;
1043  size_t div_feat = 0;
1044  for (size_t i=0;i<veclen_;++i) {
1045  ElementType span = std::abs(point[i]-leaf_point[i]);
1046  if (span > max_span) {
1047  max_span = span;
1048  div_feat = i;
1049  }
1050  }
1051  NodePtr left = new(pool_) Node();
1052  left->child1 = left->child2 = NULL;
1053  NodePtr right = new(pool_) Node();
1054  right->child1 = right->child2 = NULL;
1055 
1056  if (point[div_feat]<leaf_point[div_feat]) {
1057  left->divfeat = ind;
1058  left->point = point;
1059  right->divfeat = node->divfeat;
1060  right->point = node->point;
1061  }
1062  else {
1063  left->divfeat = node->divfeat;
1064  left->point = node->point;
1065  right->divfeat = ind;
1066  right->point = point;
1067  }
1068  node->divfeat = div_feat;
1069  node->divval = (point[div_feat]+leaf_point[div_feat])/2;
1070  node->child1 = left;
1071  node->child2 = right;
1072  }
1073  else {
1074  if (point[node->divfeat]<node->divval) {
1075  addPointToTree(node->child1,ind);
1076  }
1077  else {
1078  addPointToTree(node->child2,ind);
1079  }
1080  }
1081  }
1082 private:
1083  void swap(KDTreeIndex& other)
1084  {
1085  BaseClass::swap(other);
1086  std::swap(trees_, other.trees_);
1087  std::swap(tree_roots_, other.tree_roots_);
1088  std::swap(pool_, other.pool_);
1089  }
1090 
1091 private:
1092 
1093  enum
1094  {
1100  SAMPLE_MEAN = 100,
1108  RAND_DIM=5
1109  };
1110 
1111 
1115  int trees_;
1116 
1117  DistanceType* mean_;
1118  DistanceType* var_;
1119 
1123  std::vector<NodePtr> tree_roots_;
1124 
1132  PooledAllocator pool_;
1133 
1134  USING_BASECLASS_SYMBOLS
1135 }; // class KDTreeIndex
1136 
1137 }
1138 
1139 #endif //FLANN_KDTREE_INDEX_H_
d
d
NULL
#define NULL
rtflann::IndexParams
std::map< std::string, any > IndexParams
Definition: params.h:51
rtflann::KDTreeIndex::clone
BaseClass * clone() const
Definition: kdtree_index.h:204
rtflann::KDTreeIndex::addPoints
void addPoints(const Matrix< ElementType > &points, float rebuild_threshold=2)
Incrementally add points to the index.
Definition: kdtree_index.h:211
rtflann::KNNResultSet2::copy
void copy(size_t *indices, DistanceType *dists, size_t num_elements, bool sorted=true)
Definition: result_set.h:388
rtflann::KNNRadiusResultSet::copy
void copy(size_t *indices, DistanceType *dists, size_t num_elements, bool sorted=true)
Definition: result_set.h:612
rtflann::get_param
T get_param(const IndexParams &params, std::string name, const T &default_value)
Definition: params.h:95
rtflann::KDTreeIndex::pool_
PooledAllocator pool_
Definition: kdtree_index.h:1132
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtflann::KDTreeIndex::usedMemory
int usedMemory() const
Definition: kdtree_index.h:280
rtflann::KDTreeIndex::loadIndex
void loadIndex(FILE *stream)
Definition: kdtree_index.h:269
knn
int knn
rtflann::KDTreeIndexParams::KDTreeIndexParams
KDTreeIndexParams(int trees=4)
Definition: kdtree_index.h:57
rtflann::KDTreeIndex::BaseClass
NNIndex< Distance > BaseClass
Definition: kdtree_index.h:78
rtflann::KDTreeIndex::mean_
DistanceType * mean_
Definition: kdtree_index.h:1117
rtflann::KDTreeIndex::findNeighbors
void findNeighbors(ResultSet< DistanceType > &result, const ElementType *vec, const SearchParams &searchParams) const
Definition: kdtree_index.h:294
dynamic_bitset.h
rtflann::KDTreeIndex::getNeighbors
void getNeighbors(ResultSet< DistanceType > &result, const ElementType *vec, int maxCheck, float epsError) const
Definition: kdtree_index.h:888
rtflann::RadiusResultSet::size
size_t size() const
Definition: result_set.h:455
rtflann::KDTreeIndex::operator=
KDTreeIndex & operator=(KDTreeIndex other)
Definition: kdtree_index.h:190
rtflann::KDTreeIndex::KDTreeIndex
KDTreeIndex(const KDTreeIndex &other)
Definition: kdtree_index.h:181
rtflann::KDTreeIndex::swap
void swap(KDTreeIndex &other)
Definition: kdtree_index.h:1083
dst
char * dst
Definition: lz4.h:354
swap
void swap(linb::any &lhs, linb::any &rhs) noexcept
rtflann::Matrix_::rows
size_t rows
Definition: matrix.h:72
rtflann::KDTreeIndex::var_
DistanceType * var_
Definition: kdtree_index.h:1118
rtflann::rand_int
int rand_int(int high=RAND_MAX, int low=0)
Definition: random.h:73
rtflann::Heap::insert
void insert(const T &value)
Definition: heap.h:135
USING_BASECLASS_SYMBOLS
#define USING_BASECLASS_SYMBOLS
Definition: nn_index.h:897
rtflann::KDTreeIndex::tree_roots_
std::vector< NodePtr > tree_roots_
Definition: kdtree_index.h:1123
rtflann::KDTreeIndex::copyTree
void copyTree(NodePtr &dst, const NodePtr &src)
Definition: kdtree_index.h:699
rtflann::KDTreeIndex::BranchSt
BranchStruct< NodePtr, DistanceType > BranchSt
Definition: kdtree_index.h:147
result_set.h
rtflann::KDTreeIndex::getExactNeighbors
void getExactNeighbors(ResultSet< DistanceType > &result, const ElementType *vec, float epsError) const
Definition: kdtree_index.h:870
rtflann::DynamicBitset::set
void set(size_t index)
Definition: dynamic_bitset.h:123
rtflann::KDTreeIndex::getType
flann_algorithm_t getType() const
Definition: kdtree_index.h:230
rtflann::Matrix_::cols
size_t cols
Definition: matrix.h:73
rtflann::DynamicBitset::test
bool test(size_t index) const
Definition: dynamic_bitset.h:139
rtflann::flann_algorithm_t
flann_algorithm_t
Definition: defines.h:79
rtflann::Heap::popMin
bool popMin(T &value)
Definition: heap.h:158
glm::abs
GLM_FUNC_DECL genType abs(genType const &x)
rtflann::KDTreeIndex::searchLevel
void searchLevel(ResultSet< DistanceType > &result_set, const ElementType *vec, NodePtr node, DistanceType mindist, int &checkCount, int maxCheck, float epsError, Heap< BranchSt > *heap, DynamicBitset &checked) const
Definition: kdtree_index.h:946
rtflann::KDTreeIndex::KDTreeIndex
KDTreeIndex(const Matrix< ElementType > &dataset, const IndexParams &params=KDTreeIndexParams(), Distance d=Distance())
Definition: kdtree_index.h:173
rtflann::KDTreeIndex::Node::serialize
void serialize(Archive &ar)
Definition: kdtree_index.h:114
KNN_HEAP_THRESHOLD
#define KNN_HEAP_THRESHOLD
Definition: nn_index.h:46
rtflann::SearchParams::use_heap
tri_type use_heap
Definition: params.h:81
Index
struct Index Index
Definition: sqlite3.c:8577
rtflann::KDTreeIndex::planeSplit
void planeSplit(int *ind, int count, int cutfeat, DistanceType cutval, int &lim1, int &lim2)
Definition: kdtree_index.h:843
rtflann::KDTreeIndex::searchLevelExact
void searchLevelExact(ResultSet< DistanceType > &result_set, const ElementType *vec, const NodePtr node, DistanceType mindist, const float epsError) const
Definition: kdtree_index.h:998
params
params
rtflann::ResultSet::worstDist
virtual DistanceType worstDist() const =0
rtflann::RadiusResultSet::copy
void copy(size_t *indices, DistanceType *dists, size_t num_elements, bool sorted=true)
Definition: result_set.h:490
rtflann::KDTreeIndex::selectDivision
int selectDivision(DistanceType *v)
Definition: kdtree_index.h:805
rtflann::KDTreeIndex::DistanceType
Distance::ResultType DistanceType
Definition: kdtree_index.h:76
rtflann::ResultSet::full
virtual bool full() const =0
rtflann::Heap::clear
void clear()
Definition: heap.h:112
rtflann::KNNResultSet2::size
size_t size() const
Definition: result_set.h:335
dist
dist
freeIndex
static void freeIndex(sqlite3 *db, Index *p)
Definition: sqlite3.c:84627
rtflann::KDTreeIndex::addPointToTree
void addPointToTree(NodePtr node, int ind)
Definition: kdtree_index.h:1036
rtflann::KDTreeIndex::serialize
void serialize(Archive &ar)
Definition: kdtree_index.h:237
diff
diff
rtflann::KDTreeIndex::meanSplit
void meanSplit(int *ind, int count, int &index, int &cutfeat, DistanceType &cutval)
Definition: kdtree_index.h:755
rtflann::KNNSimpleResultSet::copy
void copy(size_t *indices, DistanceType *dists, size_t num_elements, bool sorted=true)
Definition: result_set.h:180
rtflann::BranchStruct::mindist
DistanceType mindist
Definition: result_set.h:53
rtflann::KDTreeIndex::divideTree
NodePtr divideTree(int *ind, int count)
Definition: kdtree_index.h:724
rtflann::KDTreeIndex::KDTreeIndex
KDTreeIndex(const IndexParams &params=KDTreeIndexParams(), Distance d=Distance())
Definition: kdtree_index.h:159
rtflann::ResultSet::addPoint
virtual void addPoint(DistanceType dist, size_t index)=0
nn_index.h
rtflann::KDTreeIndex::saveIndex
void saveIndex(FILE *stream)
Definition: kdtree_index.h:262
rtflann::KDTreeIndex::ElementType
Distance::ElementType ElementType
Definition: kdtree_index.h:75

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:28