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 
28 #include <rtabmap/core/FlannIndex.h>
29 #include <rtabmap/utilite/ULogger.h>
30 
31 #include "rtflann/flann.hpp"
32 
33 namespace rtabmap {
34 
35 FlannIndex::FlannIndex():
36  index_(0),
37  nextIndex_(0),
38  featuresType_(0),
39  featuresDim_(0),
40  isLSH_(false),
41  useDistanceL1_(false),
42  rebalancingFactor_(2.0f)
43 {
44 }
45 FlannIndex::~FlannIndex()
46 {
47  this->release();
48 }
49 
50 void FlannIndex::release()
51 {
52  if(index_)
53  {
54  if(featuresType_ == CV_8UC1)
55  {
56  delete (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
57  }
58  else
59  {
60  if(useDistanceL1_)
61  {
62  delete (rtflann::Index<rtflann::L1<float> >*)index_;
63  }
64  else if(featuresDim_ <= 3)
65  {
66  delete (rtflann::Index<rtflann::L2_Simple<float> >*)index_;
67  }
68  else
69  {
70  delete (rtflann::Index<rtflann::L2<float> >*)index_;
71  }
72  }
73  index_ = 0;
74  }
75  nextIndex_ = 0;
76  isLSH_ = false;
77  addedDescriptors_.clear();
78  removedIndexes_.clear();
79 }
80 
81 unsigned int FlannIndex::indexedFeatures() const
82 {
83  if(!index_)
84  {
85  return 0;
86  }
87  if(featuresType_ == CV_8UC1)
88  {
89  return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->size();
90  }
91  else
92  {
93  if(useDistanceL1_)
94  {
95  return ((const rtflann::Index<rtflann::L1<float> >*)index_)->size();
96  }
97  else if(featuresDim_ <= 3)
98  {
99  return ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->size();
100  }
101  else
102  {
103  return ((const rtflann::Index<rtflann::L2<float> >*)index_)->size();
104  }
105  }
106 }
107 
108 // return KB
109 unsigned int FlannIndex::memoryUsed() const
110 {
111  if(!index_)
112  {
113  return 0;
114  }
115  if(featuresType_ == CV_8UC1)
116  {
117  return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->usedMemory()/1000;
118  }
119  else
120  {
121  if(useDistanceL1_)
122  {
123  return ((const rtflann::Index<rtflann::L1<float> >*)index_)->usedMemory()/1000;
124  }
125  else if(featuresDim_ <= 3)
126  {
127  return ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->usedMemory()/1000;
128  }
129  else
130  {
131  return ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory()/1000;
132  }
133  }
134 }
135 
136 void FlannIndex::buildLinearIndex(
137  const cv::Mat & features,
138  bool useDistanceL1,
139  float rebalancingFactor)
140 {
141  this->release();
142  UASSERT(index_ == 0);
143  UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
144  featuresType_ = features.type();
145  featuresDim_ = features.cols;
146  useDistanceL1_ = useDistanceL1;
147  rebalancingFactor_ = rebalancingFactor;
148 
149  rtflann::LinearIndexParams params;
150 
151  if(featuresType_ == CV_8UC1)
152  {
153  rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
154  index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
155  ((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
156  }
157  else
158  {
159  rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
160  if(useDistanceL1_)
161  {
162  index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
163  ((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
164  }
165  else if(featuresDim_ <=3)
166  {
167  index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
168  ((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->buildIndex();
169  }
170  else
171  {
172  index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
173  ((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
174  }
175  }
176 
177  // incremental FLANN
178  addedDescriptors_.insert(std::make_pair(nextIndex_, features));
179 
180  nextIndex_ = features.rows;
181 }
182 
183 void FlannIndex::buildKDTreeIndex(
184  const cv::Mat & features,
185  int trees,
186  bool useDistanceL1,
187  float rebalancingFactor)
188 {
189  this->release();
190  UASSERT(index_ == 0);
191  UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
192  featuresType_ = features.type();
193  featuresDim_ = features.cols;
194  useDistanceL1_ = useDistanceL1;
195  rebalancingFactor_ = rebalancingFactor;
196 
197  rtflann::KDTreeIndexParams params(trees);
198 
199  if(featuresType_ == CV_8UC1)
200  {
201  rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
202  index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
203  ((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
204  }
205  else
206  {
207  rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
208  if(useDistanceL1_)
209  {
210  index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
211  ((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
212  }
213  else if(featuresDim_ <=3)
214  {
215  index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
216  ((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->buildIndex();
217  }
218  else
219  {
220  index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
221  ((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
222  }
223  }
224 
225  // incremental FLANN
226  addedDescriptors_.insert(std::make_pair(nextIndex_, features));
227 
228  nextIndex_ = features.rows;
229 }
230 
231 void FlannIndex::buildKDTreeSingleIndex(
232  const cv::Mat & features,
233  int leafMaxSize,
234  bool reorder,
235  bool useDistanceL1,
236  float rebalancingFactor)
237 {
238  this->release();
239  UASSERT(index_ == 0);
240  UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
241  featuresType_ = features.type();
242  featuresDim_ = features.cols;
243  useDistanceL1_ = useDistanceL1;
244  rebalancingFactor_ = rebalancingFactor;
245 
246  rtflann::KDTreeSingleIndexParams params(leafMaxSize, reorder);
247 
248  if(featuresType_ == CV_8UC1)
249  {
250  rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
251  index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
252  ((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
253  }
254  else
255  {
256  rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
257  if(useDistanceL1_)
258  {
259  index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
260  ((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
261  }
262  else if(featuresDim_ <=3)
263  {
264  index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
265  ((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->buildIndex();
266  }
267  else
268  {
269  index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
270  ((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
271  }
272  }
273 
274  // incremental FLANN
275  addedDescriptors_.insert(std::make_pair(nextIndex_, features));
276 
277  nextIndex_ = features.rows;
278 }
279 
280 void FlannIndex::buildLSHIndex(
281  const cv::Mat & features,
282  unsigned int table_number,
283  unsigned int key_size,
284  unsigned int multi_probe_level,
285  float rebalancingFactor)
286 {
287  this->release();
288  UASSERT(index_ == 0);
289  UASSERT(features.type() == CV_8UC1);
290  featuresType_ = features.type();
291  featuresDim_ = features.cols;
292  useDistanceL1_ = true;
293  rebalancingFactor_ = rebalancingFactor;
294 
295  rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
296  index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, rtflann::LshIndexParams(12, 20, 2));
297  ((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
298 
299  // incremental FLANN
300  addedDescriptors_.insert(std::make_pair(nextIndex_, features));
301 
302  nextIndex_ = features.rows;
303 }
304 
305 bool FlannIndex::isBuilt()
306 {
307  return index_!=0;
308 }
309 
310 unsigned int FlannIndex::addPoints(const cv::Mat & features)
311 {
312  if(!index_)
313  {
314  UERROR("Flann index not yet created!");
315  return 0;
316  }
317  UASSERT(features.type() == featuresType_);
318  UASSERT(features.cols == featuresDim_);
319  bool indexRebuilt = false;
320  size_t removedPts = 0;
321  if(featuresType_ == CV_8UC1)
322  {
323  rtflann::Matrix<unsigned char> points(features.data, features.rows, features.cols);
324  rtflann::Index<rtflann::Hamming<unsigned char> > * index = (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
325  removedPts = index->removedCount();
326  index->addPoints(points, 0);
327  // Rebuild index if it is now X times in size
328  if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
329  {
330  UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
331  index->buildIndex();
332  }
333  // if no more removed points, the index has been rebuilt
334  indexRebuilt = index->removedCount() == 0 && removedPts>0;
335  }
336  else
337  {
338  rtflann::Matrix<float> points((float*)features.data, features.rows, features.cols);
339  if(useDistanceL1_)
340  {
341  rtflann::Index<rtflann::L1<float> > * index = (rtflann::Index<rtflann::L1<float> >*)index_;
342  removedPts = index->removedCount();
343  index->addPoints(points, 0);
344  // Rebuild index if it doubles in size
345  if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
346  {
347  UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
348  index->buildIndex();
349  }
350  // if no more removed points, the index has been rebuilt
351  indexRebuilt = index->removedCount() == 0 && removedPts>0;
352  }
353  else if(featuresDim_ <= 3)
354  {
355  rtflann::Index<rtflann::L2_Simple<float> > * index = (rtflann::Index<rtflann::L2_Simple<float> >*)index_;
356  removedPts = index->removedCount();
357  index->addPoints(points, 0);
358  // Rebuild index if it doubles in size
359  if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
360  {
361  UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
362  index->buildIndex();
363  }
364  // if no more removed points, the index has been rebuilt
365  indexRebuilt = index->removedCount() == 0 && removedPts>0;
366  }
367  else
368  {
369  rtflann::Index<rtflann::L2<float> > * index = (rtflann::Index<rtflann::L2<float> >*)index_;
370  removedPts = index->removedCount();
371  index->addPoints(points, 0);
372  // Rebuild index if it doubles in size
373  if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
374  {
375  UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
376  index->buildIndex();
377  }
378  // if no more removed points, the index has been rebuilt
379  indexRebuilt = index->removedCount() == 0 && removedPts>0;
380  }
381  }
382 
383  if(indexRebuilt)
384  {
385  UASSERT(removedPts == removedIndexes_.size());
386  // clean not used features
387  for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
388  {
389  addedDescriptors_.erase(*iter);
390  }
391  removedIndexes_.clear();
392  }
393 
394  addedDescriptors_.insert(std::make_pair(nextIndex_, features));
395 
396  int r = nextIndex_;
397  nextIndex_ += features.rows;
398  return r;
399 }
400 
401 void FlannIndex::removePoint(unsigned int index)
402 {
403  if(!index_)
404  {
405  UERROR("Flann index not yet created!");
406  return;
407  }
408 
409  // If a Segmentation fault occurs in removePoint(), verify that you have this fix in your installed "flann/algorithms/nn_index.h":
410  // 707 - if (ids_[id]==id) {
411  // 707 + if (id < ids_.size() && ids_[id]==id) {
412  // ref: https://github.com/mariusmuja/flann/commit/23051820b2314f07cf40ba633a4067782a982ff3#diff-33762b7383f957c2df17301639af5151
413 
414  if(featuresType_ == CV_8UC1)
415  {
416  ((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->removePoint(index);
417  }
418  else if(useDistanceL1_)
419  {
420  ((rtflann::Index<rtflann::L1<float> >*)index_)->removePoint(index);
421  }
422  else if(featuresDim_ <= 3)
423  {
424  ((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->removePoint(index);
425  }
426  else
427  {
428  ((rtflann::Index<rtflann::L2<float> >*)index_)->removePoint(index);
429  }
430 
431  removedIndexes_.push_back(index);
432 }
433 
434 void FlannIndex::knnSearch(
435  const cv::Mat & query,
436  cv::Mat & indices,
437  cv::Mat & dists,
438  int knn,
439  int checks,
440  float eps,
441  bool sorted) const
442 {
443  if(!index_)
444  {
445  UERROR("Flann index not yet created!");
446  return;
447  }
448  indices.create(query.rows, knn, sizeof(size_t)==8?CV_64F:CV_32S);
449  dists.create(query.rows, knn, featuresType_ == CV_8UC1?CV_32S:CV_32F);
450 
451  rtflann::Matrix<size_t> indicesF((size_t*)indices.data, indices.rows, indices.cols);
452 
453  rtflann::SearchParams params = rtflann::SearchParams(checks, eps, sorted);
454 
455  if(featuresType_ == CV_8UC1)
456  {
457  rtflann::Matrix<unsigned int> distsF((unsigned int*)dists.data, dists.rows, dists.cols);
458  rtflann::Matrix<unsigned char> queryF(query.data, query.rows, query.cols);
459  ((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
460  }
461  else
462  {
463  rtflann::Matrix<float> distsF((float*)dists.data, dists.rows, dists.cols);
464  rtflann::Matrix<float> queryF((float*)query.data, query.rows, query.cols);
465  if(useDistanceL1_)
466  {
467  ((rtflann::Index<rtflann::L1<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
468  }
469  else if(featuresDim_ <= 3)
470  {
471  ((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
472  }
473  else
474  {
475  ((rtflann::Index<rtflann::L2<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
476  }
477  }
478 }
479 
480 void FlannIndex::radiusSearch(
481  const cv::Mat & query,
482  std::vector<std::vector<size_t> > & indices,
483  std::vector<std::vector<float> > & dists,
484  float radius,
485  int maxNeighbors,
486  int checks,
487  float eps,
488  bool sorted) const
489 {
490  if(!index_)
491  {
492  UERROR("Flann index not yet created!");
493  return;
494  }
495 
496  rtflann::SearchParams params = rtflann::SearchParams(checks, eps, sorted);
497  params.max_neighbors = maxNeighbors<=0?-1:maxNeighbors; // -1 is all in radius
498 
499  if(featuresType_ == CV_8UC1)
500  {
501  std::vector<std::vector<unsigned int> > distsF;
502  rtflann::Matrix<unsigned char> queryF(query.data, query.rows, query.cols);
503  ((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->radiusSearch(queryF, indices, distsF, radius*radius, params);
504  dists.resize(distsF.size());
505  for(unsigned int i=0; i<dists.size(); ++i)
506  {
507  dists[i].resize(distsF[i].size());
508  for(unsigned int j=0; j<distsF[i].size(); ++j)
509  {
510  dists[i][j] = (float)distsF[i][j];
511  }
512  }
513  }
514  else
515  {
516  rtflann::Matrix<float> queryF((float*)query.data, query.rows, query.cols);
517  if(useDistanceL1_)
518  {
519  ((rtflann::Index<rtflann::L1<float> >*)index_)->radiusSearch(queryF, indices, dists, radius*radius, params);
520  }
521  else if(featuresDim_ <= 3)
522  {
523  ((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->radiusSearch(queryF, indices, dists, radius*radius, params);
524  }
525  else
526  {
527  ((rtflann::Index<rtflann::L2<float> >*)index_)->radiusSearch(queryF, indices, dists, radius*radius, params);
528  }
529  }
530 }
531 
532 } /* namespace rtabmap */
rtabmap::FlannIndex::~FlannIndex
virtual ~FlannIndex()
Definition: FlannIndex.cpp:45
rtabmap::FlannIndex::removePoint
void removePoint(unsigned int index)
Definition: FlannIndex.cpp:401
rtabmap::FlannIndex::buildLSHIndex
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:280
f
f
rtflann::Index::buildIndex
void buildIndex()
Definition: flann.hpp:137
rtabmap::FlannIndex::addedDescriptors_
std::map< int, cv::Mat > addedDescriptors_
Definition: FlannIndex.h:113
rtflann::L2_Simple
Definition: dist.h:74
rtabmap::FlannIndex::knnSearch
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:434
rtabmap::FlannIndex::radiusSearch
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:480
rtabmap::FlannIndex::addPoints
unsigned int addPoints(const cv::Mat &features)
Definition: FlannIndex.cpp:310
rtflann::Index::sizeAtBuild
size_t sizeAtBuild() const
Definition: flann.hpp:208
UASSERT
#define UASSERT(condition)
rtabmap::FlannIndex::buildLinearIndex
void buildLinearIndex(const cv::Mat &features, bool useDistanceL1=false, float rebalancingFactor=2.0f)
Definition: FlannIndex.cpp:136
rtabmap::FlannIndex::indexedFeatures
unsigned int indexedFeatures() const
Definition: FlannIndex.cpp:81
rtflann::Index::addPoints
void addPoints(const Matrix< ElementType > &points, float rebuild_threshold=2)
Definition: flann.hpp:149
rtabmap::FlannIndex::buildKDTreeSingleIndex
void buildKDTreeSingleIndex(const cv::Mat &features, int leafMaxSize=10, bool reorder=true, bool useDistanceL1=false, float rebalancingFactor=2.0f)
Definition: FlannIndex.cpp:231
rtabmap::FlannIndex::nextIndex_
unsigned int nextIndex_
Definition: FlannIndex.h:104
rtabmap::FlannIndex::memoryUsed
unsigned int memoryUsed() const
Definition: FlannIndex.cpp:109
false
#define false
Definition: ConvertUTF.c:56
UDEBUG
#define UDEBUG(...)
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
rtflann::Index::size
size_t size() const
Definition: flann.hpp:198
rtflann::Index::removedCount
size_t removedCount() const
Definition: flann.hpp:203
rtabmap::FlannIndex::buildKDTreeIndex
void buildKDTreeIndex(const cv::Mat &features, int trees=4, bool useDistanceL1=false, float rebalancingFactor=2.0f)
Definition: FlannIndex.cpp:183
rtabmap::FlannIndex::removedIndexes_
std::list< int > removedIndexes_
Definition: FlannIndex.h:114

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:31