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 <matlabbe@gmail.com>

.github/workflows/cmake-linux.yml

@@ -101,4 +101,4 @@ jobs:
       env:
         PYTHONNOUSERSITE: 1
       run: |
-        ctest -C ${{env.BUILD_TYPE}} -V
+        ctest -C ${{env.BUILD_TYPE}} -V -LE performance

.github/workflows/cmake-macos.yml

@@ -124,7 +124,7 @@ jobs:
       env:
         PYTHONNOUSERSITE: 1
       run: |
-        ctest -C ${{env.BUILD_TYPE}} -V
+        ctest -C ${{env.BUILD_TYPE}} -V -LE performance
 
     # A ctest SEGFAULT is reported as just "SEGFAULT" with no backtrace, which
     # makes a crash inside a long integration test invisible -- the log simply

.github/workflows/cmake-windows.yml

@@ -125,7 +125,7 @@ jobs:
         PYTHONHOME: ${{env.VCPKG_EXPORT_PATH}}/installed/x64-windows-release/tools/python3
         PYTHONNOUSERSITE: 1
       run: |
-        ctest -C ${{env.BUILD_TYPE}} -V --timeout 300
+        ctest -C ${{env.BUILD_TYPE}} -V --timeout 300 -LE performance
 
     - name: Info
       # Skipped for CUDA: the binary links ZED (sl_zed64.dll -> nvcuvid/nvEncodeAPI64),

.github/workflows/coverage.yml

@@ -128,7 +128,7 @@ jobs:
           #
           # The replays still run (and gate) in the cmake-linux / macos /
           # windows jobs; they are just not measured here.
-          ctest -V -LE long
+          ctest -V -LE "long|performance"
 
       - name: Generate LCOV report
         run: |

CMakeLists.txt

@@ -63,10 +63,6 @@ ELSE()
     option(FLANN_KDTREE_MEM_OPT  "Disable multi-threaded FLANN kd-tree to minimize memory allocations"   ON)
 ENDIF()
 
-IF(FLANN_KDTREE_MEM_OPT)
-    ADD_DEFINITIONS("-DFLANN_KDTREE_MEM_OPT")
-ENDIF(FLANN_KDTREE_MEM_OPT)
-
 IF(WIN32 AND NOT MINGW)
     ADD_DEFINITIONS("-DNOMINMAX")
     ADD_DEFINITIONS("-wd4100 -wd4127 -wd4150 -wd4191 -wd4242 -wd4244 -wd4251 -wd4305 -wd4365 -wd4512 -wd4514 -wd4548 -wd4571 -wd4619 -wd4625 -wd4626 -wd4628 -wd4668 -wd4710 -wd4711 -wd4738 -wd4820 -wd4946 -wd4986")
@@ -182,6 +178,13 @@ if(ANDROID OR IOS OR CMAKE_SYSTEM_NAME STREQUAL "iOS")
    set(BUILD_TESTING OFF CACHE BOOL "Build the testing tree." FORCE)
 endif()
 
+# Performance tests are benchmarks: they report times, memory and recall
+# instead of asserting on them, as those depend on the machine. They are built
+# by default so they cannot rot, but are labelled "performance" so that a run
+# opts in with `ctest -L performance` and CI opts out with `ctest -LE
+# performance`. They can also be run directly, e.g. bin/test_flann_index_perf.
+OPTION(BUILD_PERF_TESTS "Build the performance tests (see ctest -L performance)" ON)
+
 include(CTest)
 if(BUILD_TESTING)
    # Add GTest using FetchContent
@@ -1565,6 +1568,7 @@ MESSAGE(STATUS "  BUILD_APP =            ${BUILD_APP}")
 MESSAGE(STATUS "  BUILD_TOOLS =          ${BUILD_TOOLS}")
 MESSAGE(STATUS "  BUILD_EXAMPLES =       ${BUILD_EXAMPLES}")
 MESSAGE(STATUS "  BUILD_TESTING =        ${BUILD_TESTING}")
+MESSAGE(STATUS "  BUILD_PERF_TESTS =     ${BUILD_PERF_TESTS}")
 MESSAGE(STATUS "  ENABLE_COVERAGE =      ${ENABLE_COVERAGE}")
 MESSAGE(STATUS "  ENABLE_FORMAT_ERRORS = ${ENABLE_FORMAT_ERRORS}")
 IF(NOT WIN32)

corelib/include/rtabmap/core/FlannIndex.h

@@ -34,36 +34,117 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+class NanoFlannIndex;
+
+/**
+ * @class FlannIndex
+ * @brief Nearest neighbor index over a set of features
+ *
+ * Wraps the search structures of the vendored rtflann and nanoflann libraries
+ * behind one interface, the structure being chosen with flann_algorithm_t at
+ * build time. Used for the visual word dictionary (VWDictionary) and for the
+ * 2D point searches of visual registration (RegistrationVis).
+ *
+ * The features are not copied: the index refers to the matrices it is given and
+ * keeps them alive, cv::Mat data being reference counted, so they must not be
+ * modified in place while it is in use. Every point it holds is
+ * designated by an index, assigned in the order the points were added and
+ * stable for the lifetime of the index: removePoint() leaves a hole rather
+ * than renumbering the points after it.
+ */
 class RTABMAP_CORE_EXPORT FlannIndex
 {
 public:
-	// A forward of the internal enum, indexes should match. See src/rtflann/defines.h
+	/**
+	 * @enum flann_algorithm_t
+	 * @brief The index structure built by buildIndex()
+	 *
+	 * The values under 8 are forwarded from rtflann's own enum and have to
+	 * match it (see src/rtflann/defines.h); the nanoflann ones are
+	 * rtabmap-specific and kept outside its range (0-7, 254, 255). A value is
+	 * written in the serialized index header and checked back on load, so none
+	 * of them may be renumbered.
+	 *
+	 * The nanoflann structures take float features only (nanoflann has no
+	 * Hamming metric) and search exactly, ignoring "checks". That makes them
+	 * the fastest ones for 2D and 3D points, and the wrong ones for
+	 * descriptors: an exact search visits more and more of the tree as the
+	 * dimension grows, down to being as slow as an exhaustive search. Prefer
+	 * the approximate rtflann kd-trees for those.
+	 */
 	enum flann_algorithm_t
 	{
-		FLANN_INDEX_LINEAR 			= 0,
+		FLANN_INDEX_LINEAR 			= 0, ///< Exhaustive search
-		FLANN_INDEX_KDTREE 			= 1,
+		FLANN_INDEX_KDTREE 			= 1, ///< 4 randomized kd-trees, searched approximately
-		FLANN_INDEX_KDTREE_SINGLE   = 4,
+		FLANN_INDEX_KDTREE_SINGLE   = 4, ///< Single kd-tree, searched exactly
-		FLANN_INDEX_LSH 			= 6,
+		FLANN_INDEX_LSH 			= 6, ///< Locality-Sensitive Hashing (binary descriptors)
+
+		/// nanoflann kd-tree. With a rebalancing factor of 1 it is built once,
+		/// which is the cheapest to build and to search; over 1 it is the
+		/// weight-balanced tree accepting addPoints()/removePoint(), which
+		/// cannot be serialized while some of its points are removed.
+		NANOFLANN_INDEX_KDTREE_SINGLE = 100,
 	};
 
 	FlannIndex();
 	virtual ~FlannIndex();
 
+	/** @brief Drop the index and everything it holds, back to the state of a new one. */
 	void release();
+
+	/**
+	 * @brief Serialize the index, to be given back to loadIndex()
+	 * @param computeChecksum Add a checksum of the indexed features to the
+	 *        data, which loadIndex() compares against the features it is given
+	 * @return The serialized index, empty when there is nothing to serialize or
+	 *         when the structure in use cannot be
+	 *
+	 * The format depends on the architecture and on the versions of the
+	 * vendored libraries: loadIndex() refuses an index it cannot read, leaving
+	 * it to be rebuilt.
+	 */
 	std::vector<unsigned char> serializeIndex(bool computeChecksum = true) const;
 
+	/** @return Number of indexed features, the removed ones excluded. */
 	size_t indexedFeatures() const;
 
-	// return Bytes
+	/**
+	 * @return Bytes used by the index, the features themselves excluded as
+	 *         they are only referred to.
+	 */
 	size_t memoryUsed() const;
 
-	// Note that useDistanceL1 doesn't have any effect if LSH is used
+	/**
+	 * @brief Build the index over the given features, releasing any previous one
+	 * @param algorithm The structure to build
+	 * @param features One feature per row, CV_32FC1 or, for the rtflann
+	 *        structures only, CV_8UC1 for binary descriptors (Hamming distance)
+	 * @param useDistanceL1 Search with the L1 distance instead of L2, ignored
+	 *        by LSH and by the binary descriptors
+	 * @param rebalancingFactor Fraction (factor-1)/factor of the index that can
+	 *        be left removed before it is rebuilt, e.g. half of it for 2. Set
+	 *        to 1 to never rebuild it.
+	 */
 	void buildIndex(
 			flann_algorithm_t algorithm,
 			const cv::Mat & features,
 			bool useDistanceL1 = false,
 			float rebalancingFactor = 2.0f);
-	// Return false if the indexData doesn't correspond to expected features used and parameters.
+
+	/**
+	 * @brief Load an index serialized by serializeIndex(), releasing any previous one
+	 * @param indexData The serialized index
+	 * @param algorithm The structure it was built with
+	 * @param features The very same features it was built with, in the same
+	 *        order: the index refers to them by their row
+	 * @param useDistanceL1 The distance it was built with
+	 * @param rebalancingFactor See buildIndex(). The serialized data carries the
+	 *        one the index was built with, which is deprecated and ignored:
+	 *        this one is used instead.
+	 * @param errorMsg Filled with what didn't match when the index is refused
+	 * @return False if the data doesn't correspond to the given features and
+	 *         parameters, in which case the index is left released
+	 */
 	bool loadIndex(
 		const std::vector<unsigned char> & indexData,
 		flann_algorithm_t algorithm,
@@ -71,6 +152,7 @@ public:
 		bool useDistanceL1 = false,
 		float rebalancingFactor = 2.0f,
 		std::string * errorMsg = NULL);
+	/** @brief Load an index from a raw buffer, see the overload above. */
 	bool loadIndex(
 		const unsigned char * indexData,
 		size_t indexDataSize,
@@ -80,16 +162,46 @@ public:
 		float rebalancingFactor = 2.0f,
 		std::string * errorMsg = NULL);
 
+	/** @return Whether an index has been built or loaded. */
 	bool isBuilt();
 
+	/** @return Type of the indexed features (CV_32FC1 or CV_8UC1). */
 	int featuresType() const {return featuresType_;}
+	/** @return Dimension of the indexed features. */
 	int featuresDim() const {return featuresDim_;}
 
+	/**
+	 * @brief Add features to the index
+	 * @param features One feature per row, of the type and dimension the index
+	 *        was built with
+	 * @return The index assigned to each of them, empty when the structure
+	 *         doesn't accept points after it is built
+	 */
 	std::vector<unsigned int> addPoints(const cv::Mat & features);
 
+	/**
+	 * @brief Remove an indexed feature, by the index addPoints() gave for it
+	 *
+	 * The feature is only marked as removed: it is skipped by the searches, but
+	 * keeps taking memory until the index is rebuilt (see the rebalancing
+	 * factor of buildIndex()). Not supported by every structure.
+	 */
 	void removePoint(unsigned int index);
 
-	// return squared distances (indices should be casted in size_t)
+	/**
+	 * @brief Search the k nearest neighbors of each query
+	 * @param query One feature per row, of the type and dimension the index was
+	 *        built with
+	 * @param indices Neighbors found, one query per row, CV_32SC1. The
+	 *        neighbors that couldn't be found are set to -1.
+	 * @param dists Their squared distances, CV_32FC1, or CV_32SC1 for the
+	 *        Hamming distances of binary descriptors
+	 * @param knn Number of neighbors to search for
+	 * @param checks Number of leaves an approximate search visits, the exact
+	 *        structures ignoring it
+	 * @param eps Search for eps-approximate neighbors
+	 * @param sorted Give the neighbors back by increasing distance
+	 */
 	void knnSearch(
 			const cv::Mat & query,
 			cv::Mat & indices,
@@ -99,7 +211,21 @@ public:
 			float eps = 0.0,
 			bool sorted = true) const;
 
-	// return squared distances
+	/**
+	 * @brief Search the neighbors of each query within a radius
+	 * @param query One feature per row, of the type and dimension the index was
+	 *        built with
+	 * @param indices Neighbors found, one vector per query
+	 * @param dists Their squared distances, one vector per query
+	 * @param radius Search radius, squared internally: it is a distance, not a
+	 *        squared one
+	 * @param maxNeighbors Maximum number of neighbors per query, the nearest
+	 *        ones being kept. 0 for all of them.
+	 * @param checks Number of leaves an approximate search visits, the exact
+	 *        structures ignoring it
+	 * @param eps Search for eps-approximate neighbors
+	 * @param sorted Give the neighbors back by increasing distance
+	 */
 	void radiusSearch(
 			const cv::Mat & query,
 			std::vector<std::vector<size_t> > & indices,
@@ -111,7 +237,8 @@ public:
 			bool sorted = true) const;
 
 private:
-	void * index_;
+	void * index_;               // rtflann backend
+	NanoFlannIndex * nanoIndex_; // nanoflann backend, only one of the two is set
 	unsigned int nextIndex_;
 	int featuresType_;
 	int featuresDim_;

corelib/include/rtabmap/core/Parameters.h

@@ -252,10 +252,10 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Mem, RotateImagesUpsideUp,        bool, false,    "Rotate images so that upside is up if they are not already. This can be useful in case the robots don't have all same camera orientation but are using the same map, so that not rotation-invariant visual features can still be used across the fleet.");
 
     // KeypointMemory (Keypoint-based)
-    RTABMAP_PARAM(Kp, NNStrategy,               int, 1,       "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
+    RTABMAP_PARAM(Kp, NNStrategy,               int, 1,       "FLANN Linear=0, FLANN KdTree=1, FLANN LSH=2, Brute Force=3, Brute Force GPU=4, FLANN KdTree Single=5, NanoFLANN KdTree=6");
     RTABMAP_PARAM(Kp, IncrementalDictionary,    bool, true,   "");
-    RTABMAP_PARAM(Kp, IncrementalFlann,         bool, true,   uFormat("When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary increases of the factor \"%s\" in size).", kKpFlannRebalancingFactor().c_str()));
+    RTABMAP_PARAM(Kp, IncrementalFlann,         bool, true,   uFormat("When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is only rebuilt when too many of its features have been removed, see \"%s\").", kKpFlannRebalancingFactor().c_str()));
-    RTABMAP_PARAM(Kp, FlannRebalancingFactor,   float, 2.0,   uFormat("Factor used when rebuilding the incremental FLANN index (see \"%s\"). Set <=1 to disable.", kKpIncrementalFlann().c_str()));
+    RTABMAP_PARAM(Kp, FlannRebalancingFactor,   float, 2.0,   uFormat("Rebuild the incremental FLANN index (see \"%s\") once the ratio (factor-1)/factor of its features has been removed, e.g. half of them for a factor of 2. Rebuilding frees the memory of the removed features and speeds up the searches. Features are mostly removed when memory management is enabled (\"%s\" or \"%s\"). Set to 1 to never rebuild, which also uses less memory as the features don't have to be referenced one by one.", kKpIncrementalFlann().c_str(), kRtabmapTimeThr().c_str(), kRtabmapMemoryThr().c_str()));
     RTABMAP_PARAM(Kp, ByteToFloat,              bool, false,  uFormat("For %s=1, binary descriptors are converted to float by converting each byte to float instead of converting each bit to float. When converting bytes instead of bits, less memory is used and search is faster at the cost of slightly less accurate matching.", kKpNNStrategy().c_str()));
     RTABMAP_PARAM(Kp, MaxDepth,                 float, 0,     "Filter extracted keypoints by depth (0=inf).");
     RTABMAP_PARAM(Kp, MinDepth,                 float, 0,     "Filter extracted keypoints by depth.");
@@ -780,7 +780,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Vis, GridRows,                  int,   1,     uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
     RTABMAP_PARAM(Vis, GridCols,                  int,   1,     uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
     RTABMAP_PARAM(Vis, CorType,                   int,   0,     "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
-    RTABMAP_PARAM(Vis, CorNNType,                 int,   1,     uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4, BruteForceCrossCheck=5, SuperGlue=6, GMS=7. Used for features matching approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorNNType,                 int,   1,     uFormat("[%s=0] FLANN Linear=0, FLANN KdTree=1, FLANN LSH=2, Brute Force=3, Brute Force GPU=4, Brute Force Cross Check=5, SuperGlue=6, GMS=7, FLANN KdTree Single=8, NanoFLANN KdTree=9. Used for features matching approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorNNDR,                   float, 0.8,   uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorGuessWinSize,           int,   40,    uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool,  false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str()));

corelib/include/rtabmap/core/RegistrationVis.h

@@ -75,6 +75,13 @@ public:
 	int getMinInliers() const {return _minInliers;}
 	/** @return **Vis/CorNNType** nearest-neighbor strategy. */
 	int getNNType() const {return _nnType;}
+	/** @return Name of the **Vis/CorNNType** nearest-neighbor strategy in use. */
+	std::string getNNTypeName() const {return getNNTypeName(_nnType);}
+
+	/**
+	 * @brief Name of a Vis/CorNNType value
+	 */
+	static std::string getNNTypeName(int nnType);
 	/** @return **Vis/CorNNDR** ratio test threshold. */
 	float getNNDR() const {return _nndr;}
 	/** @return **Vis/EstimationType** (0: 3D→3D, 1: PnP, 2: epipolar). */

corelib/include/rtabmap/core/VWDictionary.h

@@ -68,6 +68,11 @@ public:
 	/**
 	 * @enum NNStrategy
 	 * @brief Nearest neighbor search strategies for descriptor matching
+	 *
+	 * The values are those of the Kp/NNStrategy parameter, saved in user
+	 * configurations and databases: append, never renumber. The Vis/CorNNType
+	 * parameter has strategies of its own, its values are mapped to these ones
+	 * by RegistrationVis::nnStrategyFromCorNNType().
 	 */
 	enum NNStrategy{
 		kNNFlannNaive,      ///< FLANN naive search (exhaustive)
@@ -75,6 +80,8 @@ public:
 		kNNFlannLSH,        ///< FLANN Locality-Sensitive Hashing (ideal for binary descriptors)
 		kNNBruteForce,      ///< Brute force CPU search
 		kNNBruteForceGPU,   ///< Brute force GPU-accelerated search (requires CUDA)
+		kNNFlannKdTreeSingle, ///< FLANN single exact kd-tree index (rebuilt whenever a word is added, for an index built once)
+		kNNNanoFlannKdTree,   ///< nanoflann kd-tree index (float descriptors only, incremental)
 		kNNUndef            ///< Undefined strategy
 	};
 	
@@ -106,11 +113,17 @@ public:
 			return "BRUTE FORCE";
 		case kNNBruteForceGPU:
 			return "BRUTE FORCE GPU";
+		case kNNNanoFlannKdTree:
+			return "NANOFLANN KD-TREE";
+		case kNNFlannKdTreeSingle:
+			return "FLANN KD-TREE SINGLE";
 		default:
 			return "Unknown";
 		}
 	}
 
+
+
 public:
 	/**
 	 * @brief Constructor

corelib/src/CMakeLists.txt

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

corelib/src/FlannIndex.cpp

@@ -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) {
+			// Each removed index is one point, whatever the headers cover.
-				for(const auto & index: removedIndexes_)
+			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
+			int header[FLANN_INDEX_HEADER_SIZE];
-			memcpy(&rebalancingFactorAsInt, &rebalancingFactor_, sizeof(rebalancingFactor_)); // Deprecated
+			fillIndexHeader(header,
-			int crcValueAsInt;
+				algorithm_,
-			memcpy(&crcValueAsInt, &crcValue, sizeof(crcValue));
+				featuresDim_,
-			int header[FLANN_INDEX_HEADER_SIZE] = {
+				useDistanceL1_,
-					RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH, // 0,1,2
+				rebalancingFactor_,
-					algorithm_,                                                          // 3,
+				dataRows,
-					featuresDim_,                                                        // 4,
+				dataCols,
-					useDistanceL1_?1:0,                                                  // 5,
+				dataType,
-					rebalancingFactorAsInt,                                              // 6, Deprecated
+				computeChecksum?computeCrc(dataset):0,
-					dataRows,                                                            // 7,
+				(int)bytes_written);
-					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]);
 			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),
+		rebalancingFactor,
-		error;
+		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.");
+	if(!isNanoFlannAlgorithm(algorithm)) {
-	return false;
+		UERROR("FLANN index deserialization is not yet implemented on Windows. Index cannot be loaded from memory buffer.");
-#else
+		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(needsRebuild(index, removedRatio))
-		if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
 		{
-			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(needsRebuild(index, removedRatio))
-			if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
 			{
-				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(needsRebuild(index, removedRatio))
-			if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
 			{
-				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(needsRebuild(index, removedRatio))
-			if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
 			{
-				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_);
+		indexes.push_back(nextIndex_ + i);
-		addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(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!");

corelib/src/RegistrationVis.cpp

@@ -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
+							// Index the projected keypoints. A rebalancing factor of 1:
-							rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
+							// the index is thrown away with the frame, nothing is ever
-							rtflann::Index<rtflann::L2_Simple<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
+							// added to or removed from it. cv::Point2f being two floats,
-							index.buildIndex();
+							// 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);
+							cv::Mat pointsToMat((int)pointsTo.size(), 2, CV_32FC1, (void*)pointsTo.data());
-							index.radiusSearch(pointsToMat, indices, dists, radius*radius, rtflann::SearchParams());
+							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);
+							cv::Mat pointsToMat((int)pointsTo.size(), 2, CV_32FC1, (void*)pointsTo.data());
-							rtflann::Index<rtflann::L2_Simple<float> > index(pointsToMat, rtflann::KDTreeIndexParams());
+							FlannIndex flannIndex;
-							index.buildIndex();
+							flannIndex.buildIndex(FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, pointsToMat, false, 1.0f);
 
-							std::vector< std::vector<size_t> > indices;
+							cv::Mat queryMat((int)cornersProjected.size(), 2, CV_32FC1, (void*)cornersProjected.data());
-							std::vector<std::vector<float> > dists;
+
+							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);
+							flannIndex.radiusSearch(queryMat, indices, dists, radius, 0, 32, 0.0, false);
-							UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
+
-							UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
+							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];
 

corelib/src/VWDictionary.cpp

@@ -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:
+							flannAlgorithm(_strategy),
-							_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
-							FlannIndex::FLANN_INDEX_KDTREE, // kNNFlannKdTree
 							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).",
+					"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(), kNNBruteForce);
+					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:
+						flannAlgorithm(_strategy),
-							_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
-								FlannIndex::FLANN_INDEX_KDTREE, // kNNFlannKdTree
 						_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:
+		flannAlgorithm(_strategy),
-			_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
-				FlannIndex::FLANN_INDEX_KDTREE, 
 		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);
 					}

corelib/src/nanoflann/NanoFlannIndex.cpp

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

corelib/src/nanoflann/NanoFlannIndex.h

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

corelib/src/nanoflann/readme.txt

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

corelib/src/rtflann/config.h.in

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

corelib/src/rtflann/defines.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

corelib/test/CMakeLists.txt

@@ -28,6 +28,7 @@ set(corelib_test_sources
 	test_util3d_surface.cpp             #util3d_surface.h
 	test_visualword.cpp                 #VisualWord.h
 	test_vwdictionary.cpp               #VWDictionary.h
+	test_flann_index.cpp                #FlannIndex.h
 	test_transform.cpp                  #Transform.h
 	test_stereo_dense.cpp               #StereoDense.h (BM and SGBM strategies)
 	test_stereo.cpp                     #Stereo.h (BlockMatching and OpticalFlow)
@@ -102,6 +103,24 @@ foreach(shard RANGE 0 ${_corelib_last_shard})
 			ENVIRONMENT "GTEST_TOTAL_SHARDS=${CORELIB_TEST_SHARDS};GTEST_SHARD_INDEX=${shard}")
 endforeach()
 
+# Comparison of the FlannIndex backends (times, memory, recall). Its own
+# executable so that it can be run on demand:
+#   bin/test_flann_index_perf
+#   bin/test_flann_index_perf --gtest_filter=*Descriptors*
+# and so that its seconds of benchmarking stay out of the unit test shards.
+IF(BUILD_PERF_TESTS)
+	add_executable(test_flann_index_perf perf_flann_index.cpp)
+	target_link_libraries(test_flann_index_perf gtest_main rtabmap_core)
+
+	# Labelled "performance" so CI jobs can opt out via `ctest -LE performance`,
+	# and run them alone with `ctest -L performance`.
+	add_test(NAME test_flann_index_perf COMMAND test_flann_index_perf)
+	math(EXPR _perf_timeout "600 * ${_test_timeout_scale}")
+	set_tests_properties(test_flann_index_perf PROPERTIES
+			TIMEOUT ${_perf_timeout}
+			LABELS  "performance")
+ENDIF(BUILD_PERF_TESTS)
+
 # Rtabmap end-to-end replay of sample DBs (test data fetched by
 # scripts/fetch_test_data.sh into data/tests/*.db). Skips at runtime if the
 # assets are absent, so the build stays green for contributors without them.

corelib/test/FlannIndexBackends.h

@@ -0,0 +1,313 @@
+#ifndef RTABMAP_CORELIB_TEST_FLANNINDEXBACKENDS_H_
+#define RTABMAP_CORELIB_TEST_FLANNINDEXBACKENDS_H_
+
+#include <gtest/gtest.h>
+#include <rtabmap/core/FlannIndex.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <opencv2/core.hpp>
+#include <opencv2/features2d.hpp>
+
+#include <algorithm>
+#include <iostream>
+
+using namespace rtabmap;
+
+// Everything here is inline rather than static: the two test files including
+// this header each use a subset of it, and unused functions with internal
+// linkage warn.
+namespace {
+
+struct Backend
+{
+	const char * name;
+	FlannIndex::flann_algorithm_t algorithm;
+	// The nanoflann structures differ by their rebalancing factor: 1 for the one
+	// built once, more for the one accepting points afterwards.
+	float rebalancingFactor = 2.0f;
+	// Not a FlannIndex at all: cv::BFMatcher, what the brute force strategies of
+	// VWDictionary and RegistrationVis use. Kept in the comparisons as the
+	// baseline every index has to beat. OpenCV threads its search where the
+	// indexes here search on one core, so it comes in two flavours: as the
+	// application gets it, and held to one core to compare the work done rather
+	// than the time it takes on an idle machine.
+	bool bruteForce = false;
+	bool singleCore = false;
+};
+
+// Every algorithm that indexes float features. The exhaustive search comes
+// first: it is the reference the others are compared to, both for the neighbors
+// found and for the time taken.
+const Backend FLOAT_BACKENDS[] = {
+	{"linear    exhaustive                ", FlannIndex::FLANN_INDEX_LINEAR},
+	// No single core row for the float features: OpenCV doesn't thread that
+	// match at these sizes, it measures the same thing as the one above.
+	{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true},
+	{"rtflann   kd-tree (4 randomized)    ", FlannIndex::FLANN_INDEX_KDTREE},
+	{"rtflann   kd-tree single            ", FlannIndex::FLANN_INDEX_KDTREE_SINGLE},
+	{"nanoflann kd-tree single            ", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 1.0f},
+	{"nanoflann kd-tree single incremental", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 2.0f},
+};
+
+// Those of them that search exactly, so are expected to return the very same
+// neighbors. The randomized kd-tree is left out: it trades recall for speed.
+const Backend EXACT_BACKENDS[] = {
+	{"linear    exhaustive                ", FlannIndex::FLANN_INDEX_LINEAR},
+	{"rtflann   kd-tree single            ", FlannIndex::FLANN_INDEX_KDTREE_SINGLE},
+	{"nanoflann kd-tree single            ", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 1.0f},
+	{"nanoflann kd-tree single incremental", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 2.0f},
+};
+
+// Binary features only have the Hamming based algorithms: nanoflann has no
+// Hamming metric, and a kd-tree splitting on bits doesn't work, which is what
+// LSH is for.
+const Backend BINARY_BACKENDS[] = {
+	{"linear    exhaustive (hamming)      ", FlannIndex::FLANN_INDEX_LINEAR},
+	{"cv        BFMatcher (hamming)       ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true},
+	{"cv        BFMatcher (hamming,1 core)", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, true},
+	{"rtflann   LSH                       ", FlannIndex::FLANN_INDEX_LSH},
+};
+
+// Those compared when points are added after the index is built.
+const Backend INCREMENTAL_BACKENDS[] = {
+	{"linear    exhaustive                ", FlannIndex::FLANN_INDEX_LINEAR},
+	{"rtflann   kd-tree (4 randomized)    ", FlannIndex::FLANN_INDEX_KDTREE},
+	{"rtflann   kd-tree single            ", FlannIndex::FLANN_INDEX_KDTREE_SINGLE},
+	{"nanoflann kd-tree single incremental", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 2.0f},
+};
+
+// Note when reading the times of the first configuration of a comparison: it
+// pays the warm up of the allocator, which the runs after it reuse. It shows on
+// the nanoflann trees, which allocate a node per point, and the smaller the
+// index the more it weighs.
+//
+// The rebalancing factor doesn't change how a search is done, only how the
+// index is built and grown: with a factor over 1 the rtflann backend keeps one
+// cv::Mat header per indexed point so that it can rebuild itself later, and the
+// nanoflann incremental tree turns it into the fraction of removed points it
+// tolerates. Both are compared where that matters, building and inserting.
+const float REBALANCING_FACTORS[] = {1.0f, 2.0f};
+
+// Synthetic point cloud, uniformly distributed in a 100 m box. The seeds are
+// fixed so that a comparison that passes keeps passing.
+inline cv::Mat makeCloud(int points, int dim, uint64_t seed)
+{
+	cv::RNG rng(seed);
+	cv::Mat cloud(points, dim, CV_32FC1);
+	rng.fill(cloud, cv::RNG::UNIFORM, 0.0f, 100.0f);
+	return cloud;
+}
+
+// Float descriptors drawn around a limited number of centers. Uniformly
+// distributed descriptors would be the worst case there is for any of the
+// approximate algorithms, and nothing like the clustered distribution real
+// descriptors have.
+inline cv::Mat makeDescriptors(int count, int dim, int clusters, uint64_t seed)
+{
+	cv::RNG rng(seed);
+	cv::Mat centers(clusters, dim, CV_32FC1);
+	rng.fill(centers, cv::RNG::UNIFORM, 0.0f, 255.0f);
+
+	// Filled in one go: a fill() per row costs seconds on a million descriptors.
+	cv::Mat descriptors(count, dim, CV_32FC1);
+	rng.fill(descriptors, cv::RNG::NORMAL, 0.0f, 12.0f);
+	for(int i=0; i<count; ++i)
+	{
+		descriptors.row(i) += centers.row(rng.uniform(0, clusters));
+	}
+	return descriptors;
+}
+
+// Binary descriptors around a limited number of centers, a few bits flipped.
+inline cv::Mat makeBinaryDescriptors(int count, int bytes, int clusters, uint64_t seed)
+{
+	cv::RNG rng(seed);
+	cv::Mat centers(clusters, bytes, CV_8UC1);
+	rng.fill(centers, cv::RNG::UNIFORM, 0, 256);
+
+	cv::Mat descriptors(count, bytes, CV_8UC1);
+	for(int i=0; i<count; ++i)
+	{
+		centers.row(rng.uniform(0, clusters)).copyTo(descriptors.row(i));
+		for(int bit=0; bit<bytes; ++bit) // ~1 bit flipped per byte
+		{
+			descriptors.at<unsigned char>(i, rng.uniform(0, bytes)) ^= (1 << rng.uniform(0, 8));
+		}
+	}
+	return descriptors;
+}
+
+// Queries taken from the indexed descriptors and perturbed, the way the same
+// feature observed twice would be. Their nearest neighbor is unambiguous, which
+// is what makes the recall of the approximate algorithms meaningful.
+inline cv::Mat perturbedQueries(const cv::Mat & descriptors, int count, uint64_t seed)
+{
+	cv::RNG rng(seed);
+	cv::Mat queries(count, descriptors.cols, descriptors.type());
+	for(int i=0; i<count; ++i)
+	{
+		descriptors.row(rng.uniform(0, descriptors.rows)).copyTo(queries.row(i));
+		if(descriptors.type() == CV_32FC1)
+		{
+			cv::Mat noise(1, descriptors.cols, CV_32FC1);
+			rng.fill(noise, cv::RNG::NORMAL, 0.0f, 2.0f);
+			queries.row(i) += noise;
+		}
+		else
+		{
+			for(int bit=0; bit<descriptors.cols/8; ++bit)
+			{
+				queries.at<unsigned char>(i, rng.uniform(0, descriptors.cols)) ^= (1 << rng.uniform(0, 8));
+			}
+		}
+	}
+	return queries;
+}
+
+struct Result
+{
+	double buildTime;
+	double knnTime;
+	double radiusTime; // negative when no radius search was done
+	size_t memory;
+	cv::Mat indices;
+};
+
+inline Result run(
+		const Backend & backend,
+		const cv::Mat & data,
+		const cv::Mat & queries,
+		int knn,
+		float radius,
+		float rebalancingFactor)
+{
+	Result result;
+	result.radiusTime = -1.0;
+	cv::Mat dists;
+
+	if(backend.bruteForce)
+	{
+		// cv::setNumThreads() is global, put it back before leaving.
+		const int threads = cv::getNumThreads();
+		if(backend.singleCore)
+		{
+			cv::setNumThreads(1);
+		}
+
+		UTimer timer;
+		cv::BFMatcher matcher(data.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
+		matcher.add(std::vector<cv::Mat>(1, data));
+		matcher.train(); // nothing to build, kept for the symmetry of the times
+		result.buildTime = timer.ticks();
+
+		std::vector<std::vector<cv::DMatch> > matches;
+		matcher.knnMatch(queries, matches, knn);
+		result.knnTime = timer.ticks();
+
+		result.indices = cv::Mat(queries.rows, knn, CV_32SC1, cv::Scalar(-1));
+		for(size_t i=0; i<matches.size(); ++i)
+		{
+			for(size_t j=0; j<matches[i].size() && (int)j<knn; ++j)
+			{
+				result.indices.at<int>((int)i, (int)j) = matches[i][j].trainIdx;
+			}
+		}
+
+		if(radius > 0.0f)
+		{
+			std::vector<std::vector<cv::DMatch> > radiusMatches;
+			matcher.radiusMatch(queries, radiusMatches, radius);
+			result.radiusTime = timer.ticks();
+		}
+		result.memory = 0; // it indexes nothing
+		if(backend.singleCore)
+		{
+			cv::setNumThreads(threads);
+		}
+		return result;
+	}
+
+	FlannIndex index;
+	UTimer timer;
+	index.buildIndex(backend.algorithm, data, false, rebalancingFactor);
+	result.buildTime = timer.ticks();
+
+	index.knnSearch(queries, result.indices, dists, knn);
+	result.knnTime = timer.ticks();
+
+	if(radius > 0.0f)
+	{
+		std::vector<std::vector<size_t> > radiusIndices;
+		std::vector<std::vector<float> > radiusDists;
+		index.radiusSearch(queries, radiusIndices, radiusDists, radius);
+		result.radiusTime = timer.ticks();
+	}
+
+	result.memory = index.memoryUsed();
+	return result;
+}
+
+// Fraction of the neighbors that are the ones the exhaustive search finds.
+inline float recall(const cv::Mat & indices, const cv::Mat & reference)
+{
+	UASSERT(indices.size() == reference.size());
+	int found = 0;
+	for(int i=0; i<indices.rows; ++i)
+	{
+		for(int j=0; j<indices.cols; ++j)
+		{
+			if(indices.at<int>(i, j) == reference.at<int>(i, j))
+			{
+				++found;
+			}
+		}
+	}
+	return float(found)/float(indices.total());
+}
+
+// Note on the memory column: the rtflann indexes point into the features they
+// were built with, while the nanoflann ones copy them into their own storage,
+// which their memoryUsed() includes.
+inline void report(const char * name, const Result & result, float recallRatio)
+{
+	std::cout << "[          ]   " << name
+			  << " build=" << uFormat("%7.1f", result.buildTime*1000.0) << " ms"
+			  << " knn=" << uFormat("%7.1f", result.knnTime*1000.0) << " ms";
+	if(result.radiusTime >= 0.0)
+	{
+		std::cout << " radius=" << uFormat("%7.1f", result.radiusTime*1000.0) << " ms";
+	}
+	std::cout << " memory=" << uFormat("%6d", (int)(result.memory/1024)) << " KB"
+			  << " recall=" << uFormat("%5.1f", recallRatio*100.0f) << " %"
+			  << std::endl;
+}
+
+// Run every backend of a list on the same data, the first one being the
+// reference the others are compared to.
+inline void compare(
+		const Backend * backends,
+		size_t count,
+		const cv::Mat & data,
+		const cv::Mat & queries,
+		int knn,
+		float radius = 0.0f,
+		float rebalancingFactor = 1.0f)
+{
+	cv::Mat reference;
+	for(size_t i=0; i<count; ++i)
+	{
+		const Result result = run(backends[i], data, queries, knn, radius,
+				backends[i].rebalancingFactor!=2.0f?backends[i].rebalancingFactor:rebalancingFactor);
+		ASSERT_EQ(result.indices.rows, queries.rows) << backends[i].name;
+		if(reference.empty())
+		{
+			reference = result.indices;
+		}
+		report(backends[i].name, result, recall(result.indices, reference));
+	}
+}
+
+} // namespace
+
+#endif /* RTABMAP_CORELIB_TEST_FLANNINDEXBACKENDS_H_ */

corelib/test/perf_flann_index.cpp

@@ -0,0 +1,629 @@
+// Comparison of the FlannIndex backends: times, memory and recall of every
+// algorithm on synthetic point clouds and descriptors.
+//
+// Their own executable, run by ctest under the "performance" label, so that
+// they don't slow down the unit tests and can be scaled up freely:
+//   ctest -L performance    to run them
+//   ctest -LE performance   to skip them
+//
+// Nothing is asserted on the times, which depend on the machine: they are
+// printed so that a change of backend, of parameters or of a vendored library
+// version can be compared to what it replaces.
+#include "FlannIndexBackends.h"
+
+// The times are reported rather than asserted on: which backend is the fastest
+// depends on the machine. They are here so that a change of backend, of
+// parameters or of nanoflann version can be compared to what it replaces.
+TEST(FlannIndexPerfTest, AllBackendsOnPointClouds)
+{
+	const int cloudSize = 100000;
+	const int querySize = 10000;
+	const int knn = 2;
+	const float radius = 1.0f;
+
+	for(int dim = 2; dim <= 3; ++dim)
+	{
+		const cv::Mat cloud = makeCloud(cloudSize, dim, 5);
+		const cv::Mat queries = makeCloud(querySize, dim, 6);
+
+		for(float factor: REBALANCING_FACTORS)
+		{
+			std::cout << "[          ] " << dim << "D cloud of " << cloudSize
+					  << " points, " << querySize << " queries, knn=" << knn
+					  << ", rebalancing factor=" << factor << std::endl;
+			compare(FLOAT_BACKENDS, sizeof(FLOAT_BACKENDS)/sizeof(Backend), cloud, queries, knn, radius, factor);
+		}
+	}
+}
+
+// The same backends on descriptor dimensions, where the exact kd-trees lose
+// their advantage over the exhaustive search: the higher the dimension, the
+// more of the tree a search has to visit. A single rebalancing factor here, it
+// doesn't affect the searches being compared.
+TEST(FlannIndexPerfTest, AllBackendsOnFloatDescriptors)
+{
+	const int descriptorCount = 50000;
+	const int querySize = 300;
+	const int knn = 2;
+
+	for(int dim: {32, 64, 128, 256})
+	{
+		const cv::Mat descriptors = makeDescriptors(descriptorCount, dim, 1000, 11);
+		const cv::Mat queries = perturbedQueries(descriptors, querySize, 12);
+
+		std::cout << "[          ] " << dim << "D float descriptors, " << descriptorCount
+				  << " indexed, " << querySize << " queries, knn=" << knn << std::endl;
+		compare(FLOAT_BACKENDS, sizeof(FLOAT_BACKENDS)/sizeof(Backend), descriptors, queries, knn);
+	}
+}
+
+TEST(FlannIndexPerfTest, AllBackendsOnBinaryDescriptors)
+{
+	const int descriptorCount = 50000;
+	const int querySize = 300;
+	const int knn = 2;
+
+	for(int bytes: {32, 64}) // 256 and 512 bits
+	{
+		const cv::Mat descriptors = makeBinaryDescriptors(descriptorCount, bytes, 1000, 13);
+		const cv::Mat queries = perturbedQueries(descriptors, querySize, 14);
+
+		std::cout << "[          ] " << bytes*8 << " bits binary descriptors, " << descriptorCount
+				  << " indexed, " << querySize << " queries, knn=" << knn << std::endl;
+		compare(BINARY_BACKENDS, sizeof(BINARY_BACKENDS)/sizeof(Backend), descriptors, queries, knn);
+	}
+}
+
+// rtflann's single kd-tree rebuilds itself entirely on every addPoints() call
+// (see KDTreeSingleIndex::addPoints()), which is what the incremental nanoflann
+// tree is for. The cloud is kept small here because of it.
+TEST(FlannIndexPerfTest, IncrementalInsertionSpeed)
+{
+	const int dim = 3;
+	const int cloudSize = 10000;
+	const int addedPoints = 500;
+
+	const cv::Mat cloud = makeCloud(cloudSize, dim, 7);
+	const cv::Mat addedCloud = makeCloud(addedPoints, dim, 8);
+
+	for(float factor: REBALANCING_FACTORS)
+	{
+		std::cout << "[          ] " << dim << "D cloud of " << cloudSize << " points, "
+				  << addedPoints << " points added one by one, rebalancing factor=" << factor << std::endl;
+
+		for(const Backend & backend: INCREMENTAL_BACKENDS)
+		{
+			FlannIndex index;
+			index.buildIndex(backend.algorithm, cloud, false, factor);
+
+			UTimer timer;
+			for(int i=0; i<addedPoints; ++i)
+			{
+				ASSERT_EQ(index.addPoints(addedCloud.row(i)).size(), 1u) << backend.name;
+			}
+			const double addTime = timer.ticks();
+
+			EXPECT_EQ(index.indexedFeatures(), (size_t)(cloudSize + addedPoints)) << backend.name;
+
+			// The added points have to be findable, whichever way they got in.
+			cv::Mat indices;
+			cv::Mat dists;
+			index.knnSearch(addedCloud.row(addedPoints-1), indices, dists, 1);
+			EXPECT_NEAR(dists.at<float>(0, 0), 0.0f, 1e-3f) << backend.name;
+
+			std::cout << "[          ]   " << backend.name
+					  << " insertions=" << uFormat("%7.1f", addTime*1000.0) << " ms"
+					  << " memory=" << uFormat("%6d", (int)(index.memoryUsed()/1024)) << " KB"
+					  << std::endl;
+			RecordProperty(uFormat("alg%d_factor%d_insert_us", (int)backend.algorithm, (int)factor), (int)(addTime*1e6));
+		}
+	}
+}
+
+// What the rebalancing factor buys. rtflann inserts new points into the tree
+// the split planes of which were chosen for the points it was built with, so
+// the tree slowly degrades as it grows; over 1, the factor tells by how much it
+// is allowed to grow before being rebuilt. That only shows on an index that
+// grew a lot since it was built, which is what this does: a quarter of the
+// points are indexed, the rest is added one by one, as the dictionary does.
+//
+// The single kd-trees are not part of it: rtflann's rebuilds itself on every
+// addPoints() whatever the factor, which takes minutes at this size.
+TEST(FlannIndexPerfTest, RebalancingFactorOnAGrowingIndex)
+{
+	const int dim = 128;
+	const int initialCount = 5000;
+	const int addedCount = 10000;
+	const int querySize = 500;
+	const int knn = 2;
+
+	const cv::Mat descriptors = makeDescriptors(initialCount+addedCount, dim, 200, 15);
+	const cv::Mat queries = perturbedQueries(descriptors, querySize, 16);
+
+	// Ground truth over all the points, indexed or added.
+	cv::Mat reference;
+	{
+		FlannIndex linear;
+		cv::Mat dists;
+		linear.buildIndex(FlannIndex::FLANN_INDEX_LINEAR, descriptors, false, 1.0f);
+		linear.knnSearch(queries, reference, dists, knn);
+	}
+
+	const Backend growingBackends[] = {
+		{"rtflann   kd-tree (4 randomized)    ", FlannIndex::FLANN_INDEX_KDTREE},
+		{"nanoflann kd-tree single incremental", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 2.0f},
+	};
+
+	std::cout << "[          ] " << dim << "D float descriptors, " << initialCount
+			  << " indexed then " << addedCount << " added one by one, "
+			  << querySize << " queries, knn=" << knn << std::endl;
+
+	for(const Backend & backend: growingBackends)
+	{
+		for(float factor: REBALANCING_FACTORS)
+		{
+			FlannIndex index;
+			index.buildIndex(backend.algorithm, descriptors.rowRange(0, initialCount), false, factor);
+
+			UTimer timer;
+			for(int i=0; i<addedCount; ++i)
+			{
+				index.addPoints(descriptors.row(initialCount+i));
+			}
+			const double insertTime = timer.ticks();
+
+			cv::Mat indices;
+			cv::Mat dists;
+			index.knnSearch(queries, indices, dists, knn);
+			const double knnTime = timer.ticks();
+
+			ASSERT_EQ(index.indexedFeatures(), (size_t)descriptors.rows) << backend.name;
+
+			std::cout << "[          ]   " << backend.name
+					  << " factor=" << factor
+					  << " insertions=" << uFormat("%7.1f", insertTime*1000.0) << " ms"
+					  << " knn=" << uFormat("%7.1f", knnTime*1000.0) << " ms"
+					  << " memory=" << uFormat("%6d", (int)(index.memoryUsed()/1024)) << " KB"
+					  << " recall=" << uFormat("%5.1f", recall(indices, reference)*100.0f) << " %"
+					  << std::endl;
+		}
+	}
+}
+
+// The index of a dictionary that forgets: as many points removed as added, so
+// the number of indexed points stays the same while the dead ones accumulate.
+// Only a rebuild drops them, which is what a factor over 1 allows.
+TEST(FlannIndexPerfTest, RebalancingFactorOnAChurningIndex)
+{
+	const int dim = 128;
+	const int initialCount = 5000;
+	const int churnCount = 10000;
+	const int querySize = 500;
+	const int knn = 2;
+
+	const cv::Mat descriptors = makeDescriptors(initialCount+churnCount, dim, 200, 17);
+	const cv::Mat queries = perturbedQueries(descriptors, querySize, 18);
+
+	const Backend churningBackends[] = {
+		{"rtflann   kd-tree (4 randomized)    ", FlannIndex::FLANN_INDEX_KDTREE},
+		{"nanoflann kd-tree single incremental", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 2.0f},
+	};
+
+	std::cout << "[          ] " << dim << "D float descriptors, " << initialCount
+			  << " indexed, then " << churnCount << " added and as many removed" << std::endl;
+
+	for(const Backend & backend: churningBackends)
+	{
+		for(float factor: REBALANCING_FACTORS)
+		{
+			FlannIndex index;
+			index.buildIndex(backend.algorithm, descriptors.rowRange(0, initialCount), false, factor);
+
+			UTimer timer;
+			for(int i=0; i<churnCount; ++i)
+			{
+				index.addPoints(descriptors.row(initialCount+i));
+				index.removePoint(i); // the oldest one still indexed
+			}
+			const double churnTime = timer.ticks();
+
+			cv::Mat indices;
+			cv::Mat dists;
+			index.knnSearch(queries, indices, dists, knn);
+			const double knnTime = timer.ticks();
+
+			// Whatever the factor, the same points are searched: the removed
+			// ones are gone from the results either way.
+			EXPECT_EQ(index.indexedFeatures(), (size_t)initialCount) << backend.name;
+
+			std::cout << "[          ]   " << backend.name
+					  << " factor=" << factor
+					  << " add+remove=" << uFormat("%7.1f", churnTime*1000.0) << " ms"
+					  << " knn=" << uFormat("%7.1f", knnTime*1000.0) << " ms"
+					  << " memory=" << uFormat("%6d", (int)(index.memoryUsed()/1024)) << " KB"
+					  << std::endl;
+		}
+	}
+}
+
+namespace {
+
+// Both comparisons below are about the randomized kd-tree: it is the only
+// strategy the dictionary uses whose accuracy can move, the exact ones always
+// returning the true neighbors. Its split dimensions are picked at random, so
+// every configuration is averaged over several seeds, without which a couple of
+// points of recall cannot be told from run to run variation.
+const int DIM = 128;
+const int KNN = 2;
+
+struct Average
+{
+	void add(float recallRatio, double buildTime, double knnTime, size_t memory)
+	{
+		recall_ += recallRatio;
+		buildTime_ += buildTime;
+		knnTime_ += knnTime;
+		memory_ += memory;
+		++count_;
+	}
+	float recall() const {return recall_/float(count_);}
+	double buildTime() const {return buildTime_/double(count_);}
+	double knnTime() const {return knnTime_/double(count_);}
+	size_t memory() const {return memory_/count_;}
+
+	float recall_ = 0.0f;
+	double buildTime_ = 0.0;
+	double knnTime_ = 0.0;
+	size_t memory_ = 0;
+	int count_ = 0;
+};
+
+void reportAverage(const std::string & name, const Average & average)
+{
+	std::cout << "[          ]   " << name
+			  << " build=" << uFormat("%8.1f", average.buildTime()*1000.0) << " ms"
+			  << " knn=" << uFormat("%7.1f", average.knnTime()*1000.0) << " ms"
+			  << " memory=" << uFormat("%7d", (int)(average.memory()/1024)) << " KB"
+			  << " recall=" << uFormat("%5.1f", average.recall()*100.0f) << " %"
+			  << std::endl;
+}
+
+// Descriptors are drawn around cluster centers, and the density of a cluster is
+// what makes a nearest neighbor unambiguous: with too few centers for the
+// number of descriptors, every point of a cluster is about as close to a query
+// as its neighbors are, and the recall of every approximate search collapses
+// whatever the index. Ten descriptors per center keeps that out of the way.
+int clusterCount(int descriptors)
+{
+	return std::max(100, descriptors/10);
+}
+
+// The neighbors an exact search finds, the reference recall is measured
+// against. The single kd-tree is used rather than the exhaustive search: it
+// returns the same neighbors an order of magnitude faster, which is what makes
+// the million descriptors comparisons affordable.
+cv::Mat groundTruth(const cv::Mat & data, const cv::Mat & queries)
+{
+	FlannIndex exact;
+	cv::Mat indices;
+	cv::Mat dists;
+	exact.buildIndex(FlannIndex::FLANN_INDEX_KDTREE_SINGLE, data, false, 1.0f);
+	exact.knnSearch(queries, indices, dists, KNN);
+	return indices;
+}
+
+// Is an index that grew by insertion worse than the same points indexed in one
+// go? The freshly built index is what a rebuild would give, so the difference
+// between the two is exactly what rebuilding on growth would recover.
+void compareGrownAndFreshlyBuilt(int finalCount, int seeds, int queryCount, const std::vector<int> & growths)
+{
+	std::cout << "[          ] " << DIM << "D float descriptors, " << finalCount
+			  << " indexed, " << queryCount << " queries, knn=" << KNN
+			  << ", averaged over " << seeds << " seed" << (seeds>1?"s":"") << std::endl;
+
+	std::map<int, Average> grown; // growth factor -> average
+	Average fresh;
+	for(int seed=0; seed<seeds; ++seed)
+	{
+		const cv::Mat descriptors = makeDescriptors(finalCount, DIM, clusterCount(finalCount), 40+seed);
+		const cv::Mat queries = perturbedQueries(descriptors, queryCount, 60+seed);
+		const cv::Mat reference = groundTruth(descriptors, queries);
+
+		cv::Mat indices;
+		cv::Mat dists;
+
+		// The whole set indexed at once: the tree a rebuild would give.
+		{
+			FlannIndex index;
+			UTimer timer;
+			index.buildIndex(FlannIndex::FLANN_INDEX_KDTREE, descriptors, false, 1.0f);
+			const double buildTime = timer.ticks();
+			index.knnSearch(queries, indices, dists, KNN);
+			fresh.add(recall(indices, reference), buildTime, timer.ticks(), index.memoryUsed());
+		}
+
+		// The same set reached by building the index with finalCount/growth of
+		// the descriptors, then adding all the others one by one, never
+		// rebuilding: the state the index would be in after growing that much.
+		for(int growth: growths)
+		{
+			const int initialCount = finalCount/growth;
+			FlannIndex index;
+			UTimer timer;
+			index.buildIndex(FlannIndex::FLANN_INDEX_KDTREE, descriptors.rowRange(0, initialCount), false, 1.0f);
+			for(int i=initialCount; i<finalCount; ++i)
+			{
+				index.addPoints(descriptors.row(i));
+			}
+			const double buildTime = timer.ticks();
+			ASSERT_EQ(index.indexedFeatures(), (size_t)finalCount);
+
+			index.knnSearch(queries, indices, dists, KNN);
+			grown[growth].add(recall(indices, reference), buildTime, timer.ticks(), index.memoryUsed());
+		}
+	}
+
+	reportAverage("all indexed in one go               ", fresh);
+	for(const auto & iter: grown)
+	{
+		// e.g. 100x: built with 10000 of them, the 990000 others added one by one
+		reportAverage(uFormat("grown %4dx (%8d indexed first)", iter.first, finalCount/iter.first), iter.second);
+	}
+}
+
+// What accumulating removed points costs, and what a rebuild recovers. The
+// index that is never rebuilt keeps them: they still take memory and are still
+// visited by the searches.
+void compareRemovedFractions(int count, int seeds, int queryCount, const std::vector<int> & removedPercents)
+{
+	std::cout << "[          ] " << DIM << "D float descriptors, " << count
+			  << " indexed then partly removed, " << queryCount << " queries, knn="
+			  << KNN << ", averaged over " << seeds << " seed" << (seeds>1?"s":"") << std::endl;
+
+	std::map<int, Average> kept;    // removed % -> index that kept the removed points
+	std::map<int, Average> rebuilt; // removed % -> index built on the live ones only
+	for(int seed=0; seed<seeds; ++seed)
+	{
+		const cv::Mat descriptors = makeDescriptors(count, DIM, clusterCount(count), 80+seed);
+
+		for(int removed: removedPercents)
+		{
+			// Spread the removed points over the whole set.
+			std::vector<int> live;
+			std::vector<int> indexOfLive(count, -1);
+			for(int i=0; i<count; ++i)
+			{
+				if(i%100 >= removed)
+				{
+					indexOfLive[i] = (int)live.size();
+					live.push_back(i);
+				}
+			}
+			cv::Mat liveDescriptors((int)live.size(), DIM, CV_32FC1);
+			for(size_t i=0; i<live.size(); ++i)
+			{
+				descriptors.row(live[i]).copyTo(liveDescriptors.row((int)i));
+			}
+			// Queried with points that are still indexed: queries taken from the
+			// removed ones would lower the recall of both configurations, their
+			// nearest live neighbor being another point altogether.
+			const cv::Mat queries = perturbedQueries(liveDescriptors, queryCount, 100+seed);
+			const cv::Mat reference = groundTruth(liveDescriptors, queries);
+
+			cv::Mat indices;
+			cv::Mat dists;
+
+			// Everything indexed, the removed points only marked as such.
+			{
+				FlannIndex index;
+				UTimer timer;
+				index.buildIndex(FlannIndex::FLANN_INDEX_KDTREE, descriptors, false, 1.0f);
+				for(int i=0; i<count; ++i)
+				{
+					if(indexOfLive[i] < 0)
+					{
+						index.removePoint(i);
+					}
+				}
+				const double buildTime = timer.ticks();
+				ASSERT_EQ(index.indexedFeatures(), live.size());
+
+				index.knnSearch(queries, indices, dists, KNN);
+				const double knnTime = timer.ticks();
+
+				// Its indexes are those of the whole set, the reference's are
+				// those of the live points only.
+				cv::Mat translated(indices.size(), CV_32SC1, cv::Scalar(-1));
+				for(int i=0; i<indices.rows; ++i)
+				{
+					for(int j=0; j<indices.cols; ++j)
+					{
+						const int found = indices.at<int>(i, j);
+						translated.at<int>(i, j) = found>=0?indexOfLive[found]:-1;
+					}
+				}
+				kept[removed].add(recall(translated, reference), buildTime, knnTime, index.memoryUsed());
+			}
+
+			// Only the live points indexed: what rebuilding gives.
+			{
+				FlannIndex index;
+				UTimer timer;
+				index.buildIndex(FlannIndex::FLANN_INDEX_KDTREE, liveDescriptors, false, 1.0f);
+				const double buildTime = timer.ticks();
+				index.knnSearch(queries, indices, dists, KNN);
+				rebuilt[removed].add(recall(indices, reference), buildTime, timer.ticks(), index.memoryUsed());
+			}
+		}
+	}
+
+	for(const auto & iter: kept)
+	{
+		reportAverage(uFormat("%3d%% removed, kept in the index     ", iter.first), iter.second);
+		reportAverage(uFormat("%3d%% removed, rebuilt without them  ", iter.first), rebuilt[iter.first]);
+	}
+}
+
+} // namespace
+
+TEST(FlannIndexPerfTest, GrownIndexAgainstFreshlyBuiltOne)
+{
+	compareGrownAndFreshlyBuilt(20000, 3, 200, {2, 10, 100, 1000});
+}
+
+TEST(FlannIndexPerfTest, RecallAgainstTheFractionOfRemovedPoints)
+{
+	compareRemovedFractions(10000, 3, 200, {0, 25, 50, 75, 90});
+}
+
+// The same two comparisons on a dictionary of a million words, where a single
+// descriptor matrix is already 512 MB and each of them takes minutes. Disabled
+// so that a plain run of this executable stays in the seconds, run them with:
+//   bin/test_flann_index_perf --gtest_also_run_disabled_tests
+// They stay compiled, so they cannot rot as FlannIndex changes.
+TEST(FlannIndexPerfTest, DISABLED_GrownIndexAgainstFreshlyBuiltOneOnAMillionWords)
+{
+	compareGrownAndFreshlyBuilt(1000000, 3, 100, {2, 100, 1000});
+}
+
+TEST(FlannIndexPerfTest, DISABLED_RecallAgainstTheFractionOfRemovedPointsOnAMillionWords)
+{
+	compareRemovedFractions(1000000, 3, 100, {50, 90});
+}
+
+// The search RegistrationVis does per frame when a guess transform is given
+// (Vis/CorGuessWinSize): the keypoints of the frame are indexed, and the points
+// projected from the previous frame are looked up around their projection. The
+// index is built and thrown away every frame, so its build time weighs as much
+// as its search time. Before the nanoflann backend, this was a rtflann
+// randomized kd-tree forest.
+TEST(FlannIndexPerfTest, RegistrationGuessMatching)
+{
+	const int keypoints = 1000;   // Vis/MaxFeatures
+	const float radius = 40.0f;   // Vis/CorGuessWinSize
+	const int frames = 1000;      // ~ a 50 s sequence at 20 Hz
+
+	// Image points rather than a cube of them.
+	cv::RNG rng(140);
+	cv::Mat points(keypoints, 2, CV_32FC1);
+	cv::Mat projected(keypoints, 2, CV_32FC1);
+	for(int i=0; i<keypoints; ++i)
+	{
+		points.at<float>(i, 0) = rng.uniform(0.0f, 640.0f);
+		points.at<float>(i, 1) = rng.uniform(0.0f, 480.0f);
+		projected.at<float>(i, 0) = rng.uniform(0.0f, 640.0f);
+		projected.at<float>(i, 1) = rng.uniform(0.0f, 480.0f);
+	}
+
+	// A factor of 1 for the rtflann rows keeps their per-point bookkeeping out
+	// of the measurement, and picks the nanoflann tree that is built once.
+	const Backend backends[] = {
+		{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true},
+		{"rtflann   kd-tree (4 randomized)    ", FlannIndex::FLANN_INDEX_KDTREE, 1.0f},
+		{"rtflann   kd-tree single            ", FlannIndex::FLANN_INDEX_KDTREE_SINGLE, 1.0f},
+		{"nanoflann kd-tree single            ", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 1.0f},
+		{"nanoflann kd-tree single incremental", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 2.0f},
+	};
+
+	std::cout << "[          ] " << keypoints << " keypoints indexed and as many looked up in a "
+			  << radius << " px radius, per frame" << std::endl;
+
+	for(const Backend & backend: backends)
+	{
+		std::vector<std::vector<size_t> > indices;
+		std::vector<std::vector<float> > dists;
+
+		UTimer timer;
+		for(int frame=0; frame<frames; ++frame)
+		{
+			FlannIndex index;
+			index.buildIndex(backend.algorithm, points, false, backend.rebalancingFactor);
+			index.radiusSearch(projected, indices, dists, radius, 0, 32, 0.0f, false);
+		}
+		const double perFrame = timer.ticks()/double(frames);
+
+		size_t found = 0;
+		for(const auto & neighbors: indices)
+		{
+			found += neighbors.size();
+		}
+		ASSERT_GT(found, 0u) << backend.name;
+
+		std::cout << "[          ]   " << backend.name
+				  << " build+search=" << uFormat("%6.3f", perFrame*1000.0) << " ms/frame"
+				  << " (" << uFormat("%5.2f", perFrame*1000.0*20.0) << " ms/s at 20 Hz)"
+				  << std::endl;
+	}
+}
+
+// The dictionary that is built to match two sets of descriptors and thrown
+// away: the "from" ones are indexed, the "to" ones are searched with knn=2 for
+// the ratio test. Both are a frame's worth of features, or a feature map's
+// worth for the odometry, which makes the build weigh as much as the searches,
+// unlike the vocabulary sized comparison above.
+namespace {
+
+void compareDictionaryMatching(int indexedCount, int queriedCount)
+{
+	const int frames = 10;
+
+	// A factor of 1 as RegistrationVis sets it for that dictionary: the index is
+	// built once, so it is neither kept ready to be added to nor rebuilt. The
+	// incremental nanoflann tree is kept in the comparison to show what asking
+	// for one costs here.
+	const Backend backends[] = {
+		{"linear    exhaustive                ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f},
+		{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true},
+		{"rtflann   kd-tree (4 randomized)    ", FlannIndex::FLANN_INDEX_KDTREE, 1.0f},
+		{"rtflann   kd-tree single            ", FlannIndex::FLANN_INDEX_KDTREE_SINGLE, 1.0f},
+		{"nanoflann kd-tree single            ", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 1.0f},
+		{"nanoflann kd-tree single incremental", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 2.0f},
+	};
+
+	for(int dim: {32, 64, 128, 256})
+	{
+		const cv::Mat from = makeDescriptors(indexedCount, dim, clusterCount(indexedCount), 150);
+		const cv::Mat to = perturbedQueries(from, queriedCount, 151);
+		const cv::Mat reference = groundTruth(from, to);
+
+		std::cout << "[          ] " << dim << "D float descriptors, " << indexedCount
+				  << " indexed and " << queriedCount << " matched against them, per frame" << std::endl;
+
+		for(const Backend & backend: backends)
+		{
+			cv::Mat indices;
+			cv::Mat dists;
+
+			UTimer timer;
+			for(int frame=0; frame<frames; ++frame)
+			{
+				FlannIndex index;
+				index.buildIndex(backend.algorithm, from, false, backend.rebalancingFactor);
+				index.knnSearch(to, indices, dists, KNN);
+			}
+			const double perFrame = timer.ticks()/double(frames);
+
+			std::cout << "[          ]   " << backend.name
+					  << " build+search=" << uFormat("%7.2f", perFrame*1000.0) << " ms/frame"
+					  << " recall=" << uFormat("%5.1f", recall(indices, reference)*100.0f) << " %"
+					  << std::endl;
+		}
+	}
+}
+
+} // namespace
+
+// What RegistrationVis does to match two frames: as many descriptors indexed as
+// searched (Vis/MaxFeatures on both sides).
+TEST(FlannIndexPerfTest, RegistrationDictionaryMatching)
+{
+	compareDictionaryMatching(1000, 1000);
+}
+
+// What OdometryF2M does: the frame is matched against the feature map, which
+// holds more of them (Odom/F2M/MaxSize), so the index is bigger than the set of
+// queries and its build weighs more.
+TEST(FlannIndexPerfTest, OdometryFrameToMapMatching)
+{
+	compareDictionaryMatching(2000, 1000);
+}

corelib/test/test_flann_index.cpp

@@ -0,0 +1,548 @@
+#include "FlannIndexBackends.h"
+
+TEST(FlannIndexTest, ExactBackendsFindTheSameNeighbors)
+{
+	for(int dim = 2; dim <= 3; ++dim)
+	{
+		const cv::Mat cloud = makeCloud(5000, dim, 1);
+		// An odd number of queries: with an odd knn, the rtflann backend used
+		// to write one index past the end of the output matrix, which needs
+		// query.rows*knn to be odd to show.
+		const cv::Mat queries = makeCloud(501, dim, 2);
+		for(int knn = 1; knn <= 2; ++knn)
+		{
+			cv::Mat referenceIndices;
+			cv::Mat referenceDists;
+			for(const Backend & backend: EXACT_BACKENDS)
+			{
+				FlannIndex index;
+				index.buildIndex(backend.algorithm, cloud, false, backend.rebalancingFactor);
+				ASSERT_TRUE(index.isBuilt()) << backend.name;
+				EXPECT_EQ(index.indexedFeatures(), (size_t)cloud.rows) << backend.name;
+
+				cv::Mat indices;
+				cv::Mat dists;
+				index.knnSearch(queries, indices, dists, knn);
+				ASSERT_EQ(indices.rows, queries.rows) << backend.name;
+				ASSERT_EQ(indices.cols, knn) << backend.name;
+
+				if(referenceIndices.empty())
+				{
+					referenceIndices = indices;
+					referenceDists = dists;
+					// Sanity check the reference itself: every query is inside
+					// the cloud's box, so all neighbors have been found.
+					for(int i=0; i<indices.rows; ++i)
+					{
+						for(int j=0; j<knn; ++j)
+						{
+							ASSERT_GE(indices.at<int>(i, j), 0) << "dim=" << dim << " knn=" << knn;
+						}
+					}
+					continue;
+				}
+
+				for(int i=0; i<indices.rows; ++i)
+				{
+					for(int j=0; j<knn; ++j)
+					{
+						EXPECT_EQ(indices.at<int>(i, j), referenceIndices.at<int>(i, j))
+							<< backend.name << " dim=" << dim << " knn=" << knn << " query=" << i << " n=" << j;
+						EXPECT_NEAR(dists.at<float>(i, j), referenceDists.at<float>(i, j), 1e-3f)
+							<< backend.name << " dim=" << dim << " knn=" << knn << " query=" << i << " n=" << j;
+					}
+				}
+			}
+		}
+	}
+}
+
+TEST(FlannIndexTest, ExactBackendsFindTheSamePointsInRadius)
+{
+	for(int dim = 2; dim <= 3; ++dim)
+	{
+		const cv::Mat cloud = makeCloud(5000, dim, 3);
+		const cv::Mat queries = makeCloud(200, dim, 4);
+		const float radius = 5.0f;
+
+		std::vector<std::vector<size_t> > referenceIndices;
+		for(const Backend & backend: EXACT_BACKENDS)
+		{
+			FlannIndex index;
+			index.buildIndex(backend.algorithm, cloud, false, backend.rebalancingFactor);
+
+			std::vector<std::vector<size_t> > indices;
+			std::vector<std::vector<float> > dists;
+			index.radiusSearch(queries, indices, dists, radius);
+			ASSERT_EQ(indices.size(), (size_t)queries.rows) << backend.name;
+
+			// The backends don't return the points in the same order when they
+			// are not sorted by distance, compare them as sets.
+			for(size_t i=0; i<indices.size(); ++i)
+			{
+				std::sort(indices[i].begin(), indices[i].end());
+			}
+
+			if(referenceIndices.empty())
+			{
+				referenceIndices = indices;
+				size_t found = 0;
+				for(const auto & neighbors: indices)
+				{
+					found += neighbors.size();
+				}
+				ASSERT_GT(found, 0u) << "dim=" << dim << ", the radius is too small to compare anything";
+				continue;
+			}
+
+			for(size_t i=0; i<indices.size(); ++i)
+			{
+				EXPECT_EQ(indices[i], referenceIndices[i]) << backend.name << " dim=" << dim << " query=" << i;
+			}
+		}
+	}
+}
+
+TEST(FlannIndexTest, SerializedIndexIsLoadedBack)
+{
+	const cv::Mat cloud = makeCloud(2000, 3, 20);
+	const cv::Mat queries = makeCloud(101, 3, 21);
+	const int knn = 2;
+
+	for(const Backend & backend: EXACT_BACKENDS)
+	{
+		for(bool checksum: {true, false})
+		{
+			FlannIndex index;
+			index.buildIndex(backend.algorithm, cloud, false, backend.rebalancingFactor);
+			cv::Mat indices;
+			cv::Mat dists;
+			index.knnSearch(queries, indices, dists, knn);
+
+			const std::vector<unsigned char> data = index.serializeIndex(checksum);
+#ifdef _WIN32
+			// rtflann serialization needs fmemopen, only the nanoflann backends
+			// give back something on Windows.
+			if(backend.algorithm != FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE)
+			{
+				EXPECT_TRUE(data.empty()) << backend.name;
+				continue;
+			}
+#endif
+			ASSERT_FALSE(data.empty()) << backend.name << " checksum=" << checksum;
+
+			FlannIndex loaded;
+			std::string error;
+			ASSERT_TRUE(loaded.loadIndex(data, backend.algorithm, cloud, false, backend.rebalancingFactor, &error))
+				<< backend.name << " checksum=" << checksum << ": " << error;
+			EXPECT_TRUE(loaded.isBuilt()) << backend.name;
+			EXPECT_EQ(loaded.indexedFeatures(), (size_t)cloud.rows) << backend.name;
+			EXPECT_EQ(loaded.featuresType(), cloud.type()) << backend.name;
+			EXPECT_EQ(loaded.featuresDim(), cloud.cols) << backend.name;
+
+			// The loaded index has to give the very same neighbors.
+			cv::Mat loadedIndices;
+			cv::Mat loadedDists;
+			loaded.knnSearch(queries, loadedIndices, loadedDists, knn);
+			ASSERT_EQ(loadedIndices.size(), indices.size()) << backend.name;
+			for(int i=0; i<indices.rows; ++i)
+			{
+				for(int j=0; j<knn; ++j)
+				{
+					EXPECT_EQ(loadedIndices.at<int>(i, j), indices.at<int>(i, j))
+						<< backend.name << " checksum=" << checksum << " query=" << i;
+				}
+			}
+
+			// The raw pointer overload takes the same data.
+			FlannIndex loadedRaw;
+			EXPECT_TRUE(loadedRaw.loadIndex(data.data(), data.size(), backend.algorithm, cloud, false, backend.rebalancingFactor, &error))
+				<< backend.name << ": " << error;
+		}
+	}
+}
+
+TEST(FlannIndexTest, LoadIndexRefusesDataItCannotUse)
+{
+	const cv::Mat cloud = makeCloud(500, 3, 22);
+	FlannIndex index;
+	index.buildIndex(FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, cloud);
+	const std::vector<unsigned char> data = index.serializeIndex(true);
+	ASSERT_FALSE(data.empty());
+
+	const FlannIndex::flann_algorithm_t algorithm = FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE;
+	FlannIndex loaded;
+	std::string error;
+
+	// Another algorithm than the one it was built with
+	EXPECT_FALSE(loaded.loadIndex(data, FlannIndex::FLANN_INDEX_KDTREE_SINGLE, cloud, false, 2.0f, &error));
+	EXPECT_FALSE(error.empty());
+
+	// Another number of features
+	error.clear();
+	EXPECT_FALSE(loaded.loadIndex(data, algorithm, cloud.rowRange(0, cloud.rows-1), false, 2.0f, &error));
+	EXPECT_FALSE(error.empty());
+
+	// Another dimension
+	error.clear();
+	EXPECT_FALSE(loaded.loadIndex(data, algorithm, makeCloud(cloud.rows, 2, 22), false, 2.0f, &error));
+	EXPECT_FALSE(error.empty());
+
+	// Another distance
+	error.clear();
+	EXPECT_FALSE(loaded.loadIndex(data, algorithm, cloud, true, 2.0f, &error));
+	EXPECT_FALSE(error.empty());
+
+	// Same shape, other content: caught by the checksum
+	error.clear();
+	EXPECT_FALSE(loaded.loadIndex(data, algorithm, makeCloud(cloud.rows, cloud.cols, 23), false, 2.0f, &error));
+	EXPECT_FALSE(error.empty());
+
+	// Truncated
+	error.clear();
+	EXPECT_FALSE(loaded.loadIndex(data.data(), data.size()/2, algorithm, cloud, false, 2.0f, &error));
+	EXPECT_FALSE(error.empty());
+
+	// Nothing at all
+	error.clear();
+	std::vector<unsigned char> empty;
+	EXPECT_FALSE(loaded.loadIndex(empty, algorithm, cloud, false, 2.0f, &error));
+
+	// None of it left a half loaded index behind
+	EXPECT_FALSE(loaded.isBuilt());
+}
+
+// A descriptor header can cover a whole batch of points when the index is never
+// rebuilt (rebalancing factor of 1), which used to make serializeIndex() look
+// them up one by one and throw.
+TEST(FlannIndexTest, SerializesAnIndexWithBatchedHeadersAndRemovedPoints)
+{
+	const cv::Mat cloud = makeCloud(500, 3, 24);
+	const cv::Mat added = makeCloud(100, 3, 25);
+
+	for(float factor: {1.0f, 2.0f})
+	{
+		FlannIndex index;
+		index.buildIndex(FlannIndex::FLANN_INDEX_KDTREE, cloud, false, factor);
+		const std::vector<unsigned int> indexes = index.addPoints(added);
+		ASSERT_EQ(indexes.size(), (size_t)added.rows);
+
+		for(size_t i=0; i<10; ++i)
+		{
+			index.removePoint(indexes[i]);
+		}
+		EXPECT_EQ(index.indexedFeatures(), (size_t)(cloud.rows + added.rows - 10)) << "factor=" << factor;
+
+		EXPECT_NO_THROW(index.serializeIndex(true)) << "factor=" << factor;
+		EXPECT_NO_THROW(index.serializeIndex(false)) << "factor=" << factor;
+	}
+}
+
+TEST(FlannIndexTest, AddedPointsAreFoundAndRemovedOnesAreNot)
+{
+	const cv::Mat cloud = makeCloud(500, 3, 26);
+	const cv::Mat added = makeCloud(50, 3, 27);
+
+	const Backend backends[] = {
+		{"rtflann   kd-tree (4 randomized)    ", FlannIndex::FLANN_INDEX_KDTREE},
+		{"nanoflann kd-tree single incremental", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 2.0f},
+	};
+
+	for(const Backend & backend: backends)
+	{
+		FlannIndex index;
+		index.buildIndex(backend.algorithm, cloud, false, backend.rebalancingFactor);
+		ASSERT_EQ(index.indexedFeatures(), (size_t)cloud.rows) << backend.name;
+
+		const std::vector<unsigned int> indexes = index.addPoints(added);
+		ASSERT_EQ(indexes.size(), (size_t)added.rows) << backend.name;
+		for(size_t i=0; i<indexes.size(); ++i)
+		{
+			EXPECT_EQ(indexes[i], (unsigned int)(cloud.rows + i)) << backend.name;
+		}
+		EXPECT_EQ(index.indexedFeatures(), (size_t)(cloud.rows + added.rows)) << backend.name;
+
+		// An added point is its own nearest neighbor.
+		cv::Mat indices;
+		cv::Mat dists;
+		index.knnSearch(added.row(0), indices, dists, 1);
+		EXPECT_EQ(indices.at<int>(0, 0), (int)indexes[0]) << backend.name;
+		EXPECT_NEAR(dists.at<float>(0, 0), 0.0f, 1e-3f) << backend.name;
+
+		// Once removed, it is not returned anymore, by either search.
+		index.removePoint(indexes[0]);
+		EXPECT_EQ(index.indexedFeatures(), (size_t)(cloud.rows + added.rows - 1)) << backend.name;
+
+		index.knnSearch(added.row(0), indices, dists, 1);
+		EXPECT_NE(indices.at<int>(0, 0), (int)indexes[0]) << backend.name;
+
+		std::vector<std::vector<size_t> > radiusIndices;
+		std::vector<std::vector<float> > radiusDists;
+		index.radiusSearch(added.row(0), radiusIndices, radiusDists, 1.0f);
+		ASSERT_EQ(radiusIndices.size(), 1u) << backend.name;
+		for(size_t neighbor: radiusIndices[0])
+		{
+			EXPECT_NE(neighbor, (size_t)indexes[0]) << backend.name;
+		}
+	}
+}
+
+// The rebuild triggered by the removals renumbers nothing: the indexes handed
+// out before it still designate the same points, which VWDictionary relies on.
+TEST(FlannIndexTest, IndexesSurviveARebuild)
+{
+	const cv::Mat cloud = makeCloud(400, 3, 28);
+	const cv::Mat added = makeCloud(400, 3, 29);
+
+	const Backend backends[] = {
+		{"rtflann   kd-tree (4 randomized)    ", FlannIndex::FLANN_INDEX_KDTREE},
+		{"nanoflann kd-tree single incremental", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 2.0f},
+	};
+
+	for(const Backend & backend: backends)
+	{
+		FlannIndex index;
+		index.buildIndex(backend.algorithm, cloud, false, 2.0f);
+
+		std::vector<unsigned int> indexes;
+		for(int i=0; i<added.rows; ++i)
+		{
+			const std::vector<unsigned int> added1 = index.addPoints(added.row(i));
+			ASSERT_EQ(added1.size(), 1u) << backend.name;
+			indexes.push_back(added1[0]);
+		}
+
+		// Enough removals to get over the ratio a factor of 2 tolerates.
+		for(int i=0; i<cloud.rows; ++i)
+		{
+			index.removePoint(i);
+		}
+		EXPECT_EQ(index.indexedFeatures(), (size_t)added.rows) << backend.name;
+
+		// The points added before the rebuild are still where they were.
+		cv::Mat indices;
+		cv::Mat dists;
+		index.knnSearch(added, indices, dists, 1);
+		for(int i=0; i<added.rows; ++i)
+		{
+			EXPECT_EQ(indices.at<int>(i, 0), (int)indexes[i]) << backend.name << " point=" << i;
+		}
+	}
+}
+
+TEST(FlannIndexTest, UnsupportedOperationsAreRefused)
+{
+	const cv::Mat cloud = makeCloud(200, 3, 30);
+	const cv::Mat added = makeCloud(10, 3, 31);
+
+	// A nanoflann index built to never be rebuilt (factor 1) still takes points,
+	// rebuilding itself as the tree that accepts them.
+	FlannIndex staticIndex;
+	staticIndex.buildIndex(FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, cloud, false, 1.0f);
+	const std::vector<unsigned int> addedIndexes = staticIndex.addPoints(added);
+	ASSERT_EQ(addedIndexes.size(), (size_t)added.rows);
+	EXPECT_EQ(addedIndexes[0], (unsigned int)cloud.rows);
+	EXPECT_EQ(staticIndex.indexedFeatures(), (size_t)(cloud.rows + added.rows));
+	staticIndex.removePoint(0);
+	EXPECT_EQ(staticIndex.indexedFeatures(), (size_t)(cloud.rows + added.rows - 1));
+
+	// The points it held are still there, under the same indexes.
+	cv::Mat indices;
+	cv::Mat dists;
+	staticIndex.knnSearch(cloud.row(1), indices, dists, 1);
+	EXPECT_EQ(indices.at<int>(0, 0), 1);
+	staticIndex.knnSearch(added.row(0), indices, dists, 1);
+	EXPECT_EQ(indices.at<int>(0, 0), (int)addedIndexes[0]);
+
+	// An index with removed points refers to holes in the features it was built
+	// with, which the ones given back to loadIndex() cannot reproduce.
+	FlannIndex incremental;
+	incremental.buildIndex(FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, cloud, false, 2.0f);
+	EXPECT_FALSE(incremental.serializeIndex(true).empty());
+	incremental.removePoint(0);
+	EXPECT_TRUE(incremental.serializeIndex(true).empty());
+}
+
+TEST(FlannIndexTest, DistanceL1BackendsAgree)
+{
+	const cv::Mat cloud = makeCloud(1000, 8, 32);
+	const cv::Mat queries = makeCloud(51, 8, 33);
+	const int knn = 2;
+
+	cv::Mat reference;
+	cv::Mat referenceDists;
+	for(const Backend & backend: EXACT_BACKENDS)
+	{
+		FlannIndex index;
+		index.buildIndex(backend.algorithm, cloud, true /* useDistanceL1 */, backend.rebalancingFactor);
+
+		cv::Mat indices;
+		cv::Mat dists;
+		index.knnSearch(queries, indices, dists, knn);
+		ASSERT_EQ(indices.rows, queries.rows) << backend.name;
+
+		if(reference.empty())
+		{
+			reference = indices;
+			referenceDists = dists;
+			// The exhaustive reference is L1 indeed, not L2.
+			const float * query = queries.ptr<float>(0);
+			const float * neighbor = cloud.ptr<float>(indices.at<int>(0, 0));
+			float l1 = 0.0f;
+			for(int i=0; i<cloud.cols; ++i)
+			{
+				l1 += std::abs(query[i] - neighbor[i]);
+			}
+			EXPECT_NEAR(dists.at<float>(0, 0), l1, 1e-2f);
+			continue;
+		}
+
+		for(int i=0; i<indices.rows; ++i)
+		{
+			for(int j=0; j<knn; ++j)
+			{
+				EXPECT_EQ(indices.at<int>(i, j), reference.at<int>(i, j)) << backend.name << " query=" << i;
+				EXPECT_NEAR(dists.at<float>(i, j), referenceDists.at<float>(i, j), 1e-2f) << backend.name;
+			}
+		}
+	}
+}
+
+TEST(FlannIndexTest, BinaryDescriptorsUseHammingDistances)
+{
+	const cv::Mat descriptors = makeBinaryDescriptors(1000, 32, 100, 34);
+	const cv::Mat queries = descriptors.rowRange(0, 20); // the indexed ones
+
+	const Backend backends[] = {
+		{"linear    exhaustive (hamming)      ", FlannIndex::FLANN_INDEX_LINEAR},
+		{"rtflann   LSH                       ", FlannIndex::FLANN_INDEX_LSH},
+	};
+
+	for(const Backend & backend: backends)
+	{
+		FlannIndex index;
+		index.buildIndex(backend.algorithm, descriptors, false, backend.rebalancingFactor);
+		EXPECT_EQ(index.featuresType(), CV_8UC1) << backend.name;
+		EXPECT_EQ(index.featuresDim(), descriptors.cols) << backend.name;
+
+		cv::Mat indices;
+		cv::Mat dists;
+		index.knnSearch(queries, indices, dists, 1);
+		// Hamming distances are integers
+		ASSERT_EQ(dists.type(), CV_32S) << backend.name;
+		for(int i=0; i<queries.rows; ++i)
+		{
+			EXPECT_EQ(indices.at<int>(i, 0), i) << backend.name << " query=" << i;
+			EXPECT_EQ(dists.at<int>(i, 0), 0) << backend.name << " query=" << i;
+		}
+	}
+}
+
+TEST(FlannIndexTest, RadiusSearchKeepsTheNearestMaxNeighbors)
+{
+	const cv::Mat cloud = makeCloud(2000, 2, 35);
+	const cv::Mat queries = makeCloud(50, 2, 36);
+	const float radius = 10.0f;
+	const int maxNeighbors = 3;
+
+	std::vector<std::vector<size_t> > reference;
+	for(const Backend & backend: EXACT_BACKENDS)
+	{
+		FlannIndex index;
+		index.buildIndex(backend.algorithm, cloud, false, backend.rebalancingFactor);
+
+		std::vector<std::vector<size_t> > indices;
+		std::vector<std::vector<float> > dists;
+		index.radiusSearch(queries, indices, dists, radius, maxNeighbors, 32, 0.0f, true);
+		ASSERT_EQ(indices.size(), (size_t)queries.rows) << backend.name;
+
+		for(size_t i=0; i<indices.size(); ++i)
+		{
+			EXPECT_LE(indices[i].size(), (size_t)maxNeighbors) << backend.name << " query=" << i;
+			ASSERT_EQ(indices[i].size(), dists[i].size()) << backend.name;
+			// sorted=true, so they come back closest first
+			for(size_t j=1; j<dists[i].size(); ++j)
+			{
+				EXPECT_LE(dists[i][j-1], dists[i][j]) << backend.name << " query=" << i;
+			}
+		}
+
+		if(reference.empty())
+		{
+			reference = indices;
+			size_t truncated = 0;
+			for(const auto & neighbors: reference)
+			{
+				truncated += neighbors.size() == (size_t)maxNeighbors ? 1 : 0;
+			}
+			ASSERT_GT(truncated, 0u) << "the radius is too small to truncate anything";
+			continue;
+		}
+		for(size_t i=0; i<indices.size(); ++i)
+		{
+			EXPECT_EQ(indices[i], reference[i]) << backend.name << " query=" << i;
+		}
+	}
+}
+
+TEST(FlannIndexTest, ReleasedIndexIsEmptyAndSearchable)
+{
+	const cv::Mat cloud = makeCloud(100, 3, 37);
+
+	for(const Backend & backend: EXACT_BACKENDS)
+	{
+		FlannIndex index;
+		EXPECT_FALSE(index.isBuilt()) << backend.name;
+		EXPECT_EQ(index.indexedFeatures(), 0u) << backend.name;
+		EXPECT_EQ(index.memoryUsed(), 0u) << backend.name;
+
+		index.buildIndex(backend.algorithm, cloud, false, backend.rebalancingFactor);
+		EXPECT_TRUE(index.isBuilt()) << backend.name;
+		EXPECT_GT(index.memoryUsed(), 0u) << backend.name;
+
+		index.release();
+		EXPECT_FALSE(index.isBuilt()) << backend.name;
+		EXPECT_EQ(index.indexedFeatures(), 0u) << backend.name;
+
+		// Searching an index that is not built is an error, not a crash.
+		cv::Mat indices;
+		cv::Mat dists;
+		index.knnSearch(cloud.row(0), indices, dists, 1);
+		EXPECT_TRUE(indices.empty()) << backend.name;
+
+		std::vector<std::vector<size_t> > radiusIndices;
+		std::vector<std::vector<float> > radiusDists;
+		index.radiusSearch(cloud.row(0), radiusIndices, radiusDists, 1.0f);
+		EXPECT_TRUE(radiusIndices.empty()) << backend.name;
+	}
+}
+
+TEST(FlannIndexTest, AsksForMoreNeighborsThanIndexed)
+{
+	const cv::Mat cloud = makeCloud(3, 3, 38);
+	const int knn = 5;
+
+	for(const Backend & backend: EXACT_BACKENDS)
+	{
+		FlannIndex index;
+		index.buildIndex(backend.algorithm, cloud, false, backend.rebalancingFactor);
+
+		cv::Mat indices;
+		cv::Mat dists;
+		index.knnSearch(cloud.row(0), indices, dists, knn);
+		ASSERT_EQ(indices.cols, knn) << backend.name;
+
+		// The neighbors that couldn't be found are marked
+		for(int j=0; j<knn; ++j)
+		{
+			if(j < cloud.rows)
+			{
+				EXPECT_GE(indices.at<int>(0, j), 0) << backend.name << " n=" << j;
+			}
+			else
+			{
+				EXPECT_EQ(indices.at<int>(0, j), -1) << backend.name << " n=" << j;
+			}
+		}
+	}
+}

corelib/test/test_registrationvis.cpp

@@ -135,14 +135,14 @@ static Transform computeRegistration(
 		const SensorData & fromData,
 		const SensorData & toData,
 		const ParametersMap & params,
-		RegistrationInfo * infoOut = nullptr)
+		RegistrationInfo * infoOut = nullptr,
+		const Transform & guess = Transform())
 {
 	RegistrationVis reg(params);
 	Signature from(fromData);
 	Signature to(toData);
 	RegistrationInfo info;
-	Transform nullGuess;
+	return reg.computeTransformation(from, to, guess, infoOut ? infoOut : &info);
-	return reg.computeTransformation(from, to, nullGuess, infoOut ? infoOut : &info);
 }
 
 // Golden transforms (GFTT/ORB, MinDistance=3, QualityLevel=0.01, MaxFeatures=3000, RoiRatios=0 0 0 0.3).
@@ -197,7 +197,8 @@ static Transform computeRegistrationRobust(
 		const SensorData & fromData,
 		const SensorData & toData,
 		const ParametersMap & params,
-		RegistrationInfo * infoOut = nullptr)
+		RegistrationInfo * infoOut = nullptr,
+		const Transform & guess = Transform())
 {
 	const int estimationType = std::atoi(params.at(Parameters::kVisEstimationType()).c_str());
 	// Epipolar (type=2) is the most RANSAC-sensitive so it gets the biggest
@@ -213,7 +214,7 @@ static Transform computeRegistrationRobust(
 	{
 		cv::theRNG() = cv::RNG(static_cast<uint64_t>(0x9e3779b97f4a7c15ULL) ^
 				static_cast<uint64_t>(attempt + 1));
-		result = computeRegistration(fromData, toData, params, &info);
+		result = computeRegistration(fromData, toData, params, &info, guess);
 		if(!result.isNull())
 		{
 			break;
@@ -514,6 +515,44 @@ TEST(RegistrationVisTest, StereoFeatureMatchingAndOpticalFlowSucceed3DTo3D)
 }
 
 #if defined(RTABMAP_G2O)
+// Guess based matching (Vis/CorGuessWinSize): the words of "from" are projected
+// into "to" with the guess, and only the features within a radius of their
+// projection are compared. Vis/CorGuessMatchToProjection picks which of the two
+// sets is indexed and which one searches it, so both directions index 2D points
+// and search them by radius.
+TEST(RegistrationVisTest, RgbdGuessMatchingBothDirections)
+{
+	const SensorData fromData = loadRgbdSensorData("17");
+	const SensorData toData = loadRgbdSensorData("154");
+	ASSERT_FALSE(fromData.imageRaw().empty());
+	ASSERT_FALSE(toData.imageRaw().empty());
+
+	// A guess a few centimeters and a degree away from the answer, the way
+	// odometry gives one. Far enough to matter, close enough for the projected
+	// words to land inside the search radius.
+	const Transform expected = kRgbd17To154Expected[1]; // PnP
+	const Transform guess = expected * Transform(0.03f, -0.02f, 0.01f, 0.0f, 0.0f, 0.02f);
+
+	for(bool matchToProjection: {false, true})
+	{
+		ParametersMap params = registrationVisTestParams(1 /* PnP */);
+		params[Parameters::kVisCorGuessMatchToProjection()] = matchToProjection?"true":"false";
+
+		RegistrationInfo info;
+		const Transform result = computeRegistrationRobust(fromData, toData, params, &info, guess);
+
+		const std::string label = std::string(Parameters::kVisCorGuessMatchToProjection()) +
+				(matchToProjection?"=true":"=false");
+		expectTransformNearExpected(result, expected, kGoldenTransTolM, kGoldenAngleTolRad, label);
+		EXPECT_GE(info.inliers, 6) << label;
+		// Only the guess based matching fills projectedIDs, whichever of its two
+		// directions is taken, so this is what tells that it produced the
+		// result rather than the plain descriptor matching. OdometryF2M relies
+		// on them being filled to know which words of its map are still seen.
+		EXPECT_FALSE(info.projectedIDs.empty()) << label;
+	}
+}
+
 TEST(RegistrationVisTest, RgbdTwoFramesMatchExpectedTransformWithG2oBundleAdjustment)
 {
 	const SensorData fromData = loadRgbdSensorData("17");

corelib/test/test_rtabmap_integration.cpp

@@ -2420,8 +2420,9 @@ TEST_F(RtabmapIntegrationFixture, AppearanceOnly_PrecisionRecall)
 		const bool looseFloors = binaryDescriptors || daisyDescriptor;
 		const bool xfeatures2dDescriptor = freakOrBriefDescriptor || daisyDescriptor;
 
+		const bool kazeDescriptor = detectorType == Feature2D::kFeatureKaze;
 		const float kMinPrecision = tfIdfUsed ? 0.70f :
-				(looseFloors ? 0.85f : 0.9f);
+				(looseFloors || kazeDescriptor ? 0.85f : 0.9f);
 		const float kMinRecall    = xfeatures2dDescriptor ? 0.5f :
 				(looseFloors ? 0.7f : 0.85f);
 		EXPECT_GE(acceptedPrec, kMinPrecision)

corelib/test/test_vwdictionary.cpp

@@ -20,6 +20,20 @@
 
 using namespace rtabmap;
 
+namespace {
+// Spelled out rather than taken from VWDictionary: the point is to check the
+// strategies that are expected to build an index, not to agree with whatever
+// the implementation classifies as one.
+bool hasFlannIndex(VWDictionary::NNStrategy strategy)
+{
+    return strategy == VWDictionary::kNNFlannNaive ||
+           strategy == VWDictionary::kNNFlannKdTree ||
+           strategy == VWDictionary::kNNFlannLSH ||
+           strategy == VWDictionary::kNNNanoFlannKdTree ||
+           strategy == VWDictionary::kNNFlannKdTreeSingle;
+}
+} // namespace
+
 class VWDictionaryTest : public ::testing::Test {
 protected:
     void SetUp() override {
@@ -52,7 +66,9 @@ TEST_F(VWDictionaryTest, AddNewWordsIncremental)
         VWDictionary::kNNFlannKdTree,
         VWDictionary::kNNFlannLSH,
         VWDictionary::kNNBruteForce,
-        VWDictionary::kNNBruteForceGPU
+        VWDictionary::kNNBruteForceGPU,
+        VWDictionary::kNNNanoFlannKdTree,
+        VWDictionary::kNNFlannKdTreeSingle
     };
 
     // That will mke logic below works with numbers chosen
@@ -556,6 +572,8 @@ TEST_F(VWDictionaryTest, NNStrategyName)
     EXPECT_EQ(VWDictionary::nnStrategyName(VWDictionary::kNNFlannLSH), "FLANN LSH");
     EXPECT_EQ(VWDictionary::nnStrategyName(VWDictionary::kNNBruteForce), "BRUTE FORCE");
     EXPECT_EQ(VWDictionary::nnStrategyName(VWDictionary::kNNBruteForceGPU), "BRUTE FORCE GPU");
+    EXPECT_EQ(VWDictionary::nnStrategyName(VWDictionary::kNNNanoFlannKdTree), "NANOFLANN KD-TREE");
+    EXPECT_EQ(VWDictionary::nnStrategyName(VWDictionary::kNNFlannKdTreeSingle), "FLANN KD-TREE SINGLE");
     EXPECT_EQ(VWDictionary::nnStrategyName(VWDictionary::kNNUndef), "Unknown");
 }
 
@@ -631,7 +649,9 @@ TEST_F(VWDictionaryTest, SerializeDeserializeIndex)
         VWDictionary::kNNFlannKdTree,
         VWDictionary::kNNFlannLSH,
         VWDictionary::kNNBruteForce,
-        VWDictionary::kNNBruteForceGPU
+        VWDictionary::kNNBruteForceGPU,
+        VWDictionary::kNNNanoFlannKdTree,
+        VWDictionary::kNNFlannKdTreeSingle
     };
 
     for(VWDictionary::NNStrategy strategy : strategies)
@@ -680,22 +700,23 @@ TEST_F(VWDictionaryTest, SerializeDeserializeIndex)
         // Serialize
         std::vector<unsigned char> data = dict->serializeIndex();
 #ifdef _WIN32
-        // FlannIndex::serializeIndex() is not implemented on Windows
+        // The rtflann serialization needs fmemopen, which Windows doesn't have
-        // (see corelib/src/FlannIndex.cpp), so it always returns empty
+        // (see FlannIndex::serializeIndex()): there, only the nanoflann index
-        // data regardless of the strategy. Skip the rest of the
+        // gives data back, the others are left out of the round trip below.
-        // round-trip assertions on Windows.
+        if(strategy != VWDictionary::kNNNanoFlannKdTree)
-        EXPECT_EQ(data.size(), 0u) << "Strategy: " << VWDictionary::nnStrategyName(strategy);
+        {
-        continue;
+            EXPECT_EQ(data.size(), 0u) << "Strategy: " << VWDictionary::nnStrategyName(strategy);
-#else
+            continue;
-        if(strategy < VWDictionary::kNNBruteForce)
+        }
+#endif
+        if(hasFlannIndex(strategy))
         {
-            // flann strategies
             EXPECT_GT(data.size(), 0u) << "Strategy: " << VWDictionary::nnStrategyName(strategy);
         }
         else {
+            // brute force strategies have no index to serialize
             EXPECT_EQ(data.size(), 0u) << "Strategy: " << VWDictionary::nnStrategyName(strategy);
         }
-#endif
         
         // Create new dictionary and deserialize
         VWDictionary dict2;
@@ -722,9 +743,8 @@ TEST_F(VWDictionaryTest, SerializeDeserializeIndex)
         dict3.setNNStrategy(strategy);
         dict3.addNewWords(descriptors, 1);
         success = dict3.deserializeIndex(data);
-        if(strategy < VWDictionary::kNNBruteForce)
+        if(hasFlannIndex(strategy))
         {
-            // flann strategies
             EXPECT_TRUE(success) << "Strategy: " << VWDictionary::nnStrategyName(strategy);
 
             // Index should be loaded
@@ -1001,3 +1021,108 @@ TEST_F(VWDictionaryTest, FindNNWithVisualWords)
     }
 }
 
+
+// What Kp/ByteToFloat costs in matching quality. Both conversions feed the same
+// exact index, so any difference comes from the distance they induce: expanding
+// each bit to a float keeps the L1 distance equal to the Hamming distance,
+// while converting each byte to a float doesn't, one flipped bit moving a byte
+// by 1 or by 128 depending on which bit it is.
+TEST_F(VWDictionaryTest, ByteToFloatMatchingQuality)
+{
+    const int words = 200;
+    const int bytes = 32; // ORB
+    const int queries = 100;
+
+    cv::RNG rng(42);
+    cv::Mat descriptors(words, bytes, CV_8U);
+    rng.fill(descriptors, cv::RNG::UNIFORM, 0, 256);
+
+    int totalBitExpansion = 0;
+    int totalByteToFloat = 0;
+    // From a query a few bits away from its word, which any distance finds, to
+    // one almost as far as the others are from each other (two random 256 bits
+    // descriptors differ by about 128).
+    for(int flippedBits: {8, 32, 64, 96})
+    {
+        // Queries are indexed descriptors with bits flipped, the way the same
+        // feature looks when seen again.
+        cv::Mat queryDescriptors(queries, bytes, CV_8U);
+        for(int i=0; i<queries; ++i)
+        {
+            descriptors.row(rng.uniform(0, words)).copyTo(queryDescriptors.row(i));
+            for(int b=0; b<flippedBits; ++b)
+            {
+                queryDescriptors.at<unsigned char>(i, rng.uniform(0, bytes)) ^= (1 << rng.uniform(0, 8));
+            }
+        }
+
+        // Ground truth: the closest descriptor in Hamming distance.
+        std::vector<int> hammingNN(queries);
+        for(int i=0; i<queries; ++i)
+        {
+            int best = -1;
+            double bestDistance = -1;
+            for(int w=0; w<words; ++w)
+            {
+                const double distance = cv::norm(queryDescriptors.row(i), descriptors.row(w), cv::NORM_HAMMING);
+                if(best < 0 || distance < bestDistance)
+                {
+                    best = w;
+                    bestDistance = distance;
+                }
+            }
+            hammingNN[i] = best;
+        }
+
+        int correct[2] = {0, 0};
+        for(int byteToFloat=0; byteToFloat<2; ++byteToFloat)
+        {
+            VWDictionary dictionary;
+            ParametersMap params;
+            // An exact index, so that only the distance the conversion induces
+            // can change what is found.
+            params.insert(ParametersPair(Parameters::kKpNNStrategy(),
+                    uNumber2Str((int)VWDictionary::kNNFlannKdTreeSingle)));
+            params.insert(ParametersPair(Parameters::kKpIncrementalFlann(), "false"));
+            params.insert(ParametersPair(Parameters::kKpByteToFloat(), byteToFloat?"true":"false"));
+            params.insert(ParametersPair(Parameters::kKpNndrRatio(), "0.8"));
+            // One word per descriptor: without this, two of the random ones
+            // that happen to be close are merged as they are added.
+            params.insert(ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
+            dictionary.parseParameters(params);
+
+            const std::list<int> addedIds = dictionary.addNewWords(descriptors, 1);
+            ASSERT_EQ(addedIds.size(), (size_t)words) << "byteToFloat=" << byteToFloat;
+            dictionary.update();
+            ASSERT_EQ(dictionary.getVisualWords().size(), (size_t)words) << "byteToFloat=" << byteToFloat;
+
+            const std::vector<int> ids(addedIds.begin(), addedIds.end());
+            const std::vector<int> matched = dictionary.findNN(queryDescriptors);
+            ASSERT_EQ(matched.size(), (size_t)queries) << "byteToFloat=" << byteToFloat;
+            for(int i=0; i<queries; ++i)
+            {
+                if(matched[i] == ids[hammingNN[i]])
+                {
+                    ++correct[byteToFloat];
+                }
+            }
+        }
+
+        std::cerr << "[          ] " << flippedBits << "/" << bytes*8
+                  << " bits flipped: " << Parameters::kKpByteToFloat() << "=false found "
+                  << correct[0] << "/" << queries << " of the Hamming nearest neighbors, =true found "
+                  << correct[1] << "/" << queries << "\n";
+
+        // Expanding the bits keeps the Hamming ordering, so it finds what an
+        // exhaustive Hamming search finds; converting the bytes cannot do
+        // better than that.
+        EXPECT_EQ(correct[0], queries) << "flippedBits=" << flippedBits;
+        EXPECT_LE(correct[1], correct[0]) << "flippedBits=" << flippedBits;
+        totalBitExpansion += correct[0];
+        totalByteToFloat += correct[1];
+    }
+
+    // The distortion is what the parameter trades for its smaller descriptors:
+    // it shows once the true match is not much closer than the others.
+    EXPECT_LT(totalByteToFloat, totalBitExpansion);
+}

guilib/src/ui/preferencesDialog.ui

@@ -11902,7 +11902,7 @@ generate the number of words requested.</string>
                        <item row="9" column="2">
                         <widget class="QLabel" name="label_451">
                          <property name="text">
-                          <string>Factor used when rebuilding the incremental FLANN index. Set 1 to disable.</string>
+                          <string>Rebuild the incremental FLANN index once the ratio (factor-1)/factor of its features has been removed, e.g. half of them for a factor of 2. Rebuilding frees the memory of the removed features and speeds up the searches. Features are mostly removed when memory management is enabled. Set to 1 to never rebuild, which also uses less memory as the features don't have to be referenced one by one.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -11930,7 +11930,7 @@ Lower the ratio -&gt; higher the precision.</string>
                        <item row="8" column="2">
                         <widget class="QLabel" name="label_260">
                          <property name="text">
-                          <string>When using a FLANN-based nearest neighbor strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary increases of the factor below in size).</string>
+                          <string>When using a FLANN-based nearest neighbor strategy, add/remove points to its index without always rebuilding the index (the index is only rebuilt when too many of its features have been removed, see the factor below).</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -11980,6 +11980,16 @@ Lower the ratio -&gt; higher the precision.</string>
                            <string>Brute Force GPU</string>
                           </property>
                          </item>
+                         <item>
+                          <property name="text">
+                           <string>NanoFLANN KdTree</string>
+                          </property>
+                         </item>
+                         <item>
+                          <property name="text">
+                           <string>FLANN KdTree Single</string>
+                          </property>
+                         </item>
                         </widget>
                        </item>
                        <item row="2" column="0">
@@ -23538,6 +23548,16 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                              <string>GMS</string>
                             </property>
                            </item>
+                           <item>
+                            <property name="text">
+                             <string>NanoFLANN KdTree</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>FLANN KdTree Single</string>
+                            </property>
+                           </item>
                           </widget>
                          </item>
                          <item row="0" column="1">

tools/Matcher/main.cpp

@@ -414,6 +414,24 @@ int main(int argc, char * argv[])
 		std::string pyMatcherPath;
 		Parameters::parse(parameters, Parameters::kVisPnPReprojError(), reprojError);
 		Parameters::parse(parameters, Parameters::kPyMatcherPath(), pyMatcherPath);
+
+		// PyMatcher cannot match binary features, RegistrationVis falls back to
+		// brute force with cross check (see the warning above).
+		const bool pyMatcherOnBinaryFeatures =
+				reg.getNNType()==6 &&
+				!dataFrom.getWordsDescriptors().empty() &&
+				dataFrom.getWordsDescriptors().type()!=CV_32F;
+		QString nnTypeName = (pyMatcherOnBinaryFeatures?
+				RegistrationVis::getNNTypeName(5):reg.getNNTypeName()).c_str();
+		// 5, 6 and 7 are the approaches RegistrationVis matches with itself,
+		// the others search for the k nearest neighbors and do the ratio test.
+		const bool nndrUsed = reg.getNNType()<5 || reg.getNNType()>7;
+		if(reg.getNNType()==6 && !pyMatcherOnBinaryFeatures)
+		{
+			// the script actually used is more telling than the generic name
+			nnTypeName = QString(uSplit(UFile::getName(pyMatcherPath), '.').front().c_str()).replace("rtabmap_", "");
+		}
+
 		dialog.setWindowTitle(QString("Matches (%1/%2) %3 sec [%4=%5 (%6) %7=%8 (%9)%10 %11=%12 (%13) %14=%15]")
 				.arg(info.inliers)
 				.arg(info.matches)
@@ -423,11 +441,8 @@ int main(int argc, char * argv[])
 				.arg(reg.getDetector()?Feature2D::typeName(reg.getDetector()->getType()).c_str():"?")
 				.arg(Parameters::kVisCorNNType().c_str())
 				.arg(reg.getNNType())
-				.arg(reg.getNNType()<VWDictionary::kNNUndef?VWDictionary::nnStrategyName((VWDictionary::NNStrategy)reg.getNNType()).c_str():
+				.arg(nnTypeName)
-						reg.getNNType()==5||(reg.getNNType()==6&&!dataFrom.getWordsDescriptors().empty()&& dataFrom.getWordsDescriptors().type()!=CV_32F)?"BFCrossCheck":
+				.arg(nndrUsed?QString(" %1=%2").arg(Parameters::kVisCorNNDR().c_str()).arg(reg.getNNDR()):"")
-						reg.getNNType()==6?QString(uSplit(UFile::getName(pyMatcherPath), '.').front().c_str()).replace("rtabmap_", ""):
-						reg.getNNType()==7?"GMS":"?")
-				.arg(reg.getNNType()<5?QString(" %1=%2").arg(Parameters::kVisCorNNDR().c_str()).arg(reg.getNNDR()):"")
 				.arg(Parameters::kVisEstimationType().c_str())
 				.arg(reg.getEstimationType())
 				.arg(reg.getEstimationType()==0?"3D->3D":reg.getEstimationType()==1?"3D->2D":reg.getEstimationType()==2?"2D->2D":"?")