mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-04 17:17:47 +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
@@ -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