FlannIndex.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
30 
31 #include "rtflann/flann.hpp"
32 
33 namespace rtabmap {
34 
36  index_(0),
37  nextIndex_(0),
38  featuresType_(0),
39  featuresDim_(0),
40  isLSH_(false),
41  useDistanceL1_(false),
42  rebalancingFactor_(2.0f)
43 {
44 }
46 {
47  this->release();
48 }
49 
51 {
52  UDEBUG("");
53  if(index_)
54  {
55  if(featuresType_ == CV_8UC1)
56  {
58  }
59  else
60  {
61  if(useDistanceL1_)
62  {
64  }
65  else if(featuresDim_ <= 3)
66  {
68  }
69  else
70  {
72  }
73  }
74  index_ = 0;
75  }
76  nextIndex_ = 0;
77  isLSH_ = false;
78  addedDescriptors_.clear();
79  removedIndexes_.clear();
80  UDEBUG("");
81 }
82 
83 unsigned int FlannIndex::indexedFeatures() const
84 {
85  if(!index_)
86  {
87  return 0;
88  }
89  if(featuresType_ == CV_8UC1)
90  {
91  return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->size();
92  }
93  else
94  {
95  if(useDistanceL1_)
96  {
97  return ((const rtflann::Index<rtflann::L1<float> >*)index_)->size();
98  }
99  else if(featuresDim_ <= 3)
100  {
101  return ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->size();
102  }
103  else
104  {
105  return ((const rtflann::Index<rtflann::L2<float> >*)index_)->size();
106  }
107  }
108 }
109 
110 // return Bytes
111 unsigned long FlannIndex::memoryUsed() const
112 {
113  if(!index_)
114  {
115  return 0;
116  }
117  unsigned long memoryUsage = sizeof(FlannIndex);
118  memoryUsage += addedDescriptors_.size() * (sizeof(int) + sizeof(cv::Mat) + sizeof(std::map<int, cv::Mat>::iterator)) + sizeof(std::map<int, cv::Mat>);
119  memoryUsage += sizeof(std::list<int>) + removedIndexes_.size() * sizeof(int);
120  if(featuresType_ == CV_8UC1)
121  {
122  memoryUsage += ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->usedMemory();
123  }
124  else
125  {
126  if(useDistanceL1_)
127  {
128  memoryUsage += ((const rtflann::Index<rtflann::L1<float> >*)index_)->usedMemory();
129  }
130  else if(featuresDim_ <= 3)
131  {
132  memoryUsage += ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->usedMemory();
133  }
134  else
135  {
136  memoryUsage += ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory();
137  }
138  }
139  return memoryUsage;
140 }
141 
143  const cv::Mat & features,
144  bool useDistanceL1,
145  float rebalancingFactor)
146 {
147  UDEBUG("");
148  this->release();
149  UASSERT(index_ == 0);
150  UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
151  featuresType_ = features.type();
152  featuresDim_ = features.cols;
153  useDistanceL1_ = useDistanceL1;
154  rebalancingFactor_ = rebalancingFactor;
155 
157 
158  if(featuresType_ == CV_8UC1)
159  {
160  rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
163  }
164  else
165  {
166  rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
167  if(useDistanceL1_)
168  {
169  index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
171  }
172  else if(featuresDim_ <=3)
173  {
174  index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
176  }
177  else
178  {
179  index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
181  }
182  }
183 
184  // incremental FLANN: we should add all headers separately in case we remove
185  // some indexes (to keep underlying matrix data allocated)
186  if(rebalancingFactor_ > 1.0f)
187  {
188  for(int i=0; i<features.rows; ++i)
189  {
190  addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
191  }
192  }
193  else
194  {
195  // tree won't ever be rebalanced, so just keep only one header for the data
196  addedDescriptors_.insert(std::make_pair(nextIndex_, features));
197  nextIndex_ += features.rows;
198  }
199  UDEBUG("");
200 }
201 
203  const cv::Mat & features,
204  int trees,
205  bool useDistanceL1,
206  float rebalancingFactor)
207 {
208  UDEBUG("");
209  this->release();
210  UASSERT(index_ == 0);
211  UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
212  featuresType_ = features.type();
213  featuresDim_ = features.cols;
214  useDistanceL1_ = useDistanceL1;
215  rebalancingFactor_ = rebalancingFactor;
216 
218 
219  if(featuresType_ == CV_8UC1)
220  {
221  rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
224  }
225  else
226  {
227  rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
228  if(useDistanceL1_)
229  {
230  index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
232  }
233  else if(featuresDim_ <=3)
234  {
235  index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
237  }
238  else
239  {
240  index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
242  }
243  }
244 
245  // incremental FLANN: we should add all headers separately in case we remove
246  // some indexes (to keep underlying matrix data allocated)
247  if(rebalancingFactor_ > 1.0f)
248  {
249  for(int i=0; i<features.rows; ++i)
250  {
251  addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
252  }
253  }
254  else
255  {
256  // tree won't ever be rebalanced, so just keep only one header for the data
257  addedDescriptors_.insert(std::make_pair(nextIndex_, features));
258  nextIndex_ += features.rows;
259  }
260  UDEBUG("");
261 }
262 
264  const cv::Mat & features,
265  int leafMaxSize,
266  bool reorder,
267  bool useDistanceL1,
268  float rebalancingFactor)
269 {
270  UDEBUG("");
271  this->release();
272  UASSERT(index_ == 0);
273  UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
274  featuresType_ = features.type();
275  featuresDim_ = features.cols;
276  useDistanceL1_ = useDistanceL1;
277  rebalancingFactor_ = rebalancingFactor;
278 
279  rtflann::KDTreeSingleIndexParams params(leafMaxSize, reorder);
280 
281  if(featuresType_ == CV_8UC1)
282  {
283  rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
286  }
287  else
288  {
289  rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
290  if(useDistanceL1_)
291  {
292  index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
294  }
295  else if(featuresDim_ <=3)
296  {
297  index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
299  }
300  else
301  {
302  index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
304  }
305  }
306 
307  // incremental FLANN: we should add all headers separately in case we remove
308  // some indexes (to keep underlying matrix data allocated)
309  if(rebalancingFactor_ > 1.0f)
310  {
311  for(int i=0; i<features.rows; ++i)
312  {
313  addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
314  }
315  }
316  else
317  {
318  // tree won't ever be rebalanced, so just keep only one header for the data
319  addedDescriptors_.insert(std::make_pair(nextIndex_, features));
320  nextIndex_ += features.rows;
321  }
322  UDEBUG("");
323 }
324 
326  const cv::Mat & features,
327  unsigned int table_number,
328  unsigned int key_size,
329  unsigned int multi_probe_level,
330  float rebalancingFactor)
331 {
332  UDEBUG("");
333  this->release();
334  UASSERT(index_ == 0);
335  UASSERT(features.type() == CV_8UC1);
336  featuresType_ = features.type();
337  featuresDim_ = features.cols;
338  useDistanceL1_ = true;
339  rebalancingFactor_ = rebalancingFactor;
340 
341  rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
344 
345  // incremental FLANN: we should add all headers separately in case we remove
346  // some indexes (to keep underlying matrix data allocated)
347  if(rebalancingFactor_ > 1.0f)
348  {
349  for(int i=0; i<features.rows; ++i)
350  {
351  addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
352  }
353  }
354  else
355  {
356  // tree won't ever be rebalanced, so just keep only one header for the data
357  addedDescriptors_.insert(std::make_pair(nextIndex_, features));
358  nextIndex_ += features.rows;
359  }
360  UDEBUG("");
361 }
362 
364 {
365  return index_!=0;
366 }
367 
368 std::vector<unsigned int> FlannIndex::addPoints(const cv::Mat & features)
369 {
370  if(!index_)
371  {
372  UERROR("Flann index not yet created!");
373  return std::vector<unsigned int>();
374  }
375  UASSERT(features.type() == featuresType_);
376  UASSERT(features.cols == featuresDim_);
377  bool indexRebuilt = false;
378  size_t removedPts = 0;
379  if(featuresType_ == CV_8UC1)
380  {
381  rtflann::Matrix<unsigned char> points(features.data, features.rows, features.cols);
383  removedPts = index->removedCount();
384  index->addPoints(points, 0);
385  // Rebuild index if it is now X times in size
386  if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
387  {
388  UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
389  index->buildIndex();
390  }
391  // if no more removed points, the index has been rebuilt
392  indexRebuilt = index->removedCount() == 0 && removedPts>0;
393  }
394  else
395  {
396  rtflann::Matrix<float> points((float*)features.data, features.rows, features.cols);
397  if(useDistanceL1_)
398  {
400  removedPts = index->removedCount();
401  index->addPoints(points, 0);
402  // Rebuild index if it doubles in size
403  if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
404  {
405  UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
406  index->buildIndex();
407  }
408  // if no more removed points, the index has been rebuilt
409  indexRebuilt = index->removedCount() == 0 && removedPts>0;
410  }
411  else if(featuresDim_ <= 3)
412  {
414  removedPts = index->removedCount();
415  index->addPoints(points, 0);
416  // Rebuild index if it doubles in size
417  if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
418  {
419  UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
420  index->buildIndex();
421  }
422  // if no more removed points, the index has been rebuilt
423  indexRebuilt = index->removedCount() == 0 && removedPts>0;
424  }
425  else
426  {
428  removedPts = index->removedCount();
429  index->addPoints(points, 0);
430  // Rebuild index if it doubles in size
431  if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
432  {
433  UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
434  index->buildIndex();
435  }
436  // if no more removed points, the index has been rebuilt
437  indexRebuilt = index->removedCount() == 0 && removedPts>0;
438  }
439  }
440 
441  if(indexRebuilt)
442  {
443  UASSERT(removedPts == removedIndexes_.size());
444  // clean not used features
445  for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
446  {
447  addedDescriptors_.erase(*iter);
448  }
449  removedIndexes_.clear();
450  }
451 
452  // incremental FLANN: we should add all headers separately in case we remove
453  // some indexes (to keep underlying matrix data allocated)
454  std::vector<unsigned int> indexes;
455  for(int i=0; i<features.rows; ++i)
456  {
457  indexes.push_back(nextIndex_);
458  addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
459  }
460 
461  return indexes;
462 }
463 
464 void FlannIndex::removePoint(unsigned int index)
465 {
466  if(!index_)
467  {
468  UERROR("Flann index not yet created!");
469  return;
470  }
471 
472  // If a Segmentation fault occurs in removePoint(), verify that you have this fix in your installed "flann/algorithms/nn_index.h":
473  // 707 - if (ids_[id]==id) {
474  // 707 + if (id < ids_.size() && ids_[id]==id) {
475  // ref: https://github.com/mariusmuja/flann/commit/23051820b2314f07cf40ba633a4067782a982ff3#diff-33762b7383f957c2df17301639af5151
476 
477  if(featuresType_ == CV_8UC1)
478  {
479  ((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->removePoint(index);
480  }
481  else if(useDistanceL1_)
482  {
483  ((rtflann::Index<rtflann::L1<float> >*)index_)->removePoint(index);
484  }
485  else if(featuresDim_ <= 3)
486  {
487  ((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->removePoint(index);
488  }
489  else
490  {
491  ((rtflann::Index<rtflann::L2<float> >*)index_)->removePoint(index);
492  }
493 
494  removedIndexes_.push_back(index);
495 }
496 
498  const cv::Mat & query,
499  cv::Mat & indices,
500  cv::Mat & dists,
501  int knn,
502  int checks,
503  float eps,
504  bool sorted) const
505 {
506  if(!index_)
507  {
508  UERROR("Flann index not yet created!");
509  return;
510  }
511  indices.create(query.rows, knn, sizeof(size_t)==8?CV_64F:CV_32S);
512  dists.create(query.rows, knn, featuresType_ == CV_8UC1?CV_32S:CV_32F);
513 
514  rtflann::Matrix<size_t> indicesF((size_t*)indices.data, indices.rows, indices.cols);
515 
516  rtflann::SearchParams params = rtflann::SearchParams(checks, eps, sorted);
517 
518  if(featuresType_ == CV_8UC1)
519  {
520  rtflann::Matrix<unsigned int> distsF((unsigned int*)dists.data, dists.rows, dists.cols);
521  rtflann::Matrix<unsigned char> queryF(query.data, query.rows, query.cols);
522  ((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
523  }
524  else
525  {
526  rtflann::Matrix<float> distsF((float*)dists.data, dists.rows, dists.cols);
527  rtflann::Matrix<float> queryF((float*)query.data, query.rows, query.cols);
528  if(useDistanceL1_)
529  {
530  ((rtflann::Index<rtflann::L1<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
531  }
532  else if(featuresDim_ <= 3)
533  {
534  ((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
535  }
536  else
537  {
538  ((rtflann::Index<rtflann::L2<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
539  }
540  }
541 }
542 
544  const cv::Mat & query,
545  std::vector<std::vector<size_t> > & indices,
546  std::vector<std::vector<float> > & dists,
547  float radius,
548  int maxNeighbors,
549  int checks,
550  float eps,
551  bool sorted) const
552 {
553  if(!index_)
554  {
555  UERROR("Flann index not yet created!");
556  return;
557  }
558 
559  rtflann::SearchParams params = rtflann::SearchParams(checks, eps, sorted);
560  params.max_neighbors = maxNeighbors<=0?-1:maxNeighbors; // -1 is all in radius
561 
562  if(featuresType_ == CV_8UC1)
563  {
564  std::vector<std::vector<unsigned int> > distsF;
565  rtflann::Matrix<unsigned char> queryF(query.data, query.rows, query.cols);
566  ((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->radiusSearch(queryF, indices, distsF, radius*radius, params);
567  dists.resize(distsF.size());
568  for(unsigned int i=0; i<dists.size(); ++i)
569  {
570  dists[i].resize(distsF[i].size());
571  for(unsigned int j=0; j<distsF[i].size(); ++j)
572  {
573  dists[i][j] = (float)distsF[i][j];
574  }
575  }
576  }
577  else
578  {
579  rtflann::Matrix<float> queryF((float*)query.data, query.rows, query.cols);
580  if(useDistanceL1_)
581  {
582  ((rtflann::Index<rtflann::L1<float> >*)index_)->radiusSearch(queryF, indices, dists, radius*radius, params);
583  }
584  else if(featuresDim_ <= 3)
585  {
586  ((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->radiusSearch(queryF, indices, dists, radius*radius, params);
587  }
588  else
589  {
590  ((rtflann::Index<rtflann::L2<float> >*)index_)->radiusSearch(queryF, indices, dists, radius*radius, params);
591  }
592  }
593 }
594 
595 } /* namespace rtabmap */
virtual ~FlannIndex()
Definition: FlannIndex.cpp:45
void knnSearch(const cv::Mat &query, cv::Mat &indices, cv::Mat &dists, int knn, int checks=32, float eps=0.0, bool sorted=true) const
Definition: FlannIndex.cpp:497
void removePoint(unsigned int index)
Definition: FlannIndex.cpp:464
void buildLSHIndex(const cv::Mat &features, unsigned int table_number=12, unsigned int key_size=20, unsigned int multi_probe_level=2, float rebalancingFactor=2.0f)
Definition: FlannIndex.cpp:325
f
void buildIndex()
Definition: flann.hpp:137
size_t size() const
Definition: flann.hpp:198
std::map< int, cv::Mat > addedDescriptors_
Definition: FlannIndex.h:113
void radiusSearch(const cv::Mat &query, std::vector< std::vector< size_t > > &indices, std::vector< std::vector< float > > &dists, float radius, int maxNeighbors=0, int checks=32, float eps=0.0, bool sorted=true) const
Definition: FlannIndex.cpp:543
#define UASSERT(condition)
void buildLinearIndex(const cv::Mat &features, bool useDistanceL1=false, float rebalancingFactor=2.0f)
Definition: FlannIndex.cpp:142
unsigned long memoryUsed() const
Definition: FlannIndex.cpp:111
void addPoints(const Matrix< ElementType > &points, float rebuild_threshold=2)
Definition: flann.hpp:149
void buildKDTreeSingleIndex(const cv::Mat &features, int leafMaxSize=10, bool reorder=true, bool useDistanceL1=false, float rebalancingFactor=2.0f)
Definition: FlannIndex.cpp:263
unsigned int nextIndex_
Definition: FlannIndex.h:104
unsigned int indexedFeatures() const
Definition: FlannIndex.cpp:83
params
#define false
Definition: ConvertUTF.c:56
#define UDEBUG(...)
#define UERROR(...)
ULogger class and convenient macros.
std::vector< unsigned int > addPoints(const cv::Mat &features)
Definition: FlannIndex.cpp:368
void buildKDTreeIndex(const cv::Mat &features, int trees=4, bool useDistanceL1=false, float rebalancingFactor=2.0f)
Definition: FlannIndex.cpp:202
size_t removedCount() const
Definition: flann.hpp:203
std::list< int > removedIndexes_
Definition: FlannIndex.h:114
size_t sizeAtBuild() const
Definition: flann.hpp:208


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