Refactored FlannIndex: Moved class into its own include file. Added radiusSearch() and buildKDtreeSingleIndex() methods. If dimensions <= 3 and float, use L2_Simple distance type.

corelib/include/rtabmap/core/FlannIndex.h

@@ -0,0 +1,114 @@
+/*
+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.
+*/
+
+#ifndef CORELIB_SRC_FLANNINDEX_H_
+#define CORELIB_SRC_FLANNINDEX_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/opencv.hpp>
+
+namespace rtabmap {
+
+class RTABMAP_EXP FlannIndex
+{
+public:
+	FlannIndex();
+	virtual ~FlannIndex();
+
+	void release();
+	unsigned int indexedFeatures() const;
+
+	// return KB
+	unsigned int memoryUsed() const;
+
+	// Note that useDistanceL1 doesn't have any effect if LSH is used
+	void buildLinearIndex(
+			const cv::Mat & features,
+			bool useDistanceL1 = false);
+	void buildKDTreeIndex(
+			const cv::Mat & features,
+			int trees = 4,
+			bool useDistanceL1 = false);
+	void buildKDTreeSingleIndex(
+			const cv::Mat & features,
+			int leafMaxSize = 10,
+			bool reorder = true,
+			bool useDistanceL1 = false);
+	void buildLSHIndex(
+			const cv::Mat & features,
+			unsigned int table_number = 12,
+			unsigned int key_size = 20,
+			unsigned int multi_probe_level = 2);
+
+	bool isBuilt();
+
+	int featuresType() const {return featuresType_;}
+	int featuresDim() const {return featuresDim_;}
+
+	unsigned int addPoints(const cv::Mat & features);
+
+	void removePoint(unsigned int index);
+
+	// return squared distances
+	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;
+
+	// return squared distances
+	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;
+
+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_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* CORELIB_SRC_FLANNINDEX_H_ */

corelib/src/CMakeLists.txt

@@ -69,6 +69,7 @@ SET(SRC_FILES
 		
 	rtflann/ext/lz4.c
 	rtflann/ext/lz4hc.c
+	FlannIndex.cpp
 	
 	sqlite3/sqlite3.c	
 )

corelib/src/FlannIndex.cpp

@@ -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 */

corelib/src/VWDictionary.cpp

@@ -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;