mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
Refactored FlannIndex: Moved class into its own include file. Added radiusSearch() and buildKDtreeSingleIndex() methods. If dimensions <= 3 and float, use L2_Simple distance type.
This commit is contained in:
matlabbe
@@ -69,6 +69,7 @@ SET(SRC_FILES
|
||||
|
||||
rtflann/ext/lz4.c
|
||||
rtflann/ext/lz4hc.c
|
||||
FlannIndex.cpp
|
||||
|
||||
sqlite3/sqlite3.c
|
||||
)
|
||||
|
||||
518
corelib/src/FlannIndex.cpp
Normal file
518
corelib/src/FlannIndex.cpp
Normal file
@@ -0,0 +1,518 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <rtabmap/core/FlannIndex.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
|
||||
#include "rtflann/flann.hpp"
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
FlannIndex::FlannIndex():
|
||||
index_(0),
|
||||
nextIndex_(0),
|
||||
featuresType_(0),
|
||||
featuresDim_(0),
|
||||
isLSH_(false),
|
||||
useDistanceL1_(false)
|
||||
{
|
||||
}
|
||||
FlannIndex::~FlannIndex()
|
||||
{
|
||||
this->release();
|
||||
}
|
||||
|
||||
void FlannIndex::release()
|
||||
{
|
||||
if(index_)
|
||||
{
|
||||
if(featuresType_ == CV_8UC1)
|
||||
{
|
||||
delete (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(useDistanceL1_)
|
||||
{
|
||||
delete (rtflann::Index<rtflann::L1<float> >*)index_;
|
||||
}
|
||||
else if(featuresDim_ <= 3)
|
||||
{
|
||||
delete (rtflann::Index<rtflann::L2_Simple<float> >*)index_;
|
||||
}
|
||||
else
|
||||
{
|
||||
delete (rtflann::Index<rtflann::L2<float> >*)index_;
|
||||
}
|
||||
}
|
||||
index_ = 0;
|
||||
}
|
||||
nextIndex_ = 0;
|
||||
isLSH_ = false;
|
||||
addedDescriptors_.clear();
|
||||
removedIndexes_.clear();
|
||||
}
|
||||
|
||||
unsigned int FlannIndex::indexedFeatures() const
|
||||
{
|
||||
if(!index_)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
if(featuresType_ == CV_8UC1)
|
||||
{
|
||||
return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->size();
|
||||
}
|
||||
else
|
||||
{
|
||||
if(useDistanceL1_)
|
||||
{
|
||||
return ((const rtflann::Index<rtflann::L1<float> >*)index_)->size();
|
||||
}
|
||||
else if(featuresDim_ <= 3)
|
||||
{
|
||||
return ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->size();
|
||||
}
|
||||
else
|
||||
{
|
||||
return ((const rtflann::Index<rtflann::L2<float> >*)index_)->size();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// return KB
|
||||
unsigned int FlannIndex::memoryUsed() const
|
||||
{
|
||||
if(!index_)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
if(featuresType_ == CV_8UC1)
|
||||
{
|
||||
return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->usedMemory()/1000;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(useDistanceL1_)
|
||||
{
|
||||
return ((const rtflann::Index<rtflann::L1<float> >*)index_)->usedMemory()/1000;
|
||||
}
|
||||
else if(featuresDim_ <= 3)
|
||||
{
|
||||
return ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->usedMemory()/1000;
|
||||
}
|
||||
else
|
||||
{
|
||||
return ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory()/1000;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void FlannIndex::buildLinearIndex(
|
||||
const cv::Mat & features,
|
||||
bool useDistanceL1)
|
||||
{
|
||||
this->release();
|
||||
UASSERT(index_ == 0);
|
||||
UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
|
||||
featuresType_ = features.type();
|
||||
featuresDim_ = features.cols;
|
||||
useDistanceL1_ = useDistanceL1;
|
||||
|
||||
rtflann::LinearIndexParams params;
|
||||
|
||||
if(featuresType_ == CV_8UC1)
|
||||
{
|
||||
rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
|
||||
index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
|
||||
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
|
||||
}
|
||||
else
|
||||
{
|
||||
rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
|
||||
if(useDistanceL1_)
|
||||
{
|
||||
index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
|
||||
((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
|
||||
}
|
||||
else if(featuresDim_ <=3)
|
||||
{
|
||||
index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
|
||||
((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->buildIndex();
|
||||
}
|
||||
else
|
||||
{
|
||||
index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
|
||||
((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
|
||||
}
|
||||
}
|
||||
|
||||
// incremental FLANN
|
||||
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
|
||||
|
||||
nextIndex_ = features.rows;
|
||||
}
|
||||
|
||||
void FlannIndex::buildKDTreeIndex(
|
||||
const cv::Mat & features,
|
||||
int trees,
|
||||
bool useDistanceL1)
|
||||
{
|
||||
this->release();
|
||||
UASSERT(index_ == 0);
|
||||
UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
|
||||
featuresType_ = features.type();
|
||||
featuresDim_ = features.cols;
|
||||
useDistanceL1_ = useDistanceL1;
|
||||
|
||||
rtflann::KDTreeIndexParams params(trees);
|
||||
|
||||
if(featuresType_ == CV_8UC1)
|
||||
{
|
||||
rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
|
||||
index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
|
||||
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
|
||||
}
|
||||
else
|
||||
{
|
||||
rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
|
||||
if(useDistanceL1_)
|
||||
{
|
||||
index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
|
||||
((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
|
||||
}
|
||||
else if(featuresDim_ <=3)
|
||||
{
|
||||
index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
|
||||
((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->buildIndex();
|
||||
}
|
||||
else
|
||||
{
|
||||
index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
|
||||
((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
|
||||
}
|
||||
}
|
||||
|
||||
// incremental FLANN
|
||||
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
|
||||
|
||||
nextIndex_ = features.rows;
|
||||
}
|
||||
|
||||
void FlannIndex::buildKDTreeSingleIndex(
|
||||
const cv::Mat & features,
|
||||
int leafMaxSize,
|
||||
bool reorder,
|
||||
bool useDistanceL1)
|
||||
{
|
||||
this->release();
|
||||
UASSERT(index_ == 0);
|
||||
UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
|
||||
featuresType_ = features.type();
|
||||
featuresDim_ = features.cols;
|
||||
useDistanceL1_ = useDistanceL1;
|
||||
|
||||
rtflann::KDTreeSingleIndexParams params(leafMaxSize, reorder);
|
||||
|
||||
if(featuresType_ == CV_8UC1)
|
||||
{
|
||||
rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
|
||||
index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
|
||||
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
|
||||
}
|
||||
else
|
||||
{
|
||||
rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
|
||||
if(useDistanceL1_)
|
||||
{
|
||||
index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
|
||||
((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
|
||||
}
|
||||
else if(featuresDim_ <=3)
|
||||
{
|
||||
index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
|
||||
((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->buildIndex();
|
||||
}
|
||||
else
|
||||
{
|
||||
index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
|
||||
((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
|
||||
}
|
||||
}
|
||||
|
||||
// incremental FLANN
|
||||
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
|
||||
|
||||
nextIndex_ = features.rows;
|
||||
}
|
||||
|
||||
void FlannIndex::buildLSHIndex(
|
||||
const cv::Mat & features,
|
||||
unsigned int table_number,
|
||||
unsigned int key_size,
|
||||
unsigned int multi_probe_level)
|
||||
{
|
||||
this->release();
|
||||
UASSERT(index_ == 0);
|
||||
UASSERT(features.type() == CV_8UC1);
|
||||
featuresType_ = features.type();
|
||||
featuresDim_ = features.cols;
|
||||
useDistanceL1_ = true;
|
||||
|
||||
rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
|
||||
index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, rtflann::LshIndexParams(12, 20, 2));
|
||||
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
|
||||
|
||||
// incremental FLANN
|
||||
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
|
||||
|
||||
nextIndex_ = features.rows;
|
||||
}
|
||||
|
||||
bool FlannIndex::isBuilt()
|
||||
{
|
||||
return index_!=0;
|
||||
}
|
||||
|
||||
unsigned int FlannIndex::addPoints(const cv::Mat & features)
|
||||
{
|
||||
if(!index_)
|
||||
{
|
||||
UERROR("Flann index not yet created!");
|
||||
return 0;
|
||||
}
|
||||
UASSERT(features.type() == featuresType_);
|
||||
UASSERT(features.cols == featuresDim_);
|
||||
if(featuresType_ == CV_8UC1)
|
||||
{
|
||||
rtflann::Matrix<unsigned char> points(features.data, features.rows, features.cols);
|
||||
rtflann::Index<rtflann::Hamming<unsigned char> > * index = (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
|
||||
index->addPoints(points, 0);
|
||||
// Rebuild index if it doubles in size
|
||||
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
|
||||
{
|
||||
// clean not used features
|
||||
for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
|
||||
{
|
||||
addedDescriptors_.erase(*iter);
|
||||
}
|
||||
removedIndexes_.clear();
|
||||
index->buildIndex();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
rtflann::Matrix<float> points((float*)features.data, features.rows, features.cols);
|
||||
if(useDistanceL1_)
|
||||
{
|
||||
rtflann::Index<rtflann::L1<float> > * index = (rtflann::Index<rtflann::L1<float> >*)index_;
|
||||
index->addPoints(points, 0);
|
||||
// Rebuild index if it doubles in size
|
||||
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
|
||||
{
|
||||
// clean not used features
|
||||
for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
|
||||
{
|
||||
addedDescriptors_.erase(*iter);
|
||||
}
|
||||
removedIndexes_.clear();
|
||||
index->buildIndex();
|
||||
}
|
||||
}
|
||||
else if(featuresDim_ <= 3)
|
||||
{
|
||||
rtflann::Index<rtflann::L2_Simple<float> > * index = (rtflann::Index<rtflann::L2_Simple<float> >*)index_;
|
||||
index->addPoints(points, 0);
|
||||
// Rebuild index if it doubles in size
|
||||
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
|
||||
{
|
||||
// clean not used features
|
||||
for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
|
||||
{
|
||||
addedDescriptors_.erase(*iter);
|
||||
}
|
||||
removedIndexes_.clear();
|
||||
index->buildIndex();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
rtflann::Index<rtflann::L2<float> > * index = (rtflann::Index<rtflann::L2<float> >*)index_;
|
||||
index->addPoints(points, 0);
|
||||
// Rebuild index if it doubles in size
|
||||
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
|
||||
{
|
||||
// clean not used features
|
||||
for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
|
||||
{
|
||||
addedDescriptors_.erase(*iter);
|
||||
}
|
||||
removedIndexes_.clear();
|
||||
index->buildIndex();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
|
||||
|
||||
int r = nextIndex_;
|
||||
nextIndex_ += features.rows;
|
||||
return r;
|
||||
}
|
||||
|
||||
void FlannIndex::removePoint(unsigned int index)
|
||||
{
|
||||
if(!index_)
|
||||
{
|
||||
UERROR("Flann index not yet created!");
|
||||
return;
|
||||
}
|
||||
|
||||
// If a Segmentation fault occurs in removePoint(), verify that you have this fix in your installed "flann/algorithms/nn_index.h":
|
||||
// 707 - if (ids_[id]==id) {
|
||||
// 707 + if (id < ids_.size() && ids_[id]==id) {
|
||||
// ref: https://github.com/mariusmuja/flann/commit/23051820b2314f07cf40ba633a4067782a982ff3#diff-33762b7383f957c2df17301639af5151
|
||||
|
||||
if(featuresType_ == CV_8UC1)
|
||||
{
|
||||
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->removePoint(index);
|
||||
}
|
||||
else if(useDistanceL1_)
|
||||
{
|
||||
((rtflann::Index<rtflann::L1<float> >*)index_)->removePoint(index);
|
||||
}
|
||||
else if(featuresDim_ <= 3)
|
||||
{
|
||||
((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->removePoint(index);
|
||||
}
|
||||
else
|
||||
{
|
||||
((rtflann::Index<rtflann::L2<float> >*)index_)->removePoint(index);
|
||||
}
|
||||
|
||||
removedIndexes_.push_back(index);
|
||||
}
|
||||
|
||||
void FlannIndex::knnSearch(
|
||||
const cv::Mat & query,
|
||||
cv::Mat & indices,
|
||||
cv::Mat & dists,
|
||||
int knn,
|
||||
int checks,
|
||||
float eps,
|
||||
bool sorted) const
|
||||
{
|
||||
if(!index_)
|
||||
{
|
||||
UERROR("Flann index not yet created!");
|
||||
return;
|
||||
}
|
||||
indices.create(query.rows, knn, CV_32S);
|
||||
dists.create(query.rows, knn, featuresType_ == CV_8UC1?CV_32S:CV_32F);
|
||||
|
||||
rtflann::Matrix<int> indicesF((int*)indices.data, indices.rows, indices.cols);
|
||||
|
||||
rtflann::SearchParams params = rtflann::SearchParams(checks, eps, sorted);
|
||||
|
||||
if(featuresType_ == CV_8UC1)
|
||||
{
|
||||
rtflann::Matrix<unsigned int> distsF((unsigned int*)dists.data, dists.rows, dists.cols);
|
||||
rtflann::Matrix<unsigned char> queryF(query.data, query.rows, query.cols);
|
||||
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
|
||||
}
|
||||
else
|
||||
{
|
||||
rtflann::Matrix<float> distsF((float*)dists.data, dists.rows, dists.cols);
|
||||
rtflann::Matrix<float> queryF((float*)query.data, query.rows, query.cols);
|
||||
if(useDistanceL1_)
|
||||
{
|
||||
((rtflann::Index<rtflann::L1<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
|
||||
}
|
||||
else if(featuresDim_ <= 3)
|
||||
{
|
||||
((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
|
||||
}
|
||||
else
|
||||
{
|
||||
((rtflann::Index<rtflann::L2<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void FlannIndex::radiusSearch(
|
||||
const cv::Mat & query,
|
||||
std::vector<std::vector<size_t> > & indices,
|
||||
std::vector<std::vector<float> > & dists,
|
||||
float radius,
|
||||
int maxNeighbors,
|
||||
int checks,
|
||||
float eps,
|
||||
bool sorted) const
|
||||
{
|
||||
if(!index_)
|
||||
{
|
||||
UERROR("Flann index not yet created!");
|
||||
return;
|
||||
}
|
||||
|
||||
rtflann::SearchParams params = rtflann::SearchParams(checks, eps, sorted);
|
||||
params.max_neighbors = maxNeighbors<=0?-1:maxNeighbors; // -1 is all in radius
|
||||
|
||||
if(featuresType_ == CV_8UC1)
|
||||
{
|
||||
std::vector<std::vector<unsigned int> > distsF;
|
||||
rtflann::Matrix<unsigned char> queryF(query.data, query.rows, query.cols);
|
||||
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->radiusSearch(queryF, indices, distsF, radius*radius, params);
|
||||
dists.resize(distsF.size());
|
||||
for(unsigned int i=0; i<dists.size(); ++i)
|
||||
{
|
||||
dists[i].resize(distsF[i].size());
|
||||
for(unsigned int j=0; j<distsF[i].size(); ++j)
|
||||
{
|
||||
dists[i][j] = (float)distsF[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
rtflann::Matrix<float> queryF((float*)query.data, query.rows, query.cols);
|
||||
if(useDistanceL1_)
|
||||
{
|
||||
((rtflann::Index<rtflann::L1<float> >*)index_)->radiusSearch(queryF, indices, dists, radius*radius, params);
|
||||
}
|
||||
else if(featuresDim_ <= 3)
|
||||
{
|
||||
((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->radiusSearch(queryF, indices, dists, radius*radius, params);
|
||||
}
|
||||
else
|
||||
{
|
||||
((rtflann::Index<rtflann::L2<float> >*)index_)->radiusSearch(queryF, indices, dists, radius*radius, params);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} /* namespace rtabmap */
|
||||
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "rtabmap/core/Signature.h"
|
||||
#include "rtabmap/core/DBDriver.h"
|
||||
#include "rtabmap/core/Parameters.h"
|
||||
#include "rtabmap/core/FlannIndex.h"
|
||||
|
||||
#include "rtabmap/utilite/UtiLite.h"
|
||||
|
||||
@@ -45,307 +46,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#include "rtflann/flann.hpp"
|
||||
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
class FlannIndex
|
||||
{
|
||||
public:
|
||||
FlannIndex():
|
||||
index_(0),
|
||||
nextIndex_(0),
|
||||
featuresType_(0),
|
||||
featuresDim_(0),
|
||||
isLSH_(false),
|
||||
useDistanceL1_(false)
|
||||
{
|
||||
}
|
||||
virtual ~FlannIndex()
|
||||
{
|
||||
this->release();
|
||||
}
|
||||
|
||||
void release()
|
||||
{
|
||||
if(index_)
|
||||
{
|
||||
if(featuresType_ == CV_8UC1)
|
||||
{
|
||||
delete (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(useDistanceL1_)
|
||||
{
|
||||
delete (rtflann::Index<rtflann::L1<float> >*)index_;
|
||||
}
|
||||
else
|
||||
{
|
||||
delete (rtflann::Index<rtflann::L2<float> >*)index_;
|
||||
}
|
||||
}
|
||||
index_ = 0;
|
||||
}
|
||||
nextIndex_ = 0;
|
||||
isLSH_ = false;
|
||||
addedDescriptors_.clear();
|
||||
removedIndexes_.clear();
|
||||
}
|
||||
|
||||
unsigned int indexedFeatures() const
|
||||
{
|
||||
if(!index_)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
if(featuresType_ == CV_8UC1)
|
||||
{
|
||||
return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->size();
|
||||
}
|
||||
else
|
||||
{
|
||||
if(useDistanceL1_)
|
||||
{
|
||||
return ((const rtflann::Index<rtflann::L1<float> >*)index_)->size();
|
||||
}
|
||||
else
|
||||
{
|
||||
return ((const rtflann::Index<rtflann::L2<float> >*)index_)->size();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// return KB
|
||||
unsigned int memoryUsed() const
|
||||
{
|
||||
if(!index_)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
if(featuresType_ == CV_8UC1)
|
||||
{
|
||||
return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->usedMemory()/1000;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(useDistanceL1_)
|
||||
{
|
||||
return ((const rtflann::Index<rtflann::L1<float> >*)index_)->usedMemory()/1000;
|
||||
}
|
||||
else
|
||||
{
|
||||
return ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory()/1000;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Note that useDistanceL1 doesn't have any effect if LSH is used
|
||||
void build(
|
||||
const cv::Mat & features,
|
||||
const rtflann::IndexParams& params,
|
||||
bool useDistanceL1)
|
||||
{
|
||||
this->release();
|
||||
UASSERT(index_ == 0);
|
||||
UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
|
||||
featuresType_ = features.type();
|
||||
featuresDim_ = features.cols;
|
||||
useDistanceL1_ = useDistanceL1;
|
||||
|
||||
if(featuresType_ == CV_8UC1)
|
||||
{
|
||||
rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
|
||||
index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
|
||||
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
|
||||
}
|
||||
else
|
||||
{
|
||||
rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
|
||||
if(useDistanceL1_)
|
||||
{
|
||||
index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
|
||||
((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
|
||||
}
|
||||
else
|
||||
{
|
||||
index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
|
||||
((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
|
||||
}
|
||||
}
|
||||
|
||||
if(features.rows == 1)
|
||||
{
|
||||
// incremental FLANN
|
||||
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
|
||||
}
|
||||
// else assume that the features are kept in memory outside this class (e.g., dataTree_)
|
||||
|
||||
nextIndex_ = features.rows;
|
||||
}
|
||||
|
||||
bool isBuilt()
|
||||
{
|
||||
return index_!=0;
|
||||
}
|
||||
|
||||
int featuresType() const {return featuresType_;}
|
||||
int featuresDim() const {return featuresDim_;}
|
||||
|
||||
unsigned int addPoint(const cv::Mat & feature)
|
||||
{
|
||||
if(!index_)
|
||||
{
|
||||
UERROR("Flann index not yet created!");
|
||||
return 0;
|
||||
}
|
||||
UASSERT(feature.type() == featuresType_);
|
||||
UASSERT(feature.cols == featuresDim_);
|
||||
UASSERT(feature.rows == 1);
|
||||
if(featuresType_ == CV_8UC1)
|
||||
{
|
||||
rtflann::Matrix<unsigned char> point(feature.data, feature.rows, feature.cols);
|
||||
rtflann::Index<rtflann::Hamming<unsigned char> > * index = (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
|
||||
index->addPoints(point, 0);
|
||||
// Rebuild index if it doubles in size
|
||||
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
|
||||
{
|
||||
// clean not used features
|
||||
for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
|
||||
{
|
||||
addedDescriptors_.erase(*iter);
|
||||
}
|
||||
removedIndexes_.clear();
|
||||
index->buildIndex();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
rtflann::Matrix<float> point((float*)feature.data, feature.rows, feature.cols);
|
||||
if(useDistanceL1_)
|
||||
{
|
||||
rtflann::Index<rtflann::L1<float> > * index = (rtflann::Index<rtflann::L1<float> >*)index_;
|
||||
index->addPoints(point, 0);
|
||||
// Rebuild index if it doubles in size
|
||||
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
|
||||
{
|
||||
// clean not used features
|
||||
for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
|
||||
{
|
||||
addedDescriptors_.erase(*iter);
|
||||
}
|
||||
removedIndexes_.clear();
|
||||
index->buildIndex();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
rtflann::Index<rtflann::L2<float> > * index = (rtflann::Index<rtflann::L2<float> >*)index_;
|
||||
index->addPoints(point, 0);
|
||||
// Rebuild index if it doubles in size
|
||||
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
|
||||
{
|
||||
// clean not used features
|
||||
for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
|
||||
{
|
||||
addedDescriptors_.erase(*iter);
|
||||
}
|
||||
removedIndexes_.clear();
|
||||
index->buildIndex();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
addedDescriptors_.insert(std::make_pair(nextIndex_, feature));
|
||||
|
||||
return nextIndex_++;
|
||||
}
|
||||
|
||||
void removePoint(unsigned int index)
|
||||
{
|
||||
if(!index_)
|
||||
{
|
||||
UERROR("Flann index not yet created!");
|
||||
return;
|
||||
}
|
||||
|
||||
// If a Segmentation fault occurs in removePoint(), verify that you have this fix in your installed "flann/algorithms/nn_index.h":
|
||||
// 707 - if (ids_[id]==id) {
|
||||
// 707 + if (id < ids_.size() && ids_[id]==id) {
|
||||
// ref: https://github.com/mariusmuja/flann/commit/23051820b2314f07cf40ba633a4067782a982ff3#diff-33762b7383f957c2df17301639af5151
|
||||
|
||||
if(featuresType_ == CV_8UC1)
|
||||
{
|
||||
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->removePoint(index);
|
||||
}
|
||||
else if(useDistanceL1_)
|
||||
{
|
||||
((rtflann::Index<rtflann::L1<float> >*)index_)->removePoint(index);
|
||||
}
|
||||
else
|
||||
{
|
||||
((rtflann::Index<rtflann::L2<float> >*)index_)->removePoint(index);
|
||||
}
|
||||
|
||||
removedIndexes_.push_back(index);
|
||||
}
|
||||
|
||||
void knnSearch(
|
||||
const cv::Mat & query,
|
||||
cv::Mat & indices,
|
||||
cv::Mat & dists,
|
||||
int knn,
|
||||
const rtflann::SearchParams& params=rtflann::SearchParams())
|
||||
{
|
||||
if(!index_)
|
||||
{
|
||||
UERROR("Flann index not yet created!");
|
||||
return;
|
||||
}
|
||||
indices.create(query.rows, knn, CV_32S);
|
||||
dists.create(query.rows, knn, featuresType_ == CV_8UC1?CV_32S:CV_32F);
|
||||
|
||||
rtflann::Matrix<int> indicesF((int*)indices.data, indices.rows, indices.cols);
|
||||
|
||||
if(featuresType_ == CV_8UC1)
|
||||
{
|
||||
rtflann::Matrix<unsigned int> distsF((unsigned int*)dists.data, dists.rows, dists.cols);
|
||||
rtflann::Matrix<unsigned char> queryF(query.data, query.rows, query.cols);
|
||||
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
|
||||
}
|
||||
else
|
||||
{
|
||||
rtflann::Matrix<float> distsF((float*)dists.data, dists.rows, dists.cols);
|
||||
rtflann::Matrix<float> queryF((float*)query.data, query.rows, query.cols);
|
||||
if(useDistanceL1_)
|
||||
{
|
||||
((rtflann::Index<rtflann::L1<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
|
||||
}
|
||||
else
|
||||
{
|
||||
((rtflann::Index<rtflann::L2<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
void * index_;
|
||||
unsigned int nextIndex_;
|
||||
int featuresType_;
|
||||
int featuresDim_;
|
||||
bool isLSH_;
|
||||
bool useDistanceL1_; // true=EUCLEDIAN_L2 false=MANHATTAN_L1
|
||||
|
||||
// keep feature in memory until the tree is rebuilt
|
||||
// (in case the word is deleted when removed from the VWDictionary)
|
||||
std::map<int, cv::Mat> addedDescriptors_;
|
||||
std::list<int> removedIndexes_;
|
||||
};
|
||||
|
||||
const int VWDictionary::ID_START = 1;
|
||||
const int VWDictionary::ID_INVALID = 0;
|
||||
|
||||
@@ -652,15 +358,15 @@ void VWDictionary::update()
|
||||
switch(_strategy)
|
||||
{
|
||||
case kNNFlannNaive:
|
||||
_flannIndex->build(descriptor, rtflann::LinearIndexParams(), useDistanceL1_);
|
||||
_flannIndex->buildLinearIndex(descriptor, useDistanceL1_);
|
||||
break;
|
||||
case kNNFlannKdTree:
|
||||
UASSERT_MSG(descriptor.type() == CV_32F, "To use KdTree dictionary, float descriptors are required!");
|
||||
_flannIndex->build(descriptor, rtflann::KDTreeIndexParams(), useDistanceL1_);
|
||||
_flannIndex->buildKDTreeIndex(descriptor, 4, useDistanceL1_);
|
||||
break;
|
||||
case kNNFlannLSH:
|
||||
UASSERT_MSG(descriptor.type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
|
||||
_flannIndex->build(descriptor, rtflann::LshIndexParams(12, 20, 2), useDistanceL1_);
|
||||
_flannIndex->buildLSHIndex(descriptor, 12, 20, 2);
|
||||
break;
|
||||
default:
|
||||
UFATAL("Not supposed to be here!");
|
||||
@@ -672,7 +378,7 @@ void VWDictionary::update()
|
||||
{
|
||||
UASSERT(descriptor.cols == _flannIndex->featuresDim());
|
||||
UASSERT(descriptor.type() == _flannIndex->featuresType());
|
||||
index = _flannIndex->addPoint(descriptor);
|
||||
index = _flannIndex->addPoints(descriptor);
|
||||
}
|
||||
std::pair<std::map<int, int>::iterator, bool> inserted;
|
||||
inserted = _mapIndexId.insert(std::pair<int, int>(index, w->id()));
|
||||
@@ -775,15 +481,15 @@ void VWDictionary::update()
|
||||
switch(_strategy)
|
||||
{
|
||||
case kNNFlannNaive:
|
||||
_flannIndex->build(_dataTree, rtflann::LinearIndexParams(), useDistanceL1_);
|
||||
_flannIndex->buildLinearIndex(_dataTree, useDistanceL1_);
|
||||
break;
|
||||
case kNNFlannKdTree:
|
||||
UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
|
||||
_flannIndex->build(_dataTree, rtflann::KDTreeIndexParams(), useDistanceL1_);
|
||||
_flannIndex->buildKDTreeIndex(_dataTree, useDistanceL1_);
|
||||
break;
|
||||
case kNNFlannLSH:
|
||||
UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
|
||||
_flannIndex->build(_dataTree, rtflann::LshIndexParams(12, 20, 2), useDistanceL1_);
|
||||
_flannIndex->buildLSHIndex(_dataTree, 12, 20, 2);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
||||
Reference in New Issue
Block a user