Add FlannIndex abstract interface and implement NanoFlannIndex subclass (#1744)

* Add FlannIndex abstract interface and implement NanoFlannIndex subclass * Refactored: made NanoFlann a new NN type instead of inheriting FlannIndex. Added tests. Vendoring nanoflann.h directly in the repo. RegistrationVis now use NANOFLANN_INDEX_KDTREE_SINGLE (instead of FLANN_INDEX_KDTREE_SINGLE) flann index for 2d points matching. * cleanup comments, added FlannIndex doxygen * Fixing windows tests * updating flaky test * Simplified interface, added flann kdtree single approach selectable by parameters. * RegVis: symmetry of nanoflann for two branches of guess feature matching * cv::BFMatcher baseline * Small cmake optimization FLANN_KDTREE_MEM_OPT only defined for FlannIndex * Refactored where FLANN_KDTREE_MEM_OPT is defined * fixed file name already exist * cleanup * fixup build --------- Co-authored-by: matlabbe <[email protected]>
2026-10-06 18:17:47 +08:00 · 2026-08-16 09:51:39 -07:00
parent df52523a0c
commit f647014f54
28 changed files with 7537 additions and 179 deletions
@@ -131,7 +131,8 @@ SET(SRC_FILES
    rtflann/ext/lz4.c
    rtflann/ext/lz4hc.c
    FlannIndex.cpp
-    
+    nanoflann/NanoFlannIndex.cpp
+
    #clams stuff
    clams/discrete_depth_distortion_model_helpers.cpp
    clams/discrete_depth_distortion_model.cpp
@@ -872,6 +873,14 @@ add_definitions(${PCL_DEFINITIONS})

 # Add binary that is built from the source file "main.cpp".
 # The extension is automatically found.
+# rtflann's config.h is generated, as it is upstream, so that the
+# FLANN_KDTREE_MEM_OPT option travels in a header instead of a compile
+# definition: toggling it then recompiles the sources including rtflann rather
+# than every object file of the library (see the header for the details).
+configure_file(
+        ${CMAKE_CURRENT_SOURCE_DIR}/rtflann/config.h.in
+        ${CMAKE_CURRENT_BINARY_DIR}/rtflann/config.h)
+
 ADD_LIBRARY(rtabmap_core ${SRC_FILES} ${RESOURCES_HEADERS})
 ADD_LIBRARY(rtabmap::core ALIAS rtabmap_core)

@@ -34,12 +34,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Parameters.h>

 #include "rtflann/flann.hpp"
+#include "nanoflann/NanoFlannIndex.h"
 #include <boost/crc.hpp>

 namespace rtabmap {

 FlannIndex::FlannIndex():
 		index_(0),
+		nanoIndex_(0),
 		nextIndex_(0),
 		featuresType_(0),
 		featuresDim_(0),
@@ -54,6 +56,13 @@ FlannIndex::~FlannIndex()

 void FlannIndex::release()
 {
+	if(nanoIndex_)
+	{
+		UDEBUG("Clearing nanoflann index...");
+		delete nanoIndex_;
+		nanoIndex_ = 0;
+		UDEBUG("Clearing nanoflann index... done!");
+	}
 	if(index_)
 	{
 		UDEBUG("Clearing flann index...");
@@ -86,7 +95,112 @@ void FlannIndex::release()

 #define FLANN_INDEX_HEADER_SIZE 12

+// The rebalancing factor is turned into the fraction of removed features an
+// index is allowed to hold before being rebuilt. A factor of 2 used to mean
+// "rebuild once the index has doubled in size", it now means "rebuild once half
+// of it has been removed": growing an index doesn't degrade it enough to be
+// worth a rebuild, removing from it does, as removed features are only marked
+// as such and stay in the index until it is rebuilt (see
+// corelib/test/test_flann_index.cpp). This is nanoflann's alpha_deleted, which
+// both backends now share.
+static float removedRatioThreshold(float rebalancingFactor)
+{
+	if(rebalancingFactor <= 1.0f)
+	{
+		return 1.0f; // never rebuilt, a ratio of 1 is never reached
+	}
+	return (rebalancingFactor-1.0f)/rebalancingFactor;
+}
+
+template<class T>
+static bool needsRebuild(const T * index, float removedRatio)
+{
+	const size_t total = index->size() + index->removedCount();
+	return removedRatio < 1.0f &&
+		   total > 0 &&
+		   float(index->removedCount()) > removedRatio * float(total);
+}
+
+static bool isNanoFlannAlgorithm(FlannIndex::flann_algorithm_t algorithm)
+{
+	return algorithm == FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE;
+}
+
+static unsigned int computeCrc(const cv::Mat & data)
+{
+	boost::crc_32_type result;
+	result.process_bytes(data.data, data.total()*data.elemSize());
+	return result.checksum();
+}
+
+// Fill the header prefixing a serialized index. Shared by both backends: the
+// same fields are checked back by loadIndex() whichever one produced the index.
+static void fillIndexHeader(
+		int * header,
+		int algorithm,
+		int featuresDim,
+		bool useDistanceL1,
+		float rebalancingFactor, // Deprecated
+		int dataRows,
+		int dataCols,
+		int dataType,
+		unsigned int crcValue,
+		int indexSize)
+{
+	int rebalancingFactorAsInt; // Deprecated
+	memcpy(&rebalancingFactorAsInt, &rebalancingFactor, sizeof(rebalancingFactor)); // Deprecated
+	int crcValueAsInt;
+	memcpy(&crcValueAsInt, &crcValue, sizeof(crcValue));
+	// Not checked on load: kept so that a later change of the format, adding or
+	// removing a field, can tell which one it is reading.
+	header[0] = RTABMAP_VERSION_MAJOR;
+	header[1] = RTABMAP_VERSION_MINOR;
+	header[2] = RTABMAP_VERSION_PATCH;
+	header[3] = algorithm;
+	header[4] = featuresDim;
+	header[5] = useDistanceL1?1:0;
+	header[6] = rebalancingFactorAsInt; // Deprecated
+	header[7] = dataRows;
+	header[8] = dataCols;
+	header[9] = dataType;
+	header[10] = crcValueAsInt;
+	header[11] = indexSize;
+	UDEBUG("Header: \"%d.%d.%d\" alg=%d dim=%d L1=%d factor=%f data(%dx%d type=%d, crc=%X) %d",
+		header[0],header[1],header[2],
+		header[3],
+		header[4],
+		header[5],
+		rebalancingFactor, // Deprecated
+		header[7], header[8], header[9], crcValue,
+		header[11]);
+}
+
 std::vector<unsigned char> FlannIndex::serializeIndex(bool computeChecksum) const {
+	if(nanoIndex_)
+	{
+		std::vector<unsigned char> nanoIndexData = nanoIndex_->serializeIndex();
+		if(!nanoIndexData.empty())
+		{
+			const size_t headerSizeBytes = sizeof(int)*FLANN_INDEX_HEADER_SIZE;
+			const cv::Mat dataset = nanoIndex_->indexedPoints();
+			std::vector<unsigned char> indexData(headerSizeBytes + nanoIndexData.size());
+			int header[FLANN_INDEX_HEADER_SIZE];
+			fillIndexHeader(header,
+				algorithm_,
+				featuresDim_,
+				useDistanceL1_,
+				rebalancingFactor_,
+				dataset.rows,
+				dataset.cols,
+				dataset.type(),
+				computeChecksum?computeCrc(dataset):0,
+				(int)nanoIndexData.size());
+			memcpy(indexData.data(), header, headerSizeBytes);
+			memcpy(indexData.data()+headerSizeBytes, nanoIndexData.data(), nanoIndexData.size());
+			return indexData;
+		}
+		return std::vector<unsigned char>();
+	}
 	if(index_ && !addedDescriptors_.empty())
 	{
 #ifdef WIN32
@@ -147,6 +261,10 @@ std::vector<unsigned char> FlannIndex::serializeIndex(bool computeChecksum) cons
 			if(computeChecksum){
 				removedDescriptors.insert(removedIndexes_.begin(), removedIndexes_.end());
 			}
+			// A descriptor header can cover more than one point (see the end of
+			// buildIndex() and addPoints()), the index of its row r being
+			// iter.first+r. addedDescriptors_ is sorted by index, so walking it
+			// gives the points back in the order they were added.
 			for(const auto & iter: addedDescriptors_)
 			{
 				UASSERT(!iter.second.empty());
@@ -163,59 +281,43 @@ std::vector<unsigned char> FlannIndex::serializeIndex(bool computeChecksum) cons
 				else {
 					UASSERT(dataType == iter.second.type());
 				}
-				if(computeChecksum){
-					if(removedDescriptors.find(iter.first) == removedDescriptors.end()) {
-						if(dataset.empty()) {
-							dataset = iter.second.clone();
-						}
-						else {
-							dataset.push_back(iter.second);
-						}
-					}
-					else {
-						dataRows -= iter.second.rows;
-					}
-				}
 			}
-			if(!computeChecksum) {
-				for(const auto & index: removedIndexes_)
+			// Each removed index is one point, whatever the headers cover.
+			dataRows -= (int)removedIndexes_.size();
+
+			if(computeChecksum && dataRows > 0) {
+				// The checksum is compared against the one of the features
+				// given back to loadIndex(), so it is computed over the points
+				// still indexed, in the same order.
+				dataset.create(dataRows, dataCols, dataType);
+				int row = 0;
+				for(const auto & iter: addedDescriptors_)
 				{
-					dataRows -= addedDescriptors_.at(index).rows;
+					for(int r=0; r<iter.second.rows; ++r)
+					{
+						if(removedDescriptors.find(iter.first+r) == removedDescriptors.end())
+						{
+							UASSERT(row < dataRows);
+							iter.second.row(r).copyTo(dataset.row(row++));
+						}
+					}
 				}
+				UASSERT(row == dataRows);
 			}

-			unsigned int crcValue = 0;
-			if(computeChecksum) {
-				boost::crc_32_type result;
-				result.process_bytes(dataset.data, dataset.total()*dataset.elemSize());
-				crcValue = result.checksum(); 
-			}
-			
 			indexData.resize(bytes_written+headerSizeBytes);
 			indexData.shrink_to_fit();
-			int rebalancingFactorAsInt; // Deprecated
-			memcpy(&rebalancingFactorAsInt, &rebalancingFactor_, sizeof(rebalancingFactor_)); // Deprecated
-			int crcValueAsInt;
-			memcpy(&crcValueAsInt, &crcValue, sizeof(crcValue));
-			int header[FLANN_INDEX_HEADER_SIZE] = {
-					RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH, // 0,1,2
-					algorithm_,                                                          // 3,
-					featuresDim_,                                                        // 4,
-					useDistanceL1_?1:0,                                                  // 5,
-					rebalancingFactorAsInt,                                              // 6, Deprecated
-					dataRows,                                                            // 7,
-					dataCols,                                                            // 8,			
-					dataType,                                                            // 9,
-					crcValueAsInt,                                                       // 10         
-					(int)bytes_written};                                                 // 11
-			UDEBUG("Header: \"%d.%d.%d\" alg=%d dim=%d L1=%d factor=%f data(%dx%d type=%d, crc=%X) %d", 
-				header[0],header[1],header[2],
-				header[3],
-				header[4],
-				header[5],
-				rebalancingFactor_, // Deprecated
-				header[7], header[8], header[9], crcValueAsInt, 
-				header[11]);
+			int header[FLANN_INDEX_HEADER_SIZE];
+			fillIndexHeader(header,
+				algorithm_,
+				featuresDim_,
+				useDistanceL1_,
+				rebalancingFactor_,
+				dataRows,
+				dataCols,
+				dataType,
+				computeChecksum?computeCrc(dataset):0,
+				(int)bytes_written);
 			memcpy(indexData.data(), header, headerSizeBytes);
 			return indexData;
 		}
@@ -230,6 +332,10 @@ std::vector<unsigned char> FlannIndex::serializeIndex(bool computeChecksum) cons

 size_t FlannIndex::indexedFeatures() const
 {
+	if(nanoIndex_)
+	{
+		return nanoIndex_->indexedFeatures();
+	}
 	if(!index_)
 	{
 		return 0;
@@ -258,6 +364,10 @@ size_t FlannIndex::indexedFeatures() const
 // return Bytes
 size_t FlannIndex::memoryUsed() const
 {
+	if(nanoIndex_)
+	{
+		return nanoIndex_->memoryUsed();
+	}
 	if(!index_)
 	{
 		return 0;
@@ -303,6 +413,22 @@ void FlannIndex::buildIndex(
 	rebalancingFactor_ = rebalancingFactor;
 	algorithm_ = algorithm;

+	if(isNanoFlannAlgorithm(algorithm))
+	{
+		// The tree keeps its own copy of the points and rebuilds itself, so
+		// addedDescriptors_ is not used here.
+		nanoIndex_ = new NanoFlannIndex();
+		// Nothing to rebuild for a factor of 1: the tree that cannot be added
+		// to is the cheapest one, and it upgrades itself if points are added
+		// after all.
+		nanoIndex_->buildIndex(
+			features,
+			useDistanceL1_,
+			rebalancingFactor_ > 1.0f,
+			removedRatioThreshold(rebalancingFactor_));
+		return;
+	}
+
 	rtflann::IndexParams params;

 	switch (algorithm)
@@ -384,8 +510,8 @@ bool FlannIndex::loadIndex(
 		algorithm,
 		features,
 		useDistanceL1,
-		rebalancingFactor),
-		error;
+		rebalancingFactor,
+		error);
 }
 bool FlannIndex::loadIndex(
 	const unsigned char * indexData,
@@ -396,16 +522,23 @@ bool FlannIndex::loadIndex(
 	float rebalancingFactor,
 	std::string * error)
 {
-	UASSERT(indexData!=NULL);
 	if(indexDataSize == 0) {
 		UWARN("Trying to load empty index....");
+		if(error) {
+			*error = "Trying to load an empty index.";
+		}
 		return false;
 	}
-
+	UASSERT(indexData!=NULL);
 #ifdef WIN32
-	UERROR("FLANN index deserialization is not yet implemented on Windows. Index cannot be loaded from memory buffer.");
-	return false;
-#else
+	if(!isNanoFlannAlgorithm(algorithm)) {
+		UERROR("FLANN index deserialization is not yet implemented on Windows. Index cannot be loaded from memory buffer.");
+		if(error) {
+			*error = "FLANN index deserialization is not yet implemented on Windows.";
+		}
+		return false;
+	}
+#endif

 	// Check if the features match the expected data from the index
 	size_t headerSizeBytes = sizeof(int)*FLANN_INDEX_HEADER_SIZE;
@@ -451,6 +584,15 @@ bool FlannIndex::loadIndex(
 		}
 		return false;
 	}
+	if(isNanoFlannAlgorithm(algorithm) && (savedRebalancingFactor > 1.0f) != (rebalancingFactor > 1.0f)) {
+		// The factor is what tells the nanoflann structures apart, and they
+		// don't serialize to the same thing.
+		if(error) {
+			*error = uFormat("Serialized index was built with a rebalancing factor of %f, which doesn't select the same structure as %f.",
+				savedRebalancingFactor, rebalancingFactor);
+		}
+		return false;
+	}
 	if(savedDistanceL1 != useDistanceL1) {
 		if(error) {
 			*error = uFormat("Serialized \"use distance L1\" (%s) doesn't match the expected one (%s).", savedDistanceL1?"true":"false", useDistanceL1?"true":"false");
@@ -514,6 +656,28 @@ bool FlannIndex::loadIndex(

 	UDEBUG("algorithm=%d", (int)algorithm);

+	if(isNanoFlannAlgorithm(algorithm))
+	{
+		nanoIndex_ = new NanoFlannIndex();
+		if(!nanoIndex_->loadIndex(
+				features,
+				useDistanceL1_,
+				rebalancingFactor_ > 1.0f,
+				indexData+headerSizeBytes,
+				indexDataSize-headerSizeBytes,
+				removedRatioThreshold(rebalancingFactor_),
+				10,
+				error))
+		{
+			this->release();
+			return false;
+		}
+		return true;
+	}
+
+#ifdef WIN32
+	return false; // rtflann deserialization is not implemented on Windows, rejected above
+#else
 	rtflann::IndexParams params;

 	switch (algorithm)
@@ -587,11 +751,15 @@ bool FlannIndex::loadIndex(

 bool FlannIndex::isBuilt()
 {
-	return index_!=0;
+	return index_!=0 || nanoIndex_!=0;
 }

 std::vector<unsigned int> FlannIndex::addPoints(const cv::Mat & features)
 {
+	if(nanoIndex_)
+	{
+		return nanoIndex_->addPoints(features);
+	}
 	if(!index_)
 	{
 		UERROR("Flann index not yet created!");
@@ -601,16 +769,16 @@ std::vector<unsigned int> FlannIndex::addPoints(const cv::Mat & features)
 	UASSERT(features.cols == featuresDim_);
 	bool indexRebuilt = false;
 	size_t removedPts = 0;
+	const float removedRatio = removedRatioThreshold(rebalancingFactor_);
 	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_;
 		removedPts = index->removedCount();
 		index->addPoints(points, 0);
-		// Rebuild index if it is now X times in size
-		if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
+		if(needsRebuild(index, removedRatio))
 		{
-			UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
+			UDEBUG("Rebuilding FLANN index: %d removed of %d", (int)index->removedCount(), (int)(index->size()+index->removedCount()));
 			index->buildIndex();
 		}
 		// if no more removed points, the index has been rebuilt
@@ -624,10 +792,9 @@ std::vector<unsigned int> FlannIndex::addPoints(const cv::Mat & features)
 			rtflann::Index<rtflann::L1<float> > * index = (rtflann::Index<rtflann::L1<float> >*)index_;
 			removedPts = index->removedCount();
 			index->addPoints(points, 0);
-			// Rebuild index if it doubles in size
-			if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
+			if(needsRebuild(index, removedRatio))
 			{
-				UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
+				UDEBUG("Rebuilding FLANN index: %d removed of %d", (int)index->removedCount(), (int)(index->size()+index->removedCount()));
 				index->buildIndex();
 			}
 			// if no more removed points, the index has been rebuilt
@@ -638,10 +805,9 @@ std::vector<unsigned int> FlannIndex::addPoints(const cv::Mat & features)
 			rtflann::Index<rtflann::L2_Simple<float> > * index = (rtflann::Index<rtflann::L2_Simple<float> >*)index_;
 			removedPts = index->removedCount();
 			index->addPoints(points, 0);
-			// Rebuild index if it doubles in size
-			if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
+			if(needsRebuild(index, removedRatio))
 			{
-				UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
+				UDEBUG("Rebuilding FLANN index: %d removed of %d", (int)index->removedCount(), (int)(index->size()+index->removedCount()));
 				index->buildIndex();
 			}
 			// if no more removed points, the index has been rebuilt
@@ -652,10 +818,9 @@ std::vector<unsigned int> FlannIndex::addPoints(const cv::Mat & features)
 			rtflann::Index<rtflann::L2<float> > * index = (rtflann::Index<rtflann::L2<float> >*)index_;
 			removedPts = index->removedCount();
 			index->addPoints(points, 0);
-			// Rebuild index if it doubles in size
-			if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
+			if(needsRebuild(index, removedRatio))
 			{
-				UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
+				UDEBUG("Rebuilding FLANN index: %d removed of %d", (int)index->removedCount(), (int)(index->size()+index->removedCount()));
 				index->buildIndex();
 			}
 			// if no more removed points, the index has been rebuilt
@@ -674,13 +839,27 @@ std::vector<unsigned int> FlannIndex::addPoints(const cv::Mat & features)
 		removedIndexes_.clear();
 	}

-	// incremental FLANN: we should add all headers separately in case we remove
-	// some indexes (to keep underlying matrix data allocated)
 	std::vector<unsigned int> indexes;
+	indexes.reserve(features.rows);
 	for(int i=0; i<features.rows; ++i)
 	{
-		indexes.push_back(nextIndex_);
-		addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
+		indexes.push_back(nextIndex_ + i);
+	}
+
+	// incremental FLANN: we should add all headers separately in case we remove
+	// some indexes (to keep underlying matrix data allocated)
+	if(rebalancingFactor_ > 1.0f)
+	{
+		for(int i=0; i<features.rows; ++i)
+		{
+			addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
+		}
+	}
+	else
+	{
+		// tree won't ever be rebalanced, so just keep only one header for the data
+		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+		nextIndex_ += features.rows;
 	}

 	return indexes;
@@ -688,6 +867,11 @@ std::vector<unsigned int> FlannIndex::addPoints(const cv::Mat & features)

 void FlannIndex::removePoint(unsigned int index)
 {
+	if(nanoIndex_)
+	{
+		nanoIndex_->removePoint(index);
+		return;
+	}
 	if(!index_)
 	{
 		UERROR("Flann index not yet created!");
@@ -728,6 +912,12 @@ void FlannIndex::knnSearch(
 		float eps,
 		bool sorted) const
 {
+	if(nanoIndex_)
+	{
+		// exact search, "checks", "eps" and "sorted" don't apply
+		nanoIndex_->knnSearch(query, indices, dists, knn);
+		return;
+	}
 	if(!index_)
 	{
 		UERROR("Flann index not yet created!");
@@ -767,10 +957,12 @@ void FlannIndex::knnSearch(

 	indices.create(query.rows, knn, CV_32S);
 	int * ptr = indices.ptr<int>();
-	for(size_t i=0 ; i<indicesBuffer.size(); i+=2)
+	for(size_t i=0 ; i<indicesBuffer.size(); ++i)
 	{
+		// Note: this loop used to write two entries per iteration, which read
+		// and wrote one past the end when query.rows*knn is odd (an odd knn on
+		// an odd number of queries).
 		ptr[i] = indicesBuffer[i] == std::numeric_limits<size_t>::max()?-1:(int)indicesBuffer[i];
-		ptr[i+1] = indicesBuffer[i+1] == std::numeric_limits<size_t>::max()?-1:(int)indicesBuffer[i+1];
 	}
 }

@@ -784,6 +976,12 @@ void FlannIndex::radiusSearch(
 		float eps,
 		bool sorted) const
 {
+	if(nanoIndex_)
+	{
+		// "checks" doesn't apply
+		nanoIndex_->radiusSearch(query, indices, dists, radius, maxNeighbors, eps, sorted);
+		return;
+	}
 	if(!index_)
 	{
 		UERROR("Flann index not yet created!");
@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/VisualWord.h>
 #include <rtabmap/core/Optimizer.h>
 #include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/FlannIndex.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UStl.h>
@@ -56,7 +57,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/cudaimgproc.hpp>
 #endif

-#include <rtflann/flann.hpp>


 #ifdef RTABMAP_PYTHON
@@ -65,6 +65,52 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 namespace rtabmap {

+// The dictionary strategy a Vis/CorNNType value stands for. Vis/CorNNType
+// shares the values of Kp/NNStrategy for the strategies the dictionary
+// implements, and extends them with matching approaches of its own, hence the
+// mapping. Return VWDictionary::kNNUndef for the values RegistrationVis handles
+// itself (BruteForceCrossCheck, SuperGlue, GMS).
+static VWDictionary::NNStrategy nnStrategyFromCorNNType(int nnType)
+{
+	// 0 to 4 are the dictionary strategies themselves, 5, 6 and 7 are the
+	// approaches RegistrationVis implements (BruteForceCrossCheck, SuperGlue
+	// and GMS), and the ones after them are dictionary strategies again, at an
+	// offset of the three above.
+	if(nnType >= 0 && nnType <= VWDictionary::kNNBruteForceGPU)
+	{
+		return (VWDictionary::NNStrategy)nnType;
+	}
+	if(nnType > 7)
+	{
+		const int strategy = nnType - 3;
+		if(strategy < VWDictionary::kNNUndef)
+		{
+			return (VWDictionary::NNStrategy)strategy;
+		}
+	}
+	return VWDictionary::kNNUndef;
+}
+
+std::string RegistrationVis::getNNTypeName(int nnType)
+{
+	const VWDictionary::NNStrategy strategy = nnStrategyFromCorNNType(nnType);
+	if(strategy != VWDictionary::kNNUndef)
+	{
+		return VWDictionary::nnStrategyName(strategy);
+	}
+	switch(nnType)
+	{
+	case 5:
+		return "BRUTE FORCE CROSS CHECK";
+	case 6:
+		return "PY MATCHER";
+	case 7:
+		return "GMS";
+	default:
+		return "Unknown";
+	}
+}
+
 RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration * child) :
 		Registration(parameters, child),
 		_minInliers(Parameters::defaultVisMinInliers()),
@@ -123,6 +169,12 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpGridRows(), _featureParameters.at(Parameters::kVisGridRows())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpGridCols(), _featureParameters.at(Parameters::kVisGridCols())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
+	// The dictionary used to match descriptors (see computeTransformationImpl())
+	// is built once and searched once, then thrown away: the words added while
+	// searching it are never indexed. Nothing is gained by keeping its index
+	// ready to be added to, and the bookkeeping that needs costs a descriptor
+	// reference per feature on every registration.
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpIncrementalFlann(), "false"));

 	this->parseParameters(parameters);
 }
@@ -237,9 +289,10 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)

 	if(uContains(parameters, Parameters::kVisCorNNType()))
 	{
-		if(_nnType<VWDictionary::kNNUndef)
+		const VWDictionary::NNStrategy strategy = nnStrategyFromCorNNType(_nnType);
+		if(strategy != VWDictionary::kNNUndef)
 		{
-			uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_nnType)));
+			uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str((int)strategy)));
 		}
 	}
 	if(uContains(parameters, Parameters::kVisCorNNDR()))
@@ -1082,24 +1135,27 @@ Transform RegistrationVis::computeTransformationImpl(
 						if(_guessMatchToProjection)
 						{
 							UDEBUG("match frame to projected");
-							// Create kd-tree for projected keypoints
-							rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
-							rtflann::Index<rtflann::L2_Simple<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
-							index.buildIndex();
+							// Index the projected keypoints. A rebalancing factor of 1:
+							// the index is thrown away with the frame, nothing is ever
+							// added to or removed from it. cv::Point2f being two floats,
+							// the points are indexed where they are.
+							cv::Mat cornersProjectedMat((int)cornersProjected.size(), 2, CV_32FC1, (void*)cornersProjected.data());
+							FlannIndex flannIndex;
+							flannIndex.buildIndex(FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, cornersProjectedMat, false, 1.0f);

 							std::vector< std::vector<size_t> > indices;
 							std::vector<std::vector<float> > dists;
 							float radius = (float)_guessWinSize; // pixels
 							std::vector<cv::Point2f> pointsTo;
 							cv::KeyPoint::convert(kptsTo, pointsTo);
-							rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
-							index.radiusSearch(pointsToMat, indices, dists, radius*radius, rtflann::SearchParams());
+							cv::Mat pointsToMat((int)pointsTo.size(), 2, CV_32FC1, (void*)pointsTo.data());
+							flannIndex.radiusSearch(pointsToMat, indices, dists, radius);

-							UASSERT(indices.size() == pointsToMat.rows);
+							UASSERT(indices.size() == (size_t)pointsToMat.rows);
 							UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
 							UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
 							UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
-							UASSERT(pointsToMat.rows == kptsTo.size());
+							UASSERT(pointsToMat.rows == (int)kptsTo.size());
 							UDEBUG("radius search done for guess");

 							// Process results (Nearest Neighbor Distance Ratio)
@@ -1107,9 +1163,21 @@ Transform RegistrationVis::computeTransformationImpl(
 							std::map<int,int> addedWordsFrom; //<id, index>
 							std::map<int, int> duplicates; //<fromId, toId>
 							int newWords = 0;
+							// The projected words that a keypoint of the frame was found
+							// near, as the other branch collects them: several keypoints
+							// can be near the same one, hence the set. OdometryF2M uses
+							// them to know which words of its map are still seen.
+							std::set<int> projectedIDs;
 							cv::Mat descriptors(10, descriptorsTo.cols, descriptorsTo.type());
-							for(unsigned int i = 0; i < pointsToMat.rows; ++i)
+							for(int i = 0; i < pointsToMat.rows; ++i)
 							{
+								for(unsigned int j=0; j<indices[i].size(); ++j)
+								{
+									const int projectedIndexFrom = projectedIndexToDescIndex[indices[i].at(j)];
+									projectedIDs.insert(!orignalWordsFromIds.empty()?
+											orignalWordsFromIds[projectedIndexFrom]:projectedIndexFrom);
+								}
+
 								int matchedIndex = -1;
 								if(indices[i].size() >= 2)
 								{
@@ -1200,9 +1268,10 @@ Transform RegistrationVis::computeTransformationImpl(
 									++newWords;
 								}
 							}
-							UDEBUG("addedWordsFrom=%d/%d (duplicates=%d, newWords=%d), kptsTo=%d, wordsTo=%d, words3From=%d",
+							info.projectedIDs = std::vector<int>(projectedIDs.begin(), projectedIDs.end());
+							UDEBUG("addedWordsFrom=%d/%d (duplicates=%d, newWords=%d), kptsTo=%d, wordsTo=%d, words3From=%d, projectedIDs=%d",
 								(int)addedWordsFrom.size(), (int)cornersProjected.size(), (int)duplicates.size(), newWords,
-								(int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size());
+								(int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size(), (int)info.projectedIDs.size());

 							// create fake ids for not matched words from "from"
 							int addWordsFromNotMatched = 0;
@@ -1224,25 +1293,29 @@ Transform RegistrationVis::computeTransformationImpl(
 						else
 						{
 							UDEBUG("match projected to frame");
+							// Index the frame's keypoints. A rebalancing factor of 1:
+							// the index is thrown away with the frame, nothing is ever
+							// added to or removed from it. cv::Point2f being two floats,
+							// the points are indexed where they are.
 							std::vector<cv::Point2f> pointsTo;
 							cv::KeyPoint::convert(kptsTo, pointsTo);
-							rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
-							rtflann::Index<rtflann::L2_Simple<float> > index(pointsToMat, rtflann::KDTreeIndexParams());
-							index.buildIndex();
+							cv::Mat pointsToMat((int)pointsTo.size(), 2, CV_32FC1, (void*)pointsTo.data());
+							FlannIndex flannIndex;
+							flannIndex.buildIndex(FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, pointsToMat, false, 1.0f);

-							std::vector< std::vector<size_t> > indices;
-							std::vector<std::vector<float> > dists;
+							cv::Mat queryMat((int)cornersProjected.size(), 2, CV_32FC1, (void*)cornersProjected.data());
+
+							std::vector<std::vector<size_t>> indices;
+							std::vector<std::vector<float>> dists;
 							float radius = (float)_guessWinSize; // pixels
-							rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
-							index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams(32, 0, false));

-							UASSERT(indices.size() == cornersProjectedMat.rows);
-							UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
-							UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
+							flannIndex.radiusSearch(queryMat, indices, dists, radius, 0, 32, 0.0, false);
+
+							UASSERT(indices.size() == cornersProjected.size());
 							UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
-							UASSERT(pointsToMat.rows == kptsTo.size());
+							UASSERT(pointsToMat.rows == (int)kptsTo.size());
 							UDEBUG("radius search done for guess");
-
+							
 							// Process results (Nearest Neighbor Distance Ratio)
 							std::set<int> addedWordsTo;
 							std::set<int> addedWordsFrom;
@@ -1250,7 +1323,7 @@ Transform RegistrationVis::computeTransformationImpl(
 							double bruteForceDescCopy = 0.0;
 							UTimer bruteForceTimer;
 							cv::Mat descriptors(10, descriptorsTo.cols, descriptorsTo.type());
-							for(unsigned int i = 0; i < cornersProjectedMat.rows; ++i)
+							for(unsigned int i = 0; i < cornersProjected.size(); ++i)
 							{
 								int matchedIndexFrom = projectedIndexToDescIndex[i];

@@ -56,6 +56,43 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {

+// Whether the strategy searches with a FlannIndex, as opposed to the brute
+// force ones matching against the _dataTree matrix.
+static bool isFlannStrategy(VWDictionary::NNStrategy strategy)
+{
+	return strategy == VWDictionary::kNNFlannNaive ||
+		   strategy == VWDictionary::kNNFlannKdTree ||
+		   strategy == VWDictionary::kNNFlannLSH ||
+		   strategy == VWDictionary::kNNNanoFlannKdTree ||
+		   strategy == VWDictionary::kNNFlannKdTreeSingle;
+}
+
+// Whether the strategy indexes float descriptors in a kd-tree, in which case
+// binary descriptors have to be converted first.
+static bool isKdTreeStrategy(VWDictionary::NNStrategy strategy)
+{
+	return strategy == VWDictionary::kNNFlannKdTree ||
+		   strategy == VWDictionary::kNNNanoFlannKdTree ||
+		   strategy == VWDictionary::kNNFlannKdTreeSingle;
+}
+
+static FlannIndex::flann_algorithm_t flannAlgorithm(VWDictionary::NNStrategy strategy)
+{
+	switch(strategy)
+	{
+	case VWDictionary::kNNFlannNaive:
+		return FlannIndex::FLANN_INDEX_LINEAR;
+	case VWDictionary::kNNFlannLSH:
+		return FlannIndex::FLANN_INDEX_LSH;
+	case VWDictionary::kNNNanoFlannKdTree:
+		return FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE;
+	case VWDictionary::kNNFlannKdTreeSingle:
+		return FlannIndex::FLANN_INDEX_KDTREE_SINGLE;
+	default:
+		return FlannIndex::FLANN_INDEX_KDTREE; // kNNFlannKdTree
+	}
+}
+
 const int VWDictionary::ID_START = 1;
 const int VWDictionary::ID_INVALID = 0;

@@ -115,7 +152,17 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 		NNStrategy nnStrategy = (NNStrategy)std::atoi((*iter).second.c_str());
 		treeUpdated = this->setNNStrategy(nnStrategy);
 	}
-	if(!treeUpdated && byteToFloat!=_byteToFloat && _strategy == kNNFlannKdTree)
+	if(_strategy == kNNFlannKdTreeSingle && _incrementalDictionary && _incrementalFlann)
+	{
+		UWARN("%s=%d (%s) rebuilds its whole index every time a word is added, which "
+			  "is very slow with %s=true. It is meant for an index built once and "
+			  "searched once, like the one matching the features of two frames.",
+			  Parameters::kKpNNStrategy().c_str(), (int)_strategy,
+			  nnStrategyName(_strategy).c_str(),
+			  Parameters::kKpIncrementalFlann().c_str());
+	}
+
+	if(!treeUpdated && byteToFloat!=_byteToFloat && isKdTreeStrategy(_strategy))
 	{
 		UINFO("KDTree: Binary to Float conversion approach has changed, re-initialize kd-tree.");
 		this->rebuildIndex();
@@ -392,7 +439,7 @@ unsigned long VWDictionary::getMemoryUsed() const
 		memoryUsage += _visualWords.size()*(sizeof(int) + _visualWords.rbegin()->second->getMemoryUsed() + sizeof(std::map<int, VisualWord *>::iterator)) + sizeof(std::map<int, VisualWord *>);
 		if(_dataTree.empty() &&
 			_visualWords.begin()->second->getDescriptor().type() == CV_8U &&
-			_strategy == kNNFlannKdTree)
+			isKdTreeStrategy(_strategy))
 		{
 			// Binary descriptors were converted to float, and not included in _dataTree
 			memoryUsage += _visualWords.size() * _visualWords.begin()->second->getDescriptor().total() * sizeof(float) * (_byteToFloat?1:8);
@@ -507,7 +554,7 @@ void VWDictionary::update()

 		if(!firstUpdate &&
 			_incrementalFlann &&
-		   _strategy < kNNBruteForce &&
+		   isFlannStrategy(_strategy) &&
 		   _visualWords.size())
 		{
 			ULOGGER_DEBUG("Incremental FLANN: Removing %d words...", (int)_removedIndexedWords.size());
@@ -535,7 +582,7 @@ void VWDictionary::update()
 					if(w->getDescriptor().type() == CV_8U)
 					{
 						useDistanceL1_ = true;
-						if(_strategy == kNNFlannKdTree)
+						if(isKdTreeStrategy(_strategy))
 						{
 							descriptor = convertBinTo32F(w->getDescriptor(), _byteToFloat);
 						}
@@ -555,9 +602,7 @@ void VWDictionary::update()
 						UDEBUG("Building FLANN index... (strategy=%s, byteToFloat=%s, useDistanceL1=%s, rebalancingFactor=%f)",
 							nnStrategyName(_strategy).c_str(), _byteToFloat?"true":"false", useDistanceL1_?"true":"false", _rebalancingFactor);
 						_flannIndex->buildIndex(
-							_strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR:
-							_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
-							FlannIndex::FLANN_INDEX_KDTREE, // kNNFlannKdTree
+							flannAlgorithm(_strategy),
 							descriptor, useDistanceL1_, _rebalancingFactor);
 						UDEBUG("Building FLANN index... done!");
 					}
@@ -577,7 +622,7 @@ void VWDictionary::update()
 				ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done! (in %f s)", (int)_notIndexedWords.size(), timer.ticks());
 			}
 		}
-		else if(_strategy >= kNNBruteForce &&
+		else if(!isFlannStrategy(_strategy) &&
 				_notIndexedWords.size() &&
 				_removedIndexedWords.size() == 0 &&
 				_visualWords.size())
@@ -587,8 +632,8 @@ void VWDictionary::update()
 			if(_dataTree.rows >= IMGIDX_ONE)
 			{
 				UWARN("%s=%d is not a FLANN strategy and the number of words in the vocabulary (%d) is over %d (IMGIDX_ONE), so opencv may "
-					"assert on an IMGIDX_ONE check when adding new words. Use a FLANN strategy instead (%s<%d).",
-					Parameters::kKpNNStrategy().c_str(), _strategy, _dataTree.rows, IMGIDX_ONE, Parameters::kKpNNStrategy().c_str(), kNNBruteForce);
+					"assert on an IMGIDX_ONE check when adding new words. Use a FLANN strategy instead (e.g. %s=%d).",
+					Parameters::kKpNNStrategy().c_str(), _strategy, _dataTree.rows, IMGIDX_ONE, Parameters::kKpNNStrategy().c_str(), kNNFlannKdTree);
 			}

 			//just add not indexed words
@@ -633,7 +678,7 @@ void VWDictionary::update()
 				if(_visualWords.begin()->second->getDescriptor().type() == CV_8U)
 				{
 					useDistanceL1_ = true;
-					if(_strategy == kNNFlannKdTree)
+					if(isKdTreeStrategy(_strategy))
 					{
 						type = CV_32F;
 						if(!_byteToFloat)
@@ -662,7 +707,7 @@ void VWDictionary::update()
 					cv::Mat descriptor;
 					if(iter->second->getDescriptor().type() == CV_8U)
 					{
-						if(_strategy == kNNFlannKdTree)
+						if(isKdTreeStrategy(_strategy))
 						{
 							descriptor = convertBinTo32F(iter->second->getDescriptor(), _byteToFloat);
 						}
@@ -687,12 +732,10 @@ void VWDictionary::update()
 				ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",(int)_mapIndexId.size(), (int)_visualWords.size(), dim);
 				ULOGGER_DEBUG("copying data = %f s", timer.ticks());

-				if(_strategy < kNNBruteForce)
+				if(isFlannStrategy(_strategy))
 				{
 					_flannIndex->buildIndex(
-						_strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR:
-							_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
-								FlannIndex::FLANN_INDEX_KDTREE, // kNNFlannKdTree
+						flannAlgorithm(_strategy),
 						_dataTree,
 						useDistanceL1_,
 						_incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
@@ -714,7 +757,7 @@ void VWDictionary::update()

 std::vector<unsigned char> VWDictionary::serializeIndex() const
 {
-	if(_strategy >= kNNBruteForce) {
+	if(!isFlannStrategy(_strategy)) {
 		UINFO("Not flann strategy, ignoring serialization...");
 		return std::vector<unsigned char>();
 	}
@@ -739,7 +782,7 @@ bool VWDictionary::deserializeIndex(const unsigned char * data, size_t size)
 		return false;
 	}
 	UDEBUG("Loading flann index... (data size=%ld bytes)", size);
-	if(_strategy >= kNNBruteForce) {
+	if(!isFlannStrategy(_strategy)) {
 		//ignore
 		return false;
 	}
@@ -772,7 +815,7 @@ bool VWDictionary::deserializeIndex(const unsigned char * data, size_t size)
 	if(_visualWords.begin()->second->getDescriptor().type() == CV_8U)
 	{
 		useDistanceL1_ = true;
-		if(_strategy == kNNFlannKdTree)
+		if(isKdTreeStrategy(_strategy))
 		{
 			type = CV_32F;
 			if(!_byteToFloat)
@@ -801,7 +844,7 @@ bool VWDictionary::deserializeIndex(const unsigned char * data, size_t size)
 		cv::Mat descriptor;
 		if(iter->second->getDescriptor().type() == CV_8U)
 		{
-			if(_strategy == kNNFlannKdTree)
+			if(isKdTreeStrategy(_strategy))
 			{
 				descriptor = convertBinTo32F(iter->second->getDescriptor(), _byteToFloat);
 			}
@@ -830,9 +873,7 @@ bool VWDictionary::deserializeIndex(const unsigned char * data, size_t size)
 	if(_flannIndex->loadIndex(
 		data,
 		size,
-		_strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR:
-			_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
-				FlannIndex::FLANN_INDEX_KDTREE, 
+		flannAlgorithm(_strategy),
 		dataTree, 
 		useDistanceL1_,
 		_incrementalDictionary && _incrementalFlann ? _rebalancingFactor:1,
@@ -975,7 +1016,7 @@ std::list<int> VWDictionary::addNewWords(
 	if(descriptorsIn.type() == CV_8U)
 	{
 		useDistanceL1_ = true;
-		if(_strategy == kNNFlannKdTree)
+		if(isKdTreeStrategy(_strategy))
 		{
 			descriptors = convertBinTo32F(descriptorsIn, _byteToFloat);
 		}
@@ -1031,7 +1072,7 @@ std::list<int> VWDictionary::addNewWords(
 		//Find nearest neighbors
 		UDEBUG("newPts.total()=%d _strategy=%d", descriptors.rows, _strategy);

-		if(_strategy == kNNFlannNaive || _strategy == kNNFlannKdTree || _strategy == kNNFlannLSH)
+		if(isFlannStrategy(_strategy))
 		{
 			_flannIndex->knnSearch(descriptors, results, dists, k, KNN_CHECKS);
 		}
@@ -1308,7 +1349,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 		cv::Mat query;
 		if(queryIn.type() == CV_8U)
 		{
-			if(_strategy == kNNFlannKdTree)
+			if(isKdTreeStrategy(_strategy))
 			{
 				query = convertBinTo32F(queryIn, _byteToFloat);
 			}
@@ -1353,7 +1394,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 			//Find nearest neighbors
 			UDEBUG("query.rows=%d ", query.rows);

-			if(_strategy == kNNFlannNaive || _strategy == kNNFlannKdTree || _strategy == kNNFlannLSH)
+			if(isFlannStrategy(_strategy))
 			{
 				_flannIndex->knnSearch(query, results, dists, k, KNN_CHECKS);
 			}
@@ -1436,7 +1477,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 				cv::Mat descriptor;
 				if(vw->getDescriptor().type() == CV_8U)
 				{
-					if(_strategy == kNNFlannKdTree)
+					if(isKdTreeStrategy(_strategy))
 					{
 						descriptor = convertBinTo32F(vw->getDescriptor(), _byteToFloat);
 					}
@@ -0,0 +1,535 @@
+/*
+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 "nanoflann/NanoFlannIndex.h"
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+
+#include <algorithm>
+
+#include "nanoflann/nanoflann.h"
+
+#include <sstream>
+
+namespace rtabmap {
+
+namespace {
+
+// Points indexed by the tree. nanoflann is zero-copy: it only stores indexes in
+// this container, which holds a pointer to the first coordinate of each point
+// rather than a copy of it, the way rtflann's NNIndex::points_ does. The
+// features they point into are kept alive by "blocks" below. The container is
+// append-only so that the indexes handed out by addPoints() stay valid (removed
+// points leave a hole behind).
+struct PointCloud
+{
+	std::vector<const float*> pts;
+	std::vector<cv::Mat> blocks; // owners of the rows pts points into
+	int dim = 0;
+
+	inline size_t kdtree_get_point_count() const {return pts.size();}
+	inline float kdtree_get_pt(const size_t idx, const size_t d) const {return pts[idx][d];}
+	template <class BBOX> bool kdtree_get_bbox(BBOX & /* bb */) const {return false;}
+};
+
+}
+
+// Type-erases the metric and the compile-time dimension of the tree, so that
+// nanoflann's templates stay in this file.
+class NanoFlannIndexImpl
+{
+public:
+	virtual ~NanoFlannIndexImpl() {}
+
+	// index every point currently in cloud
+	virtual void buildIndex() = 0;
+	virtual bool isIncremental() const = 0;
+
+	// [start, end] indexes of points already appended to cloud
+	virtual void addPoints(size_t start, size_t end) = 0;
+	virtual void removePoint(size_t index) = 0;
+	virtual size_t size() const = 0;
+	virtual size_t usedMemory() const = 0;
+	virtual size_t knnSearch(const float * query, size_t knn, unsigned int * indices, float * dists) const = 0;
+	virtual size_t radiusSearch(
+			const float * query,
+			float radiusSqr,
+			std::vector<nanoflann::ResultItem<unsigned int, float> > & matches,
+			const nanoflann::SearchParameters & params) const = 0;
+	virtual void saveIndex(std::ostream & stream) const = 0;
+	// throws std::runtime_error if the stream doesn't match this instantiation
+	virtual void loadIndex(std::istream & stream) = 0;
+
+	PointCloud cloud;
+};
+
+namespace {
+
+template<class Metric, int32_t DIM>
+class NanoFlannTree : public NanoFlannIndexImpl
+{
+public:
+	// cloud is a base class member, so it is already constructed here. The
+	// incremental tree always starts empty, whatever the dataset holds.
+	NanoFlannTree(int dim, float alphaDeleted) :
+		tree_(dim, cloud, nanoflann::KDTreeIncrementalIndexParams(0.75f, alphaDeleted)) {}
+
+	virtual void buildIndex() override
+	{
+		const size_t count = cloud.kdtree_get_point_count();
+		if(count)
+		{
+			tree_.addPoints(0, (unsigned int)(count-1));
+		}
+	}
+	virtual bool isIncremental() const override {return true;}
+	virtual void addPoints(size_t start, size_t end) override {tree_.addPoints((unsigned int)start, (unsigned int)end);}
+	virtual void removePoint(size_t index) override {tree_.removePoint((unsigned int)index);}
+	virtual size_t size() const override {return tree_.size();}
+	virtual size_t usedMemory() const override {return tree_.usedMemory();}
+	virtual size_t knnSearch(const float * query, size_t knn, unsigned int * indices, float * dists) const override
+	{
+		return tree_.knnSearch(query, knn, indices, dists);
+	}
+	virtual size_t radiusSearch(
+			const float * query,
+			float radiusSqr,
+			std::vector<nanoflann::ResultItem<unsigned int, float> > & matches,
+			const nanoflann::SearchParameters & params) const override
+	{
+		return tree_.radiusSearch(query, radiusSqr, matches, params);
+	}
+	virtual void saveIndex(std::ostream & stream) const override {tree_.saveIndex(stream);}
+	virtual void loadIndex(std::istream & stream) override {tree_.loadIndex(stream);}
+
+private:
+	nanoflann::KDTreeSingleIndexIncrementalAdaptor<Metric, PointCloud, DIM, unsigned int> tree_;
+};
+
+template<class Metric, int32_t DIM>
+class NanoFlannStaticTree : public NanoFlannIndexImpl
+{
+public:
+	// The static tree indexes the dataset as it is when it is built, and cloud
+	// is still empty here: the initial build is skipped, buildIndex() or
+	// loadIndex() is called once the points are in.
+	NanoFlannStaticTree(int dim, size_t leafMaxSize) :
+		tree_(dim, cloud, nanoflann::KDTreeSingleIndexAdaptorParams(
+			leafMaxSize, nanoflann::KDTreeSingleIndexAdaptorFlags::SkipInitialBuildIndex)) {}
+
+	virtual void buildIndex() override {tree_.buildIndex();}
+	virtual bool isIncremental() const override {return false;}
+	virtual void addPoints(size_t, size_t) override {UFATAL("Not supported by the static nanoflann index.");}
+	virtual void removePoint(size_t) override {UFATAL("Not supported by the static nanoflann index.");}
+	// no removed points to exclude
+	virtual size_t size() const override {return cloud.kdtree_get_point_count();}
+	virtual size_t usedMemory() const override {return tree_.usedMemory(tree_);}
+	virtual size_t knnSearch(const float * query, size_t knn, unsigned int * indices, float * dists) const override
+	{
+		return tree_.knnSearch(query, knn, indices, dists);
+	}
+	virtual size_t radiusSearch(
+			const float * query,
+			float radiusSqr,
+			std::vector<nanoflann::ResultItem<unsigned int, float> > & matches,
+			const nanoflann::SearchParameters & params) const override
+	{
+		return tree_.radiusSearch(query, radiusSqr, matches, params);
+	}
+	virtual void saveIndex(std::ostream & stream) const override {tree_.saveIndex(stream);}
+	virtual void loadIndex(std::istream & stream) override {tree_.loadIndex(stream);}
+
+private:
+	nanoflann::KDTreeSingleIndexAdaptor<Metric, PointCloud, DIM, unsigned int> tree_;
+};
+
+// L2_Simple is the metric recommended by nanoflann for 2D and 3D point clouds,
+// L2 (with its partial distance early exit) for the higher dimensions of the
+// descriptors. A compile-time dimension additionally keeps the per-node
+// bounding boxes on the stack, so the two point cloud cases are instantiated
+// with theirs.
+template<template<class, int32_t> class Tree, class ... Args>
+NanoFlannIndexImpl * createTree(int dim, bool useDistanceL1, Args ... args)
+{
+	if(useDistanceL1)
+	{
+		return new Tree<nanoflann::L1_Adaptor<float, PointCloud>, -1>(dim, args...);
+	}
+	if(dim == 2)
+	{
+		return new Tree<nanoflann::L2_Simple_Adaptor<float, PointCloud>, 2>(dim, args...);
+	}
+	if(dim == 3)
+	{
+		return new Tree<nanoflann::L2_Simple_Adaptor<float, PointCloud>, 3>(dim, args...);
+	}
+	return new Tree<nanoflann::L2_Adaptor<float, PointCloud>, -1>(dim, args...);
+}
+
+NanoFlannIndexImpl * createImpl(int dim, bool useDistanceL1, bool incremental, float removedRatio, int leafMaxSize)
+{
+	if(incremental)
+	{
+		// nanoflann's alpha_deleted: the fraction of removed points above which
+		// a subtree is rebuilt, dropping them.
+		return createTree<NanoFlannTree>(dim, useDistanceL1, removedRatio);
+	}
+	UASSERT(leafMaxSize > 0);
+	return createTree<NanoFlannStaticTree>(dim, useDistanceL1, (size_t)leafMaxSize);
+}
+
+}
+
+NanoFlannIndex::NanoFlannIndex() :
+	index_(0),
+	featuresDim_(0),
+	useDistanceL1_(false),
+	removedRatio_(0.5f)
+{
+}
+
+NanoFlannIndex::~NanoFlannIndex()
+{
+	this->release();
+}
+
+void NanoFlannIndex::release()
+{
+	delete index_;
+	index_ = 0;
+	featuresDim_ = 0;
+}
+
+void NanoFlannIndex::buildIndex(
+		const cv::Mat & features,
+		bool useDistanceL1,
+		bool incremental,
+		float removedRatio,
+		int leafMaxSize)
+{
+	this->release();
+
+	UASSERT_MSG(features.type() == CV_32FC1, "Only 32F features are supported by the nanoflann index.");
+	UASSERT(features.cols > 0);
+
+	featuresDim_ = features.cols;
+	useDistanceL1_ = useDistanceL1;
+	removedRatio_ = removedRatio;
+	index_ = createImpl(featuresDim_, useDistanceL1, incremental, removedRatio, leafMaxSize);
+	index_->cloud.dim = featuresDim_;
+
+	this->appendPoints(features);
+	index_->buildIndex();
+}
+
+size_t NanoFlannIndex::indexedFeatures() const
+{
+	return index_?index_->size():0;
+}
+
+// return Bytes
+size_t NanoFlannIndex::memoryUsed() const
+{
+	if(!index_)
+	{
+		return 0;
+	}
+	// Like the rtflann backend, the features themselves are not counted: they
+	// are owned by the caller, only referenced here.
+	return sizeof(NanoFlannIndex) +
+			index_->cloud.pts.capacity() * sizeof(const float*) +
+			index_->cloud.blocks.capacity() * sizeof(cv::Mat) +
+			index_->usedMemory();
+}
+
+// Reference the points at the end of the storage, without indexing them, and
+// return the index of the first one added.
+size_t NanoFlannIndex::appendPoints(const cv::Mat & features)
+{
+	PointCloud & cloud = index_->cloud;
+	const size_t start = cloud.pts.size();
+	if(cloud.pts.capacity() < start + (size_t)features.rows)
+	{
+		// Grow geometrically: reserving exactly what is needed would make every
+		// single point insertion reallocate and copy the whole storage.
+		cloud.pts.reserve(std::max(start + (size_t)features.rows, cloud.pts.capacity()*2));
+	}
+	// Keeping the header alive is what keeps the rows valid, cv::Mat data being
+	// reference counted. One header covers the whole batch.
+	cloud.blocks.push_back(features);
+	for(int i=0; i<features.rows; ++i)
+	{
+		cloud.pts.push_back(features.ptr<float>(i));
+	}
+	return start;
+}
+
+// Swap the tree that is built once for the one that accepts points, keeping
+// the points already indexed and the indexes they were given.
+void NanoFlannIndex::makeIncremental()
+{
+	UDEBUG("Rebuilding the nanoflann index as an incremental one (%d points)",
+		(int)index_->cloud.pts.size());
+
+	const cv::Mat points = this->indexedPoints();
+	delete index_;
+	index_ = createImpl(featuresDim_, useDistanceL1_, true, removedRatio_, 10);
+	index_->cloud.dim = featuresDim_;
+	if(!points.empty())
+	{
+		this->appendPoints(points);
+		index_->buildIndex();
+	}
+}
+
+std::vector<unsigned int> NanoFlannIndex::addPoints(const cv::Mat & features)
+{
+	if(!index_)
+	{
+		UERROR("Nanoflann index not yet created!");
+		return std::vector<unsigned int>();
+	}
+	if(!index_->isIncremental())
+	{
+		// Built as the tree that cannot be added to, but points are added after
+		// all: rebuild it as the one that can.
+		this->makeIncremental();
+	}
+	UASSERT(features.type() == CV_32FC1);
+	UASSERT(features.cols == featuresDim_);
+
+	std::vector<unsigned int> indexes;
+	if(features.rows == 0)
+	{
+		return indexes;
+	}
+
+	const size_t start = this->appendPoints(features);
+	index_->addPoints(start, start + (size_t)features.rows - 1);
+
+	indexes.resize(features.rows);
+	for(size_t i=0; i<indexes.size(); ++i)
+	{
+		indexes[i] = (unsigned int)(start + i);
+	}
+	return indexes;
+}
+
+std::vector<unsigned char> NanoFlannIndex::serializeIndex() const
+{
+	if(!index_)
+	{
+		return std::vector<unsigned char>();
+	}
+	if(index_->size() != index_->cloud.kdtree_get_point_count())
+	{
+		// The tree indexes holes in the point storage, which the features
+		// matrix given back to loadIndex() cannot reproduce.
+		UWARN("Points have been removed from the nanoflann index (%d indexed of %d points), "
+			  "it cannot be serialized before being rebuilt.",
+			(int)index_->size(), (int)index_->cloud.kdtree_get_point_count());
+		return std::vector<unsigned char>();
+	}
+
+	std::ostringstream stream(std::ios_base::out | std::ios_base::binary);
+	index_->saveIndex(stream);
+	const std::string data = stream.str();
+	return std::vector<unsigned char>(data.begin(), data.end());
+}
+
+bool NanoFlannIndex::loadIndex(
+		const cv::Mat & features,
+		bool useDistanceL1,
+		bool incremental,
+		const unsigned char * indexData,
+		size_t indexDataSize,
+		float removedRatio,
+		int leafMaxSize,
+		std::string * errorMsg)
+{
+	this->release();
+
+	UASSERT_MSG(features.type() == CV_32FC1, "Only 32F features are supported by the nanoflann index.");
+	UASSERT(features.cols > 0);
+
+	if(indexData == 0 || indexDataSize == 0)
+	{
+		if(errorMsg)
+		{
+			*errorMsg = "Trying to load an empty nanoflann index.";
+		}
+		return false;
+	}
+
+	featuresDim_ = features.cols;
+	useDistanceL1_ = useDistanceL1;
+	removedRatio_ = removedRatio;
+	index_ = createImpl(featuresDim_, useDistanceL1, incremental, removedRatio, leafMaxSize);
+	index_->cloud.dim = featuresDim_;
+	this->appendPoints(features);
+
+	// nanoflann checks its own magic number, version and type sizes, and
+	// throws when the stream wasn't written by the same instantiation.
+	try
+	{
+		std::istringstream stream(
+			std::string((const char *)indexData, indexDataSize),
+			std::ios_base::in | std::ios_base::binary);
+		index_->loadIndex(stream);
+	}
+	catch(const std::exception & e)
+	{
+		if(errorMsg)
+		{
+			*errorMsg = uFormat("Nanoflann index cannot be loaded: %s", e.what());
+		}
+		this->release();
+		return false;
+	}
+
+	if(index_->size() != (size_t)features.rows)
+	{
+		if(errorMsg)
+		{
+			*errorMsg = uFormat("Serialized nanoflann index has %d points, but %d features were given.",
+				(int)index_->size(), features.rows);
+		}
+		this->release();
+		return false;
+	}
+	return true;
+}
+
+cv::Mat NanoFlannIndex::indexedPoints() const
+{
+	if(!index_ || index_->cloud.pts.empty())
+	{
+		return cv::Mat();
+	}
+	// The points are referenced row by row, so a continuous matrix of them has
+	// to be materialized. Only used to serialize the index.
+	cv::Mat points((int)index_->cloud.pts.size(), featuresDim_, CV_32FC1);
+	for(int i=0; i<points.rows; ++i)
+	{
+		memcpy(points.ptr<float>(i), index_->cloud.pts[i], featuresDim_*sizeof(float));
+	}
+	return points;
+}
+
+void NanoFlannIndex::removePoint(unsigned int index)
+{
+	if(!index_)
+	{
+		UERROR("Nanoflann index not yet created!");
+		return;
+	}
+	if(!index_->isIncremental())
+	{
+		// Same as addPoints(): a tree built without the intention of changing
+		// it can still be changed.
+		this->makeIncremental();
+	}
+	// The point stays in cloud so that the indexes of the other points don't
+	// move, only the tree drops it.
+	index_->removePoint(index);
+}
+
+void NanoFlannIndex::knnSearch(
+		const cv::Mat & query,
+		cv::Mat & indices,
+		cv::Mat & dists,
+		int knn) const
+{
+	if(!index_)
+	{
+		UERROR("Nanoflann index not yet created!");
+		return;
+	}
+	UASSERT(query.type() == CV_32FC1 && query.cols == featuresDim_);
+	UASSERT(knn > 0);
+
+	indices = cv::Mat(query.rows, knn, CV_32SC1, cv::Scalar(-1));
+	dists = cv::Mat(query.rows, knn, CV_32FC1, cv::Scalar(-1.0f));
+
+	std::vector<unsigned int> resultIndices(knn);
+	std::vector<float> resultDists(knn);
+	for(int i=0; i<query.rows; ++i)
+	{
+		size_t found = index_->knnSearch(query.ptr<float>(i), knn, resultIndices.data(), resultDists.data());
+		for(size_t j=0; j<found; ++j)
+		{
+			indices.at<int>(i, j) = (int)resultIndices[j];
+			dists.at<float>(i, j) = resultDists[j];
+		}
+	}
+}
+
+void NanoFlannIndex::radiusSearch(
+		const cv::Mat & query,
+		std::vector<std::vector<size_t> > & indices,
+		std::vector<std::vector<float> > & dists,
+		float radius,
+		int maxNeighbors,
+		float eps,
+		bool sorted) const
+{
+	if(!index_)
+	{
+		UERROR("Nanoflann index not yet created!");
+		return;
+	}
+	UASSERT(query.type() == CV_32FC1 && query.cols == featuresDim_);
+
+	indices.resize(query.rows);
+	dists.resize(query.rows);
+
+	// nanoflann compares squared distances, and sorting is required to know
+	// which neighbors are the closest ones when maxNeighbors is set.
+	const float radiusSqr = radius * radius;
+	nanoflann::SearchParameters params(eps, sorted || maxNeighbors>0);
+
+	std::vector<nanoflann::ResultItem<unsigned int, float> > matches;
+	for(int i=0; i<query.rows; ++i)
+	{
+		size_t found = index_->radiusSearch(query.ptr<float>(i), radiusSqr, matches, params);
+		if(maxNeighbors > 0 && found > (size_t)maxNeighbors)
+		{
+			found = (size_t)maxNeighbors;
+		}
+
+		indices[i].resize(found);
+		dists[i].resize(found);
+		for(size_t j=0; j<found; ++j)
+		{
+			indices[i][j] = (size_t)matches[j].first;
+			dists[i][j] = matches[j].second;
+		}
+	}
+}
+
+} /* namespace rtabmap */
@@ -0,0 +1,159 @@
+/*
+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_NANOFLANN_NANOFLANNINDEX_H_
+#define CORELIB_SRC_NANOFLANN_NANOFLANNINDEX_H_
+
+#include <opencv2/opencv.hpp>
+
+namespace rtabmap {
+
+class NanoFlannIndexImpl;
+
+/**
+ * kd-tree backed by nanoflann, held by FlannIndex when its
+ * NANOFLANN_INDEX_KDTREE_SINGLE algorithm is selected. Two trees are available,
+ * buildIndex() picking one with its "incremental" argument:
+ *
+ * Incremental (nanoflann's KDTreeSingleIndexIncrementalAdaptor), a single
+ * weight-balanced tree accepting points after it is built:
+ *  - addPoints() inserts incrementally, and bulk-rebuilds when the batch is
+ *    large relative to the tree.
+ *  - removePoint() is lazy; a subtree is rebuilt, dropping its tombstones, once
+ *    its removed fraction gets over "removedRatio".
+ * Indexes returned by addPoints() stay valid for the lifetime of the index:
+ * points are only appended and removals never renumber the remaining ones.
+ *
+ * Static (nanoflann's KDTreeSingleIndexAdaptor), built once from all the points
+ * given to buildIndex(): cheaper to build and to search. Use it for a dataset
+ * known to be fixed, like the image points searched during registration. Adding
+ * or removing points is still possible, it rebuilds itself as the incremental
+ * tree when that happens.
+ *
+ * Only float features are supported (nanoflann has no Hamming metric, binary
+ * descriptors have to be converted first), with the L2 or L1 metric. Searches
+ * are exact, there is no equivalent of rtflann's "checks" budget.
+ */
+class NanoFlannIndex
+{
+public:
+	NanoFlannIndex();
+	~NanoFlannIndex();
+
+	NanoFlannIndex(const NanoFlannIndex &) = delete;
+	NanoFlannIndex & operator=(const NanoFlannIndex &) = delete;
+
+	void release();
+
+	// features must be a CV_32FC1 matrix, one point per row. "incremental"
+	// selects the tree accepting addPoints()/removePoint(), for which
+	// "removedRatio" is the fraction of it that can be left removed before a
+	// rebuild (1 to never rebuild). "leafMaxSize" is the number of points under
+	// which the static tree stops splitting, it doesn't apply to the
+	// incremental one, which holds a single point per node.
+	//
+	// A static tree given points to add afterwards is rebuilt as an incremental
+	// one, so that building without the intention of adding points doesn't
+	// prevent it (see addPoints()).
+	void buildIndex(
+			const cv::Mat & features,
+			bool useDistanceL1,
+			bool incremental,
+			float removedRatio = 0.5f,
+			int leafMaxSize = 10);
+
+	// Return an empty vector if the index cannot be serialized: when it is not
+	// built, or when points have been removed from it (the tree then indexes
+	// holes that the matrix given back to loadIndex() cannot reproduce).
+	std::vector<unsigned char> serializeIndex() const;
+
+	// features must hold the very same points, in the same order, than those
+	// that were indexed when the index was serialized.
+	bool loadIndex(
+			const cv::Mat & features,
+			bool useDistanceL1,
+			bool incremental,
+			const unsigned char * indexData,
+			size_t indexDataSize,
+			float removedRatio = 0.5f,
+			int leafMaxSize = 10,
+			std::string * errorMsg = 0);
+
+	// The indexed points as an indexedFeatures()x"dim" CV_32FC1 matrix, copied
+	// out of the features they are referenced from. Empty if the index is not
+	// built. Note that points removed from the tree are still part of it.
+	cv::Mat indexedPoints() const;
+
+	bool isBuilt() const {return index_ != 0;}
+
+	// removed points excluded
+	size_t indexedFeatures() const;
+
+	// return Bytes
+	size_t memoryUsed() const;
+
+	// return the index assigned to each added point
+	std::vector<unsigned int> addPoints(const cv::Mat & features);
+
+	void removePoint(unsigned int index);
+
+	// return squared distances, indices and distances are set to -1 for the
+	// neighbors that couldn't be found.
+	void knnSearch(
+			const cv::Mat & query,
+			cv::Mat & indices,
+			cv::Mat & dists,
+			int knn) 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,
+			float eps,
+			bool sorted) const;
+
+private:
+	// The metric (L2 or L1) and the compile-time dimension of the tree are only
+	// known when the index is built, so the tree type is erased behind this
+	// implementation, which also owns the points it indexes. Keeping nanoflann
+	// out of this header is a side effect, not the reason.
+	size_t appendPoints(const cv::Mat & features);
+	void makeIncremental();
+
+	NanoFlannIndexImpl * index_;
+	int featuresDim_;
+	// kept to rebuild the tree as an incremental one, see makeIncremental()
+	bool useDistanceL1_;
+	float removedRatio_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* CORELIB_SRC_NANOFLANN_NANOFLANNINDEX_H_ */
@@ -0,0 +1,7 @@
+
+nanoflann is included in rtabmap for convenience
+
+Source: https://github.com/jlblancoc/nanoflann
+Version: 1.12.1 
+Commit: 7812aa08260b6971af2230b1ac446d54f7939822
+License: BSD
@@ -35,4 +35,11 @@
 #endif
 #define FLANN_VERSION_ "1.8.4"

+// Generated by CMake from config.h.in, as upstream flann does. Carrying the
+// option here rather than in a compile definition keeps a toggle of it from
+// rebuilding the whole library: a definition given to the compiler lands in the
+// target's flags, which every object file depends on, while this header is only
+// included where rtflann is.
+#cmakedefine FLANN_KDTREE_MEM_OPT
+
 #endif /* RTABMAP_FLANN_CONFIG_H_ */
@@ -29,7 +29,7 @@
 #ifndef RTABMAP_FLANN_DEFINES_H_
 #define RTABMAP_FLANN_DEFINES_H_

-#include "config.h"
+#include "rtflann/config.h" // generated by CMake

 #ifdef FLANN_EXPORT
 #undef FLANN_EXPORT