kdtree_single_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_SINGLE_INDEX_H_
32 #define RTABMAP_FLANN_KDTREE_SINGLE_INDEX_H_
33 
34 #include <algorithm>
35 #include <map>
36 #include <cassert>
37 #include <cstring>
38 
39 #include "rtflann/general.h"
41 #include "rtflann/util/matrix.h"
43 #include "rtflann/util/heap.h"
44 #include "rtflann/util/allocator.h"
45 #include "rtflann/util/random.h"
46 #include "rtflann/util/saving.h"
47 
48 namespace rtflann
49 {
50 
52 {
53  KDTreeSingleIndexParams(int leaf_max_size = 10, bool reorder = true)
54  {
55  (*this)["algorithm"] = FLANN_INDEX_KDTREE_SINGLE;
56  (*this)["leaf_max_size"] = leaf_max_size;
57  (*this)["reorder"] = reorder;
58  }
59 };
60 
61 
68 template <typename Distance>
69 class KDTreeSingleIndex : public NNIndex<Distance>
70 {
71 public:
72  typedef typename Distance::ElementType ElementType;
73  typedef typename Distance::ResultType DistanceType;
74 
76 
77  typedef bool needs_kdtree_distance;
78 
85  KDTreeSingleIndex(const IndexParams& params = KDTreeSingleIndexParams(), Distance d = Distance() ) :
86  BaseClass(params, d), root_node_(NULL)
87  {
88  leaf_max_size_ = get_param(params,"leaf_max_size",10);
89  reorder_ = get_param(params, "reorder", true);
90  }
91 
100  Distance d = Distance() ) : BaseClass(params, d), root_node_(NULL)
101  {
102  leaf_max_size_ = get_param(params,"leaf_max_size",10);
103  reorder_ = get_param(params, "reorder", true);
104 
105  setDataset(inputData);
106  }
107 
108 
109  KDTreeSingleIndex(const KDTreeSingleIndex& other) : BaseClass(other),
110  leaf_max_size_(other.leaf_max_size_),
111  reorder_(other.reorder_),
112  vind_(other.vind_),
113  root_bbox_(other.root_bbox_)
114  {
115  if (reorder_) {
116  data_ = rtflann::Matrix<ElementType>(new ElementType[size_*veclen_], size_, veclen_);
117  std::copy(other.data_[0], other.data_[0]+size_*veclen_, data_[0]);
118  }
119  copyTree(root_node_, other.root_node_);
120  }
121 
123  {
124  this->swap(other);
125  return *this;
126  }
127 
132  {
133  freeIndex();
134  }
135 
136  BaseClass* clone() const
137  {
138  return new KDTreeSingleIndex(*this);
139  }
140 
141  using BaseClass::buildIndex;
142 
143  void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
144  {
145  assert(points.cols==veclen_);
146  extendDataset(points);
147  buildIndex();
148  }
149 
151  {
153  }
154 
155 
156  template<typename Archive>
157  void serialize(Archive& ar)
158  {
159  ar.setObject(this);
160 
161  if (reorder_) index_params_["save_dataset"] = false;
162 
163  ar & *static_cast<NNIndex<Distance>*>(this);
164 
165  ar & reorder_;
166  ar & leaf_max_size_;
167  ar & root_bbox_;
168  ar & vind_;
169 
170  if (reorder_) {
171  ar & data_;
172  }
173 
174  if (Archive::is_loading::value) {
175  root_node_ = new(pool_) Node();
176  }
177 
178  ar & *root_node_;
179 
180  if (Archive::is_loading::value) {
181  index_params_["algorithm"] = getType();
182  index_params_["leaf_max_size"] = leaf_max_size_;
183  index_params_["reorder"] = reorder_;
184  }
185  }
186 
187 
188  void saveIndex(FILE* stream)
189  {
190  serialization::SaveArchive sa(stream);
191  sa & *this;
192  }
193 
194 
195  void loadIndex(FILE* stream)
196  {
197  freeIndex();
198  serialization::LoadArchive la(stream);
199  la & *this;
200  }
201 
206  int usedMemory() const
207  {
208  return pool_.usedMemory+pool_.wastedMemory+size_*sizeof(int); // pool memory and vind array memory
209  }
210 
220  void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
221  {
222  float epsError = 1+searchParams.eps;
223 
224  std::vector<DistanceType> dists(veclen_,0);
225  DistanceType distsq = computeInitialDistances(vec, dists);
226  if (removed_) {
227  searchLevel<true>(result, vec, root_node_, distsq, dists, epsError);
228  }
229  else {
230  searchLevel<false>(result, vec, root_node_, distsq, dists, epsError);
231  }
232  }
233 
234 protected:
235 
240  {
241  // Create a permutable array of indices to the input vectors.
242  vind_.resize(size_);
243  for (size_t i = 0; i < size_; i++) {
244  vind_[i] = i;
245  }
246 
247  computeBoundingBox(root_bbox_);
248  root_node_ = divideTree(0, size_, root_bbox_ ); // construct the tree
249 
250  if (reorder_) {
251  data_ = rtflann::Matrix<ElementType>(new ElementType[size_*veclen_], size_, veclen_);
252  for (size_t i=0; i<size_; ++i) {
253  std::copy(points_[vind_[i]], points_[vind_[i]]+veclen_, data_[i]);
254  }
255  }
256  }
257 
258 private:
259 
260 
261  /*--------------------- Internal Data Structures --------------------------*/
262  struct Node
263  {
267  int left, right;
271  int divfeat;
275  DistanceType divlow, divhigh;
279  Node* child1, * child2;
280 
282  {
283  if (child1) child1->~Node();
284  if (child2) child2->~Node();
285  }
286 
287  private:
288  template<typename Archive>
289  void serialize(Archive& ar)
290  {
292  Index* obj = static_cast<Index*>(ar.getObject());
293 
294  ar & left;
295  ar & right;
296  ar & divfeat;
297  ar & divlow;
298  ar & divhigh;
299 
300  bool leaf_node = false;
301  if (Archive::is_saving::value) {
302  leaf_node = ((child1==NULL) && (child2==NULL));
303  }
304  ar & leaf_node;
305 
306  if (!leaf_node) {
307  if (Archive::is_loading::value) {
308  child1 = new(obj->pool_) Node();
309  child2 = new(obj->pool_) Node();
310  }
311  ar & *child1;
312  ar & *child2;
313  }
314  }
315  friend struct serialization::access;
316  };
317  typedef Node* NodePtr;
318 
319 
320  struct Interval
321  {
322  DistanceType low, high;
323 
324  private:
325  template <typename Archive>
326  void serialize(Archive& ar)
327  {
328  ar & low;
329  ar & high;
330  }
331  friend struct serialization::access;
332  };
333 
334  typedef std::vector<Interval> BoundingBox;
335 
337  typedef BranchSt* Branch;
338 
339 
340 
341  void freeIndex()
342  {
343  if (data_.ptr()) {
344  delete[] data_.ptr();
346  }
347  if (root_node_) root_node_->~Node();
348  pool_.free();
349  }
350 
351  void copyTree(NodePtr& dst, const NodePtr& src)
352  {
353  dst = new(pool_) Node();
354  *dst = *src;
355  if (src->child1!=NULL && src->child2!=NULL) {
356  copyTree(dst->child1, src->child1);
357  copyTree(dst->child2, src->child2);
358  }
359  }
360 
361 
362 
363  void computeBoundingBox(BoundingBox& bbox)
364  {
365  bbox.resize(veclen_);
366  for (size_t i=0; i<veclen_; ++i) {
367  bbox[i].low = (DistanceType)points_[0][i];
368  bbox[i].high = (DistanceType)points_[0][i];
369  }
370  for (size_t k=1; k<size_; ++k) {
371  for (size_t i=0; i<veclen_; ++i) {
372  if (points_[k][i]<bbox[i].low) bbox[i].low = (DistanceType)points_[k][i];
373  if (points_[k][i]>bbox[i].high) bbox[i].high = (DistanceType)points_[k][i];
374  }
375  }
376  }
377 
378 
388  NodePtr divideTree(int left, int right, BoundingBox& bbox)
389  {
390  NodePtr node = new (pool_) Node(); // allocate memory
391 
392  /* If too few exemplars remain, then make this a leaf node. */
393  if ( (right-left) <= leaf_max_size_) {
394  node->child1 = node->child2 = NULL; /* Mark as leaf node. */
395  node->left = left;
396  node->right = right;
397 
398  // compute bounding-box of leaf points
399  for (size_t i=0; i<veclen_; ++i) {
400  bbox[i].low = (DistanceType)points_[vind_[left]][i];
401  bbox[i].high = (DistanceType)points_[vind_[left]][i];
402  }
403  for (int k=left+1; k<right; ++k) {
404  for (size_t i=0; i<veclen_; ++i) {
405  if (bbox[i].low>points_[vind_[k]][i]) bbox[i].low=(DistanceType)points_[vind_[k]][i];
406  if (bbox[i].high<points_[vind_[k]][i]) bbox[i].high=(DistanceType)points_[vind_[k]][i];
407  }
408  }
409  }
410  else {
411  int idx;
412  int cutfeat;
413  DistanceType cutval;
414  middleSplit(&vind_[0]+left, right-left, idx, cutfeat, cutval, bbox);
415 
416  node->divfeat = cutfeat;
417 
418  BoundingBox left_bbox(bbox);
419  left_bbox[cutfeat].high = cutval;
420  node->child1 = divideTree(left, left+idx, left_bbox);
421 
422  BoundingBox right_bbox(bbox);
423  right_bbox[cutfeat].low = cutval;
424  node->child2 = divideTree(left+idx, right, right_bbox);
425 
426  node->divlow = left_bbox[cutfeat].high;
427  node->divhigh = right_bbox[cutfeat].low;
428 
429  for (size_t i=0; i<veclen_; ++i) {
430  bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
431  bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
432  }
433  }
434 
435  return node;
436  }
437 
438  void computeMinMax(int* ind, int count, int dim, ElementType& min_elem, ElementType& max_elem)
439  {
440  min_elem = points_[ind[0]][dim];
441  max_elem = points_[ind[0]][dim];
442  for (int i=1; i<count; ++i) {
443  ElementType val = points_[ind[i]][dim];
444  if (val<min_elem) min_elem = val;
445  if (val>max_elem) max_elem = val;
446  }
447  }
448 
449  void middleSplit(int* ind, int count, int& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
450  {
451  // find the largest span from the approximate bounding box
452  ElementType max_span = bbox[0].high-bbox[0].low;
453  cutfeat = 0;
454  cutval = (bbox[0].high+bbox[0].low)/2;
455  for (size_t i=1; i<veclen_; ++i) {
456  ElementType span = bbox[i].high-bbox[i].low;
457  if (span>max_span) {
458  max_span = span;
459  cutfeat = i;
460  cutval = (bbox[i].high+bbox[i].low)/2;
461  }
462  }
463 
464  // compute exact span on the found dimension
465  ElementType min_elem, max_elem;
466  computeMinMax(ind, count, cutfeat, min_elem, max_elem);
467  cutval = (min_elem+max_elem)/2;
468  max_span = max_elem - min_elem;
469 
470  // check if a dimension of a largest span exists
471  size_t k = cutfeat;
472  for (size_t i=0; i<veclen_; ++i) {
473  if (i==k) continue;
474  ElementType span = bbox[i].high-bbox[i].low;
475  if (span>max_span) {
476  computeMinMax(ind, count, i, min_elem, max_elem);
477  span = max_elem - min_elem;
478  if (span>max_span) {
479  max_span = span;
480  cutfeat = i;
481  cutval = (min_elem+max_elem)/2;
482  }
483  }
484  }
485  int lim1, lim2;
486  planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
487 
488  if (lim1>count/2) index = lim1;
489  else if (lim2<count/2) index = lim2;
490  else index = count/2;
491 
492  assert(index > 0 && index < count);
493  }
494 
495 
496  void middleSplit_(int* ind, int count, int& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
497  {
498  const float eps_val=0.00001f;
499  DistanceType max_span = bbox[0].high-bbox[0].low;
500  for (size_t i=1; i<veclen_; ++i) {
501  DistanceType span = bbox[i].high-bbox[i].low;
502  if (span>max_span) {
503  max_span = span;
504  }
505  }
506  DistanceType max_spread = -1;
507  cutfeat = 0;
508  for (size_t i=0; i<veclen_; ++i) {
509  DistanceType span = bbox[i].high-bbox[i].low;
510  if (span>(DistanceType)((1-eps_val)*max_span)) {
511  ElementType min_elem, max_elem;
512  computeMinMax(ind, count, cutfeat, min_elem, max_elem);
513  DistanceType spread = (DistanceType)(max_elem-min_elem);
514  if (spread>max_spread) {
515  cutfeat = i;
516  max_spread = spread;
517  }
518  }
519  }
520  // split in the middle
521  DistanceType split_val = (bbox[cutfeat].low+bbox[cutfeat].high)/2;
522  ElementType min_elem, max_elem;
523  computeMinMax(ind, count, cutfeat, min_elem, max_elem);
524 
525  if (split_val<min_elem) cutval = (DistanceType)min_elem;
526  else if (split_val>max_elem) cutval = (DistanceType)max_elem;
527  else cutval = split_val;
528 
529  int lim1, lim2;
530  planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
531 
532  if (lim1>count/2) index = lim1;
533  else if (lim2<count/2) index = lim2;
534  else index = count/2;
535 
536  assert(index > 0 && index < count);
537  }
538 
539 
549  void planeSplit(int* ind, int count, int cutfeat, DistanceType cutval, int& lim1, int& lim2)
550  {
551  int left = 0;
552  int right = count-1;
553  for (;; ) {
554  while (left<=right && points_[ind[left]][cutfeat]<cutval) ++left;
555  while (left<=right && points_[ind[right]][cutfeat]>=cutval) --right;
556  if (left>right) break;
557  std::swap(ind[left], ind[right]); ++left; --right;
558  }
559 
560  lim1 = left;
561  right = count-1;
562  for (;; ) {
563  while (left<=right && points_[ind[left]][cutfeat]<=cutval) ++left;
564  while (left<=right && points_[ind[right]][cutfeat]>cutval) --right;
565  if (left>right) break;
566  std::swap(ind[left], ind[right]); ++left; --right;
567  }
568  lim2 = left;
569  }
570 
571  DistanceType computeInitialDistances(const ElementType* vec, std::vector<DistanceType>& dists) const
572  {
573  DistanceType distsq = 0.0;
574 
575  for (size_t i = 0; i < veclen_; ++i) {
576  if (vec[i] < root_bbox_[i].low) {
577  dists[i] = distance_.accum_dist(vec[i], root_bbox_[i].low, i);
578  distsq += dists[i];
579  }
580  if (vec[i] > root_bbox_[i].high) {
581  dists[i] = distance_.accum_dist(vec[i], root_bbox_[i].high, i);
582  distsq += dists[i];
583  }
584  }
585 
586  return distsq;
587  }
588 
592  template <bool with_removed>
593  void searchLevel(ResultSet<DistanceType>& result_set, const ElementType* vec, const NodePtr node, DistanceType mindistsq,
594  std::vector<DistanceType>& dists, const float epsError) const
595  {
596  /* If this is a leaf node, then do check and return. */
597  if ((node->child1 == NULL)&&(node->child2 == NULL)) {
598  DistanceType worst_dist = result_set.worstDist();
599  for (int i=node->left; i<node->right; ++i) {
600  if (with_removed) {
601  if (removed_points_.test(vind_[i])) continue;
602  }
603  ElementType* point = reorder_ ? data_[i] : points_[vind_[i]];
604  DistanceType dist = distance_(vec, point, veclen_, worst_dist);
605  if (dist<worst_dist) {
606  result_set.addPoint(dist,vind_[i]);
607  }
608  }
609  return;
610  }
611 
612  /* Which child branch should be taken first? */
613  int idx = node->divfeat;
614  ElementType val = vec[idx];
615  DistanceType diff1 = val - node->divlow;
616  DistanceType diff2 = val - node->divhigh;
617 
618  NodePtr bestChild;
619  NodePtr otherChild;
620  DistanceType cut_dist;
621  if ((diff1+diff2)<0) {
622  bestChild = node->child1;
623  otherChild = node->child2;
624  cut_dist = distance_.accum_dist(val, node->divhigh, idx);
625  }
626  else {
627  bestChild = node->child2;
628  otherChild = node->child1;
629  cut_dist = distance_.accum_dist( val, node->divlow, idx);
630  }
631 
632  /* Call recursively to search next level down. */
633  searchLevel<with_removed>(result_set, vec, bestChild, mindistsq, dists, epsError);
634 
635  DistanceType dst = dists[idx];
636  mindistsq = mindistsq + cut_dist - dst;
637  dists[idx] = cut_dist;
638  if (mindistsq*epsError<=result_set.worstDist()) {
639  searchLevel<with_removed>(result_set, vec, otherChild, mindistsq, dists, epsError);
640  }
641  dists[idx] = dst;
642  }
643 
644 
645  void swap(KDTreeSingleIndex& other)
646  {
647  BaseClass::swap(other);
648  std::swap(leaf_max_size_, other.leaf_max_size_);
649  std::swap(reorder_, other.reorder_);
650  std::swap(vind_, other.vind_);
651  std::swap(data_, other.data_);
652  std::swap(root_node_, other.root_node_);
653  std::swap(root_bbox_, other.root_bbox_);
654  std::swap(pool_, other.pool_);
655  }
656 
657 private:
658 
659 
660 
662 
663 
664  bool reorder_;
665 
669  std::vector<int> vind_;
670 
672 
676  NodePtr root_node_;
677 
681  BoundingBox root_bbox_;
682 
691 
693 
694 }; // class KDTreeSingleIndex
695 
696 }
697 
698 #endif //FLANN_KDTREE_SINGLE_INDEX_H_
d
#define NULL
std::map< std::string, any > IndexParams
Definition: params.h:51
void addPoints(const Matrix< ElementType > &points, float rebuild_threshold=2)
Incrementally add points to the index.
T get_param(const IndexParams &params, std::string name, const T &default_value)
Definition: params.h:95
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
Distance::ElementType ElementType
void findNeighbors(ResultSet< DistanceType > &result, const ElementType *vec, const SearchParams &searchParams) const
NNIndex< Distance > BaseClass
char * dst
Definition: lz4.h:354
void swap(linb::any &lhs, linb::any &rhs) noexcept
void middleSplit(int *ind, int count, int &index, int &cutfeat, DistanceType &cutval, const BoundingBox &bbox)
NodePtr divideTree(int left, int right, BoundingBox &bbox)
#define USING_BASECLASS_SYMBOLS
Definition: nn_index.h:897
void searchLevel(ResultSet< DistanceType > &result_set, const ElementType *vec, const NodePtr node, DistanceType mindistsq, std::vector< DistanceType > &dists, const float epsError) const
BranchStruct< NodePtr, DistanceType > BranchSt
void copyTree(NodePtr &dst, const NodePtr &src)
void swap(KDTreeSingleIndex &other)
void middleSplit_(int *ind, int count, int &index, int &cutfeat, DistanceType &cutval, const BoundingBox &bbox)
KDTreeSingleIndex(const Matrix< ElementType > &inputData, const IndexParams &params=KDTreeSingleIndexParams(), Distance d=Distance())
size_t cols
Definition: matrix.h:73
KDTreeSingleIndex(const KDTreeSingleIndex &other)
flann_algorithm_t
Definition: defines.h:79
Distance::ResultType DistanceType
Matrix< ElementType > data_
KDTreeSingleIndexParams(int leaf_max_size=10, bool reorder=true)
struct Index Index
Definition: sqlite3.c:8577
DistanceType computeInitialDistances(const ElementType *vec, std::vector< DistanceType > &dists) const
params
void computeMinMax(int *ind, int count, int dim, ElementType &min_elem, ElementType &max_elem)
KDTreeSingleIndex & operator=(KDTreeSingleIndex other)
virtual DistanceType worstDist() const =0
void RTABMAP_EXP computeMinMax(const std::map< int, Transform > &poses, cv::Vec3f &min, cv::Vec3f &max)
Definition: Graph.cpp:2329
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
flann_algorithm_t getType() const
dist
static void freeIndex(sqlite3 *db, Index *p)
Definition: sqlite3.c:84627
KDTreeSingleIndex(const IndexParams &params=KDTreeSingleIndexParams(), Distance d=Distance())
void computeBoundingBox(BoundingBox &bbox)
std::vector< Interval > BoundingBox
virtual void addPoint(DistanceType dist, size_t index)=0
void planeSplit(int *ind, int count, int cutfeat, DistanceType cutval, int &lim1, int &lim2)


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