Merge branch 'master' of github.com:introlab/rtabmap into jade-devel

CMakeLists.txt

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 10)
-SET(RTABMAP_PATCH_VERSION 4)
+SET(RTABMAP_PATCH_VERSION 10)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -121,26 +121,97 @@ IF(APPLE)
 ENDIF(APPLE)
 
 ####### DEPENDENCIES #######
-FIND_PACKAGE(OpenCV REQUIRED)
-FIND_PACKAGE(PCL 1.7 REQUIRED)
-FIND_PACKAGE(VTK REQUIRED)
-IF("${VTK_MAJOR_VERSION}" EQUAL 5)
-	FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5
-ENDIF("${VTK_MAJOR_VERSION}" EQUAL 5)
-FIND_PACKAGE(ZLIB REQUIRED)
-FIND_PACKAGE(Freenect)
-FIND_PACKAGE(freenect2 QUIET)
-FIND_PACKAGE(OpenNI2)
-FIND_PACKAGE(DC1394)
-FIND_PACKAGE(G2O)
-FIND_PACKAGE(FlyCapture2)
 
+option(WITH_QT            "Include Qt support"                   ON)
+option(WITH_FREENECT      "Include Freenect support"             ON)
+option(WITH_FREENECT2     "Include Freenect2 support"            ON)
+option(WITH_OPENNI2       "Include OpenNI2 support"              ON)
+option(WITH_DC1394        "Include dc1394 support"               ON)
+option(WITH_G2O           "Include g2o support"                  ON)
+option(WITH_GTSAM         "Include GTSAM support"                ON)
+option(WITH_CVSBA         "Include cvsba support"                ON)
+option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
+
+FIND_PACKAGE(OpenCV REQUIRED QUIET)
+FIND_PACKAGE(PCL 1.7 REQUIRED QUIET)
+FIND_PACKAGE(ZLIB REQUIRED QUIET)
+IF(OpenCV_FOUND)
+   MESSAGE(STATUS "Found OpenCV: ${OpenCV_INCLUDE_DIRS}")
+ENDIF(OpenCV_FOUND)
+IF(PCL_FOUND)
+   MESSAGE(STATUS "Found PCL: ${PCL_INCLUDE_DIRS}")
+ENDIF(PCL_FOUND)
+IF(ZLIB_FOUND)
+   MESSAGE(STATUS "Found ZLIB: ${ZLIB_INCLUDE_DIRS}")
+ENDIF(ZLIB_FOUND)
+
+IF(WITH_QT)
 # If Qt is here, the GUI will be built
-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
-    FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
+        FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
-ELSE()
+    ELSE()
-    FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
+        FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
-ENDIF()
+    ENDIF()
+    IF(QT4_FOUND OR Qt5_FOUND)
+        FIND_PACKAGE(VTK REQUIRED)
+        IF("${VTK_MAJOR_VERSION}" EQUAL 5)
+            FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5
+        ENDIF("${VTK_MAJOR_VERSION}" EQUAL 5)
+    ENDIF(QT4_FOUND OR Qt5_FOUND)
+ENDIF(WITH_QT)
+
+IF(WITH_FREENECT)
+    FIND_PACKAGE(Freenect QUIET)
+    IF(Freenect_FOUND)
+       MESSAGE(STATUS "Found Freenect: ${Freenect_INCLUDE_DIRS}")
+    ENDIF(Freenect_FOUND)
+ENDIF(WITH_FREENECT)
+
+IF(WITH_FREENECT2)
+    FIND_PACKAGE(freenect2 QUIET)
+    IF(freenect2_FOUND)
+       MESSAGE(STATUS "Found freenect2: ${freenect2_INCLUDE_DIRS}")
+    ENDIF(freenect2_FOUND)
+ENDIF(WITH_FREENECT2)
+
+IF(WITH_OPENNI2)
+    FIND_PACKAGE(OpenNI2 QUIET)
+    IF(OpenNI2_FOUND)
+       MESSAGE(STATUS "Found OpenNI2: ${OpenNI2_INCLUDE_DIRS}")
+    ENDIF(OpenNI2_FOUND)
+ENDIF(WITH_OPENNI2)
+
+IF(WITH_DC1394)
+    FIND_PACKAGE(DC1394 QUIET)
+    IF(DC1394_FOUND)
+       MESSAGE(STATUS "Found DC1394: ${DC1394_INCLUDE_DIRS}")
+    ENDIF(DC1394_FOUND)
+ENDIF(WITH_DC1394)
+
+IF(WITH_G2O)
+    FIND_PACKAGE(G2O QUIET)
+    IF(G2O_FOUND)
+       MESSAGE(STATUS "Found g2o: ${G2O_INCLUDE_DIRS}")
+    ENDIF(G2O_FOUND)
+ENDIF(WITH_G2O)
+
+IF(WITH_GTSAM)
+    FIND_PACKAGE(GTSAM QUIET)
+ENDIF(WITH_GTSAM)
+
+IF(WITH_FLYCAPTURE2)
+    FIND_PACKAGE(FlyCapture2 QUIET)
+    IF(FlyCapture2_FOUND)
+       MESSAGE(STATUS "Found FlyCapture2: ${FlyCapture2_INCLUDE_DIRS}")
+    ENDIF(FlyCapture2_FOUND)
+ENDIF(WITH_FLYCAPTURE2)
+
+IF(WITH_CVSBA)
+    FIND_PACKAGE(cvsba)
+    IF(cvsba_FOUND)
+       MESSAGE(STATUS "Found cvsba: ${cvsba_INCLUDE_DIRS}")
+    ENDIF(cvsba_FOUND)
+ENDIF(WITH_CVSBA)
 
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
@@ -183,8 +254,8 @@ ADD_SUBDIRECTORY( corelib )
 IF(Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
    ADD_SUBDIRECTORY( guilib )
    ADD_SUBDIRECTORY( app )
-ELSE()
+ELSEIF(WITH_QT)
-   MESSAGE(STATUS "[WARNING] Qt not found, the GUI lib and the stand-alone application will not be compiled...")
+   MESSAGE(WARNING "Qt not found, the GUI lib and the stand-alone application will not be compiled...")
 ENDIF()
 
 ADD_SUBDIRECTORY( tools )
@@ -213,6 +284,7 @@ set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/corelib/include"
 					  "${PROJECT_SOURCE_DIR}/guilib/include" 
 					  "${PROJECT_SOURCE_DIR}/utilite/include")
 set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")
+set(CONF_WITH_GUI ${WITH_QT})
 configure_file(RTABMapConfig.cmake.in
   "${PROJECT_BINARY_DIR}/RTABMapConfig.cmake" @ONLY)
   
@@ -346,44 +418,74 @@ ENDIF(OpenCV_FOUND)
 
 IF(Freenect_FOUND)
 MESSAGE(STATUS "  With Freenect             = YES")
+ELSEIF(NOT WITH_FREENECT)
+MESSAGE(STATUS "  With Freenect             = NO (WITH_FREENECT=OFF)")
 ELSE()
 MESSAGE(STATUS "  With Freenect             = NO (libfreenect not found)")
 ENDIF()
 
 IF(OpenNI2_FOUND)
 MESSAGE(STATUS "  With OpenNI2              = YES")
+ELSEIF(NOT WITH_OPENNI2)
+MESSAGE(STATUS "  With OpenNI2              = NO (WITH_OPENNI2=OFF)")
 ELSE()
 MESSAGE(STATUS "  With OpenNI2              = NO (OpenNI2 not found)")
 ENDIF()
 
 IF(freenect2_FOUND)
 MESSAGE(STATUS "  With Freenect2            = YES")
+ELSEIF(NOT WITH_FREENECT2)
+MESSAGE(STATUS "  With Freenect2            = NO (WITH_FREENECT2=OFF)")
 ELSE()
 MESSAGE(STATUS "  With Freenect2            = NO (libfreenect2 not found)")
 ENDIF()
 
 IF(DC1394_FOUND)
 MESSAGE(STATUS "  With dc1394               = YES")
+ELSEIF(NOT WITH_DC1394)
+MESSAGE(STATUS "  With dc1394               = NO (WITH_DC1394=OFF)")
 ELSE()
 MESSAGE(STATUS "  With dc1394               = NO (dc1394 not found)")
 ENDIF()
 
 IF(FlyCapture2_FOUND)
 MESSAGE(STATUS "  With FlyCapture2/Triclops = YES")
+ELSEIF(NOT WITH_FLYCAPTURE2)
+MESSAGE(STATUS "  With FlyCapture2/Triclops = NO (WITH_FLYCAPTURE2=OFF)")
 ELSE()
 MESSAGE(STATUS "  With FlyCapture2/Triclops = NO (Point Grey SDK not found)")
 ENDIF()
 
 IF(G2O_FOUND)
 MESSAGE(STATUS "  With g2o                  = YES")
+ELSEIF(NOT WITH_G2O)
+MESSAGE(STATUS "  With g2o                  = NO (WITH_G2O=OFF)")
 ELSE()
 MESSAGE(STATUS "  With g2o                  = NO (g2o not found)")
 ENDIF()
 
+IF(GTSAM_FOUND)
+MESSAGE(STATUS "  With GTSAM                = YES")
+ELSEIF(NOT WITH_GTSAM)
+MESSAGE(STATUS "  With GTSAM                = NO (WITH_GTSAM=OFF)")
+ELSE()
+MESSAGE(STATUS "  With GTSAM                = NO (GTSAM not found)")
+ENDIF()
+
+IF(cvsba_FOUND)
+MESSAGE(STATUS "  With cvsba                = YES")
+ELSEIF(NOT WITH_CVSBA)
+MESSAGE(STATUS "  With cvsba                = NO (WITH_CVSBA=OFF)")
+ELSE()
+MESSAGE(STATUS "  With cvsba                = NO (cvsba not found)")
+ENDIF()
+
 IF(QT4_FOUND)
 MESSAGE(STATUS "  With Qt                   = YES (version 4)")
 ELSEIF(Qt5_FOUND)
 MESSAGE(STATUS "  With Qt                   = YES (version 5)")
+ELSEIF(NOT WITH_QT)
+MESSAGE(STATUS "  With Qt                   = NO (WITH_QT=OFF)")
 ELSE()
 MESSAGE(STATUS "  With Qt                   = NO (Qt not found, to use Qt5 you should set -DRTABMAP_QT_VERSION=5)")
 ENDIF()

RTABMapConfig.cmake.in

@@ -2,12 +2,72 @@
 # It defines the following variables
 #  RTABMap_INCLUDE_DIRS - include directories for RTABMap
 #  RTABMap_LIBRARIES    - libraries to link against
+#  RTABMap_CORE         - core library
+#  RTABMap_UTILITE      - utilite library
+#  RTABMap_GUI          - gui library (set if RTABMap is built with Qt)
 
 # Compute paths
 get_filename_component(RTABMap_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
 set(RTABMap_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@")
 
-find_library(RTABMAP_CORE NAMES rtabmap_core rtabmap_cored NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+#core lib
-find_library(RTABMAP_GUI NAMES rtabmap_gui rtabmap_guid NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
-find_library(RTABMAP_UTILITE NAMES rtabmap_utilite rtabmap_utilited NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
-set(RTABMap_LIBRARIES ${RTABMAP_CORE} ${RTABMAP_GUI} ${RTABMAP_UTILITE})
+
+IF(RTABMap_CORE_DEBUG AND RTABMap_CORE_RELEASE)
+   SET(RTABMap_CORE
+      debug ${RTABMap_CORE_DEBUG}
+      optimized ${RTABMap_CORE_RELEASE}
+   )
+ELSEIF(RTABMap_CORE_RELEASE)
+   SET(RTABMap_CORE ${RTABMap_CORE_RELEASE})
+ELSEIF(RTABMap_CORE_DEBUG)
+   SET(RTABMap_CORE ${RTABMap_CORE_DEBUG})
+ENDIF()
+
+#utilite lib
+find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+
+IF(RTABMap_UTILITE_DEBUG AND RTABMap_UTILITE_RELEASE)
+   SET(RTABMap_UTILITE
+      debug ${RTABMap_UTILITE_DEBUG}
+      optimized ${RTABMap_UTILITE_RELEASE}
+   )
+ELSEIF(RTABMap_UTILITE_RELEASE)
+   SET(RTABMap_UTILITE ${RTABMap_UTILITE_RELEASE})
+ELSEIF(RTABMap_UTILITE_DEBUG)
+   SET(RTABMap_UTILITE ${RTABMap_UTILITE_DEBUG})
+ENDIF()
+
+set(RTABMap_LIBRARIES ${RTABMap_CORE} ${RTABMap_UTILITE})
+
+#gui lib (OFF if RTAB-Map is not built with Qt)
+if(@CONF_WITH_GUI@)
+   find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+   find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+   
+   IF(RTABMap_GUI_DEBUG AND RTABMap_GUI_RELEASE)
+      SET(RTABMap_GUI
+         debug ${RTABMap_GUI_DEBUG}
+         optimized ${RTABMap_GUI_RELEASE}
+      )
+   ELSEIF(RTABMap_GUI_RELEASE)
+      SET(RTABMap_GUI ${RTABMap_GUI_RELEASE})
+   ELSEIF(RTABMap_GUI_DEBUG)
+      SET(RTABMap_GUI ${RTABMap_GUI_DEBUG})
+   ENDIF()
+   
+   set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} ${RTABMap_GUI})
+endif(@CONF_WITH_GUI@)
+
+#backward compatibilities
+if(RTABMap_CORE)
+   set(RTABMAP_CORE ${RTABMap_CORE})
+endif(RTABMap_CORE)
+if(RTABMap_UTILITE)
+   set(RTABMAP_UTILITE ${RTABMap_UTILITE})
+endif(RTABMap_UTILITE)
+if(RTABMap_GUI)
+   set(RTABMAP_GUI ${RTABMap_GUI})
+endif(RTABMap_GUI)

cmake_modules/FindFLANN.cmake

@@ -0,0 +1,68 @@
+###############################################################################
+# Find FLANN
+#
+# This sets the following variables:
+# FLANN_FOUND - True if FLANN was found.
+# FLANN_INCLUDE_DIRS - Directories containing the FLANN include files.
+# FLANN_LIBRARIES - Libraries needed to use FLANN.
+# FLANN_DEFINITIONS - Compiler flags for FLANN.
+# If FLANN_USE_STATIC is specified and then look for static libraries ONLY else
+# look for shared ones
+#
+# Original from https://github.com/PointCloudLibrary/pcl/blob/master/cmake/Modules/FindFLANN.cmake
+#
+
+if(FLANN_USE_STATIC)
+  set(FLANN_RELEASE_NAME flann_cpp_s)
+  set(FLANN_DEBUG_NAME flann_cpp_s-gd)
+else(FLANN_USE_STATIC)
+  set(FLANN_RELEASE_NAME flann_cpp)
+  set(FLANN_DEBUG_NAME flann_cpp-gd)
+endif(FLANN_USE_STATIC)
+
+find_package(PkgConfig QUIET)
+if (FLANN_FIND_VERSION)
+    pkg_check_modules(PC_FLANN flann>=${FLANN_FIND_VERSION})
+else(FLANN_FIND_VERSION)
+    pkg_check_modules(PC_FLANN flann)
+endif(FLANN_FIND_VERSION)
+
+set(FLANN_DEFINITIONS ${PC_FLANN_CFLAGS_OTHER})
+
+find_path(FLANN_INCLUDE_DIR flann/flann.hpp
+          HINTS ${PC_FLANN_INCLUDEDIR} ${PC_FLANN_INCLUDE_DIRS} "${FLANN_ROOT}" "$ENV{FLANN_ROOT}"
+          PATHS "$ENV{PROGRAMFILES}/Flann" "$ENV{PROGRAMW6432}/Flann" 
+          PATH_SUFFIXES include)
+
+find_library(FLANN_LIBRARY
+             NAMES ${FLANN_RELEASE_NAME}
+             HINTS ${PC_FLANN_LIBDIR} ${PC_FLANN_LIBRARY_DIRS} "${FLANN_ROOT}" "$ENV{FLANN_ROOT}"
+             PATHS "$ENV{PROGRAMFILES}/Flann" "$ENV{PROGRAMW6432}/Flann" 
+	     PATH_SUFFIXES lib)
+
+find_library(FLANN_LIBRARY_DEBUG 
+             NAMES ${FLANN_DEBUG_NAME} ${FLANN_RELEASE_NAME}
+	     HINTS ${PC_FLANN_LIBDIR} ${PC_FLANN_LIBRARY_DIRS} "${FLANN_ROOT}" "$ENV{FLANN_ROOT}"
+	     PATHS "$ENV{PROGRAMFILES}/Flann" "$ENV{PROGRAMW6432}/Flann" 
+	     PATH_SUFFIXES lib)
+
+if(NOT FLANN_LIBRARY_DEBUG)
+  set(FLANN_LIBRARY_DEBUG ${FLANN_LIBRARY})
+endif(NOT FLANN_LIBRARY_DEBUG)
+
+set(FLANN_INCLUDE_DIRS ${FLANN_INCLUDE_DIR})
+set(FLANN_LIBRARIES optimized ${FLANN_LIBRARY} debug ${FLANN_LIBRARY_DEBUG})
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(FLANN DEFAULT_MSG FLANN_LIBRARY FLANN_INCLUDE_DIR)
+
+mark_as_advanced(FLANN_LIBRARY FLANN_LIBRARY_DEBUG FLANN_INCLUDE_DIR)
+
+if(FLANN_FOUND)
+  IF (NOT FLANN_FIND_QUIETLY)
+    message(STATUS "FLANN found (include: ${FLANN_INCLUDE_DIRS}, lib: ${FLANN_LIBRARIES})")
+  ENDIF (NOT FLANN_FIND_QUIETLY)
+  if(FLANN_USE_STATIC)
+    add_definitions(-DFLANN_STATIC)
+  endif(FLANN_USE_STATIC)
+endif(FLANN_FOUND)

cmake_modules/FindFlyCapture2.cmake

@@ -34,9 +34,6 @@ IF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND
    SET(FlyCapture2_LIBRARIES ${FlyCapture2_LIBRARY} ${Triclops_LIBRARY} ${FlyCaptureBridge_LIBRARY} ${pnmutils_LIBRARY})
 ENDIF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY AND pnmutils_LIBRARY)
 
-MESSAGE(STATUS "FlyCapture2_INCLUDE_DIRS={FlyCapture2_INCLUDE_DIRS}")
-MESSAGE(STATUS "FlyCapture2_LIBRARIES={FlyCapture2_LIBRARIES}")
-
 IF (FlyCapture2_FOUND)
    # show which FlyCapture2 was found only if not quiet
    IF (NOT FlyCapture2_FIND_QUIETLY)

cmake_modules/FindG2O.cmake

@@ -70,11 +70,13 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FO
   SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR})
   SET(G2O_LIBRARIES 
 	${G2O_STUFF_LIBRARY} 
-	${G2O_CORE_LIBRARY} 
+	${G2O_CORE_LIBRARY}
+	${G2O_SOLVER_CHOLMOD}
 	${G2O_SOLVER_CSPARSE} 
 	${G2O_SOLVER_CSPARSE_EXTENSION}
 	${G2O_TYPES_SLAM2D} 
 	${G2O_TYPES_SLAM3D} 
-	${CSPARSE_LIBRARY})
+	${CSPARSE_LIBRARY}
+        cholmod)
   SET(G2O_FOUND "YES")
 ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND AND CSPARSE_FOUND)

corelib/include/rtabmap/core/CameraModel.h

@@ -96,15 +96,19 @@ public:
 	void setLocalTransform(const Transform & transform) {localTransform_ = transform;}
 	const Transform & localTransform() const {return localTransform_;}
 
+	void setImageSize(const cv::Size & size) {imageSize_ = size;}
 	const cv::Size & imageSize() const {return imageSize_;}
 	int imageWidth() const {return imageSize_.width;}
-	int imageWeight() const {return imageSize_.height;}
+	int imageHeight() const {return imageSize_.height;}
 
 	bool load(const std::string & directory, const std::string & cameraName);
 	bool save(const std::string & directory) const;
 
 	void scale(double scale);
 
+	double horizontalFOV() const; // in degrees
+	double verticalFOV() const;   // in degrees
+
 	// For depth images, your should use cv::INTER_NEAREST
 	cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
 	cv::Mat rectifyDepth(const cv::Mat & raw) const;

corelib/include/rtabmap/core/CameraRGBD.h

@@ -160,6 +160,7 @@ public:
 	bool setExposure(int value);
 	bool setGain(int value);
 	bool setMirroring(bool enabled);
+	void setOpenNI2StampsAndIDsUsed(bool used) {_openNI2StampsAndIDsUsed = used;}
 
 protected:
 	virtual SensorData captureImage();
@@ -171,6 +172,7 @@ private:
 	float _depthFx;
 	float _depthFy;
 	std::string _deviceId;
+	bool _openNI2StampsAndIDsUsed;
 };
 
 

corelib/include/rtabmap/core/CameraThread.h

@@ -52,6 +52,7 @@ public:
 
 	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
 	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
+	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
 
 	//getters
 	bool isPaused() const {return !this->isRunning();}
@@ -68,6 +69,7 @@ private:
 	Camera * _camera;
 	bool _mirroring;
 	bool _colorOnly;
+	bool _stereoToDepth;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/DBDriver.h

@@ -60,6 +60,9 @@ class VisualWord;
 //
 class RTABMAP_EXP DBDriver : public UThreadNode
 {
+public:
+	static DBDriver * create(const ParametersMap & parameters = ParametersMap());
+
 public:
 	virtual ~DBDriver();
 
@@ -75,6 +78,15 @@ public:
 	double getEmptyTrashesTime() const {return _emptyTrashesTime;}
 	void setTimestampUpdateEnabled(bool enabled) {_timestampUpdate = enabled;} // used on Update Signature and Word queries
 
+	// Warning: the following functions don't look in the trash, direct database modifications
+	void generateGraph(
+			const std::string & fileName,
+			const std::set<int> & ids = std::set<int>(),
+			const std::map<int, Signature *> & otherSignatures = std::map<int, Signature *>());
+	void addLink(const Link & link);
+	void removeLink(int from, int to);
+	void updateLink(const Link & link);
+
 public:
 	void addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const;
 
@@ -128,6 +140,8 @@ private:
 	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const = 0;
 	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const = 0;
 
+	virtual void addLinkQuery(const Link & link) const = 0;
+	virtual void updateLinkQuery(const Link & link) const = 0;
 
 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const = 0;

corelib/include/rtabmap/core/DBReader.h

@@ -50,11 +50,13 @@ public:
 	DBReader(const std::string & databasePath,
 			 float frameRate = 0.0f,
 			 bool odometryIgnored = false,
-			 bool ignoreGoalDelay = false);
+			 bool ignoreGoalDelay = false,
+			 bool goalsIgnored = false);
 	DBReader(const std::list<std::string> & databasePaths,
 			 float frameRate = 0.0f,
 			 bool odometryIgnored = false,
-			 bool ignoreGoalDelay = false);
+			 bool ignoreGoalDelay = false,
+			 bool goalsIgnored = false);
 	virtual ~DBReader();
 
 	bool init(int startIndex=0);
@@ -70,6 +72,7 @@ private:
 	float _frameRate; // -1 = use Database stamps, 0 = inf
 	bool _odometryIgnored;
 	bool _ignoreGoalDelay;
+	bool _goalsIgnored;
 
 	DBDriver * _dbDriver;
 	UTimer _timer;

corelib/include/rtabmap/core/Graph.h

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <list>
 #include <rtabmap/core/Link.h>
 #include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Signature.h>
 
 namespace rtabmap {
 class Memory;
@@ -49,7 +50,9 @@ public:
 	enum Type {
 		kTypeUndef = -1,
 		kTypeTORO = 0,
-		kTypeG2O = 1
+		kTypeG2O = 1,
+		kTypeGTSAM = 2,
+		kTypeCVSBA = 3
 	};
 	static Optimizer * create(const ParametersMap & parameters);
 	static Optimizer * create(Optimizer::Type & type, const ParametersMap & parameters = ParametersMap());
@@ -58,7 +61,7 @@ public:
 	static void getConnectedGraph(
 			int fromId,
 			const std::map<int, Transform> & posesIn,
-			const std::multimap<int, Link> & linksIn,
+			const std::multimap<int, Link> & linksIn, // only one link between two poses
 			std::map<int, Transform> & posesOut,
 			std::multimap<int, Link> & linksOut,
 			int depth = 0);
@@ -72,12 +75,19 @@ public:
 	bool isSlam2d() const {return slam2d_;}
 	bool isCovarianceIgnored() const {return covarianceIgnored_;}
 	double epsilon() const {return epsilon_;}
+	bool isRobust() const {return robust_;}
 
+	// inherited classes should implement one of these methods
 	virtual std::map<int, Transform> optimize(
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & constraints,
-			std::list<std::map<int, Transform> > * intermediateGraphes = 0) = 0;
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0);
+	virtual std::map<int, Transform> optimizeBA(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, Signature> & signatures);
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
@@ -86,7 +96,8 @@ protected:
 			int iterations         = Parameters::defaultRGBDOptimizeIterations(),
 			bool slam2d            = Parameters::defaultRGBDOptimizeSlam2D(),
 			bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
-			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon());
+			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon(),
+			bool robust            = Parameters::defaultRGBDOptimizeRobust());
 	Optimizer(const ParametersMap & parameters);
 
 private:
@@ -94,6 +105,7 @@ private:
 	bool slam2d_;
 	bool covarianceIgnored_;
 	double epsilon_;
+	bool robust_;
 };
 
 class RTABMAP_EXP TOROOptimizer : public Optimizer
@@ -109,8 +121,12 @@ public:
 			std::multimap<int, Link> & edgeConstraints);
 
 public:
-	TOROOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) :
+	TOROOptimizer(
-		Optimizer(iterations, slam2d, covarianceIgnored) {}
+			int iterations         = Parameters::defaultRGBDOptimizeIterations(),
+			bool slam2d            = Parameters::defaultRGBDOptimizeSlam2D(),
+			bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
+			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon()) :
+		Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {}
 	TOROOptimizer(const ParametersMap & parameters) :
 		Optimizer(parameters) {}
 	virtual ~TOROOptimizer() {}
@@ -128,10 +144,21 @@ class RTABMAP_EXP G2OOptimizer : public Optimizer
 {
 public:
 	static bool available();
+	static bool saveGraph(
+			const std::string & fileName,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & edgeConstraints,
+			bool useRobustConstraints = false);
 
 public:
-	G2OOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) :
+	G2OOptimizer(
-		Optimizer(iterations, slam2d, covarianceIgnored) {}
+			int iterations         = Parameters::defaultRGBDOptimizeIterations(),
+			bool slam2d            = Parameters::defaultRGBDOptimizeSlam2D(),
+			bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
+			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon(),
+			bool robust            = Parameters::defaultRGBDOptimizeRobust()) :
+		Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
+
 	G2OOptimizer(const ParametersMap & parameters) :
 		Optimizer(parameters) {}
 	virtual ~G2OOptimizer() {}
@@ -145,6 +172,72 @@ public:
 			std::list<std::map<int, Transform> > * intermediateGraphes = 0);
 };
 
+class RTABMAP_EXP GTSAMOptimizer : public Optimizer
+{
+public:
+	static bool available();
+
+public:
+	GTSAMOptimizer(
+			int iterations         = Parameters::defaultRGBDOptimizeIterations(),
+			bool slam2d            = Parameters::defaultRGBDOptimizeSlam2D(),
+			bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
+			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon(),
+			bool robust            = Parameters::defaultRGBDOptimizeRobust()) :
+		Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
+
+	GTSAMOptimizer(const ParametersMap & parameters) :
+		Optimizer(parameters) {}
+	virtual ~GTSAMOptimizer() {}
+
+	virtual Type type() const {return kTypeGTSAM;}
+
+	virtual std::map<int, Transform> optimize(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & edgeConstraints,
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0);
+};
+
+class RTABMAP_EXP CVSBAOptimizer : public Optimizer
+{
+public:
+	static bool available();
+
+public:
+	CVSBAOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) :
+		Optimizer(iterations, slam2d, covarianceIgnored),
+		inlierDistance_(0.02),
+		minInliers_(10){}
+	CVSBAOptimizer(const ParametersMap & parameters) :
+		Optimizer(parameters),
+		inlierDistance_(0.02),
+		minInliers_(10){}
+	virtual ~CVSBAOptimizer() {}
+
+	virtual Type type() const {return kTypeCVSBA;}
+
+	void setInlierDistance(float inlierDistance) {inlierDistance_ = inlierDistance;}
+	void setMinInliers(int minInliers) {minInliers_ = minInliers;}
+
+	virtual std::map<int, Transform> optimizeBA(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, Signature> & signatures);
+
+private:
+	float inlierDistance_;
+	float minInliers_;
+};
+
+bool RTABMAP_EXP exportPoses(
+		const std::string & filePath,
+		int format, // 0=Raw (*.txt), 1=RGBD-SLAM (*.txt), 2=KITTI (*.txt), 3=TORO (*.graph), 4=g2o (*.g2o)
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & constraints, // required for formats 3 and 4
+		const std::map<int, double> & stamps); // required for format 1
+
 ////////////////////////////////////////////
 // Graph utilities
 ////////////////////////////////////////////
@@ -221,7 +314,9 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
 		int toId,
 		const Memory * memory,
 		bool lookInDatabase = true,
-		bool updateNewCosts = false);
+		bool updateNewCosts = false,
+		float linearVelocity = 0.0f,   // m/sec
+		float angularVelocity = 0.0f); // rad/sec
 
 int RTABMAP_EXP findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
@@ -249,6 +344,10 @@ float RTABMAP_EXP computePathLength(
 		unsigned int fromIndex = 0,
 		unsigned int toIndex = 0);
 
+std::list<std::map<int, Transform> > RTABMAP_EXP getPaths(
+		std::map<int, Transform> poses,
+		const std::multimap<int, Link> & links);
+
 
 } /* namespace graph */
 

corelib/include/rtabmap/core/Link.h

@@ -29,8 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define LINK_H_
 
 #include <rtabmap/core/Transform.h>
-#include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/utilite/UMath.h>
 #include <opencv2/core/core.hpp>
 
 namespace rtabmap {
@@ -39,38 +37,20 @@ class Link
 {
 public:
 	enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure, kUserClosure, kVirtualClosure, kUndef};
-	Link() :
+	Link();
-		from_(0),
-		to_(0),
-		type_(kUndef),
-		infMatrix_(cv::Mat::eye(6,6,CV_64FC1))
-	{
-	}
 	Link(int from,
 			int to,
 			Type type,
 			const Transform & transform,
-			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1)) :
+			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1),
-		from_(from),
+			const cv::Mat & userData = cv::Mat());
-		to_(to),
-		transform_(transform),
-		type_(type)
-	{
-		setInfMatrix(infMatrix);
-	}
 	Link(int from,
 			int to,
 			Type type,
 			const Transform & transform,
 			double rotVariance,
-			double transVariance) :
+			double transVariance,
-		from_(from),
+			const cv::Mat & userData = cv::Mat());
-		to_(to),
-		transform_(transform),
-		type_(type)
-	{
-		setVariance(rotVariance, transVariance);
-	}
 
 	bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;}
 
@@ -79,65 +59,25 @@ public:
 	const Transform & transform() const {return transform_;}
 	Type type() const {return type_;}
 	const cv::Mat & infMatrix() const {return infMatrix_;}
-	double rotVariance() const
+	double rotVariance() const;
-	{
+	double transVariance() const;
-		double min = uMin3(infMatrix_.at<double>(3,3), infMatrix_.at<double>(4,4), infMatrix_.at<double>(5,5));
-		UASSERT(min > 0.0);
-		return 1.0/min;
-	}
-	double transVariance() const
-	{
-		double min = uMin3(infMatrix_.at<double>(0,0), infMatrix_.at<double>(1,1), infMatrix_.at<double>(2,2));
-		UASSERT(min > 0.0);
-		return 1.0/min;
-	}
 
 	void setFrom(int from) {from_ = from;}
 	void setTo(int to) {to_ = to;}
 	void setTransform(const Transform & transform) {transform_ = transform;}
 	void setType(Type type) {type_ = type;}
-	void setInfMatrix(const cv::Mat & infMatrix) {
+	void setInfMatrix(const cv::Mat & infMatrix);
-		UASSERT(infMatrix.cols == 6 && infMatrix.rows == 6 && infMatrix.type() == CV_64FC1);
+	void setVariance(double rotVariance, double transVariance);
-		UASSERT_MSG(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0, "Transitional information should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, "Transitional information should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, "Transitional information should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, "Rotational information should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, "Rotational information should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0, "Rotational information should not be null! (set to 1 if unknown)");
-		infMatrix_ = infMatrix;
-	}
-	void setVariance(double rotVariance, double transVariance) {
-		UASSERT(uIsFinite(rotVariance) && rotVariance>0);
-		UASSERT(uIsFinite(transVariance) && transVariance>0);
-		infMatrix_ = cv::Mat::eye(6,6,CV_64FC1);
-		infMatrix_.at<double>(0,0) = 1.0/transVariance;
-		infMatrix_.at<double>(1,1) = 1.0/transVariance;
-		infMatrix_.at<double>(2,2) = 1.0/transVariance;
-		infMatrix_.at<double>(3,3) = 1.0/rotVariance;
-		infMatrix_.at<double>(4,4) = 1.0/rotVariance;
-		infMatrix_.at<double>(5,5) = 1.0/rotVariance;
-	}
 
-	Link merge(const Link & link) const
+	void setUserDataRaw(const cv::Mat & userDataRaw); // only set raw
-	{
+	void setUserData(const cv::Mat & userData); // detect automatically if raw or compressed. If raw, the data is compressed too.
-		UASSERT(to_ == link.from());
+	const cv::Mat & userDataRaw() const {return _userDataRaw;}
-		UASSERT(type_ == link.type());
+	const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
-		UASSERT(!transform_.isNull());
+	void uncompressUserData();
-		UASSERT(!link.transform().isNull());
+	cv::Mat uncompressUserDataConst() const;
-		UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1);
-		UASSERT(link.infMatrix().cols == 6 && link.infMatrix().rows == 6 && link.infMatrix().type() == CV_64FC1);
-		return Link(
-				from_,
-				link.to(),
-				type_,
-				transform_ * link.transform(),
-				infMatrix_ + link.infMatrix());
-	}
 
-	Link inverse() const
+	Link merge(const Link & link, Type outputType) const;
-	{
+	Link inverse() const;
-		return Link(to_, from_, type_, transform_.inverse(), infMatrix_);
-	}
 
 private:
 	int from_;
@@ -145,6 +85,10 @@ private:
 	Transform transform_;
 	Type type_;
 	cv::Mat infMatrix_; // Information matrix = covariance matrix ^ -1
+
+	// user data
+	cv::Mat _userDataCompressed;      // compressed data
+	cv::Mat _userDataRaw;
 };
 
 }

corelib/include/rtabmap/core/Memory.h

@@ -48,7 +48,6 @@ namespace rtabmap {
 
 class Signature;
 class DBDriver;
-class GraphNode;
 class VWDictionary;
 class VisualWord;
 class Feature2D;
@@ -141,7 +140,7 @@ public:
 			double & stamp,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
-	SensorData getNodeData(int nodeId, bool uncompressedData = false);
+	SensorData getNodeData(int nodeId, bool uncompressedData = false, bool keepLoadedDataInMemory = true);
 	void getNodeWords(int nodeId,
 			std::multimap<int, cv::KeyPoint> & words,
 			std::multimap<int, pcl::PointXYZ> & words3);
@@ -164,10 +163,7 @@ public:
 	virtual void dumpSignatures(const char * fileNameSign, bool words3D) const;
 	void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
 
-	void generateGraph(const std::string & fileName, std::set<int> ids = std::set<int>());
+	void generateGraph(const std::string & fileName, const std::set<int> & ids = std::set<int>());
-	void createGraph(GraphNode * parent,
-			unsigned int maxDepth,
-			const std::set<int> & endIds = std::set<int>());
 
 	//keypoint stuff
 	const VWDictionary * getVWDictionary() const;
@@ -232,6 +228,7 @@ private:
 	float _similarityThreshold;
 	bool _rawDataKept;
 	bool _binDataKept;
+	bool _saveDepth16Format;
 	bool _notLinkedNodesKeptInDb;
 	bool _incrementalMemory;
 	int _maxStMemSize;
@@ -280,6 +277,7 @@ private:
 	int _bowEstimationType;
 	double _bowPnPReprojError;
 	int _bowPnPFlags;
+	bool _bowVarianceFromInliersCount;
 	float _icpMaxTranslation;
 	float _icpMaxRotation;
 	int _icpDecimation;

corelib/include/rtabmap/core/Odometry.h

@@ -88,6 +88,7 @@ private:
 	int _estimationType;
 	double _pnpReprojError;
 	int _pnpFlags;
+	bool _varianceFromInliersCount;
 	Transform _pose;
 	int _resetCurrentCount;
 	double previousStamp_;

corelib/include/rtabmap/core/OdometryInfo.h

@@ -43,6 +43,7 @@ public:
 		features(-1),
 		localMapSize(-1),
 		timeEstimation(-1),
+		timeParticleFiltering(-1),
 		stamp(0),
 		interval(0),
 		distanceTravelled(0),

corelib/include/rtabmap/core/Parameters.h

@@ -56,7 +56,7 @@ typedef std::pair<std::string, std::string> ParametersPair;
  * 				DummyVideoImageWidth() {parameters_.insert(ParametersPair("Video/ImageWidth", "640"));}
  * 			};
  * 			DummyVideoImageWidth dummyVideoImageWidth;
- * @endcode
+ * @endcode
  */
 #define RTABMAP_PARAM(PREFIX, NAME, TYPE, DEFAULT_VALUE, DESCRIPTION) \
 	public: \
@@ -156,7 +156,7 @@ typedef std::pair<std::string, std::string> ParametersPair;
  * 		std::string strValue = Util::value(Parameters::getDefaultParameters(), theKey); // strValue = "640"
  * @endcode
  * @see getDefaultParameters()
- * TODO Add a detailed example with simple classes
+ * TODO Add a detailed example with simple classes
  */
 class RTABMAP_EXP Parameters
 {
@@ -186,6 +186,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, RehearsalSimilarity,     float, 0.6, 	"Rehearsal similarity.");
 	RTABMAP_PARAM(Mem, ImageKept, 		        bool, false, 	"Keep raw images in RAM.");
 	RTABMAP_PARAM(Mem, BinDataKept, 		    bool, true, 	"Keep binary data in db.");
+	RTABMAP_PARAM(Mem, SaveDepth16Format, 		bool, true, 	"Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
 	RTABMAP_PARAM(Mem, NotLinkedNodesKept, 	    bool, true, 	"Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
 	RTABMAP_PARAM(Mem, STMSize, 		        unsigned int, 10, "Short-term memory size.");
 	RTABMAP_PARAM(Mem, IncrementalMemory, 	    bool, true, 	"SLAM mode, otherwise it is Localization mode.");
@@ -200,12 +201,12 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, LaserScanVoxelSize,      float, 0.0,      "If > 0.0, voxelize laser scans when creating a signature.");
 	RTABMAP_PARAM(Mem, LocalSpaceLinksKeptInWM, bool, true,      "If local space links are kept in WM.");
 
-
 	// KeypointMemory (Keypoint-based)
 	RTABMAP_PARAM_COND(Kp, NNStrategy,       int, RTABMAP_NONFREE, 1, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
 	RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, 		"");
+	RTABMAP_PARAM(Kp, IncrementalFlann,      bool, true, 		"When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary doubles in size).");
 	RTABMAP_PARAM(Kp, MaxDepth,              float, 0.0, 		"Filter extracted keypoints by depth (0=inf)");
-	RTABMAP_PARAM(Kp, WordsPerImage,         int, 400, 			"");
+	RTABMAP_PARAM(Kp, WordsPerImage,         int, 400, 			"Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
 	RTABMAP_PARAM(Kp, BadSignRatio,          float, 0.2, 		"Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
 	RTABMAP_PARAM_COND(Kp, NndrRatio, 	     float, RTABMAP_NONFREE, 0.8, 0.9,		"NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
 	RTABMAP_PARAM_COND(Kp, DetectorStrategy, int, RTABMAP_NONFREE, 0, 2, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
@@ -248,7 +249,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(FAST, Gpu,                bool, false, 	"GPU-FAST: Use GPU version of FAST. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.");
 	RTABMAP_PARAM(FAST, GpuKeypointsRatio,  double, 0.05, 	"Used with FAST GPU.");
 
-	RTABMAP_PARAM(GFTT, QualityLevel, double, 0.01, "");
+	RTABMAP_PARAM(GFTT, QualityLevel, double, 0.001, "");
 	RTABMAP_PARAM(GFTT, MinDistance, double, 5, "");
 	RTABMAP_PARAM(GFTT, BlockSize, int, 3, "");
 	RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false, "");
@@ -283,18 +284,22 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(VhEp, RansacParam2,  float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC.");
 
 	// RGB-D SLAM
-	RTABMAP_PARAM(RGBD, Enabled,           bool, true, 	"");
+	RTABMAP_PARAM(RGBD, Enabled,                  bool, true,  "");
-	RTABMAP_PARAM(RGBD, PoseScanMatching,  bool, false, "Laser scan matching for odometry pose correction (laser scans are required).");
+	RTABMAP_PARAM(RGBD, PoseScanMatching,         bool, false, "Laser scan matching for odometry pose correction (laser scans are required).");
-	RTABMAP_PARAM(RGBD, LinearUpdate,      float, 0.0, 	"Min linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
+	RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.0,  "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
-	RTABMAP_PARAM(RGBD, AngularUpdate,     float, 0.0, 	"Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
+	RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.0,  "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
-	RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
+	RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
-	RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false,    "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
+	RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
-	RTABMAP_PARAM(RGBD, GoalReachedRadius,    float, 0.5, "Goal reached radius (m).");
+	RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 1.0,  "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph.");
-	RTABMAP_PARAM(RGBD, PlanVirtualLinks,  bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
+	RTABMAP_PARAM(RGBD, GoalReachedRadius,        float, 0.5,  "Goal reached radius (m).");
+	RTABMAP_PARAM(RGBD, PlanStuckIterations,      int, 0,      "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
+	RTABMAP_PARAM(RGBD, PlanLinearVelocity,       float, 0.0,  "Linear velocity (m/sec) used to compute path weights.");
+	RTABMAP_PARAM(RGBD, PlanAngularVelocity,      float, 0.0,  "Angular velocity (rad/sec) used to compute path weights.");
 	RTABMAP_PARAM(RGBD, GoalsSavedInUserData,     bool, false, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
-	RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
+	RTABMAP_PARAM(RGBD, MaxLocalRetrieved,        unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
-	RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
+	RTABMAP_PARAM(RGBD, LocalRadius,              float, 10,   "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
-	RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");
+	RTABMAP_PARAM(RGBD, LocalImmunizationRatio,   float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");
+	RTABMAP_PARAM(RGBD,  ScanMatchingIdsSavedInLinks, bool, true,      "Save scan matching IDs in link's user data.");
 
 	// Local loop closure detection
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionTime,     bool, false, 	"Detection over all locations in STM.");
@@ -308,24 +313,26 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, OptimizeIterations,        int, 100,      "Optimization iterations.");
 	RTABMAP_PARAM(RGBD, OptimizeSlam2D,            bool, false,  "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
 	RTABMAP_PARAM(RGBD, OptimizeVarianceIgnored,   bool, false,  "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
-	RTABMAP_PARAM(RGBD, OptimizeEpsilon,           double, 0.001,  "Stop optimizing when the error improvement is less than this value.");
+	RTABMAP_PARAM(RGBD, OptimizeEpsilon,           double, 0.0001,  "Stop optimizing when the error improvement is less than this value.");
+	RTABMAP_PARAM(RGBD, OptimizeRobust,            bool, true,  "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies).");
 
 	// Odometry
 	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Bag-of-words 1=Optical Flow");
 	RTABMAP_PARAM(Odom, FeatureType,            int, 6, 	    "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
 	RTABMAP_PARAM(Odom, EstimationType,  		int, 0,    	    "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP)");
-	RTABMAP_PARAM(Odom, MaxFeatures,            int, 400, 		"0 no limits.");
+	RTABMAP_PARAM(Odom, MaxFeatures,            int, 1000, 		"0 no limits.");
-	RTABMAP_PARAM(Odom, InlierDistance,         float, 0.02, 	"Maximum distance for visual word correspondences.");
+	RTABMAP_PARAM(Odom, InlierDistance,         float, 0.1,     "Maximum distance for visual word correspondences. Used by 3D->3D estimation approach.");
 	RTABMAP_PARAM(Odom, MinInliers,             int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
 	RTABMAP_PARAM(Odom, Iterations,             int, 100, 		"Maximum iterations to compute the transform from visual words.");
-	RTABMAP_PARAM(Odom, RefineIterations,       int, 5,        "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
+	RTABMAP_PARAM(Odom, RefineIterations,       int, 5,         "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
-	RTABMAP_PARAM(Odom, MaxDepth,               float, 4.0, 	"Max depth of the words (0 means no limit).");
+	RTABMAP_PARAM(Odom, MaxDepth,               float, 0,       "Max depth of the words (0 means no limit).");
 	RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,         "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
 	RTABMAP_PARAM_STR(Odom, RoiRatios,          "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
 	RTABMAP_PARAM(Odom, Force2D, 		        bool, false,     "Force 2D transform (3Dof: x,y and yaw).");
 	RTABMAP_PARAM(Odom, Holonomic, 		        bool, true,     "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
 	RTABMAP_PARAM(Odom, FillInfoData, 		    bool, true,     "Fill info with data (inliers/outliers features).");
 	RTABMAP_PARAM(Odom, ImageBufferSize,        unsigned int, 1, "Data buffer size (0 min inf).");
+	RTABMAP_PARAM(Odom, VarianceFromInliersCount, bool, false,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
 	RTABMAP_PARAM(Odom, PnPReprojError, 		double, 5.0,     "PnP reprojection error.");
 	RTABMAP_PARAM(Odom, PnPFlags, 				int, 1,    	     "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
 	RTABMAP_PARAM(Odom, ParticleFiltering, 		bool, false,     "Particle filtering to smooth the odometry trajectory.");
@@ -364,34 +371,35 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(LccIcp, MaxRotation,     float, 0.78,     "Maximum ICP rotation correction accepted (rad).");
 
 	RTABMAP_PARAM(LccBow, EstimationType,  int, 0,    	    "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
-	RTABMAP_PARAM(LccBow, MinInliers,      int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
+	RTABMAP_PARAM(LccBow, MinInliers,      int, 10, 		"Minimum visual word correspondences to compute geometry transform.");
-	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.02, 	"Maximum distance for visual word correspondences.");
+	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.1,      "Maximum distance for visual word correspondences. Used by 3D->3D estimation approach.");
 	RTABMAP_PARAM(LccBow, Iterations,      int, 100, 		"Maximum iterations to compute the transform from visual words.");
 	RTABMAP_PARAM(LccBow, RefineIterations,  int, 10,        "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
 	RTABMAP_PARAM(LccBow, Force2D, 		   bool, false,     "Force 2D transform (3Dof: x,y and yaw).");
 	RTABMAP_PARAM(LccBow, EpipolarGeometryVar, float, 0.02, "Epipolar geometry maximum variance to accept the loop closure.");
 	RTABMAP_PARAM(LccBow, PnPReprojError, 	 double, 5.0,    "PnP reprojection error.");
 	RTABMAP_PARAM(LccBow, PnPFlags, 	     int, 1,    	 "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
+	RTABMAP_PARAM(LccBow, VarianceFromInliersCount, bool, false,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
 	RTABMAP_PARAM_COND(LccReextract, Activated, bool, RTABMAP_NONFREE, false, true, "Activate re-extracting features on global loop closure.");
 	RTABMAP_PARAM(LccReextract, NNType, 	int, 3, 		"kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4.");
 	RTABMAP_PARAM(LccReextract, NNDR, 		float, 0.8, 	"NNDR: nearest neighbor distance ratio.");
 	RTABMAP_PARAM(LccReextract, FeatureType, int, 4, 		"0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
-	RTABMAP_PARAM(LccReextract, MaxWords, 	int, 600, 		"0 no limits.");
+	RTABMAP_PARAM(LccReextract, MaxWords, 	int, 1000, 		"0 no limits.");
 	RTABMAP_PARAM(LccReextract, MaxDepth, 	float, 0.0,     "Max depth of the words (0 means no limit).");
 
-	RTABMAP_PARAM(LccIcp3, Decimation,      int, 8, 		"Depth image decimation.");
+	RTABMAP_PARAM(LccIcp3, Decimation,      int, 4, 		"Depth image decimation.");
-	RTABMAP_PARAM(LccIcp3, MaxDepth,        float, 4.0, 	"Max cloud depth.");
+	RTABMAP_PARAM(LccIcp3, MaxDepth,        float, 3.0, 	"Max cloud depth.");
-	RTABMAP_PARAM(LccIcp3, VoxelSize,       float, 0.01, 	"Voxel size to be used for ICP computation.");
+	RTABMAP_PARAM(LccIcp3, VoxelSize,       float, 0.025, 	"Voxel size to be used for ICP computation.");
 	RTABMAP_PARAM(LccIcp3, Samples,         int, 0, 		"Random samples to be used for ICP computation. Not used if voxelSize is set.");
 	RTABMAP_PARAM(LccIcp3, MaxCorrespondenceDistance, float, 0.05, "ICP 3D: Max distance for point correspondences.");
 	RTABMAP_PARAM(LccIcp3, Iterations,      int, 30, 		"Max iterations.");
-	RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.0, 	"Ratio of matching correspondences to accept the transform.");
+	RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.2, 	"Ratio of matching correspondences to accept the transform.");
 	RTABMAP_PARAM(LccIcp3, PointToPlane,      bool, false, 	"Use point to plane ICP.");
 	RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors,      int, 20, 	"Number of neighbors to compute normals for point to plane.");
 
 	RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.05, 	"Max distance for point correspondences.");
 	RTABMAP_PARAM(LccIcp2, Iterations,      int, 30, 				"Max iterations.");
-	RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.0, 		"Ratio of matching correspondences to accept the transform.");
+	RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.3, 		"Ratio of matching correspondences to accept the transform.");
 	RTABMAP_PARAM(LccIcp2, VoxelSize,       float, 0.025, 			"Voxel size to be used for ICP computation.");
 
 	// Stereo disparity

corelib/include/rtabmap/core/Rtabmap.h

@@ -112,7 +112,7 @@ public:
 			const std::string & path,
 			bool optimized,
 			bool global,
-			int type // 0=raw/KITTI format, 1=rgbd-slam format, 2=TORO
+			int format // 0=raw, 1=rgbd-slam format, 2=KITTI format, 3=TORO, 4=g2o
 	);
 	void resetMemory();
 	void dumpPrediction() const;
@@ -129,14 +129,18 @@ public:
 			std::multimap<int, Link> & constraints,
 			bool optimized,
 			bool global,
-			std::map<int, Signature> * signatures = 0);
+			std::map<int, Signature> * signatures = 0);
-	void clearPath();
+
+	int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
+	void clearPath(int status); // -1=failed 0=idle/executing 1=success
 	bool computePath(int targetNode, bool global);
 	bool computePath(const Transform & targetPose); // only in current optimized map
 	const std::vector<std::pair<int, Transform> > & getPath() const {return _path;}
 	std::vector<std::pair<int, Transform> > getPathNextPoses() const;
 	std::vector<int> getPathNextNodes() const;
 	int getPathCurrentGoalId() const;
+	unsigned int getPathCurrentIndex() const {return _pathCurrentIndex;}
+	unsigned int getPathCurrentGoalIndex() const {return _pathGoalIndex;}
 	const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
 
 	std::map<int, Transform> getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const;
@@ -184,6 +188,7 @@ private:
 	bool _poseScanMatching;
 	bool _localLoopClosureDetectionTime;
 	bool _localLoopClosureDetectionSpace;
+	bool _scanMatchingIdsSavedInLinks;
 	float _localRadius;
 	float _localImmunizationRatio;
 	int _localDetectMaxGraphDepth;
@@ -191,6 +196,7 @@ private:
 	bool _localPathOdomPosesUsed;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
+	float _optimizationMaxLinearError;
 	bool _reextractLoopClosureFeatures;
 	int _reextractNNType;
 	float _reextractNNDR;
@@ -199,12 +205,16 @@ private:
 	float _reextractMaxDepth;
 	bool _startNewMapOnLoopClosure;
 	float _goalReachedRadius; // meters
-	bool _planVirtualLinks;
 	bool _goalsSavedInUserData;
+	int _pathStuckIterations;
+	float _pathLinearVelocity;
+	float _pathAngularVelocity;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;
 	double _lastProcessTime;
+	bool _someNodesHaveBeenTransferred;
+	float _distanceTravelled;
 
 	// Abstract classes containing all loop closure
 	// strategies for a type of signature or configuration.
@@ -227,14 +237,17 @@ private:
 	std::map<int, Transform> _optimizedPoses;
 	std::multimap<int, Link> _constraints;
 	Transform _mapCorrection;
-	Transform _mapTransform; // for localization mode
 	Transform _lastLocalizationPose; // for localization mode
+	int _lastLocalizationNodeId; // for localization mode
 
 	// Planning stuff
+	int _pathStatus;
 	std::vector<std::pair<int,Transform> > _path;
+	std::set<unsigned int> _pathUnreachableNodes;
 	unsigned int _pathCurrentIndex;
 	unsigned int _pathGoalIndex;
 	Transform _pathTransformToGoal;
+	int _pathStuckCount;
 
 };
 

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -65,7 +65,7 @@ public:
 			kCmdDumpMemory,
 			kCmdDumpPrediction,
 			kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin
-			kCmdExportPoses,      // params: [bool] global, [bool] optimized, [string] path, [int] type (0=KITTI/raw format, 1=RGBD-SLAM format, 2=TORO)
+			kCmdExportPoses,      // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO, 4=g2o)
 			kCmdCleanDataBuffer,
 			kCmdPublish3DMap,     // params: [bool] global, [bool] optimized, [bool] graphOnly
 			kCmdTriggerNewMap,
@@ -202,13 +202,19 @@ public:
 	RtabmapGlobalPathEvent(int goalId, const std::vector<std::pair<int, Transform> > & poses) :
 		UEvent(goalId),
 		_poses(poses) {}
+	RtabmapGlobalPathEvent(int goalId, const std::string & goalLabel, const std::vector<std::pair<int, Transform> > & poses) :
+			UEvent(goalId),
+			_goalLabel(goalLabel),
+			_poses(poses) {}
 
 	virtual ~RtabmapGlobalPathEvent() {}
 	int getGoal() const {return this->getCode();}
+	const std::string & getGoalLabel() const {return _goalLabel;}
 	const std::vector<std::pair<int, Transform> > & getPoses() const {return _poses;}
 	virtual std::string getClassName() const {return std::string("RtabmapGlobalPathEvent");}
 
 private:
+	std::string _goalLabel;
 	std::vector<std::pair<int, Transform> > _poses;
 };
 
@@ -228,6 +234,16 @@ private:
 	std::string _label;
 };
 
+class RtabmapGoalStatusEvent : public UEvent
+{
+public:
+	RtabmapGoalStatusEvent(int status):
+		UEvent(status){}
+
+	virtual ~RtabmapGoalStatusEvent() {}
+	virtual std::string getClassName() const {return std::string("RtabmapGoalStatusEvent");}
+};
+
 } // namespace rtabmap
 
 #endif /* RTABMAPEVENT_H_ */

corelib/include/rtabmap/core/SensorData.h

@@ -75,6 +75,7 @@ public:
 	SensorData(
 			const cv::Mat & laserScan,
 			int laserScanMaxPts,
+			float laserScanMaxRange,
 			const cv::Mat & rgb,
 			const cv::Mat & depth,
 			const CameraModel & cameraModel,
@@ -95,6 +96,7 @@ public:
 	SensorData(
 			const cv::Mat & laserScan,
 			int laserScanMaxPts,
+			float laserScanMaxRange,
 			const cv::Mat & rgb,
 			const cv::Mat & depth,
 			const std::vector<CameraModel> & cameraModels,
@@ -115,6 +117,7 @@ public:
 	SensorData(
 			const cv::Mat & laserScan,
 			int laserScanMaxPts,
+			float laserScanMaxRange,
 			const cv::Mat & left,
 			const cv::Mat & right,
 			const StereoCameraModel & cameraModel,
@@ -136,8 +139,8 @@ public:
 			_laserScanCompressed.empty() &&
 			_cameraModels.size() == 0 &&
 			!_stereoCameraModel.isValid() &&
-			!_userDataRaw.empty() &&
+			_userDataRaw.empty() &&
-			!_userDataCompressed.empty() &&
+			_userDataCompressed.empty() &&
 			_keypoints.size() == 0 &&
 			_descriptors.empty());
 	}
@@ -147,6 +150,7 @@ public:
 	double stamp() const {return _stamp;}
 	void setStamp(double stamp) {_stamp = stamp;}
 	int laserScanMaxPts() const {return _laserScanMaxPts;}
+	float laserScanMaxRange() const {return _laserScanMaxRange;}
 
 	const cv::Mat & imageCompressed() const {return _imageCompressed;}
 	const cv::Mat & depthOrRightCompressed() const {return _depthOrRightCompressed;}
@@ -157,7 +161,7 @@ public:
 	const cv::Mat & laserScanRaw() const {return _laserScanRaw;}
 	void setImageRaw(const cv::Mat & imageRaw) {_imageRaw = imageRaw;}
 	void setDepthOrRightRaw(const cv::Mat & depthOrImageRaw) {_depthOrRightRaw =depthOrImageRaw;}
-	void setLaserScanRaw(const cv::Mat & laserScanRaw, int laserScanMaxPts) {_laserScanRaw =laserScanRaw;_laserScanMaxPts = laserScanMaxPts;}
+	void setLaserScanRaw(const cv::Mat & laserScanRaw, int maxPts, float maxRange) {_laserScanRaw =laserScanRaw;_laserScanMaxPts = maxPts;_laserScanMaxRange=maxRange;}
 	void setCameraModel(const CameraModel & model) {_cameraModels.clear(); _cameraModels.push_back(model);}
 	void setCameraModels(const std::vector<CameraModel> & models) {_cameraModels = models;}
 	void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModel = stereoCameraModel;}
@@ -190,6 +194,7 @@ private:
 	int _id;
 	double _stamp;
 	int _laserScanMaxPts;
+	float _laserScanMaxRange;
 
 	cv::Mat _imageCompressed;          // compressed image
 	cv::Mat _depthOrRightCompressed;   // compressed image

corelib/include/rtabmap/core/Statistics.h

@@ -57,11 +57,12 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Highest_hypothesis_id,);
 	RTABMAP_STATS(Loop, Highest_hypothesis_value,);
 	RTABMAP_STATS(Loop, Vp_hypothesis,);
-	RTABMAP_STATS(Loop, ReactivateId,);
+	RTABMAP_STATS(Loop, Reactivate_id,);
 	RTABMAP_STATS(Loop, Hypothesis_ratio,);
 	RTABMAP_STATS(Loop, Hypothesis_reactivated,);
-	RTABMAP_STATS(Loop, VisualInliers,);
+	RTABMAP_STATS(Loop, Visual_inliers,);
 	RTABMAP_STATS(Loop, Last_id,);
+	RTABMAP_STATS(Loop, Optimization_max_error, m);
 
 	RTABMAP_STATS(LocalLoop, Time_closures,);
 	RTABMAP_STATS(LocalLoop, Space_last_closure_id,);
@@ -83,8 +84,11 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Signatures_retrieved,);
 	RTABMAP_STATS(Memory, Images_buffered,);
 	RTABMAP_STATS(Memory, Rehearsal_sim,);
+	RTABMAP_STATS(Memory, Rehearsal_id,);
 	RTABMAP_STATS(Memory, Rehearsal_merged,);
 	RTABMAP_STATS(Memory, Local_graph_size,);
+	RTABMAP_STATS(Memory, Small_movement,);
+	RTABMAP_STATS(Memory, Distance_travelled, m);
 
 	RTABMAP_STATS(Timing, Memory_update, ms);
 	RTABMAP_STATS(Timing, Scan_matching, ms);
@@ -118,6 +122,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Compressing_data, ms);
 
 	RTABMAP_STATS(Keypoint, Dictionary_size, words);
+	RTABMAP_STATS(Keypoint, Indexed_words, words);
+	RTABMAP_STATS(Keypoint, Index_memory_usage, KB);
 	RTABMAP_STATS(Keypoint, Response_threshold,);
 
 public:

corelib/include/rtabmap/core/Transform.h

@@ -51,6 +51,8 @@ public:
 	Transform(const cv::Mat & transformationMatrix);
 	// x,y,z, roll,pitch,yaw
 	Transform(float x, float y, float z, float roll, float pitch, float yaw);
+	// x,y, theta
+	Transform(float x, float y, float theta);
 
 	float r11() const {return data()[0];}
 	float r12() const {return data()[1];}

corelib/include/rtabmap/core/VWDictionary.h

@@ -41,6 +41,7 @@ namespace rtabmap
 
 class DBDriver;
 class VisualWord;
+class FlannIndex;
 
 class RTABMAP_EXP VWDictionary
 {
@@ -74,6 +75,8 @@ public:
 	unsigned int getNotIndexedWordsCount() const {return (int)_notIndexedWords.size();}
 	int getLastIndexedWordId() const;
 	int getTotalActiveReferences() const {return _totalActiveReferences;}
+	unsigned int getIndexedWordsCount() const;
+	unsigned int getIndexMemoryUsed() const;
 	void setNNStrategy(NNStrategy strategy);
 	bool isIncremental() const {return _incrementalDictionary;}
 	void setIncrementalDictionary();
@@ -97,14 +100,16 @@ protected:
 
 private:
 	bool _incrementalDictionary;
+	bool _incrementalFlann;
 	float _nndrRatio;
 	std::string _dictionaryPath; // a pre-computed dictionary (.txt)
 	bool _newWordsComparedTogether;
 	int _lastWordId;
-	cv::flann::Index * _flannIndex;
+	FlannIndex * _flannIndex;
 	cv::Mat _dataTree;
 	NNStrategy _strategy;
 	std::map<int ,int> _mapIndexId;
+	std::map<int ,int> _mapIdIndex;
 	std::map<int, VisualWord*> _unusedWords; //<id,VisualWord*>, note that these words stay in _visualWords
 	std::set<int> _notIndexedWords; // Words that are not indexed in the dictionary
 	std::set<int> _removedIndexedWords; // Words not anymore in the dictionary but still indexed in the dictionary

corelib/include/rtabmap/core/util2d.h

@@ -41,7 +41,8 @@ namespace util2d
 
 cv::Mat RTABMAP_EXP disparityFromStereoImages(
 		const cv::Mat & leftImage,
-		const cv::Mat & rightImage);
+		const cv::Mat & rightImage,
+		int type = CV_32FC1); // CV_32FC1 or CV_16SC1
 
 cv::Mat RTABMAP_EXP disparityFromStereoImages(
 		const cv::Mat & leftImage,
@@ -53,6 +54,10 @@ cv::Mat RTABMAP_EXP disparityFromStereoImages(
 		double flowEps = 0.02,
 		float maxCorrespondencesSlope = 0.1f);
 
+cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
+		float fx, float baseline,
+		int type = CV_32FC1); // CV_32FC1 or CV_16UC1
+
 cv::Mat RTABMAP_EXP depthFromStereoImages(
 		const cv::Mat & leftImage,
 		const cv::Mat & rightImage,
@@ -78,6 +83,9 @@ cv::Mat RTABMAP_EXP depthFromStereoCorrespondences(
 		const std::vector<unsigned char> & mask,
 		float fx, float baseline);
 
+cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
+cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
+
 float RTABMAP_EXP getDepth(
 		const cv::Mat & depthImage,
 		float x, float y,

corelib/include/rtabmap/core/util3d.h

@@ -127,9 +127,6 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
 					float maxDepth = 0,
 					const Transform & localTransform = Transform::getIdentity());
 
-cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
-cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
-
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
 
@@ -147,10 +144,6 @@ pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
 		const cv::Mat & disparity,
 		float cx, float cy, float fx, float baseline);
 
-cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
-		float fx, float baseline,
-		int type = CV_32FC1);
-
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(
 		const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds);
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP concatenateClouds(

corelib/include/rtabmap/core/util3d_filtering.h

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define UTIL3D_FILTERING_H_
 
 #include <rtabmap/core/RtabmapExp.h>
+#include <rtabmap/core/Transform.h>
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -46,6 +47,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP voxelize(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float voxelSize);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP voxelize(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		float voxelSize);
 
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP sampling(
@@ -60,12 +64,31 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP passThrough(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const std::string & axis,
 		float min,
-		float max);
+		float max,
+		bool negative = false);
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP passThrough(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const std::string & axis,
 		float min,
-		float max);
+		float max,
+		bool negative = false);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP frustumFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const Transform & cameraPose,
+		float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2
+		float verticalFOV,   // in degrees, yfov = atan((image_height/2)/fy)*2
+		float nearClipPlaneDistance,
+		float farClipPlaneDistance,
+		bool negative = false);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP frustumFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const Transform & cameraPose,
+		float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2
+		float verticalFOV,   // in degrees, yfov = atan((image_height/2)/fy)*2
+		float nearClipPlaneDistance,
+		float farClipPlaneDistance,
+		bool negative = false);
 
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
@@ -141,6 +164,32 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
 		float radiusSearch,
 		int minNeighborsInRadius = 0);
 
+/**
+ * For convenience.
+ */
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
+		float radiusSearch,
+		int minNeighborsInRadius = 0);
+
+/**
+ * Subtract a cloud from another one using radius filtering.
+ * @param cloud the input cloud.
+ * @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
+ * @param cloud the input cloud to subtract.
+ * @param indices the input indices of the subtracted cloud to check, if empty, all points in the cloud are checked.
+ * @param radiusSearch the radius in meter.
+ * @return the indices of the points satisfying the parameters.
+ */
+pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
+		const pcl::IndicesPtr & substractIndices,
+		float radiusSearch,
+		int minNeighborsInRadius = 0);
+
 
 /**
  * For convenience.

corelib/include/rtabmap/core/util3d_mapping.h

@@ -47,7 +47,9 @@ void RTABMAP_EXP occupancy2DFromLaserScan(
 		const cv::Mat & scan,
 		cv::Mat & ground,
 		cv::Mat & obstacles,
-		float cellSize);
+		float cellSize,
+		bool unknownSpaceFilled = false,
+		float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true
 
 cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(
 		const std::map<int, Transform> & poses,
@@ -64,7 +66,8 @@ cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
 		bool unknownSpaceFilled,
 		float & xMin,
 		float & yMin,
-		float minMapSize = 0.0f);
+		float minMapSize = 0.0f,
+		float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true
 
 void RTABMAP_EXP rayTrace(const cv::Point2i & start,
 		const cv::Point2i & end,

corelib/include/rtabmap/core/util3d_surface.h

@@ -30,10 +30,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/RtabmapExp.h>
 
-
 #include <pcl/PolygonMesh.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
+#include <pcl/TextureMesh.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/CameraModel.h>
 
 namespace rtabmap
 {
@@ -49,7 +51,14 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(
 		float gp3MaximumSurfaceAngle = M_PI/4,
 		float gp3MinimumAngle = M_PI/18,
 		float gp3MaximumAngle = 2*M_PI/3,
-		bool gp3NormalConsistency = false);
+		bool gp3NormalConsistency = true);
+
+pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
+		const pcl::PolygonMesh::Ptr & mesh,
+		const std::map<int, Transform> & poses,
+		const std::map<int, CameraModel> & cameraModels,
+		const std::map<int, cv::Mat> & images,
+		const std::string & tmpDirectory = ".");
 
 pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -59,10 +68,54 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		int normalKSearch = 20);
 
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormalsSmoothed(
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		float smoothingSearchRadius = 0.025,
+		float searchRadius = 0.0f,
-		bool smoothingPolynomialFit = true);
+		int polygonialOrder = 2,
+		int upsamplingMethod = 0, // NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION
+		float upsamplingRadius = 0.0f,   // SAMPLE_LOCAL_PLANE
+		float upsamplingStep = 0.0f,     // SAMPLE_LOCAL_PLANE
+		int pointDensity = 0,            // RANDOM_UNIFORM_DENSITY
+		float dilationVoxelSize = 1.0f,  // VOXEL_GRID_DILATION
+		int dilationIterations = 0);     // VOXEL_GRID_DILATION
+
+void RTABMAP_EXP adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
+		const std::vector<int> & rawCameraIndices,
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud);
+
+pcl::PolygonMesh::Ptr RTABMAP_EXP meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor);
+
+template<typename pointT>
+std::vector<pcl::Vertices> normalizePolygonsSide(
+		const pcl::PointCloud<pointT> & cloud,
+		const std::vector<pcl::Vertices> & polygons,
+		const pcl::PointXYZ & viewPoint = pcl::PointXYZ(0,0,0))
+{
+	std::vector<pcl::Vertices> output(polygons.size());
+	for(unsigned int i=0; i<polygons.size(); ++i)
+	{
+		pcl::Vertices polygon = polygons[i];
+		Eigen::Vector3f v1 = cloud.at(polygon.vertices[1]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
+		Eigen::Vector3f v2 = cloud.at(polygon.vertices[2]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
+		Eigen::Vector3f n = (v1.cross(v2)).normalized();
+
+		Eigen::Vector3f p = Eigen::Vector3f(viewPoint.x, viewPoint.y, viewPoint.z) - cloud.at(polygon.vertices[1]).getVector3fMap();
+
+		float result = n.dot(p);
+		if(result < 0)
+		{
+			//reverse vertices order
+			int tmp = polygon.vertices[0];
+			polygon.vertices[0] = polygon.vertices[2];
+			polygon.vertices[2] = tmp;
+		}
+
+		output[i] = polygon;
+	}
+	return output;
+}
 
 } // namespace util3d
 } // namespace rtabmap

corelib/include/rtabmap/core/util3d_transforms.h

@@ -46,6 +46,12 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const Transform & transform);
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const Transform & transform);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const Transform & transform);
 
 pcl::PointXYZ RTABMAP_EXP transformPoint(
 		const pcl::PointXYZ & pt,

corelib/src/BayesFilter.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "BayesFilter.h"
+#include "rtabmap/core/BayesFilter.h"
 #include "rtabmap/core/Memory.h"
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/Parameters.h"
@@ -289,9 +289,9 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
 				for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
 				{
 					float sum = 0.0f; // sum values added
-					sum += this->addNeighborProb(prediction, i, neighbors, idToIndexMap);
+					sum += this->addNeighborProb(prediction, idToIndexMap.at(*iter), neighbors, idToIndexMap);
 					idsDone.insert(*iter);
-					this->normalize(prediction, i, sum, ids[0]<0);
+					this->normalize(prediction, idToIndexMap.at(*iter), sum, ids[0]<0);
 				}
 			}
 			else
@@ -451,6 +451,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 	UDEBUG("time getting removed ids = %fs", timer.restart());
 
 	int added = 0;
+	float epsilon = 0.00001f;
 	// get ids to update
 	std::set<int> idsToUpdate;
 	for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
@@ -460,16 +461,19 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 			if(removedIds.find(oldIds[i]) != removedIds.end())
 			{
 				unsigned int cols = oldPrediction.cols;
+				int count = 0;
 				for(unsigned int j=0; j<cols; ++j)
 				{
-					if(((const float *)oldPrediction.data)[i + j*cols] != 0.0f &&
+					if(((const float *)oldPrediction.data)[i + j*cols] > epsilon &&
 					   j!=i &&
 					   removedIds.find(oldIds[j]) == removedIds.end())
 					{
 						//UDEBUG("to update id=%d from id=%d removed (value=%f)", oldIds[j], oldIds[i], ((const float *)oldPrediction.data)[i + j*cols]);
 						idsToUpdate.insert(oldIds[j]);
+						++count;
 					}
 				}
+				UDEBUG("From removed id %d, %d neighbors to update.", oldIds[i], count);
 			}
 		}
 		if(i<newIds.size() && !uContains(oldIdToIndexMap,newIds[i]))
@@ -478,29 +482,66 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 			float sum = this->addNeighborProb(prediction, i, neighbors, newIdToIndexMap);
 			this->normalize(prediction, i, sum, newIds[0]<0);
 			++added;
+			int count = 0;
 			for(std::map<int,int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
 			{
 				if(uContains(oldIdToIndexMap, iter->first) &&
 				   removedIds.find(iter->first) == removedIds.end())
 				{
 					idsToUpdate.insert(iter->first);
+					++count;
 				}
 			}
+			UDEBUG("From added id %d, %d neighbors to update.", newIds[i], count);
 		}
 	}
-	UDEBUG("time getting ids to update = %fs", timer.restart());
+	UDEBUG("time getting %d ids to update = %fs", idsToUpdate.size(), timer.restart());
 
 	// update modified/added ids
 	int modified = 0;
+	std::set<int> idsDone;
 	for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
 	{
-		std::map<int, int> neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, true);
+		if(idsDone.find(*iter) == idsDone.end() && *iter > 0)
-		int index = newIdToIndexMap.at(*iter);
+		{
-		float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap);
+			std::map<int, int> neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, true);
-		this->normalize(prediction, index, sum, newIds[0]<0);
+
-		++modified;
+			std::list<int> idsLoopMargin;
+			//filter neighbors in STM
+			for(std::map<int, int>::iterator jter=neighbors.begin(); jter!=neighbors.end();)
+			{
+				if(memory->isInSTM(jter->first))
+				{
+					neighbors.erase(jter++);
+				}
+				else
+				{
+					if(jter->second == 0)
+					{
+						idsLoopMargin.push_back(jter->first);
+					}
+					++jter;
+				}
+			}
+
+			// should at least have 1 id in idsMarginLoop
+			if(idsLoopMargin.size() == 0)
+			{
+				UFATAL("No 0 margin neighbor for signature %d !?!?", *iter);
+			}
+
+			// same neighbor tree for loop signatures (margin = 0)
+			for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
+			{
+				int index = newIdToIndexMap.at(*iter);
+				float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap);
+				idsDone.insert(*iter);
+				this->normalize(prediction, index, sum, newIds[0]<0);
+				++modified;
+			}
+		}
 	}
-	UDEBUG("time updating modified/added ids = %fs", timer.restart());
+	UDEBUG("time updating modified/added %d ids = %fs", idsToUpdate.size(), timer.restart());
 
 	//UDEBUG("oldIds.size()=%d, oldPrediction.cols=%d, oldPrediction.rows=%d", oldIds.size(), oldPrediction.cols, oldPrediction.rows);
 	//UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows);
@@ -510,15 +551,22 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 	{
 		if(oldIds[i]>0 && removedIds.find(oldIds[i]) == removedIds.end() && idsToUpdate.find(oldIds[i]) == idsToUpdate.end())
 		{
-			for(int j=0; j<oldPrediction.cols; ++j)
+			for(int j=i; j<oldPrediction.cols; ++j)
 			{
-				if(removedIds.find(oldIds[j]) == removedIds.end() && ((const float *)oldPrediction.data)[i + j*oldPrediction.cols] != 0.0f)
+				if(removedIds.find(oldIds[j]) == removedIds.end() && ((const float *)oldPrediction.data)[i + j*oldPrediction.cols] > epsilon)
 				{
 					//UDEBUG("i=%d, j=%d", i, j);
 					//UDEBUG("oldIds[i]=%d, oldIds[j]=%d", oldIds[i], oldIds[j]);
 					//UDEBUG("newIdToIndexMap.at(oldIds[i])=%d", newIdToIndexMap.at(oldIds[i]));
 					//UDEBUG("newIdToIndexMap.at(oldIds[j])=%d", newIdToIndexMap.at(oldIds[j]));
-					((float *)prediction.data)[newIdToIndexMap.at(oldIds[i]) + newIdToIndexMap.at(oldIds[j])*prediction.cols] = ((const float *)oldPrediction.data)[i + j*oldPrediction.cols];
+					float v = ((const float *)oldPrediction.data)[i + j*oldPrediction.cols];
+					int ii = newIdToIndexMap.at(oldIds[i]);
+					int jj = newIdToIndexMap.at(oldIds[j]);
+					((float *)prediction.data)[ii + jj*prediction.cols] = v;
+					if(ii != jj)
+					{
+						((float *)prediction.data)[jj + ii*prediction.cols] = v;
+					}
 				}
 			}
 			++copied;
@@ -536,6 +584,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 			for(int j=1; j<prediction.cols; j++)
 			{
 				((float*)prediction.data)[j*prediction.cols] = val;
+				((float*)prediction.data)[j] = _predictionLC[0];
 			}
 		}
 		else if(prediction.cols>0)

corelib/src/CMakeLists.txt

@@ -42,6 +42,7 @@ SET(SRC_FILES
 	SensorData.cpp
 	Graph.cpp
 	Compression.cpp
+	Link.cpp
 	
 	Odometry.cpp
 	OdometryThread.cpp
@@ -57,6 +58,9 @@ SET(SRC_FILES
 	toro3d/posegraph2.cpp
 	toro3d/treeoptimizer2.cpp
 	
+	rtflann/ext/lz4.c
+	rtflann/ext/lz4hc.c
+	
 	sqlite3/sqlite3.c
 )
 
@@ -149,8 +153,44 @@ IF(G2O_FOUND)
 		${LIBRARIES}
 		${G2O_LIBRARIES}
 	)
+	#Newest versions require std11
+	SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+	
+	SET(SRC_FILES
+    	${SRC_FILES}
+		vertigo/g2o/edge_se2MaxMixture.cpp
+		vertigo/g2o/edge_se2Switchable.cpp
+		vertigo/g2o/edge_se3Switchable.cpp
+		vertigo/g2o/edge_switchPrior.cpp
+		vertigo/g2o/types_g2o_robust.cpp
+		vertigo/g2o/vertex_switchLinear.cpp
+	)
 ENDIF(G2O_FOUND)
 
+IF(GTSAM_FOUND)
+	ADD_DEFINITIONS("-DWITH_GTSAM")
+	SET(INCLUDE_DIRS 
+		${INCLUDE_DIRS} 
+		${GTSAM_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		gtsam
+	)
+ENDIF(GTSAM_FOUND)
+
+IF(cvsba_FOUND)
+	ADD_DEFINITIONS("-DWITH_CVSBA")
+	SET(INCLUDE_DIRS 
+		${INCLUDE_DIRS} 
+		${cvsba_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${cvsba_LIBS}
+	)
+ENDIF(cvsba_FOUND)
+
 ####################################
 # Generate resources files
 ####################################

corelib/src/CameraModel.cpp

@@ -231,6 +231,17 @@ bool CameraModel::save(const std::string & directory) const
 		fs << "data" << std::vector<double>((double*)D_.data, ((double*)D_.data)+(D_.rows*D_.cols));
 		fs << "}";
 
+		// compaibility with ROS
+
+		if(D_.cols > 5)
+		{
+			fs << "distortion_model" << "rational_polynomial";
+		}
+		else
+		{
+			fs << "distortion_model" << "plumb_bob";
+		}
+
 		fs << "rectification_matrix" << "{";
 		fs << "rows" << R_.rows;
 		fs << "cols" << R_.cols;
@@ -266,6 +277,24 @@ void CameraModel::scale(double scale)
 	P_.at<double>(1,2) *= scale;
 }
 
+double CameraModel::horizontalFOV() const
+{
+	if(imageWidth() > 0 && fx() > 0.0)
+	{
+		return atan((double(imageWidth())/2.0)/fx())*2.0*180.0/CV_PI;
+	}
+	return 0.0;
+}
+
+double CameraModel::verticalFOV() const
+{
+	if(imageHeight() > 0 && fy() > 0.0)
+	{
+		return atan((double(imageHeight())/2.0)/fy())*2.0*180.0/CV_PI;
+	}
+	return 0.0;
+}
+
 cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const
 {
 	if(!mapX_.empty() && !mapY_.empty())

corelib/src/CameraRGBD.cpp

@@ -390,7 +390,8 @@ CameraOpenNI2::CameraOpenNI2(
 #endif
 	_depthFx(0.0f),
 	_depthFy(0.0f),
-	_deviceId(deviceId)
+	_deviceId(deviceId),
+	_openNI2StampsAndIDsUsed(false)
 {
 }
 
@@ -715,7 +716,14 @@ SensorData CameraOpenNI2::captureImage()
 						float(rgb.cols/2) - 0.5f,  //cx
 						float(rgb.rows/2) - 0.5f,  //cy
 						this->getLocalTransform());
-				data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
+				if(_openNI2StampsAndIDsUsed)
+				{
+					data = SensorData(rgb, depth, model, depthFrame.getFrameIndex(), double(depthFrame.getTimestamp()) / 1000000.0);
+				}
+				else
+				{
+					data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
+				}
 			}
 		}
 	}

corelib/src/CameraThread.cpp

@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/core/util3d.h"
 
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/ULogger.h>
@@ -40,7 +42,8 @@ namespace rtabmap
 CameraThread::CameraThread(Camera * camera) :
 		_camera(camera),
 		_mirroring(false),
-		_colorOnly(false)
+		_colorOnly(false),
+		_stereoToDepth(false)
 {
 	UASSERT(_camera != 0);
 }
@@ -96,6 +99,16 @@ void CameraThread::mainLoop()
 				data.setDepthOrRightRaw(tmpDepth);
 			}
 		}
+		if(_stereoToDepth && data.stereoCameraModel().isValid() && !data.rightRaw().empty())
+		{
+			cv::Mat depth = util2d::depthFromDisparity(
+					util2d::disparityFromStereoImages(data.imageRaw(), data.rightRaw()),
+					data.stereoCameraModel().left().fx(),
+					data.stereoCameraModel().baseline());
+			data.setCameraModel(data.stereoCameraModel().left());
+			data.setDepthOrRightRaw(depth);
+			data.setStereoCameraModel(StereoCameraModel());
+		}
 
 		this->post(new CameraEvent(data, _camera->getSerial()));
 	}

corelib/src/Compression.cpp

@@ -88,7 +88,16 @@ std::vector<unsigned char> compressImage(const cv::Mat & image, const std::strin
 	std::vector<unsigned char> bytes;
 	if(!image.empty())
 	{
-		cv::imencode(format, image, bytes);
+		if(image.type() == CV_32FC1)
+		{
+			//save in 8bits-4channel
+			cv::Mat bgra(image.size(), CV_8UC4, image.data);
+			cv::imencode(format, bgra, bytes);
+		}
+		else
+		{
+			cv::imencode(format, image, bytes);
+		}
 	}
 	return bytes;
 }
@@ -114,6 +123,10 @@ cv::Mat uncompressImage(const cv::Mat & bytes)
 #else
 		image = cv::imdecode(bytes, -1);
 #endif
+		if(image.type() == CV_8UC4)
+		{
+			image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+		}
 	}
 	return image;
 }
@@ -128,6 +141,10 @@ cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
 #else
 		image = cv::imdecode(bytes, -1);
 #endif
+		if(image.type() == CV_8UC4)
+		{
+			image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+		}
 	}
 	return image;
 }

corelib/src/DBDriver.cpp

@@ -28,15 +28,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/DBDriver.h"
 
 #include "rtabmap/core/Signature.h"
-#include "VisualWord.h"
+#include "rtabmap/core/VisualWord.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UMath.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UStl.h"
+#include "DBDriverSqlite3.h"
 
 namespace rtabmap {
 
+DBDriver * DBDriver::create(const ParametersMap & parameters)
+{
+	// well, we only have Sqlite3 database type for now :P
+	return new DBDriverSqlite3(parameters);
+}
+
 DBDriver::DBDriver(const ParametersMap & parameters) :
 	_emptyTrashesTime(0),
 	_timestampUpdate(true)
@@ -291,6 +298,23 @@ void DBDriver::saveOrUpdate(const std::vector<VisualWord *> & words) const
 	}
 }
 
+void DBDriver::addLink(const Link & link)
+{
+	_dbSafeAccessMutex.lock();
+	this->addLinkQuery(link);
+	_dbSafeAccessMutex.unlock();
+}
+void DBDriver::removeLink(int from, int to)
+{
+	this->executeNoResult(uFormat("DELETE FROM Link WHERE from_id=%d and to_id=%d", from, to).c_str());
+}
+void DBDriver::updateLink(const Link & link)
+{
+	_dbSafeAccessMutex.lock();
+	this->updateLinkQuery(link);
+	_dbSafeAccessMutex.unlock();
+}
+
 void DBDriver::load(VWDictionary * dictionary) const
 {
 	_dbSafeAccessMutex.lock();
@@ -715,4 +739,155 @@ void DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int proce
 	}
 }
 
+void DBDriver::generateGraph(
+		const std::string & fileName,
+		const std::set<int> & idsInput,
+		const std::map<int, Signature *> & otherSignatures)
+{
+	if(this->isConnected())
+	{
+		if(!fileName.empty())
+		{
+			FILE* fout = 0;
+			#ifdef _MSC_VER
+				fopen_s(&fout, fileName.c_str(), "w");
+			#else
+				fout = fopen(fileName.c_str(), "w");
+			#endif
+
+			 if (!fout)
+			 {
+				 UERROR("Cannot open file %s!", fileName.c_str());
+				 return;
+			 }
+
+			 std::set<int> ids;
+			 if(idsInput.size() == 0)
+			 {
+				 this->getAllNodeIds(ids);
+				 UDEBUG("ids.size()=%d", ids.size());
+				 for(std::map<int, Signature*>::const_iterator iter=otherSignatures.begin(); iter!=otherSignatures.end(); ++iter)
+				 {
+					 ids.insert(iter->first);
+				 }
+			 }
+			 else
+			 {
+				 ids = idsInput;
+			 }
+
+			 const char * colorG = "green";
+			 const char * colorP = "pink";
+	;		 UINFO("Generating map with %d locations", ids.size());
+			 fprintf(fout, "digraph G {\n");
+			 for(std::set<int>::iterator i=ids.begin(); i!=ids.end(); ++i)
+			 {
+				 if(otherSignatures.find(*i) == otherSignatures.end())
+				 {
+					 int id = *i;
+					 std::map<int, Link> links;
+					 this->loadLinks(id, links);
+					 int weight = 0;
+					 this->getWeight(id, weight);
+					 for(std::map<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
+					 {
+						 int weightNeighbor = 0;
+						 if(otherSignatures.find(iter->first) == otherSignatures.end())
+						 {
+							 this->getWeight(iter->first, weightNeighbor);
+						 }
+						 else
+						 {
+							 weightNeighbor = otherSignatures.find(iter->first)->second->getWeight();
+						 }
+						 //UDEBUG("Add neighbor link from %d to %d", id, iter->first);
+						 if(iter->second.type() == Link::kNeighbor)
+						 {
+							 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\"\n",
+									 id,
+									 weight,
+									 iter->first,
+									 weightNeighbor);
+						 }
+						 else if(iter->first > id)
+						 {
+							 //loop
+							 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n",
+									 id,
+									 weight,
+									 iter->first,
+									 weightNeighbor,
+									 colorG);
+						 }
+						 else
+						 {
+							 //child
+							 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",
+									 id,
+									 weight,
+									 iter->first,
+									 weightNeighbor,
+									 colorP);
+						 }
+					 }
+				 }
+			 }
+			 for(std::map<int, Signature*>::const_iterator i=otherSignatures.begin(); i!=otherSignatures.end(); ++i)
+			 {
+				 if(ids.find(i->first) != ids.end())
+				 {
+					 int id = i->second->id();
+					 const std::map<int, Link> & links = i->second->getLinks();
+					 int weight = i->second->getWeight();
+					 for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
+					 {
+						 int weightNeighbor = 0;
+						 const Signature * s = uValue(otherSignatures, iter->first, (Signature*)0);
+						 if(s)
+						 {
+							 weightNeighbor = s->getWeight();
+						 }
+						 else
+						 {
+							 this->getWeight(iter->first, weightNeighbor);
+						 }
+						 //UDEBUG("Add neighbor link from %d to %d", id, iter->first);
+						 if(iter->second.type() == Link::kNeighbor)
+						 {
+							 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\"\n",
+									 id,
+									 weight,
+									 iter->first,
+									 weightNeighbor);
+						 }
+						 else if(iter->first > id)
+						 {
+							 //loop
+							 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n",
+									 id,
+									 weight,
+									 iter->first,
+									 weightNeighbor,
+									 colorG);
+						 }
+						 else
+						 {
+							 //child
+							 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",
+									 id,
+									 weight,
+									 iter->first,
+									 weightNeighbor,
+									 colorP);
+						 }
+					 }
+				 }
+			 }
+			 fprintf(fout, "}\n");
+			 fclose(fout);
+			 UINFO("Graph saved to \"%s\" (Tip: $ neato -Tpdf \"%s\" -o out.pdf)", fileName.c_str(), fileName.c_str());
+		}
+	}
+}
+
 } // namespace rtabmap

corelib/src/DBDriverSqlite3.cpp

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "DBDriverSqlite3.h"
 
 #include "rtabmap/core/Signature.h"
-#include "VisualWord.h"
+#include "rtabmap/core/VisualWord.h"
 #include "rtabmap/core/VWDictionary.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/Compression.h"
@@ -457,7 +457,14 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		if(uStrNumCmp(_version, "0.10.1") >= 0)
+		if(uStrNumCmp(_version, "0.10.7") >= 0)
+		{
+			query << "SELECT image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data "
+				  << "FROM Data "
+				  << "WHERE id = ?"
+				  <<";";
+		}
+		else if(uStrNumCmp(_version, "0.10.1") >= 0)
 		{
 			query << "SELECT image, depth, calibration, scan_max_pts, scan, user_data "
 				  << "FROM Data "
@@ -653,6 +660,12 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
 					laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
 				}
 
+				float laserScanMaxRange = 0.0f;
+				if(uStrNumCmp(_version, "0.10.7") >= 0)
+				{
+					laserScanMaxRange = sqlite3_column_int(ppStmt, index++);
+				}
+
 				data = sqlite3_column_blob(ppStmt, index);
 				dataSize = sqlite3_column_bytes(ppStmt, index++);
 				//Create the laserScan
@@ -685,6 +698,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
 					(*iter)->sensorData() = SensorData(
 							scanCompressed,
 							laserScanMaxPts,
+							laserScanMaxRange,
 							imageCompressed,
 							depthOrRightCompressed,
 							models,
@@ -697,6 +711,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
 					(*iter)->sensorData() = SensorData(
 							scanCompressed,
 							laserScanMaxPts,
+							laserScanMaxRange,
 							imageCompressed,
 							depthOrRightCompressed,
 							stereoModel,
@@ -1537,7 +1552,11 @@ void DBDriverSqlite3::loadLinksQuery(
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		if(uStrNumCmp(_version, "0.8.4") >= 0)
+		if(uStrNumCmp(_version, "0.10.10") >= 0)
+		{
+			query << "SELECT to_id, type, transform, rot_variance, trans_variance, user_data FROM Link ";
+		}
+		else if(uStrNumCmp(_version, "0.8.4") >= 0)
 		{
 			query << "SELECT to_id, type, transform, rot_variance, trans_variance FROM Link ";
 		}
@@ -1603,7 +1622,20 @@ void DBDriverSqlite3::loadLinksQuery(
 			{
 				rotVariance = sqlite3_column_double(ppStmt, index++);
 				transVariance = sqlite3_column_double(ppStmt, index++);
-				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
+
+				cv::Mat userDataCompressed;
+				if(uStrNumCmp(_version, "0.10.10") >= 0)
+				{
+					const void * data = sqlite3_column_blob(ppStmt, index);
+					dataSize = sqlite3_column_bytes(ppStmt, index++);
+					//Create the userData
+					if(dataSize>4 && data)
+					{
+						userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
+					}
+				}
+
+				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance, userDataCompressed)));
 			}
 			else if(uStrNumCmp(_version, "0.7.4") >= 0)
 			{
@@ -1643,7 +1675,13 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 		std::stringstream query;
 		int totalLinksLoaded = 0;
 
-		if(uStrNumCmp(_version, "0.8.4") >= 0)
+		if(uStrNumCmp(_version, "0.10.10") >= 0)
+		{
+			query << "SELECT to_id, type, rot_variance, trans_variance, user_data, transform FROM Link "
+				  << "WHERE from_id = ? "
+				  << "ORDER BY to_id";
+		}
+		else if(uStrNumCmp(_version, "0.8.4") >= 0)
 		{
 			query << "SELECT to_id, type, rot_variance, trans_variance, transform FROM Link "
 				  << "WHERE from_id = ? "
@@ -1687,10 +1725,22 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 
 				toId = sqlite3_column_int(ppStmt, index++);
 				linkType = sqlite3_column_int(ppStmt, index++);
+				cv::Mat userDataCompressed;
 				if(uStrNumCmp(_version, "0.8.4") >= 0)
 				{
 					rotVariance = sqlite3_column_double(ppStmt, index++);
 					transVariance = sqlite3_column_double(ppStmt, index++);
+
+					if(uStrNumCmp(_version, "0.10.10") >= 0)
+					{
+						const void * data = sqlite3_column_blob(ppStmt, index);
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						//Create the userData
+						if(dataSize>4 && data)
+						{
+							userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
+						}
+					}
 				}
 				else if(uStrNumCmp(_version, "0.7.4") >= 0)
 				{
@@ -1714,11 +1764,11 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 				{
 					if(uStrNumCmp(_version, "0.7.4") >= 0)
 					{
-						links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, rotVariance, transVariance));
+						links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, rotVariance, transVariance, userDataCompressed));
 					}
 					else // neighbor is 0, loop closures are 1 and 2 (child)
 					{
-						links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance));
+						links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance, userDataCompressed));
 					}
 				}
 				else
@@ -2129,6 +2179,61 @@ void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
 	}
 }
 
+void DBDriverSqlite3::addLinkQuery(const Link & link) const
+{
+	UDEBUG("");
+	if(_ppDb)
+	{
+		std::string type;
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+
+		// Create new entries in table Link
+		std::string query = queryStepLink();
+		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		// Save link
+		stepLink(ppStmt, link);
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		UDEBUG("Time=%fs", timer.ticks());
+	}
+
+}
+
+void DBDriverSqlite3::updateLinkQuery(const Link & link) const
+{
+	UDEBUG("");
+	if(_ppDb)
+	{
+		std::string type;
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+
+		// Create new entries in table Link
+		std::string query = queryStepLinkUpdate();
+		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		// Save link
+		stepLink(ppStmt, link);
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		UDEBUG("Time=%fs", timer.ticks());
+	}
+}
+
 std::string DBDriverSqlite3::queryStepNode() const
 {
 	if(uStrNumCmp(_version, "0.10.1") >= 0)
@@ -2360,7 +2465,11 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor
 std::string DBDriverSqlite3::queryStepSensorData() const
 {
 	UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
-	if(uStrNumCmp(_version, "0.10.1") >= 0)
+	if(uStrNumCmp(_version, "0.10.7") >= 0)
+	{
+		return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data) VALUES(?,?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.10.1") >= 0)
 	{
 		return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan, user_data) VALUES(?,?,?,?,?,?,?);";
 	}
@@ -2455,6 +2564,13 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
 	rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts());
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
+	// scan_max_range
+	if(uStrNumCmp(_version, "0.10.7") >= 0)
+	{
+		rc = sqlite3_bind_double(ppStmt, index++, sensorData.laserScanMaxRange());
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	}
+
 	// scan
 	if(!sensorData.laserScanCompressed().empty())
 	{
@@ -2488,19 +2604,43 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 }
 
-std::string DBDriverSqlite3::queryStepLink() const
+std::string DBDriverSqlite3::queryStepLinkUpdate() const
 {
-	if(uStrNumCmp(_version, "0.8.4") >= 0)
+	if(uStrNumCmp(_version, "0.10.10") >= 0)
 	{
-		return "INSERT INTO Link(from_id, to_id, type, rot_variance, trans_variance, transform) VALUES(?,?,?,?,?,?);";
+		return "UPDATE Link SET type=?, rot_variance=?, trans_variance=?, transform=?, user_data=? WHERE from_id=? AND to_id = ?;";
+	}
+	else if(uStrNumCmp(_version, "0.8.4") >= 0)
+	{
+		return "UPDATE Link SET type=?, rot_variance=?, trans_variance=?, transform=? WHERE from_id=? AND to_id = ?;";
 	}
 	else if(uStrNumCmp(_version, "0.7.4") >= 0)
 	{
-		return "INSERT INTO Link(from_id, to_id, type, variance, transform) VALUES(?,?,?,?,?);";
+		return "UPDATE Link SET type=?, variance=?, transform=? WHERE from_id=? AND to_id = ?;";
 	}
 	else
 	{
-		return "INSERT INTO Link(from_id, to_id, type, transform) VALUES(?,?,?,?);";
+		return "UPDATE Link SET type=?, transform=? WHERE from_id=? AND to_id = ?;";
+	}
+}
+std::string DBDriverSqlite3::queryStepLink() const
+{
+	// from_id, to_id are at the end to match the update query above
+	if(uStrNumCmp(_version, "0.10.10") >= 0)
+	{
+		return "INSERT INTO Link(type, rot_variance, trans_variance, transform, user_data, from_id, to_id) VALUES(?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.8.4") >= 0)
+	{
+		return "INSERT INTO Link(type, rot_variance, trans_variance, transform, from_id, to_id) VALUES(?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.7.4") >= 0)
+	{
+		return "INSERT INTO Link(type, variance, transform, from_id, to_id) VALUES(?,?,?,?,?);";
+	}
+	else
+	{
+		return "INSERT INTO Link(type, transform, from_id, to_id) VALUES(?,?,?,?);";
 	}
 }
 void DBDriverSqlite3::stepLink(
@@ -2522,10 +2662,6 @@ void DBDriverSqlite3::stepLink(
 
 	int rc = SQLITE_OK;
 	int index = 1;
-	rc = sqlite3_bind_int(ppStmt, index++, link.from());
-	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-	rc = sqlite3_bind_int(ppStmt, index++, link.to());
-	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 	rc = sqlite3_bind_int(ppStmt, index++, link.type());
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
@@ -2545,6 +2681,25 @@ void DBDriverSqlite3::stepLink(
 	rc = sqlite3_bind_blob(ppStmt, index++, link.transform().data(), link.transform().size()*sizeof(float), SQLITE_STATIC);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
+	if(uStrNumCmp(_version, "0.10.10") >= 0)
+	{
+		// user_data
+		if(!link.userDataCompressed().empty())
+		{
+			rc = sqlite3_bind_blob(ppStmt, index++, link.userDataCompressed().data, (int)link.userDataCompressed().cols, SQLITE_STATIC);
+		}
+		else
+		{
+			rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
+		}
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	}
+
+	rc = sqlite3_bind_int(ppStmt, index++, link.from());
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	rc = sqlite3_bind_int(ppStmt, index++, link.to());
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
 	rc=sqlite3_step(ppStmt);
 	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 

corelib/src/DBDriverSqlite3.h

@@ -64,6 +64,9 @@ private:
 	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const;
 	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const;
 
+	virtual void addLinkQuery(const Link & link) const;
+	virtual void updateLinkQuery(const Link & link) const;
+
 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
@@ -85,6 +88,7 @@ private:
 	std::string queryStepImage() const;
 	std::string queryStepDepth() const;
 	std::string queryStepSensorData() const;
+	std::string queryStepLinkUpdate() const;
 	std::string queryStepLink() const;
 	std::string queryStepWordsChanged() const;
 	std::string queryStepKeypoint() const;

corelib/src/DBReader.cpp

@@ -45,11 +45,13 @@ namespace rtabmap {
 DBReader::DBReader(const std::string & databasePath,
 				   float frameRate,
 				   bool odometryIgnored,
-				   bool ignoreGoalDelay) :
+				   bool ignoreGoalDelay,
+				   bool goalsIgnored) :
 	_paths(uSplit(databasePath, ';')),
 	_frameRate(frameRate),
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
+	_goalsIgnored(goalsIgnored),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousStamp(0)
@@ -59,11 +61,13 @@ DBReader::DBReader(const std::string & databasePath,
 DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   float frameRate,
 				   bool odometryIgnored,
-				   bool ignoreGoalDelay) :
+				   bool ignoreGoalDelay,
+				   bool goalsIgnored) :
    _paths(databasePaths),
    _frameRate(frameRate),
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
+	_goalsIgnored(goalsIgnored),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousStamp(0)
@@ -155,8 +159,12 @@ void DBReader::mainLoop()
 	{
 		int goalId = 0;
 		double previousStamp = odom.data().stamp();
-		odom.data().setStamp(UTimer::now());
+		if(previousStamp == 0)
-		if(odom.data().userDataRaw().type() == CV_8SC1 &&
+		{
+			odom.data().setStamp(UTimer::now());
+		}
+		if(!_goalsIgnored &&
+		   odom.data().userDataRaw().type() == CV_8SC1 &&
 		   odom.data().userDataRaw().cols >= 7 && // including null str ending
 		   odom.data().userDataRaw().rows == 1 &&
 		   memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
@@ -205,19 +213,19 @@ void DBReader::mainLoop()
 					double delay = stamp - previousStamp;
 					UWARN("Goal %d detected, posting it! Waiting %f seconds before sending next data...",
 							goalId, delay);
-					this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
+					this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
 					uSleep(delay*1000);
 				}
 				else
 				{
 					UWARN("Goal %d detected, posting it!", goalId);
-					this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
+					this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
 				}
 			}
 			else
 			{
 				UWARN("Goal %d detected, posting it!", goalId);
-				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
+				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
 			}
 		}
 

corelib/src/Link.cpp

@@ -0,0 +1,198 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/Link.h"
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/core/Compression.h>
+
+namespace rtabmap {
+
+Link::Link() :
+	from_(0),
+	to_(0),
+	type_(kUndef),
+	infMatrix_(cv::Mat::eye(6,6,CV_64FC1))
+{
+}
+Link::Link(int from,
+		int to,
+		Type type,
+		const Transform & transform,
+		const cv::Mat & infMatrix,
+		const cv::Mat & userData) :
+	from_(from),
+	to_(to),
+	transform_(transform),
+	type_(type)
+{
+	setInfMatrix(infMatrix);
+
+	if(userData.type() == CV_8UC1) // Bytes
+	{
+		_userDataCompressed = userData; // assume compressed
+	}
+	else
+	{
+		_userDataRaw = userData;
+	}
+}
+Link::Link(int from,
+		int to,
+		Type type,
+		const Transform & transform,
+		double rotVariance,
+		double transVariance,
+		const cv::Mat & userData) :
+	from_(from),
+	to_(to),
+	transform_(transform),
+	type_(type)
+{
+	setVariance(rotVariance, transVariance);
+
+	if(userData.type() == CV_8UC1) // Bytes
+	{
+		_userDataCompressed = userData; // assume compressed
+	}
+	else
+	{
+		_userDataRaw = userData;
+	}
+}
+
+double Link::rotVariance() const
+{
+	double min = uMin3(infMatrix_.at<double>(3,3), infMatrix_.at<double>(4,4), infMatrix_.at<double>(5,5));
+	UASSERT(min > 0.0);
+	return 1.0/min;
+}
+double Link::transVariance() const
+{
+	double min = uMin3(infMatrix_.at<double>(0,0), infMatrix_.at<double>(1,1), infMatrix_.at<double>(2,2));
+	UASSERT(min > 0.0);
+	return 1.0/min;
+}
+
+void Link::setInfMatrix(const cv::Mat & infMatrix) {
+	UASSERT(infMatrix.cols == 6 && infMatrix.rows == 6 && infMatrix.type() == CV_64FC1);
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0, "Transitional information should not be null! (set to 1 if unknown)");
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, "Transitional information should not be null! (set to 1 if unknown)");
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, "Transitional information should not be null! (set to 1 if unknown)");
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, "Rotational information should not be null! (set to 1 if unknown)");
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, "Rotational information should not be null! (set to 1 if unknown)");
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0, "Rotational information should not be null! (set to 1 if unknown)");
+	infMatrix_ = infMatrix;
+}
+void Link::setVariance(double rotVariance, double transVariance) {
+	UASSERT(uIsFinite(rotVariance) && rotVariance>0);
+	UASSERT(uIsFinite(transVariance) && transVariance>0);
+	infMatrix_ = cv::Mat::eye(6,6,CV_64FC1);
+	infMatrix_.at<double>(0,0) = 1.0/transVariance;
+	infMatrix_.at<double>(1,1) = 1.0/transVariance;
+	infMatrix_.at<double>(2,2) = 1.0/transVariance;
+	infMatrix_.at<double>(3,3) = 1.0/rotVariance;
+	infMatrix_.at<double>(4,4) = 1.0/rotVariance;
+	infMatrix_.at<double>(5,5) = 1.0/rotVariance;
+}
+
+void Link::setUserDataRaw(const cv::Mat & userDataRaw)
+{
+	if(!_userDataRaw.empty())
+	{
+		UWARN("Writing new user data over existing user data. This may result in data loss.");
+	}
+	_userDataRaw = userDataRaw;
+}
+
+void Link::setUserData(const cv::Mat & userData)
+{
+	if(!userData.empty() && (!_userDataCompressed.empty() || !_userDataRaw.empty()))
+	{
+		UWARN("Writing new user data over existing user data. This may result in data loss.");
+	}
+	_userDataRaw = cv::Mat();
+	_userDataCompressed = cv::Mat();
+
+	if(!userData.empty())
+	{
+		if(userData.type() == CV_8UC1) // Bytes
+		{
+			_userDataCompressed = userData; // assume compressed
+		}
+		else
+		{
+			_userDataRaw = userData;
+			_userDataCompressed = compressData2(userData);
+		}
+	}
+}
+
+void Link::uncompressUserData()
+{
+	cv::Mat dataRaw = uncompressUserDataConst();
+	if(!dataRaw.empty() && _userDataRaw.empty())
+	{
+		_userDataRaw = dataRaw;
+	}
+}
+
+cv::Mat Link::uncompressUserDataConst() const
+{
+	if(!_userDataRaw.empty())
+	{
+		return _userDataRaw;
+	}
+	return uncompressData(_userDataCompressed);
+}
+
+Link Link::merge(const Link & link, Type outputType) const
+{
+	UASSERT(to_ == link.from());
+	UASSERT(outputType != Link::kUndef);
+	UASSERT((link.transform().isNull() && transform_.isNull()) || (!link.transform().isNull() && !transform_.isNull()));
+	UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1);
+	UASSERT(link.infMatrix().cols == 6 && link.infMatrix().rows == 6 && link.infMatrix().type() == CV_64FC1);
+	return Link(
+			from_,
+			link.to(),
+			outputType,
+			transform_.isNull()?Transform():transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2)
+			transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_ + link.infMatrix());
+}
+
+Link Link::inverse() const
+{
+	return Link(
+			to_,
+			from_,
+			type_,
+			transform_.isNull()?Transform():transform_.inverse(),
+			transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_);
+}
+
+}

corelib/src/Odometry.cpp

@@ -30,7 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UConversion.h"
-#include "ParticleFilter.h"
+#include "rtabmap/core/ParticleFilter.h"
 
 namespace rtabmap {
 
@@ -54,6 +54,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_estimationType(Parameters::defaultOdomEstimationType()),
 		_pnpReprojError(Parameters::defaultOdomPnPReprojError()),
 		_pnpFlags(Parameters::defaultOdomPnPFlags()),
+		_varianceFromInliersCount(Parameters::defaultOdomVarianceFromInliersCount()),
 		_resetCurrentCount(0),
 		previousStamp_(0),
 		previousTransform_(Transform::getIdentity()),
@@ -73,6 +74,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomPnPReprojError(), _pnpReprojError);
 	Parameters::parse(parameters, Parameters::kOdomPnPFlags(), _pnpFlags);
 	UASSERT(_pnpFlags>=0 && _pnpFlags <=2);
+	Parameters::parse(parameters, Parameters::kOdomVarianceFromInliersCount(), _varianceFromInliersCount);
 	Parameters::parse(parameters, Parameters::kOdomParticleFiltering(), _particleFiltering);
 	Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
 	Parameters::parse(parameters, Parameters::kOdomParticleNoiseT(), _particleNoiseT);
@@ -269,6 +271,11 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 		{
 			distanceTravelled_ += t.getNorm();
 			info->distanceTravelled = distanceTravelled_;
+
+			if(_varianceFromInliersCount)
+			{
+				info->variance = info->inliers > 0?1.0/double(info->inliers):1.0;
+			}
 		}
 
 		return _pose *= t; // updated

corelib/src/OdometryBOW.cpp

@@ -120,6 +120,7 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 		// init the local map with a all 3D features contained in the database
 		customParameters.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false"));
 		customParameters.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true"));
+		customParameters.insert(ParametersPair(Parameters::kMemSaveDepth16Format(), "false"));
 		_memory = new Memory(customParameters);
 		if(!_memory->init(_fixedLocalMapPath, false, ParametersMap()))
 		{

corelib/src/OdometryICP.cpp

@@ -72,6 +72,7 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
 	bool hasConverged = false;
 	double variance = 0;
 	unsigned int minPoints = 100;
+	int correspondences = 0;
 	if(!data.depthOrRightRaw().empty())
 	{
 		if(data.depthOrRightRaw().type() == CV_8UC1)
@@ -121,7 +122,6 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
 						hasConverged,
 						*newCloudRegistered);
 
-				int correspondences = 0;
 				util3d::computeVarianceAndCorrespondences(
 						newCloudRegistered,
 						_previousCloudNormal,
@@ -164,7 +164,6 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
 						hasConverged,
 						*newCloudRegistered);
 
-				int correspondences = 0;
 				util3d::computeVarianceAndCorrespondences(
 						newCloudRegistered,
 						_previousCloud,
@@ -202,6 +201,7 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
 	if(info)
 	{
 		info->variance = variance;
+		info->inliers = correspondences;
 	}
 
 	UINFO("Odom update time = %fs hasConverged=%s variance=%f cloud=%d",

corelib/src/OdometryMono.cpp

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UMath.h"
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/video/tracking.hpp>

corelib/src/RtabmapThread.cpp

@@ -262,10 +262,10 @@ void RtabmapThread::mainLoop()
 		{
 			UERROR("Failed to set a goal to location=%d.", id);
 		}
-		this->post(new RtabmapGlobalPathEvent(id, _rtabmap->getPath()));
+		this->post(new RtabmapGlobalPathEvent(id, parameters.at("label"), _rtabmap->getPath()));
 		break;
 	case kStateCancellingGoal:
-		_rtabmap->clearPath();
+		_rtabmap->clearPath(0);
 		break;
 	case kStateLabelling:
 		if(!_rtabmap->labelLocation(atoi(parameters.at("id").c_str()), parameters.at("label").c_str()))
@@ -463,12 +463,19 @@ void RtabmapThread::process()
 	{
 		if(_rtabmap->getMemory())
 		{
+			bool wasPlanning = _rtabmap->getPath().size()>0;
 			if(_rtabmap->process(data.data(), data.pose(), data.covariance()))
 			{
 				Statistics stats = _rtabmap->getStatistics();
 				stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
 				ULOGGER_DEBUG("posting statistics_ event...");
 				this->post(new RtabmapEvent(stats));
+
+				if(wasPlanning && _rtabmap->getPath().size() == 0)
+				{
+					// Goal reached or failed
+					this->post(new RtabmapGoalStatusEvent(_rtabmap->getPathStatus()));
+				}
 			}
 		}
 		else
@@ -510,6 +517,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		lastPose_ = odomEvent.pose();
 		double maxRotVar = odomEvent.rotVariance();
 		double maxTransVar = odomEvent.transVariance();
+		// FIXME: should merge the transformations/variances like Link::merge();
 		if(maxRotVar > _rotVariance)
 		{
 			_rotVariance = maxRotVar;

corelib/src/SensorData.cpp

@@ -38,7 +38,8 @@ namespace rtabmap
 SensorData::SensorData() :
 		_id(0),
 		_stamp(0.0),
-		_laserScanMaxPts(0)
+		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f)
 {
 }
 
@@ -50,7 +51,8 @@ SensorData::SensorData(
 		const cv::Mat & userData) :
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(0)
+		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f)
 {
 	if(image.rows == 1)
 	{
@@ -84,6 +86,7 @@ SensorData::SensorData(
 		_id(id),
 		_stamp(stamp),
 		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f),
 		_cameraModels(std::vector<CameraModel>(1, cameraModel))
 {
 	if(image.rows == 1)
@@ -119,6 +122,7 @@ SensorData::SensorData(
 		_id(id),
 		_stamp(stamp),
 		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f),
 		_cameraModels(std::vector<CameraModel>(1, cameraModel))
 {
 	if(rgb.rows == 1)
@@ -159,6 +163,7 @@ SensorData::SensorData(
 SensorData::SensorData(
 		const cv::Mat & laserScan,
 		int laserScanMaxPts,
+		float laserScanMaxRange,
 		const cv::Mat & rgb,
 		const cv::Mat & depth,
 		const CameraModel & cameraModel,
@@ -168,6 +173,7 @@ SensorData::SensorData(
 		_id(id),
 		_stamp(stamp),
 		_laserScanMaxPts(laserScanMaxPts),
+		_laserScanMaxRange(laserScanMaxRange),
 		_cameraModels(std::vector<CameraModel>(1, cameraModel))
 {
 	if(rgb.rows == 1)
@@ -224,6 +230,7 @@ SensorData::SensorData(
 		_id(id),
 		_stamp(stamp),
 		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f),
 		_cameraModels(cameraModels)
 {
 	if(rgb.rows == 1)
@@ -263,6 +270,7 @@ SensorData::SensorData(
 SensorData::SensorData(
 		const cv::Mat & laserScan,
 		int laserScanMaxPts,
+		float laserScanMaxRange,
 		const cv::Mat & rgb,
 		const cv::Mat & depth,
 		const std::vector<CameraModel> & cameraModels,
@@ -272,6 +280,7 @@ SensorData::SensorData(
 		_id(id),
 		_stamp(stamp),
 		_laserScanMaxPts(laserScanMaxPts),
+		_laserScanMaxRange(laserScanMaxRange),
 		_cameraModels(cameraModels)
 {
 	if(rgb.rows == 1)
@@ -328,6 +337,7 @@ SensorData::SensorData(
 		_id(id),
 		_stamp(stamp),
 		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f),
 		_stereoCameraModel(cameraModel)
 {
 	if(left.rows == 1)
@@ -367,6 +377,7 @@ SensorData::SensorData(
 SensorData::SensorData(
 		const cv::Mat & laserScan,
 		int laserScanMaxPts,
+		float laserScanMaxRange,
 		const cv::Mat & left,
 		const cv::Mat & right,
 		const StereoCameraModel & cameraModel,
@@ -376,6 +387,7 @@ SensorData::SensorData(
 		_id(id),
 		_stamp(stamp),
 		_laserScanMaxPts(laserScanMaxPts),
+		_laserScanMaxRange(laserScanMaxRange),
 		_stereoCameraModel(cameraModel)
 {
 	if(left.rows == 1)

corelib/src/Signature.cpp

@@ -39,6 +39,7 @@ namespace rtabmap
 Signature::Signature() :
 	_id(0), // invalid id
 	_mapId(-1),
+	_stamp(0.0),
 	_weight(0),
 	_saved(false),
 	_modified(true),

corelib/src/Transform.cpp

@@ -66,6 +66,12 @@ Transform::Transform(float x, float y, float z, float roll, float pitch, float y
 	*this = fromEigen3f(t);
 }
 
+Transform::Transform(float x, float y, float theta)
+{
+	Eigen::Affine3f t = pcl::getTransformation (x, y, 0, 0, 0, theta);
+	*this = fromEigen3f(t);
+}
+
 bool Transform::isNull() const
 {
 	return (data()[0] == 0.0f &&

corelib/src/VWDictionary.cpp

@@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "rtabmap/core/VWDictionary.h"
-#include "VisualWord.h"
+#include "rtabmap/core/VisualWord.h"
 
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/DBDriver.h"
@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UtiLite.h"
 
 #include <opencv2/opencv_modules.hpp>
+
 #if CV_MAJOR_VERSION < 3
 #include <opencv2/gpu/gpu.hpp>
 #else
@@ -44,23 +45,254 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #endif
 #endif
 
+#include "rtflann/flann.hpp"
+
 #include <fstream>
 #include <string>
 
 namespace rtabmap
 {
 
+class FlannIndex
+{
+public:
+	FlannIndex():
+		index_(0),
+		nextIndex_(0),
+		featuresType_(0),
+		featuresDim_(0),
+		isLSH_(false)
+	{
+	}
+	virtual ~FlannIndex()
+	{
+		this->release();
+	}
+
+	void release()
+	{
+		if(index_)
+		{
+			if(featuresType_ == CV_8UC1)
+			{
+				delete (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
+			}
+			else
+			{
+				delete (rtflann::Index<rtflann::L2<float> >*)index_;
+			}
+			index_ = 0;
+		}
+		nextIndex_ = 0;
+		isLSH_ = false;
+		addedDescriptors_.clear();
+		removedIndexes_.clear();
+	}
+
+	unsigned int indexedFeatures() const
+	{
+		if(!index_)
+		{
+			return 0;
+		}
+		if(featuresType_ == CV_8UC1)
+		{
+			return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->size();
+		}
+		else
+		{
+			return ((const rtflann::Index<rtflann::L2<float> >*)index_)->size();
+		}
+	}
+
+	// return KB
+	unsigned int memoryUsed() const
+	{
+		if(!index_)
+		{
+			return 0;
+		}
+		if(featuresType_ == CV_8UC1)
+		{	
+			return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->usedMemory()/1000;
+		}
+		else
+		{
+			return ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory()/1000;
+		}
+	}
+
+	void build(
+			const cv::Mat & features,
+			const rtflann::IndexParams& params)
+	{
+		this->release();
+		UASSERT(index_ == 0);
+		UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
+		featuresType_ = features.type();
+		featuresDim_ = features.cols;
+
+		if(featuresType_ == CV_8UC1)
+		{
+			rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
+			index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
+			((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
+		}
+		else
+		{
+			rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
+			index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
+			((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
+		}
+
+		if(features.rows == 1)
+		{
+			// incremental FLANN
+			addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+		}
+		// else assume that the features are kept in memory outside this class (e.g., dataTree_)
+
+		nextIndex_ = features.rows;
+	}
+
+	bool isBuilt()
+	{
+		return index_!=0;
+	}
+
+	int featuresType() const {return featuresType_;}
+	int featuresDim() const {return featuresDim_;}
+
+	unsigned int addPoint(const cv::Mat & feature)
+	{
+		if(!index_)
+		{
+			UERROR("Flann index not yet created!");
+			return 0;
+		}
+		UASSERT(feature.type() == featuresType_);
+		UASSERT(feature.cols == featuresDim_);
+		UASSERT(feature.rows == 1);
+		if(featuresType_ == CV_8UC1)
+		{
+			rtflann::Matrix<unsigned char> point(feature.data, feature.rows, feature.cols);
+			rtflann::Index<rtflann::Hamming<unsigned char> > * index = (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
+			index->addPoints(point, 0);
+			// Rebuild index if it doubles in size
+			if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
+			{
+				// clean not used features
+				for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
+				{
+					addedDescriptors_.erase(*iter);
+				}
+				removedIndexes_.clear();
+				index->buildIndex();
+			}
+		}
+		else
+		{
+			rtflann::Matrix<float> point((float*)feature.data, feature.rows, feature.cols);
+			rtflann::Index<rtflann::L2<float> > * index = (rtflann::Index<rtflann::L2<float> >*)index_;
+			index->addPoints(point, 0);
+			// Rebuild index if it doubles in size
+			if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
+			{
+				// clean not used features
+				for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
+				{
+					addedDescriptors_.erase(*iter);
+				}
+				removedIndexes_.clear();
+				index->buildIndex();
+			}
+		}
+
+		addedDescriptors_.insert(std::make_pair(nextIndex_, feature));
+
+		return nextIndex_++;
+	}
+
+	void removePoint(unsigned int index)
+	{
+		if(!index_)
+		{
+			UERROR("Flann index not yet created!");
+			return;
+		}
+
+		// If a Segmentation fault occurs in removePoint(), verify that you have this fix in your installed "flann/algorithms/nn_index.h":
+		// 707 - if (ids_[id]==id) {
+		// 707 + if (id < ids_.size() && ids_[id]==id) {
+		// ref: https://github.com/mariusmuja/flann/commit/23051820b2314f07cf40ba633a4067782a982ff3#diff-33762b7383f957c2df17301639af5151
+
+		if(featuresType_ == CV_8UC1)
+		{
+			((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->removePoint(index);
+		}
+		else
+		{
+			((rtflann::Index<rtflann::L2<float> >*)index_)->removePoint(index);
+		}
+		removedIndexes_.push_back(index);
+	}
+
+	void knnSearch(
+			const cv::Mat & query,
+			cv::Mat & indices,
+			cv::Mat & dists,
+	        int knn,
+	        const rtflann::SearchParams& params=rtflann::SearchParams())
+	{
+		if(!index_)
+		{
+			UERROR("Flann index not yet created!");
+			return;
+		}
+		indices.create(query.rows, knn, CV_32S);
+		dists.create(query.rows, knn, featuresType_ == CV_8UC1?CV_32S:CV_32F);
+
+		rtflann::Matrix<int> indicesF((int*)indices.data, indices.rows, indices.cols);
+
+		if(featuresType_ == CV_8UC1)
+		{
+			rtflann::Matrix<unsigned int> distsF((unsigned int*)dists.data, dists.rows, dists.cols);
+			rtflann::Matrix<unsigned char> queryF(query.data, query.rows, query.cols);
+			((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
+		}
+		else
+		{
+			rtflann::Matrix<float> distsF((float*)dists.data, dists.rows, dists.cols);
+			rtflann::Matrix<float> queryF((float*)query.data, query.rows, query.cols);
+			((rtflann::Index<rtflann::L2<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
+		}
+	}
+
+private:
+	void * index_;
+	unsigned int nextIndex_;
+	int featuresType_;
+	int featuresDim_;
+	bool isLSH_;
+
+	// keep feature in memory until the tree is rebuilt
+	// (in case the word is deleted when removed from the VWDictionary)
+	std::map<int, cv::Mat> addedDescriptors_;
+	std::list<int> removedIndexes_;
+};
+
 const int VWDictionary::ID_START = 1;
 const int VWDictionary::ID_INVALID = 0;
 
 VWDictionary::VWDictionary(const ParametersMap & parameters) :
 	_totalActiveReferences(0),
 	_incrementalDictionary(Parameters::defaultKpIncrementalDictionary()),
+	_incrementalFlann(Parameters::defaultKpIncrementalFlann()),
 	_nndrRatio(Parameters::defaultKpNndrRatio()),
 	_dictionaryPath(Parameters::defaultKpDictionaryPath()),
 	_newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()),
 	_lastWordId(0),
-	_flannIndex(new cv::flann::Index()),
+	_flannIndex(new FlannIndex()),
 	_strategy(kNNBruteForce)
 {
 	this->setNNStrategy((NNStrategy)Parameters::defaultKpNNStrategy());
@@ -78,6 +310,7 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 	ParametersMap::const_iterator iter;
 	Parameters::parse(parameters, Parameters::kKpNndrRatio(), _nndrRatio);
 	Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether);
+	Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann);
 
 	UASSERT_MSG(_nndrRatio > 0.0f, uFormat("String=%s value=%f", uContains(parameters, Parameters::kKpNndrRatio())?parameters.at(Parameters::kKpNndrRatio()).c_str():"", _nndrRatio).c_str());
 
@@ -257,7 +490,15 @@ void VWDictionary::setNNStrategy(NNStrategy strategy)
 		}
 		else
 		{
+			bool update = _strategy != strategy;
 			_strategy = strategy;
+			if(update)
+			{
+				_dataTree = cv::Mat();
+				_notIndexedWords = uKeysSet(_visualWords);
+				_removedIndexedWords.clear();
+				this->update();
+			}
 		}
 	}
 }
@@ -274,6 +515,16 @@ int VWDictionary::getLastIndexedWordId() const
 	}
 }
 
+unsigned int VWDictionary::getIndexedWordsCount() const
+{
+	return _flannIndex->indexedFeatures();
+}
+
+unsigned int VWDictionary::getIndexMemoryUsed() const
+{
+	return _flannIndex->memoryUsed();
+}
+
 void VWDictionary::update()
 {
 	ULOGGER_DEBUG("");
@@ -287,58 +538,144 @@ void VWDictionary::update()
 
 	if(_notIndexedWords.size() || _visualWords.size() == 0 || _removedIndexedWords.size())
 	{
-		_mapIndexId.clear();
+		if(_incrementalFlann &&
-		int oldSize = _dataTree.rows;
+		   _strategy < kNNBruteForce &&
-		_dataTree = cv::Mat();
+		   _visualWords.size())
-		_flannIndex->release();
-
-		if(_visualWords.size())
 		{
-			UTimer timer;
+			ULOGGER_DEBUG("Incremental FLANN: Removing %d words...", (int)_removedIndexedWords.size());
-			timer.start();
+			for(std::set<int>::iterator iter=_removedIndexedWords.begin(); iter!=_removedIndexedWords.end(); ++iter)
-
-			int type = _visualWords.begin()->second->getDescriptor().type();
-			int dim = _visualWords.begin()->second->getDescriptor().cols;
-
-			UASSERT(type == CV_32F || type == CV_8U);
-			UASSERT(dim > 0);
-
-			// Create the data matrix
-			_dataTree = cv::Mat(_visualWords.size(), dim, type); // SURF descriptors are CV_32F
-			std::map<int, VisualWord*>::const_iterator iter = _visualWords.begin();
-			for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter)
 			{
-				UASSERT(iter->second->getDescriptor().cols == dim);
+				UASSERT(uContains(_mapIdIndex, *iter));
-				UASSERT(iter->second->getDescriptor().type() == type);
+				UASSERT(uContains(_mapIndexId, _mapIdIndex.at(*iter)));
-
+				_flannIndex->removePoint(_mapIdIndex.at(*iter));
-				iter->second->getDescriptor().copyTo(_dataTree.row(i));
+				_mapIndexId.erase(_mapIdIndex.at(*iter));
-				_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, iter->second->id()));
+				_mapIdIndex.erase(*iter);
 			}
+			ULOGGER_DEBUG("Incremental FLANN: Removing %d words... done!", (int)_removedIndexedWords.size());
 
-			ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), dim);
+			if(_notIndexedWords.size())
-			ULOGGER_DEBUG("copying data = %f s", timer.ticks());
-
-			switch(_strategy)
 			{
-			case kNNFlannNaive:
+				ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size());
-				_flannIndex->build(_dataTree, cv::flann::LinearIndexParams(), type == CV_32F?cvflann::FLANN_DIST_L2:cvflann::FLANN_DIST_HAMMING);
+				for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
-				break;
+				{
-			case kNNFlannKdTree:
+					VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
-				UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
+					UASSERT(w);
-				_flannIndex->build(_dataTree, cv::flann::KDTreeIndexParams(), cvflann::FLANN_DIST_L2);
+					int index = 0;
-				break;
+					if(!_flannIndex->isBuilt())
-			case kNNFlannLSH:
+					{
-				UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
+						UDEBUG("Building FLANN index...");
-				_flannIndex->build(_dataTree, cv::flann::LshIndexParams(12, 20, 2), cvflann::FLANN_DIST_HAMMING);
+						switch(_strategy)
-				break;
+						{
-			default:
+						case kNNFlannNaive:
-				break;
+							_flannIndex->build(w->getDescriptor(), rtflann::LinearIndexParams());
+							break;
+						case kNNFlannKdTree:
+							UASSERT_MSG(w->getDescriptor().type() == CV_32F, "To use KdTree dictionary, float descriptors are required!");
+							_flannIndex->build(w->getDescriptor(), rtflann::KDTreeIndexParams());
+							break;
+						case kNNFlannLSH:
+							UASSERT_MSG(w->getDescriptor().type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
+							_flannIndex->build(w->getDescriptor(), rtflann::LshIndexParams(12, 20, 2));
+							break;
+						default:
+							UFATAL("Not supposed to be here!");
+							break;
+						}
+						UDEBUG("Building FLANN index... done!");
+					}
+					else
+					{
+						UASSERT(w->getDescriptor().cols == _flannIndex->featuresDim());
+						UASSERT(w->getDescriptor().type() == _flannIndex->featuresType());
+						index = _flannIndex->addPoint(w->getDescriptor());
+					}
+					std::pair<std::map<int, int>::iterator, bool> inserted;
+					inserted = _mapIndexId.insert(std::pair<int, int>(index, w->id()));
+					UASSERT(inserted.second);
+					inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), index));
+					UASSERT(inserted.second);
+				}
+				ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done!", (int)_notIndexedWords.size());
 			}
-
-			ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks());
 		}
-		UDEBUG("Dictionary updated! (size=%d->%d added=%d removed=%d)",
+		else if(_strategy >= kNNBruteForce &&
-				oldSize, _dataTree.rows, _notIndexedWords.size(), _removedIndexedWords.size());
+				_notIndexedWords.size() &&
+				_removedIndexedWords.size() == 0 &&
+				_visualWords.size() &&
+				_dataTree.rows)
+		{
+			//just add not indexed words
+			int i = _dataTree.rows;
+			_dataTree.reserve(_dataTree.rows + _notIndexedWords.size());
+			for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
+			{
+				VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
+				UASSERT(w);
+				UASSERT(w->getDescriptor().cols == _dataTree.cols);
+				UASSERT(w->getDescriptor().type() == _dataTree.type());
+				_dataTree.push_back(w->getDescriptor());
+				_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, w->id()));
+				std::pair<std::map<int, int>::iterator, bool> inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), i));
+				UASSERT(inserted.second);
+				++i;
+			}
+		}
+		else
+		{
+			_mapIndexId.clear();
+			_mapIdIndex.clear();
+			_dataTree = cv::Mat();
+			_flannIndex->release();
+
+			if(_visualWords.size())
+			{
+				UTimer timer;
+				timer.start();
+
+				int type = _visualWords.begin()->second->getDescriptor().type();
+				int dim = _visualWords.begin()->second->getDescriptor().cols;
+
+				UASSERT(type == CV_32F || type == CV_8U);
+				UASSERT(dim > 0);
+
+				// Create the data matrix
+				_dataTree = cv::Mat(_visualWords.size(), dim, type); // SURF descriptors are CV_32F
+				std::map<int, VisualWord*>::const_iterator iter = _visualWords.begin();
+				for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter)
+				{
+					UASSERT(iter->second->getDescriptor().cols == dim);
+					UASSERT(iter->second->getDescriptor().type() == type);
+
+					iter->second->getDescriptor().copyTo(_dataTree.row(i));
+					_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, iter->second->id()));
+					_mapIdIndex.insert(_mapIdIndex.end(), std::pair<int, int>(iter->second->id(), i));
+				}
+
+				ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), dim);
+				ULOGGER_DEBUG("copying data = %f s", timer.ticks());
+
+				switch(_strategy)
+				{
+				case kNNFlannNaive:
+					_flannIndex->build(_dataTree, rtflann::LinearIndexParams());
+					break;
+				case kNNFlannKdTree:
+					UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
+					_flannIndex->build(_dataTree, rtflann::KDTreeIndexParams());
+					break;
+				case kNNFlannLSH:
+					UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
+					_flannIndex->build(_dataTree, rtflann::LshIndexParams(12, 20, 2));
+					break;
+				default:
+					break;
+				}
+
+				ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks());
+			}
+		}
+		UDEBUG("Dictionary updated! (size=%d added=%d removed=%d)",
+				_dataTree.rows, _notIndexedWords.size(), _removedIndexedWords.size());
 	}
 	else
 	{
@@ -346,6 +683,7 @@ void VWDictionary::update()
 	}
 	_notIndexedWords.clear();
 	_removedIndexedWords.clear();
+	UDEBUG("");
 }
 
 void VWDictionary::clear()
@@ -370,6 +708,7 @@ void VWDictionary::clear()
 	_lastWordId = 0;
 	_dataTree = cv::Mat();
 	_mapIndexId.clear();
+	_mapIdIndex.clear();
 	_unusedWords.clear();
 	_flannIndex->release();
 }
@@ -471,7 +810,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 	UTimer timerLocal;
 	timerLocal.start();
 
-	if(!_dataTree.empty() && _dataTree.rows >= (int)k)
+	if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k))
 	{
 		//Find nearest neighbors
 		UDEBUG("newPts.total()=%d ", descriptors.rows);
@@ -544,10 +883,15 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 		{
 			for(int j=0; j<dists.cols; ++j)
 			{
-				if(results.at<int>(i,j) >= 0)
+				float d = dists.at<float>(i,j);
+				int id = uValue(_mapIndexId, results.at<int>(i,j));
+				if(d >= 0.0f && id > 0)
 				{
-					float d = dists.at<float>(i,j);
+					fullResults.insert(std::pair<float, int>(d, id));
-					fullResults.insert(std::pair<float, int>(d, uValue(_mapIndexId, results.at<int>(i,j))));
+				}
+				else
+				{
+					break;
 				}
 			}
 		}
@@ -555,10 +899,15 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 		{
 			for(unsigned int j=0; j<matches.at(i).size(); ++j)
 			{
-				if(matches.at(i).at(j).trainIdx >= 0)
+				float d = matches.at(i).at(j).distance;
+				int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx);
+				if(d >= 0.0f && id > 0)
 				{
-					float d = matches.at(i).at(j).distance;
+					fullResults.insert(std::pair<float, int>(d, id));
-					fullResults.insert(std::pair<float, int>(d, uValue(_mapIndexId, matches.at(i).at(j).trainIdx)));
+				}
+				else
+				{
+					break;
 				}
 			}
 		}
@@ -566,27 +915,21 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 		// Check if this descriptor matches with a word from the last signature (a word not already added to the tree)
 		if(_newWordsComparedTogether && newWords.rows)
 		{
-			cv::flann::Index linearSeach;
+			std::vector<std::vector<cv::DMatch> > matchesNewWords;
-			linearSeach.build(newWords, cv::flann::LinearIndexParams(), type == CV_32F?cvflann::FLANN_DIST_L2:cvflann::FLANN_DIST_HAMMING);
+			cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
-			cv::Mat resultsLinear;
+			matcher.knnMatch(descriptors.row(i), newWords, matchesNewWords, newWords.rows>1?2:1);
-			cv::Mat distsLinear;
+			UASSERT(matchesNewWords.size() == 1);
-			linearSeach.knnSearch(descriptors.row(i), resultsLinear, distsLinear, newWords.rows>1?2:1);
+			for(unsigned int j=0; j<matchesNewWords.at(0).size(); ++j)
-			// In case of binary descriptors
-			if(distsLinear.type() == CV_32S)
 			{
-				cv::Mat temp;
+				float d = matchesNewWords.at(0).at(j).distance;
-				distsLinear.convertTo(temp, CV_32F);
+				int id = newWordsId[matchesNewWords.at(0).at(j).trainIdx];
-				distsLinear = temp;
+				if(d >= 0.0f && id > 0)
-			}
-			if(resultsLinear.cols)
-			{
-				for(int j=0; j<resultsLinear.cols; ++j)
 				{
-					if(resultsLinear.at<int>(0,j) >= 0)
+					fullResults.insert(std::pair<float, int>(d, id));
-					{
+				}
-						 float d = distsLinear.at<float>(0,j);
+				else
-						 fullResults.insert(std::pair<float, int>(d, newWordsId[resultsLinear.at<int>(0,j)]));
+				{
-					}
+					break;
 				}
 			}
 		}
@@ -637,7 +980,6 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 
 				this->addWordRef(fullResults.begin()->second, signatureId);
 				wordIds.push_back(fullResults.begin()->second);
-				UASSERT(fullResults.begin()->second>0);
 			}
 		}
 		else if(fullResults.size())
@@ -704,7 +1046,7 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 		}
 		ULOGGER_DEBUG("Preparation time = %fs", timer.ticks());
 
-		if(!_dataTree.empty() && _dataTree.rows >= (int)k)
+		if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k))
 		{
 			//Find nearest neighbors
 			UDEBUG("newPts.total()=%d ", query.total());
@@ -768,9 +1110,8 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 		}
 		ULOGGER_DEBUG("Search dictionary time = %fs", timer.ticks());
 
-		cv::Mat resultsNotIndexed;
-		cv::Mat distsNotIndexed;
 		std::map<int, int> mapIndexIdNotIndexed;
+		std::vector<std::vector<cv::DMatch> > matchesNotIndexed;
 		if(_notIndexedWords.size())
 		{
 			cv::Mat dataNotIndexed = cv::Mat::zeros(_notIndexedWords.size(), dim, type);
@@ -786,16 +1127,8 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 
 			// Find nearest neighbor
 			ULOGGER_DEBUG("Searching in words not indexed...");
-			cv::flann::Index linearSeach;
+			cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
-			linearSeach.build(dataNotIndexed, cv::flann::LinearIndexParams(), type == CV_32F?cvflann::FLANN_DIST_L2:cvflann::FLANN_DIST_HAMMING);
+			matcher.knnMatch(query, dataNotIndexed, matchesNotIndexed, dataNotIndexed.rows>1?2:1);
-			linearSeach.knnSearch(query, resultsNotIndexed, distsNotIndexed, _notIndexedWords.size()>1?2:1);
-			// In case of binary descriptors
-			if(distsNotIndexed.type() == CV_32S)
-			{
-				cv::Mat temp;
-				distsNotIndexed.convertTo(temp, CV_32F);
-				distsNotIndexed = temp;
-			}
 		}
 		ULOGGER_DEBUG("Search not yet indexed words time = %fs", timer.ticks());
 
@@ -806,10 +1139,11 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 			{
 				for(int j=0; j<dists.cols; ++j)
 				{
-					if(results.at<int>(i,j) > 0)
+					float d = dists.at<float>(i,j);
+					int id = uValue(_mapIndexId, results.at<int>(i,j));
+					if(d >= 0.0f && id > 0)
 					{
-						float d = dists.at<float>(i,j);
+						fullResults.insert(std::pair<float, int>(d, id));
-						fullResults.insert(std::pair<float, int>(d, uValue(_mapIndexId, results.at<int>(i,j))));
 					}
 				}
 			}
@@ -817,21 +1151,30 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 			{
 				for(unsigned int j=0; j<matches.at(i).size(); ++j)
 				{
-					if(matches.at(i).at(j).trainIdx > 0)
+					float d = matches.at(i).at(j).distance;
+					int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx);
+					if(d >= 0.0f && id > 0)
 					{
-						float d = matches.at(i).at(j).distance;
+						fullResults.insert(std::pair<float, int>(d, id));
-						fullResults.insert(std::pair<float, int>(d, uValue(_mapIndexId, matches.at(i).at(j).trainIdx)));
 					}
 				}
 			}
 
 			// not indexed..
-			for(int j=0; j<distsNotIndexed.cols; ++j)
+			if(matchesNotIndexed.size())
 			{
-				if(resultsNotIndexed.at<int>(i,j) > 0)
+				for(unsigned int j=0; j<matchesNotIndexed.at(i).size(); ++j)
 				{
-					float d = distsNotIndexed.at<float>(i,j);
+					float d = matchesNotIndexed.at(i).at(j).distance;
-					fullResults.insert(std::pair<float, int>(d, uValue(mapIndexIdNotIndexed, resultsNotIndexed.at<int>(i,j))));
+					int id = uValue(mapIndexIdNotIndexed, matchesNotIndexed.at(i).at(j).trainIdx);
+					if(d >= 0.0f && id > 0)
+					{
+						fullResults.insert(std::pair<float, int>(d, id));
+					}
+					else
+					{
+						break;
+					}
 				}
 			}
 

corelib/src/VisualWord.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "VisualWord.h"
+#include "rtabmap/core/VisualWord.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UStl.h"
 

corelib/src/resources/DatabaseSchema.sql.in

@@ -31,6 +31,7 @@ CREATE TABLE Data (
 	calibration BLOB,         -- fx, fy, cx, cy [,baseline] local_transform
 	scan BLOB,                -- compressed data (Laser scan)
 	scan_max_pts INTEGER,     -- Laser scan max points
+	scan_max_range FLOAT,     -- Laser max range
 	user_data BLOB,           -- compressed data (User data)
 	time_enter DATE,
 	PRIMARY KEY (id)
@@ -39,10 +40,11 @@ CREATE TABLE Data (
 CREATE TABLE Link (
 	from_id INTEGER NOT NULL,
 	to_id INTEGER NOT NULL,
-	type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
+	type INTEGER NOT NULL,    -- neighbor=0, loop=1, child=2
 	rot_variance FLOAT NOT NULL,
 	trans_variance FLOAT NOT NULL,
 	transform BLOB,
+	user_data BLOB,           -- compressed data (User data)
 	FOREIGN KEY (from_id) REFERENCES Node(id),
 	FOREIGN KEY (to_id) REFERENCES Node(id)
 );
@@ -81,7 +83,7 @@ CREATE TABLE Statistics (
 );
 
 CREATE TABLE Admin (
-	version INTEGER,
+	version TEXT,
 	time_enter DATE
 );
 

corelib/src/rtflann/algorithms/all_indices.h

@@ -0,0 +1,197 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_ALL_INDICES_H_
+#define RTABMAP_FLANN_ALL_INDICES_H_
+
+#include "rtflann/general.h"
+
+#include "rtflann/algorithms/nn_index.h"
+#include "rtflann/algorithms/kdtree_index.h"
+#include "rtflann/algorithms/kdtree_single_index.h"
+#include "rtflann/algorithms/kmeans_index.h"
+#include "rtflann/algorithms/composite_index.h"
+#include "rtflann/algorithms/linear_index.h"
+#include "rtflann/algorithms/hierarchical_clustering_index.h"
+#include "rtflann/algorithms/lsh_index.h"
+#include "rtflann/algorithms/autotuned_index.h"
+#ifdef FLANN_USE_CUDA
+#include "rtflann/algorithms/kdtree_cuda_3d_index.h"
+#endif
+
+
+namespace rtflann
+{
+
+/**
+ * enable_if sfinae helper
+ */
+template<bool, typename T = void> struct enable_if{};
+template<typename T> struct enable_if<true,T> { typedef T type; };
+
+/**
+ * disable_if sfinae helper
+ */
+template<bool, typename T> struct disable_if{ typedef T type; };
+template<typename T> struct disable_if<true,T> { };
+
+/**
+ * Check if two type are the same
+ */
+template <typename T, typename U>
+struct same_type
+{
+    enum {value = false};
+};
+
+template<typename T>
+struct same_type<T,T>
+{
+    enum {value = true};
+};
+
+#define HAS_MEMBER(member) \
+    template<typename T> \
+    struct member { \
+        typedef char No; \
+        typedef long Yes; \
+        template<typename C> static Yes test( typename C::member* ); \
+        template<typename C> static No test( ... ); \
+        enum { value = sizeof (test<T>(0))==sizeof(Yes) }; \
+    };
+
+HAS_MEMBER(needs_kdtree_distance)
+HAS_MEMBER(needs_vector_space_distance)
+HAS_MEMBER(is_kdtree_distance)
+HAS_MEMBER(is_vector_space_distance)
+
+struct DummyDistance
+{
+    typedef float ElementType;
+    typedef float ResultType;
+
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
+    {
+        return ResultType(0);
+    }
+
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        return ResultType(0);
+    }
+};
+
+/**
+ * Checks if an index and a distance can be used together
+ */
+template<template <typename> class Index, typename Distance, typename ElemType>
+struct valid_combination
+{
+    static const bool value = same_type<ElemType,typename Distance::ElementType>::value &&
+    				(!needs_kdtree_distance<Index<DummyDistance> >::value || is_kdtree_distance<Distance>::value) &&
+    				(!needs_vector_space_distance<Index<DummyDistance> >::value || is_kdtree_distance<Distance>::value || is_vector_space_distance<Distance>::value);
+
+};
+
+
+/*********************************************************
+ * Create index
+ **********************************************************/
+template <template<typename> class Index, typename Distance, typename T>
+inline NNIndex<Distance>* create_index_(rtflann::Matrix<T> data, const rtflann::IndexParams& params, const Distance& distance,
+		typename enable_if<valid_combination<Index,Distance,T>::value,void>::type* = 0)
+{
+    return new Index<Distance>(data, params, distance);
+}
+
+template <template<typename> class Index, typename Distance, typename T>
+inline NNIndex<Distance>* create_index_(rtflann::Matrix<T> data, const rtflann::IndexParams& params, const Distance& distance,
+		typename disable_if<valid_combination<Index,Distance,T>::value,void>::type* = 0)
+{
+    return NULL;
+}
+
+template<typename Distance>
+inline NNIndex<Distance>*
+  create_index_by_type(const flann_algorithm_t index_type,
+		const Matrix<typename Distance::ElementType>& dataset, const IndexParams& params, const Distance& distance)
+{
+	typedef typename Distance::ElementType ElementType;
+
+	NNIndex<Distance>* nnIndex;
+
+	switch (index_type) {
+
+	case FLANN_INDEX_LINEAR:
+		nnIndex = create_index_<LinearIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+	case FLANN_INDEX_KDTREE_SINGLE:
+		nnIndex = create_index_<KDTreeSingleIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+	case FLANN_INDEX_KDTREE:
+		nnIndex = create_index_<KDTreeIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+		//! #define this symbol before including flann.h to enable GPU search algorithms. But you have
+		//! to link libflann_cuda then!
+#ifdef FLANN_USE_CUDA
+	case FLANN_INDEX_KDTREE_CUDA:
+		nnIndex = create_index_<KDTreeCuda3dIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+#endif
+
+	case FLANN_INDEX_KMEANS:
+		nnIndex = create_index_<KMeansIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+	case FLANN_INDEX_COMPOSITE:
+		nnIndex = create_index_<CompositeIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+	case FLANN_INDEX_AUTOTUNED:
+		nnIndex = create_index_<AutotunedIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+	case FLANN_INDEX_HIERARCHICAL:
+		nnIndex = create_index_<HierarchicalClusteringIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+	case FLANN_INDEX_LSH:
+		nnIndex = create_index_<LshIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+	default:
+		throw FLANNException("Unknown index type");
+	}
+
+    if (nnIndex==NULL) {
+    	throw FLANNException("Unsupported index/distance combination");
+    }
+    return nnIndex;
+}
+
+}
+
+#endif /* RTABMAP_FLANN_ALL_INDICES_H_ */

corelib/src/rtflann/algorithms/autotuned_index.h

@@ -0,0 +1,763 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_AUTOTUNED_INDEX_H_
+#define RTABMAP_FLANN_AUTOTUNED_INDEX_H_
+
+#include "rtflann/general.h"
+#include "rtflann/algorithms/nn_index.h"
+#include "rtflann/nn/ground_truth.h"
+#include "rtflann/nn/index_testing.h"
+#include "rtflann/util/sampling.h"
+#include "rtflann/algorithms/kdtree_index.h"
+#include "rtflann/algorithms/kdtree_single_index.h"
+#include "rtflann/algorithms/kmeans_index.h"
+#include "rtflann/algorithms/composite_index.h"
+#include "rtflann/algorithms/linear_index.h"
+#include "rtflann/util/logger.h"
+
+
+namespace rtflann
+{
+
+template<typename Distance>
+inline NNIndex<Distance>*
+  create_index_by_type(const flann_algorithm_t index_type,
+        const Matrix<typename Distance::ElementType>& dataset, const IndexParams& params, const Distance& distance = Distance());
+
+
+struct AutotunedIndexParams : public IndexParams
+{
+    AutotunedIndexParams(float target_precision = 0.8, float build_weight = 0.01, float memory_weight = 0, float sample_fraction = 0.1)
+    {
+        (*this)["algorithm"] = FLANN_INDEX_AUTOTUNED;
+        // precision desired (used for autotuning, -1 otherwise)
+        (*this)["target_precision"] = target_precision;
+        // build tree time weighting factor
+        (*this)["build_weight"] = build_weight;
+        // index memory weighting factor
+        (*this)["memory_weight"] = memory_weight;
+        // what fraction of the dataset to use for autotuning
+        (*this)["sample_fraction"] = sample_fraction;
+    }
+};
+
+
+template <typename Distance>
+class AutotunedIndex : public NNIndex<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+    
+    typedef NNIndex<Distance> BaseClass;
+
+    typedef AutotunedIndex<Distance> IndexType;
+
+    typedef bool needs_kdtree_distance;
+
+    AutotunedIndex(const Matrix<ElementType>& inputData, const IndexParams& params = AutotunedIndexParams(), Distance d = Distance()) :
+        BaseClass(params, d), bestIndex_(NULL), speedup_(0), dataset_(inputData)
+    {
+        target_precision_ = get_param(params, "target_precision",0.8f);
+        build_weight_ =  get_param(params,"build_weight", 0.01f);
+        memory_weight_ = get_param(params, "memory_weight", 0.0f);
+        sample_fraction_ = get_param(params,"sample_fraction", 0.1f);
+    }
+
+    AutotunedIndex(const IndexParams& params = AutotunedIndexParams(), Distance d = Distance()) :
+        BaseClass(params, d), bestIndex_(NULL), speedup_(0)
+    {
+        target_precision_ = get_param(params, "target_precision",0.8f);
+        build_weight_ =  get_param(params,"build_weight", 0.01f);
+        memory_weight_ = get_param(params, "memory_weight", 0.0f);
+        sample_fraction_ = get_param(params,"sample_fraction", 0.1f);
+    }
+
+    AutotunedIndex(const AutotunedIndex& other) : BaseClass(other),
+    		bestParams_(other.bestParams_),
+    		bestSearchParams_(other.bestSearchParams_),
+    		speedup_(other.speedup_),
+    		dataset_(other.dataset_),
+    		target_precision_(other.target_precision_),
+    		build_weight_(other.build_weight_),
+    		memory_weight_(other.memory_weight_),
+    		sample_fraction_(other.sample_fraction_)
+    {
+    		bestIndex_ = other.bestIndex_->clone();
+    }
+
+    AutotunedIndex& operator=(AutotunedIndex other)
+    {
+    	this->swap(other);
+    	return * this;
+    }
+
+    virtual ~AutotunedIndex()
+    {
+    	delete bestIndex_;
+    }
+
+    BaseClass* clone() const
+    {
+    	return new AutotunedIndex(*this);
+    }
+
+    /**
+     *          Method responsible with building the index.
+     */
+    void buildIndex()
+    {
+        bestParams_ = estimateBuildParams();
+        Logger::info("----------------------------------------------------\n");
+        Logger::info("Autotuned parameters:\n");
+        if (Logger::getLevel()>=FLANN_LOG_INFO)
+        	print_params(bestParams_);
+        Logger::info("----------------------------------------------------\n");
+
+        flann_algorithm_t index_type = get_param<flann_algorithm_t>(bestParams_,"algorithm");
+        bestIndex_ = create_index_by_type(index_type, dataset_, bestParams_, distance_);
+        bestIndex_->buildIndex();
+        speedup_ = estimateSearchParams(bestSearchParams_);
+        Logger::info("----------------------------------------------------\n");
+        Logger::info("Search parameters:\n");
+        if (Logger::getLevel()>=FLANN_LOG_INFO)
+        	print_params(bestSearchParams_);
+        Logger::info("----------------------------------------------------\n");
+        bestParams_["search_params"] = bestSearchParams_;
+        bestParams_["speedup"] = speedup_;
+    }
+    
+    void buildIndex(const Matrix<ElementType>& dataset)
+    {
+    	dataset_ = dataset;
+    	this->buildIndex();
+    }
+
+
+    void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        if (bestIndex_) {
+            bestIndex_->addPoints(points, rebuild_threshold);
+        }
+    }
+    
+    void removePoint(size_t id)
+    {
+        if (bestIndex_) {
+            bestIndex_->removePoint(id);
+        }
+    }
+
+    
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+    	ar.setObject(this);
+
+    	ar & *static_cast<NNIndex<Distance>*>(this);
+
+    	ar & target_precision_;
+    	ar & build_weight_;
+    	ar & memory_weight_;
+    	ar & sample_fraction_;
+
+    	flann_algorithm_t index_type;
+    	if (Archive::is_saving::value) {
+    		index_type = get_param<flann_algorithm_t>(bestParams_,"algorithm");
+    	}
+    	ar & index_type;
+    	ar & bestSearchParams_.checks;
+
+    	if (Archive::is_loading::value) {
+    		bestParams_["algorithm"] = index_type;
+
+    		index_params_["algorithm"] = getType();
+            index_params_["target_precision_"] = target_precision_;
+            index_params_["build_weight_"] = build_weight_;
+            index_params_["memory_weight_"] = memory_weight_;
+            index_params_["sample_fraction_"] = sample_fraction_;
+    	}
+    }
+
+    void saveIndex(FILE* stream)
+    {
+        {
+            serialization::SaveArchive sa(stream);
+            sa & *this;
+        }
+
+    	bestIndex_->saveIndex(stream);
+    }
+
+    void loadIndex(FILE* stream)
+    {
+        {
+            serialization::LoadArchive la(stream);
+            la & *this;
+        }
+        
+        IndexParams params;
+        flann_algorithm_t index_type = get_param<flann_algorithm_t>(bestParams_,"algorithm");
+        bestIndex_ = create_index_by_type<Distance>((flann_algorithm_t)index_type, dataset_, params, distance_);
+        bestIndex_->loadIndex(stream);
+    }
+
+    int knnSearch(const Matrix<ElementType>& queries,
+            Matrix<size_t>& indices,
+            Matrix<DistanceType>& dists,
+            size_t knn,
+            const SearchParams& params) const
+    {
+        if (params.checks == FLANN_CHECKS_AUTOTUNED) {
+            return bestIndex_->knnSearch(queries, indices, dists, knn, bestSearchParams_);
+        }
+        else {
+            return bestIndex_->knnSearch(queries, indices, dists, knn, params);
+        }
+    }
+
+    int knnSearch(const Matrix<ElementType>& queries,
+            std::vector< std::vector<size_t> >& indices,
+            std::vector<std::vector<DistanceType> >& dists,
+            size_t knn,
+            const SearchParams& params) const
+    {
+        if (params.checks == FLANN_CHECKS_AUTOTUNED) {
+            return bestIndex_->knnSearch(queries, indices, dists, knn, bestSearchParams_);
+        }
+        else {
+            return bestIndex_->knnSearch(queries, indices, dists, knn, params);
+        }
+
+    }
+    
+    int radiusSearch(const Matrix<ElementType>& queries,
+            Matrix<size_t>& indices,
+            Matrix<DistanceType>& dists,
+            DistanceType radius,
+            const SearchParams& params) const
+    {
+        if (params.checks == FLANN_CHECKS_AUTOTUNED) {
+            return bestIndex_->radiusSearch(queries, indices, dists, radius, bestSearchParams_);
+        }
+        else {
+            return bestIndex_->radiusSearch(queries, indices, dists, radius, params);
+        }
+    }
+
+    int radiusSearch(const Matrix<ElementType>& queries,
+            std::vector< std::vector<size_t> >& indices,
+            std::vector<std::vector<DistanceType> >& dists,
+            DistanceType radius,
+            const SearchParams& params) const
+    {
+        if (params.checks == FLANN_CHECKS_AUTOTUNED) {
+            return bestIndex_->radiusSearch(queries, indices, dists, radius, bestSearchParams_);
+        }
+        else {
+            return bestIndex_->radiusSearch(queries, indices, dists, radius, params);
+        }        
+    }
+
+    
+    
+    /**
+     *      Method that searches for nearest-neighbors
+     */
+    void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
+    {
+        // should not get here
+        assert(false);
+    }
+
+    IndexParams getParameters() const
+    {
+        return bestParams_;
+    }
+
+    FLANN_DEPRECATED SearchParams getSearchParameters() const
+    {
+        return bestSearchParams_;
+    }
+
+    FLANN_DEPRECATED float getSpeedup() const
+    {
+        return speedup_;
+    }
+
+
+    /**
+     *      Number of features in this index.
+     */
+    size_t size() const
+    {
+        return bestIndex_->size();
+    }
+
+    /**
+     *  The length of each vector in this index.
+     */
+    size_t veclen() const
+    {
+        return bestIndex_->veclen();
+    }
+
+    /**
+     * The amount of memory (in bytes) this index uses.
+     */
+    int usedMemory() const
+    {
+        return bestIndex_->usedMemory();
+    }
+
+    /**
+     * Algorithm name
+     */
+    flann_algorithm_t getType() const
+    {
+        return FLANN_INDEX_AUTOTUNED;
+    }
+
+protected:
+    void buildIndexImpl()
+    {
+        /* nothing to do here */
+    }
+
+    void freeIndex()
+    {
+        /* nothing to do here */
+    }
+
+private:
+
+    struct CostData
+    {
+        float searchTimeCost;
+        float buildTimeCost;
+        float memoryCost;
+        float totalCost;
+        IndexParams params;
+    };
+
+    void evaluate_kmeans(CostData& cost)
+    {
+        StartStopTimer t;
+        int checks;
+        const int nn = 1;
+
+        Logger::info("KMeansTree using params: max_iterations=%d, branching=%d\n",
+                     get_param<int>(cost.params,"iterations"),
+                     get_param<int>(cost.params,"branching"));
+        KMeansIndex<Distance> kmeans(sampledDataset_, cost.params, distance_);
+        // measure index build time
+        t.start();
+        kmeans.buildIndex();
+        t.stop();
+        float buildTime = (float)t.value;
+
+        // measure search time
+        float searchTime = test_index_precision(kmeans, sampledDataset_, testDataset_, gt_matches_, target_precision_, checks, distance_, nn);
+
+        float datasetMemory = float(sampledDataset_.rows * sampledDataset_.cols * sizeof(float));
+        cost.memoryCost = (kmeans.usedMemory() + datasetMemory) / datasetMemory;
+        cost.searchTimeCost = searchTime;
+        cost.buildTimeCost = buildTime;
+        Logger::info("KMeansTree buildTime=%g, searchTime=%g, build_weight=%g\n", buildTime, searchTime, build_weight_);
+    }
+
+
+    void evaluate_kdtree(CostData& cost)
+    {
+        StartStopTimer t;
+        int checks;
+        const int nn = 1;
+
+        Logger::info("KDTree using params: trees=%d\n", get_param<int>(cost.params,"trees"));
+        KDTreeIndex<Distance> kdtree(sampledDataset_, cost.params, distance_);
+
+        t.start();
+        kdtree.buildIndex();
+        t.stop();
+        float buildTime = (float)t.value;
+
+        //measure search time
+        float searchTime = test_index_precision(kdtree, sampledDataset_, testDataset_, gt_matches_, target_precision_, checks, distance_, nn);
+
+        float datasetMemory = float(sampledDataset_.rows * sampledDataset_.cols * sizeof(float));
+        cost.memoryCost = (kdtree.usedMemory() + datasetMemory) / datasetMemory;
+        cost.searchTimeCost = searchTime;
+        cost.buildTimeCost = buildTime;
+        Logger::info("KDTree buildTime=%g, searchTime=%g\n", buildTime, searchTime);
+    }
+
+
+    //    struct KMeansSimpleDownhillFunctor {
+    //
+    //        Autotune& autotuner;
+    //        KMeansSimpleDownhillFunctor(Autotune& autotuner_) : autotuner(autotuner_) {};
+    //
+    //        float operator()(int* params) {
+    //
+    //            float maxFloat = numeric_limits<float>::max();
+    //
+    //            if (params[0]<2) return maxFloat;
+    //            if (params[1]<0) return maxFloat;
+    //
+    //            CostData c;
+    //            c.params["algorithm"] = KMEANS;
+    //            c.params["centers-init"] = CENTERS_RANDOM;
+    //            c.params["branching"] = params[0];
+    //            c.params["max-iterations"] = params[1];
+    //
+    //            autotuner.evaluate_kmeans(c);
+    //
+    //            return c.timeCost;
+    //
+    //        }
+    //    };
+    //
+    //    struct KDTreeSimpleDownhillFunctor {
+    //
+    //        Autotune& autotuner;
+    //        KDTreeSimpleDownhillFunctor(Autotune& autotuner_) : autotuner(autotuner_) {};
+    //
+    //        float operator()(int* params) {
+    //            float maxFloat = numeric_limits<float>::max();
+    //
+    //            if (params[0]<1) return maxFloat;
+    //
+    //            CostData c;
+    //            c.params["algorithm"] = KDTREE;
+    //            c.params["trees"] = params[0];
+    //
+    //            autotuner.evaluate_kdtree(c);
+    //
+    //            return c.timeCost;
+    //
+    //        }
+    //    };
+
+
+
+    void optimizeKMeans(std::vector<CostData>& costs)
+    {
+        Logger::info("KMEANS, Step 1: Exploring parameter space\n");
+
+        // explore kmeans parameters space using combinations of the parameters below
+        int maxIterations[] = { 1, 5, 10, 15 };
+        int branchingFactors[] = { 16, 32, 64, 128, 256 };
+
+        int kmeansParamSpaceSize = FLANN_ARRAY_LEN(maxIterations) * FLANN_ARRAY_LEN(branchingFactors);
+        costs.reserve(costs.size() + kmeansParamSpaceSize);
+
+        // evaluate kmeans for all parameter combinations
+        for (size_t i = 0; i < FLANN_ARRAY_LEN(maxIterations); ++i) {
+            for (size_t j = 0; j < FLANN_ARRAY_LEN(branchingFactors); ++j) {
+                CostData cost;
+                cost.params["algorithm"] = FLANN_INDEX_KMEANS;
+                cost.params["centers_init"] = FLANN_CENTERS_RANDOM;
+                cost.params["iterations"] = maxIterations[i];
+                cost.params["branching"] = branchingFactors[j];
+
+                evaluate_kmeans(cost);
+                costs.push_back(cost);
+            }
+        }
+
+        //         Logger::info("KMEANS, Step 2: simplex-downhill optimization\n");
+        //
+        //         const int n = 2;
+        //         // choose initial simplex points as the best parameters so far
+        //         int kmeansNMPoints[n*(n+1)];
+        //         float kmeansVals[n+1];
+        //         for (int i=0;i<n+1;++i) {
+        //             kmeansNMPoints[i*n] = (int)kmeansCosts[i].params["branching"];
+        //             kmeansNMPoints[i*n+1] = (int)kmeansCosts[i].params["max-iterations"];
+        //             kmeansVals[i] = kmeansCosts[i].timeCost;
+        //         }
+        //         KMeansSimpleDownhillFunctor kmeans_cost_func(*this);
+        //         // run optimization
+        //         optimizeSimplexDownhill(kmeansNMPoints,n,kmeans_cost_func,kmeansVals);
+        //         // store results
+        //         for (int i=0;i<n+1;++i) {
+        //             kmeansCosts[i].params["branching"] = kmeansNMPoints[i*2];
+        //             kmeansCosts[i].params["max-iterations"] = kmeansNMPoints[i*2+1];
+        //             kmeansCosts[i].timeCost = kmeansVals[i];
+        //         }
+    }
+
+
+    void optimizeKDTree(std::vector<CostData>& costs)
+    {
+        Logger::info("KD-TREE, Step 1: Exploring parameter space\n");
+
+        // explore kd-tree parameters space using the parameters below
+        int testTrees[] = { 1, 4, 8, 16, 32 };
+
+        // evaluate kdtree for all parameter combinations
+        for (size_t i = 0; i < FLANN_ARRAY_LEN(testTrees); ++i) {
+            CostData cost;
+            cost.params["algorithm"] = FLANN_INDEX_KDTREE;
+            cost.params["trees"] = testTrees[i];
+
+            evaluate_kdtree(cost);
+            costs.push_back(cost);
+        }
+
+        //         Logger::info("KD-TREE, Step 2: simplex-downhill optimization\n");
+        //
+        //         const int n = 1;
+        //         // choose initial simplex points as the best parameters so far
+        //         int kdtreeNMPoints[n*(n+1)];
+        //         float kdtreeVals[n+1];
+        //         for (int i=0;i<n+1;++i) {
+        //             kdtreeNMPoints[i] = (int)kdtreeCosts[i].params["trees"];
+        //             kdtreeVals[i] = kdtreeCosts[i].timeCost;
+        //         }
+        //         KDTreeSimpleDownhillFunctor kdtree_cost_func(*this);
+        //         // run optimization
+        //         optimizeSimplexDownhill(kdtreeNMPoints,n,kdtree_cost_func,kdtreeVals);
+        //         // store results
+        //         for (int i=0;i<n+1;++i) {
+        //             kdtreeCosts[i].params["trees"] = kdtreeNMPoints[i];
+        //             kdtreeCosts[i].timeCost = kdtreeVals[i];
+        //         }
+    }
+
+    /**
+     *  Chooses the best nearest-neighbor algorithm and estimates the optimal
+     *  parameters to use when building the index (for a given precision).
+     *  Returns a dictionary with the optimal parameters.
+     */
+    IndexParams estimateBuildParams()
+    {
+        std::vector<CostData> costs;
+
+        int sampleSize = int(sample_fraction_ * dataset_.rows);
+        int testSampleSize = std::min(sampleSize / 10, 1000);
+
+        Logger::info("Entering autotuning, dataset size: %d, sampleSize: %d, testSampleSize: %d, target precision: %g\n", dataset_.rows, sampleSize, testSampleSize, target_precision_);
+
+        // For a very small dataset, it makes no sense to build any fancy index, just
+        // use linear search
+        if (testSampleSize < 10) {
+            Logger::info("Choosing linear, dataset too small\n");
+            return LinearIndexParams();
+        }
+
+        // We use a fraction of the original dataset to speedup the autotune algorithm
+        sampledDataset_ = random_sample(dataset_, sampleSize);
+        // We use a cross-validation approach, first we sample a testset from the dataset
+        testDataset_ = random_sample(sampledDataset_, testSampleSize, true);
+
+        // We compute the ground truth using linear search
+        Logger::info("Computing ground truth... \n");
+        gt_matches_ = Matrix<size_t>(new size_t[testDataset_.rows], testDataset_.rows, 1);
+        StartStopTimer t;
+        int repeats = 0;
+        t.reset();
+        while (t.value<0.2) {
+        	repeats++;
+            t.start();
+        	compute_ground_truth<Distance>(sampledDataset_, testDataset_, gt_matches_, 0, distance_);
+            t.stop();
+        }
+
+        CostData linear_cost;
+        linear_cost.searchTimeCost = (float)t.value/repeats;
+        linear_cost.buildTimeCost = 0;
+        linear_cost.memoryCost = 0;
+        linear_cost.params["algorithm"] = FLANN_INDEX_LINEAR;
+
+        costs.push_back(linear_cost);
+
+        // Start parameter autotune process
+        Logger::info("Autotuning parameters...\n");
+
+        optimizeKMeans(costs);
+        optimizeKDTree(costs);
+
+        float bestTimeCost = costs[0].buildTimeCost * build_weight_ + costs[0].searchTimeCost;
+        for (size_t i = 0; i < costs.size(); ++i) {
+            float timeCost = costs[i].buildTimeCost * build_weight_ + costs[i].searchTimeCost;
+            Logger::debug("Time cost: %g\n", timeCost);
+            if (timeCost < bestTimeCost) {
+                bestTimeCost = timeCost;
+            }
+        }
+        Logger::debug("Best time cost: %g\n", bestTimeCost);
+
+    	IndexParams bestParams = costs[0].params;
+        if (bestTimeCost > 0) {
+        	float bestCost = (costs[0].buildTimeCost * build_weight_ + costs[0].searchTimeCost) / bestTimeCost;
+        	for (size_t i = 0; i < costs.size(); ++i) {
+        		float crtCost = (costs[i].buildTimeCost * build_weight_ + costs[i].searchTimeCost) / bestTimeCost +
+        				memory_weight_ * costs[i].memoryCost;
+        		Logger::debug("Cost: %g\n", crtCost);
+        		if (crtCost < bestCost) {
+        			bestCost = crtCost;
+        			bestParams = costs[i].params;
+        		}
+        	}
+            Logger::debug("Best cost: %g\n", bestCost);
+        }
+
+        delete[] gt_matches_.ptr();
+        delete[] testDataset_.ptr();
+        delete[] sampledDataset_.ptr();
+
+        return bestParams;
+    }
+
+
+
+    /**
+     *  Estimates the search time parameters needed to get the desired precision.
+     *  Precondition: the index is built
+     *  Postcondition: the searchParams will have the optimum params set, also the speedup obtained over linear search.
+     */
+    float estimateSearchParams(SearchParams& searchParams)
+    {
+        const int nn = 1;
+        const size_t SAMPLE_COUNT = 1000;
+
+        assert(bestIndex_ != NULL); // must have a valid index
+
+        float speedup = 0;
+
+        int samples = (int)std::min(dataset_.rows / 10, SAMPLE_COUNT);
+        if (samples > 0) {
+            Matrix<ElementType> testDataset = random_sample(dataset_, samples);
+
+            Logger::info("Computing ground truth\n");
+
+            // we need to compute the ground truth first
+            Matrix<size_t> gt_matches(new size_t[testDataset.rows], testDataset.rows, 1);
+            StartStopTimer t;
+            int repeats = 0;
+            t.reset();
+            while (t.value<0.2) {
+            	repeats++;
+                t.start();
+            	compute_ground_truth<Distance>(dataset_, testDataset, gt_matches, 1, distance_);
+                t.stop();
+            }
+            float linear = (float)t.value/repeats;
+
+            int checks;
+            Logger::info("Estimating number of checks\n");
+
+            float searchTime;
+            float cb_index;
+            if (bestIndex_->getType() == FLANN_INDEX_KMEANS) {
+                Logger::info("KMeans algorithm, estimating cluster border factor\n");
+                KMeansIndex<Distance>* kmeans = static_cast<KMeansIndex<Distance>*>(bestIndex_);
+                float bestSearchTime = -1;
+                float best_cb_index = -1;
+                int best_checks = -1;
+                for (cb_index = 0; cb_index < 1.1f; cb_index += 0.2f) {
+                    kmeans->set_cb_index(cb_index);
+                    searchTime = test_index_precision(*kmeans, dataset_, testDataset, gt_matches, target_precision_, checks, distance_, nn, 1);
+                    if ((searchTime < bestSearchTime) || (bestSearchTime == -1)) {
+                        bestSearchTime = searchTime;
+                        best_cb_index = cb_index;
+                        best_checks = checks;
+                    }
+                }
+                searchTime = bestSearchTime;
+                cb_index = best_cb_index;
+                checks = best_checks;
+
+                kmeans->set_cb_index(best_cb_index);
+                Logger::info("Optimum cb_index: %g\n", cb_index);
+                bestParams_["cb_index"] = cb_index;
+            }
+            else {
+                searchTime = test_index_precision(*bestIndex_, dataset_, testDataset, gt_matches, target_precision_, checks, distance_, nn, 1);
+            }
+
+            Logger::info("Required number of checks: %d \n", checks);
+            searchParams.checks = checks;
+
+            speedup = linear / searchTime;
+
+            delete[] gt_matches.ptr();
+            delete[] testDataset.ptr();
+        }
+
+        return speedup;
+    }
+
+
+    void swap(AutotunedIndex& other)
+    {
+    	BaseClass::swap(other);
+    	std::swap(bestIndex_, other.bestIndex_);
+    	std::swap(bestParams_, other.bestParams_);
+    	std::swap(bestSearchParams_, other.bestSearchParams_);
+    	std::swap(speedup_, other.speedup_);
+    	std::swap(dataset_, other.dataset_);
+    	std::swap(target_precision_, other.target_precision_);
+    	std::swap(build_weight_, other.build_weight_);
+    	std::swap(memory_weight_, other.memory_weight_);
+    	std::swap(sample_fraction_, other.sample_fraction_);
+    }
+
+private:
+    NNIndex<Distance>* bestIndex_;
+
+    IndexParams bestParams_;
+    SearchParams bestSearchParams_;
+
+    Matrix<ElementType> sampledDataset_;
+    Matrix<ElementType> testDataset_;
+    Matrix<size_t> gt_matches_;
+
+    float speedup_;
+
+    /**
+     * The dataset used by this index
+     */
+    Matrix<ElementType> dataset_;
+
+    /**
+     * Index parameters
+     */
+    float target_precision_;
+    float build_weight_;
+    float memory_weight_;
+    float sample_fraction_;
+
+    USING_BASECLASS_SYMBOLS
+};
+}
+
+#endif /* RTABMAP_FLANN_AUTOTUNED_INDEX_H_ */

corelib/src/rtflann/algorithms/center_chooser.h

@@ -0,0 +1,385 @@
+/*
+ * center_chooser.h
+ *
+ *  Created on: 2012-11-04
+ *      Author: marius
+ */
+
+#ifndef RTABMAP_CENTER_CHOOSER_H_
+#define RTABMAP_CENTER_CHOOSER_H_
+
+#include "rtflann/util/matrix.h"
+
+namespace rtflann
+{
+
+template <typename Distance, typename ElementType>
+struct squareDistance
+{
+    typedef typename Distance::ResultType ResultType;
+    ResultType operator()( ResultType dist ) { return dist*dist; }
+};
+
+
+template <typename ElementType>
+struct squareDistance<L2_Simple<ElementType>, ElementType>
+{
+    typedef typename L2_Simple<ElementType>::ResultType ResultType;
+    ResultType operator()( ResultType dist ) { return dist; }
+};
+
+template <typename ElementType>
+struct squareDistance<L2_3D<ElementType>, ElementType>
+{
+    typedef typename L2_3D<ElementType>::ResultType ResultType;
+    ResultType operator()( ResultType dist ) { return dist; }
+};
+
+template <typename ElementType>
+struct squareDistance<L2<ElementType>, ElementType>
+{
+    typedef typename L2<ElementType>::ResultType ResultType;
+    ResultType operator()( ResultType dist ) { return dist; }
+};
+
+
+template <typename ElementType>
+struct squareDistance<HellingerDistance<ElementType>, ElementType>
+{
+    typedef typename HellingerDistance<ElementType>::ResultType ResultType;
+    ResultType operator()( ResultType dist ) { return dist; }
+};
+
+
+template <typename ElementType>
+struct squareDistance<ChiSquareDistance<ElementType>, ElementType>
+{
+    typedef typename ChiSquareDistance<ElementType>::ResultType ResultType;
+    ResultType operator()( ResultType dist ) { return dist; }
+};
+
+
+template <typename Distance>
+typename Distance::ResultType ensureSquareDistance( typename Distance::ResultType dist )
+{
+    typedef typename Distance::ElementType ElementType;
+
+    squareDistance<Distance, ElementType> dummy;
+    return dummy( dist );
+}
+
+
+
+template <typename Distance>
+class CenterChooser
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    CenterChooser(const Distance& distance, const std::vector<ElementType*>& points) : distance_(distance), points_(points) {};
+
+    virtual ~CenterChooser() {};
+    
+    void setDataSize(size_t cols) { cols_ = cols; }
+
+    /**
+     * Chooses cluster centers
+     *
+     * @param k number of centers to choose
+     * @param indices indices of points to choose the centers from
+     * @param indices_length length of indices
+     * @param centers indices of chosen centers
+     * @param centers_length length of centers array
+     */
+	virtual void operator()(int k, int* indices, int indices_length, int* centers, int& centers_length) = 0;
+
+protected:
+	const Distance distance_;
+    const std::vector<ElementType*>& points_;
+    size_t cols_;
+};
+
+
+template <typename Distance>
+class RandomCenterChooser : public CenterChooser<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+    using CenterChooser<Distance>::points_;
+    using CenterChooser<Distance>::distance_;
+    using CenterChooser<Distance>::cols_;
+
+    RandomCenterChooser(const Distance& distance, const std::vector<ElementType*>& points) :
+    	CenterChooser<Distance>(distance, points) {}
+
+    void operator()(int k, int* indices, int indices_length, int* centers, int& centers_length)
+    {
+        UniqueRandom r(indices_length);
+
+        int index;
+        for (index=0; index<k; ++index) {
+            bool duplicate = true;
+            int rnd;
+            while (duplicate) {
+                duplicate = false;
+                rnd = r.next();
+                if (rnd<0) {
+                    centers_length = index;
+                    return;
+                }
+
+                centers[index] = indices[rnd];
+
+                for (int j=0; j<index; ++j) {
+                    DistanceType sq = distance_(points_[centers[index]], points_[centers[j]], cols_);
+                    if (sq<1e-16) {
+                        duplicate = true;
+                    }
+                }
+            }
+        }
+
+        centers_length = index;
+    }
+};
+
+
+
+/**
+ * Chooses the initial centers using the Gonzales algorithm.
+ */
+template <typename Distance>
+class GonzalesCenterChooser : public CenterChooser<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    using CenterChooser<Distance>::points_;
+    using CenterChooser<Distance>::distance_;
+    using CenterChooser<Distance>::cols_;
+
+    GonzalesCenterChooser(const Distance& distance, const std::vector<ElementType*>& points) : 
+        CenterChooser<Distance>(distance, points) {}
+
+    void operator()(int k, int* indices, int indices_length, int* centers, int& centers_length)
+    {
+        int n = indices_length;
+
+        int rnd = rand_int(n);
+        assert(rnd >=0 && rnd < n);
+
+        centers[0] = indices[rnd];
+
+        int index;
+        for (index=1; index<k; ++index) {
+
+            int best_index = -1;
+            DistanceType best_val = 0;
+            for (int j=0; j<n; ++j) {
+            	DistanceType dist = distance_(points_[centers[0]],points_[indices[j]],cols_);
+                for (int i=1; i<index; ++i) {
+                    DistanceType tmp_dist = distance_(points_[centers[i]],points_[indices[j]],cols_);
+                    if (tmp_dist<dist) {
+                        dist = tmp_dist;
+                    }
+                }
+                if (dist>best_val) {
+                    best_val = dist;
+                    best_index = j;
+                }
+            }
+            if (best_index!=-1) {
+                centers[index] = indices[best_index];
+            }
+            else {
+                break;
+            }
+        }
+        centers_length = index;
+    }
+};
+
+
+/**
+ * Chooses the initial centers using the algorithm proposed in the KMeans++ paper:
+ * Arthur, David; Vassilvitskii, Sergei - k-means++: The Advantages of Careful Seeding
+ */
+template <typename Distance>
+class KMeansppCenterChooser : public CenterChooser<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    using CenterChooser<Distance>::points_;
+    using CenterChooser<Distance>::distance_;
+    using CenterChooser<Distance>::cols_;
+
+    KMeansppCenterChooser(const Distance& distance, const std::vector<ElementType*>& points) : 
+        CenterChooser<Distance>(distance, points) {}
+
+    void operator()(int k, int* indices, int indices_length, int* centers, int& centers_length)
+    {
+        int n = indices_length;
+
+        double currentPot = 0;
+        DistanceType* closestDistSq = new DistanceType[n];
+
+        // Choose one random center and set the closestDistSq values
+        int index = rand_int(n);
+        assert(index >=0 && index < n);
+        centers[0] = indices[index];
+
+        // Computing distance^2 will have the advantage of even higher probability further to pick new centers
+        // far from previous centers (and this complies to "k-means++: the advantages of careful seeding" article)
+        for (int i = 0; i < n; i++) {
+            closestDistSq[i] = distance_(points_[indices[i]], points_[indices[index]], cols_);
+            closestDistSq[i] = ensureSquareDistance<Distance>( closestDistSq[i] );
+            currentPot += closestDistSq[i];
+        }
+
+
+        const int numLocalTries = 1;
+
+        // Choose each center
+        int centerCount;
+        for (centerCount = 1; centerCount < k; centerCount++) {
+
+            // Repeat several trials
+            double bestNewPot = -1;
+            int bestNewIndex = 0;
+            for (int localTrial = 0; localTrial < numLocalTries; localTrial++) {
+
+                // Choose our center - have to be slightly careful to return a valid answer even accounting
+                // for possible rounding errors
+                double randVal = rand_double(currentPot);
+                for (index = 0; index < n-1; index++) {
+                    if (randVal <= closestDistSq[index]) break;
+                    else randVal -= closestDistSq[index];
+                }
+
+                // Compute the new potential
+                double newPot = 0;
+                for (int i = 0; i < n; i++) {
+                    DistanceType dist = distance_(points_[indices[i]], points_[indices[index]], cols_);
+                    newPot += std::min( ensureSquareDistance<Distance>(dist), closestDistSq[i] );
+                }
+
+                // Store the best result
+                if ((bestNewPot < 0)||(newPot < bestNewPot)) {
+                    bestNewPot = newPot;
+                    bestNewIndex = index;
+                }
+            }
+
+            // Add the appropriate center
+            centers[centerCount] = indices[bestNewIndex];
+            currentPot = bestNewPot;
+            for (int i = 0; i < n; i++) {
+                DistanceType dist = distance_(points_[indices[i]], points_[indices[bestNewIndex]], cols_);
+                closestDistSq[i] = std::min( ensureSquareDistance<Distance>(dist), closestDistSq[i] );
+            }
+        }
+
+        centers_length = centerCount;
+
+        delete[] closestDistSq;
+    }
+};
+
+
+
+/**
+ * Chooses the initial centers in a way inspired by Gonzales (by Pierre-Emmanuel Viel):
+ * select the first point of the list as a candidate, then parse the points list. If another
+ * point is further than current candidate from the other centers, test if it is a good center
+ * of a local aggregation. If it is, replace current candidate by this point. And so on...
+ *
+ * Used with KMeansIndex that computes centers coordinates by averaging positions of clusters points,
+ * this doesn't make a real difference with previous methods. But used with HierarchicalClusteringIndex
+ * class that pick centers among existing points instead of computing the barycenters, there is a real
+ * improvement.
+ */
+template <typename Distance>
+class GroupWiseCenterChooser : public CenterChooser<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    using CenterChooser<Distance>::points_;
+    using CenterChooser<Distance>::distance_;
+    using CenterChooser<Distance>::cols_;
+
+    GroupWiseCenterChooser(const Distance& distance, const std::vector<ElementType*>& points) :
+        CenterChooser<Distance>(distance, points) {}
+
+    void operator()(int k, int* indices, int indices_length, int* centers, int& centers_length)
+    {
+        const float kSpeedUpFactor = 1.3f;
+
+        int n = indices_length;
+
+        DistanceType* closestDistSq = new DistanceType[n];
+
+        // Choose one random center and set the closestDistSq values
+        int index = rand_int(n);
+        assert(index >=0 && index < n);
+        centers[0] = indices[index];
+
+        for (int i = 0; i < n; i++) {
+            closestDistSq[i] = distance_(points_[indices[i]], points_[indices[index]], cols_);
+        }
+
+
+        // Choose each center
+        int centerCount;
+        for (centerCount = 1; centerCount < k; centerCount++) {
+
+            // Repeat several trials
+            double bestNewPot = -1;
+            int bestNewIndex = 0;
+            DistanceType furthest = 0;
+            for (index = 0; index < n; index++) {
+
+                // We will test only the potential of the points further than current candidate
+                if( closestDistSq[index] > kSpeedUpFactor * (float)furthest ) {
+
+                    // Compute the new potential
+                    double newPot = 0;
+                    for (int i = 0; i < n; i++) {
+                        newPot += std::min( distance_(points_[indices[i]], points_[indices[index]], cols_)
+                                            , closestDistSq[i] );
+                    }
+
+                    // Store the best result
+                    if ((bestNewPot < 0)||(newPot <= bestNewPot)) {
+                        bestNewPot = newPot;
+                        bestNewIndex = index;
+                        furthest = closestDistSq[index];
+                    }
+                }
+            }
+
+            // Add the appropriate center
+            centers[centerCount] = indices[bestNewIndex];
+            for (int i = 0; i < n; i++) {
+                closestDistSq[i] = std::min( distance_(points_[indices[i]], points_[indices[bestNewIndex]], cols_)
+                                             , closestDistSq[i] );
+            }
+        }
+
+        centers_length = centerCount;
+
+        delete[] closestDistSq;
+    }
+};
+
+
+}
+
+
+#endif /* RTABMAP_CENTER_CHOOSER_H_ */

corelib/src/rtflann/algorithms/composite_index.h

@@ -0,0 +1,239 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_COMPOSITE_INDEX_H_
+#define RTABMAP_FLANN_COMPOSITE_INDEX_H_
+
+#include "rtflann/general.h"
+#include "rtflann/algorithms/nn_index.h"
+#include "rtflann/algorithms/kdtree_index.h"
+#include "rtflann/algorithms/kmeans_index.h"
+
+namespace rtflann
+{
+
+/**
+ * Index parameters for the CompositeIndex.
+ */
+struct CompositeIndexParams : public IndexParams
+{
+    CompositeIndexParams(int trees = 4, int branching = 32, int iterations = 11,
+                         flann_centers_init_t centers_init = FLANN_CENTERS_RANDOM, float cb_index = 0.2 )
+    {
+        (*this)["algorithm"] = FLANN_INDEX_KMEANS;
+        // number of randomized trees to use (for kdtree)
+        (*this)["trees"] = trees;
+        // branching factor
+        (*this)["branching"] = branching;
+        // max iterations to perform in one kmeans clustering (kmeans tree)
+        (*this)["iterations"] = iterations;
+        // algorithm used for picking the initial cluster centers for kmeans tree
+        (*this)["centers_init"] = centers_init;
+        // cluster boundary index. Used when searching the kmeans tree
+        (*this)["cb_index"] = cb_index;
+    }
+};
+
+
+/**
+ * This index builds a kd-tree index and a k-means index and performs nearest
+ * neighbour search both indexes. This gives a slight boost in search performance
+ * as some of the neighbours that are missed by one index are found by the other.
+ */
+template <typename Distance>
+class CompositeIndex : public NNIndex<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    typedef NNIndex<Distance> BaseClass;
+
+    typedef bool needs_kdtree_distance;
+
+    /**
+     * Index constructor
+     * @param inputData dataset containing the points to index
+     * @param params Index parameters
+     * @param d Distance functor
+     * @return
+     */
+    CompositeIndex(const IndexParams& params = CompositeIndexParams(), Distance d = Distance()) :
+    	BaseClass(params, d)
+    {
+        kdtree_index_ = new KDTreeIndex<Distance>(params, d);
+        kmeans_index_ = new KMeansIndex<Distance>(params, d);
+
+    }
+
+    CompositeIndex(const Matrix<ElementType>& inputData, const IndexParams& params = CompositeIndexParams(),
+                   Distance d = Distance()) : BaseClass(params, d)
+    {
+        kdtree_index_ = new KDTreeIndex<Distance>(inputData, params, d);
+        kmeans_index_ = new KMeansIndex<Distance>(inputData, params, d);
+    }
+
+    CompositeIndex(const CompositeIndex& other) : BaseClass(other),
+    	kmeans_index_(other.kmeans_index_), kdtree_index_(other.kdtree_index_)
+    {
+    }
+
+    CompositeIndex& operator=(CompositeIndex other)
+    {
+    	this->swap(other);
+    	return *this;
+    }
+
+    virtual ~CompositeIndex()
+    {
+        delete kdtree_index_;
+        delete kmeans_index_;
+    }
+
+    BaseClass* clone() const
+    {
+    	return new CompositeIndex(*this);
+    }
+
+    /**
+     * @return The index type
+     */
+    flann_algorithm_t getType() const
+    {
+        return FLANN_INDEX_COMPOSITE;
+    }
+
+    /**
+     * @return Size of the index
+     */
+    size_t size() const
+    {
+        return kdtree_index_->size();
+    }
+
+    /**
+     * \returns The dimensionality of the features in this index.
+     */
+    size_t veclen() const
+    {
+        return kdtree_index_->veclen();
+    }
+
+    /**
+     * \returns The amount of memory (in bytes) used by the index.
+     */
+    int usedMemory() const
+    {
+        return kmeans_index_->usedMemory() + kdtree_index_->usedMemory();
+    }
+
+    using NNIndex<Distance>::buildIndex;
+    /**
+     * \brief Builds the index
+     */
+    void buildIndex()
+    {
+        Logger::info("Building kmeans tree...\n");
+        kmeans_index_->buildIndex();
+        Logger::info("Building kdtree tree...\n");
+        kdtree_index_->buildIndex();
+    }
+    
+    void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        kmeans_index_->addPoints(points, rebuild_threshold);
+        kdtree_index_->addPoints(points, rebuild_threshold);
+    }
+
+    void removePoint(size_t index)
+    {
+        kmeans_index_->removePoint(index);
+        kdtree_index_->removePoint(index);
+    }
+
+
+    /**
+     * \brief Saves the index to a stream
+     * \param stream The stream to save the index to
+     */
+    void saveIndex(FILE* stream)
+    {
+        kmeans_index_->saveIndex(stream);
+        kdtree_index_->saveIndex(stream);
+    }
+
+    /**
+     * \brief Loads the index from a stream
+     * \param stream The stream from which the index is loaded
+     */
+    void loadIndex(FILE* stream)
+    {
+        kmeans_index_->loadIndex(stream);
+        kdtree_index_->loadIndex(stream);
+    }
+
+    /**
+     * \brief Method that searches for nearest-neighbours
+     */
+    void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
+    {
+        kmeans_index_->findNeighbors(result, vec, searchParams);
+        kdtree_index_->findNeighbors(result, vec, searchParams);
+    }
+
+protected:
+    void swap(CompositeIndex& other)
+    {
+    	std::swap(kmeans_index_, other.kmeans_index_);
+    	std::swap(kdtree_index_, other.kdtree_index_);
+    }
+
+    void buildIndexImpl()
+    {
+        /* nothing to do here */
+    }
+
+    void freeIndex()
+    {
+        /* nothing to do here */
+    }
+
+
+private:
+    /** The k-means index */
+    KMeansIndex<Distance>* kmeans_index_;
+
+    /** The kd-tree index */
+    KDTreeIndex<Distance>* kdtree_index_;
+};
+
+}
+
+#endif //FLANN_COMPOSITE_INDEX_H_

corelib/src/rtflann/algorithms/dist.h

@@ -0,0 +1,790 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_DIST_H_
+#define RTABMAP_FLANN_DIST_H_
+
+#include <cmath>
+#include <cstdlib>
+#include <string.h>
+#ifdef _MSC_VER
+typedef unsigned __int32 uint32_t;
+typedef unsigned __int64 uint64_t;
+#else
+#include <stdint.h>
+#endif
+
+#include "rtflann/defines.h"
+
+
+namespace rtflann
+{
+
+template<typename T>
+struct Accumulator { typedef T Type; };
+template<>
+struct Accumulator<unsigned char>  { typedef float Type; };
+template<>
+struct Accumulator<unsigned short> { typedef float Type; };
+template<>
+struct Accumulator<unsigned int> { typedef float Type; };
+template<>
+struct Accumulator<char>   { typedef float Type; };
+template<>
+struct Accumulator<short>  { typedef float Type; };
+template<>
+struct Accumulator<int> { typedef float Type; };
+
+
+
+/**
+ * Squared Euclidean distance functor.
+ *
+ * This is the simpler, unrolled version. This is preferable for
+ * very low dimensionality data (eg 3D points)
+ */
+template<class T>
+struct L2_Simple
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
+    {
+        ResultType result = ResultType();
+        ResultType diff;
+        for(size_t i = 0; i < size; ++i ) {
+            diff = *a++ - *b++;
+            result += diff*diff;
+        }
+        return result;
+    }
+
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        return (a-b)*(a-b);
+    }
+};
+
+template<class T>
+struct L2_3D
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
+    {
+        ResultType result = ResultType();        
+        ResultType diff;
+        diff = *a++ - *b++;
+        result += diff*diff;
+        diff = *a++ - *b++;
+        result += diff*diff;
+        diff = *a++ - *b++;
+        result += diff*diff;        
+        return result;
+    }
+
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        return (a-b)*(a-b);
+    }
+};
+
+/**
+ * Squared Euclidean distance functor, optimized version
+ */
+template<class T>
+struct L2
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    /**
+     *  Compute the squared Euclidean distance between two vectors.
+     *
+     *	This is highly optimised, with loop unrolling, as it is one
+     *	of the most expensive inner loops.
+     *
+     *	The computation of squared root at the end is omitted for
+     *	efficiency.
+     */
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
+    {
+        ResultType result = ResultType();
+        ResultType diff0, diff1, diff2, diff3;
+        Iterator1 last = a + size;
+        Iterator1 lastgroup = last - 3;
+
+        /* Process 4 items with each loop for efficiency. */
+        while (a < lastgroup) {
+            diff0 = (ResultType)(a[0] - b[0]);
+            diff1 = (ResultType)(a[1] - b[1]);
+            diff2 = (ResultType)(a[2] - b[2]);
+            diff3 = (ResultType)(a[3] - b[3]);
+            result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
+            a += 4;
+            b += 4;
+
+            if ((worst_dist>0)&&(result>worst_dist)) {
+                return result;
+            }
+        }
+        /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
+        while (a < last) {
+            diff0 = (ResultType)(*a++ - *b++);
+            result += diff0 * diff0;
+        }
+        return result;
+    }
+
+    /**
+     *	Partial euclidean distance, using just one dimension. This is used by the
+     *	kd-tree when computing partial distances while traversing the tree.
+     *
+     *	Squared root is omitted for efficiency.
+     */
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        return (a-b)*(a-b);
+    }
+};
+
+
+/*
+ * Manhattan distance functor, optimized version
+ */
+template<class T>
+struct L1
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    /**
+     *  Compute the Manhattan (L_1) distance between two vectors.
+     *
+     *	This is highly optimised, with loop unrolling, as it is one
+     *	of the most expensive inner loops.
+     */
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
+    {
+        ResultType result = ResultType();
+        ResultType diff0, diff1, diff2, diff3;
+        Iterator1 last = a + size;
+        Iterator1 lastgroup = last - 3;
+
+        /* Process 4 items with each loop for efficiency. */
+        while (a < lastgroup) {
+            diff0 = (ResultType)std::abs(a[0] - b[0]);
+            diff1 = (ResultType)std::abs(a[1] - b[1]);
+            diff2 = (ResultType)std::abs(a[2] - b[2]);
+            diff3 = (ResultType)std::abs(a[3] - b[3]);
+            result += diff0 + diff1 + diff2 + diff3;
+            a += 4;
+            b += 4;
+
+            if ((worst_dist>0)&&(result>worst_dist)) {
+                return result;
+            }
+        }
+        /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
+        while (a < last) {
+            diff0 = (ResultType)std::abs(*a++ - *b++);
+            result += diff0;
+        }
+        return result;
+    }
+
+    /**
+     * Partial distance, used by the kd-tree.
+     */
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        return std::abs(a-b);
+    }
+};
+
+
+
+template<class T>
+struct MinkowskiDistance
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    int order;
+
+    MinkowskiDistance(int order_) : order(order_) {}
+
+    /**
+     *  Compute the Minkowsky (L_p) distance between two vectors.
+     *
+     *	This is highly optimised, with loop unrolling, as it is one
+     *	of the most expensive inner loops.
+     *
+     *	The computation of squared root at the end is omitted for
+     *	efficiency.
+     */
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
+    {
+        ResultType result = ResultType();
+        ResultType diff0, diff1, diff2, diff3;
+        Iterator1 last = a + size;
+        Iterator1 lastgroup = last - 3;
+
+        /* Process 4 items with each loop for efficiency. */
+        while (a < lastgroup) {
+            diff0 = (ResultType)std::abs(a[0] - b[0]);
+            diff1 = (ResultType)std::abs(a[1] - b[1]);
+            diff2 = (ResultType)std::abs(a[2] - b[2]);
+            diff3 = (ResultType)std::abs(a[3] - b[3]);
+            result += pow(diff0,order) + pow(diff1,order) + pow(diff2,order) + pow(diff3,order);
+            a += 4;
+            b += 4;
+
+            if ((worst_dist>0)&&(result>worst_dist)) {
+                return result;
+            }
+        }
+        /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
+        while (a < last) {
+            diff0 = (ResultType)std::abs(*a++ - *b++);
+            result += pow(diff0,order);
+        }
+        return result;
+    }
+
+    /**
+     * Partial distance, used by the kd-tree.
+     */
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        return pow(static_cast<ResultType>(std::abs(a-b)),order);
+    }
+};
+
+
+
+template<class T>
+struct MaxDistance
+{
+    typedef bool is_vector_space_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    /**
+     *  Compute the max distance (L_infinity) between two vectors.
+     *
+     *  This distance is not a valid kdtree distance, it's not dimensionwise additive.
+     */
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
+    {
+        ResultType result = ResultType();
+        ResultType diff0, diff1, diff2, diff3;
+        Iterator1 last = a + size;
+        Iterator1 lastgroup = last - 3;
+
+        /* Process 4 items with each loop for efficiency. */
+        while (a < lastgroup) {
+            diff0 = std::abs(a[0] - b[0]);
+            diff1 = std::abs(a[1] - b[1]);
+            diff2 = std::abs(a[2] - b[2]);
+            diff3 = std::abs(a[3] - b[3]);
+            if (diff0>result) {result = diff0; }
+            if (diff1>result) {result = diff1; }
+            if (diff2>result) {result = diff2; }
+            if (diff3>result) {result = diff3; }
+            a += 4;
+            b += 4;
+
+            if ((worst_dist>0)&&(result>worst_dist)) {
+                return result;
+            }
+        }
+        /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
+        while (a < last) {
+            diff0 = std::abs(*a++ - *b++);
+            result = (diff0>result) ? diff0 : result;
+        }
+        return result;
+    }
+
+    /* This distance functor is not dimension-wise additive, which
+     * makes it an invalid kd-tree distance, not implementing the accum_dist method */
+
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/**
+ * Hamming distance functor - counts the bit differences between two strings - useful for the Brief descriptor
+ * bit count of A exclusive XOR'ed with B
+ */
+struct HammingLUT
+{
+    typedef unsigned char ElementType;
+    typedef int ResultType;
+
+    /** this will count the bits in a ^ b
+     */
+    ResultType operator()(const unsigned char* a, const unsigned char* b, int size) const
+    {
+        ResultType result = 0;
+        for (int i = 0; i < size; i++) {
+            result += byteBitsLookUp(a[i] ^ b[i]);
+        }
+        return result;
+    }
+
+
+    /** \brief given a byte, count the bits using a look up table
+     *  \param b the byte to count bits.  The look up table has an entry for all
+     *  values of b, where that entry is the number of bits.
+     *  \return the number of bits in byte b
+     */
+    static unsigned char byteBitsLookUp(unsigned char b)
+    {
+        static const unsigned char table[256]  = {
+            /* 0 */ 0, /* 1 */ 1, /* 2 */ 1, /* 3 */ 2,
+            /* 4 */ 1, /* 5 */ 2, /* 6 */ 2, /* 7 */ 3,
+            /* 8 */ 1, /* 9 */ 2, /* a */ 2, /* b */ 3,
+            /* c */ 2, /* d */ 3, /* e */ 3, /* f */ 4,
+            /* 10 */ 1, /* 11 */ 2, /* 12 */ 2, /* 13 */ 3,
+            /* 14 */ 2, /* 15 */ 3, /* 16 */ 3, /* 17 */ 4,
+            /* 18 */ 2, /* 19 */ 3, /* 1a */ 3, /* 1b */ 4,
+            /* 1c */ 3, /* 1d */ 4, /* 1e */ 4, /* 1f */ 5,
+            /* 20 */ 1, /* 21 */ 2, /* 22 */ 2, /* 23 */ 3,
+            /* 24 */ 2, /* 25 */ 3, /* 26 */ 3, /* 27 */ 4,
+            /* 28 */ 2, /* 29 */ 3, /* 2a */ 3, /* 2b */ 4,
+            /* 2c */ 3, /* 2d */ 4, /* 2e */ 4, /* 2f */ 5,
+            /* 30 */ 2, /* 31 */ 3, /* 32 */ 3, /* 33 */ 4,
+            /* 34 */ 3, /* 35 */ 4, /* 36 */ 4, /* 37 */ 5,
+            /* 38 */ 3, /* 39 */ 4, /* 3a */ 4, /* 3b */ 5,
+            /* 3c */ 4, /* 3d */ 5, /* 3e */ 5, /* 3f */ 6,
+            /* 40 */ 1, /* 41 */ 2, /* 42 */ 2, /* 43 */ 3,
+            /* 44 */ 2, /* 45 */ 3, /* 46 */ 3, /* 47 */ 4,
+            /* 48 */ 2, /* 49 */ 3, /* 4a */ 3, /* 4b */ 4,
+            /* 4c */ 3, /* 4d */ 4, /* 4e */ 4, /* 4f */ 5,
+            /* 50 */ 2, /* 51 */ 3, /* 52 */ 3, /* 53 */ 4,
+            /* 54 */ 3, /* 55 */ 4, /* 56 */ 4, /* 57 */ 5,
+            /* 58 */ 3, /* 59 */ 4, /* 5a */ 4, /* 5b */ 5,
+            /* 5c */ 4, /* 5d */ 5, /* 5e */ 5, /* 5f */ 6,
+            /* 60 */ 2, /* 61 */ 3, /* 62 */ 3, /* 63 */ 4,
+            /* 64 */ 3, /* 65 */ 4, /* 66 */ 4, /* 67 */ 5,
+            /* 68 */ 3, /* 69 */ 4, /* 6a */ 4, /* 6b */ 5,
+            /* 6c */ 4, /* 6d */ 5, /* 6e */ 5, /* 6f */ 6,
+            /* 70 */ 3, /* 71 */ 4, /* 72 */ 4, /* 73 */ 5,
+            /* 74 */ 4, /* 75 */ 5, /* 76 */ 5, /* 77 */ 6,
+            /* 78 */ 4, /* 79 */ 5, /* 7a */ 5, /* 7b */ 6,
+            /* 7c */ 5, /* 7d */ 6, /* 7e */ 6, /* 7f */ 7,
+            /* 80 */ 1, /* 81 */ 2, /* 82 */ 2, /* 83 */ 3,
+            /* 84 */ 2, /* 85 */ 3, /* 86 */ 3, /* 87 */ 4,
+            /* 88 */ 2, /* 89 */ 3, /* 8a */ 3, /* 8b */ 4,
+            /* 8c */ 3, /* 8d */ 4, /* 8e */ 4, /* 8f */ 5,
+            /* 90 */ 2, /* 91 */ 3, /* 92 */ 3, /* 93 */ 4,
+            /* 94 */ 3, /* 95 */ 4, /* 96 */ 4, /* 97 */ 5,
+            /* 98 */ 3, /* 99 */ 4, /* 9a */ 4, /* 9b */ 5,
+            /* 9c */ 4, /* 9d */ 5, /* 9e */ 5, /* 9f */ 6,
+            /* a0 */ 2, /* a1 */ 3, /* a2 */ 3, /* a3 */ 4,
+            /* a4 */ 3, /* a5 */ 4, /* a6 */ 4, /* a7 */ 5,
+            /* a8 */ 3, /* a9 */ 4, /* aa */ 4, /* ab */ 5,
+            /* ac */ 4, /* ad */ 5, /* ae */ 5, /* af */ 6,
+            /* b0 */ 3, /* b1 */ 4, /* b2 */ 4, /* b3 */ 5,
+            /* b4 */ 4, /* b5 */ 5, /* b6 */ 5, /* b7 */ 6,
+            /* b8 */ 4, /* b9 */ 5, /* ba */ 5, /* bb */ 6,
+            /* bc */ 5, /* bd */ 6, /* be */ 6, /* bf */ 7,
+            /* c0 */ 2, /* c1 */ 3, /* c2 */ 3, /* c3 */ 4,
+            /* c4 */ 3, /* c5 */ 4, /* c6 */ 4, /* c7 */ 5,
+            /* c8 */ 3, /* c9 */ 4, /* ca */ 4, /* cb */ 5,
+            /* cc */ 4, /* cd */ 5, /* ce */ 5, /* cf */ 6,
+            /* d0 */ 3, /* d1 */ 4, /* d2 */ 4, /* d3 */ 5,
+            /* d4 */ 4, /* d5 */ 5, /* d6 */ 5, /* d7 */ 6,
+            /* d8 */ 4, /* d9 */ 5, /* da */ 5, /* db */ 6,
+            /* dc */ 5, /* dd */ 6, /* de */ 6, /* df */ 7,
+            /* e0 */ 3, /* e1 */ 4, /* e2 */ 4, /* e3 */ 5,
+            /* e4 */ 4, /* e5 */ 5, /* e6 */ 5, /* e7 */ 6,
+            /* e8 */ 4, /* e9 */ 5, /* ea */ 5, /* eb */ 6,
+            /* ec */ 5, /* ed */ 6, /* ee */ 6, /* ef */ 7,
+            /* f0 */ 4, /* f1 */ 5, /* f2 */ 5, /* f3 */ 6,
+            /* f4 */ 5, /* f5 */ 6, /* f6 */ 6, /* f7 */ 7,
+            /* f8 */ 5, /* f9 */ 6, /* fa */ 6, /* fb */ 7,
+            /* fc */ 6, /* fd */ 7, /* fe */ 7, /* ff */ 8
+        };
+        return table[b];
+    }
+};
+
+/**
+ * Hamming distance functor (pop count between two binary vectors, i.e. xor them and count the number of bits set)
+ * That code was taken from brief.cpp in OpenCV
+ */
+template<class T>
+struct HammingPopcnt
+{
+    typedef T ElementType;
+    typedef int ResultType;
+
+    template<typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
+    {
+        ResultType result = 0;
+#if __GNUC__
+#if ANDROID && HAVE_NEON
+        static uint64_t features = android_getCpuFeatures();
+        if ((features& ANDROID_CPU_ARM_FEATURE_NEON)) {
+            for (size_t i = 0; i < size; i += 16) {
+                uint8x16_t A_vec = vld1q_u8 (a + i);
+                uint8x16_t B_vec = vld1q_u8 (b + i);
+                //uint8x16_t veorq_u8 (uint8x16_t, uint8x16_t)
+                uint8x16_t AxorB = veorq_u8 (A_vec, B_vec);
+
+                uint8x16_t bitsSet += vcntq_u8 (AxorB);
+                //uint16x8_t vpadalq_u8 (uint16x8_t, uint8x16_t)
+                uint16x8_t bitSet8 = vpaddlq_u8 (bitsSet);
+                uint32x4_t bitSet4 = vpaddlq_u16 (bitSet8);
+
+                uint64x2_t bitSet2 = vpaddlq_u32 (bitSet4);
+                result += vgetq_lane_u64 (bitSet2,0);
+                result += vgetq_lane_u64 (bitSet2,1);
+            }
+        }
+        else
+#endif
+        //for portability just use unsigned long -- and use the __builtin_popcountll (see docs for __builtin_popcountll)
+        typedef unsigned long long pop_t;
+        const size_t modulo = size % sizeof(pop_t);
+        const pop_t* a2 = reinterpret_cast<const pop_t*> (a);
+        const pop_t* b2 = reinterpret_cast<const pop_t*> (b);
+        const pop_t* a2_end = a2 + (size / sizeof(pop_t));
+
+        for (; a2 != a2_end; ++a2, ++b2) result += __builtin_popcountll((*a2) ^ (*b2));
+
+        if (modulo) {
+            //in the case where size is not dividable by sizeof(size_t)
+            //need to mask off the bits at the end
+            pop_t a_final = 0, b_final = 0;
+            memcpy(&a_final, a2, modulo);
+            memcpy(&b_final, b2, modulo);
+            result += __builtin_popcountll(a_final ^ b_final);
+        }
+#else
+        HammingLUT lut;
+        result = lut(reinterpret_cast<const unsigned char*> (a),
+                     reinterpret_cast<const unsigned char*> (b), size * sizeof(pop_t));
+#endif
+        return result;
+    }
+};
+
+template<typename T>
+struct Hamming
+{
+    typedef T ElementType;
+    typedef unsigned int ResultType;
+
+    /** This is popcount_3() from:
+     * http://en.wikipedia.org/wiki/Hamming_weight */
+    unsigned int popcnt32(uint32_t n) const
+    {
+        n -= ((n >> 1) & 0x55555555);
+        n = (n & 0x33333333) + ((n >> 2) & 0x33333333);
+        return (((n + (n >> 4))& 0xF0F0F0F)* 0x1010101) >> 24;
+    }
+
+    unsigned int popcnt64(uint64_t n) const
+    {
+        n -= ((n >> 1) & 0x5555555555555555LL);
+        n = (n & 0x3333333333333333LL) + ((n >> 2) & 0x3333333333333333LL);
+        return (((n + (n >> 4))& 0x0f0f0f0f0f0f0f0fLL)* 0x0101010101010101LL) >> 56;
+    }
+
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = 0) const
+    {
+#ifdef FLANN_PLATFORM_64_BIT
+        const uint64_t* pa = reinterpret_cast<const uint64_t*>(a);
+        const uint64_t* pb = reinterpret_cast<const uint64_t*>(b);
+        ResultType result = 0;
+        size /= (sizeof(uint64_t)/sizeof(unsigned char));
+        for(size_t i = 0; i < size; ++i ) {
+            result += popcnt64(*pa ^ *pb);
+            ++pa;
+            ++pb;
+        }
+#else
+        const uint32_t* pa = reinterpret_cast<const uint32_t*>(a);
+        const uint32_t* pb = reinterpret_cast<const uint32_t*>(b);
+        ResultType result = 0;
+        size /= (sizeof(uint32_t)/sizeof(unsigned char));
+        for(size_t i = 0; i < size; ++i ) {
+        	result += popcnt32(*pa ^ *pb);
+        	++pa;
+        	++pb;
+        }
+#endif
+        return result;
+    }
+};
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+template<class T>
+struct HistIntersectionDistance
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    /**
+     *  Compute the histogram intersection distance
+     */
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
+    {
+        ResultType result = ResultType();
+        ResultType min0, min1, min2, min3;
+        Iterator1 last = a + size;
+        Iterator1 lastgroup = last - 3;
+
+        /* Process 4 items with each loop for efficiency. */
+        while (a < lastgroup) {
+            min0 = (ResultType)(a[0] < b[0] ? a[0] : b[0]);
+            min1 = (ResultType)(a[1] < b[1] ? a[1] : b[1]);
+            min2 = (ResultType)(a[2] < b[2] ? a[2] : b[2]);
+            min3 = (ResultType)(a[3] < b[3] ? a[3] : b[3]);
+            result += min0 + min1 + min2 + min3;
+            a += 4;
+            b += 4;
+            if ((worst_dist>0)&&(result>worst_dist)) {
+                return result;
+            }
+        }
+        /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
+        while (a < last) {
+            min0 = (ResultType)(*a < *b ? *a : *b);
+            result += min0;
+            ++a;
+            ++b;
+        }
+        return result;
+    }
+
+    /**
+     * Partial distance, used by the kd-tree.
+     */
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        return a<b ? a : b;
+    }
+};
+
+
+
+template<class T>
+struct HellingerDistance
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    /**
+     *  Compute the Hellinger distance
+     */
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
+    {
+        ResultType result = ResultType();
+        ResultType diff0, diff1, diff2, diff3;
+        Iterator1 last = a + size;
+        Iterator1 lastgroup = last - 3;
+
+        /* Process 4 items with each loop for efficiency. */
+        while (a < lastgroup) {
+            diff0 = sqrt(static_cast<ResultType>(a[0])) - sqrt(static_cast<ResultType>(b[0]));
+            diff1 = sqrt(static_cast<ResultType>(a[1])) - sqrt(static_cast<ResultType>(b[1]));
+            diff2 = sqrt(static_cast<ResultType>(a[2])) - sqrt(static_cast<ResultType>(b[2]));
+            diff3 = sqrt(static_cast<ResultType>(a[3])) - sqrt(static_cast<ResultType>(b[3]));
+            result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
+            a += 4;
+            b += 4;
+        }
+        while (a < last) {
+            diff0 = sqrt(static_cast<ResultType>(*a++)) - sqrt(static_cast<ResultType>(*b++));
+            result += diff0 * diff0;
+        }
+        return result;
+    }
+
+    /**
+     * Partial distance, used by the kd-tree.
+     */
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        ResultType dist = sqrt(static_cast<ResultType>(a)) - sqrt(static_cast<ResultType>(b));
+        return dist * dist;
+    }
+};
+
+
+template<class T>
+struct ChiSquareDistance
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    /**
+     *  Compute the chi-square distance
+     */
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
+    {
+        ResultType result = ResultType();
+        ResultType sum, diff;
+        Iterator1 last = a + size;
+
+        while (a < last) {
+            sum = (ResultType)(*a + *b);
+            if (sum>0) {
+                diff = (ResultType)(*a - *b);
+                result += diff*diff/sum;
+            }
+            ++a;
+            ++b;
+
+            if ((worst_dist>0)&&(result>worst_dist)) {
+                return result;
+            }
+        }
+        return result;
+    }
+
+    /**
+     * Partial distance, used by the kd-tree.
+     */
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        ResultType result = ResultType();
+        ResultType sum, diff;
+
+        sum = (ResultType)(a+b);
+        if (sum>0) {
+            diff = (ResultType)(a-b);
+            result = diff*diff/sum;
+        }
+        return result;
+    }
+};
+
+
+template<class T>
+struct KL_Divergence
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    /**
+     *  Compute the Kullback–Leibler divergence
+     */
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
+    {
+        ResultType result = ResultType();
+        Iterator1 last = a + size;
+
+        while (a < last) {
+            if ( *a != 0 && *b != 0 ) {
+                ResultType ratio = (ResultType)(*a / *b);
+                if (ratio>0) {
+                    result += *a * log(ratio);
+                }
+            }
+            ++a;
+            ++b;
+
+            if ((worst_dist>0)&&(result>worst_dist)) {
+                return result;
+            }
+        }
+        return result;
+    }
+
+    /**
+     * Partial distance, used by the kd-tree.
+     */
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        ResultType result = ResultType();
+        if( a != 0 && b != 0 ) {
+            ResultType ratio = (ResultType)(a / b);
+            if (ratio>0) {
+                result = a * log(ratio);
+            }
+		}
+        return result;
+    }
+};
+
+}
+
+#endif //FLANN_DIST_H_

corelib/src/rtflann/algorithms/hierarchical_clustering_index.h

@@ -0,0 +1,724 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2011  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2011  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_HIERARCHICAL_CLUSTERING_INDEX_H_
+#define RTABMAP_FLANN_HIERARCHICAL_CLUSTERING_INDEX_H_
+
+#include <algorithm>
+#include <string>
+#include <map>
+#include <cassert>
+#include <limits>
+#include <cmath>
+
+#ifndef SIZE_MAX
+#define SIZE_MAX ((size_t) -1)
+#endif
+
+#include "rtflann/general.h"
+#include "rtflann/algorithms/nn_index.h"
+#include "rtflann/algorithms/dist.h"
+#include "rtflann/util/matrix.h"
+#include "rtflann/util/result_set.h"
+#include "rtflann/util/heap.h"
+#include "rtflann/util/allocator.h"
+#include "rtflann/util/random.h"
+#include "rtflann/util/saving.h"
+#include "rtflann/util/serialization.h"
+
+namespace rtflann
+{
+
+struct HierarchicalClusteringIndexParams : public IndexParams
+{
+    HierarchicalClusteringIndexParams(int branching = 32,
+                                      flann_centers_init_t centers_init = FLANN_CENTERS_RANDOM,
+                                      int trees = 4, int leaf_max_size = 100)
+    {
+        (*this)["algorithm"] = FLANN_INDEX_HIERARCHICAL;
+        // The branching factor used in the hierarchical clustering
+        (*this)["branching"] = branching;
+        // Algorithm used for picking the initial cluster centers
+        (*this)["centers_init"] = centers_init;
+        // number of parallel trees to build
+        (*this)["trees"] = trees;
+        // maximum leaf size
+        (*this)["leaf_max_size"] = leaf_max_size;
+    }
+};
+
+
+
+/**
+ * Hierarchical index
+ *
+ * Contains a tree constructed through a hierarchical clustering
+ * and other information for indexing a set of points for nearest-neighbour matching.
+ */
+template <typename Distance>
+class HierarchicalClusteringIndex : public NNIndex<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    typedef NNIndex<Distance> BaseClass;
+
+    /**
+     * Constructor.
+     *
+     * @param index_params
+     * @param d
+     */
+    HierarchicalClusteringIndex(const IndexParams& index_params = HierarchicalClusteringIndexParams(), Distance d = Distance())
+        : BaseClass(index_params, d)
+    {
+        memoryCounter_ = 0;
+
+        branching_ = get_param(index_params_,"branching",32);
+        centers_init_ = get_param(index_params_,"centers_init", FLANN_CENTERS_RANDOM);
+        trees_ = get_param(index_params_,"trees",4);
+        leaf_max_size_ = get_param(index_params_,"leaf_max_size",100);
+
+        initCenterChooser();
+    }
+
+
+    /**
+     * Index constructor
+     *
+     * Params:
+     *          inputData = dataset with the input features
+     *          params = parameters passed to the hierarchical k-means algorithm
+     */
+    HierarchicalClusteringIndex(const Matrix<ElementType>& inputData, const IndexParams& index_params = HierarchicalClusteringIndexParams(),
+                                Distance d = Distance())
+        : BaseClass(index_params, d)
+    {
+        memoryCounter_ = 0;
+
+        branching_ = get_param(index_params_,"branching",32);
+        centers_init_ = get_param(index_params_,"centers_init", FLANN_CENTERS_RANDOM);
+        trees_ = get_param(index_params_,"trees",4);
+        leaf_max_size_ = get_param(index_params_,"leaf_max_size",100);
+
+        initCenterChooser();
+        
+        setDataset(inputData);
+
+        chooseCenters_->setDataSize(veclen_);
+    }
+
+
+    HierarchicalClusteringIndex(const HierarchicalClusteringIndex& other) : BaseClass(other),
+    		memoryCounter_(other.memoryCounter_),
+    		branching_(other.branching_),
+    		trees_(other.trees_),
+    		centers_init_(other.centers_init_),
+    		leaf_max_size_(other.leaf_max_size_)
+
+    {
+    	initCenterChooser();
+        tree_roots_.resize(other.tree_roots_.size());
+        for (size_t i=0;i<tree_roots_.size();++i) {
+        	copyTree(tree_roots_[i], other.tree_roots_[i]);
+        }
+    }
+
+    HierarchicalClusteringIndex& operator=(HierarchicalClusteringIndex other)
+    {
+    	this->swap(other);
+    	return *this;
+    }
+
+
+    void initCenterChooser()
+    {
+        switch(centers_init_) {
+        case FLANN_CENTERS_RANDOM:
+        	chooseCenters_ = new RandomCenterChooser<Distance>(distance_, points_);
+        	break;
+        case FLANN_CENTERS_GONZALES:
+        	chooseCenters_ = new GonzalesCenterChooser<Distance>(distance_, points_);
+        	break;
+        case FLANN_CENTERS_KMEANSPP:
+            chooseCenters_ = new KMeansppCenterChooser<Distance>(distance_, points_);
+        	break;
+        case FLANN_CENTERS_GROUPWISE:
+            chooseCenters_ = new GroupWiseCenterChooser<Distance>(distance_, points_);
+            break;
+        default:
+            throw FLANNException("Unknown algorithm for choosing initial centers.");
+        }
+    }
+
+    /**
+     * Index destructor.
+     *
+     * Release the memory used by the index.
+     */
+    virtual ~HierarchicalClusteringIndex()
+    {
+    	delete chooseCenters_;
+    	freeIndex();
+    }
+
+    BaseClass* clone() const
+    {
+    	return new HierarchicalClusteringIndex(*this);
+    }
+
+    /**
+     * Computes the inde memory usage
+     * Returns: memory used by the index
+     */
+    int usedMemory() const
+    {
+        return pool_.usedMemory+pool_.wastedMemory+memoryCounter_;
+    }
+    
+    using BaseClass::buildIndex;
+
+    void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        assert(points.cols==veclen_);
+        size_t old_size = size_;
+
+        extendDataset(points);
+        
+        if (rebuild_threshold>1 && size_at_build_*rebuild_threshold<size_) {
+            buildIndex();
+        }
+        else {
+            for (size_t i=0;i<points.rows;++i) {
+                for (int j = 0; j < trees_; j++) {
+                    addPointToTree(tree_roots_[j], old_size + i);
+                }
+            }            
+        }
+    }
+
+
+    flann_algorithm_t getType() const
+    {
+        return FLANN_INDEX_HIERARCHICAL;
+    }
+
+
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+    	ar.setObject(this);
+
+    	ar & *static_cast<NNIndex<Distance>*>(this);
+
+    	ar & branching_;
+    	ar & trees_;
+    	ar & centers_init_;
+    	ar & leaf_max_size_;
+
+    	if (Archive::is_loading::value) {
+    		tree_roots_.resize(trees_);
+    	}
+    	for (size_t i=0;i<tree_roots_.size();++i) {
+    		if (Archive::is_loading::value) {
+    			tree_roots_[i] = new(pool_) Node();
+    		}
+    		ar & *tree_roots_[i];
+    	}
+
+    	if (Archive::is_loading::value) {
+            index_params_["algorithm"] = getType();
+            index_params_["branching"] = branching_;
+            index_params_["trees"] = trees_;
+            index_params_["centers_init"] = centers_init_;
+            index_params_["leaf_size"] = leaf_max_size_;
+    	}
+    }
+
+    void saveIndex(FILE* stream)
+    {
+    	serialization::SaveArchive sa(stream);
+    	sa & *this;
+    }
+
+
+    void loadIndex(FILE* stream)
+    {
+    	serialization::LoadArchive la(stream);
+    	la & *this;
+    }
+
+
+    /**
+     * Find set of nearest neighbors to vec. Their indices are stored inside
+     * the result object.
+     *
+     * Params:
+     *     result = the result object in which the indices of the nearest-neighbors are stored
+     *     vec = the vector for which to search the nearest neighbors
+     *     searchParams = parameters that influence the search algorithm (checks)
+     */
+
+    void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
+    {
+    	if (removed_) {
+    		findNeighborsWithRemoved<true>(result, vec, searchParams);
+    	}
+    	else {
+    		findNeighborsWithRemoved<false>(result, vec, searchParams);
+    	}
+    }
+
+protected:
+
+    /**
+     * Builds the index
+     */
+    void buildIndexImpl()
+    {
+        chooseCenters_->setDataSize(veclen_);
+
+        if (branching_<2) {
+            throw FLANNException("Branching factor must be at least 2");
+        }
+        tree_roots_.resize(trees_);
+        std::vector<int> indices(size_);
+        for (int i=0; i<trees_; ++i) {
+            for (size_t j=0; j<size_; ++j) {
+                indices[j] = j;
+            }
+            tree_roots_[i] = new(pool_) Node();
+            computeClustering(tree_roots_[i], &indices[0], size_);
+        }
+    }
+
+private:
+
+    struct PointInfo
+    {
+    	/** Point index */
+    	size_t index;
+    	/** Point data */
+    	ElementType* point;
+
+    private:
+    	template<typename Archive>
+    	void serialize(Archive& ar)
+    	{
+    		typedef HierarchicalClusteringIndex<Distance> Index;
+    		Index* obj = static_cast<Index*>(ar.getObject());
+
+    		ar & index;
+//    		ar & point;
+
+			if (Archive::is_loading::value) {
+				point = obj->points_[index];
+			}
+    	}
+    	friend struct serialization::access;
+    };
+
+    /**
+     * Struture representing a node in the hierarchical k-means tree.
+     */
+    struct Node
+    {
+        /**
+         * The cluster center
+         */
+    	ElementType* pivot;
+    	size_t pivot_index;
+        /**
+         * Child nodes (only for non-terminal nodes)
+         */
+        std::vector<Node*> childs;
+        /**
+         * Node points (only for terminal nodes)
+         */
+        std::vector<PointInfo> points;
+
+		Node(){
+			pivot = NULL;
+			pivot_index = SIZE_MAX;
+		}
+        /**
+         * destructor
+         * calling Node destructor explicitly
+         */
+        ~Node()
+        {
+        	for(size_t i=0; i<childs.size(); i++){
+        		childs[i]->~Node();
+				pivot = NULL;
+				pivot_index = -1;
+        	}
+        };
+
+    private:
+    	template<typename Archive>
+    	void serialize(Archive& ar)
+    	{
+    		typedef HierarchicalClusteringIndex<Distance> Index;
+    		Index* obj = static_cast<Index*>(ar.getObject());
+    		ar & pivot_index;
+    		if (Archive::is_loading::value) {
+				if (pivot_index != SIZE_MAX)
+					pivot = obj->points_[pivot_index];
+				else
+					pivot = NULL;
+    		}
+    		size_t childs_size;
+    		if (Archive::is_saving::value) {
+    			childs_size = childs.size();
+    		}
+    		ar & childs_size;
+
+    		if (childs_size==0) {
+    			ar & points;
+    		}
+    		else {
+    			if (Archive::is_loading::value) {
+    				childs.resize(childs_size);
+    			}
+    			for (size_t i=0;i<childs_size;++i) {
+    				if (Archive::is_loading::value) {
+    					childs[i] = new(obj->pool_) Node();
+    				}
+    				ar & *childs[i];
+    			}
+    		}
+
+    	}
+    	friend struct serialization::access;
+    };
+    typedef Node* NodePtr;
+
+
+
+    /**
+     * Alias definition for a nicer syntax.
+     */
+    typedef BranchStruct<NodePtr, DistanceType> BranchSt;
+
+
+    /**
+     * Clears Node tree
+     * calling Node destructor explicitly
+     */
+    void freeIndex(){
+    	for (size_t i=0; i<tree_roots_.size(); ++i) {
+    		tree_roots_[i]->~Node();
+    	}
+    	pool_.free();
+    }
+
+    void copyTree(NodePtr& dst, const NodePtr& src)
+    {
+    	dst = new(pool_) Node();
+    	dst->pivot_index = src->pivot_index;
+    	dst->pivot = points_[dst->pivot_index];
+
+    	if (src->childs.size()==0) {
+    		dst->points = src->points;
+    	}
+    	else {
+    		dst->childs.resize(src->childs.size());
+    		for (size_t i=0;i<src->childs.size();++i) {
+    			copyTree(dst->childs[i], src->childs[i]);
+    		}
+    	}
+    }
+
+
+
+    void computeLabels(int* indices, int indices_length,  int* centers, int centers_length, int* labels, DistanceType& cost)
+    {
+        cost = 0;
+        for (int i=0; i<indices_length; ++i) {
+            ElementType* point = points_[indices[i]];
+            DistanceType dist = distance_(point, points_[centers[0]], veclen_);
+            labels[i] = 0;
+            for (int j=1; j<centers_length; ++j) {
+                DistanceType new_dist = distance_(point, points_[centers[j]], veclen_);
+                if (dist>new_dist) {
+                    labels[i] = j;
+                    dist = new_dist;
+                }
+            }
+            cost += dist;
+        }
+    }
+
+    /**
+     * The method responsible with actually doing the recursive hierarchical
+     * clustering
+     *
+     * Params:
+     *     node = the node to cluster
+     *     indices = indices of the points belonging to the current node
+     *     branching = the branching factor to use in the clustering
+     *
+     */
+    void computeClustering(NodePtr node, int* indices, int indices_length)
+    {
+        if (indices_length < leaf_max_size_) { // leaf node
+            node->points.resize(indices_length);
+            for (int i=0;i<indices_length;++i) {
+            	node->points[i].index = indices[i];
+            	node->points[i].point = points_[indices[i]];
+            }
+            node->childs.clear();
+            return;
+        }
+
+        std::vector<int> centers(branching_);
+        std::vector<int> labels(indices_length);
+
+        int centers_length;
+        (*chooseCenters_)(branching_, indices, indices_length, &centers[0], centers_length);
+
+        if (centers_length<branching_) {
+            node->points.resize(indices_length);
+            for (int i=0;i<indices_length;++i) {
+            	node->points[i].index = indices[i];
+            	node->points[i].point = points_[indices[i]];
+            }
+            node->childs.clear();
+            return;
+        }
+
+
+        //  assign points to clusters
+        DistanceType cost;
+        computeLabels(indices, indices_length, &centers[0], centers_length, &labels[0], cost);
+
+        node->childs.resize(branching_);
+        int start = 0;
+        int end = start;
+        for (int i=0; i<branching_; ++i) {
+            for (int j=0; j<indices_length; ++j) {
+                if (labels[j]==i) {
+                    std::swap(indices[j],indices[end]);
+                    std::swap(labels[j],labels[end]);
+                    end++;
+                }
+            }
+
+            node->childs[i] = new(pool_) Node();
+            node->childs[i]->pivot_index = centers[i];
+            node->childs[i]->pivot = points_[centers[i]];
+            node->childs[i]->points.clear();
+            computeClustering(node->childs[i],indices+start, end-start);
+            start=end;
+        }
+    }
+
+
+    template<bool with_removed>
+    void findNeighborsWithRemoved(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
+    {
+        int maxChecks = searchParams.checks;
+
+        // Priority queue storing intermediate branches in the best-bin-first search
+        Heap<BranchSt>* heap = new Heap<BranchSt>(size_);
+
+        DynamicBitset checked(size_);
+        int checks = 0;
+        for (int i=0; i<trees_; ++i) {
+            findNN<with_removed>(tree_roots_[i], result, vec, checks, maxChecks, heap, checked);
+        }
+
+        BranchSt branch;
+        while (heap->popMin(branch) && (checks<maxChecks || !result.full())) {
+            NodePtr node = branch.node;
+            findNN<with_removed>(node, result, vec, checks, maxChecks, heap, checked);
+        }
+
+        delete heap;
+    }
+
+
+    /**
+     * Performs one descent in the hierarchical k-means tree. The branches not
+     * visited are stored in a priority queue.
+     *
+     * Params:
+     *      node = node to explore
+     *      result = container for the k-nearest neighbors found
+     *      vec = query points
+     *      checks = how many points in the dataset have been checked so far
+     *      maxChecks = maximum dataset points to checks
+     */
+
+    template<bool with_removed>
+    void findNN(NodePtr node, ResultSet<DistanceType>& result, const ElementType* vec, int& checks, int maxChecks,
+                Heap<BranchSt>* heap,  DynamicBitset& checked) const
+    {
+        if (node->childs.empty()) {
+            if (checks>=maxChecks) {
+                if (result.full()) return;
+            }
+
+            for (size_t i=0; i<node->points.size(); ++i) {
+            	PointInfo& pointInfo = node->points[i];
+            	if (with_removed) {
+            		if (removed_points_.test(pointInfo.index)) continue;
+            	}
+                if (checked.test(pointInfo.index)) continue;
+                DistanceType dist = distance_(pointInfo.point, vec, veclen_);
+                result.addPoint(dist, pointInfo.index);
+                checked.set(pointInfo.index);
+                ++checks;
+            }
+        }
+        else {
+            DistanceType* domain_distances = new DistanceType[branching_];
+            int best_index = 0;
+            domain_distances[best_index] = distance_(vec, node->childs[best_index]->pivot, veclen_);
+            for (int i=1; i<branching_; ++i) {
+                domain_distances[i] = distance_(vec, node->childs[i]->pivot, veclen_);
+                if (domain_distances[i]<domain_distances[best_index]) {
+                    best_index = i;
+                }
+            }
+            for (int i=0; i<branching_; ++i) {
+                if (i!=best_index) {
+                    heap->insert(BranchSt(node->childs[i],domain_distances[i]));
+                }
+            }
+            delete[] domain_distances;
+            findNN<with_removed>(node->childs[best_index],result,vec, checks, maxChecks, heap, checked);
+        }
+    }
+    
+    void addPointToTree(NodePtr node, size_t index)
+    {
+        ElementType* point = points_[index];
+        
+        if (node->childs.empty()) { // leaf node
+        	PointInfo pointInfo;
+        	pointInfo.point = point;
+        	pointInfo.index = index;
+            node->points.push_back(pointInfo);
+
+            if (node->points.size()>=size_t(branching_)) {
+                std::vector<int> indices(node->points.size());
+
+                for (size_t i=0;i<node->points.size();++i) {
+                	indices[i] = node->points[i].index;
+                }
+                computeClustering(node, &indices[0], indices.size());
+            }
+        }
+        else {            
+            // find the closest child
+            int closest = 0;
+            ElementType* center = node->childs[closest]->pivot;
+            DistanceType dist = distance_(center, point, veclen_);
+            for (size_t i=1;i<size_t(branching_);++i) {
+                center = node->childs[i]->pivot;
+                DistanceType crt_dist = distance_(center, point, veclen_);
+                if (crt_dist<dist) {
+                    dist = crt_dist;
+                    closest = i;
+                }
+            }
+            addPointToTree(node->childs[closest], index);
+        }                
+    }
+
+    void swap(HierarchicalClusteringIndex& other)
+    {
+    	BaseClass::swap(other);
+
+    	std::swap(tree_roots_, other.tree_roots_);
+    	std::swap(pool_, other.pool_);
+    	std::swap(memoryCounter_, other.memoryCounter_);
+    	std::swap(branching_, other.branching_);
+    	std::swap(trees_, other.trees_);
+    	std::swap(centers_init_, other.centers_init_);
+    	std::swap(leaf_max_size_, other.leaf_max_size_);
+    	std::swap(chooseCenters_, other.chooseCenters_);
+    }
+
+private:
+
+    /**
+     * The root nodes in the tree.
+     */
+    std::vector<Node*> tree_roots_;
+
+    /**
+     * Pooled memory allocator.
+     *
+     * Using a pooled memory allocator is more efficient
+     * than allocating memory directly when there is a large
+     * number small of memory allocations.
+     */
+    PooledAllocator pool_;
+
+    /**
+     * Memory occupied by the index.
+     */
+    int memoryCounter_;
+
+    /** index parameters */
+    /**
+     * Branching factor to use for clustering
+     */
+    int branching_;
+    
+    /**
+     * How many parallel trees to build
+     */
+    int trees_;
+    
+    /**
+     * Algorithm to use for choosing cluster centers
+     */
+    flann_centers_init_t centers_init_;
+    
+    /**
+     * Max size of leaf nodes
+     */
+    int leaf_max_size_;
+    
+    /**
+     * Algorithm used to choose initial centers
+     */
+    CenterChooser<Distance>* chooseCenters_;
+
+    USING_BASECLASS_SYMBOLS
+};
+
+}
+
+#endif /* FLANN_HIERARCHICAL_CLUSTERING_INDEX_H_ */

corelib/src/rtflann/algorithms/kdtree_index.h

@@ -0,0 +1,765 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_KDTREE_INDEX_H_
+#define RTABMAP_FLANN_KDTREE_INDEX_H_
+
+#include <algorithm>
+#include <map>
+#include <cassert>
+#include <cstring>
+#include <stdarg.h>
+#include <cmath>
+
+#include "rtflann/general.h"
+#include "rtflann/algorithms/nn_index.h"
+#include "rtflann/util/dynamic_bitset.h"
+#include "rtflann/util/matrix.h"
+#include "rtflann/util/result_set.h"
+#include "rtflann/util/heap.h"
+#include "rtflann/util/allocator.h"
+#include "rtflann/util/random.h"
+#include "rtflann/util/saving.h"
+
+
+namespace rtflann
+{
+
+struct KDTreeIndexParams : public IndexParams
+{
+    KDTreeIndexParams(int trees = 4)
+    {
+        (*this)["algorithm"] = FLANN_INDEX_KDTREE;
+        (*this)["trees"] = trees;
+    }
+};
+
+
+/**
+ * Randomized kd-tree index
+ *
+ * Contains the k-d trees and other information for indexing a set of points
+ * for nearest-neighbor matching.
+ */
+template <typename Distance>
+class KDTreeIndex : public NNIndex<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    typedef NNIndex<Distance> BaseClass;
+
+    typedef bool needs_kdtree_distance;
+
+
+    /**
+     * KDTree constructor
+     *
+     * Params:
+     *          inputData = dataset with the input features
+     *          params = parameters passed to the kdtree algorithm
+     */
+    KDTreeIndex(const IndexParams& params = KDTreeIndexParams(), Distance d = Distance() ) :
+    	BaseClass(params, d), mean_(NULL), var_(NULL)
+    {
+        trees_ = get_param(index_params_,"trees",4);
+    }
+
+
+    /**
+     * KDTree constructor
+     *
+     * Params:
+     *          inputData = dataset with the input features
+     *          params = parameters passed to the kdtree algorithm
+     */
+    KDTreeIndex(const Matrix<ElementType>& dataset, const IndexParams& params = KDTreeIndexParams(),
+                Distance d = Distance() ) : BaseClass(params,d ), mean_(NULL), var_(NULL)
+    {
+        trees_ = get_param(index_params_,"trees",4);
+
+        setDataset(dataset);
+    }
+
+    KDTreeIndex(const KDTreeIndex& other) : BaseClass(other),
+    		trees_(other.trees_)
+    {
+        tree_roots_.resize(other.tree_roots_.size());
+        for (size_t i=0;i<tree_roots_.size();++i) {
+        	copyTree(tree_roots_[i], other.tree_roots_[i]);
+        }
+    }
+
+    KDTreeIndex& operator=(KDTreeIndex other)
+    {
+    	this->swap(other);
+    	return *this;
+    }
+
+    /**
+     * Standard destructor
+     */
+    virtual ~KDTreeIndex()
+    {
+    	freeIndex();
+    }
+
+    BaseClass* clone() const
+    {
+    	return new KDTreeIndex(*this);
+    }
+
+    using BaseClass::buildIndex;
+    
+    void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        assert(points.cols==veclen_);
+
+        size_t old_size = size_;
+        extendDataset(points);
+        
+        if (rebuild_threshold>1 && size_at_build_*rebuild_threshold<size_) {
+            buildIndex();
+        }
+        else {
+            for (size_t i=old_size;i<size_;++i) {
+                for (int j = 0; j < trees_; j++) {
+                    addPointToTree(tree_roots_[j], i);
+                }
+            }
+        }        
+    }
+
+    flann_algorithm_t getType() const
+    {
+        return FLANN_INDEX_KDTREE;
+    }
+
+
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+    	ar.setObject(this);
+
+    	ar & *static_cast<NNIndex<Distance>*>(this);
+
+    	ar & trees_;
+
+    	if (Archive::is_loading::value) {
+    		tree_roots_.resize(trees_);
+    	}
+    	for (size_t i=0;i<tree_roots_.size();++i) {
+    		if (Archive::is_loading::value) {
+    			tree_roots_[i] = new(pool_) Node();
+    		}
+    		ar & *tree_roots_[i];
+    	}
+
+    	if (Archive::is_loading::value) {
+            index_params_["algorithm"] = getType();
+            index_params_["trees"] = trees_;
+    	}
+    }
+
+
+    void saveIndex(FILE* stream)
+    {
+    	serialization::SaveArchive sa(stream);
+    	sa & *this;
+    }
+
+
+    void loadIndex(FILE* stream)
+    {
+    	freeIndex();
+    	serialization::LoadArchive la(stream);
+    	la & *this;
+    }
+
+    /**
+     * Computes the inde memory usage
+     * Returns: memory used by the index
+     */
+    int usedMemory() const
+    {
+        return int(pool_.usedMemory+pool_.wastedMemory+size_*sizeof(int));  // pool memory and vind array memory
+    }
+
+    /**
+     * Find set of nearest neighbors to vec. Their indices are stored inside
+     * the result object.
+     *
+     * Params:
+     *     result = the result object in which the indices of the nearest-neighbors are stored
+     *     vec = the vector for which to search the nearest neighbors
+     *     maxCheck = the maximum number of restarts (in a best-bin-first manner)
+     */
+    void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
+    {
+        int maxChecks = searchParams.checks;
+        float epsError = 1+searchParams.eps;
+
+        if (maxChecks==FLANN_CHECKS_UNLIMITED) {
+        	if (removed_) {
+        		getExactNeighbors<true>(result, vec, epsError);
+        	}
+        	else {
+        		getExactNeighbors<false>(result, vec, epsError);
+        	}
+        }
+        else {
+        	if (removed_) {
+        		getNeighbors<true>(result, vec, maxChecks, epsError);
+        	}
+        	else {
+        		getNeighbors<false>(result, vec, maxChecks, epsError);
+        	}
+        }
+    }
+
+protected:
+
+    /**
+     * Builds the index
+     */
+    void buildIndexImpl()
+    {
+        // Create a permutable array of indices to the input vectors.
+    	std::vector<int> ind(size_);
+        for (size_t i = 0; i < size_; ++i) {
+            ind[i] = int(i);
+        }
+
+        mean_ = new DistanceType[veclen_];
+        var_ = new DistanceType[veclen_];
+
+        tree_roots_.resize(trees_);
+        /* Construct the randomized trees. */
+        for (int i = 0; i < trees_; i++) {
+            /* Randomize the order of vectors to allow for unbiased sampling. */
+            std::random_shuffle(ind.begin(), ind.end());
+            tree_roots_[i] = divideTree(&ind[0], int(size_) );
+        }
+        delete[] mean_;
+        delete[] var_;
+    }
+
+    void freeIndex()
+    {
+    	for (size_t i=0;i<tree_roots_.size();++i) {
+    		// using placement new, so call destructor explicitly
+    		if (tree_roots_[i]!=NULL) tree_roots_[i]->~Node();
+    	}
+    	pool_.free();
+    }
+
+
+private:
+
+    /*--------------------- Internal Data Structures --------------------------*/
+    struct Node
+    {
+    	/**
+         * Dimension used for subdivision.
+         */
+        int divfeat;
+        /**
+         * The values used for subdivision.
+         */
+        DistanceType divval;
+        /**
+         * Point data
+         */
+        ElementType* point;
+		/**
+		* The child nodes.
+		*/
+		Node* child1, *child2;
+		Node(){
+			child1 = NULL;
+			child2 = NULL;
+		}
+		~Node() {
+			if (child1 != NULL) { child1->~Node(); child1 = NULL; }
+
+			if (child2 != NULL) { child2->~Node(); child2 = NULL; }
+		}
+
+    private:
+    	template<typename Archive>
+    	void serialize(Archive& ar)
+    	{
+    		typedef KDTreeIndex<Distance> Index;
+    		Index* obj = static_cast<Index*>(ar.getObject());
+
+    		ar & divfeat;
+    		ar & divval;
+
+    		bool leaf_node = false;
+    		if (Archive::is_saving::value) {
+    			leaf_node = ((child1==NULL) && (child2==NULL));
+    		}
+    		ar & leaf_node;
+
+    		if (leaf_node) {
+    			if (Archive::is_loading::value) {
+    				point = obj->points_[divfeat];
+    			}
+    		}
+
+    		if (!leaf_node) {
+				if (Archive::is_loading::value) {
+					child1 = new(obj->pool_) Node();
+					child2 = new(obj->pool_) Node();
+				}
+    			ar & *child1;
+    			ar & *child2;
+    		}
+    	}
+    	friend struct serialization::access;
+    };
+    typedef Node* NodePtr;
+    typedef BranchStruct<NodePtr, DistanceType> BranchSt;
+    typedef BranchSt* Branch;
+
+
+    void copyTree(NodePtr& dst, const NodePtr& src)
+    {
+    	dst = new(pool_) Node();
+    	dst->divfeat = src->divfeat;
+    	dst->divval = src->divval;
+    	if (src->child1==NULL && src->child2==NULL) {
+    		dst->point = points_[dst->divfeat];
+    		dst->child1 = NULL;
+    		dst->child2 = NULL;
+    	}
+    	else {
+    		copyTree(dst->child1, src->child1);
+    		copyTree(dst->child2, src->child2);
+    	}
+    }
+
+    /**
+     * Create a tree node that subdivides the list of vecs from vind[first]
+     * to vind[last].  The routine is called recursively on each sublist.
+     * Place a pointer to this new tree node in the location pTree.
+     *
+     * Params: pTree = the new node to create
+     *                  first = index of the first vector
+     *                  last = index of the last vector
+     */
+    NodePtr divideTree(int* ind, int count)
+    {
+        NodePtr node = new(pool_) Node(); // allocate memory
+
+        /* If too few exemplars remain, then make this a leaf node. */
+        if (count == 1) {
+            node->child1 = node->child2 = NULL;    /* Mark as leaf node. */
+            node->divfeat = *ind;    /* Store index of this vec. */
+            node->point = points_[*ind];
+        }
+        else {
+            int idx;
+            int cutfeat;
+            DistanceType cutval;
+            meanSplit(ind, count, idx, cutfeat, cutval);
+
+            node->divfeat = cutfeat;
+            node->divval = cutval;
+            node->child1 = divideTree(ind, idx);
+            node->child2 = divideTree(ind+idx, count-idx);
+        }
+
+        return node;
+    }
+
+
+    /**
+     * Choose which feature to use in order to subdivide this set of vectors.
+     * Make a random choice among those with the highest variance, and use
+     * its variance as the threshold value.
+     */
+    void meanSplit(int* ind, int count, int& index, int& cutfeat, DistanceType& cutval)
+    {
+        memset(mean_,0,veclen_*sizeof(DistanceType));
+        memset(var_,0,veclen_*sizeof(DistanceType));
+
+        /* Compute mean values.  Only the first SAMPLE_MEAN values need to be
+            sampled to get a good estimate.
+         */
+        int cnt = std::min((int)SAMPLE_MEAN+1, count);
+        for (int j = 0; j < cnt; ++j) {
+            ElementType* v = points_[ind[j]];
+            for (size_t k=0; k<veclen_; ++k) {
+                mean_[k] += v[k];
+            }
+        }
+        DistanceType div_factor = DistanceType(1)/cnt;
+        for (size_t k=0; k<veclen_; ++k) {
+            mean_[k] *= div_factor;
+        }
+
+        /* Compute variances (no need to divide by count). */
+        for (int j = 0; j < cnt; ++j) {
+            ElementType* v = points_[ind[j]];
+            for (size_t k=0; k<veclen_; ++k) {
+                DistanceType dist = v[k] - mean_[k];
+                var_[k] += dist * dist;
+            }
+        }
+        /* Select one of the highest variance indices at random. */
+        cutfeat = selectDivision(var_);
+        cutval = mean_[cutfeat];
+
+        int lim1, lim2;
+        planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
+
+        if (lim1>count/2) index = lim1;
+        else if (lim2<count/2) index = lim2;
+        else index = count/2;
+
+        /* If either list is empty, it means that all remaining features
+         * are identical. Split in the middle to maintain a balanced tree.
+         */
+        if ((lim1==count)||(lim2==0)) index = count/2;
+    }
+
+
+    /**
+     * Select the top RAND_DIM largest values from v and return the index of
+     * one of these selected at random.
+     */
+    int selectDivision(DistanceType* v)
+    {
+        int num = 0;
+        size_t topind[RAND_DIM];
+
+        /* Create a list of the indices of the top RAND_DIM values. */
+        for (size_t i = 0; i < veclen_; ++i) {
+            if ((num < RAND_DIM)||(v[i] > v[topind[num-1]])) {
+                /* Put this element at end of topind. */
+                if (num < RAND_DIM) {
+                    topind[num++] = i;            /* Add to list. */
+                }
+                else {
+                    topind[num-1] = i;         /* Replace last element. */
+                }
+                /* Bubble end value down to right location by repeated swapping. */
+                int j = num - 1;
+                while (j > 0  &&  v[topind[j]] > v[topind[j-1]]) {
+                    std::swap(topind[j], topind[j-1]);
+                    --j;
+                }
+            }
+        }
+        /* Select a random integer in range [0,num-1], and return that index. */
+        int rnd = rand_int(num);
+        return (int)topind[rnd];
+    }
+
+
+    /**
+     *  Subdivide the list of points by a plane perpendicular on axe corresponding
+     *  to the 'cutfeat' dimension at 'cutval' position.
+     *
+     *  On return:
+     *  dataset[ind[0..lim1-1]][cutfeat]<cutval
+     *  dataset[ind[lim1..lim2-1]][cutfeat]==cutval
+     *  dataset[ind[lim2..count]][cutfeat]>cutval
+     */
+    void planeSplit(int* ind, int count, int cutfeat, DistanceType cutval, int& lim1, int& lim2)
+    {
+        /* Move vector indices for left subtree to front of list. */
+        int left = 0;
+        int right = count-1;
+        for (;; ) {
+            while (left<=right && points_[ind[left]][cutfeat]<cutval) ++left;
+            while (left<=right && points_[ind[right]][cutfeat]>=cutval) --right;
+            if (left>right) break;
+            std::swap(ind[left], ind[right]); ++left; --right;
+        }
+        lim1 = left;
+        right = count-1;
+        for (;; ) {
+            while (left<=right && points_[ind[left]][cutfeat]<=cutval) ++left;
+            while (left<=right && points_[ind[right]][cutfeat]>cutval) --right;
+            if (left>right) break;
+            std::swap(ind[left], ind[right]); ++left; --right;
+        }
+        lim2 = left;
+    }
+
+    /**
+     * Performs an exact nearest neighbor search. The exact search performs a full
+     * traversal of the tree.
+     */
+    template<bool with_removed>
+    void getExactNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, float epsError) const
+    {
+        //		checkID -= 1;  /* Set a different unique ID for each search. */
+
+        if (trees_ > 1) {
+            fprintf(stderr,"It doesn't make any sense to use more than one tree for exact search");
+        }
+        if (trees_>0) {
+            searchLevelExact<with_removed>(result, vec, tree_roots_[0], 0.0, epsError);
+        }
+    }
+
+    /**
+     * Performs the approximate nearest-neighbor search. The search is approximate
+     * because the tree traversal is abandoned after a given number of descends in
+     * the tree.
+     */
+    template<bool with_removed>
+    void getNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, int maxCheck, float epsError) const
+    {
+        int i;
+        BranchSt branch;
+
+        int checkCount = 0;
+        Heap<BranchSt>* heap = new Heap<BranchSt>((int)size_);
+        DynamicBitset checked(size_);
+
+        /* Search once through each tree down to root. */
+        for (i = 0; i < trees_; ++i) {
+            searchLevel<with_removed>(result, vec, tree_roots_[i], 0, checkCount, maxCheck, epsError, heap, checked);
+        }
+
+        /* Keep searching other branches from heap until finished. */
+        while ( heap->popMin(branch) && (checkCount < maxCheck || !result.full() )) {
+            searchLevel<with_removed>(result, vec, branch.node, branch.mindist, checkCount, maxCheck, epsError, heap, checked);
+        }
+
+        delete heap;
+
+    }
+
+    /**
+     *  Search starting from a given node of the tree.  Based on any mismatches at
+     *  higher levels, all exemplars below this level must have a distance of
+     *  at least "mindistsq".
+     */
+    template<bool with_removed>
+    void searchLevel(ResultSet<DistanceType>& result_set, const ElementType* vec, NodePtr node, DistanceType mindist, int& checkCount, int maxCheck,
+                     float epsError, Heap<BranchSt>* heap, DynamicBitset& checked) const
+    {
+        if (result_set.worstDist()<mindist) {
+            //			printf("Ignoring branch, too far\n");
+            return;
+        }
+
+        /* If this is a leaf node, then do check and return. */
+        if ((node->child1 == NULL)&&(node->child2 == NULL)) {
+            int index = node->divfeat;
+            if (with_removed) {
+            	if (removed_points_.test(index)) return;
+            }
+            /*  Do not check same node more than once when searching multiple trees. */
+            if ( checked.test(index) || ((checkCount>=maxCheck)&& result_set.full()) ) return;
+            checked.set(index);
+            checkCount++;
+
+            DistanceType dist = distance_(node->point, vec, veclen_);
+            result_set.addPoint(dist,index);
+            return;
+        }
+
+        /* Which child branch should be taken first? */
+        ElementType val = vec[node->divfeat];
+        DistanceType diff = val - node->divval;
+        NodePtr bestChild = (diff < 0) ? node->child1 : node->child2;
+        NodePtr otherChild = (diff < 0) ? node->child2 : node->child1;
+
+        /* Create a branch record for the branch not taken.  Add distance
+            of this feature boundary (we don't attempt to correct for any
+            use of this feature in a parent node, which is unlikely to
+            happen and would have only a small effect).  Don't bother
+            adding more branches to heap after halfway point, as cost of
+            adding exceeds their value.
+         */
+
+        DistanceType new_distsq = mindist + distance_.accum_dist(val, node->divval, node->divfeat);
+        //		if (2 * checkCount < maxCheck  ||  !result.full()) {
+        if ((new_distsq*epsError < result_set.worstDist())||  !result_set.full()) {
+            heap->insert( BranchSt(otherChild, new_distsq) );
+        }
+
+        /* Call recursively to search next level down. */
+        searchLevel<with_removed>(result_set, vec, bestChild, mindist, checkCount, maxCheck, epsError, heap, checked);
+    }
+
+    /**
+     * Performs an exact search in the tree starting from a node.
+     */
+    template<bool with_removed>
+    void searchLevelExact(ResultSet<DistanceType>& result_set, const ElementType* vec, const NodePtr node, DistanceType mindist, const float epsError) const
+    {
+        /* If this is a leaf node, then do check and return. */
+        if ((node->child1 == NULL)&&(node->child2 == NULL)) {
+            int index = node->divfeat;
+            if (with_removed) {
+            	if (removed_points_.test(index)) return; // ignore removed points
+            }
+            DistanceType dist = distance_(node->point, vec, veclen_);
+            result_set.addPoint(dist,index);
+
+            return;
+        }
+
+        /* Which child branch should be taken first? */
+        ElementType val = vec[node->divfeat];
+        DistanceType diff = val - node->divval;
+        NodePtr bestChild = (diff < 0) ? node->child1 : node->child2;
+        NodePtr otherChild = (diff < 0) ? node->child2 : node->child1;
+
+        /* Create a branch record for the branch not taken.  Add distance
+            of this feature boundary (we don't attempt to correct for any
+            use of this feature in a parent node, which is unlikely to
+            happen and would have only a small effect).  Don't bother
+            adding more branches to heap after halfway point, as cost of
+            adding exceeds their value.
+         */
+
+        DistanceType new_distsq = mindist + distance_.accum_dist(val, node->divval, node->divfeat);
+
+        /* Call recursively to search next level down. */
+        searchLevelExact<with_removed>(result_set, vec, bestChild, mindist, epsError);
+
+        if (mindist*epsError<=result_set.worstDist()) {
+            searchLevelExact<with_removed>(result_set, vec, otherChild, new_distsq, epsError);
+        }
+    }
+    
+    void addPointToTree(NodePtr node, int ind)
+    {
+        ElementType* point = points_[ind];
+        
+        if ((node->child1==NULL) && (node->child2==NULL)) {
+            ElementType* leaf_point = node->point;
+            ElementType max_span = 0;
+            size_t div_feat = 0;
+            for (size_t i=0;i<veclen_;++i) {
+                ElementType span = std::abs(point[i]-leaf_point[i]);
+                if (span > max_span) {
+                    max_span = span;
+                    div_feat = i;
+                }
+            }
+            NodePtr left = new(pool_) Node();
+            left->child1 = left->child2 = NULL;
+            NodePtr right = new(pool_) Node();
+            right->child1 = right->child2 = NULL;
+
+            if (point[div_feat]<leaf_point[div_feat]) {
+                left->divfeat = ind;
+                left->point = point;
+                right->divfeat = node->divfeat;
+                right->point = node->point;
+            }
+            else {
+                left->divfeat = node->divfeat;
+                left->point = node->point;
+                right->divfeat = ind;
+                right->point = point;
+            }
+            node->divfeat = div_feat;
+            node->divval = (point[div_feat]+leaf_point[div_feat])/2;
+            node->child1 = left;
+            node->child2 = right;            
+        }
+        else {
+            if (point[node->divfeat]<node->divval) {
+                addPointToTree(node->child1,ind);
+            }
+            else {
+                addPointToTree(node->child2,ind);                
+            }
+        }
+    }
+private:
+    void swap(KDTreeIndex& other)
+    {
+    	BaseClass::swap(other);
+    	std::swap(trees_, other.trees_);
+    	std::swap(tree_roots_, other.tree_roots_);
+    	std::swap(pool_, other.pool_);
+    }
+
+private:
+
+    enum
+    {
+        /**
+         * To improve efficiency, only SAMPLE_MEAN random values are used to
+         * compute the mean and variance at each level when building a tree.
+         * A value of 100 seems to perform as well as using all values.
+         */
+        SAMPLE_MEAN = 100,
+        /**
+         * Top random dimensions to consider
+         *
+         * When creating random trees, the dimension on which to subdivide is
+         * selected at random from among the top RAND_DIM dimensions with the
+         * highest variance.  A value of 5 works well.
+         */
+        RAND_DIM=5
+    };
+
+
+    /**
+     * Number of randomized trees that are used
+     */
+    int trees_;
+
+    DistanceType* mean_;
+    DistanceType* var_;
+
+    /**
+     * Array of k-d trees used to find neighbours.
+     */
+    std::vector<NodePtr> tree_roots_;
+
+    /**
+     * Pooled memory allocator.
+     *
+     * Using a pooled memory allocator is more efficient
+     * than allocating memory directly when there is a large
+     * number small of memory allocations.
+     */
+    PooledAllocator pool_;
+
+    USING_BASECLASS_SYMBOLS
+};   // class KDTreeIndex
+
+}
+
+#endif //FLANN_KDTREE_INDEX_H_

corelib/src/rtflann/algorithms/kdtree_single_index.h

@@ -0,0 +1,698 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_KDTREE_SINGLE_INDEX_H_
+#define RTABMAP_FLANN_KDTREE_SINGLE_INDEX_H_
+
+#include <algorithm>
+#include <map>
+#include <cassert>
+#include <cstring>
+
+#include "rtflann/general.h"
+#include "rtflann/algorithms/nn_index.h"
+#include "rtflann/util/matrix.h"
+#include "rtflann/util/result_set.h"
+#include "rtflann/util/heap.h"
+#include "rtflann/util/allocator.h"
+#include "rtflann/util/random.h"
+#include "rtflann/util/saving.h"
+
+namespace rtflann
+{
+
+struct KDTreeSingleIndexParams : public IndexParams
+{
+    KDTreeSingleIndexParams(int leaf_max_size = 10, bool reorder = true)
+    {
+        (*this)["algorithm"] = FLANN_INDEX_KDTREE_SINGLE;
+        (*this)["leaf_max_size"] = leaf_max_size;
+        (*this)["reorder"] = reorder;
+    }
+};
+
+
+/**
+ * Single kd-tree index
+ *
+ * Contains the k-d trees and other information for indexing a set of points
+ * for nearest-neighbor matching.
+ */
+template <typename Distance>
+class KDTreeSingleIndex : public NNIndex<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    typedef NNIndex<Distance> BaseClass;
+
+    typedef bool needs_kdtree_distance;
+
+    /**
+     * KDTree constructor
+     *
+     * Params:
+     *          params = parameters passed to the kdtree algorithm
+     */
+    KDTreeSingleIndex(const IndexParams& params = KDTreeSingleIndexParams(), Distance d = Distance() ) :
+        BaseClass(params, d), root_node_(NULL)
+    {
+        leaf_max_size_ = get_param(params,"leaf_max_size",10);
+        reorder_ = get_param(params, "reorder", true);
+    }
+
+    /**
+     * KDTree constructor
+     *
+     * Params:
+     *          inputData = dataset with the input features
+     *          params = parameters passed to the kdtree algorithm
+     */
+    KDTreeSingleIndex(const Matrix<ElementType>& inputData, const IndexParams& params = KDTreeSingleIndexParams(),
+                      Distance d = Distance() ) : BaseClass(params, d), root_node_(NULL)
+    {
+        leaf_max_size_ = get_param(params,"leaf_max_size",10);
+        reorder_ = get_param(params, "reorder", true);
+
+        setDataset(inputData);
+    }
+
+
+    KDTreeSingleIndex(const KDTreeSingleIndex& other) : BaseClass(other),
+            leaf_max_size_(other.leaf_max_size_),
+            reorder_(other.reorder_),
+            vind_(other.vind_),
+            root_bbox_(other.root_bbox_)
+    {
+        if (reorder_) {
+            data_ = rtflann::Matrix<ElementType>(new ElementType[size_*veclen_], size_, veclen_);
+            std::copy(other.data_[0], other.data_[0]+size_*veclen_, data_[0]);
+        }
+        copyTree(root_node_, other.root_node_);
+    }
+
+    KDTreeSingleIndex& operator=(KDTreeSingleIndex other)
+    {
+        this->swap(other);
+        return *this;
+    }
+    
+    /**
+     * Standard destructor
+     */
+    virtual ~KDTreeSingleIndex()
+    {
+        freeIndex();
+    }
+    
+    BaseClass* clone() const
+    {
+        return new KDTreeSingleIndex(*this);
+    }
+
+    using BaseClass::buildIndex;
+
+    void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        assert(points.cols==veclen_);
+        extendDataset(points);
+        buildIndex();
+    }
+
+    flann_algorithm_t getType() const
+    {
+        return FLANN_INDEX_KDTREE_SINGLE;
+    }
+
+
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+        ar.setObject(this);
+
+        if (reorder_) index_params_["save_dataset"] = false;
+
+        ar & *static_cast<NNIndex<Distance>*>(this);
+
+        ar & reorder_;
+        ar & leaf_max_size_;
+        ar & root_bbox_;
+        ar & vind_;
+
+        if (reorder_) {
+            ar & data_;
+        }
+
+        if (Archive::is_loading::value) {
+            root_node_ = new(pool_) Node();
+        }
+
+        ar & *root_node_;
+
+        if (Archive::is_loading::value) {
+            index_params_["algorithm"] = getType();
+            index_params_["leaf_max_size"] = leaf_max_size_;
+            index_params_["reorder"] = reorder_;
+        }
+    }
+
+
+    void saveIndex(FILE* stream)
+    {
+        serialization::SaveArchive sa(stream);
+        sa & *this;
+    }
+
+
+    void loadIndex(FILE* stream)
+    {
+        freeIndex();
+        serialization::LoadArchive la(stream);
+        la & *this;
+    }
+
+    /**
+     * Computes the inde memory usage
+     * Returns: memory used by the index
+     */
+    int usedMemory() const
+    {
+        return pool_.usedMemory+pool_.wastedMemory+size_*sizeof(int);  // pool memory and vind array memory
+    }
+
+    /**
+     * Find set of nearest neighbors to vec. Their indices are stored inside
+     * the result object.
+     *
+     * Params:
+     *     result = the result object in which the indices of the nearest-neighbors are stored
+     *     vec = the vector for which to search the nearest neighbors
+     *     maxCheck = the maximum number of restarts (in a best-bin-first manner)
+     */
+    void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
+    {
+        float epsError = 1+searchParams.eps;
+
+        std::vector<DistanceType> dists(veclen_,0);
+        DistanceType distsq = computeInitialDistances(vec, dists);
+        if (removed_) {
+            searchLevel<true>(result, vec, root_node_, distsq, dists, epsError);
+        }
+        else {
+            searchLevel<false>(result, vec, root_node_, distsq, dists, epsError);
+        }
+    }
+
+protected:
+
+    /**
+     * Builds the index
+     */
+    void buildIndexImpl()
+    {
+        // Create a permutable array of indices to the input vectors.
+        vind_.resize(size_);
+        for (size_t i = 0; i < size_; i++) {
+            vind_[i] = i;
+        }
+
+        computeBoundingBox(root_bbox_);
+        root_node_ = divideTree(0, size_, root_bbox_ );   // construct the tree
+
+        if (reorder_) {
+            data_ = rtflann::Matrix<ElementType>(new ElementType[size_*veclen_], size_, veclen_);
+            for (size_t i=0; i<size_; ++i) {
+                std::copy(points_[vind_[i]], points_[vind_[i]]+veclen_, data_[i]);
+            }
+        }
+    }
+
+private:
+
+
+    /*--------------------- Internal Data Structures --------------------------*/
+    struct Node
+    {
+    	/**
+    	 * Indices of points in leaf node
+    	 */
+    	int left, right;
+    	/**
+    	 * Dimension used for subdivision.
+    	 */
+    	int divfeat;
+    	/**
+    	 * The values used for subdivision.
+    	 */
+    	DistanceType divlow, divhigh;
+        /**
+         * The child nodes.
+         */
+        Node* child1, * child2;
+        
+        ~Node()
+        {
+            if (child1) child1->~Node();
+            if (child2) child2->~Node();
+        }
+
+    private:
+        template<typename Archive>
+        void serialize(Archive& ar)
+        {
+            typedef KDTreeSingleIndex<Distance> Index;
+            Index* obj = static_cast<Index*>(ar.getObject());
+
+            ar & left;
+            ar & right;
+            ar & divfeat;
+            ar & divlow;
+            ar & divhigh;
+
+            bool leaf_node = false;
+            if (Archive::is_saving::value) {
+                leaf_node = ((child1==NULL) && (child2==NULL));
+            }
+            ar & leaf_node;
+
+            if (!leaf_node) {
+                if (Archive::is_loading::value) {
+                    child1 = new(obj->pool_) Node();
+                    child2 = new(obj->pool_) Node();
+                }
+                ar & *child1;
+                ar & *child2;
+            }
+        }
+        friend struct serialization::access;
+    };
+    typedef Node* NodePtr;
+
+
+    struct Interval
+    {
+        DistanceType low, high;
+        
+    private:
+        template <typename Archive>
+        void serialize(Archive& ar)
+        {
+            ar & low;
+            ar & high;
+        }
+        friend struct serialization::access;
+    };
+
+    typedef std::vector<Interval> BoundingBox;
+
+    typedef BranchStruct<NodePtr, DistanceType> BranchSt;
+    typedef BranchSt* Branch;
+
+
+    
+    void freeIndex()
+    {
+        if (data_.ptr()) {
+            delete[] data_.ptr();
+            data_ = rtflann::Matrix<ElementType>();
+        }
+        if (root_node_) root_node_->~Node();
+        pool_.free();
+    }
+    
+    void copyTree(NodePtr& dst, const NodePtr& src)
+    {
+        dst = new(pool_) Node();
+        *dst = *src;
+        if (src->child1!=NULL && src->child2!=NULL) {
+            copyTree(dst->child1, src->child1);
+            copyTree(dst->child2, src->child2);
+        }
+    }
+
+
+
+    void computeBoundingBox(BoundingBox& bbox)
+    {
+        bbox.resize(veclen_);
+        for (size_t i=0; i<veclen_; ++i) {
+            bbox[i].low = (DistanceType)points_[0][i];
+            bbox[i].high = (DistanceType)points_[0][i];
+        }
+        for (size_t k=1; k<size_; ++k) {
+            for (size_t i=0; i<veclen_; ++i) {
+                if (points_[k][i]<bbox[i].low) bbox[i].low = (DistanceType)points_[k][i];
+                if (points_[k][i]>bbox[i].high) bbox[i].high = (DistanceType)points_[k][i];
+            }
+        }
+    }
+
+
+    /**
+     * Create a tree node that subdivides the list of vecs from vind[first]
+     * to vind[last].  The routine is called recursively on each sublist.
+     * Place a pointer to this new tree node in the location pTree.
+     *
+     * Params: pTree = the new node to create
+     *                  first = index of the first vector
+     *                  last = index of the last vector
+     */
+    NodePtr divideTree(int left, int right, BoundingBox& bbox)
+    {
+        NodePtr node = new (pool_) Node(); // allocate memory
+
+        /* If too few exemplars remain, then make this a leaf node. */
+        if ( (right-left) <= leaf_max_size_) {
+            node->child1 = node->child2 = NULL;    /* Mark as leaf node. */
+            node->left = left;
+            node->right = right;
+
+            // compute bounding-box of leaf points
+            for (size_t i=0; i<veclen_; ++i) {
+                bbox[i].low = (DistanceType)points_[vind_[left]][i];
+                bbox[i].high = (DistanceType)points_[vind_[left]][i];
+            }
+            for (int k=left+1; k<right; ++k) {
+                for (size_t i=0; i<veclen_; ++i) {
+                    if (bbox[i].low>points_[vind_[k]][i]) bbox[i].low=(DistanceType)points_[vind_[k]][i];
+                    if (bbox[i].high<points_[vind_[k]][i]) bbox[i].high=(DistanceType)points_[vind_[k]][i];
+                }
+            }
+        }
+        else {
+            int idx;
+            int cutfeat;
+            DistanceType cutval;
+            middleSplit(&vind_[0]+left, right-left, idx, cutfeat, cutval, bbox);
+
+            node->divfeat = cutfeat;
+
+            BoundingBox left_bbox(bbox);
+            left_bbox[cutfeat].high = cutval;
+            node->child1 = divideTree(left, left+idx, left_bbox);
+
+            BoundingBox right_bbox(bbox);
+            right_bbox[cutfeat].low = cutval;
+            node->child2 = divideTree(left+idx, right, right_bbox);
+
+            node->divlow = left_bbox[cutfeat].high;
+            node->divhigh = right_bbox[cutfeat].low;
+
+            for (size_t i=0; i<veclen_; ++i) {
+            	bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
+            	bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
+            }
+        }
+
+        return node;
+    }
+
+    void computeMinMax(int* ind, int count, int dim, ElementType& min_elem, ElementType& max_elem)
+    {
+        min_elem = points_[ind[0]][dim];
+        max_elem = points_[ind[0]][dim];
+        for (int i=1; i<count; ++i) {
+            ElementType val = points_[ind[i]][dim];
+            if (val<min_elem) min_elem = val;
+            if (val>max_elem) max_elem = val;
+        }
+    }
+
+    void middleSplit(int* ind, int count, int& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
+    {
+        // find the largest span from the approximate bounding box
+        ElementType max_span = bbox[0].high-bbox[0].low;
+        cutfeat = 0;
+        cutval = (bbox[0].high+bbox[0].low)/2;
+        for (size_t i=1; i<veclen_; ++i) {
+            ElementType span = bbox[i].high-bbox[i].low;
+            if (span>max_span) {
+                max_span = span;
+                cutfeat = i;
+                cutval = (bbox[i].high+bbox[i].low)/2;
+            }
+        }
+
+        // compute exact span on the found dimension
+        ElementType min_elem, max_elem;
+        computeMinMax(ind, count, cutfeat, min_elem, max_elem);
+        cutval = (min_elem+max_elem)/2;
+        max_span = max_elem - min_elem;
+
+        // check if a dimension of a largest span exists
+        size_t k = cutfeat;
+        for (size_t i=0; i<veclen_; ++i) {
+            if (i==k) continue;
+            ElementType span = bbox[i].high-bbox[i].low;
+            if (span>max_span) {
+                computeMinMax(ind, count, i, min_elem, max_elem);
+                span = max_elem - min_elem;
+                if (span>max_span) {
+                    max_span = span;
+                    cutfeat = i;
+                    cutval = (min_elem+max_elem)/2;
+                }
+            }
+        }
+        int lim1, lim2;
+        planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
+
+        if (lim1>count/2) index = lim1;
+        else if (lim2<count/2) index = lim2;
+        else index = count/2;
+        
+        assert(index > 0 && index < count);
+    }
+
+
+    void middleSplit_(int* ind, int count, int& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
+    {
+        const float eps_val=0.00001f;
+        DistanceType max_span = bbox[0].high-bbox[0].low;
+        for (size_t i=1; i<veclen_; ++i) {
+            DistanceType span = bbox[i].high-bbox[i].low;
+            if (span>max_span) {
+                max_span = span;
+            }
+        }
+        DistanceType max_spread = -1;
+        cutfeat = 0;
+        for (size_t i=0; i<veclen_; ++i) {
+            DistanceType span = bbox[i].high-bbox[i].low;
+            if (span>(DistanceType)((1-eps_val)*max_span)) {
+                ElementType min_elem, max_elem;
+                computeMinMax(ind, count, cutfeat, min_elem, max_elem);
+                DistanceType spread = (DistanceType)(max_elem-min_elem);
+                if (spread>max_spread) {
+                    cutfeat = i;
+                    max_spread = spread;
+                }
+            }
+        }
+        // split in the middle
+        DistanceType split_val = (bbox[cutfeat].low+bbox[cutfeat].high)/2;
+        ElementType min_elem, max_elem;
+        computeMinMax(ind, count, cutfeat, min_elem, max_elem);
+
+        if (split_val<min_elem) cutval = (DistanceType)min_elem;
+        else if (split_val>max_elem) cutval = (DistanceType)max_elem;
+        else cutval = split_val;
+
+        int lim1, lim2;
+        planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
+
+        if (lim1>count/2) index = lim1;
+        else if (lim2<count/2) index = lim2;
+        else index = count/2;
+        
+        assert(index > 0 && index < count);
+    }
+
+
+    /**
+     *  Subdivide the list of points by a plane perpendicular on axe corresponding
+     *  to the 'cutfeat' dimension at 'cutval' position.
+     *
+     *  On return:
+     *  dataset[ind[0..lim1-1]][cutfeat]<cutval
+     *  dataset[ind[lim1..lim2-1]][cutfeat]==cutval
+     *  dataset[ind[lim2..count]][cutfeat]>cutval
+     */
+    void planeSplit(int* ind, int count, int cutfeat, DistanceType cutval, int& lim1, int& lim2)
+    {
+        int left = 0;
+        int right = count-1;
+        for (;; ) {
+            while (left<=right && points_[ind[left]][cutfeat]<cutval) ++left;
+            while (left<=right && points_[ind[right]][cutfeat]>=cutval) --right;
+            if (left>right) break;
+            std::swap(ind[left], ind[right]); ++left; --right;
+        }
+
+        lim1 = left;
+        right = count-1;
+        for (;; ) {
+            while (left<=right && points_[ind[left]][cutfeat]<=cutval) ++left;
+            while (left<=right && points_[ind[right]][cutfeat]>cutval) --right;
+            if (left>right) break;
+            std::swap(ind[left], ind[right]); ++left; --right;
+        }
+        lim2 = left;
+    }
+
+    DistanceType computeInitialDistances(const ElementType* vec, std::vector<DistanceType>& dists) const
+    {
+        DistanceType distsq = 0.0;
+
+        for (size_t i = 0; i < veclen_; ++i) {
+            if (vec[i] < root_bbox_[i].low) {
+                dists[i] = distance_.accum_dist(vec[i], root_bbox_[i].low, i);
+                distsq += dists[i];
+            }
+            if (vec[i] > root_bbox_[i].high) {
+                dists[i] = distance_.accum_dist(vec[i], root_bbox_[i].high, i);
+                distsq += dists[i];
+            }
+        }
+
+        return distsq;
+    }
+
+    /**
+     * Performs an exact search in the tree starting from a node.
+     */
+    template <bool with_removed>
+    void searchLevel(ResultSet<DistanceType>& result_set, const ElementType* vec, const NodePtr node, DistanceType mindistsq,
+                     std::vector<DistanceType>& dists, const float epsError) const
+    {
+        /* If this is a leaf node, then do check and return. */
+        if ((node->child1 == NULL)&&(node->child2 == NULL)) {
+            DistanceType worst_dist = result_set.worstDist();
+            for (int i=node->left; i<node->right; ++i) {
+                if (with_removed) {
+                    if (removed_points_.test(vind_[i])) continue;
+                }
+                ElementType* point = reorder_ ? data_[i] : points_[vind_[i]];
+                DistanceType dist = distance_(vec, point, veclen_, worst_dist);
+                if (dist<worst_dist) {
+                    result_set.addPoint(dist,vind_[i]);
+                }
+            }
+            return;
+        }
+
+        /* Which child branch should be taken first? */
+        int idx = node->divfeat;
+        ElementType val = vec[idx];
+        DistanceType diff1 = val - node->divlow;
+        DistanceType diff2 = val - node->divhigh;
+
+        NodePtr bestChild;
+        NodePtr otherChild;
+        DistanceType cut_dist;
+        if ((diff1+diff2)<0) {
+            bestChild = node->child1;
+            otherChild = node->child2;
+            cut_dist = distance_.accum_dist(val, node->divhigh, idx);
+        }
+        else {
+            bestChild = node->child2;
+            otherChild = node->child1;
+            cut_dist = distance_.accum_dist( val, node->divlow, idx);
+        }
+
+        /* Call recursively to search next level down. */
+        searchLevel<with_removed>(result_set, vec, bestChild, mindistsq, dists, epsError);
+
+        DistanceType dst = dists[idx];
+        mindistsq = mindistsq + cut_dist - dst;
+        dists[idx] = cut_dist;
+        if (mindistsq*epsError<=result_set.worstDist()) {
+            searchLevel<with_removed>(result_set, vec, otherChild, mindistsq, dists, epsError);
+        }
+        dists[idx] = dst;
+    }
+
+    
+    void swap(KDTreeSingleIndex& other)
+    {
+        BaseClass::swap(other);
+        std::swap(leaf_max_size_, other.leaf_max_size_);
+        std::swap(reorder_, other.reorder_);
+        std::swap(vind_, other.vind_);
+        std::swap(data_, other.data_);
+        std::swap(root_node_, other.root_node_);
+        std::swap(root_bbox_, other.root_bbox_);
+        std::swap(pool_, other.pool_);
+    }
+    
+private:
+
+
+
+    int leaf_max_size_;
+    
+    
+    bool reorder_;
+
+    /**
+     *  Array of indices to vectors in the dataset.
+     */
+    std::vector<int> vind_;
+
+    Matrix<ElementType> data_;
+
+    /**
+     * Array of k-d trees used to find neighbours.
+     */
+    NodePtr root_node_;
+
+    /**
+     * Root bounding box
+     */
+    BoundingBox root_bbox_;
+
+    /**
+     * Pooled memory allocator.
+     *
+     * Using a pooled memory allocator is more efficient
+     * than allocating memory directly when there is a large
+     * number small of memory allocations.
+     */
+    PooledAllocator pool_;
+
+    USING_BASECLASS_SYMBOLS
+
+};   // class KDTreeSingleIndex
+
+}
+
+#endif //FLANN_KDTREE_SINGLE_INDEX_H_

corelib/src/rtflann/algorithms/linear_index.h

@@ -0,0 +1,163 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_LINEAR_INDEX_H_
+#define RTABMAP_FLANN_LINEAR_INDEX_H_
+
+#include "rtflann/general.h"
+#include "nn_index.h"
+
+namespace rtflann
+{
+
+struct LinearIndexParams : public IndexParams
+{
+    LinearIndexParams()
+    {
+        (* this)["algorithm"] = FLANN_INDEX_LINEAR;
+    }
+};
+
+template <typename Distance>
+class LinearIndex : public NNIndex<Distance>
+{
+public:
+
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    typedef NNIndex<Distance> BaseClass;
+
+    LinearIndex(const IndexParams& params = LinearIndexParams(), Distance d = Distance()) :
+    	BaseClass(params, d)
+    {
+    }
+
+    LinearIndex(const Matrix<ElementType>& input_data, const IndexParams& params = LinearIndexParams(), Distance d = Distance()) :
+    	BaseClass(params, d)
+    {
+        setDataset(input_data);
+    }
+
+    LinearIndex(const LinearIndex& other) : BaseClass(other)
+    {
+    }
+
+    LinearIndex& operator=(LinearIndex other)
+    {
+    	this->swap(other);
+    	return *this;
+    }
+
+    virtual ~LinearIndex()
+    {
+    }
+
+    BaseClass* clone() const
+    {
+    	return new LinearIndex(*this);
+    }
+
+    void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        assert(points.cols==veclen_);
+        extendDataset(points);
+    }
+
+    flann_algorithm_t getType() const
+    {
+        return FLANN_INDEX_LINEAR;
+    }
+
+
+    int usedMemory() const
+    {
+        return 0;
+    }
+
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+    	ar.setObject(this);
+
+    	ar & *static_cast<NNIndex<Distance>*>(this);
+
+    	if (Archive::is_loading::value) {
+            index_params_["algorithm"] = getType();
+    	}
+    }
+
+    void saveIndex(FILE* stream)
+    {
+    	serialization::SaveArchive sa(stream);
+    	sa & *this;
+    }
+
+    void loadIndex(FILE* stream)
+    {
+    	serialization::LoadArchive la(stream);
+    	la & *this;
+    }
+
+    void findNeighbors(ResultSet<DistanceType>& resultSet, const ElementType* vec, const SearchParams& /*searchParams*/) const
+    {
+    	if (removed_) {
+    		for (size_t i = 0; i < points_.size(); ++i) {
+    			if (removed_points_.test(i)) continue;
+    			DistanceType dist = distance_(points_[i], vec, veclen_);
+    			resultSet.addPoint(dist, i);
+    		}
+    	}
+    	else {
+    		for (size_t i = 0; i < points_.size(); ++i) {
+    			DistanceType dist = distance_(points_[i], vec, veclen_);
+    			resultSet.addPoint(dist, i);
+    		}
+    	}
+    }
+protected:
+    void buildIndexImpl()
+    {
+        /* nothing to do here for linear search */
+    }
+
+    void freeIndex()
+    {
+        /* nothing to do here for linear search */
+    }
+
+private:
+
+    USING_BASECLASS_SYMBOLS
+};
+
+}
+
+#endif // FLANN_LINEAR_INDEX_H_

corelib/src/rtflann/algorithms/lsh_index.h

@@ -0,0 +1,548 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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.
+ *************************************************************************/
+
+/***********************************************************************
+ * Author: Vincent Rabaud
+ *************************************************************************/
+
+#ifndef RTABMAP_FLANN_LSH_INDEX_H_
+#define RTABMAP_FLANN_LSH_INDEX_H_
+
+#include <algorithm>
+#include <cassert>
+#include <cstring>
+#include <map>
+#include <vector>
+
+#include "rtflann/general.h"
+#include "rtflann/algorithms/nn_index.h"
+#include "rtflann/util/matrix.h"
+#include "rtflann/util/result_set.h"
+#include "rtflann/util/heap.h"
+#include "rtflann/util/lsh_table.h"
+#include "rtflann/util/allocator.h"
+#include "rtflann/util/random.h"
+#include "rtflann/util/saving.h"
+
+namespace rtflann
+{
+
+struct LshIndexParams : public IndexParams
+{
+    LshIndexParams(unsigned int table_number = 12, unsigned int key_size = 20, unsigned int multi_probe_level = 2)
+    {
+        (* this)["algorithm"] = FLANN_INDEX_LSH;
+        // The number of hash tables to use
+        (*this)["table_number"] = table_number;
+        // The length of the key in the hash tables
+        (*this)["key_size"] = key_size;
+        // Number of levels to use in multi-probe (0 for standard LSH)
+        (*this)["multi_probe_level"] = multi_probe_level;
+    }
+};
+
+/**
+ * Randomized kd-tree index
+ *
+ * Contains the k-d trees and other information for indexing a set of points
+ * for nearest-neighbor matching.
+ */
+template<typename Distance>
+class LshIndex : public NNIndex<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    typedef NNIndex<Distance> BaseClass;
+
+    /** Constructor
+     * @param params parameters passed to the LSH algorithm
+     * @param d the distance used
+     */
+    LshIndex(const IndexParams& params = LshIndexParams(), Distance d = Distance()) :
+    	BaseClass(params, d)
+    {
+        table_number_ = get_param<unsigned int>(index_params_,"table_number",12);
+        key_size_ = get_param<unsigned int>(index_params_,"key_size",20);
+        multi_probe_level_ = get_param<unsigned int>(index_params_,"multi_probe_level",2);
+
+        fill_xor_mask(0, key_size_, multi_probe_level_, xor_masks_);
+    }
+
+
+    /** Constructor
+     * @param input_data dataset with the input features
+     * @param params parameters passed to the LSH algorithm
+     * @param d the distance used
+     */
+    LshIndex(const Matrix<ElementType>& input_data, const IndexParams& params = LshIndexParams(), Distance d = Distance()) :
+    	BaseClass(params, d)
+    {
+        table_number_ = get_param<unsigned int>(index_params_,"table_number",12);
+        key_size_ = get_param<unsigned int>(index_params_,"key_size",20);
+        multi_probe_level_ = get_param<unsigned int>(index_params_,"multi_probe_level",2);
+
+        fill_xor_mask(0, key_size_, multi_probe_level_, xor_masks_);
+
+        setDataset(input_data);
+    }
+
+    LshIndex(const LshIndex& other) : BaseClass(other),
+    	tables_(other.tables_),
+    	table_number_(other.table_number_),
+    	key_size_(other.key_size_),
+    	multi_probe_level_(other.multi_probe_level_),
+    	xor_masks_(other.xor_masks_)
+    {
+    }
+    
+    LshIndex& operator=(LshIndex other)
+    {
+    	this->swap(other);
+    	return *this;
+    }
+
+    virtual ~LshIndex()
+    {
+    	freeIndex();
+    }
+
+
+    BaseClass* clone() const
+    {
+    	return new LshIndex(*this);
+    }
+    
+    using BaseClass::buildIndex;
+
+    void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        assert(points.cols==veclen_);
+        size_t old_size = size_;
+
+        extendDataset(points);
+        
+        if (rebuild_threshold>1 && size_at_build_*rebuild_threshold<size_) {
+            buildIndex();
+        }
+        else {
+            for (unsigned int i = 0; i < table_number_; ++i) {
+                lsh::LshTable<ElementType>& table = tables_[i];                
+                for (size_t i=old_size;i<size_;++i) {
+                    table.add(i, points_[i]);
+                }            
+            }
+        }
+    }
+
+
+    flann_algorithm_t getType() const
+    {
+        return FLANN_INDEX_LSH;
+    }
+
+
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+    	ar.setObject(this);
+
+    	ar & *static_cast<NNIndex<Distance>*>(this);
+
+    	ar & table_number_;
+    	ar & key_size_;
+    	ar & multi_probe_level_;
+
+    	ar & xor_masks_;
+    	ar & tables_;
+
+    	if (Archive::is_loading::value) {
+            index_params_["algorithm"] = getType();
+            index_params_["table_number"] = table_number_;
+            index_params_["key_size"] = key_size_;
+            index_params_["multi_probe_level"] = multi_probe_level_;
+    	}
+    }
+
+    void saveIndex(FILE* stream)
+    {
+    	serialization::SaveArchive sa(stream);
+    	sa & *this;
+    }
+
+    void loadIndex(FILE* stream)
+    {
+    	serialization::LoadArchive la(stream);
+    	la & *this;
+    }
+
+    /**
+     * Computes the index memory usage
+     * Returns: memory used by the index
+     */
+    int usedMemory() const
+    {
+        return size_ * sizeof(int);
+    }
+
+    /**
+     * \brief Perform k-nearest neighbor search
+     * \param[in] queries The query points for which to find the nearest neighbors
+     * \param[out] indices The indices of the nearest neighbors found
+     * \param[out] dists Distances to the nearest neighbors found
+     * \param[in] knn Number of nearest neighbors to return
+     * \param[in] params Search parameters
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+    					Matrix<size_t>& indices,
+    					Matrix<DistanceType>& dists,
+    					size_t knn,
+    					const SearchParams& params) const
+    {
+        assert(queries.cols == veclen_);
+        assert(indices.rows >= queries.rows);
+        assert(dists.rows >= queries.rows);
+        assert(indices.cols >= knn);
+        assert(dists.cols >= knn);
+
+        int count = 0;
+        if (params.use_heap==FLANN_True) {
+#pragma omp parallel num_threads(params.cores)
+        	{
+        		KNNUniqueResultSet<DistanceType> resultSet(knn);
+#pragma omp for schedule(static) reduction(+:count)
+        		for (int i = 0; i < (int)queries.rows; i++) {
+        			resultSet.clear();
+        			findNeighbors(resultSet, queries[i], params);
+        			size_t n = std::min(resultSet.size(), knn);
+        			resultSet.copy(indices[i], dists[i], n, params.sorted);
+        			indices_to_ids(indices[i], indices[i], n);
+        			count += n;
+        		}
+        	}
+        }
+        else {
+#pragma omp parallel num_threads(params.cores)
+        	{
+        		KNNResultSet<DistanceType> resultSet(knn);
+#pragma omp for schedule(static) reduction(+:count)
+        		for (int i = 0; i < (int)queries.rows; i++) {
+        			resultSet.clear();
+        			findNeighbors(resultSet, queries[i], params);
+        			size_t n = std::min(resultSet.size(), knn);
+        			resultSet.copy(indices[i], dists[i], n, params.sorted);
+        			indices_to_ids(indices[i], indices[i], n);
+        			count += n;
+        		}
+        	}
+        }
+
+        return count;
+    }
+
+    /**
+     * \brief Perform k-nearest neighbor search
+     * \param[in] queries The query points for which to find the nearest neighbors
+     * \param[out] indices The indices of the nearest neighbors found
+     * \param[out] dists Distances to the nearest neighbors found
+     * \param[in] knn Number of nearest neighbors to return
+     * \param[in] params Search parameters
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+					std::vector< std::vector<size_t> >& indices,
+					std::vector<std::vector<DistanceType> >& dists,
+    				size_t knn,
+    				const SearchParams& params) const
+    {
+        assert(queries.cols == veclen_);
+		if (indices.size() < queries.rows ) indices.resize(queries.rows);
+		if (dists.size() < queries.rows ) dists.resize(queries.rows);
+
+		int count = 0;
+		if (params.use_heap==FLANN_True) {
+#pragma omp parallel num_threads(params.cores)
+			{
+				KNNUniqueResultSet<DistanceType> resultSet(knn);
+#pragma omp for schedule(static) reduction(+:count)
+				for (int i = 0; i < (int)queries.rows; i++) {
+					resultSet.clear();
+					findNeighbors(resultSet, queries[i], params);
+					size_t n = std::min(resultSet.size(), knn);
+					indices[i].resize(n);
+					dists[i].resize(n);
+					if (n > 0) {
+						resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
+						indices_to_ids(&indices[i][0], &indices[i][0], n);
+					}
+					count += n;
+				}
+			}
+		}
+		else {
+#pragma omp parallel num_threads(params.cores)
+			{
+				KNNResultSet<DistanceType> resultSet(knn);
+#pragma omp for schedule(static) reduction(+:count)
+				for (int i = 0; i < (int)queries.rows; i++) {
+					resultSet.clear();
+					findNeighbors(resultSet, queries[i], params);
+					size_t n = std::min(resultSet.size(), knn);
+					indices[i].resize(n);
+					dists[i].resize(n);
+					if (n > 0) {
+						resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
+						indices_to_ids(&indices[i][0], &indices[i][0], n);
+					}
+					count += n;
+				}
+			}
+		}
+
+		return count;
+    }
+
+    /**
+     * Find set of nearest neighbors to vec. Their indices are stored inside
+     * the result object.
+     *
+     * Params:
+     *     result = the result object in which the indices of the nearest-neighbors are stored
+     *     vec = the vector for which to search the nearest neighbors
+     *     maxCheck = the maximum number of restarts (in a best-bin-first manner)
+     */
+    void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& /*searchParams*/) const
+    {
+        getNeighbors(vec, result);
+    }
+
+protected:
+
+    /**
+     * Builds the index
+     */
+    void buildIndexImpl()
+    {
+        tables_.resize(table_number_);
+        std::vector<std::pair<size_t,ElementType*> > features;
+        features.reserve(points_.size());
+        for (size_t i=0;i<points_.size();++i) {
+        	features.push_back(std::make_pair(i, points_[i]));
+        }
+        for (unsigned int i = 0; i < table_number_; ++i) {
+            lsh::LshTable<ElementType>& table = tables_[i];
+            table = lsh::LshTable<ElementType>(veclen_, key_size_);
+
+            // Add the features to the table
+            table.add(features);
+        }
+    }
+
+    void freeIndex()
+    {
+        /* nothing to do here */
+    }
+
+
+private:
+    /** Defines the comparator on score and index
+     */
+    typedef std::pair<float, unsigned int> ScoreIndexPair;
+    struct SortScoreIndexPairOnSecond
+    {
+        bool operator()(const ScoreIndexPair& left, const ScoreIndexPair& right) const
+        {
+            return left.second < right.second;
+        }
+    };
+
+    /** Fills the different xor masks to use when getting the neighbors in multi-probe LSH
+     * @param key the key we build neighbors from
+     * @param lowest_index the lowest index of the bit set
+     * @param level the multi-probe level we are at
+     * @param xor_masks all the xor mask
+     */
+    void fill_xor_mask(lsh::BucketKey key, int lowest_index, unsigned int level,
+                       std::vector<lsh::BucketKey>& xor_masks)
+    {
+        xor_masks.push_back(key);
+        if (level == 0) return;
+        for (int index = lowest_index - 1; index >= 0; --index) {
+            // Create a new key
+            lsh::BucketKey new_key = key | (lsh::BucketKey(1) << index);
+            fill_xor_mask(new_key, index, level - 1, xor_masks);
+        }
+    }
+
+    /** Performs the approximate nearest-neighbor search.
+     * @param vec the feature to analyze
+     * @param do_radius flag indicating if we check the radius too
+     * @param radius the radius if it is a radius search
+     * @param do_k flag indicating if we limit the number of nn
+     * @param k_nn the number of nearest neighbors
+     * @param checked_average used for debugging
+     */
+    void getNeighbors(const ElementType* vec, bool do_radius, float radius, bool do_k, unsigned int k_nn,
+                      float& checked_average)
+    {
+        static std::vector<ScoreIndexPair> score_index_heap;
+
+        if (do_k) {
+            unsigned int worst_score = std::numeric_limits<unsigned int>::max();
+            typename std::vector<lsh::LshTable<ElementType> >::const_iterator table = tables_.begin();
+            typename std::vector<lsh::LshTable<ElementType> >::const_iterator table_end = tables_.end();
+            for (; table != table_end; ++table) {
+                size_t key = table->getKey(vec);
+                std::vector<lsh::BucketKey>::const_iterator xor_mask = xor_masks_.begin();
+                std::vector<lsh::BucketKey>::const_iterator xor_mask_end = xor_masks_.end();
+                for (; xor_mask != xor_mask_end; ++xor_mask) {
+                    size_t sub_key = key ^ (*xor_mask);
+                    const lsh::Bucket* bucket = table->getBucketFromKey(sub_key);
+                    if (bucket == 0) continue;
+
+                    // Go over each descriptor index
+                    std::vector<lsh::FeatureIndex>::const_iterator training_index = bucket->begin();
+                    std::vector<lsh::FeatureIndex>::const_iterator last_training_index = bucket->end();
+                    DistanceType hamming_distance;
+
+                    // Process the rest of the candidates
+                    for (; training_index < last_training_index; ++training_index) {
+                    	if (removed_ && removed_points_.test(*training_index)) continue;
+                        hamming_distance = distance_(vec, points_[*training_index].point, veclen_);
+
+                        if (hamming_distance < worst_score) {
+                            // Insert the new element
+                            score_index_heap.push_back(ScoreIndexPair(hamming_distance, training_index));
+                            std::push_heap(score_index_heap.begin(), score_index_heap.end());
+
+                            if (score_index_heap.size() > (unsigned int)k_nn) {
+                                // Remove the highest distance value as we have too many elements
+                                std::pop_heap(score_index_heap.begin(), score_index_heap.end());
+                                score_index_heap.pop_back();
+                                // Keep track of the worst score
+                                worst_score = score_index_heap.front().first;
+                            }
+                        }
+                    }
+                }
+            }
+        }
+        else {
+            typename std::vector<lsh::LshTable<ElementType> >::const_iterator table = tables_.begin();
+            typename std::vector<lsh::LshTable<ElementType> >::const_iterator table_end = tables_.end();
+            for (; table != table_end; ++table) {
+                size_t key = table->getKey(vec);
+                std::vector<lsh::BucketKey>::const_iterator xor_mask = xor_masks_.begin();
+                std::vector<lsh::BucketKey>::const_iterator xor_mask_end = xor_masks_.end();
+                for (; xor_mask != xor_mask_end; ++xor_mask) {
+                    size_t sub_key = key ^ (*xor_mask);
+                    const lsh::Bucket* bucket = table->getBucketFromKey(sub_key);
+                    if (bucket == 0) continue;
+
+                    // Go over each descriptor index
+                    std::vector<lsh::FeatureIndex>::const_iterator training_index = bucket->begin();
+                    std::vector<lsh::FeatureIndex>::const_iterator last_training_index = bucket->end();
+                    DistanceType hamming_distance;
+
+                    // Process the rest of the candidates
+                    for (; training_index < last_training_index; ++training_index) {
+                    	if (removed_ && removed_points_.test(*training_index)) continue;
+                        // Compute the Hamming distance
+                        hamming_distance = distance_(vec, points_[*training_index].point, veclen_);
+                        if (hamming_distance < radius) score_index_heap.push_back(ScoreIndexPair(hamming_distance, training_index));
+                    }
+                }
+            }
+        }
+    }
+
+    /** Performs the approximate nearest-neighbor search.
+     * This is a slower version than the above as it uses the ResultSet
+     * @param vec the feature to analyze
+     */
+    void getNeighbors(const ElementType* vec, ResultSet<DistanceType>& result) const
+    {
+        typename std::vector<lsh::LshTable<ElementType> >::const_iterator table = tables_.begin();
+        typename std::vector<lsh::LshTable<ElementType> >::const_iterator table_end = tables_.end();
+        for (; table != table_end; ++table) {
+            size_t key = table->getKey(vec);
+            std::vector<lsh::BucketKey>::const_iterator xor_mask = xor_masks_.begin();
+            std::vector<lsh::BucketKey>::const_iterator xor_mask_end = xor_masks_.end();
+            for (; xor_mask != xor_mask_end; ++xor_mask) {
+                size_t sub_key = key ^ (*xor_mask);
+                const lsh::Bucket* bucket = table->getBucketFromKey(sub_key);
+                if (bucket == 0) continue;
+
+                // Go over each descriptor index
+                std::vector<lsh::FeatureIndex>::const_iterator training_index = bucket->begin();
+                std::vector<lsh::FeatureIndex>::const_iterator last_training_index = bucket->end();
+                DistanceType hamming_distance;
+
+                // Process the rest of the candidates
+                for (; training_index < last_training_index; ++training_index) {
+                	if (removed_ && removed_points_.test(*training_index)) continue;
+                    // Compute the Hamming distance
+                    hamming_distance = distance_(vec, points_[*training_index], veclen_);
+                    result.addPoint(hamming_distance, *training_index);
+                }
+            }
+        }
+    }
+
+
+    void swap(LshIndex& other)
+    {
+    	BaseClass::swap(other);
+    	std::swap(tables_, other.tables_);
+    	std::swap(size_at_build_, other.size_at_build_);
+    	std::swap(table_number_, other.table_number_);
+    	std::swap(key_size_, other.key_size_);
+    	std::swap(multi_probe_level_, other.multi_probe_level_);
+    	std::swap(xor_masks_, other.xor_masks_);
+    }
+
+    /** The different hash tables */
+    std::vector<lsh::LshTable<ElementType> > tables_;
+    
+    /** table number */
+    unsigned int table_number_;
+    /** key size */
+    unsigned int key_size_;
+    /** How far should we look for neighbors in multi-probe LSH */
+    unsigned int multi_probe_level_;
+
+    /** The XOR masks to apply to a key to get the neighboring buckets */
+    std::vector<lsh::BucketKey> xor_masks_;
+
+    USING_BASECLASS_SYMBOLS
+};
+}
+
+#endif //FLANN_LSH_INDEX_H_

corelib/src/rtflann/algorithms/nn_index.h

@@ -0,0 +1,917 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_NNINDEX_H
+#define RTABMAP_FLANN_NNINDEX_H
+
+#include <vector>
+
+#include "rtflann/general.h"
+#include "rtflann/util/matrix.h"
+#include "rtflann/util/params.h"
+#include "rtflann/util/result_set.h"
+#include "rtflann/util/dynamic_bitset.h"
+#include "rtflann/util/saving.h"
+
+namespace rtflann
+{
+
+#define KNN_HEAP_THRESHOLD 250
+
+
+class IndexBase
+{
+public:
+    virtual ~IndexBase() {};
+
+    virtual size_t veclen() const = 0;
+
+    virtual size_t size() const = 0;
+
+    virtual flann_algorithm_t getType() const = 0;
+
+    virtual int usedMemory() const = 0;
+
+    virtual IndexParams getParameters() const = 0;
+
+    virtual void loadIndex(FILE* stream) = 0;
+
+    virtual void saveIndex(FILE* stream) = 0;
+};
+
+/**
+ * Nearest-neighbour index base class
+ */
+template <typename Distance>
+class NNIndex : public IndexBase
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+	NNIndex(Distance d) : distance_(d), last_id_(0), size_(0), size_at_build_(0), veclen_(0),
+			removed_(false), removed_count_(0), data_ptr_(NULL)
+	{
+	}
+
+	NNIndex(const IndexParams& params, Distance d) : distance_(d), last_id_(0), size_(0), size_at_build_(0), veclen_(0),
+			index_params_(params), removed_(false), removed_count_(0), data_ptr_(NULL)
+	{
+	}
+
+	NNIndex(const NNIndex& other) :
+		distance_(other.distance_),
+		last_id_(other.last_id_),
+		size_(other.size_),
+		size_at_build_(other.size_at_build_),
+		veclen_(other.veclen_),
+		index_params_(other.index_params_),
+		removed_(other.removed_),
+		removed_points_(other.removed_points_),
+		removed_count_(other.removed_count_),
+		ids_(other.ids_),
+		points_(other.points_),
+		data_ptr_(NULL)
+	{
+		if (other.data_ptr_) {
+			data_ptr_ = new ElementType[size_*veclen_];
+			std::copy(other.data_ptr_, other.data_ptr_+size_*veclen_, data_ptr_);
+			for (size_t i=0;i<size_;++i) {
+				points_[i] = data_ptr_ + i*veclen_;
+			}
+		}
+	}
+
+	virtual ~NNIndex()
+	{
+		if (data_ptr_) {
+			delete[] data_ptr_;
+		}
+	}
+
+
+	virtual NNIndex* clone() const = 0;
+
+	/**
+	 * Builds the index
+	 */
+	virtual void buildIndex()
+	{
+    	freeIndex();
+    	cleanRemovedPoints();
+
+    	// building index
+		buildIndexImpl();
+
+        size_at_build_ = size_;
+
+	}
+
+	/**
+	 * Builds the index using the specified dataset
+	 * @param dataset the dataset to use
+	 */
+    virtual void buildIndex(const Matrix<ElementType>& dataset)
+    {
+        setDataset(dataset);
+        this->buildIndex();
+    }
+
+	/**
+	 * @brief Incrementally add points to the index.
+	 * @param points Matrix with points to be added
+	 * @param rebuild_threshold
+	 */
+    virtual void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        throw FLANNException("Functionality not supported by this index");
+    }
+
+    /**
+     * Remove point from the index
+     * @param index Index of point to be removed
+     */
+    virtual void removePoint(size_t id)
+    {
+    	if (!removed_) {
+    		ids_.resize(size_);
+    		for (size_t i=0;i<size_;++i) {
+    			ids_[i] = i;
+    		}
+    		removed_points_.resize(size_);
+    		removed_points_.reset();
+    		last_id_ = size_;
+        	removed_ = true;
+    	}
+
+    	size_t point_index = id_to_index(id);
+    	if (point_index!=size_t(-1) && !removed_points_.test(point_index)) {
+    		removed_points_.set(point_index);
+    		removed_count_++;
+    	}
+    }
+
+
+    /**
+     * Get point with specific id
+     * @param id
+     * @return
+     */
+    virtual ElementType* getPoint(size_t id)
+    {
+    	size_t index = id_to_index(id);
+    	if (index!=size_t(-1)) {
+    		return points_[index];
+    	}
+    	else {
+    		return NULL;
+    	}
+    }
+
+    /**
+     * @return number of features in this index.
+     */
+    inline size_t size() const
+    {
+    	return size_ - removed_count_;
+    }
+
+    inline size_t removedCount() const
+	{
+		return removed_count_;
+	}
+
+    inline size_t sizeAtBuild() const
+	{
+		return size_at_build_;
+	}
+
+    /**
+     * @return The dimensionality of the features in this index.
+     */
+    inline size_t veclen() const
+    {
+        return veclen_;
+    }
+
+    /**
+     * Returns the parameters used by the index.
+     *
+     * @return The index parameters
+     */
+    IndexParams getParameters() const
+    {
+        return index_params_;
+    }
+
+
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+    	IndexHeader header;
+
+    	if (Archive::is_saving::value) {
+            header.h.data_type = flann_datatype_value<ElementType>::value;
+            header.h.index_type = getType();
+            header.h.rows = size_;
+            header.h.cols = veclen_;
+    	}
+    	ar & header;
+
+    	// sanity checks
+    	if (Archive::is_loading::value) {
+            if (strncmp(header.h.signature,
+                        FLANN_SIGNATURE_,
+                        strlen(FLANN_SIGNATURE_) - strlen("v0.0")) != 0) {
+    	        throw FLANNException("Invalid index file, wrong signature");
+    	    }
+
+            if (header.h.data_type != flann_datatype_value<ElementType>::value) {
+                throw FLANNException("Datatype of saved index is different than of the one to be created.");
+            }
+
+            if (header.h.index_type != getType()) {
+                throw FLANNException("Saved index type is different then the current index type.");
+            }
+            // TODO: check for distance type
+
+    	}
+
+    	ar & size_;
+    	ar & veclen_;
+    	ar & size_at_build_;
+
+    	bool save_dataset;
+    	if (Archive::is_saving::value) {
+    		save_dataset = get_param(index_params_,"save_dataset", false);
+    	}
+    	ar & save_dataset;
+
+    	if (save_dataset) {
+    		if (Archive::is_loading::value) {
+    			if (data_ptr_) {
+    				delete[] data_ptr_;
+    			}
+    			data_ptr_ = new ElementType[size_*veclen_];
+    			points_.resize(size_);
+        		for (size_t i=0;i<size_;++i) {
+        			points_[i] = data_ptr_ + i*veclen_;
+        		}
+    		}
+    		for (size_t i=0;i<size_;++i) {
+    			ar & serialization::make_binary_object (points_[i], veclen_*sizeof(ElementType));
+    		}
+    	} else {
+    		if (points_.size()!=size_) {
+    			throw FLANNException("Saved index does not contain the dataset and no dataset was provided.");
+    		}
+    	}
+
+    	ar & last_id_;
+    	ar & ids_;
+    	ar & removed_;
+    	if (removed_) {
+    		ar & removed_points_;
+    	}
+    	ar & removed_count_;
+    }
+
+
+    /**
+     * @brief Perform k-nearest neighbor search
+     * @param[in] queries The query points for which to find the nearest neighbors
+     * @param[out] indices The indices of the nearest neighbors found
+     * @param[out] dists Distances to the nearest neighbors found
+     * @param[in] knn Number of nearest neighbors to return
+     * @param[in] params Search parameters
+     */
+    virtual int knnSearch(const Matrix<ElementType>& queries,
+    		Matrix<size_t>& indices,
+    		Matrix<DistanceType>& dists,
+    		size_t knn,
+    		const SearchParams& params) const
+    {
+    	assert(queries.cols == veclen());
+    	assert(indices.rows >= queries.rows);
+    	assert(dists.rows >= queries.rows);
+    	assert(indices.cols >= knn);
+    	assert(dists.cols >= knn);
+    	bool use_heap;
+
+    	if (params.use_heap==FLANN_Undefined) {
+    		use_heap = (knn>KNN_HEAP_THRESHOLD)?true:false;
+    	}
+    	else {
+    		use_heap = (params.use_heap==FLANN_True)?true:false;
+    	}
+    	int count = 0;
+
+    	if (use_heap) {
+#pragma omp parallel num_threads(params.cores)
+    		{
+    			KNNResultSet2<DistanceType> resultSet(knn);
+#pragma omp for schedule(static) reduction(+:count)
+    			for (int i = 0; i < (int)queries.rows; i++) {
+    				resultSet.clear();
+    				findNeighbors(resultSet, queries[i], params);
+    				size_t n = std::min(resultSet.size(), knn);
+    				resultSet.copy(indices[i], dists[i], n, params.sorted);
+    				indices_to_ids(indices[i], indices[i], n);
+    				count += n;
+    			}
+    		}
+    	}
+    	else {
+#pragma omp parallel num_threads(params.cores)
+    		{
+    			KNNSimpleResultSet<DistanceType> resultSet(knn);
+#pragma omp for schedule(static) reduction(+:count)
+    			for (int i = 0; i < (int)queries.rows; i++) {
+    				resultSet.clear();
+    				findNeighbors(resultSet, queries[i], params);
+    				size_t n = std::min(resultSet.size(), knn);
+    				resultSet.copy(indices[i], dists[i], n, params.sorted);
+    				indices_to_ids(indices[i], indices[i], n);
+    				count += n;
+    			}
+    		}
+    	}
+    	return count;
+    }
+
+    /**
+     *
+     * @param queries
+     * @param indices
+     * @param dists
+     * @param knn
+     * @param params
+     * @return
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+                                 Matrix<int>& indices,
+                                 Matrix<DistanceType>& dists,
+                                 size_t knn,
+                           const SearchParams& params) const
+    {
+    	rtflann::Matrix<size_t> indices_(new size_t[indices.rows*indices.cols], indices.rows, indices.cols);
+    	int result = knnSearch(queries, indices_, dists, knn, params);
+
+    	for (size_t i=0;i<indices.rows;++i) {
+    		for (size_t j=0;j<indices.cols;++j) {
+    			indices[i][j] = indices_[i][j];
+    		}
+    	}
+        delete[] indices_.ptr();
+    	return result;
+    }
+
+
+    /**
+     * @brief Perform k-nearest neighbor search
+     * @param[in] queries The query points for which to find the nearest neighbors
+     * @param[out] indices The indices of the nearest neighbors found
+     * @param[out] dists Distances to the nearest neighbors found
+     * @param[in] knn Number of nearest neighbors to return
+     * @param[in] params Search parameters
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+					std::vector< std::vector<size_t> >& indices,
+					std::vector<std::vector<DistanceType> >& dists,
+    				size_t knn,
+    				const SearchParams& params) const
+    {
+        assert(queries.cols == veclen());
+        bool use_heap;
+        if (params.use_heap==FLANN_Undefined) {
+        	use_heap = (knn>KNN_HEAP_THRESHOLD)?true:false;
+        }
+        else {
+        	use_heap = (params.use_heap==FLANN_True)?true:false;
+        }
+
+        if (indices.size() < queries.rows ) indices.resize(queries.rows);
+		if (dists.size() < queries.rows ) dists.resize(queries.rows);
+
+		int count = 0;
+		if (use_heap) {
+#pragma omp parallel num_threads(params.cores)
+			{
+				KNNResultSet2<DistanceType> resultSet(knn);
+#pragma omp for schedule(static) reduction(+:count)
+				for (int i = 0; i < (int)queries.rows; i++) {
+					resultSet.clear();
+					findNeighbors(resultSet, queries[i], params);
+					size_t n = std::min(resultSet.size(), knn);
+					indices[i].resize(n);
+					dists[i].resize(n);
+					if (n>0) {
+						resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
+						indices_to_ids(&indices[i][0], &indices[i][0], n);
+					}
+					count += n;
+				}
+			}
+		}
+		else {
+#pragma omp parallel num_threads(params.cores)
+			{
+				KNNSimpleResultSet<DistanceType> resultSet(knn);
+#pragma omp for schedule(static) reduction(+:count)
+				for (int i = 0; i < (int)queries.rows; i++) {
+					resultSet.clear();
+					findNeighbors(resultSet, queries[i], params);
+					size_t n = std::min(resultSet.size(), knn);
+					indices[i].resize(n);
+					dists[i].resize(n);
+					if (n>0) {
+						resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
+						indices_to_ids(&indices[i][0], &indices[i][0], n);
+					}
+					count += n;
+				}
+			}
+		}
+
+		return count;
+    }
+
+
+    /**
+     *
+     * @param queries
+     * @param indices
+     * @param dists
+     * @param knn
+     * @param params
+     * @return
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+                                 std::vector< std::vector<int> >& indices,
+                                 std::vector<std::vector<DistanceType> >& dists,
+                                 size_t knn,
+                           const SearchParams& params) const
+    {
+    	std::vector<std::vector<size_t> > indices_;
+    	int result = knnSearch(queries, indices_, dists, knn, params);
+
+    	indices.resize(indices_.size());
+    	for (size_t i=0;i<indices_.size();++i) {
+            indices[i].assign(indices_[i].begin(), indices_[i].end());
+    	}
+    	return result;
+    }
+
+    /**
+     * @brief Perform radius search
+     * @param[in] query The query point
+     * @param[out] indices The indices of the neighbors found within the given radius
+     * @param[out] dists The distances to the nearest neighbors found
+     * @param[in] radius The radius used for search
+     * @param[in] params Search parameters
+     * @return Number of neighbors found
+     */
+    int radiusSearch(const Matrix<ElementType>& queries,
+    		Matrix<size_t>& indices,
+    		Matrix<DistanceType>& dists,
+    		float radius,
+    		const SearchParams& params) const
+    {
+    	assert(queries.cols == veclen());
+    	int count = 0;
+    	size_t num_neighbors = std::min(indices.cols, dists.cols);
+    	int max_neighbors = params.max_neighbors;
+    	if (max_neighbors<0) max_neighbors = num_neighbors;
+    	else max_neighbors = std::min(max_neighbors,(int)num_neighbors);
+
+    	if (max_neighbors==0) {
+#pragma omp parallel num_threads(params.cores)
+    		{
+    			CountRadiusResultSet<DistanceType> resultSet(radius);
+#pragma omp for schedule(static) reduction(+:count)
+    			for (int i = 0; i < (int)queries.rows; i++) {
+    				resultSet.clear();
+    				findNeighbors(resultSet, queries[i], params);
+    				count += resultSet.size();
+    			}
+    		}
+    	}
+    	else {
+    		// explicitly indicated to use unbounded radius result set
+    		// and we know there'll be enough room for resulting indices and dists
+    		if (params.max_neighbors<0 && (num_neighbors>=size())) {
+#pragma omp parallel num_threads(params.cores)
+    			{
+    				RadiusResultSet<DistanceType> resultSet(radius);
+#pragma omp for schedule(static) reduction(+:count)
+    				for (int i = 0; i < (int)queries.rows; i++) {
+    					resultSet.clear();
+    					findNeighbors(resultSet, queries[i], params);
+    					size_t n = resultSet.size();
+    					count += n;
+    					if (n>num_neighbors) n = num_neighbors;
+    					resultSet.copy(indices[i], dists[i], n, params.sorted);
+
+    					// mark the next element in the output buffers as unused
+    					if (n<indices.cols) indices[i][n] = size_t(-1);
+    					if (n<dists.cols) dists[i][n] = std::numeric_limits<DistanceType>::infinity();
+    					indices_to_ids(indices[i], indices[i], n);
+    				}
+    			}
+    		}
+    		else {
+    			// number of neighbors limited to max_neighbors
+#pragma omp parallel num_threads(params.cores)
+    			{
+    				KNNRadiusResultSet<DistanceType> resultSet(radius, max_neighbors);
+#pragma omp for schedule(static) reduction(+:count)
+    				for (int i = 0; i < (int)queries.rows; i++) {
+    					resultSet.clear();
+    					findNeighbors(resultSet, queries[i], params);
+    					size_t n = resultSet.size();
+    					count += n;
+    					if ((int)n>max_neighbors) n = max_neighbors;
+    					resultSet.copy(indices[i], dists[i], n, params.sorted);
+
+    					// mark the next element in the output buffers as unused
+    					if (n<indices.cols) indices[i][n] = size_t(-1);
+    					if (n<dists.cols) dists[i][n] = std::numeric_limits<DistanceType>::infinity();
+    					indices_to_ids(indices[i], indices[i], n);
+    				}
+    			}
+    		}
+    	}
+        return count;
+    }
+
+
+    /**
+     *
+     * @param queries
+     * @param indices
+     * @param dists
+     * @param radius
+     * @param params
+     * @return
+     */
+    int radiusSearch(const Matrix<ElementType>& queries,
+                                    Matrix<int>& indices,
+                                    Matrix<DistanceType>& dists,
+                                    float radius,
+                              const SearchParams& params) const
+    {
+    	rtflann::Matrix<size_t> indices_(new size_t[indices.rows*indices.cols], indices.rows, indices.cols);
+    	int result = radiusSearch(queries, indices_, dists, radius, params);
+
+    	for (size_t i=0;i<indices.rows;++i) {
+    		for (size_t j=0;j<indices.cols;++j) {
+    			indices[i][j] = indices_[i][j];
+    		}
+    	}
+        delete[] indices_.ptr();
+    	return result;
+    }
+
+    /**
+     * @brief Perform radius search
+     * @param[in] query The query point
+     * @param[out] indices The indices of the neighbors found within the given radius
+     * @param[out] dists The distances to the nearest neighbors found
+     * @param[in] radius The radius used for search
+     * @param[in] params Search parameters
+     * @return Number of neighbors found
+     */
+    int radiusSearch(const Matrix<ElementType>& queries,
+    		std::vector< std::vector<size_t> >& indices,
+    		std::vector<std::vector<DistanceType> >& dists,
+    		float radius,
+    		const SearchParams& params) const
+    {
+        assert(queries.cols == veclen());
+    	int count = 0;
+    	// just count neighbors
+    	if (params.max_neighbors==0) {
+#pragma omp parallel num_threads(params.cores)
+    		{
+    			CountRadiusResultSet<DistanceType> resultSet(radius);
+#pragma omp for schedule(static) reduction(+:count)
+    			for (int i = 0; i < (int)queries.rows; i++) {
+    				resultSet.clear();
+    				findNeighbors(resultSet, queries[i], params);
+    				count += resultSet.size();
+    			}
+    		}
+    	}
+    	else {
+    		if (indices.size() < queries.rows ) indices.resize(queries.rows);
+    		if (dists.size() < queries.rows ) dists.resize(queries.rows);
+
+    		if (params.max_neighbors<0) {
+    			// search for all neighbors
+#pragma omp parallel num_threads(params.cores)
+    			{
+    				RadiusResultSet<DistanceType> resultSet(radius);
+#pragma omp for schedule(static) reduction(+:count)
+    				for (int i = 0; i < (int)queries.rows; i++) {
+    					resultSet.clear();
+    					findNeighbors(resultSet, queries[i], params);
+    					size_t n = resultSet.size();
+    					count += n;
+    					indices[i].resize(n);
+    					dists[i].resize(n);
+    					if (n > 0) {
+    						resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
+        					indices_to_ids(&indices[i][0], &indices[i][0], n);
+    					}
+    				}
+    			}
+    		}
+    		else {
+    			// number of neighbors limited to max_neighbors
+#pragma omp parallel num_threads(params.cores)
+    			{
+    				KNNRadiusResultSet<DistanceType> resultSet(radius, params.max_neighbors);
+#pragma omp for schedule(static) reduction(+:count)
+    				for (int i = 0; i < (int)queries.rows; i++) {
+    					resultSet.clear();
+    					findNeighbors(resultSet, queries[i], params);
+    					size_t n = resultSet.size();
+    					count += n;
+    					if ((int)n>params.max_neighbors) n = params.max_neighbors;
+    					indices[i].resize(n);
+    					dists[i].resize(n);
+    					if (n > 0) {
+    						resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
+        					indices_to_ids(&indices[i][0], &indices[i][0], n);
+    					}
+    				}
+    			}
+    		}
+    	}
+    	return count;
+    }
+
+    /**
+     *
+     * @param queries
+     * @param indices
+     * @param dists
+     * @param radius
+     * @param params
+     * @return
+     */
+    int radiusSearch(const Matrix<ElementType>& queries,
+                                    std::vector< std::vector<int> >& indices,
+                                    std::vector<std::vector<DistanceType> >& dists,
+                                    float radius,
+                              const SearchParams& params) const
+    {
+    	std::vector<std::vector<size_t> > indices_;
+    	int result = radiusSearch(queries, indices_, dists, radius, params);
+
+    	indices.resize(indices_.size());
+    	for (size_t i=0;i<indices_.size();++i) {
+            indices[i].assign(indices_[i].begin(), indices_[i].end());
+    	}
+    	return result;
+    }
+
+
+    virtual void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const = 0;
+
+protected:
+
+    virtual void freeIndex() = 0;
+
+    virtual void buildIndexImpl() = 0;
+
+    size_t id_to_index(size_t id)
+    {
+    	if (ids_.size()==0) {
+    		return id;
+    	}
+    	size_t point_index = size_t(-1);
+    	if (id < ids_.size() && ids_[id]==id) {
+    		return id;
+    	}
+    	else {
+    		// binary search
+    		size_t start = 0;
+    		size_t end = ids_.size();
+
+    		while (start<end) {
+    			size_t mid = (start+end)/2;
+    			if (ids_[mid]==id) {
+    				point_index = mid;
+    				break;
+    			}
+    			else if (ids_[mid]<id) {
+    				start = mid + 1;
+    			}
+    			else {
+    				end = mid;
+    			}
+    		}
+    	}
+    	return point_index;
+    }
+
+
+    void indices_to_ids(const size_t* in, size_t* out, size_t size) const
+    {
+		if (removed_) {
+			for (size_t i=0;i<size;++i) {
+				out[i] = ids_[in[i]];
+			}
+		}
+    }
+
+    void setDataset(const Matrix<ElementType>& dataset)
+    {
+    	size_ = dataset.rows;
+    	veclen_ = dataset.cols;
+    	last_id_ = 0;
+
+    	ids_.clear();
+    	removed_points_.clear();
+    	removed_ = false;
+    	removed_count_ = 0;
+
+    	points_.resize(size_);
+    	for (size_t i=0;i<size_;++i) {
+    		points_[i] = dataset[i];
+    	}
+    }
+
+    void extendDataset(const Matrix<ElementType>& new_points)
+    {
+    	size_t new_size = size_ + new_points.rows;
+    	if (removed_) {
+    		removed_points_.resize(new_size);
+    		ids_.resize(new_size);
+    	}
+    	points_.resize(new_size);
+    	for (size_t i=size_;i<new_size;++i) {
+    		points_[i] = new_points[i-size_];
+    		if (removed_) {
+    			ids_[i] = last_id_++;
+    			removed_points_.reset(i);
+    		}
+    	}
+    	size_ = new_size;
+    }
+
+
+    void cleanRemovedPoints()
+    {
+    	if (!removed_) return;
+
+    	size_t last_idx = 0;
+    	for (size_t i=0;i<size_;++i) {
+    		if (!removed_points_.test(i)) {
+    			points_[last_idx] = points_[i];
+    			ids_[last_idx] = ids_[i];
+    			removed_points_.reset(last_idx);
+    			++last_idx;
+    		}
+    	}
+    	points_.resize(last_idx);
+    	ids_.resize(last_idx);
+    	removed_points_.resize(last_idx);
+    	size_ = last_idx;
+    	removed_count_ = 0;
+    }
+
+    void swap(NNIndex& other)
+    {
+    	std::swap(distance_, other.distance_);
+    	std::swap(last_id_, other.last_id_);
+    	std::swap(size_, other.size_);
+    	std::swap(size_at_build_, other.size_at_build_);
+    	std::swap(veclen_, other.veclen_);
+    	std::swap(index_params_, other.index_params_);
+    	std::swap(removed_, other.removed_);
+    	std::swap(removed_points_, other.removed_points_);
+    	std::swap(removed_count_, other.removed_count_);
+    	std::swap(ids_, other.ids_);
+    	std::swap(points_, other.points_);
+    	std::swap(data_ptr_, other.data_ptr_);
+    }
+
+protected:
+
+    /**
+     * The distance functor
+     */
+    Distance distance_;
+
+
+    /**
+     * Each index point has an associated ID. IDs are assigned sequentially in
+     * increasing order. This indicates the ID assigned to the last point added to the
+     * index.
+     */
+    size_t last_id_;
+
+    /**
+     * Number of points in the index (and database)
+     */
+    size_t size_;
+
+    /**
+     * Number of features in the dataset when the index was last built.
+     */
+    size_t size_at_build_;
+
+    /**
+     * Size of one point in the index (and database)
+     */
+    size_t veclen_;
+
+    /**
+     * Parameters of the index.
+     */
+    IndexParams index_params_;
+
+    /**
+     * Flag indicating if at least a point was removed from the index
+     */
+    bool removed_;
+
+    /**
+     * Array used to mark points removed from the index
+     */
+    DynamicBitset removed_points_;
+
+    /**
+     * Number of points removed from the index
+     */
+    size_t removed_count_;
+
+    /**
+     * Array of point IDs, returned by nearest-neighbour operations
+     */
+    std::vector<size_t> ids_;
+
+    /**
+     * Point data
+     */
+    std::vector<ElementType*> points_;
+
+    /**
+     * Pointer to dataset memory if allocated by this index, otherwise NULL
+     */
+    ElementType* data_ptr_;
+
+
+};
+
+
+#define USING_BASECLASS_SYMBOLS \
+		using NNIndex<Distance>::distance_;\
+		using NNIndex<Distance>::size_;\
+		using NNIndex<Distance>::size_at_build_;\
+		using NNIndex<Distance>::veclen_;\
+		using NNIndex<Distance>::index_params_;\
+		using NNIndex<Distance>::removed_points_;\
+		using NNIndex<Distance>::ids_;\
+		using NNIndex<Distance>::removed_;\
+		using NNIndex<Distance>::points_;\
+		using NNIndex<Distance>::extendDataset;\
+		using NNIndex<Distance>::setDataset;\
+		using NNIndex<Distance>::cleanRemovedPoints;\
+		using NNIndex<Distance>::indices_to_ids;
+
+
+
+}
+
+
+#endif //FLANN_NNINDEX_H

corelib/src/rtflann/config.h

@@ -0,0 +1,38 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2011  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2011  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_CONFIG_H_
+#define RTABMAP_FLANN_CONFIG_H_
+
+#ifdef FLANN_VERSION_
+#undef FLANN_VERSION_
+#endif
+#define FLANN_VERSION_ "1.8.4"
+
+#endif /* RTABMAP_FLANN_CONFIG_H_ */

corelib/src/rtflann/defines.h

@@ -0,0 +1,156 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2011  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2011  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_DEFINES_H_
+#define RTABMAP_FLANN_DEFINES_H_
+
+#include "config.h"
+
+#ifdef FLANN_EXPORT
+#undef FLANN_EXPORT
+#endif
+#ifdef WIN32
+/* win32 dll export/import directives */
+ #ifdef FLANN_EXPORTS
+  #define FLANN_EXPORT __declspec(dllexport)
+ #elif defined(FLANN_STATIC)
+  #define FLANN_EXPORT
+ #else
+  #define FLANN_EXPORT __declspec(dllimport)
+ #endif
+#else
+/* unix needs nothing */
+ #define FLANN_EXPORT
+#endif
+
+#ifdef FLANN_DEPRECATED
+#undef FLANN_DEPRECATED
+#endif
+#ifdef __GNUC__
+#define FLANN_DEPRECATED __attribute__ ((deprecated))
+#elif defined(_MSC_VER)
+#define FLANN_DEPRECATED __declspec(deprecated)
+#else
+#pragma message("WARNING: You need to implement FLANN_DEPRECATED for this compiler")
+#define FLANN_DEPRECATED
+#endif
+
+#undef FLANN_PLATFORM_64_BIT
+#undef FLANN_PLATFORM_32_BIT
+#if __amd64__ || __x86_64__ || _WIN64 || _M_X64
+#define FLANN_PLATFORM_64_BIT
+#else
+#define FLANN_PLATFORM_32_BIT
+#endif
+
+#undef FLANN_ARRAY_LEN
+#define FLANN_ARRAY_LEN(a) (sizeof(a)/sizeof(a[0]))
+
+//#ifdef __cplusplus
+namespace rtflann {
+//#endif
+
+/* Nearest neighbour index algorithms */
+enum flann_algorithm_t
+{
+    FLANN_INDEX_LINEAR 			= 0,
+    FLANN_INDEX_KDTREE 			= 1,
+    FLANN_INDEX_KMEANS 			= 2,
+    FLANN_INDEX_COMPOSITE 		= 3,
+    FLANN_INDEX_KDTREE_SINGLE 	= 4,
+    FLANN_INDEX_HIERARCHICAL 	= 5,
+    FLANN_INDEX_LSH 			= 6,
+#ifdef FLANN_USE_CUDA
+    FLANN_INDEX_KDTREE_CUDA 	= 7,
+#endif
+    FLANN_INDEX_SAVED 			= 254,
+    FLANN_INDEX_AUTOTUNED 		= 255,
+};
+
+enum flann_centers_init_t
+{
+    FLANN_CENTERS_RANDOM = 0,
+    FLANN_CENTERS_GONZALES = 1,
+    FLANN_CENTERS_KMEANSPP = 2,
+    FLANN_CENTERS_GROUPWISE = 3,
+};
+
+enum flann_log_level_t
+{
+    FLANN_LOG_NONE = 0,
+    FLANN_LOG_FATAL = 1,
+    FLANN_LOG_ERROR = 2,
+    FLANN_LOG_WARN = 3,
+    FLANN_LOG_INFO = 4,
+    FLANN_LOG_DEBUG = 5
+};
+
+enum flann_distance_t
+{
+    FLANN_DIST_EUCLIDEAN 			= 1,
+    FLANN_DIST_L2 					= 1,
+    FLANN_DIST_MANHATTAN 			= 2,
+    FLANN_DIST_L1 					= 2,
+    FLANN_DIST_MINKOWSKI 			= 3,
+    FLANN_DIST_MAX   				= 4,
+    FLANN_DIST_HIST_INTERSECT  		= 5,
+    FLANN_DIST_HELLINGER 			= 6,
+    FLANN_DIST_CHI_SQUARE		 	= 7,
+    FLANN_DIST_KULLBACK_LEIBLER  	= 8,
+    FLANN_DIST_HAMMING         		= 9,
+    FLANN_DIST_HAMMING_LUT			= 10,
+    FLANN_DIST_HAMMING_POPCNT   	= 11,
+    FLANN_DIST_L2_SIMPLE	   		= 12,
+};
+
+enum flann_datatype_t
+{
+    FLANN_NONE 		= -1,
+    FLANN_INT8 		= 0,
+    FLANN_INT16 	= 1,
+    FLANN_INT32 	= 2,
+    FLANN_INT64 	= 3,
+    FLANN_UINT8 	= 4,
+    FLANN_UINT16 	= 5,
+    FLANN_UINT32 	= 6,
+    FLANN_UINT64 	= 7,
+    FLANN_FLOAT32 	= 8,
+    FLANN_FLOAT64 	= 9
+};
+
+enum flann_checks_t {
+    FLANN_CHECKS_UNLIMITED = -1,
+    FLANN_CHECKS_AUTOTUNED = -2,
+};
+
+//#ifdef __cplusplus
+}
+//#endif
+
+
+#endif /* RTABMAP_FLANN_DEFINES_H_ */

corelib/src/rtflann/ext/lz4.h

@@ -0,0 +1,360 @@
+/*
+   LZ4 - Fast LZ compression algorithm
+   Header File
+   Copyright (C) 2011-2015, Yann Collet.
+
+   BSD 2-Clause License (http://www.opensource.org/licenses/bsd-license.php)
+
+   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.
+
+   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
+   OWNER 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.
+
+   You can contact the author at :
+   - LZ4 source repository : https://github.com/Cyan4973/lz4
+   - LZ4 public forum : https://groups.google.com/forum/#!forum/lz4c
+*/
+#pragma once
+
+#if defined (__cplusplus)
+extern "C" {
+#endif
+
+/*
+ * lz4.h provides block compression functions, and gives full buffer control to programmer.
+ * If you need to generate inter-operable compressed data (respecting LZ4 frame specification),
+ * and can let the library handle its own memory, please use lz4frame.h instead.
+*/
+
+/**************************************
+*  Version
+**************************************/
+#define LZ4_VERSION_MAJOR    1    /* for breaking interface changes  */
+#define LZ4_VERSION_MINOR    7    /* for new (non-breaking) interface capabilities */
+#define LZ4_VERSION_RELEASE  1    /* for tweaks, bug-fixes, or development */
+#define LZ4_VERSION_NUMBER (LZ4_VERSION_MAJOR *100*100 + LZ4_VERSION_MINOR *100 + LZ4_VERSION_RELEASE)
+int LZ4_versionNumber (void);
+
+/**************************************
+*  Tuning parameter
+**************************************/
+/*
+ * LZ4_MEMORY_USAGE :
+ * Memory usage formula : N->2^N Bytes (examples : 10 -> 1KB; 12 -> 4KB ; 16 -> 64KB; 20 -> 1MB; etc.)
+ * Increasing memory usage improves compression ratio
+ * Reduced memory usage can improve speed, due to cache effect
+ * Default value is 14, for 16KB, which nicely fits into Intel x86 L1 cache
+ */
+#define LZ4_MEMORY_USAGE 14
+
+
+/**************************************
+*  Simple Functions
+**************************************/
+
+int LZ4_compress_default(const char* source, char* dest, int sourceSize, int maxDestSize);
+int LZ4_decompress_safe (const char* source, char* dest, int compressedSize, int maxDecompressedSize);
+
+/*
+LZ4_compress_default() :
+    Compresses 'sourceSize' bytes from buffer 'source'
+    into already allocated 'dest' buffer of size 'maxDestSize'.
+    Compression is guaranteed to succeed if 'maxDestSize' >= LZ4_compressBound(sourceSize).
+    It also runs faster, so it's a recommended setting.
+    If the function cannot compress 'source' into a more limited 'dest' budget,
+    compression stops *immediately*, and the function result is zero.
+    As a consequence, 'dest' content is not valid.
+    This function never writes outside 'dest' buffer, nor read outside 'source' buffer.
+        sourceSize  : Max supported value is LZ4_MAX_INPUT_VALUE
+        maxDestSize : full or partial size of buffer 'dest' (which must be already allocated)
+        return : the number of bytes written into buffer 'dest' (necessarily <= maxOutputSize)
+              or 0 if compression fails
+
+LZ4_decompress_safe() :
+    compressedSize : is the precise full size of the compressed block.
+    maxDecompressedSize : is the size of destination buffer, which must be already allocated.
+    return : the number of bytes decompressed into destination buffer (necessarily <= maxDecompressedSize)
+             If destination buffer is not large enough, decoding will stop and output an error code (<0).
+             If the source stream is detected malformed, the function will stop decoding and return a negative result.
+             This function is protected against buffer overflow exploits, including malicious data packets.
+             It never writes outside output buffer, nor reads outside input buffer.
+*/
+
+
+/**************************************
+*  Advanced Functions
+**************************************/
+#define LZ4_MAX_INPUT_SIZE        0x7E000000   /* 2 113 929 216 bytes */
+#define LZ4_COMPRESSBOUND(isize)  ((unsigned)(isize) > (unsigned)LZ4_MAX_INPUT_SIZE ? 0 : (isize) + ((isize)/255) + 16)
+
+/*
+LZ4_compressBound() :
+    Provides the maximum size that LZ4 compression may output in a "worst case" scenario (input data not compressible)
+    This function is primarily useful for memory allocation purposes (destination buffer size).
+    Macro LZ4_COMPRESSBOUND() is also provided for compilation-time evaluation (stack memory allocation for example).
+    Note that LZ4_compress_default() compress faster when dest buffer size is >= LZ4_compressBound(srcSize)
+        inputSize  : max supported value is LZ4_MAX_INPUT_SIZE
+        return : maximum output size in a "worst case" scenario
+              or 0, if input size is too large ( > LZ4_MAX_INPUT_SIZE)
+*/
+int LZ4_compressBound(int inputSize);
+
+/*
+LZ4_compress_fast() :
+    Same as LZ4_compress_default(), but allows to select an "acceleration" factor.
+    The larger the acceleration value, the faster the algorithm, but also the lesser the compression.
+    It's a trade-off. It can be fine tuned, with each successive value providing roughly +~3% to speed.
+    An acceleration value of "1" is the same as regular LZ4_compress_default()
+    Values <= 0 will be replaced by ACCELERATION_DEFAULT (see lz4.c), which is 1.
+*/
+int LZ4_compress_fast (const char* source, char* dest, int sourceSize, int maxDestSize, int acceleration);
+
+
+/*
+LZ4_compress_fast_extState() :
+    Same compression function, just using an externally allocated memory space to store compression state.
+    Use LZ4_sizeofState() to know how much memory must be allocated,
+    and allocate it on 8-bytes boundaries (using malloc() typically).
+    Then, provide it as 'void* state' to compression function.
+*/
+int LZ4_sizeofState(void);
+int LZ4_compress_fast_extState (void* state, const char* source, char* dest, int inputSize, int maxDestSize, int acceleration);
+
+
+/*
+LZ4_compress_destSize() :
+    Reverse the logic, by compressing as much data as possible from 'source' buffer
+    into already allocated buffer 'dest' of size 'targetDestSize'.
+    This function either compresses the entire 'source' content into 'dest' if it's large enough,
+    or fill 'dest' buffer completely with as much data as possible from 'source'.
+        *sourceSizePtr : will be modified to indicate how many bytes where read from 'source' to fill 'dest'.
+                         New value is necessarily <= old value.
+        return : Nb bytes written into 'dest' (necessarily <= targetDestSize)
+              or 0 if compression fails
+*/
+int LZ4_compress_destSize (const char* source, char* dest, int* sourceSizePtr, int targetDestSize);
+
+
+/*
+LZ4_decompress_fast() :
+    originalSize : is the original and therefore uncompressed size
+    return : the number of bytes read from the source buffer (in other words, the compressed size)
+             If the source stream is detected malformed, the function will stop decoding and return a negative result.
+             Destination buffer must be already allocated. Its size must be a minimum of 'originalSize' bytes.
+    note : This function fully respect memory boundaries for properly formed compressed data.
+           It is a bit faster than LZ4_decompress_safe().
+           However, it does not provide any protection against intentionally modified data stream (malicious input).
+           Use this function in trusted environment only (data to decode comes from a trusted source).
+*/
+int LZ4_decompress_fast (const char* source, char* dest, int originalSize);
+
+/*
+LZ4_decompress_safe_partial() :
+    This function decompress a compressed block of size 'compressedSize' at position 'source'
+    into destination buffer 'dest' of size 'maxDecompressedSize'.
+    The function tries to stop decompressing operation as soon as 'targetOutputSize' has been reached,
+    reducing decompression time.
+    return : the number of bytes decoded in the destination buffer (necessarily <= maxDecompressedSize)
+       Note : this number can be < 'targetOutputSize' should the compressed block to decode be smaller.
+             Always control how many bytes were decoded.
+             If the source stream is detected malformed, the function will stop decoding and return a negative result.
+             This function never writes outside of output buffer, and never reads outside of input buffer. It is therefore protected against malicious data packets
+*/
+int LZ4_decompress_safe_partial (const char* source, char* dest, int compressedSize, int targetOutputSize, int maxDecompressedSize);
+
+
+/***********************************************
+*  Streaming Compression Functions
+***********************************************/
+#define LZ4_STREAMSIZE_U64 ((1 << (LZ4_MEMORY_USAGE-3)) + 4)
+#define LZ4_STREAMSIZE     (LZ4_STREAMSIZE_U64 * sizeof(long long))
+/*
+ * LZ4_stream_t
+ * information structure to track an LZ4 stream.
+ * important : init this structure content before first use !
+ * note : only allocated directly the structure if you are statically linking LZ4
+ *        If you are using liblz4 as a DLL, please use below construction methods instead.
+ */
+typedef struct { long long table[LZ4_STREAMSIZE_U64]; } LZ4_stream_t;
+
+/*
+ * LZ4_resetStream
+ * Use this function to init an allocated LZ4_stream_t structure
+ */
+void LZ4_resetStream (LZ4_stream_t* streamPtr);
+
+/*
+ * LZ4_createStream will allocate and initialize an LZ4_stream_t structure
+ * LZ4_freeStream releases its memory.
+ * In the context of a DLL (liblz4), please use these methods rather than the static struct.
+ * They are more future proof, in case of a change of LZ4_stream_t size.
+ */
+LZ4_stream_t* LZ4_createStream(void);
+int           LZ4_freeStream (LZ4_stream_t* streamPtr);
+
+/*
+ * LZ4_loadDict
+ * Use this function to load a static dictionary into LZ4_stream.
+ * Any previous data will be forgotten, only 'dictionary' will remain in memory.
+ * Loading a size of 0 is allowed.
+ * Return : dictionary size, in bytes (necessarily <= 64 KB)
+ */
+int LZ4_loadDict (LZ4_stream_t* streamPtr, const char* dictionary, int dictSize);
+
+/*
+ * LZ4_compress_fast_continue
+ * Compress buffer content 'src', using data from previously compressed blocks as dictionary to improve compression ratio.
+ * Important : Previous data blocks are assumed to still be present and unmodified !
+ * 'dst' buffer must be already allocated.
+ * If maxDstSize >= LZ4_compressBound(srcSize), compression is guaranteed to succeed, and runs faster.
+ * If not, and if compressed data cannot fit into 'dst' buffer size, compression stops, and function returns a zero.
+ */
+int LZ4_compress_fast_continue (LZ4_stream_t* streamPtr, const char* src, char* dst, int srcSize, int maxDstSize, int acceleration);
+
+/*
+ * LZ4_saveDict
+ * If previously compressed data block is not guaranteed to remain available at its memory location
+ * save it into a safer place (char* safeBuffer)
+ * Note : you don't need to call LZ4_loadDict() afterwards,
+ *        dictionary is immediately usable, you can therefore call LZ4_compress_fast_continue()
+ * Return : saved dictionary size in bytes (necessarily <= dictSize), or 0 if error
+ */
+int LZ4_saveDict (LZ4_stream_t* streamPtr, char* safeBuffer, int dictSize);
+
+
+/************************************************
+*  Streaming Decompression Functions
+************************************************/
+
+#define LZ4_STREAMDECODESIZE_U64  4
+#define LZ4_STREAMDECODESIZE     (LZ4_STREAMDECODESIZE_U64 * sizeof(unsigned long long))
+typedef struct { unsigned long long table[LZ4_STREAMDECODESIZE_U64]; } LZ4_streamDecode_t;
+/*
+ * LZ4_streamDecode_t
+ * information structure to track an LZ4 stream.
+ * init this structure content using LZ4_setStreamDecode or memset() before first use !
+ *
+ * In the context of a DLL (liblz4) please prefer usage of construction methods below.
+ * They are more future proof, in case of a change of LZ4_streamDecode_t size in the future.
+ * LZ4_createStreamDecode will allocate and initialize an LZ4_streamDecode_t structure
+ * LZ4_freeStreamDecode releases its memory.
+ */
+LZ4_streamDecode_t* LZ4_createStreamDecode(void);
+int                 LZ4_freeStreamDecode (LZ4_streamDecode_t* LZ4_stream);
+
+/*
+ * LZ4_setStreamDecode
+ * Use this function to instruct where to find the dictionary.
+ * Setting a size of 0 is allowed (same effect as reset).
+ * Return : 1 if OK, 0 if error
+ */
+int LZ4_setStreamDecode (LZ4_streamDecode_t* LZ4_streamDecode, const char* dictionary, int dictSize);
+
+/*
+*_continue() :
+    These decoding functions allow decompression of multiple blocks in "streaming" mode.
+    Previously decoded blocks *must* remain available at the memory position where they were decoded (up to 64 KB)
+    In the case of a ring buffers, decoding buffer must be either :
+    - Exactly same size as encoding buffer, with same update rule (block boundaries at same positions)
+      In which case, the decoding & encoding ring buffer can have any size, including very small ones ( < 64 KB).
+    - Larger than encoding buffer, by a minimum of maxBlockSize more bytes.
+      maxBlockSize is implementation dependent. It's the maximum size you intend to compress into a single block.
+      In which case, encoding and decoding buffers do not need to be synchronized,
+      and encoding ring buffer can have any size, including small ones ( < 64 KB).
+    - _At least_ 64 KB + 8 bytes + maxBlockSize.
+      In which case, encoding and decoding buffers do not need to be synchronized,
+      and encoding ring buffer can have any size, including larger than decoding buffer.
+    Whenever these conditions are not possible, save the last 64KB of decoded data into a safe buffer,
+    and indicate where it is saved using LZ4_setStreamDecode()
+*/
+int LZ4_decompress_safe_continue (LZ4_streamDecode_t* LZ4_streamDecode, const char* source, char* dest, int compressedSize, int maxDecompressedSize);
+int LZ4_decompress_fast_continue (LZ4_streamDecode_t* LZ4_streamDecode, const char* source, char* dest, int originalSize);
+
+
+/*
+Advanced decoding functions :
+*_usingDict() :
+    These decoding functions work the same as
+    a combination of LZ4_setStreamDecode() followed by LZ4_decompress_x_continue()
+    They are stand-alone. They don't need nor update an LZ4_streamDecode_t structure.
+*/
+int LZ4_decompress_safe_usingDict (const char* source, char* dest, int compressedSize, int maxDecompressedSize, const char* dictStart, int dictSize);
+int LZ4_decompress_fast_usingDict (const char* source, char* dest, int originalSize, const char* dictStart, int dictSize);
+
+
+
+/**************************************
+*  Obsolete Functions
+**************************************/
+/* Deprecate Warnings */
+/* Should these warnings messages be a problem,
+   it is generally possible to disable them,
+   with -Wno-deprecated-declarations for gcc
+   or _CRT_SECURE_NO_WARNINGS in Visual for example.
+   You can also define LZ4_DEPRECATE_WARNING_DEFBLOCK. */
+#ifndef LZ4_DEPRECATE_WARNING_DEFBLOCK
+#  define LZ4_DEPRECATE_WARNING_DEFBLOCK
+#  define LZ4_GCC_VERSION (__GNUC__ * 100 + __GNUC_MINOR__)
+#  if (LZ4_GCC_VERSION >= 405) || defined(__clang__)
+#    define LZ4_DEPRECATED(message) __attribute__((deprecated(message)))
+#  elif (LZ4_GCC_VERSION >= 301)
+#    define LZ4_DEPRECATED(message) __attribute__((deprecated))
+#  elif defined(_MSC_VER)
+#    define LZ4_DEPRECATED(message) __declspec(deprecated(message))
+#  else
+#    pragma message("WARNING: You need to implement LZ4_DEPRECATED for this compiler")
+#    define LZ4_DEPRECATED(message)
+#  endif
+#endif /* LZ4_DEPRECATE_WARNING_DEFBLOCK */
+
+/* Obsolete compression functions */
+/* These functions are planned to start generate warnings by r131 approximately */
+int LZ4_compress               (const char* source, char* dest, int sourceSize);
+int LZ4_compress_limitedOutput (const char* source, char* dest, int sourceSize, int maxOutputSize);
+int LZ4_compress_withState               (void* state, const char* source, char* dest, int inputSize);
+int LZ4_compress_limitedOutput_withState (void* state, const char* source, char* dest, int inputSize, int maxOutputSize);
+int LZ4_compress_continue                (LZ4_stream_t* LZ4_streamPtr, const char* source, char* dest, int inputSize);
+int LZ4_compress_limitedOutput_continue  (LZ4_stream_t* LZ4_streamPtr, const char* source, char* dest, int inputSize, int maxOutputSize);
+
+/* Obsolete decompression functions */
+/* These function names are completely deprecated and must no longer be used.
+   They are only provided here for compatibility with older programs.
+    - LZ4_uncompress is the same as LZ4_decompress_fast
+    - LZ4_uncompress_unknownOutputSize is the same as LZ4_decompress_safe
+   These function prototypes are now disabled; uncomment them only if you really need them.
+   It is highly recommended to stop using these prototypes and migrate to maintained ones */
+/* int LZ4_uncompress (const char* source, char* dest, int outputSize); */
+/* int LZ4_uncompress_unknownOutputSize (const char* source, char* dest, int isize, int maxOutputSize); */
+
+/* Obsolete streaming functions; use new streaming interface whenever possible */
+LZ4_DEPRECATED("use LZ4_createStream() instead") void* LZ4_create (char* inputBuffer);
+LZ4_DEPRECATED("use LZ4_createStream() instead") int   LZ4_sizeofStreamState(void);
+LZ4_DEPRECATED("use LZ4_resetStream() instead")  int   LZ4_resetStreamState(void* state, char* inputBuffer);
+LZ4_DEPRECATED("use LZ4_saveDict() instead")     char* LZ4_slideInputBuffer (void* state);
+
+/* Obsolete streaming decoding functions */
+LZ4_DEPRECATED("use LZ4_decompress_safe_usingDict() instead") int LZ4_decompress_safe_withPrefix64k (const char* src, char* dst, int compressedSize, int maxDstSize);
+LZ4_DEPRECATED("use LZ4_decompress_fast_usingDict() instead") int LZ4_decompress_fast_withPrefix64k (const char* src, char* dst, int originalSize);
+
+
+#if defined (__cplusplus)
+}
+#endif

corelib/src/rtflann/ext/lz4hc.c

@@ -0,0 +1,731 @@
+/*
+    LZ4 HC - High Compression Mode of LZ4
+    Copyright (C) 2011-2015, Yann Collet.
+
+    BSD 2-Clause License (http://www.opensource.org/licenses/bsd-license.php)
+
+    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.
+
+    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
+    OWNER 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.
+
+    You can contact the author at :
+       - LZ4 source repository : https://github.com/Cyan4973/lz4
+       - LZ4 public forum : https://groups.google.com/forum/#!forum/lz4c
+*/
+
+
+
+/**************************************
+*  Tuning Parameter
+**************************************/
+static const int LZ4HC_compressionLevel_default = 9;
+
+
+/**************************************
+*  Includes
+**************************************/
+#include "lz4hc.h"
+
+
+/**************************************
+*  Local Compiler Options
+**************************************/
+#if defined(__GNUC__)
+#  pragma GCC diagnostic ignored "-Wunused-function"
+#endif
+
+#if defined (__clang__)
+#  pragma clang diagnostic ignored "-Wunused-function"
+#endif
+
+
+/**************************************
+*  Common LZ4 definition
+**************************************/
+#define LZ4_COMMONDEFS_ONLY
+#include "lz4.c"
+
+
+/**************************************
+*  Local Constants
+**************************************/
+#define DICTIONARY_LOGSIZE 16
+#define MAXD (1<<DICTIONARY_LOGSIZE)
+#define MAXD_MASK (MAXD - 1)
+
+#define HASH_LOG (DICTIONARY_LOGSIZE-1)
+#define HASHTABLESIZE (1 << HASH_LOG)
+#define HASH_MASK (HASHTABLESIZE - 1)
+
+#define OPTIMAL_ML (int)((ML_MASK-1)+MINMATCH)
+
+static const int g_maxCompressionLevel = 16;
+
+
+/**************************************
+*  Local Types
+**************************************/
+typedef struct
+{
+    U32   hashTable[HASHTABLESIZE];
+    U16   chainTable[MAXD];
+    const BYTE* end;        /* next block here to continue on current prefix */
+    const BYTE* base;       /* All index relative to this position */
+    const BYTE* dictBase;   /* alternate base for extDict */
+    BYTE* inputBuffer;      /* deprecated */
+    U32   dictLimit;        /* below that point, need extDict */
+    U32   lowLimit;         /* below that point, no more dict */
+    U32   nextToUpdate;     /* index from which to continue dictionary update */
+    U32   compressionLevel;
+} LZ4HC_Data_Structure;
+
+
+/**************************************
+*  Local Macros
+**************************************/
+#define HASH_FUNCTION(i)       (((i) * 2654435761U) >> ((MINMATCH*8)-HASH_LOG))
+//#define DELTANEXTU16(p)        chainTable[(p) & MAXD_MASK]   /* flexible, MAXD dependent */
+#define DELTANEXTU16(p)        chainTable[(U16)(p)]   /* faster */
+
+static U32 LZ4HC_hashPtr(const void* ptr) { return HASH_FUNCTION(LZ4_read32(ptr)); }
+
+
+
+/**************************************
+*  HC Compression
+**************************************/
+static void LZ4HC_init (LZ4HC_Data_Structure* hc4, const BYTE* start)
+{
+    MEM_INIT((void*)hc4->hashTable, 0, sizeof(hc4->hashTable));
+    MEM_INIT(hc4->chainTable, 0xFF, sizeof(hc4->chainTable));
+    hc4->nextToUpdate = 64 KB;
+    hc4->base = start - 64 KB;
+    hc4->end = start;
+    hc4->dictBase = start - 64 KB;
+    hc4->dictLimit = 64 KB;
+    hc4->lowLimit = 64 KB;
+}
+
+
+/* Update chains up to ip (excluded) */
+FORCE_INLINE void LZ4HC_Insert (LZ4HC_Data_Structure* hc4, const BYTE* ip)
+{
+    U16* chainTable = hc4->chainTable;
+    U32* HashTable  = hc4->hashTable;
+    const BYTE* const base = hc4->base;
+    const U32 target = (U32)(ip - base);
+    U32 idx = hc4->nextToUpdate;
+
+    while(idx < target)
+    {
+        U32 h = LZ4HC_hashPtr(base+idx);
+        size_t delta = idx - HashTable[h];
+        if (delta>MAX_DISTANCE) delta = MAX_DISTANCE;
+        DELTANEXTU16(idx) = (U16)delta;
+        HashTable[h] = idx;
+        idx++;
+    }
+
+    hc4->nextToUpdate = target;
+}
+
+
+FORCE_INLINE int LZ4HC_InsertAndFindBestMatch (LZ4HC_Data_Structure* hc4,   /* Index table will be updated */
+                                               const BYTE* ip, const BYTE* const iLimit,
+                                               const BYTE** matchpos,
+                                               const int maxNbAttempts)
+{
+    U16* const chainTable = hc4->chainTable;
+    U32* const HashTable = hc4->hashTable;
+    const BYTE* const base = hc4->base;
+    const BYTE* const dictBase = hc4->dictBase;
+    const U32 dictLimit = hc4->dictLimit;
+    const U32 lowLimit = (hc4->lowLimit + 64 KB > (U32)(ip-base)) ? hc4->lowLimit : (U32)(ip - base) - (64 KB - 1);
+    U32 matchIndex;
+    const BYTE* match;
+    int nbAttempts=maxNbAttempts;
+    size_t ml=0;
+
+    /* HC4 match finder */
+    LZ4HC_Insert(hc4, ip);
+    matchIndex = HashTable[LZ4HC_hashPtr(ip)];
+
+    while ((matchIndex>=lowLimit) && (nbAttempts))
+    {
+        nbAttempts--;
+        if (matchIndex >= dictLimit)
+        {
+            match = base + matchIndex;
+            if (*(match+ml) == *(ip+ml)
+                && (LZ4_read32(match) == LZ4_read32(ip)))
+            {
+                size_t mlt = LZ4_count(ip+MINMATCH, match+MINMATCH, iLimit) + MINMATCH;
+                if (mlt > ml) { ml = mlt; *matchpos = match; }
+            }
+        }
+        else
+        {
+            match = dictBase + matchIndex;
+            if (LZ4_read32(match) == LZ4_read32(ip))
+            {
+                size_t mlt;
+                const BYTE* vLimit = ip + (dictLimit - matchIndex);
+                if (vLimit > iLimit) vLimit = iLimit;
+                mlt = LZ4_count(ip+MINMATCH, match+MINMATCH, vLimit) + MINMATCH;
+                if ((ip+mlt == vLimit) && (vLimit < iLimit))
+                    mlt += LZ4_count(ip+mlt, base+dictLimit, iLimit);
+                if (mlt > ml) { ml = mlt; *matchpos = base + matchIndex; }   /* virtual matchpos */
+            }
+        }
+        matchIndex -= DELTANEXTU16(matchIndex);
+    }
+
+    return (int)ml;
+}
+
+
+FORCE_INLINE int LZ4HC_InsertAndGetWiderMatch (
+    LZ4HC_Data_Structure* hc4,
+    const BYTE* const ip,
+    const BYTE* const iLowLimit,
+    const BYTE* const iHighLimit,
+    int longest,
+    const BYTE** matchpos,
+    const BYTE** startpos,
+    const int maxNbAttempts)
+{
+    U16* const chainTable = hc4->chainTable;
+    U32* const HashTable = hc4->hashTable;
+    const BYTE* const base = hc4->base;
+    const U32 dictLimit = hc4->dictLimit;
+    const BYTE* const lowPrefixPtr = base + dictLimit;
+    const U32 lowLimit = (hc4->lowLimit + 64 KB > (U32)(ip-base)) ? hc4->lowLimit : (U32)(ip - base) - (64 KB - 1);
+    const BYTE* const dictBase = hc4->dictBase;
+    U32   matchIndex;
+    int nbAttempts = maxNbAttempts;
+    int delta = (int)(ip-iLowLimit);
+
+
+    /* First Match */
+    LZ4HC_Insert(hc4, ip);
+    matchIndex = HashTable[LZ4HC_hashPtr(ip)];
+
+    while ((matchIndex>=lowLimit) && (nbAttempts))
+    {
+        nbAttempts--;
+        if (matchIndex >= dictLimit)
+        {
+            const BYTE* matchPtr = base + matchIndex;
+            if (*(iLowLimit + longest) == *(matchPtr - delta + longest))
+                if (LZ4_read32(matchPtr) == LZ4_read32(ip))
+                {
+                    int mlt = MINMATCH + LZ4_count(ip+MINMATCH, matchPtr+MINMATCH, iHighLimit);
+                    int back = 0;
+
+                    while ((ip+back>iLowLimit)
+                           && (matchPtr+back > lowPrefixPtr)
+                           && (ip[back-1] == matchPtr[back-1]))
+                            back--;
+
+                    mlt -= back;
+
+                    if (mlt > longest)
+                    {
+                        longest = (int)mlt;
+                        *matchpos = matchPtr+back;
+                        *startpos = ip+back;
+                    }
+                }
+        }
+        else
+        {
+            const BYTE* matchPtr = dictBase + matchIndex;
+            if (LZ4_read32(matchPtr) == LZ4_read32(ip))
+            {
+                size_t mlt;
+                int back=0;
+                const BYTE* vLimit = ip + (dictLimit - matchIndex);
+                if (vLimit > iHighLimit) vLimit = iHighLimit;
+                mlt = LZ4_count(ip+MINMATCH, matchPtr+MINMATCH, vLimit) + MINMATCH;
+                if ((ip+mlt == vLimit) && (vLimit < iHighLimit))
+                    mlt += LZ4_count(ip+mlt, base+dictLimit, iHighLimit);
+                while ((ip+back > iLowLimit) && (matchIndex+back > lowLimit) && (ip[back-1] == matchPtr[back-1])) back--;
+                mlt -= back;
+                if ((int)mlt > longest) { longest = (int)mlt; *matchpos = base + matchIndex + back; *startpos = ip+back; }
+            }
+        }
+        matchIndex -= DELTANEXTU16(matchIndex);
+    }
+
+    return longest;
+}
+
+
+typedef enum { noLimit = 0, limitedOutput = 1 } limitedOutput_directive;
+
+#define LZ4HC_DEBUG 0
+#if LZ4HC_DEBUG
+static unsigned debug = 0;
+#endif
+
+FORCE_INLINE int LZ4HC_encodeSequence (
+    const BYTE** ip,
+    BYTE** op,
+    const BYTE** anchor,
+    int matchLength,
+    const BYTE* const match,
+    limitedOutput_directive limitedOutputBuffer,
+    BYTE* oend)
+{
+    int length;
+    BYTE* token;
+
+#if LZ4HC_DEBUG
+    if (debug) printf("literal : %u  --  match : %u  --  offset : %u\n", (U32)(*ip - *anchor), (U32)matchLength, (U32)(*ip-match));
+#endif
+
+    /* Encode Literal length */
+    length = (int)(*ip - *anchor);
+    token = (*op)++;
+    if ((limitedOutputBuffer) && ((*op + (length>>8) + length + (2 + 1 + LASTLITERALS)) > oend)) return 1;   /* Check output limit */
+    if (length>=(int)RUN_MASK) { int len; *token=(RUN_MASK<<ML_BITS); len = length-RUN_MASK; for(; len > 254 ; len-=255) *(*op)++ = 255;  *(*op)++ = (BYTE)len; }
+    else *token = (BYTE)(length<<ML_BITS);
+
+    /* Copy Literals */
+    LZ4_wildCopy(*op, *anchor, (*op) + length);
+    *op += length;
+
+    /* Encode Offset */
+    LZ4_writeLE16(*op, (U16)(*ip-match)); *op += 2;
+
+    /* Encode MatchLength */
+    length = (int)(matchLength-MINMATCH);
+    if ((limitedOutputBuffer) && (*op + (length>>8) + (1 + LASTLITERALS) > oend)) return 1;   /* Check output limit */
+    if (length>=(int)ML_MASK) { *token+=ML_MASK; length-=ML_MASK; for(; length > 509 ; length-=510) { *(*op)++ = 255; *(*op)++ = 255; } if (length > 254) { length-=255; *(*op)++ = 255; } *(*op)++ = (BYTE)length; }
+    else *token += (BYTE)(length);
+
+    /* Prepare next loop */
+    *ip += matchLength;
+    *anchor = *ip;
+
+    return 0;
+}
+
+
+static int LZ4HC_compress_generic (
+    void* ctxvoid,
+    const char* source,
+    char* dest,
+    int inputSize,
+    int maxOutputSize,
+    int compressionLevel,
+    limitedOutput_directive limit
+    )
+{
+    LZ4HC_Data_Structure* ctx = (LZ4HC_Data_Structure*) ctxvoid;
+    const BYTE* ip = (const BYTE*) source;
+    const BYTE* anchor = ip;
+    const BYTE* const iend = ip + inputSize;
+    const BYTE* const mflimit = iend - MFLIMIT;
+    const BYTE* const matchlimit = (iend - LASTLITERALS);
+
+    BYTE* op = (BYTE*) dest;
+    BYTE* const oend = op + maxOutputSize;
+
+    unsigned maxNbAttempts;
+    int   ml, ml2, ml3, ml0;
+    const BYTE* ref=NULL;
+    const BYTE* start2=NULL;
+    const BYTE* ref2=NULL;
+    const BYTE* start3=NULL;
+    const BYTE* ref3=NULL;
+    const BYTE* start0;
+    const BYTE* ref0;
+
+
+    /* init */
+    if (compressionLevel > g_maxCompressionLevel) compressionLevel = g_maxCompressionLevel;
+    if (compressionLevel < 1) compressionLevel = LZ4HC_compressionLevel_default;
+    maxNbAttempts = 1 << (compressionLevel-1);
+    ctx->end += inputSize;
+
+    ip++;
+
+    /* Main Loop */
+    while (ip < mflimit)
+    {
+        ml = LZ4HC_InsertAndFindBestMatch (ctx, ip, matchlimit, (&ref), maxNbAttempts);
+        if (!ml) { ip++; continue; }
+
+        /* saved, in case we would skip too much */
+        start0 = ip;
+        ref0 = ref;
+        ml0 = ml;
+
+_Search2:
+        if (ip+ml < mflimit)
+            ml2 = LZ4HC_InsertAndGetWiderMatch(ctx, ip + ml - 2, ip + 1, matchlimit, ml, &ref2, &start2, maxNbAttempts);
+        else ml2 = ml;
+
+        if (ml2 == ml)  /* No better match */
+        {
+            if (LZ4HC_encodeSequence(&ip, &op, &anchor, ml, ref, limit, oend)) return 0;
+            continue;
+        }
+
+        if (start0 < ip)
+        {
+            if (start2 < ip + ml0)   /* empirical */
+            {
+                ip = start0;
+                ref = ref0;
+                ml = ml0;
+            }
+        }
+
+        /* Here, start0==ip */
+        if ((start2 - ip) < 3)   /* First Match too small : removed */
+        {
+            ml = ml2;
+            ip = start2;
+            ref =ref2;
+            goto _Search2;
+        }
+
+_Search3:
+        /*
+        * Currently we have :
+        * ml2 > ml1, and
+        * ip1+3 <= ip2 (usually < ip1+ml1)
+        */
+        if ((start2 - ip) < OPTIMAL_ML)
+        {
+            int correction;
+            int new_ml = ml;
+            if (new_ml > OPTIMAL_ML) new_ml = OPTIMAL_ML;
+            if (ip+new_ml > start2 + ml2 - MINMATCH) new_ml = (int)(start2 - ip) + ml2 - MINMATCH;
+            correction = new_ml - (int)(start2 - ip);
+            if (correction > 0)
+            {
+                start2 += correction;
+                ref2 += correction;
+                ml2 -= correction;
+            }
+        }
+        /* Now, we have start2 = ip+new_ml, with new_ml = min(ml, OPTIMAL_ML=18) */
+
+        if (start2 + ml2 < mflimit)
+            ml3 = LZ4HC_InsertAndGetWiderMatch(ctx, start2 + ml2 - 3, start2, matchlimit, ml2, &ref3, &start3, maxNbAttempts);
+        else ml3 = ml2;
+
+        if (ml3 == ml2) /* No better match : 2 sequences to encode */
+        {
+            /* ip & ref are known; Now for ml */
+            if (start2 < ip+ml)  ml = (int)(start2 - ip);
+            /* Now, encode 2 sequences */
+            if (LZ4HC_encodeSequence(&ip, &op, &anchor, ml, ref, limit, oend)) return 0;
+            ip = start2;
+            if (LZ4HC_encodeSequence(&ip, &op, &anchor, ml2, ref2, limit, oend)) return 0;
+            continue;
+        }
+
+        if (start3 < ip+ml+3) /* Not enough space for match 2 : remove it */
+        {
+            if (start3 >= (ip+ml)) /* can write Seq1 immediately ==> Seq2 is removed, so Seq3 becomes Seq1 */
+            {
+                if (start2 < ip+ml)
+                {
+                    int correction = (int)(ip+ml - start2);
+                    start2 += correction;
+                    ref2 += correction;
+                    ml2 -= correction;
+                    if (ml2 < MINMATCH)
+                    {
+                        start2 = start3;
+                        ref2 = ref3;
+                        ml2 = ml3;
+                    }
+                }
+
+                if (LZ4HC_encodeSequence(&ip, &op, &anchor, ml, ref, limit, oend)) return 0;
+                ip  = start3;
+                ref = ref3;
+                ml  = ml3;
+
+                start0 = start2;
+                ref0 = ref2;
+                ml0 = ml2;
+                goto _Search2;
+            }
+
+            start2 = start3;
+            ref2 = ref3;
+            ml2 = ml3;
+            goto _Search3;
+        }
+
+        /*
+        * OK, now we have 3 ascending matches; let's write at least the first one
+        * ip & ref are known; Now for ml
+        */
+        if (start2 < ip+ml)
+        {
+            if ((start2 - ip) < (int)ML_MASK)
+            {
+                int correction;
+                if (ml > OPTIMAL_ML) ml = OPTIMAL_ML;
+                if (ip + ml > start2 + ml2 - MINMATCH) ml = (int)(start2 - ip) + ml2 - MINMATCH;
+                correction = ml - (int)(start2 - ip);
+                if (correction > 0)
+                {
+                    start2 += correction;
+                    ref2 += correction;
+                    ml2 -= correction;
+                }
+            }
+            else
+            {
+                ml = (int)(start2 - ip);
+            }
+        }
+        if (LZ4HC_encodeSequence(&ip, &op, &anchor, ml, ref, limit, oend)) return 0;
+
+        ip = start2;
+        ref = ref2;
+        ml = ml2;
+
+        start2 = start3;
+        ref2 = ref3;
+        ml2 = ml3;
+
+        goto _Search3;
+    }
+
+    /* Encode Last Literals */
+    {
+        int lastRun = (int)(iend - anchor);
+        if ((limit) && (((char*)op - dest) + lastRun + 1 + ((lastRun+255-RUN_MASK)/255) > (U32)maxOutputSize)) return 0;  /* Check output limit */
+        if (lastRun>=(int)RUN_MASK) { *op++=(RUN_MASK<<ML_BITS); lastRun-=RUN_MASK; for(; lastRun > 254 ; lastRun-=255) *op++ = 255; *op++ = (BYTE) lastRun; }
+        else *op++ = (BYTE)(lastRun<<ML_BITS);
+        memcpy(op, anchor, iend - anchor);
+        op += iend-anchor;
+    }
+
+    /* End */
+    return (int) (((char*)op)-dest);
+}
+
+
+int LZ4_sizeofStateHC(void) { return sizeof(LZ4HC_Data_Structure); }
+
+int LZ4_compress_HC_extStateHC (void* state, const char* src, char* dst, int srcSize, int maxDstSize, int compressionLevel)
+{
+    if (((size_t)(state)&(sizeof(void*)-1)) != 0) return 0;   /* Error : state is not aligned for pointers (32 or 64 bits) */
+    LZ4HC_init ((LZ4HC_Data_Structure*)state, (const BYTE*)src);
+    if (maxDstSize < LZ4_compressBound(srcSize))
+        return LZ4HC_compress_generic (state, src, dst, srcSize, maxDstSize, compressionLevel, limitedOutput);
+    else
+        return LZ4HC_compress_generic (state, src, dst, srcSize, maxDstSize, compressionLevel, noLimit);
+}
+
+int LZ4_compress_HC(const char* src, char* dst, int srcSize, int maxDstSize, int compressionLevel)
+{
+    LZ4HC_Data_Structure state;
+    return LZ4_compress_HC_extStateHC(&state, src, dst, srcSize, maxDstSize, compressionLevel);
+}
+
+
+
+/**************************************
+*  Streaming Functions
+**************************************/
+/* allocation */
+LZ4_streamHC_t* LZ4_createStreamHC(void) { return (LZ4_streamHC_t*)malloc(sizeof(LZ4_streamHC_t)); }
+int             LZ4_freeStreamHC (LZ4_streamHC_t* LZ4_streamHCPtr) { free(LZ4_streamHCPtr); return 0; }
+
+
+/* initialization */
+void LZ4_resetStreamHC (LZ4_streamHC_t* LZ4_streamHCPtr, int compressionLevel)
+{
+    LZ4_STATIC_ASSERT(sizeof(LZ4HC_Data_Structure) <= sizeof(LZ4_streamHC_t));   /* if compilation fails here, LZ4_STREAMHCSIZE must be increased */
+    ((LZ4HC_Data_Structure*)LZ4_streamHCPtr)->base = NULL;
+    ((LZ4HC_Data_Structure*)LZ4_streamHCPtr)->compressionLevel = (unsigned)compressionLevel;
+}
+
+int LZ4_loadDictHC (LZ4_streamHC_t* LZ4_streamHCPtr, const char* dictionary, int dictSize)
+{
+    LZ4HC_Data_Structure* ctxPtr = (LZ4HC_Data_Structure*) LZ4_streamHCPtr;
+    if (dictSize > 64 KB)
+    {
+        dictionary += dictSize - 64 KB;
+        dictSize = 64 KB;
+    }
+    LZ4HC_init (ctxPtr, (const BYTE*)dictionary);
+    if (dictSize >= 4) LZ4HC_Insert (ctxPtr, (const BYTE*)dictionary +(dictSize-3));
+    ctxPtr->end = (const BYTE*)dictionary + dictSize;
+    return dictSize;
+}
+
+
+/* compression */
+
+static void LZ4HC_setExternalDict(LZ4HC_Data_Structure* ctxPtr, const BYTE* newBlock)
+{
+    if (ctxPtr->end >= ctxPtr->base + 4)
+        LZ4HC_Insert (ctxPtr, ctxPtr->end-3);   /* Referencing remaining dictionary content */
+    /* Only one memory segment for extDict, so any previous extDict is lost at this stage */
+    ctxPtr->lowLimit  = ctxPtr->dictLimit;
+    ctxPtr->dictLimit = (U32)(ctxPtr->end - ctxPtr->base);
+    ctxPtr->dictBase  = ctxPtr->base;
+    ctxPtr->base = newBlock - ctxPtr->dictLimit;
+    ctxPtr->end  = newBlock;
+    ctxPtr->nextToUpdate = ctxPtr->dictLimit;   /* match referencing will resume from there */
+}
+
+static int LZ4_compressHC_continue_generic (LZ4HC_Data_Structure* ctxPtr,
+                                            const char* source, char* dest,
+                                            int inputSize, int maxOutputSize, limitedOutput_directive limit)
+{
+    /* auto-init if forgotten */
+    if (ctxPtr->base == NULL)
+        LZ4HC_init (ctxPtr, (const BYTE*) source);
+
+    /* Check overflow */
+    if ((size_t)(ctxPtr->end - ctxPtr->base) > 2 GB)
+    {
+        size_t dictSize = (size_t)(ctxPtr->end - ctxPtr->base) - ctxPtr->dictLimit;
+        if (dictSize > 64 KB) dictSize = 64 KB;
+
+        LZ4_loadDictHC((LZ4_streamHC_t*)ctxPtr, (const char*)(ctxPtr->end) - dictSize, (int)dictSize);
+    }
+
+    /* Check if blocks follow each other */
+    if ((const BYTE*)source != ctxPtr->end)
+        LZ4HC_setExternalDict(ctxPtr, (const BYTE*)source);
+
+    /* Check overlapping input/dictionary space */
+    {
+        const BYTE* sourceEnd = (const BYTE*) source + inputSize;
+        const BYTE* dictBegin = ctxPtr->dictBase + ctxPtr->lowLimit;
+        const BYTE* dictEnd   = ctxPtr->dictBase + ctxPtr->dictLimit;
+        if ((sourceEnd > dictBegin) && ((const BYTE*)source < dictEnd))
+        {
+            if (sourceEnd > dictEnd) sourceEnd = dictEnd;
+            ctxPtr->lowLimit = (U32)(sourceEnd - ctxPtr->dictBase);
+            if (ctxPtr->dictLimit - ctxPtr->lowLimit < 4) ctxPtr->lowLimit = ctxPtr->dictLimit;
+        }
+    }
+
+    return LZ4HC_compress_generic (ctxPtr, source, dest, inputSize, maxOutputSize, ctxPtr->compressionLevel, limit);
+}
+
+int LZ4_compress_HC_continue (LZ4_streamHC_t* LZ4_streamHCPtr, const char* source, char* dest, int inputSize, int maxOutputSize)
+{
+    if (maxOutputSize < LZ4_compressBound(inputSize))
+        return LZ4_compressHC_continue_generic ((LZ4HC_Data_Structure*)LZ4_streamHCPtr, source, dest, inputSize, maxOutputSize, limitedOutput);
+    else
+        return LZ4_compressHC_continue_generic ((LZ4HC_Data_Structure*)LZ4_streamHCPtr, source, dest, inputSize, maxOutputSize, noLimit);
+}
+
+
+/* dictionary saving */
+
+int LZ4_saveDictHC (LZ4_streamHC_t* LZ4_streamHCPtr, char* safeBuffer, int dictSize)
+{
+    LZ4HC_Data_Structure* streamPtr = (LZ4HC_Data_Structure*)LZ4_streamHCPtr;
+    int prefixSize = (int)(streamPtr->end - (streamPtr->base + streamPtr->dictLimit));
+    if (dictSize > 64 KB) dictSize = 64 KB;
+    if (dictSize < 4) dictSize = 0;
+    if (dictSize > prefixSize) dictSize = prefixSize;
+    memmove(safeBuffer, streamPtr->end - dictSize, dictSize);
+    {
+        U32 endIndex = (U32)(streamPtr->end - streamPtr->base);
+        streamPtr->end = (const BYTE*)safeBuffer + dictSize;
+        streamPtr->base = streamPtr->end - endIndex;
+        streamPtr->dictLimit = endIndex - dictSize;
+        streamPtr->lowLimit = endIndex - dictSize;
+        if (streamPtr->nextToUpdate < streamPtr->dictLimit) streamPtr->nextToUpdate = streamPtr->dictLimit;
+    }
+    return dictSize;
+}
+
+
+/***********************************
+*  Deprecated Functions
+***********************************/
+/* Deprecated compression functions */
+/* These functions are planned to start generate warnings by r131 approximately */
+int LZ4_compressHC(const char* src, char* dst, int srcSize) { return LZ4_compress_HC (src, dst, srcSize, LZ4_compressBound(srcSize), 0); }
+int LZ4_compressHC_limitedOutput(const char* src, char* dst, int srcSize, int maxDstSize) { return LZ4_compress_HC(src, dst, srcSize, maxDstSize, 0); }
+int LZ4_compressHC2(const char* src, char* dst, int srcSize, int cLevel) { return LZ4_compress_HC (src, dst, srcSize, LZ4_compressBound(srcSize), cLevel); }
+int LZ4_compressHC2_limitedOutput(const char* src, char* dst, int srcSize, int maxDstSize, int cLevel) { return LZ4_compress_HC(src, dst, srcSize, maxDstSize, cLevel); }
+int LZ4_compressHC_withStateHC (void* state, const char* src, char* dst, int srcSize) { return LZ4_compress_HC_extStateHC (state, src, dst, srcSize, LZ4_compressBound(srcSize), 0); }
+int LZ4_compressHC_limitedOutput_withStateHC (void* state, const char* src, char* dst, int srcSize, int maxDstSize) { return LZ4_compress_HC_extStateHC (state, src, dst, srcSize, maxDstSize, 0); }
+int LZ4_compressHC2_withStateHC (void* state, const char* src, char* dst, int srcSize, int cLevel) { return LZ4_compress_HC_extStateHC(state, src, dst, srcSize, LZ4_compressBound(srcSize), cLevel); }
+int LZ4_compressHC2_limitedOutput_withStateHC (void* state, const char* src, char* dst, int srcSize, int maxDstSize, int cLevel) { return LZ4_compress_HC_extStateHC(state, src, dst, srcSize, maxDstSize, cLevel); }
+int LZ4_compressHC_continue (LZ4_streamHC_t* ctx, const char* src, char* dst, int srcSize) { return LZ4_compress_HC_continue (ctx, src, dst, srcSize, LZ4_compressBound(srcSize)); }
+int LZ4_compressHC_limitedOutput_continue (LZ4_streamHC_t* ctx, const char* src, char* dst, int srcSize, int maxDstSize) { return LZ4_compress_HC_continue (ctx, src, dst, srcSize, maxDstSize); }
+
+
+/* Deprecated streaming functions */
+/* These functions currently generate deprecation warnings */
+int LZ4_sizeofStreamStateHC(void) { return LZ4_STREAMHCSIZE; }
+
+int LZ4_resetStreamStateHC(void* state, char* inputBuffer)
+{
+    if ((((size_t)state) & (sizeof(void*)-1)) != 0) return 1;   /* Error : pointer is not aligned for pointer (32 or 64 bits) */
+    LZ4HC_init((LZ4HC_Data_Structure*)state, (const BYTE*)inputBuffer);
+    ((LZ4HC_Data_Structure*)state)->inputBuffer = (BYTE*)inputBuffer;
+    return 0;
+}
+
+void* LZ4_createHC (char* inputBuffer)
+{
+    void* hc4 = ALLOCATOR(1, sizeof(LZ4HC_Data_Structure));
+    if (hc4 == NULL) return NULL;   /* not enough memory */
+    LZ4HC_init ((LZ4HC_Data_Structure*)hc4, (const BYTE*)inputBuffer);
+    ((LZ4HC_Data_Structure*)hc4)->inputBuffer = (BYTE*)inputBuffer;
+    return hc4;
+}
+
+int LZ4_freeHC (void* LZ4HC_Data)
+{
+    FREEMEM(LZ4HC_Data);
+    return (0);
+}
+
+int LZ4_compressHC2_continue (void* LZ4HC_Data, const char* source, char* dest, int inputSize, int compressionLevel)
+{
+    return LZ4HC_compress_generic (LZ4HC_Data, source, dest, inputSize, 0, compressionLevel, noLimit);
+}
+
+int LZ4_compressHC2_limitedOutput_continue (void* LZ4HC_Data, const char* source, char* dest, int inputSize, int maxOutputSize, int compressionLevel)
+{
+    return LZ4HC_compress_generic (LZ4HC_Data, source, dest, inputSize, maxOutputSize, compressionLevel, limitedOutput);
+}
+
+char* LZ4_slideInputBufferHC(void* LZ4HC_Data)
+{
+    LZ4HC_Data_Structure* hc4 = (LZ4HC_Data_Structure*)LZ4HC_Data;
+    int dictSize = LZ4_saveDictHC((LZ4_streamHC_t*)LZ4HC_Data, (char*)(hc4->inputBuffer), 64 KB);
+    return (char*)(hc4->inputBuffer + dictSize);
+}

corelib/src/rtflann/ext/lz4hc.h

@@ -0,0 +1,189 @@
+/*
+   LZ4 HC - High Compression Mode of LZ4
+   Header File
+   Copyright (C) 2011-2015, Yann Collet.
+   BSD 2-Clause License (http://www.opensource.org/licenses/bsd-license.php)
+
+   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.
+
+   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
+   OWNER 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.
+
+   You can contact the author at :
+   - LZ4 source repository : https://github.com/Cyan4973/lz4
+   - LZ4 public forum : https://groups.google.com/forum/#!forum/lz4c
+*/
+#pragma once
+
+
+#if defined (__cplusplus)
+extern "C" {
+#endif
+
+/*****************************
+*  Includes
+*****************************/
+#include <stddef.h>   /* size_t */
+
+
+/**************************************
+*  Block Compression
+**************************************/
+int LZ4_compress_HC (const char* src, char* dst, int srcSize, int maxDstSize, int compressionLevel);
+/*
+LZ4_compress_HC :
+    Destination buffer 'dst' must be already allocated.
+    Compression completion is guaranteed if 'dst' buffer is sized to handle worst circumstances (data not compressible)
+    Worst size evaluation is provided by function LZ4_compressBound() (see "lz4.h")
+      srcSize  : Max supported value is LZ4_MAX_INPUT_SIZE (see "lz4.h")
+      compressionLevel : Recommended values are between 4 and 9, although any value between 0 and 16 will work.
+                         0 means "use default value" (see lz4hc.c).
+                         Values >16 behave the same as 16.
+      return : the number of bytes written into buffer 'dst'
+            or 0 if compression fails.
+*/
+
+
+/* Note :
+   Decompression functions are provided within LZ4 source code (see "lz4.h") (BSD license)
+*/
+
+
+int LZ4_sizeofStateHC(void);
+int LZ4_compress_HC_extStateHC(void* state, const char* src, char* dst, int srcSize, int maxDstSize, int compressionLevel);
+/*
+LZ4_compress_HC_extStateHC() :
+   Use this function if you prefer to manually allocate memory for compression tables.
+   To know how much memory must be allocated for the compression tables, use :
+      int LZ4_sizeofStateHC();
+
+   Allocated memory must be aligned on 8-bytes boundaries (which a normal malloc() will do properly).
+
+   The allocated memory can then be provided to the compression functions using 'void* state' parameter.
+   LZ4_compress_HC_extStateHC() is equivalent to previously described function.
+   It just uses externally allocated memory for stateHC.
+*/
+
+
+/**************************************
+*  Streaming Compression
+**************************************/
+#define LZ4_STREAMHCSIZE        262192
+#define LZ4_STREAMHCSIZE_SIZET (LZ4_STREAMHCSIZE / sizeof(size_t))
+typedef struct { size_t table[LZ4_STREAMHCSIZE_SIZET]; } LZ4_streamHC_t;
+/*
+  LZ4_streamHC_t
+  This structure allows static allocation of LZ4 HC streaming state.
+  State must then be initialized using LZ4_resetStreamHC() before first use.
+
+  Static allocation should only be used in combination with static linking.
+  If you want to use LZ4 as a DLL, please use construction functions below, which are future-proof.
+*/
+
+
+LZ4_streamHC_t* LZ4_createStreamHC(void);
+int             LZ4_freeStreamHC (LZ4_streamHC_t* streamHCPtr);
+/*
+  These functions create and release memory for LZ4 HC streaming state.
+  Newly created states are already initialized.
+  Existing state space can be re-used anytime using LZ4_resetStreamHC().
+  If you use LZ4 as a DLL, use these functions instead of static structure allocation,
+  to avoid size mismatch between different versions.
+*/
+
+void LZ4_resetStreamHC (LZ4_streamHC_t* streamHCPtr, int compressionLevel);
+int  LZ4_loadDictHC (LZ4_streamHC_t* streamHCPtr, const char* dictionary, int dictSize);
+
+int LZ4_compress_HC_continue (LZ4_streamHC_t* streamHCPtr, const char* src, char* dst, int srcSize, int maxDstSize);
+
+int LZ4_saveDictHC (LZ4_streamHC_t* streamHCPtr, char* safeBuffer, int maxDictSize);
+
+/*
+  These functions compress data in successive blocks of any size, using previous blocks as dictionary.
+  One key assumption is that previous blocks (up to 64 KB) remain read-accessible while compressing next blocks.
+  There is an exception for ring buffers, which can be smaller 64 KB.
+  Such case is automatically detected and correctly handled by LZ4_compress_HC_continue().
+
+  Before starting compression, state must be properly initialized, using LZ4_resetStreamHC().
+  A first "fictional block" can then be designated as initial dictionary, using LZ4_loadDictHC() (Optional).
+
+  Then, use LZ4_compress_HC_continue() to compress each successive block.
+  It works like LZ4_compress_HC(), but use previous memory blocks as dictionary to improve compression.
+  Previous memory blocks (including initial dictionary when present) must remain accessible and unmodified during compression.
+  As a reminder, size 'dst' buffer to handle worst cases, using LZ4_compressBound(), to ensure success of compression operation.
+
+  If, for any reason, previous data blocks can't be preserved unmodified in memory during next compression block,
+  you must save it to a safer memory space, using LZ4_saveDictHC().
+  Return value of LZ4_saveDictHC() is the size of dictionary effectively saved into 'safeBuffer'.
+*/
+
+
+
+/**************************************
+*  Deprecated Functions
+**************************************/
+/* Deprecate Warnings */
+/* Should these warnings messages be a problem,
+   it is generally possible to disable them,
+   with -Wno-deprecated-declarations for gcc
+   or _CRT_SECURE_NO_WARNINGS in Visual for example.
+   You can also define LZ4_DEPRECATE_WARNING_DEFBLOCK. */
+#ifndef LZ4_DEPRECATE_WARNING_DEFBLOCK
+#  define LZ4_DEPRECATE_WARNING_DEFBLOCK
+#  define LZ4_GCC_VERSION (__GNUC__ * 100 + __GNUC_MINOR__)
+#  if (LZ4_GCC_VERSION >= 405) || defined(__clang__)
+#    define LZ4_DEPRECATED(message) __attribute__((deprecated(message)))
+#  elif (LZ4_GCC_VERSION >= 301)
+#    define LZ4_DEPRECATED(message) __attribute__((deprecated))
+#  elif defined(_MSC_VER)
+#    define LZ4_DEPRECATED(message) __declspec(deprecated(message))
+#  else
+#    pragma message("WARNING: You need to implement LZ4_DEPRECATED for this compiler")
+#    define LZ4_DEPRECATED(message)
+#  endif
+#endif // LZ4_DEPRECATE_WARNING_DEFBLOCK
+
+/* compression functions */
+/* these functions are planned to trigger warning messages by r131 approximately */
+int LZ4_compressHC                (const char* source, char* dest, int inputSize);
+int LZ4_compressHC_limitedOutput  (const char* source, char* dest, int inputSize, int maxOutputSize);
+int LZ4_compressHC2               (const char* source, char* dest, int inputSize, int compressionLevel);
+int LZ4_compressHC2_limitedOutput (const char* source, char* dest, int inputSize, int maxOutputSize, int compressionLevel);
+int LZ4_compressHC_withStateHC               (void* state, const char* source, char* dest, int inputSize);
+int LZ4_compressHC_limitedOutput_withStateHC (void* state, const char* source, char* dest, int inputSize, int maxOutputSize);
+int LZ4_compressHC2_withStateHC              (void* state, const char* source, char* dest, int inputSize, int compressionLevel);
+int LZ4_compressHC2_limitedOutput_withStateHC(void* state, const char* source, char* dest, int inputSize, int maxOutputSize, int compressionLevel);
+int LZ4_compressHC_continue               (LZ4_streamHC_t* LZ4_streamHCPtr, const char* source, char* dest, int inputSize);
+int LZ4_compressHC_limitedOutput_continue (LZ4_streamHC_t* LZ4_streamHCPtr, const char* source, char* dest, int inputSize, int maxOutputSize);
+
+/* Streaming functions following the older model; should no longer be used */
+LZ4_DEPRECATED("use LZ4_createStreamHC() instead") void* LZ4_createHC (char* inputBuffer);
+LZ4_DEPRECATED("use LZ4_saveDictHC() instead")     char* LZ4_slideInputBufferHC (void* LZ4HC_Data);
+LZ4_DEPRECATED("use LZ4_freeStreamHC() instead")   int   LZ4_freeHC (void* LZ4HC_Data);
+LZ4_DEPRECATED("use LZ4_compress_HC_continue() instead") int   LZ4_compressHC2_continue (void* LZ4HC_Data, const char* source, char* dest, int inputSize, int compressionLevel);
+LZ4_DEPRECATED("use LZ4_compress_HC_continue() instead") int   LZ4_compressHC2_limitedOutput_continue (void* LZ4HC_Data, const char* source, char* dest, int inputSize, int maxOutputSize, int compressionLevel);
+LZ4_DEPRECATED("use LZ4_createStreamHC() instead") int   LZ4_sizeofStreamStateHC(void);
+LZ4_DEPRECATED("use LZ4_resetStreamHC() instead")  int   LZ4_resetStreamStateHC(void* state, char* inputBuffer);
+
+
+#if defined (__cplusplus)
+}
+#endif

corelib/src/rtflann/flann.hpp

@@ -0,0 +1,445 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_HPP_
+#define RTABMAP_FLANN_HPP_
+
+
+#include <vector>
+#include <string>
+#include <cassert>
+#include <cstdio>
+
+#include "general.h"
+#include "util/matrix.h"
+#include "util/params.h"
+#include "util/saving.h"
+
+#include "algorithms/all_indices.h"
+
+namespace rtflann
+{
+
+/**
+ * Sets the log level used for all flann functions
+ * @param level Verbosity level
+ */
+inline void log_verbosity(int level)
+{
+    if (level >= 0) {
+        Logger::setLevel(level);
+    }
+}
+
+/**
+ * (Deprecated) Index parameters for creating a saved index.
+ */
+struct SavedIndexParams : public IndexParams
+{
+    SavedIndexParams(std::string filename)
+    {
+        (*this)["algorithm"] = FLANN_INDEX_SAVED;
+        (*this)["filename"] = filename;
+    }
+};
+
+
+
+template<typename Distance>
+class Index
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+    typedef NNIndex<Distance> IndexType;
+
+    Index(const IndexParams& params, Distance distance = Distance() )
+        : index_params_(params)
+    {
+        flann_algorithm_t index_type = get_param<flann_algorithm_t>(params,"algorithm");
+        loaded_ = false;
+
+        Matrix<ElementType> features;
+        if (index_type == FLANN_INDEX_SAVED) {
+            nnIndex_ = load_saved_index(features, get_param<std::string>(params,"filename"), distance);
+            loaded_ = true;
+        }
+        else {
+        	flann_algorithm_t index_type = get_param<flann_algorithm_t>(params, "algorithm");
+            nnIndex_ = create_index_by_type<Distance>(index_type, features, params, distance);
+        }
+    }
+
+
+    Index(const Matrix<ElementType>& features, const IndexParams& params, Distance distance = Distance() )
+        : index_params_(params)
+    {
+        flann_algorithm_t index_type = get_param<flann_algorithm_t>(params,"algorithm");
+        loaded_ = false;
+
+        if (index_type == FLANN_INDEX_SAVED) {
+            nnIndex_ = load_saved_index(features, get_param<std::string>(params,"filename"), distance);
+            loaded_ = true;
+        }
+        else {
+        	flann_algorithm_t index_type = get_param<flann_algorithm_t>(params, "algorithm");
+            nnIndex_ = create_index_by_type<Distance>(index_type, features, params, distance);
+        }
+    }
+
+
+    Index(const Index& other) : loaded_(other.loaded_), index_params_(other.index_params_)
+    {
+    	nnIndex_ = other.nnIndex_->clone();
+    }
+
+    Index& operator=(Index other)
+    {
+    	this->swap(other);
+    	return *this;
+    }
+
+    virtual ~Index()
+    {
+        delete nnIndex_;
+    }
+
+    /**
+     * Builds the index.
+     */
+    void buildIndex()
+    {
+        if (!loaded_) {
+            nnIndex_->buildIndex();
+        }
+    }
+
+    void buildIndex(const Matrix<ElementType>& points)
+    {
+    	nnIndex_->buildIndex(points);
+    }
+
+    void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        nnIndex_->addPoints(points, rebuild_threshold);
+    }
+
+    /**
+     * Remove point from the index
+     * @param index Index of point to be removed
+     */
+    void removePoint(size_t point_id)
+    {
+    	nnIndex_->removePoint(point_id);
+    }
+
+    /**
+     * Returns pointer to a data point with the specified id.
+     * @param point_id the id of point to retrieve
+     * @return
+     */
+    ElementType* getPoint(size_t point_id)
+    {
+    	return nnIndex_->getPoint(point_id);
+    }
+
+    /**
+     * Save index to file
+     * @param filename
+     */
+    void save(std::string filename)
+    {
+        FILE* fout = fopen(filename.c_str(), "wb");
+        if (fout == NULL) {
+            throw FLANNException("Cannot open file");
+        }
+        nnIndex_->saveIndex(fout);
+        fclose(fout);
+    }
+
+    /**
+     * \returns number of features in this index.
+     */
+    size_t veclen() const
+    {
+        return nnIndex_->veclen();
+    }
+
+    /**
+     * \returns The dimensionality of the features in this index.
+     */
+    size_t size() const
+    {
+        return nnIndex_->size();
+    }
+
+    size_t removedCount() const
+	{
+		return nnIndex_->removedCount();
+	}
+
+    size_t sizeAtBuild() const
+	{
+		return nnIndex_->sizeAtBuild();
+	}
+
+    /**
+     * \returns The index type (kdtree, kmeans,...)
+     */
+    flann_algorithm_t getType() const
+    {
+        return nnIndex_->getType();
+    }
+
+    /**
+     * \returns The amount of memory (in bytes) used by the index.
+     */
+    int usedMemory() const
+    {
+        return nnIndex_->usedMemory();
+    }
+
+
+    /**
+     * \returns The index parameters
+     */
+    IndexParams getParameters() const
+    {
+        return nnIndex_->getParameters();
+    }
+
+    /**
+     * \brief Perform k-nearest neighbor search
+     * \param[in] queries The query points for which to find the nearest neighbors
+     * \param[out] indices The indices of the nearest neighbors found
+     * \param[out] dists Distances to the nearest neighbors found
+     * \param[in] knn Number of nearest neighbors to return
+     * \param[in] params Search parameters
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+                                 Matrix<size_t>& indices,
+                                 Matrix<DistanceType>& dists,
+                                 size_t knn,
+                           const SearchParams& params) const
+    {
+    	return nnIndex_->knnSearch(queries, indices, dists, knn, params);
+    }
+
+    /**
+     *
+     * @param queries
+     * @param indices
+     * @param dists
+     * @param knn
+     * @param params
+     * @return
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+                                 Matrix<int>& indices,
+                                 Matrix<DistanceType>& dists,
+                                 size_t knn,
+                           const SearchParams& params) const
+    {
+    	return nnIndex_->knnSearch(queries, indices, dists, knn, params);
+    }
+
+    /**
+     * \brief Perform k-nearest neighbor search
+     * \param[in] queries The query points for which to find the nearest neighbors
+     * \param[out] indices The indices of the nearest neighbors found
+     * \param[out] dists Distances to the nearest neighbors found
+     * \param[in] knn Number of nearest neighbors to return
+     * \param[in] params Search parameters
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+                                 std::vector< std::vector<size_t> >& indices,
+                                 std::vector<std::vector<DistanceType> >& dists,
+                                 size_t knn,
+                           const SearchParams& params) const
+    {
+    	return nnIndex_->knnSearch(queries, indices, dists, knn, params);
+    }
+
+    /**
+     *
+     * @param queries
+     * @param indices
+     * @param dists
+     * @param knn
+     * @param params
+     * @return
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+                                 std::vector< std::vector<int> >& indices,
+                                 std::vector<std::vector<DistanceType> >& dists,
+                                 size_t knn,
+                           const SearchParams& params) const
+    {
+    	return nnIndex_->knnSearch(queries, indices, dists, knn, params);
+    }
+
+    /**
+     * \brief Perform radius search
+     * \param[in] queries The query points
+     * \param[out] indices The indices of the neighbors found within the given radius
+     * \param[out] dists The distances to the nearest neighbors found
+     * \param[in] radius The radius used for search
+     * \param[in] params Search parameters
+     * \returns Number of neighbors found
+     */
+    int radiusSearch(const Matrix<ElementType>& queries,
+                                    Matrix<size_t>& indices,
+                                    Matrix<DistanceType>& dists,
+                                    float radius,
+                              const SearchParams& params) const
+    {
+    	return nnIndex_->radiusSearch(queries, indices, dists, radius, params);
+    }
+
+    /**
+     *
+     * @param queries
+     * @param indices
+     * @param dists
+     * @param radius
+     * @param params
+     * @return
+     */
+    int radiusSearch(const Matrix<ElementType>& queries,
+                                    Matrix<int>& indices,
+                                    Matrix<DistanceType>& dists,
+                                    float radius,
+                              const SearchParams& params) const
+    {
+    	return nnIndex_->radiusSearch(queries, indices, dists, radius, params);
+    }
+
+    /**
+     * \brief Perform radius search
+     * \param[in] queries The query points
+     * \param[out] indices The indices of the neighbors found within the given radius
+     * \param[out] dists The distances to the nearest neighbors found
+     * \param[in] radius The radius used for search
+     * \param[in] params Search parameters
+     * \returns Number of neighbors found
+     */
+    int radiusSearch(const Matrix<ElementType>& queries,
+                                    std::vector< std::vector<size_t> >& indices,
+                                    std::vector<std::vector<DistanceType> >& dists,
+                                    float radius,
+                              const SearchParams& params) const
+    {
+    	return nnIndex_->radiusSearch(queries, indices, dists, radius, params);
+    }
+
+    /**
+     *
+     * @param queries
+     * @param indices
+     * @param dists
+     * @param radius
+     * @param params
+     * @return
+     */
+    int radiusSearch(const Matrix<ElementType>& queries,
+                                    std::vector< std::vector<int> >& indices,
+                                    std::vector<std::vector<DistanceType> >& dists,
+                                    float radius,
+                              const SearchParams& params) const
+    {
+    	return nnIndex_->radiusSearch(queries, indices, dists, radius, params);
+    }
+
+private:
+    IndexType* load_saved_index(const Matrix<ElementType>& dataset, const std::string& filename, Distance distance)
+    {
+        FILE* fin = fopen(filename.c_str(), "rb");
+        if (fin == NULL) {
+            return NULL;
+        }
+        IndexHeader header = load_header(fin);
+        if (header.h.data_type != flann_datatype_value<ElementType>::value) {
+            throw FLANNException("Datatype of saved index is different than of the one to be loaded.");
+        }
+
+        IndexParams params;
+        params["algorithm"] = header.h.index_type;
+        IndexType* nnIndex = create_index_by_type<Distance>(header.h.index_type, dataset, params, distance);
+        rewind(fin);
+        nnIndex->loadIndex(fin);
+        fclose(fin);
+
+        return nnIndex;
+    }
+
+    void swap( Index& other)
+    {
+    	std::swap(nnIndex_, other.nnIndex_);
+    	std::swap(loaded_, other.loaded_);
+    	std::swap(index_params_, other.index_params_);
+    }
+
+private:
+    /** Pointer to actual index class */
+    IndexType* nnIndex_;
+    /** Indices if the index was loaded from a file */
+    bool loaded_;
+    /** Parameters passed to the index */
+    IndexParams index_params_;
+};
+
+
+
+
+
+/**
+ * Performs a hierarchical clustering of the points passed as argument and then takes a cut in the
+ * the clustering tree to return a flat clustering.
+ * @param[in] points Points to be clustered
+ * @param centers The computed cluster centres. Matrix should be preallocated and centers.rows is the
+ *  number of clusters requested.
+ * @param params Clustering parameters (The same as for flann::KMeansIndex)
+ * @param d Distance to be used for clustering (eg: flann::L2)
+ * @return number of clusters computed (can be different than clusters.rows and is the highest number
+ * of the form (branching-1)*K+1 smaller than clusters.rows).
+ */
+template <typename Distance>
+int hierarchicalClustering(const Matrix<typename Distance::ElementType>& points, Matrix<typename Distance::ResultType>& centers,
+                           const KMeansIndexParams& params, Distance d = Distance())
+{
+    KMeansIndex<Distance> kmeans(points, params, d);
+    kmeans.buildIndex();
+
+    int clusterNum = kmeans.getClusterCenters(centers);
+    return clusterNum;
+}
+
+}
+#endif /* RTABMAP_FLANN_HPP_ */

corelib/src/rtflann/general.h

@@ -0,0 +1,227 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_GENERAL_H_
+#define RTABMAP_FLANN_GENERAL_H_
+
+#include "defines.h"
+#include <stdexcept>
+#include <cassert>
+#include <limits.h>
+
+namespace rtflann
+{
+
+class FLANNException : public std::runtime_error
+{
+public:
+    FLANNException(const char* message) : std::runtime_error(message) { }
+
+    FLANNException(const std::string& message) : std::runtime_error(message) { }
+};
+
+
+template <typename T>
+struct flann_datatype_value
+{
+	static const flann_datatype_t value = FLANN_NONE;
+};
+
+template<>
+struct flann_datatype_value<char>
+{
+	static const flann_datatype_t value = FLANN_INT8;
+};
+
+template<>
+struct flann_datatype_value<short>
+{
+	static const flann_datatype_t value = FLANN_INT16;
+};
+
+template<>
+struct flann_datatype_value<int>
+{
+	static const flann_datatype_t value = FLANN_INT32;
+};
+
+#ifdef LLONG_MAX
+template<>
+struct flann_datatype_value<long long>
+{
+	static const flann_datatype_t value = FLANN_INT64;
+};
+#endif
+
+template<>
+struct flann_datatype_value<unsigned char>
+{
+	static const flann_datatype_t value = FLANN_UINT8;
+};
+
+template<>
+struct flann_datatype_value<unsigned short>
+{
+	static const flann_datatype_t value = FLANN_UINT16;
+};
+
+template<>
+struct flann_datatype_value<unsigned int>
+{
+	static const flann_datatype_t value = FLANN_UINT32;
+};
+
+#ifdef ULLONG_MAX
+template<>
+struct flann_datatype_value<unsigned long long>
+{
+	static const flann_datatype_t value = FLANN_UINT64;
+};
+#endif
+
+
+template<>
+struct flann_datatype_value<float>
+{
+	static const flann_datatype_t value = FLANN_FLOAT32;
+};
+
+template<>
+struct flann_datatype_value<double>
+{
+	static const flann_datatype_t value = FLANN_FLOAT64;
+};
+
+
+
+template <flann_datatype_t datatype>
+struct flann_datatype_type
+{
+	typedef void type;
+};
+
+template<>
+struct flann_datatype_type<FLANN_INT8>
+{
+	typedef char type;
+};
+
+template<>
+struct flann_datatype_type<FLANN_INT16>
+{
+	typedef short type;
+};
+
+template<>
+struct flann_datatype_type<FLANN_INT32>
+{
+	typedef int type;
+};
+
+#ifdef LLONG_MAX
+template<>
+struct flann_datatype_type<FLANN_INT64>
+{
+	typedef long long type;
+};
+#endif
+
+template<>
+struct flann_datatype_type<FLANN_UINT8>
+{
+	typedef unsigned char type;
+};
+
+
+template<>
+struct flann_datatype_type<FLANN_UINT16>
+{
+	typedef unsigned short type;
+};
+
+template<>
+struct flann_datatype_type<FLANN_UINT32>
+{
+	typedef unsigned int type;
+};
+
+#ifdef ULLONG_MAX
+template<>
+struct flann_datatype_type<FLANN_UINT64>
+{
+	typedef unsigned long long type;
+};
+#endif
+
+template<>
+struct flann_datatype_type<FLANN_FLOAT32>
+{
+	typedef float type;
+};
+
+template<>
+struct flann_datatype_type<FLANN_FLOAT64>
+{
+	typedef double type;
+};
+
+
+inline size_t flann_datatype_size(flann_datatype_t type)
+{
+	switch (type) {
+	case FLANN_INT8:
+		return sizeof(flann_datatype_type<FLANN_INT8>::type);
+	case FLANN_INT16:
+		return sizeof(flann_datatype_type<FLANN_INT16>::type);
+	case FLANN_INT32:
+		return sizeof(flann_datatype_type<FLANN_INT32>::type);
+	case FLANN_INT64:
+		return sizeof(flann_datatype_type<FLANN_INT64>::type);
+	case FLANN_UINT8:
+		return sizeof(flann_datatype_type<FLANN_UINT8>::type);
+	case FLANN_UINT16:
+		return sizeof(flann_datatype_type<FLANN_UINT16>::type);
+	case FLANN_UINT32:
+		return sizeof(flann_datatype_type<FLANN_UINT32>::type);
+	case FLANN_UINT64:
+		return sizeof(flann_datatype_type<FLANN_UINT64>::type);
+	case FLANN_FLOAT32:
+		return sizeof(flann_datatype_type<FLANN_FLOAT32>::type);
+	case FLANN_FLOAT64:
+		return sizeof(flann_datatype_type<FLANN_FLOAT64>::type);
+	default:
+		return 0;
+	}
+}
+
+}
+
+
+#endif  /* RTABMAP_FLANN_GENERAL_H_ */

corelib/src/rtflann/nn/ground_truth.h

@@ -0,0 +1,98 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_GROUND_TRUTH_H_
+#define RTABMAP_FLANN_GROUND_TRUTH_H_
+
+#include "rtflann/algorithms/dist.h"
+#include "rtflann/util/matrix.h"
+
+
+namespace rtflann
+{
+
+template <typename Distance>
+void find_nearest(const Matrix<typename Distance::ElementType>& dataset, typename Distance::ElementType* query, size_t* matches, size_t nn,
+                  size_t skip = 0, Distance distance = Distance())
+{
+    //typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+    int n = nn + skip;
+
+    int* match = new int[n];
+    DistanceType* dists = new DistanceType[n];
+
+    dists[0] = distance(dataset[0], query, dataset.cols);
+    match[0] = 0;
+    int dcnt = 1;
+
+    for (size_t i=1; i<dataset.rows; ++i) {
+        DistanceType tmp = distance(dataset[i], query, dataset.cols);
+
+        if (dcnt<n) {
+            match[dcnt] = i;
+            dists[dcnt++] = tmp;
+        }
+        else if (tmp < dists[dcnt-1]) {
+            dists[dcnt-1] = tmp;
+            match[dcnt-1] = i;
+        }
+
+        int j = dcnt-1;
+        // bubble up
+        while (j>=1 && dists[j]<dists[j-1]) {
+            std::swap(dists[j],dists[j-1]);
+            std::swap(match[j],match[j-1]);
+            j--;
+        }
+    }
+
+    for (size_t i=0; i<nn; ++i) {
+        matches[i] = match[i+skip];
+    }
+
+    delete[] match;
+    delete[] dists;
+}
+
+
+template <typename Distance>
+void compute_ground_truth(const Matrix<typename Distance::ElementType>& dataset, const Matrix<typename Distance::ElementType>& testset, Matrix<size_t>& matches,
+                          int skip=0, Distance d = Distance())
+{
+    for (size_t i=0; i<testset.rows; ++i) {
+        find_nearest<Distance>(dataset, testset[i], matches[i], matches.cols, skip, d);
+    }
+}
+
+
+}
+
+#endif //FLANN_GROUND_TRUTH_H_

corelib/src/rtflann/nn/index_testing.h

@@ -0,0 +1,323 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_INDEX_TESTING_H_
+#define RTABMAP_FLANN_INDEX_TESTING_H_
+
+#include <cstring>
+#include <cassert>
+#include <cmath>
+
+#include "rtflann/util/matrix.h"
+#include "rtflann/algorithms/nn_index.h"
+#include "rtflann/util/result_set.h"
+#include "rtflann/util/logger.h"
+#include "rtflann/util/timer.h"
+
+
+namespace rtflann
+{
+
+inline int countCorrectMatches(size_t* neighbors, size_t* groundTruth, int n)
+{
+    int count = 0;
+    for (int i=0; i<n; ++i) {
+        for (int k=0; k<n; ++k) {
+            if (neighbors[i]==groundTruth[k]) {
+                count++;
+                break;
+            }
+        }
+    }
+    return count;
+}
+
+
+template <typename Distance>
+typename Distance::ResultType computeDistanceRaport(const Matrix<typename Distance::ElementType>& inputData, typename Distance::ElementType* target,
+		size_t* neighbors, size_t* groundTruth, int veclen, int n, const Distance& distance)
+{
+    typedef typename Distance::ResultType DistanceType;
+
+    DistanceType ret = 0;
+    for (int i=0; i<n; ++i) {
+        DistanceType den = distance(inputData[groundTruth[i]], target, veclen);
+        DistanceType num = distance(inputData[neighbors[i]], target, veclen);
+
+        if ((den==0)&&(num==0)) {
+            ret += 1;
+        }
+        else {
+            ret += num/den;
+        }
+    }
+
+    return ret;
+}
+
+template <typename Index, typename Distance>
+float search_with_ground_truth(Index& index, const Matrix<typename Distance::ElementType>& inputData,
+                               const Matrix<typename Distance::ElementType>& testData, const Matrix<size_t>& matches, int nn, int checks,
+                               float& time, typename Distance::ResultType& dist, const Distance& distance, int skipMatches)
+{
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    if (matches.cols<size_t(nn)) {
+        Logger::info("matches.cols=%d, nn=%d\n",matches.cols,nn);
+        throw FLANNException("Ground truth is not computed for as many neighbors as requested");
+    }
+
+    SearchParams searchParams(checks);
+
+    size_t* indices = new size_t[nn+skipMatches];
+    DistanceType* dists = new DistanceType[nn+skipMatches];
+    
+    Matrix<size_t> indices_mat(indices, 1, nn+skipMatches);
+    Matrix<DistanceType> dists_mat(dists, 1, nn+skipMatches);
+        
+    size_t* neighbors = indices + skipMatches;
+
+    int correct = 0;
+    DistanceType distR = 0;
+    StartStopTimer t;
+    int repeats = 0;
+    while (t.value<0.2) {
+        repeats++;
+        t.start();
+        correct = 0;
+        distR = 0;        
+        for (size_t i = 0; i < testData.rows; i++) {
+            index.knnSearch(Matrix<ElementType>(testData[i], 1, testData.cols), indices_mat, dists_mat, nn+skipMatches, searchParams);
+
+            correct += countCorrectMatches(neighbors,matches[i], nn);
+            distR += computeDistanceRaport<Distance>(inputData, testData[i], neighbors, matches[i], testData.cols, nn, distance);
+        }
+        t.stop();
+    }
+    time = float(t.value/repeats);
+
+    delete[] indices;
+    delete[] dists;
+
+    float precicion = (float)correct/(nn*testData.rows);
+
+    dist = distR/(testData.rows*nn);
+
+    Logger::info("%8d %10.4g %10.5g %10.5g %10.5g\n",
+                 checks, precicion, time, 1000.0 * time / testData.rows, dist);
+
+    return precicion;
+}
+
+
+template <typename Index, typename Distance>
+float test_index_checks(Index& index, const Matrix<typename Distance::ElementType>& inputData,
+                        const Matrix<typename Distance::ElementType>& testData, const Matrix<size_t>& matches,
+                        int checks, float& precision, const Distance& distance, int nn = 1, int skipMatches = 0)
+{
+    typedef typename Distance::ResultType DistanceType;
+
+    Logger::info("  Nodes  Precision(%)   Time(s)   Time/vec(ms)  Mean dist\n");
+    Logger::info("---------------------------------------------------------\n");
+
+    float time = 0;
+    DistanceType dist = 0;
+    precision = search_with_ground_truth(index, inputData, testData, matches, nn, checks, time, dist, distance, skipMatches);
+
+    return time;
+}
+
+template <typename Index, typename Distance>
+float test_index_precision(Index& index, const Matrix<typename Distance::ElementType>& inputData,
+                           const Matrix<typename Distance::ElementType>& testData, const Matrix<size_t>& matches,
+                           float precision, int& checks, const Distance& distance, int nn = 1, int skipMatches = 0)
+{
+    typedef typename Distance::ResultType DistanceType;
+    const float SEARCH_EPS = 0.001f;
+
+    Logger::info("  Nodes  Precision(%)   Time(s)   Time/vec(ms)  Mean dist\n");
+    Logger::info("---------------------------------------------------------\n");
+
+    int c2 = 1;
+    float p2;
+    int c1 = 1;
+//     float p1;
+    float time;
+    DistanceType dist;
+
+    p2 = search_with_ground_truth(index, inputData, testData, matches, nn, c2, time, dist, distance, skipMatches);
+
+    if (p2>precision) {
+        Logger::info("Got as close as I can\n");
+        checks = c2;
+        return time;
+    }
+
+    while (p2<precision) {
+        c1 = c2;
+//         p1 = p2;
+        c2 *=2;
+        p2 = search_with_ground_truth(index, inputData, testData, matches, nn, c2, time, dist, distance, skipMatches);
+    }
+
+    int cx;
+    float realPrecision;
+    if (fabs(p2-precision)>SEARCH_EPS) {
+        Logger::info("Start linear estimation\n");
+        // after we got to values in the vecinity of the desired precision
+        // use linear approximation get a better estimation
+
+        cx = (c1+c2)/2;
+        realPrecision = search_with_ground_truth(index, inputData, testData, matches, nn, cx, time, dist, distance, skipMatches);
+        while (fabs(realPrecision-precision)>SEARCH_EPS) {
+
+            if (realPrecision<precision) {
+                c1 = cx;
+            }
+            else {
+                c2 = cx;
+            }
+            cx = (c1+c2)/2;
+            if (cx==c1) {
+                Logger::info("Got as close as I can\n");
+                break;
+            }
+            realPrecision = search_with_ground_truth(index, inputData, testData, matches, nn, cx, time, dist, distance, skipMatches);
+        }
+
+        c2 = cx;
+        p2 = realPrecision;
+
+    }
+    else {
+        Logger::info("No need for linear estimation\n");
+        cx = c2;
+        realPrecision = p2;
+    }
+
+    checks = cx;
+    return time;
+}
+
+
+template <typename Index, typename Distance>
+void test_index_precisions(Index& index, const Matrix<typename Distance::ElementType>& inputData,
+                           const Matrix<typename Distance::ElementType>& testData, const Matrix<int>& matches,
+                           float* precisions, int precisions_length, const Distance& distance, int nn = 1, int skipMatches = 0, float maxTime = 0)
+{
+    typedef typename Distance::ResultType DistanceType;
+
+    const float SEARCH_EPS = 0.001;
+
+    // make sure precisions array is sorted
+    std::sort(precisions, precisions+precisions_length);
+
+    int pindex = 0;
+    float precision = precisions[pindex];
+
+    Logger::info("  Nodes  Precision(%)   Time(s)   Time/vec(ms)  Mean dist\n");
+    Logger::info("---------------------------------------------------------\n");
+
+    int c2 = 1;
+    float p2;
+
+    int c1 = 1;
+    float p1;
+
+    float time;
+    DistanceType dist;
+
+    p2 = search_with_ground_truth(index, inputData, testData, matches, nn, c2, time, dist, distance, skipMatches);
+
+    // if precision for 1 run down the tree is already
+    // better then some of the requested precisions, then
+    // skip those
+    while (precisions[pindex]<p2 && pindex<precisions_length) {
+        pindex++;
+    }
+
+    if (pindex==precisions_length) {
+        Logger::info("Got as close as I can\n");
+        return;
+    }
+
+    for (int i=pindex; i<precisions_length; ++i) {
+
+        precision = precisions[i];
+        while (p2<precision) {
+            c1 = c2;
+            p1 = p2;
+            c2 *=2;
+            p2 = search_with_ground_truth(index, inputData, testData, matches, nn, c2, time, dist, distance, skipMatches);
+            if ((maxTime> 0)&&(time > maxTime)&&(p2<precision)) return;
+        }
+
+        int cx;
+        float realPrecision;
+        if (fabs(p2-precision)>SEARCH_EPS) {
+            Logger::info("Start linear estimation\n");
+            // after we got to values in the vecinity of the desired precision
+            // use linear approximation get a better estimation
+
+            cx = (c1+c2)/2;
+            realPrecision = search_with_ground_truth(index, inputData, testData, matches, nn, cx, time, dist, distance, skipMatches);
+            while (fabs(realPrecision-precision)>SEARCH_EPS) {
+
+                if (realPrecision<precision) {
+                    c1 = cx;
+                }
+                else {
+                    c2 = cx;
+                }
+                cx = (c1+c2)/2;
+                if (cx==c1) {
+                    Logger::info("Got as close as I can\n");
+                    break;
+                }
+                realPrecision = search_with_ground_truth(index, inputData, testData, matches, nn, cx, time, dist, distance, skipMatches);
+            }
+
+            c2 = cx;
+            p2 = realPrecision;
+
+        }
+        else {
+            Logger::info("No need for linear estimation\n");
+            cx = c2;
+            realPrecision = p2;
+        }
+
+    }
+}
+
+}
+
+#endif //FLANN_INDEX_TESTING_H_

corelib/src/rtflann/nn/simplex_downhill.h

@@ -0,0 +1,186 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_SIMPLEX_DOWNHILL_H_
+#define RTABMAP_FLANN_SIMPLEX_DOWNHILL_H_
+
+namespace rtflann
+{
+
+/**
+    Adds val to array vals (and point to array points) and keeping the arrays sorted by vals.
+ */
+template <typename T>
+void addValue(int pos, float val, float* vals, T* point, T* points, int n)
+{
+    vals[pos] = val;
+    for (int i=0; i<n; ++i) {
+        points[pos*n+i] = point[i];
+    }
+
+    // bubble down
+    int j=pos;
+    while (j>0 && vals[j]<vals[j-1]) {
+        swap(vals[j],vals[j-1]);
+        for (int i=0; i<n; ++i) {
+            swap(points[j*n+i],points[(j-1)*n+i]);
+        }
+        --j;
+    }
+}
+
+
+/**
+    Simplex downhill optimization function.
+    Preconditions: points is a 2D mattrix of size (n+1) x n
+                    func is the cost function taking n an array of n params and returning float
+                    vals is the cost function in the n+1 simplex points, if NULL it will be computed
+
+    Postcondition: returns optimum value and points[0..n] are the optimum parameters
+ */
+template <typename T, typename F>
+float optimizeSimplexDownhill(T* points, int n, F func, float* vals = NULL )
+{
+    const int MAX_ITERATIONS = 10;
+
+    assert(n>0);
+
+    T* p_o = new T[n];
+    T* p_r = new T[n];
+    T* p_e = new T[n];
+
+    int alpha = 1;
+
+    int iterations = 0;
+
+    bool ownVals = false;
+    if (vals == NULL) {
+        ownVals = true;
+        vals = new float[n+1];
+        for (int i=0; i<n+1; ++i) {
+            float val = func(points+i*n);
+            addValue(i, val, vals, points+i*n, points, n);
+        }
+    }
+    int nn = n*n;
+
+    while (true) {
+
+        if (iterations++ > MAX_ITERATIONS) break;
+
+        // compute average of simplex points (except the highest point)
+        for (int j=0; j<n; ++j) {
+            p_o[j] = 0;
+            for (int i=0; i<n; ++i) {
+                p_o[i] += points[j*n+i];
+            }
+        }
+        for (int i=0; i<n; ++i) {
+            p_o[i] /= n;
+        }
+
+        bool converged = true;
+        for (int i=0; i<n; ++i) {
+            if (p_o[i] != points[nn+i]) {
+                converged = false;
+            }
+        }
+        if (converged) break;
+
+        // trying a reflection
+        for (int i=0; i<n; ++i) {
+            p_r[i] = p_o[i] + alpha*(p_o[i]-points[nn+i]);
+        }
+        float val_r = func(p_r);
+
+        if ((val_r>=vals[0])&&(val_r<vals[n])) {
+            // reflection between second highest and lowest
+            // add it to the simplex
+            Logger::info("Choosing reflection\n");
+            addValue(n, val_r,vals, p_r, points, n);
+            continue;
+        }
+
+        if (val_r<vals[0]) {
+            // value is smaller than smalest in simplex
+
+            // expand some more to see if it drops further
+            for (int i=0; i<n; ++i) {
+                p_e[i] = 2*p_r[i]-p_o[i];
+            }
+            float val_e = func(p_e);
+
+            if (val_e<val_r) {
+                Logger::info("Choosing reflection and expansion\n");
+                addValue(n, val_e,vals,p_e,points,n);
+            }
+            else {
+                Logger::info("Choosing reflection\n");
+                addValue(n, val_r,vals,p_r,points,n);
+            }
+            continue;
+        }
+        if (val_r>=vals[n]) {
+            for (int i=0; i<n; ++i) {
+                p_e[i] = (p_o[i]+points[nn+i])/2;
+            }
+            float val_e = func(p_e);
+
+            if (val_e<vals[n]) {
+                Logger::info("Choosing contraction\n");
+                addValue(n,val_e,vals,p_e,points,n);
+                continue;
+            }
+        }
+        {
+            Logger::info("Full contraction\n");
+            for (int j=1; j<=n; ++j) {
+                for (int i=0; i<n; ++i) {
+                    points[j*n+i] = (points[j*n+i]+points[i])/2;
+                }
+                float val = func(points+j*n);
+                addValue(j,val,vals,points+j*n,points,n);
+            }
+        }
+    }
+
+    float bestVal = vals[0];
+
+    delete[] p_r;
+    delete[] p_o;
+    delete[] p_e;
+    if (ownVals) delete[] vals;
+
+    return bestVal;
+}
+
+}
+
+#endif //FLANN_SIMPLEX_DOWNHILL_H_

corelib/src/rtflann/readme.txt

@@ -0,0 +1,8 @@
+
+flann is included in rtabmap for convenience, to have this commit needed for incremental flann index:
+https://github.com/mariusmuja/flann/commit/23051820b2314f07cf40ba633a4067782a982ff3
+
+Info: http://www.cs.ubc.ca/research/flann/
+Source: https://github.com/mariusmuja/flann
+Commit: 9a01507d2231b29eb688246a6e442054defa7460
+License: BSD 

corelib/src/rtflann/util/allocator.h

@@ -0,0 +1,202 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_ALLOCATOR_H_
+#define RTABMAP_FLANN_ALLOCATOR_H_
+
+#include <stdlib.h>
+#include <stdio.h>
+
+
+namespace rtflann
+{
+
+/**
+ * Allocates (using C's malloc) a generic type T.
+ *
+ * Params:
+ *     count = number of instances to allocate.
+ * Returns: pointer (of type T*) to memory buffer
+ */
+template <typename T>
+T* allocate(size_t count = 1)
+{
+    T* mem = (T*) ::malloc(sizeof(T)*count);
+    return mem;
+}
+
+
+
+/**
+ * Pooled storage allocator
+ *
+ * The following routines allow for the efficient allocation of storage in
+ * small chunks from a specified pool.  Rather than allowing each structure
+ * to be freed individually, an entire pool of storage is freed at once.
+ * This method has two advantages over just using malloc() and free().  First,
+ * it is far more efficient for allocating small objects, as there is
+ * no overhead for remembering all the information needed to free each
+ * object or consolidating fragmented memory.  Second, the decision about
+ * how long to keep an object is made at the time of allocation, and there
+ * is no need to track down all the objects to free them.
+ *
+ */
+
+const size_t     WORDSIZE=16;
+const  size_t     BLOCKSIZE=8192;
+
+class PooledAllocator
+{
+    /* We maintain memory alignment to word boundaries by requiring that all
+        allocations be in multiples of the machine wordsize.  */
+    /* Size of machine word in bytes.  Must be power of 2. */
+    /* Minimum number of bytes requested at a time from	the system.  Must be multiple of WORDSIZE. */
+
+
+    int     remaining;  /* Number of bytes left in current block of storage. */
+    void*   base;     /* Pointer to base of current block of storage. */
+    void*   loc;      /* Current location in block to next allocate memory. */
+    int     blocksize;
+
+
+public:
+    int     usedMemory;
+    int     wastedMemory;
+
+    /**
+        Default constructor. Initializes a new pool.
+     */
+    PooledAllocator(int blocksize = BLOCKSIZE)
+    {
+        this->blocksize = blocksize;
+        remaining = 0;
+        base = NULL;
+
+        usedMemory = 0;
+        wastedMemory = 0;
+    }
+
+    /**
+     * Destructor. Frees all the memory allocated in this pool.
+     */
+    ~PooledAllocator()
+    {
+        free();
+    }
+    
+    void free()
+    {
+        void* prev;
+        while (base != NULL) {
+            prev = *((void**) base); /* Get pointer to prev block. */
+            ::free(base);
+            base = prev;
+        }
+        base = NULL;
+        remaining = 0;
+        usedMemory = 0;
+        wastedMemory = 0;
+    }
+
+    /**
+     * Returns a pointer to a piece of new memory of the given size in bytes
+     * allocated from the pool.
+     */
+    void* allocateMemory(int size)
+    {
+        int blocksize;
+
+        /* Round size up to a multiple of wordsize.  The following expression
+            only works for WORDSIZE that is a power of 2, by masking last bits of
+            incremented size to zero.
+         */
+        size = (size + (WORDSIZE - 1)) & ~(WORDSIZE - 1);
+
+        /* Check whether a new block must be allocated.  Note that the first word
+            of a block is reserved for a pointer to the previous block.
+         */
+        if (size > remaining) {
+
+            wastedMemory += remaining;
+
+            /* Allocate new storage. */
+            blocksize = (size + sizeof(void*) + (WORDSIZE-1) > BLOCKSIZE) ?
+                        size + sizeof(void*) + (WORDSIZE-1) : BLOCKSIZE;
+
+            // use the standard C malloc to allocate memory
+            void* m = ::malloc(blocksize);
+            if (!m) {
+                fprintf(stderr,"Failed to allocate memory.\n");
+                return NULL;
+            }
+
+            /* Fill first word of new block with pointer to previous block. */
+            ((void**) m)[0] = base;
+            base = m;
+
+            int shift = 0;
+            //int shift = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & (WORDSIZE-1))) & (WORDSIZE-1);
+
+            remaining = blocksize - sizeof(void*) - shift;
+            loc = ((char*)m + sizeof(void*) + shift);
+        }
+        void* rloc = loc;
+        loc = (char*)loc + size;
+        remaining -= size;
+
+        usedMemory += size;
+
+        return rloc;
+    }
+
+    /**
+     * Allocates (using this pool) a generic type T.
+     *
+     * Params:
+     *     count = number of instances to allocate.
+     * Returns: pointer (of type T*) to memory buffer
+     */
+    template <typename T>
+    T* allocate(size_t count = 1)
+    {
+        T* mem = (T*) this->allocateMemory((int)(sizeof(T)*count));
+        return mem;
+    }
+
+};
+
+}
+
+inline void* operator new (std::size_t size, rtflann::PooledAllocator& allocator)
+{
+    return allocator.allocateMemory(size) ;
+}
+
+#endif //FLANN_ALLOCATOR_H_

corelib/src/rtflann/util/any.h

@@ -0,0 +1,294 @@
+#ifndef RTABMAP_FLANN_ANY_H_
+#define RTABMAP_FLANN_ANY_H_
+/*
+ * (C) Copyright Christopher Diggins 2005-2011
+ * (C) Copyright Pablo Aguilar 2005
+ * (C) Copyright Kevlin Henney 2001
+ *
+ * Distributed under the Boost Software License, Version 1.0. (See
+ * accompanying file LICENSE_1_0.txt or copy at
+ * http://www.boost.org/LICENSE_1_0.txt
+ *
+ * Adapted for FLANN by Marius Muja
+ */
+
+#include <stdexcept>
+#include <ostream>
+#include <typeinfo>
+
+namespace rtflann
+{
+
+namespace anyimpl
+{
+
+struct bad_any_cast : public std::runtime_error
+{
+	bad_any_cast() : std::runtime_error("Cannot convert 'any' value") { }
+};
+
+struct empty_any
+{
+};
+
+inline std::ostream& operator <<(std::ostream& out, const empty_any&)
+{
+    out << "[empty_any]";
+    return out;
+}
+
+struct base_any_policy
+{
+    virtual void static_delete(void** x) = 0;
+    virtual void copy_from_value(void const* src, void** dest) = 0;
+    virtual void clone(void* const* src, void** dest) = 0;
+    virtual void move(void* const* src, void** dest) = 0;
+    virtual void* get_value(void** src) = 0;
+    virtual const void* get_value(void* const * src) = 0;
+    virtual ::size_t get_size() = 0;
+    virtual const std::type_info& type() = 0;
+    virtual void print(std::ostream& out, void* const* src) = 0;
+};
+
+template<typename T>
+struct typed_base_any_policy : base_any_policy
+{
+    virtual ::size_t get_size() { return sizeof(T); }
+    virtual const std::type_info& type() { return typeid(T); }
+
+};
+
+template<typename T>
+struct small_any_policy : typed_base_any_policy<T>
+{
+    virtual void static_delete(void**) { }
+    virtual void copy_from_value(void const* src, void** dest)
+    {
+        new (dest) T(* reinterpret_cast<T const*>(src));
+    }
+    virtual void clone(void* const* src, void** dest) { *dest = *src; }
+    virtual void move(void* const* src, void** dest) { *dest = *src; }
+    virtual void* get_value(void** src) { return reinterpret_cast<void*>(src); }
+    virtual const void* get_value(void* const * src) { return reinterpret_cast<const void*>(src); }
+    virtual void print(std::ostream& out, void* const* src) { out << *reinterpret_cast<T const*>(src); }
+};
+
+template<typename T>
+struct big_any_policy : typed_base_any_policy<T>
+{
+    virtual void static_delete(void** x)
+    {
+        if (* x) delete (* reinterpret_cast<T**>(x)); *x = NULL;
+    }
+    virtual void copy_from_value(void const* src, void** dest)
+    {
+        *dest = new T(*reinterpret_cast<T const*>(src));
+    }
+    virtual void clone(void* const* src, void** dest)
+    {
+        *dest = new T(**reinterpret_cast<T* const*>(src));
+    }
+    virtual void move(void* const* src, void** dest)
+    {
+        (*reinterpret_cast<T**>(dest))->~T();
+        **reinterpret_cast<T**>(dest) = **reinterpret_cast<T* const*>(src);
+    }
+    virtual void* get_value(void** src) { return *src; }
+    virtual const void* get_value(void* const * src) { return *src; }
+    virtual void print(std::ostream& out, void* const* src) { out << *reinterpret_cast<T const*>(*src); }
+};
+
+template<typename T>
+struct choose_policy
+{
+    typedef big_any_policy<T> type;
+};
+
+template<typename T>
+struct choose_policy<T*>
+{
+    typedef small_any_policy<T*> type;
+};
+
+struct any;
+
+/// Choosing the policy for an any type is illegal, but should never happen.
+/// This is designed to throw a compiler error.
+template<>
+struct choose_policy<any>
+{
+    typedef void type;
+};
+
+/// Specializations for small types.
+#define SMALL_POLICY(TYPE) \
+    template<> \
+    struct choose_policy<TYPE> { typedef small_any_policy<TYPE> type; \
+    }
+
+SMALL_POLICY(signed char);
+SMALL_POLICY(unsigned char);
+SMALL_POLICY(signed short);
+SMALL_POLICY(unsigned short);
+SMALL_POLICY(signed int);
+SMALL_POLICY(unsigned int);
+SMALL_POLICY(signed long);
+SMALL_POLICY(unsigned long);
+SMALL_POLICY(float);
+SMALL_POLICY(bool);
+
+//#undef SMALL_POLICY
+
+/// This function will return a different policy for each type.
+template<typename T>
+base_any_policy* get_policy()
+{
+    static typename choose_policy<T>::type policy;
+    return &policy;
+}
+} // namespace anyimpl
+
+class any
+{
+	typedef any any_t; // workaround for the NVCC compiler under windows
+private:
+    // fields
+    anyimpl::base_any_policy* policy;
+    void* object;
+
+public:
+    /// Initializing constructor.
+    template <typename T>
+    any(const T& x)
+        : policy(anyimpl::get_policy<anyimpl::empty_any>()), object(NULL)
+    {
+        assign(x);
+    }
+
+    /// Empty constructor.
+    any()
+        : policy(anyimpl::get_policy<anyimpl::empty_any>()), object(NULL)
+    { }
+
+    /// Special initializing constructor for string literals.
+    any(const char* x)
+        : policy(anyimpl::get_policy<anyimpl::empty_any>()), object(NULL)
+    {
+        assign(x);
+    }
+
+    /// Copy constructor.
+    any(const any& x)
+        : policy(anyimpl::get_policy<anyimpl::empty_any>()), object(NULL)
+    {
+        assign(x);
+    }
+
+    /// Destructor.
+    ~any()
+    {
+        policy->static_delete(&object);
+    }
+
+    /// Assignment function from another any.
+    any& assign(const any& x)
+    {
+        reset();
+        policy = x.policy;
+        policy->clone(&x.object, &object);
+        return *this;
+    }
+
+    /// Assignment function.
+    template <typename T>
+    any_t& assign(const T& x)
+    {
+        reset();
+        policy = anyimpl::get_policy<T>();
+        policy->copy_from_value(&x, &object);
+        return *this;
+    }
+
+    /// Assignment operator.
+    template<typename T>
+    any_t& operator=(const T& x)
+    {
+        return assign(x);
+    }
+
+    /// Assignment operator, specialed for literal strings.
+    /// They have types like const char [6] which don't work as expected.
+    any& operator=(const char* x)
+    {
+        return assign(x);
+    }
+
+    /// Utility functions
+    any& swap(any& x)
+    {
+        std::swap(policy, x.policy);
+        std::swap(object, x.object);
+        return *this;
+    }
+
+    /// Cast operator. You can only cast to the original type.
+    template<typename T>
+    T& cast()
+    {
+        if (policy->type() != typeid(T)) throw anyimpl::bad_any_cast();
+        T* r = reinterpret_cast<T*>(policy->get_value(&object));
+        return *r;
+    }
+
+    /// Cast operator. You can only cast to the original type.
+    template<typename T>
+    const T& cast() const
+    {
+        if (policy->type() != typeid(T)) throw anyimpl::bad_any_cast();
+        const T* r = reinterpret_cast<const T*>(policy->get_value(&object));
+        return *r;
+    }
+
+    /// Returns true if the any contains no value.
+    bool empty() const
+    {
+        return policy->type() == typeid(anyimpl::empty_any);
+    }
+
+    /// Frees any allocated memory, and sets the value to NULL.
+    void reset()
+    {
+        policy->static_delete(&object);
+        policy = anyimpl::get_policy<anyimpl::empty_any>();
+    }
+
+    /// Returns true if the two types are the same.
+    bool compatible(const any& x) const
+    {
+        return policy->type() == x.policy->type();
+    }
+
+    /// Returns if the type is compatible with the policy
+    template<typename T>
+    bool has_type()
+    {
+        return policy->type() == typeid(T);
+    }
+
+    const std::type_info& type() const
+    {
+        return policy->type();
+    }
+
+    friend std::ostream& operator <<(std::ostream& out, const any& any_val);
+};
+
+inline std::ostream& operator <<(std::ostream& out, const any& any_val)
+{
+    any_val.policy->print(out,&any_val.object);
+    return out;
+}
+
+}
+
+#endif // FLANN_ANY_H_

corelib/src/rtflann/util/dynamic_bitset.h

@@ -0,0 +1,163 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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.
+ *************************************************************************/
+
+/***********************************************************************
+ * Author: Vincent Rabaud
+ *************************************************************************/
+
+#ifndef RTABMAP_FLANN_DYNAMIC_BITSET_H_
+#define RTABMAP_FLANN_DYNAMIC_BITSET_H_
+
+//#define FLANN_USE_BOOST 1
+#if FLANN_USE_BOOST
+#include <boost/dynamic_bitset.hpp>
+typedef boost::dynamic_bitset<> DynamicBitset;
+#else
+
+#include <limits.h>
+
+namespace rtflann {
+
+/** Class re-implementing the boost version of it
+ * This helps not depending on boost, it also does not do the bound checks
+ * and has a way to reset a block for speed
+ */
+class DynamicBitset
+{
+public:
+    /** @param default constructor
+     */
+    DynamicBitset() : size_(0)
+    {
+    }
+
+    /** @param only constructor we use in our code
+     * @param the size of the bitset (in bits)
+     */
+    DynamicBitset(size_t size)
+    {
+        resize(size);
+        reset();
+    }
+
+    /** Sets all the bits to 0
+     */
+    void clear()
+    {
+        std::fill(bitset_.begin(), bitset_.end(), 0);
+    }
+
+    /** @brief checks if the bitset is empty
+     * @return true if the bitset is empty
+     */
+    bool empty() const
+    {
+        return bitset_.empty();
+    }
+
+    /** @param set all the bits to 0
+     */
+    void reset()
+    {
+        std::fill(bitset_.begin(), bitset_.end(), 0);
+    }
+
+    /** @brief set one bit to 0
+     * @param
+     */
+    void reset(size_t index)
+    {
+        bitset_[index / cell_bit_size_] &= ~(size_t(1) << (index % cell_bit_size_));
+    }
+
+    /** @brief sets a specific bit to 0, and more bits too
+     * This function is useful when resetting a given set of bits so that the
+     * whole bitset ends up being 0: if that's the case, we don't care about setting
+     * other bits to 0
+     * @param
+     */
+    void reset_block(size_t index)
+    {
+        bitset_[index / cell_bit_size_] = 0;
+    }
+
+    /** @param resize the bitset so that it contains at least size bits
+     * @param size
+     */
+    void resize(size_t size)
+    {
+        size_ = size;
+        bitset_.resize(size / cell_bit_size_ + 1);
+    }
+
+    /** @param set a bit to true
+     * @param index the index of the bit to set to 1
+     */
+    void set(size_t index)
+    {
+        bitset_[index / cell_bit_size_] |= size_t(1) << (index % cell_bit_size_);
+    }
+
+    /** @param gives the number of contained bits
+     */
+    size_t size() const
+    {
+        return size_;
+    }
+
+    /** @param check if a bit is set
+     * @param index the index of the bit to check
+     * @return true if the bit is set
+     */
+    bool test(size_t index) const
+    {
+        return (bitset_[index / cell_bit_size_] & (size_t(1) << (index % cell_bit_size_))) != 0;
+    }
+
+private:
+    template <typename Archive>
+    void serialize(Archive& ar)
+    {
+    	ar & size_;
+    	ar & bitset_;
+    }
+    friend struct serialization::access;
+
+private:
+    std::vector<size_t> bitset_;
+    size_t size_;
+    static const unsigned int cell_bit_size_ = CHAR_BIT * sizeof(size_t);
+};
+
+} // namespace flann
+
+#endif
+
+#endif // FLANN_DYNAMIC_BITSET_H_

corelib/src/rtflann/util/heap.h

@@ -0,0 +1,456 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_HEAP_H_
+#define RTABMAP_FLANN_HEAP_H_
+
+#include <algorithm>
+#include <vector>
+
+namespace rtflann
+{
+
+/**
+ * Priority Queue Implementation
+ *
+ * The priority queue is implemented with a heap.  A heap is a complete
+ * (full) binary tree in which each parent is less than both of its
+ * children, but the order of the children is unspecified.
+ */
+template <typename T>
+class Heap
+{
+
+    /**
+     * Storage array for the heap.
+     * Type T must be comparable.
+     */
+    std::vector<T> heap;
+    int length;
+
+    /**
+     * Number of element in the heap
+     */
+    int count;
+
+
+
+public:
+    /**
+     * Constructor.
+     *
+     * Params:
+     *     size = heap size
+     */
+
+    Heap(int size)
+    {
+        length = size;
+        heap.reserve(length);
+        count = 0;
+    }
+
+    /**
+     *
+     * Returns: heap size
+     */
+    int size()
+    {
+        return count;
+    }
+
+    /**
+     * Tests if the heap is empty
+     *
+     * Returns: true is heap empty, false otherwise
+     */
+    bool empty()
+    {
+        return size()==0;
+    }
+
+    /**
+     * Clears the heap.
+     */
+    void clear()
+    {
+        heap.clear();
+        count = 0;
+    }
+
+    struct CompareT : public std::binary_function<T,T,bool>
+    {
+        bool operator()(const T& t_1, const T& t_2) const
+        {
+            return t_2 < t_1;
+        }
+    };
+
+    /**
+     * Insert a new element in the heap.
+     *
+     * We select the next empty leaf node, and then keep moving any larger
+     * parents down until the right location is found to store this element.
+     *
+     * Params:
+     *     value = the new element to be inserted in the heap
+     */
+    void insert(const T& value)
+    {
+        /* If heap is full, then return without adding this element. */
+        if (count == length) {
+            return;
+        }
+
+        heap.push_back(value);
+        static CompareT compareT;
+        std::push_heap(heap.begin(), heap.end(), compareT);
+        ++count;
+    }
+
+
+
+    /**
+     * Returns the node of minimum value from the heap (top of the heap).
+     *
+     * Params:
+     *     value = out parameter used to return the min element
+     * Returns: false if heap empty
+     */
+    bool popMin(T& value)
+    {
+        if (count == 0) {
+            return false;
+        }
+
+        value = heap[0];
+        static CompareT compareT;
+        std::pop_heap(heap.begin(), heap.end(), compareT);
+        heap.pop_back();
+        --count;
+
+        return true;  /* Return old last node. */
+    }
+};
+
+
+template <typename T>
+class IntervalHeap
+{
+	struct Interval
+	{
+		T left;
+		T right;
+	};
+
+    /**
+     * Storage array for the heap.
+     * Type T must be comparable.
+     */
+    std::vector<Interval> heap;
+    size_t capacity_;
+    size_t size_;
+
+public:
+    /**
+     * Constructor.
+     *
+     * Params:
+     *     size = heap size
+     */
+
+    IntervalHeap(int capacity) : capacity_(capacity), size_(0)
+    {
+        heap.resize(capacity/2 + capacity%2 + 1); // 1-based indexing
+    }
+
+    /**
+     * @return Heap size
+     */
+    size_t size()
+    {
+        return size_;
+    }
+
+    /**
+     * Tests if the heap is empty
+     * @return true is heap empty, false otherwise
+     */
+    bool empty()
+    {
+        return size_==0;
+    }
+
+    /**
+     * Clears the heap.
+     */
+    void clear()
+    {
+        size_ = 0;
+    }
+
+    void insert(const T& value)
+    {
+        /* If heap is full, then return without adding this element. */
+        if (size_ == capacity_) {
+            return;
+        }
+
+        // insert into the root
+        if (size_<2) {
+        	if (size_==0) {
+        		heap[1].left = value;
+        		heap[1].right = value;
+        	}
+        	else {
+        		if (value<heap[1].left) {
+        			heap[1].left = value;
+        		}
+        		else {
+        			heap[1].right = value;
+        		}
+        	}
+        	++size_;
+        	return;
+        }
+
+        size_t last_pos = size_/2 + size_%2;
+        bool min_heap;
+
+        if (size_%2) { // odd number of elements
+        	min_heap = (value<heap[last_pos].left)? true : false;
+        }
+        else {
+        	++last_pos;
+        	min_heap = (value<heap[last_pos/2].left)? true : false;
+        }
+
+        if (min_heap) {
+        	size_t pos = last_pos;
+        	size_t par = pos/2;
+        	while (pos>1 && value < heap[par].left) {
+        		heap[pos].left = heap[par].left;
+        		pos = par;
+        		par = pos/2;
+        	}
+        	heap[pos].left = value;
+        	++size_;
+
+        	if (size_%2) { // duplicate element in last position if size is odd
+        		heap[last_pos].right = heap[last_pos].left;
+        	}
+        }
+        else {
+        	size_t pos = last_pos;
+        	size_t par = pos/2;
+        	while (pos>1 && heap[par].right < value) {
+        		heap[pos].right = heap[par].right;
+        		pos = par;
+        		par = pos/2;
+        	}
+        	heap[pos].right = value;
+        	++size_;
+
+        	if (size_%2) { // duplicate element in last position if size is odd
+        		heap[last_pos].left = heap[last_pos].right;
+        	}
+        }
+    }
+
+
+    /**
+     * Returns the node of minimum value from the heap
+     * @param value out parameter used to return the min element
+     * @return false if heap empty
+     */
+    bool popMin(T& value)
+    {
+        if (size_ == 0) {
+            return false;
+        }
+
+        value = heap[1].left;
+        size_t last_pos = size_/2 + size_%2;
+        T elem = heap[last_pos].left;
+
+        if (size_ % 2) { // odd number of elements
+        	--last_pos;
+        }
+        else {
+        	heap[last_pos].left = heap[last_pos].right;
+        }
+        --size_;
+        if (size_<2) return true;
+
+        size_t crt=1; // root node
+        size_t child = crt*2;
+
+        while (child <= last_pos) {
+        	if (child < last_pos && heap[child+1].left < heap[child].left) ++child; // pick the child with min
+
+        	if (!(heap[child].left<elem)) break;
+
+        	heap[crt].left = heap[child].left;
+        	if (heap[child].right<elem) {
+        		std::swap(elem, heap[child].right);
+        	}
+
+        	crt = child;
+    		child *= 2;
+        }
+        heap[crt].left = elem;
+        return true;
+    }
+
+
+    /**
+     * Returns the element of maximum value from the heap
+     * @param value
+     * @return false if heap empty
+     */
+    bool popMax(T& value)
+    {
+        if (size_ == 0) {
+            return false;
+        }
+
+        value = heap[1].right;
+        size_t last_pos = size_/2 + size_%2;
+        T elem = heap[last_pos].right;
+
+        if (size_%2) { // odd number of elements
+        	--last_pos;
+        }
+        else {
+        	heap[last_pos].right = heap[last_pos].left;
+        }
+        --size_;
+        if (size_<2) return true;
+
+        size_t crt=1; // root node
+        size_t child = crt*2;
+
+        while (child <= last_pos) {
+        	if (child < last_pos && heap[child].right < heap[child+1].right) ++child; // pick the child with max
+
+        	if (!(elem < heap[child].right)) break;
+
+        	heap[crt].right = heap[child].right;
+        	if (elem<heap[child].left) {
+        		std::swap(elem, heap[child].left);
+        	}
+
+        	crt = child;
+    		child *= 2;
+        }
+        heap[crt].right = elem;
+        return true;
+    }
+
+
+    bool getMin(T& value)
+    {
+    	if (size_==0) {
+    		return false;
+    	}
+    	value = heap[1].left;
+    	return true;
+    }
+
+
+    bool getMax(T& value)
+    {
+    	if (size_==0) {
+    		return false;
+    	}
+    	value = heap[1].right;
+    	return true;
+    }
+};
+
+
+template <typename T>
+class BoundedHeap
+{
+	IntervalHeap<T> interval_heap_;
+	size_t capacity_;
+public:
+	BoundedHeap(size_t capacity) : interval_heap_(capacity), capacity_(capacity)
+	{
+
+	}
+
+    /**
+     * Returns: heap size
+     */
+    int size()
+    {
+        return interval_heap_.size();
+    }
+
+    /**
+     * Tests if the heap is empty
+     * Returns: true is heap empty, false otherwise
+     */
+    bool empty()
+    {
+        return interval_heap_.empty();
+    }
+
+    /**
+     * Clears the heap.
+     */
+    void clear()
+    {
+    	interval_heap_.clear();
+    }
+
+    void insert(const T& value)
+    {
+    	if (interval_heap_.size()==capacity_) {
+    		T max;
+    		interval_heap_.getMax(max);
+    		if (max<value) return;
+   			interval_heap_.popMax(max);
+    	}
+    	interval_heap_.insert(value);
+    }
+
+    bool popMin(T& value)
+    {
+    	return interval_heap_.popMin(value);
+    }
+};
+
+
+
+}
+
+#endif //FLANN_HEAP_H_

corelib/src/rtflann/util/logger.h

@@ -0,0 +1,137 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_LOGGER_H
+#define RTABMAP_FLANN_LOGGER_H
+
+#include <stdio.h>
+#include <stdarg.h>
+
+#include "rtflann/defines.h"
+
+
+namespace rtflann
+{
+
+class Logger
+{
+    Logger() : stream(stdout), logLevel(FLANN_LOG_WARN) {}
+
+    ~Logger()
+    {
+        if ((stream!=NULL)&&(stream!=stdout)) {
+            fclose(stream);
+        }
+    }
+
+    static Logger& instance()
+    {
+        static Logger logger;
+        return logger;
+    }
+
+    void _setDestination(const char* name)
+    {
+        if (name==NULL) {
+            stream = stdout;
+        }
+        else {
+            stream = fopen(name,"w");
+            if (stream == NULL) {
+                stream = stdout;
+            }
+        }
+    }
+
+    int _log(int level, const char* fmt, va_list arglist)
+    {
+        if (level > logLevel ) return -1;
+        int ret = vfprintf(stream, fmt, arglist);
+        return ret;
+    }
+
+public:
+    /**
+     * Sets the logging level. All messages with lower priority will be ignored.
+     * @param level Logging level
+     */
+    static void setLevel(int level) { instance().logLevel = level; }
+
+    /**
+     * Returns the currently set logging level.
+     * @return current logging level
+     */
+    static int getLevel() { return instance().logLevel; }
+
+    /**
+     * Sets the logging destination
+     * @param name Filename or NULL for console
+     */
+    static void setDestination(const char* name) { instance()._setDestination(name); }
+
+    /**
+     * Print log message
+     * @param level Log level
+     * @param fmt Message format
+     * @return
+     */
+    static int log(int level, const char* fmt, ...)
+    {
+        va_list arglist;
+        va_start(arglist, fmt);
+        int ret = instance()._log(level,fmt,arglist);
+        va_end(arglist);
+        return ret;
+    }
+
+#define LOG_METHOD(NAME,LEVEL) \
+    static int NAME(const char* fmt, ...) \
+    { \
+        va_list ap; \
+        va_start(ap, fmt); \
+        int ret = instance()._log(LEVEL, fmt, ap); \
+        va_end(ap); \
+        return ret; \
+    }
+
+    LOG_METHOD(fatal, FLANN_LOG_FATAL)
+    LOG_METHOD(error, FLANN_LOG_ERROR)
+    LOG_METHOD(warn, FLANN_LOG_WARN)
+    LOG_METHOD(info, FLANN_LOG_INFO)
+    LOG_METHOD(debug, FLANN_LOG_DEBUG)
+
+private:
+    FILE* stream;
+    int logLevel;
+};
+
+}
+
+#endif //RTABMAP_FLANN_LOGGER_H

corelib/src/rtflann/util/lsh_table.h

@@ -0,0 +1,506 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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.
+ *************************************************************************/
+
+/***********************************************************************
+ * Author: Vincent Rabaud
+ *************************************************************************/
+
+#ifndef RTABMAP_FLANN_LSH_TABLE_H_
+#define RTABMAP_FLANN_LSH_TABLE_H_
+
+#include <algorithm>
+#include <iostream>
+#include <iomanip>
+#include <limits.h>
+// TODO as soon as we use C++0x, use the code in USE_UNORDERED_MAP
+#if USE_UNORDERED_MAP
+#include <unordered_map>
+#else
+#include <map>
+#endif
+#include <math.h>
+#include <stddef.h>
+
+#include "rtflann/util/dynamic_bitset.h"
+#include "rtflann/util/matrix.h"
+
+namespace rtflann
+{
+
+namespace lsh
+{
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** What is stored in an LSH bucket
+ */
+typedef uint32_t FeatureIndex;
+/** The id from which we can get a bucket back in an LSH table
+ */
+typedef unsigned int BucketKey;
+
+/** A bucket in an LSH table
+ */
+typedef std::vector<FeatureIndex> Bucket;
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** POD for stats about an LSH table
+ */
+struct LshStats
+{
+    std::vector<unsigned int> bucket_sizes_;
+    size_t n_buckets_;
+    size_t bucket_size_mean_;
+    size_t bucket_size_median_;
+    size_t bucket_size_min_;
+    size_t bucket_size_max_;
+    size_t bucket_size_std_dev;
+    /** Each contained vector contains three value: beginning/end for interval, number of elements in the bin
+     */
+    std::vector<std::vector<unsigned int> > size_histogram_;
+};
+
+/** Overload the << operator for LshStats
+ * @param out the streams
+ * @param stats the stats to display
+ * @return the streams
+ */
+inline std::ostream& operator <<(std::ostream& out, const LshStats& stats)
+{
+    size_t w = 20;
+    out << "Lsh Table Stats:\n" << std::setw(w) << std::setiosflags(std::ios::right) << "N buckets : "
+    << stats.n_buckets_ << "\n" << std::setw(w) << std::setiosflags(std::ios::right) << "mean size : "
+    << std::setiosflags(std::ios::left) << stats.bucket_size_mean_ << "\n" << std::setw(w)
+    << std::setiosflags(std::ios::right) << "median size : " << stats.bucket_size_median_ << "\n" << std::setw(w)
+    << std::setiosflags(std::ios::right) << "min size : " << std::setiosflags(std::ios::left)
+    << stats.bucket_size_min_ << "\n" << std::setw(w) << std::setiosflags(std::ios::right) << "max size : "
+    << std::setiosflags(std::ios::left) << stats.bucket_size_max_;
+
+    // Display the histogram
+    out << std::endl << std::setw(w) << std::setiosflags(std::ios::right) << "histogram : "
+    << std::setiosflags(std::ios::left);
+    for (std::vector<std::vector<unsigned int> >::const_iterator iterator = stats.size_histogram_.begin(), end =
+             stats.size_histogram_.end(); iterator != end; ++iterator) out << (*iterator)[0] << "-" << (*iterator)[1] << ": " << (*iterator)[2] << ",  ";
+
+    return out;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** Lsh hash table. As its key is a sub-feature, and as usually
+ * the size of it is pretty small, we keep it as a continuous memory array.
+ * The value is an index in the corpus of features (we keep it as an unsigned
+ * int for pure memory reasons, it could be a size_t)
+ */
+template<typename ElementType>
+class LshTable
+{
+public:
+    /** A container of all the feature indices. Optimized for space
+     */
+#if USE_UNORDERED_MAP
+    typedef std::unordered_map<BucketKey, Bucket> BucketsSpace;
+#else
+    typedef std::map<BucketKey, Bucket> BucketsSpace;
+#endif
+
+    /** A container of all the feature indices. Optimized for speed
+     */
+    typedef std::vector<Bucket> BucketsSpeed;
+
+    /** Default constructor
+     */
+    LshTable()
+    {
+    }
+
+    /** Default constructor
+     * Create the mask and allocate the memory
+     * @param feature_size is the size of the feature (considered as a ElementType[])
+     * @param key_size is the number of bits that are turned on in the feature
+     */
+    LshTable(unsigned int /*feature_size*/, unsigned int /*key_size*/)
+    {
+        std::cerr << "LSH is not implemented for that type" << std::endl;
+        throw;
+    }
+
+    /** Add a feature to the table
+     * @param value the value to store for that feature
+     * @param feature the feature itself
+     */
+    void add(unsigned int value, const ElementType* feature)
+    {
+        // Add the value to the corresponding bucket
+        BucketKey key = getKey(feature);
+
+        switch (speed_level_) {
+        case kArray:
+            // That means we get the buckets from an array
+            buckets_speed_[key].push_back(value);
+            break;
+        case kBitsetHash:
+            // That means we can check the bitset for the presence of a key
+            key_bitset_.set(key);
+            buckets_space_[key].push_back(value);
+            break;
+        case kHash:
+        {
+            // That means we have to check for the hash table for the presence of a key
+            buckets_space_[key].push_back(value);
+            break;
+        }
+        }
+    }
+
+    /** Add a set of features to the table
+     * @param dataset the values to store
+     */
+    void add(const std::vector< std::pair<size_t, ElementType*> >& features)
+    {
+#if USE_UNORDERED_MAP
+        buckets_space_.rehash((buckets_space_.size() + features.size()) * 1.2);
+#endif
+        // Add the features to the table
+        for (size_t i = 0; i < features.size(); ++i) {
+        	add(features[i].first, features[i].second);
+        }
+        // Now that the table is full, optimize it for speed/space
+        optimize();
+    }
+
+    /** Get a bucket given the key
+     * @param key
+     * @return
+     */
+    inline const Bucket* getBucketFromKey(BucketKey key) const
+    {
+        // Generate other buckets
+        switch (speed_level_) {
+        case kArray:
+            // That means we get the buckets from an array
+            return &buckets_speed_[key];
+            break;
+        case kBitsetHash:
+            // That means we can check the bitset for the presence of a key
+            if (key_bitset_.test(key)) return &buckets_space_.find(key)->second;
+            else return 0;
+            break;
+        case kHash:
+        {
+            // That means we have to check for the hash table for the presence of a key
+            BucketsSpace::const_iterator bucket_it, bucket_end = buckets_space_.end();
+            bucket_it = buckets_space_.find(key);
+            // Stop here if that bucket does not exist
+            if (bucket_it == bucket_end) return 0;
+            else return &bucket_it->second;
+            break;
+        }
+        }
+        return 0;
+    }
+
+    /** Compute the sub-signature of a feature
+     */
+    size_t getKey(const ElementType* /*feature*/) const
+    {
+        std::cerr << "LSH is not implemented for that type" << std::endl;
+        throw;
+        return 1;
+    }
+
+    /** Get statistics about the table
+     * @return
+     */
+    LshStats getStats() const;
+
+private:
+    /** defines the speed fo the implementation
+     * kArray uses a vector for storing data
+     * kBitsetHash uses a hash map but checks for the validity of a key with a bitset
+     * kHash uses a hash map only
+     */
+    enum SpeedLevel
+    {
+        kArray, kBitsetHash, kHash
+    };
+
+    /** Initialize some variables
+     */
+    void initialize(size_t key_size)
+    {
+        speed_level_ = kHash;
+        key_size_ = key_size;
+    }
+
+    /** Optimize the table for speed/space
+     */
+    void optimize()
+    {
+        // If we are already using the fast storage, no need to do anything
+        if (speed_level_ == kArray) return;
+
+        // Use an array if it will be more than half full
+        if (buckets_space_.size() > ((size_t(1) << key_size_) / 2)) {
+            speed_level_ = kArray;
+            // Fill the array version of it
+            buckets_speed_.resize(size_t(1) << key_size_);
+            for (BucketsSpace::const_iterator key_bucket = buckets_space_.begin(); key_bucket != buckets_space_.end(); ++key_bucket) buckets_speed_[key_bucket->first] = key_bucket->second;
+
+            // Empty the hash table
+            buckets_space_.clear();
+            return;
+        }
+
+        // If the bitset is going to use less than 10% of the RAM of the hash map (at least 1 size_t for the key and two
+        // for the vector) or less than 512MB (key_size_ <= 30)
+        if (((std::max(buckets_space_.size(), buckets_speed_.size()) * CHAR_BIT * 3 * sizeof(BucketKey)) / 10
+             >= size_t(size_t(1) << key_size_)) || (key_size_ <= 32)) {
+            speed_level_ = kBitsetHash;
+            key_bitset_.resize(size_t(1) << key_size_);
+            key_bitset_.reset();
+            // Try with the BucketsSpace
+            for (BucketsSpace::const_iterator key_bucket = buckets_space_.begin(); key_bucket != buckets_space_.end(); ++key_bucket) key_bitset_.set(key_bucket->first);
+        }
+        else {
+            speed_level_ = kHash;
+            key_bitset_.clear();
+        }
+    }
+
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+    	int val;
+    	if (Archive::is_saving::value) {
+    		val = (int)speed_level_;
+    	}
+    	ar & val;
+    	if (Archive::is_loading::value) {
+    		speed_level_ = (SpeedLevel) val;
+    	}
+
+    	ar & key_size_;
+    	ar & mask_;
+
+    	if (speed_level_==kArray) {
+    		ar & buckets_speed_;
+    	}
+    	if (speed_level_==kBitsetHash || speed_level_==kHash) {
+    		ar & buckets_space_;
+    	}
+		if (speed_level_==kBitsetHash) {
+			ar & key_bitset_;
+		}
+    }
+    friend struct serialization::access;
+
+    /** The vector of all the buckets if they are held for speed
+     */
+    BucketsSpeed buckets_speed_;
+
+    /** The hash table of all the buckets in case we cannot use the speed version
+     */
+    BucketsSpace buckets_space_;
+
+    /** What is used to store the data */
+    SpeedLevel speed_level_;
+
+    /** If the subkey is small enough, it will keep track of which subkeys are set through that bitset
+     * That is just a speedup so that we don't look in the hash table (which can be mush slower that checking a bitset)
+     */
+    DynamicBitset key_bitset_;
+
+    /** The size of the sub-signature in bits
+     */
+    unsigned int key_size_;
+
+    // Members only used for the unsigned char specialization
+    /** The mask to apply to a feature to get the hash key
+     * Only used in the unsigned char case
+     */
+    std::vector<size_t> mask_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Specialization for unsigned char
+
+template<>
+inline LshTable<unsigned char>::LshTable(unsigned int feature_size, unsigned int subsignature_size)
+{
+    initialize(subsignature_size);
+    // Allocate the mask
+    mask_ = std::vector<size_t>((size_t)ceil((float)(feature_size * sizeof(char)) / (float)sizeof(size_t)), 0);
+
+    // A bit brutal but fast to code
+    std::vector<size_t> indices(feature_size * CHAR_BIT);
+    for (size_t i = 0; i < feature_size * CHAR_BIT; ++i) indices[i] = i;
+    std::random_shuffle(indices.begin(), indices.end());
+
+    // Generate a random set of order of subsignature_size_ bits
+    for (unsigned int i = 0; i < key_size_; ++i) {
+        size_t index = indices[i];
+
+        // Set that bit in the mask
+        size_t divisor = CHAR_BIT * sizeof(size_t);
+        size_t idx = index / divisor; //pick the right size_t index
+        mask_[idx] |= size_t(1) << (index % divisor); //use modulo to find the bit offset
+    }
+
+    // Set to 1 if you want to display the mask for debug
+#if 0
+    {
+        size_t bcount = 0;
+        BOOST_FOREACH(size_t mask_block, mask_){
+            out << std::setw(sizeof(size_t) * CHAR_BIT / 4) << std::setfill('0') << std::hex << mask_block
+                << std::endl;
+            bcount += __builtin_popcountll(mask_block);
+        }
+        out << "bit count : " << std::dec << bcount << std::endl;
+        out << "mask size : " << mask_.size() << std::endl;
+        return out;
+    }
+#endif
+}
+
+/** Return the Subsignature of a feature
+ * @param feature the feature to analyze
+ */
+template<>
+inline size_t LshTable<unsigned char>::getKey(const unsigned char* feature) const
+{
+    // no need to check if T is dividable by sizeof(size_t) like in the Hamming
+    // distance computation as we have a mask
+    const size_t* feature_block_ptr = reinterpret_cast<const size_t*> (feature);
+
+    // Figure out the subsignature of the feature
+    // Given the feature ABCDEF, and the mask 001011, the output will be
+    // 000CEF
+    size_t subsignature = 0;
+    size_t bit_index = 1;
+
+    for (std::vector<size_t>::const_iterator pmask_block = mask_.begin(); pmask_block != mask_.end(); ++pmask_block) {
+        // get the mask and signature blocks
+        size_t feature_block = *feature_block_ptr;
+        size_t mask_block = *pmask_block;
+        while (mask_block) {
+            // Get the lowest set bit in the mask block
+            size_t lowest_bit = mask_block & (-(ptrdiff_t)mask_block);
+            // Add it to the current subsignature if necessary
+            subsignature += (feature_block & lowest_bit) ? bit_index : 0;
+            // Reset the bit in the mask block
+            mask_block ^= lowest_bit;
+            // increment the bit index for the subsignature
+            bit_index <<= 1;
+        }
+        // Check the next feature block
+        ++feature_block_ptr;
+    }
+    return subsignature;
+}
+
+template<>
+inline LshStats LshTable<unsigned char>::getStats() const
+{
+    LshStats stats;
+    stats.bucket_size_mean_ = 0;
+    if ((buckets_speed_.empty()) && (buckets_space_.empty())) {
+        stats.n_buckets_ = 0;
+        stats.bucket_size_median_ = 0;
+        stats.bucket_size_min_ = 0;
+        stats.bucket_size_max_ = 0;
+        return stats;
+    }
+
+    if (!buckets_speed_.empty()) {
+        for (BucketsSpeed::const_iterator pbucket = buckets_speed_.begin(); pbucket != buckets_speed_.end(); ++pbucket) {
+            stats.bucket_sizes_.push_back(pbucket->size());
+            stats.bucket_size_mean_ += pbucket->size();
+        }
+        stats.bucket_size_mean_ /= buckets_speed_.size();
+        stats.n_buckets_ = buckets_speed_.size();
+    }
+    else {
+        for (BucketsSpace::const_iterator x = buckets_space_.begin(); x != buckets_space_.end(); ++x) {
+            stats.bucket_sizes_.push_back(x->second.size());
+            stats.bucket_size_mean_ += x->second.size();
+        }
+        stats.bucket_size_mean_ /= buckets_space_.size();
+        stats.n_buckets_ = buckets_space_.size();
+    }
+
+    std::sort(stats.bucket_sizes_.begin(), stats.bucket_sizes_.end());
+
+    //  BOOST_FOREACH(int size, stats.bucket_sizes_)
+    //          std::cout << size << " ";
+    //  std::cout << std::endl;
+    stats.bucket_size_median_ = stats.bucket_sizes_[stats.bucket_sizes_.size() / 2];
+    stats.bucket_size_min_ = stats.bucket_sizes_.front();
+    stats.bucket_size_max_ = stats.bucket_sizes_.back();
+
+    // TODO compute mean and std
+    /*float mean, stddev;
+       stats.bucket_size_mean_ = mean;
+       stats.bucket_size_std_dev = stddev;*/
+
+    // Include a histogram of the buckets
+    unsigned int bin_start = 0;
+    unsigned int bin_end = 20;
+    bool is_new_bin = true;
+    for (std::vector<unsigned int>::iterator iterator = stats.bucket_sizes_.begin(), end = stats.bucket_sizes_.end(); iterator
+         != end; )
+        if (*iterator < bin_end) {
+            if (is_new_bin) {
+                stats.size_histogram_.push_back(std::vector<unsigned int>(3, 0));
+                stats.size_histogram_.back()[0] = bin_start;
+                stats.size_histogram_.back()[1] = bin_end - 1;
+                is_new_bin = false;
+            }
+            ++stats.size_histogram_.back()[2];
+            ++iterator;
+        }
+        else {
+            bin_start += 20;
+            bin_end += 20;
+            is_new_bin = true;
+        }
+
+    return stats;
+}
+
+// End the two namespaces
+}
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#endif /* FLANN_LSH_TABLE_H_ */

corelib/src/rtflann/util/matrix.h

@@ -0,0 +1,135 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_DATASET_H_
+#define RTABMAP_FLANN_DATASET_H_
+
+#include "rtflann/general.h"
+#include "rtflann/util/serialization.h"
+#include <stdio.h>
+
+namespace rtflann
+{
+
+typedef unsigned char uchar;
+
+class Matrix_
+{
+public:
+
+	Matrix_() : rows(0), cols(0), stride(0), type(FLANN_NONE), data(NULL)
+	{
+	};
+
+    Matrix_(void* data_, size_t rows_, size_t cols_, flann_datatype_t type_, size_t stride_ = 0) :
+        rows(rows_), cols(cols_), stride(stride_), type(type_)
+    {
+    	data = static_cast<uchar*>(data_);
+
+    	if (stride==0) stride = flann_datatype_size(type)*cols;
+    }
+
+    /**
+     * Operator that returns a (pointer to a) row of the data.
+     */
+    inline void* operator[](size_t index) const
+    {
+        return data+index*stride;
+    }
+
+    void* ptr() const
+    {
+        return data;
+    }
+
+    size_t rows;
+    size_t cols;
+    size_t stride;
+    flann_datatype_t type;
+protected:
+    uchar* data;
+
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+    	ar & rows;
+    	ar & cols;
+    	ar & stride;
+    	ar & type;
+    	if (Archive::is_loading::value) {
+    		data = new uchar[rows*stride];
+    	}
+    	ar & serialization::make_binary_object(data, rows*stride);
+    }
+    friend struct serialization::access;
+};
+
+
+/**
+ * Class that implements a simple rectangular matrix stored in a memory buffer and
+ * provides convenient matrix-like access using the [] operators.
+ *
+ * This class has the same memory structure as the un-templated class flann::Matrix_ and
+ * it's directly convertible from it.
+ */
+template <typename T>
+class Matrix : public Matrix_
+{
+public:
+    typedef T type;
+
+    Matrix() : Matrix_()
+    {
+    }
+
+    Matrix(T* data_, size_t rows_, size_t cols_, size_t stride_ = 0) :
+    	Matrix_(data_, rows_, cols_, flann_datatype_value<T>::value, stride_)
+    {
+    	if (stride==0) stride = sizeof(T)*cols;
+    }
+
+    /**
+     * Operator that returns a (pointer to a) row of the data.
+     */
+    inline T* operator[](size_t index) const
+    {
+    	return reinterpret_cast<T*>(data+index*stride);
+    }
+
+
+    T* ptr() const
+    {
+    	return reinterpret_cast<T*>(data);
+    }
+};
+
+}
+
+#endif //FLANN_DATASET_H_

corelib/src/rtflann/util/object_factory.h

@@ -0,0 +1,91 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_OBJECT_FACTORY_H_
+#define RTABMAP_FLANN_OBJECT_FACTORY_H_
+
+#include <map>
+
+namespace rtflann
+{
+
+class CreatorNotFound
+{
+};
+
+template<typename BaseClass,
+         typename UniqueIdType,
+         typename ObjectCreator = BaseClass* (*)()>
+class ObjectFactory
+{
+    typedef ObjectFactory<BaseClass,UniqueIdType,ObjectCreator> ThisClass;
+    typedef std::map<UniqueIdType, ObjectCreator> ObjectRegistry;
+
+    // singleton class, private constructor
+    ObjectFactory() {}
+
+public:
+
+    bool subscribe(UniqueIdType id, ObjectCreator creator)
+    {
+        if (object_registry.find(id) != object_registry.end()) return false;
+
+        object_registry[id] = creator;
+        return true;
+    }
+
+    bool unregister(UniqueIdType id)
+    {
+        return object_registry.erase(id) == 1;
+    }
+
+    ObjectCreator create(UniqueIdType id)
+    {
+        typename ObjectRegistry::const_iterator iter = object_registry.find(id);
+
+        if (iter == object_registry.end()) {
+            throw CreatorNotFound();
+        }
+
+        return iter->second;
+    }
+
+    static ThisClass& instance()
+    {
+        static ThisClass the_factory;
+        return the_factory;
+    }
+private:
+    ObjectRegistry object_registry;
+};
+
+}
+
+#endif /* FLANN_OBJECT_FACTORY_H_ */

corelib/src/rtflann/util/params.h

@@ -0,0 +1,139 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2011  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2011  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_PARAMS_H_
+#define RTABMAP_FLANN_PARAMS_H_
+
+#include "rtflann/util/any.h"
+#include "rtflann/general.h"
+#include <iostream>
+#include <map>
+
+
+namespace rtflann
+{
+
+namespace anyimpl
+{
+SMALL_POLICY(flann_algorithm_t);
+SMALL_POLICY(flann_centers_init_t);
+SMALL_POLICY(flann_log_level_t);
+SMALL_POLICY(flann_datatype_t);
+}
+
+
+typedef std::map<std::string, any> IndexParams;
+
+
+typedef enum {
+	FLANN_False = 0,
+	FLANN_True = 1,
+	FLANN_Undefined
+} tri_type;
+
+
+struct SearchParams
+{
+    SearchParams(int checks_ = 32, float eps_ = 0.0, bool sorted_ = true ) :
+    	checks(checks_), eps(eps_), sorted(sorted_)
+    {
+    	max_neighbors = -1;
+    	use_heap = FLANN_Undefined;
+    	cores = 1;
+    	matrices_in_gpu_ram = false;
+    }
+
+    // how many leafs to visit when searching for neighbours (-1 for unlimited)
+    int checks;
+    // search for eps-approximate neighbours (default: 0)
+    float eps;
+    // only for radius search, require neighbours sorted by distance (default: true)
+    bool sorted;
+    // maximum number of neighbors radius search should return (-1 for unlimited)
+    int max_neighbors;
+    // use a heap to manage the result set (default: FLANN_Undefined)
+    tri_type use_heap;
+    // how many cores to assign to the search (used only if compiled with OpenMP capable compiler) (0 for auto)
+    int cores;
+    // for GPU search indicates if matrices are already in GPU ram
+    bool matrices_in_gpu_ram;
+};
+
+
+inline bool has_param(const IndexParams& params, std::string name)
+{
+	return params.find(name)!=params.end();
+}
+
+template<typename T>
+T get_param(const IndexParams& params, std::string name, const T& default_value)
+{
+    IndexParams::const_iterator it = params.find(name);
+    if (it != params.end()) {
+        return it->second.cast<T>();
+    }
+    else {
+        return default_value;
+    }
+}
+
+template<typename T>
+T get_param(const IndexParams& params, std::string name)
+{
+    IndexParams::const_iterator it = params.find(name);
+    if (it != params.end()) {
+        return it->second.cast<T>();
+    }
+    else {
+        throw FLANNException(std::string("Missing parameter '")+name+std::string("' in the parameters given"));
+    }
+}
+
+inline void print_params(const IndexParams& params)
+{
+    IndexParams::const_iterator it;
+
+    for(it=params.begin(); it!=params.end(); ++it) {
+        std::cout << it->first << " : " << it->second << std::endl;
+    }
+}
+
+inline void print_params(const SearchParams& params)
+{
+	std::cout << "checks : " << params.checks << std::endl;
+	std::cout << "eps : " << params.eps << std::endl;
+	std::cout << "sorted : " << params.sorted << std::endl;
+	std::cout << "max_neighbors : " << params.max_neighbors << std::endl;
+}
+
+
+}
+
+
+#endif /* FLANN_PARAMS_H_ */

corelib/src/rtflann/util/random.h

@@ -0,0 +1,145 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_RANDOM_H
+#define RTABMAP_FLANN_RANDOM_H
+
+#include <algorithm>
+#include <cstdlib>
+#include <cstddef>
+#include <vector>
+
+#include "rtflann/general.h"
+
+namespace rtflann
+{
+
+/**
+ * Seeds the random number generator
+ *  @param seed Random seed
+ */
+inline void seed_random(unsigned int seed)
+{
+    srand(seed);
+}
+
+/*
+ * Generates a random double value.
+ */
+/**
+ * Generates a random double value.
+ * @param high Upper limit
+ * @param low Lower limit
+ * @return Random double value
+ */
+inline double rand_double(double high = 1.0, double low = 0)
+{
+    return low + ((high-low) * (std::rand() / (RAND_MAX + 1.0)));
+}
+
+/**
+ * Generates a random integer value.
+ * @param high Upper limit
+ * @param low Lower limit
+ * @return Random integer value
+ */
+inline int rand_int(int high = RAND_MAX, int low = 0)
+{
+    return low + (int) ( double(high-low) * (std::rand() / (RAND_MAX + 1.0)));
+}
+
+
+class RandomGenerator
+{
+public:
+    ptrdiff_t operator() (ptrdiff_t i) { return rand_int(i); }
+};
+
+
+/**
+ * Random number generator that returns a distinct number from
+ * the [0,n) interval each time.
+ */
+class UniqueRandom
+{
+    std::vector<int> vals_;
+    int size_;
+    int counter_;
+
+public:
+    /**
+     * Constructor.
+     * @param n Size of the interval from which to generate
+     * @return
+     */
+    UniqueRandom(int n)
+    {
+        init(n);
+    }
+
+    /**
+     * Initializes the number generator.
+     * @param n the size of the interval from which to generate random numbers.
+     */
+    void init(int n)
+    {
+        static RandomGenerator generator;
+        // create and initialize an array of size n
+        vals_.resize(n);
+        size_ = n;
+        for (int i = 0; i < size_; ++i) vals_[i] = i;
+
+        // shuffle the elements in the array
+        std::random_shuffle(vals_.begin(), vals_.end(), generator);
+
+        counter_ = 0;
+    }
+
+    /**
+     * Return a distinct random integer in greater or equal to 0 and less
+     * than 'n' on each call. It should be called maximum 'n' times.
+     * Returns: a random integer
+     */
+    int next()
+    {
+        if (counter_ == size_) {
+            return -1;
+        }
+        else {
+            return vals_[counter_++];
+        }
+    }
+};
+
+}
+
+#endif //FLANN_RANDOM_H
+
+

corelib/src/rtflann/util/result_set.h

@@ -0,0 +1,934 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_RESULTSET_H
+#define RTABMAP_FLANN_RESULTSET_H
+
+#include <algorithm>
+#include <cstring>
+#include <iostream>
+#include <limits>
+#include <set>
+#include <vector>
+
+namespace rtflann
+{
+
+/* This record represents a branch point when finding neighbors in
+    the tree.  It contains a record of the minimum distance to the query
+    point, as well as the node at which the search resumes.
+ */
+
+template <typename T, typename DistanceType>
+struct BranchStruct
+{
+    T node;           /* Tree node at which search resumes */
+    DistanceType mindist;     /* Minimum distance to query for all nodes below. */
+
+    BranchStruct() {}
+    BranchStruct(const T& aNode, DistanceType dist) : node(aNode), mindist(dist) {}
+
+    bool operator<(const BranchStruct<T, DistanceType>& rhs) const
+    {
+        return mindist<rhs.mindist;
+    }
+};
+
+
+template <typename DistanceType>
+struct DistanceIndex
+{
+    DistanceIndex(DistanceType dist, size_t index) :
+        dist_(dist), index_(index)
+    {
+    }
+    bool operator<(const DistanceIndex& dist_index) const
+    {
+        return (dist_ < dist_index.dist_) || ((dist_ == dist_index.dist_) && index_ < dist_index.index_);
+    }
+    DistanceType dist_;
+    size_t index_;
+};
+
+
+template <typename DistanceType>
+class ResultSet
+{
+public:
+    virtual ~ResultSet() {}
+
+    virtual bool full() const = 0;
+
+    virtual void addPoint(DistanceType dist, size_t index) = 0;
+
+    virtual DistanceType worstDist() const = 0;
+
+};
+
+/**
+ * KNNSimpleResultSet does not ensure that the element it holds are unique.
+ * Is used in those cases where the nearest neighbour algorithm used does not
+ * attempt to insert the same element multiple times.
+ */
+template <typename DistanceType>
+class KNNSimpleResultSet : public ResultSet<DistanceType>
+{
+public:
+	typedef DistanceIndex<DistanceType> DistIndex;
+
+	KNNSimpleResultSet(size_t capacity_) :
+        capacity_(capacity_)
+    {
+		// reserving capacity to prevent memory re-allocations
+		dist_index_.resize(capacity_, DistIndex(std::numeric_limits<DistanceType>::max(),-1));
+    	clear();
+    }
+
+    ~KNNSimpleResultSet()
+    {
+    }
+
+    /**
+     * Clears the result set
+     */
+    void clear()
+    {
+        worst_distance_ = std::numeric_limits<DistanceType>::max();
+        dist_index_[capacity_-1].dist_ = worst_distance_;
+        count_ = 0;
+    }
+
+    /**
+     *
+     * @return Number of elements in the result set
+     */
+    size_t size() const
+    {
+        return count_;
+    }
+
+    /**
+     * Radius search result set always reports full
+     * @return
+     */
+    bool full() const
+    {
+        return count_==capacity_;
+    }
+
+    /**
+     * Add a point to result set
+     * @param dist distance to point
+     * @param index index of point
+     */
+    void addPoint(DistanceType dist, size_t index)
+    {
+    	if (dist>=worst_distance_) return;
+
+        if (count_ < capacity_) ++count_;
+        size_t i;
+        for (i=count_-1; i>0; --i) {
+#ifdef FLANN_FIRST_MATCH
+            if ( (dist_index_[i-1].dist_>dist) || ((dist==dist_index_[i-1].dist_)&&(dist_index_[i-1].index_>index)) )
+#else
+            if (dist_index_[i-1].dist_>dist)
+#endif
+            {
+            	dist_index_[i] = dist_index_[i-1];
+            }
+            else break;
+        }
+        dist_index_[i].dist_ = dist;
+        dist_index_[i].index_ = index;
+        worst_distance_ = dist_index_[capacity_-1].dist_;
+    }
+
+    /**
+     * Copy indices and distances to output buffers
+     * @param indices
+     * @param dists
+     * @param num_elements Number of elements to copy
+     * @param sorted Indicates if results should be sorted
+     */
+    void copy(size_t* indices, DistanceType* dists, size_t num_elements, bool sorted = true)
+    {
+    	size_t n = std::min(count_, num_elements);
+    	for (size_t i=0; i<n; ++i) {
+    		*indices++ = dist_index_[i].index_;
+    		*dists++ = dist_index_[i].dist_;
+    	}
+    }
+
+    DistanceType worstDist() const
+    {
+    	return worst_distance_;
+    }
+
+private:
+    size_t capacity_;
+    size_t count_;
+    DistanceType worst_distance_;
+    std::vector<DistIndex> dist_index_;
+};
+
+/**
+ * K-Nearest neighbour result set. Ensures that the elements inserted are unique
+ */
+template <typename DistanceType>
+class KNNResultSet : public ResultSet<DistanceType>
+{
+public:
+	typedef DistanceIndex<DistanceType> DistIndex;
+
+    KNNResultSet(int capacity) : capacity_(capacity)
+    {
+		// reserving capacity to prevent memory re-allocations
+		dist_index_.resize(capacity_, DistIndex(std::numeric_limits<DistanceType>::max(),-1));
+    	clear();
+    }
+
+    ~KNNResultSet()
+    {
+
+    }
+
+    /**
+     * Clears the result set
+     */
+    void clear()
+    {
+        worst_distance_ = std::numeric_limits<DistanceType>::max();
+        dist_index_[capacity_-1].dist_ = worst_distance_;
+        count_ = 0;
+    }
+
+    size_t size() const
+    {
+        return count_;
+    }
+
+    bool full() const
+    {
+        return count_ == capacity_;
+    }
+
+
+    void addPoint(DistanceType dist, size_t index)
+    {
+        if (dist >= worst_distance_) return;
+        size_t i;
+        for (i = count_; i > 0; --i) {
+#ifdef FLANN_FIRST_MATCH
+            if ( (dist_index_[i-1].dist_<=dist) && ((dist!=dist_index_[i-1].dist_)||(dist_index_[i-1].index_<=index)) )
+#else
+            if (dist_index_[i-1].dist_<=dist)
+#endif
+            {
+                // Check for duplicate indices
+                for (size_t j = i - 1; dist_index_[j].dist_ == dist && j--;) {
+                    if (dist_index_[j].index_ == index) {
+                        return;
+                    }
+                }
+                break;
+            }
+        }
+        if (count_ < capacity_) ++count_;
+        for (size_t j = count_-1; j > i; --j) {
+            dist_index_[j] = dist_index_[j-1];
+        }
+        dist_index_[i].dist_ = dist;
+        dist_index_[i].index_ = index;
+        worst_distance_ = dist_index_[capacity_-1].dist_;
+    }
+
+    /**
+     * Copy indices and distances to output buffers
+     * @param indices
+     * @param dists
+     * @param num_elements Number of elements to copy
+     * @param sorted Indicates if results should be sorted
+     */
+    void copy(size_t* indices, DistanceType* dists, size_t num_elements, bool sorted = true)
+    {
+    	size_t n = std::min(count_, num_elements);
+    	for (size_t i=0; i<n; ++i) {
+    		*indices++ = dist_index_[i].index_;
+    		*dists++ = dist_index_[i].dist_;
+    	}
+    }
+
+    DistanceType worstDist() const
+    {
+        return worst_distance_;
+    }
+
+private:
+    size_t capacity_;
+    size_t count_;
+    DistanceType worst_distance_;
+    std::vector<DistIndex> dist_index_;
+
+};
+
+
+
+template <typename DistanceType>
+class KNNResultSet2 : public ResultSet<DistanceType>
+{
+public:
+	typedef DistanceIndex<DistanceType> DistIndex;
+
+	KNNResultSet2(size_t capacity_) :
+        capacity_(capacity_)
+    {
+		// reserving capacity to prevent memory re-allocations
+		dist_index_.reserve(capacity_);
+    	clear();
+    }
+
+    ~KNNResultSet2()
+    {
+    }
+
+    /**
+     * Clears the result set
+     */
+    void clear()
+    {
+        dist_index_.clear();
+        worst_dist_ = std::numeric_limits<DistanceType>::max();
+        is_full_ = false;
+    }
+
+    /**
+     *
+     * @return Number of elements in the result set
+     */
+    size_t size() const
+    {
+        return dist_index_.size();
+    }
+
+    /**
+     * Radius search result set always reports full
+     * @return
+     */
+    bool full() const
+    {
+        return is_full_;
+    }
+
+    /**
+     * Add another point to result set
+     * @param dist distance to point
+     * @param index index of point
+     * Pre-conditions: capacity_>0
+     */
+    void addPoint(DistanceType dist, size_t index)
+    {
+    	if (dist>=worst_dist_) return;
+
+    	if (dist_index_.size()==capacity_) {
+    		// if result set if filled to capacity, remove farthest element
+    		std::pop_heap(dist_index_.begin(), dist_index_.end());
+        	dist_index_.pop_back();
+    	}
+
+    	// add new element
+    	dist_index_.push_back(DistIndex(dist,index));
+    	if (is_full_) { // when is_full_==true, we have a heap
+    		std::push_heap(dist_index_.begin(), dist_index_.end());
+    	}
+
+    	if (dist_index_.size()==capacity_) {
+    		if (!is_full_) {
+    			std::make_heap(dist_index_.begin(), dist_index_.end());
+            	is_full_ = true;
+    		}
+    		// we replaced the farthest element, update worst distance
+        	worst_dist_ = dist_index_[0].dist_;
+        }
+    }
+
+    /**
+     * Copy indices and distances to output buffers
+     * @param indices
+     * @param dists
+     * @param num_elements Number of elements to copy
+     * @param sorted Indicates if results should be sorted
+     */
+    void copy(size_t* indices, DistanceType* dists, size_t num_elements, bool sorted = true)
+    {
+    	if (sorted) {
+    		// std::sort_heap(dist_index_.begin(), dist_index_.end());
+    		// sort seems faster here, even though dist_index_ is a heap
+    		std::sort(dist_index_.begin(), dist_index_.end());
+    	}
+    	else {
+    		if (num_elements<size()) {
+    			std::nth_element(dist_index_.begin(), dist_index_.begin()+num_elements, dist_index_.end());
+    		}
+    	}
+
+    	size_t n = std::min(dist_index_.size(), num_elements);
+    	for (size_t i=0; i<n; ++i) {
+    		*indices++ = dist_index_[i].index_;
+    		*dists++ = dist_index_[i].dist_;
+    	}
+    }
+
+    DistanceType worstDist() const
+    {
+    	return worst_dist_;
+    }
+
+private:
+    size_t capacity_;
+    DistanceType worst_dist_;
+    std::vector<DistIndex> dist_index_;
+    bool is_full_;
+};
+
+
+/**
+ * Unbounded radius result set. It will hold as many elements as
+ * are added to it.
+ */
+template <typename DistanceType>
+class RadiusResultSet : public ResultSet<DistanceType>
+{
+public:
+	typedef DistanceIndex<DistanceType> DistIndex;
+
+	RadiusResultSet(DistanceType radius_) :
+        radius_(radius_)
+    {
+		// reserving some memory to limit number of re-allocations
+		dist_index_.reserve(1024);
+    	clear();
+    }
+
+    ~RadiusResultSet()
+    {
+    }
+
+    /**
+     * Clears the result set
+     */
+    void clear()
+    {
+        dist_index_.clear();
+    }
+
+    /**
+     *
+     * @return Number of elements in the result set
+     */
+    size_t size() const
+    {
+        return dist_index_.size();
+    }
+
+    /**
+     * Radius search result set always reports full
+     * @return
+     */
+    bool full() const
+    {
+        return true;
+    }
+
+    /**
+     * Add another point to result set
+     * @param dist distance to point
+     * @param index index of point
+     * Pre-conditions: capacity_>0
+     */
+    void addPoint(DistanceType dist, size_t index)
+    {
+    	if (dist<radius_) {
+    		// add new element
+    		dist_index_.push_back(DistIndex(dist,index));
+    	}
+    }
+
+    /**
+     * Copy indices and distances to output buffers
+     * @param indices
+     * @param dists
+     * @param num_elements Number of elements to copy
+     * @param sorted Indicates if results should be sorted
+     */
+    void copy(size_t* indices, DistanceType* dists, size_t num_elements, bool sorted = true)
+    {
+    	if (sorted) {
+    		// std::sort_heap(dist_index_.begin(), dist_index_.end());
+    		// sort seems faster here, even though dist_index_ is a heap
+    		std::sort(dist_index_.begin(), dist_index_.end());
+    	}
+    	else {
+    		if (num_elements<size()) {
+    			std::nth_element(dist_index_.begin(), dist_index_.begin()+num_elements, dist_index_.end());
+    		}
+    	}
+
+    	size_t n = std::min(dist_index_.size(), num_elements);
+    	for (size_t i=0; i<n; ++i) {
+    		*indices++ = dist_index_[i].index_;
+    		*dists++ = dist_index_[i].dist_;
+    	}
+    }
+
+    DistanceType worstDist() const
+    {
+    	return radius_;
+    }
+
+private:
+    DistanceType radius_;
+    std::vector<DistIndex> dist_index_;
+};
+
+
+
+/**
+ * Bounded radius result set. It limits the number of elements
+ * it can hold to a preset capacity.
+ */
+template <typename DistanceType>
+class KNNRadiusResultSet : public ResultSet<DistanceType>
+{
+public:
+	typedef DistanceIndex<DistanceType> DistIndex;
+
+	KNNRadiusResultSet(DistanceType radius_, size_t capacity_) :
+        radius_(radius_), capacity_(capacity_)
+    {
+		// reserving capacity to prevent memory re-allocations
+		dist_index_.reserve(capacity_);
+    	clear();
+    }
+
+    ~KNNRadiusResultSet()
+    {
+    }
+
+    /**
+     * Clears the result set
+     */
+    void clear()
+    {
+        dist_index_.clear();
+        worst_dist_ = radius_;
+        is_heap_ = false;
+    }
+
+    /**
+     *
+     * @return Number of elements in the result set
+     */
+    size_t size() const
+    {
+        return dist_index_.size();
+    }
+
+    /**
+     * Radius search result set always reports full
+     * @return
+     */
+    bool full() const
+    {
+        return true;
+    }
+
+    /**
+     * Add another point to result set
+     * @param dist distance to point
+     * @param index index of point
+     * Pre-conditions: capacity_>0
+     */
+    void addPoint(DistanceType dist, size_t index)
+    {
+    	if (dist>=worst_dist_) return;
+
+    	if (dist_index_.size()==capacity_) {
+    		// if result set is filled to capacity, remove farthest element
+    		std::pop_heap(dist_index_.begin(), dist_index_.end());
+        	dist_index_.pop_back();
+    	}
+
+    	// add new element
+    	dist_index_.push_back(DistIndex(dist,index));
+    	if (is_heap_) {
+    		std::push_heap(dist_index_.begin(), dist_index_.end());
+    	}
+
+    	if (dist_index_.size()==capacity_) {
+    		// when got to full capacity, make it a heap
+    		if (!is_heap_) {
+    			std::make_heap(dist_index_.begin(), dist_index_.end());
+    			is_heap_ = true;
+    		}
+    		// we replaced the farthest element, update worst distance
+        	worst_dist_ = dist_index_[0].dist_;
+        }
+    }
+
+    /**
+     * Copy indices and distances to output buffers
+     * @param indices
+     * @param dists
+     * @param num_elements Number of elements to copy
+     * @param sorted Indicates if results should be sorted
+     */
+    void copy(size_t* indices, DistanceType* dists, size_t num_elements, bool sorted = true)
+    {
+    	if (sorted) {
+    		// std::sort_heap(dist_index_.begin(), dist_index_.end());
+    		// sort seems faster here, even though dist_index_ is a heap
+    		std::sort(dist_index_.begin(), dist_index_.end());
+    	}
+    	else {
+    		if (num_elements<size()) {
+    			std::nth_element(dist_index_.begin(), dist_index_.begin()+num_elements, dist_index_.end());
+    		}
+    	}
+
+    	size_t n = std::min(dist_index_.size(), num_elements);
+    	for (size_t i=0; i<n; ++i) {
+    		*indices++ = dist_index_[i].index_;
+    		*dists++ = dist_index_[i].dist_;
+    	}
+    }
+
+    DistanceType worstDist() const
+    {
+    	return worst_dist_;
+    }
+
+private:
+    bool is_heap_;
+    DistanceType radius_;
+    size_t capacity_;
+    DistanceType worst_dist_;
+    std::vector<DistIndex> dist_index_;
+};
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ * This is a result set that only counts the neighbors within a radius.
+ */
+
+template <typename DistanceType>
+class CountRadiusResultSet : public ResultSet<DistanceType>
+{
+    DistanceType radius;
+    size_t count;
+
+public:
+    CountRadiusResultSet(DistanceType radius_ ) :
+        radius(radius_)
+    {
+        clear();
+    }
+
+    ~CountRadiusResultSet()
+    {
+    }
+
+    void clear()
+    {
+        count = 0;
+    }
+
+    size_t size() const
+    {
+        return count;
+    }
+
+    bool full() const
+    {
+        return true;
+    }
+
+    void addPoint(DistanceType dist, size_t index)
+    {
+        if (dist<radius) {
+            count++;
+        }
+    }
+
+    DistanceType worstDist() const
+    {
+        return radius;
+    }
+
+};
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** Class that holds the k NN neighbors
+ */
+template<typename DistanceType>
+class UniqueResultSet : public ResultSet<DistanceType>
+{
+public:
+    struct DistIndex
+    {
+        DistIndex(DistanceType dist, unsigned int index) :
+            dist_(dist), index_(index)
+        {
+        }
+        bool operator<(const DistIndex dist_index) const
+        {
+            return (dist_ < dist_index.dist_) || ((dist_ == dist_index.dist_) && index_ < dist_index.index_);
+        }
+        DistanceType dist_;
+        unsigned int index_;
+    };
+
+    /** Default cosntructor */
+    UniqueResultSet() :
+        worst_distance_(std::numeric_limits<DistanceType>::max())
+    {
+    }
+
+    /** Check the status of the set
+     * @return true if we have k NN
+     */
+    inline bool full() const
+    {
+        return is_full_;
+    }
+
+    /** Copy the set to two C arrays
+     * @param indices pointer to a C array of indices
+     * @param dist pointer to a C array of distances
+     * @param n_neighbors the number of neighbors to copy
+     */
+    void copy(size_t* indices, DistanceType* dist, int n_neighbors, bool sorted = true)
+    {
+    	if (n_neighbors<0) n_neighbors = dist_indices_.size();
+    	int i = 0;
+    	typedef typename std::set<DistIndex>::const_iterator Iterator;
+    	for (Iterator dist_index = dist_indices_.begin(), dist_index_end =
+    			dist_indices_.end(); (dist_index != dist_index_end) && (i < n_neighbors); ++dist_index, ++indices, ++dist, ++i) {
+    		*indices = dist_index->index_;
+    		*dist = dist_index->dist_;
+    	}
+    }
+
+    /** The number of neighbors in the set
+     * @return
+     */
+    size_t size() const
+    {
+        return dist_indices_.size();
+    }
+
+    /** The distance of the furthest neighbor
+     * If we don't have enough neighbors, it returns the max possible value
+     * @return
+     */
+    inline DistanceType worstDist() const
+    {
+        return worst_distance_;
+    }
+protected:
+    /** Flag to say if the set is full */
+    bool is_full_;
+
+    /** The worst distance found so far */
+    DistanceType worst_distance_;
+
+    /** The best candidates so far */
+    std::set<DistIndex> dist_indices_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** Class that holds the k NN neighbors
+ * Faster than KNNResultSet as it uses a binary heap and does not maintain two arrays
+ */
+template<typename DistanceType>
+class KNNUniqueResultSet : public UniqueResultSet<DistanceType>
+{
+public:
+    /** Constructor
+     * @param capacity the number of neighbors to store at max
+     */
+    KNNUniqueResultSet(unsigned int capacity) : capacity_(capacity)
+    {
+        this->is_full_ = false;
+        this->clear();
+    }
+
+    /** Add a possible candidate to the best neighbors
+     * @param dist distance for that neighbor
+     * @param index index of that neighbor
+     */
+    inline void addPoint(DistanceType dist, size_t index)
+    {
+        // Don't do anything if we are worse than the worst
+        if (dist >= worst_distance_) return;
+        dist_indices_.insert(DistIndex(dist, index));
+
+        if (is_full_) {
+            if (dist_indices_.size() > capacity_) {
+                dist_indices_.erase(*dist_indices_.rbegin());
+                worst_distance_ = dist_indices_.rbegin()->dist_;
+            }
+        }
+        else if (dist_indices_.size() == capacity_) {
+            is_full_ = true;
+            worst_distance_ = dist_indices_.rbegin()->dist_;
+        }
+    }
+
+    /** Remove all elements in the set
+     */
+    void clear()
+    {
+        dist_indices_.clear();
+        worst_distance_ = std::numeric_limits<DistanceType>::max();
+        is_full_ = false;
+    }
+
+protected:
+    typedef typename UniqueResultSet<DistanceType>::DistIndex DistIndex;
+    using UniqueResultSet<DistanceType>::is_full_;
+    using UniqueResultSet<DistanceType>::worst_distance_;
+    using UniqueResultSet<DistanceType>::dist_indices_;
+
+    /** The number of neighbors to keep */
+    unsigned int capacity_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** Class that holds the radius nearest neighbors
+ * It is more accurate than RadiusResult as it is not limited in the number of neighbors
+ */
+template<typename DistanceType>
+class RadiusUniqueResultSet : public UniqueResultSet<DistanceType>
+{
+public:
+    /** Constructor
+     * @param capacity the number of neighbors to store at max
+     */
+    RadiusUniqueResultSet(DistanceType radius) :
+        radius_(radius)
+    {
+        is_full_ = true;
+    }
+
+    /** Add a possible candidate to the best neighbors
+     * @param dist distance for that neighbor
+     * @param index index of that neighbor
+     */
+    void addPoint(DistanceType dist, size_t index)
+    {
+        if (dist < radius_) dist_indices_.insert(DistIndex(dist, index));
+    }
+
+    /** Remove all elements in the set
+     */
+    inline void clear()
+    {
+        dist_indices_.clear();
+    }
+
+
+    /** Check the status of the set
+     * @return alwys false
+     */
+    inline bool full() const
+    {
+        return true;
+    }
+
+    /** The distance of the furthest neighbor
+     * If we don't have enough neighbors, it returns the max possible value
+     * @return
+     */
+    inline DistanceType worstDist() const
+    {
+        return radius_;
+    }
+private:
+    typedef typename UniqueResultSet<DistanceType>::DistIndex DistIndex;
+    using UniqueResultSet<DistanceType>::dist_indices_;
+    using UniqueResultSet<DistanceType>::is_full_;
+
+    /** The furthest distance a neighbor can be */
+    DistanceType radius_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** Class that holds the k NN neighbors within a radius distance
+ */
+template<typename DistanceType>
+class KNNRadiusUniqueResultSet : public KNNUniqueResultSet<DistanceType>
+{
+public:
+    /** Constructor
+     * @param capacity the number of neighbors to store at max
+     */
+    KNNRadiusUniqueResultSet(DistanceType radius, size_t capacity) : KNNUniqueResultSet<DistanceType>(capacity)
+    {
+        this->radius_ = radius;
+        this->clear();
+    }
+
+    /** Remove all elements in the set
+     */
+    void clear()
+    {
+        dist_indices_.clear();
+        worst_distance_ = radius_;
+        is_full_ = true;
+    }
+private:
+    using KNNUniqueResultSet<DistanceType>::dist_indices_;
+    using KNNUniqueResultSet<DistanceType>::is_full_;
+    using KNNUniqueResultSet<DistanceType>::worst_distance_;
+
+    /** The maximum distance of a neighbor */
+    DistanceType radius_;
+};
+}
+
+#endif //FLANN_RESULTSET_H
+

corelib/src/rtflann/util/sampling.h

@@ -0,0 +1,72 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_SAMPLING_H_
+#define RTABMAP_FLANN_SAMPLING_H_
+
+#include "rtflann/util/matrix.h"
+#include "rtflann/util/random.h"
+
+namespace rtflann
+{
+
+template<typename T>
+Matrix<T> random_sample(Matrix<T>& srcMatrix, size_t size, bool remove = false)
+{
+	UniqueRandom rand_unique(srcMatrix.rows);
+    Matrix<T> newSet(new T[size * srcMatrix.cols], size,srcMatrix.cols);
+
+    T* src,* dest;
+    for (size_t i=0; i<size; ++i) {
+    	size_t r;
+    	if (remove) {
+            r = static_cast<size_t>(rand_int(srcMatrix.rows-i));
+    	}
+    	else {
+    		r = static_cast<size_t>(rand_unique.next());
+    	}
+        dest = newSet[i];
+        src = srcMatrix[r];
+        std::copy(src, src+srcMatrix.cols, dest);
+        if (remove) {
+            src = srcMatrix[srcMatrix.rows-i-1];
+            dest = srcMatrix[r];
+            std::copy(src, src+srcMatrix.cols, dest);
+        }
+    }
+    if (remove) {
+        srcMatrix.rows -= size;
+    }
+    return newSet;
+}
+
+} // namespace
+
+
+#endif /* FLANN_SAMPLING_H_ */

corelib/src/rtflann/util/saving.h

@@ -0,0 +1,135 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE NNIndexGOODS 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 RTABMAP_FLANN_SAVING_H_
+#define RTABMAP_FLANN_SAVING_H_
+
+#include <cstring>
+#include <vector>
+#include <stdio.h>
+
+#include "rtflann/general.h"
+#include "rtflann/util/serialization.h"
+
+
+#ifdef FLANN_SIGNATURE_
+#undef FLANN_SIGNATURE_
+#endif
+#define FLANN_SIGNATURE_ "FLANN_INDEX_v1.1"
+
+namespace rtflann
+{
+
+/**
+ * Structure representing the index header.
+ */
+struct IndexHeader
+{
+    IndexHeaderStruct h;
+
+    IndexHeader()
+	{
+        memset(h.signature, 0, sizeof(h.signature));
+        strcpy(h.signature, FLANN_SIGNATURE_);
+        memset(h.version, 0, sizeof(h.version));
+        strcpy(h.version, FLANN_VERSION_);
+
+        h.compression = 0;
+        h.first_block_size = 0;
+	}
+
+private:
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+        ar & h.signature;
+        ar & h.version;
+        ar & h.data_type;
+        ar & h.index_type;
+        ar & h.rows;
+        ar & h.cols;
+        ar & h.compression;
+        ar & h.first_block_size;
+    }
+    friend struct serialization::access;
+};
+
+/**
+ * Saves index header to stream
+ *
+ * @param stream - Stream to save to
+ * @param index - The index to save
+ */
+template<typename Index>
+void save_header(FILE* stream, const Index& index)
+{
+    IndexHeader header;
+    header.h.data_type = flann_datatype_value<typename Index::ElementType>::value;
+    header.h.index_type = index.getType();
+    header.h.rows = index.size();
+    header.h.cols = index.veclen();
+
+    fwrite(&header, sizeof(header),1,stream);
+}
+
+
+/**
+ *
+ * @param stream - Stream to load from
+ * @return Index header
+ */
+inline IndexHeader load_header(FILE* stream)
+{
+    IndexHeader header;
+    int read_size = fread(&header,sizeof(header),1,stream);
+
+    if (read_size != 1) {
+        throw FLANNException("Invalid index file, cannot read");
+    }
+
+    if (strncmp(header.h.signature,
+                FLANN_SIGNATURE_,
+                strlen(FLANN_SIGNATURE_) - strlen("v0.0")) != 0) {
+        throw FLANNException("Invalid index file, wrong signature");
+    }
+
+    return header;
+}
+
+
+namespace serialization
+{
+ENUM_SERIALIZER(flann_algorithm_t);
+ENUM_SERIALIZER(flann_centers_init_t);
+ENUM_SERIALIZER(flann_log_level_t);
+ENUM_SERIALIZER(flann_datatype_t);
+}
+
+}
+
+#endif /* FLANN_SAVING_H_ */

corelib/src/rtflann/util/serialization.h

@@ -0,0 +1,813 @@
+#ifndef RTABMAP_SERIALIZATION_H_
+#define RTABMAP_SERIALIZATION_H_
+
+#include <vector>
+#include <map>
+#include <cstdlib>
+#include <cstring>
+#include <stdio.h>
+#include "rtflann/ext/lz4.h"
+#include "rtflann/ext/lz4hc.h"
+
+
+namespace rtflann
+{
+    struct IndexHeaderStruct {
+        char signature[24];
+        char version[16];
+        flann_datatype_t data_type;
+        flann_algorithm_t index_type;
+        size_t rows;
+        size_t cols;
+        size_t compression;
+        size_t first_block_size;
+    };
+
+namespace serialization
+{
+
+struct access 
+{
+    template<typename Archive, typename T>
+    static inline void serialize(Archive& ar, T& type)
+    {
+        type.serialize(ar);
+    }
+};
+
+
+template<typename Archive, typename T>
+inline void serialize(Archive& ar, T& type)
+{
+    access::serialize(ar,type);
+}
+
+template<typename T>
+struct Serializer
+{
+    template<typename InputArchive>
+    static inline void load(InputArchive& ar, T& val)
+    {
+        serialization::serialize(ar,val);
+    }
+    template<typename OutputArchive>
+    static inline void save(OutputArchive& ar, const T& val)
+    {
+        serialization::serialize(ar,const_cast<T&>(val));
+    }
+};
+
+#define BASIC_TYPE_SERIALIZER(type)\
+template<> \
+struct Serializer<type> \
+{\
+    template<typename InputArchive>\
+    static inline void load(InputArchive& ar, type& val)\
+    {\
+        ar.load(val);\
+    }\
+    template<typename OutputArchive>\
+    static inline void save(OutputArchive& ar, const type& val)\
+    {\
+        ar.save(val);\
+    }\
+}
+
+#define ENUM_SERIALIZER(type)\
+template<>\
+struct Serializer<type>\
+{\
+    template<typename InputArchive>\
+    static inline void load(InputArchive& ar, type& val)\
+    {\
+        int int_val;\
+        ar & int_val;\
+        val = (type) int_val;\
+    }\
+    template<typename OutputArchive>\
+    static inline void save(OutputArchive& ar, const type& val)\
+    {\
+        int int_val = (int)val;\
+        ar & int_val;\
+    }\
+}
+
+
+// declare serializers for simple types
+BASIC_TYPE_SERIALIZER(char);
+BASIC_TYPE_SERIALIZER(unsigned char);
+BASIC_TYPE_SERIALIZER(short);
+BASIC_TYPE_SERIALIZER(unsigned short);
+BASIC_TYPE_SERIALIZER(int);
+BASIC_TYPE_SERIALIZER(unsigned int);
+BASIC_TYPE_SERIALIZER(long);
+BASIC_TYPE_SERIALIZER(unsigned long);
+BASIC_TYPE_SERIALIZER(unsigned long long);
+BASIC_TYPE_SERIALIZER(float);
+BASIC_TYPE_SERIALIZER(double);
+BASIC_TYPE_SERIALIZER(bool);
+#ifdef _MSC_VER
+// unsigned __int64 ~= unsigned long long
+// Will throw error on VS2013
+#if _MSC_VER != 1800
+BASIC_TYPE_SERIALIZER(unsigned __int64);
+#endif
+#endif
+
+
+// serializer for std::vector
+template<typename T>
+struct Serializer<std::vector<T> >
+{
+    template<typename InputArchive>
+    static inline void load(InputArchive& ar, std::vector<T>& val)
+    {
+        size_t size;
+        ar & size;
+        val.resize(size);
+        for (size_t i=0;i<size;++i) {
+            ar & val[i];
+        }
+    }
+
+    template<typename OutputArchive>
+    static inline void save(OutputArchive& ar, const std::vector<T>& val)
+    {
+        ar & val.size();
+        for (size_t i=0;i<val.size();++i) {
+            ar & val[i];
+        }
+    }
+};
+
+// serializer for std::vector
+template<typename K, typename V>
+struct Serializer<std::map<K,V> >
+{
+    template<typename InputArchive>
+    static inline void load(InputArchive& ar, std::map<K,V>& map_val)
+    {
+        size_t size;
+        ar & size;
+        for (size_t i = 0; i < size; ++i)
+        {
+            K key;
+            ar & key;
+            V value;
+            ar & value;
+            map_val[key] = value;
+        }
+    }
+
+    template<typename OutputArchive>
+    static inline void save(OutputArchive& ar, const std::map<K,V>& map_val)
+    {
+        ar & map_val.size();
+        for (typename std::map<K,V>::const_iterator i=map_val.begin(); i!=map_val.end(); ++i) {
+            ar & i->first;
+            ar & i->second;
+        }
+    }
+};
+
+template<typename T>
+struct Serializer<T*>
+{
+    template<typename InputArchive>
+    static inline void load(InputArchive& ar, T*& val)
+    {
+        ar.load(val);
+    }
+
+    template<typename OutputArchive>
+    static inline void save(OutputArchive& ar,  T* const& val)
+    {
+        ar.save(val);
+    }
+};
+
+template<typename T, int N>
+struct Serializer<T[N]>
+{
+    template<typename InputArchive>
+    static inline void load(InputArchive& ar, T (&val)[N])
+    {
+        ar.load(val);
+    }
+
+    template<typename OutputArchive>
+    static inline void save(OutputArchive& ar,  T const (&val)[N])
+    {
+        ar.save(val);
+    }
+};
+
+
+
+
+struct binary_object
+{
+    void const * ptr_;
+    size_t size_;
+
+    binary_object( void * const ptr, size_t size) :
+        ptr_(ptr),
+        size_(size)
+    {}
+    binary_object(const binary_object & rhs) :
+        ptr_(rhs.ptr_),
+        size_(rhs.size_)
+    {}
+
+    binary_object & operator=(const binary_object & rhs) {
+        ptr_ = rhs.ptr_;
+        size_ = rhs.size_;
+        return *this;
+    }
+};
+
+inline const binary_object make_binary_object(/* const */ void * t, size_t size){
+    return binary_object(t, size);
+}
+
+template<>
+struct Serializer<const binary_object>
+{
+    template<typename InputArchive>
+    static inline void load(InputArchive& ar, const binary_object& b)
+    {
+        ar.load_binary(const_cast<void *>(b.ptr_), b.size_);
+    }
+
+    template<typename OutputArchive>
+    static inline void save(OutputArchive& ar,  const binary_object& b)
+    {
+        ar.save_binary(b.ptr_, b.size_);
+    }
+};
+
+template<>
+struct Serializer<binary_object>
+{
+    template<typename InputArchive>
+    static inline void load(InputArchive& ar, binary_object& b)
+    {
+        ar.load_binary(const_cast<void *>(b.ptr_), b.size_);
+    }
+
+    template<typename OutputArchive>
+    static inline void save(OutputArchive& ar,  const binary_object& b)
+    {
+        ar.save_binary(b.ptr_, b.size_);
+    }
+};
+
+
+
+template <bool C_> 
+struct bool_ {
+    static const bool value = C_;
+    typedef bool value_type;
+};
+
+
+class ArchiveBase
+{
+public:
+	void* getObject() { return object_; }
+
+	void setObject(void* object) { object_ = object; }
+
+private:
+	void* object_;
+};
+
+
+template<typename Archive>
+class InputArchive : public ArchiveBase
+{
+protected:
+    InputArchive() {};
+public:
+    typedef bool_<true> is_loading;
+    typedef bool_<false> is_saving;
+
+    template<typename T>
+    Archive& operator& (T& val)
+    {
+        Serializer<T>::load(*static_cast<Archive*>(this),val);
+        return *static_cast<Archive*>(this);
+    }
+};
+
+
+template<typename Archive>
+class OutputArchive : public ArchiveBase
+{
+protected:
+    OutputArchive() {};
+public:
+    typedef bool_<false> is_loading;
+    typedef bool_<true> is_saving;
+    
+    template<typename T>
+    Archive& operator& (const T& val)
+    {
+        Serializer<T>::save(*static_cast<Archive*>(this),val);
+        return *static_cast<Archive*>(this);
+    }
+};
+
+
+
+class SizeArchive : public OutputArchive<SizeArchive>
+{
+    size_t size_;
+public:
+
+    SizeArchive() : size_(0)
+    {
+    }
+
+    template<typename T>
+    void save(const T& val)
+    {
+        size_ += sizeof(val);
+    }
+
+    template<typename T>
+    void save_binary(T* ptr, size_t size)
+    {
+    	size_ += size;
+    }
+
+
+    void reset()
+    {
+        size_ = 0;
+    }
+
+    size_t size()
+    {
+        return size_;
+    }
+};
+
+
+//
+//class PrintArchive : public OutputArchive<PrintArchive>
+//{
+//public:
+//    template<typename T>
+//    void save(const T& val)
+//    {
+//        std::cout << val << std::endl;
+//    }
+//
+//    template<typename T>
+//    void save_binary(T* ptr, size_t size)
+//    {
+//        std::cout << "<binary object>" << std::endl;
+//    }
+//};
+    
+#define BLOCK_BYTES (1024 * 64)
+
+class SaveArchive : public OutputArchive<SaveArchive>
+{
+    /**
+     * Based on blockStreaming_doubleBuffer code at:
+     * https://github.com/Cyan4973/lz4/blob/master/examples/blockStreaming_doubleBuffer.c
+     */
+    
+    FILE* stream_;
+    bool own_stream_;
+    char *buffer_;
+    size_t offset_;
+
+    int first_block_;
+    char *buffer_blocks_;
+    char *compressed_buffer_;
+    LZ4_streamHC_t lz4Stream_body;
+    LZ4_streamHC_t* lz4Stream;
+
+    void initBlock()
+    {
+        // Alloc the space for both buffer blocks (each compressed block
+        // references the previous)
+        buffer_ = buffer_blocks_ = (char *)malloc(BLOCK_BYTES*2);
+        compressed_buffer_ = (char *)malloc(LZ4_COMPRESSBOUND(BLOCK_BYTES) + sizeof(size_t));
+        if (buffer_ == NULL || compressed_buffer_ == NULL) {
+            throw FLANNException("Error allocating compression buffer");
+        }
+        
+        // Init the LZ4 stream
+        lz4Stream = &lz4Stream_body;
+        LZ4_resetStreamHC(lz4Stream, 9);
+        first_block_ = true;
+        
+        offset_ = 0;
+    }
+    
+    void flushBlock()
+    {
+        size_t compSz = 0;
+        // Handle header
+        if (first_block_) {
+            // Copy & set the header
+            IndexHeaderStruct *head = (IndexHeaderStruct *)buffer_;
+            size_t headSz = sizeof(IndexHeaderStruct);
+            
+            assert(head->compression == 0);
+            head->compression = 1; // Bool now, enum later
+        
+            // Do the compression for the block
+            compSz = LZ4_compress_HC_continue(
+                lz4Stream, buffer_+headSz, compressed_buffer_+headSz, offset_-headSz,
+                LZ4_COMPRESSBOUND(BLOCK_BYTES));
+            
+            if(compSz <= 0) {
+                throw FLANNException("Error compressing (first block)");
+            }
+            
+            // Handle header
+            head->first_block_size = compSz;
+            memcpy(compressed_buffer_, buffer_, headSz);
+            
+            compSz += headSz;
+            first_block_ = false;
+        } else {
+            size_t headSz = sizeof(compSz);
+            
+            // Do the compression for the block
+            compSz = LZ4_compress_HC_continue(
+                lz4Stream, buffer_, compressed_buffer_+headSz, offset_,
+                LZ4_COMPRESSBOUND(BLOCK_BYTES));
+            
+            if(compSz <= 0) {
+                throw FLANNException("Error compressing");
+            }
+            
+            // Save the size of the compressed block as the header
+            memcpy(compressed_buffer_, &compSz, headSz);
+            compSz += headSz;
+        }
+        
+        // Write the compressed buffer
+        fwrite(compressed_buffer_, compSz, 1, stream_);
+        
+        // Switch the buffer to the *other* block
+        if (buffer_ == buffer_blocks_)
+            buffer_ = &buffer_blocks_[BLOCK_BYTES];
+        else
+            buffer_ = buffer_blocks_;
+        offset_ = 0;
+    }
+    
+    void endBlock()
+    {
+        // Cleanup memory
+        free(buffer_blocks_);
+        buffer_blocks_ = NULL;
+        buffer_ = NULL;
+        free(compressed_buffer_);
+        compressed_buffer_ = NULL;
+        
+        // Write a '0' size for next block
+        size_t z = 0;
+        fwrite(&z, sizeof(z), 1, stream_);
+    }
+
+public:
+    SaveArchive(const char* filename)
+    {
+        stream_ = fopen(filename, "wb");
+        own_stream_ = true;
+        initBlock();
+    }
+
+    SaveArchive(FILE* stream) : stream_(stream), own_stream_(false)
+    {
+        initBlock();
+    }
+
+    ~SaveArchive()
+    {
+        flushBlock();
+        endBlock();
+        if (buffer_) {
+            free(buffer_);
+            buffer_ = NULL;
+        }
+    	if (own_stream_) {
+    		fclose(stream_);
+    	}
+    }
+
+    template<typename T>
+    void save(const T& val)
+    {
+        assert(sizeof(val) < BLOCK_BYTES);
+        if (offset_+sizeof(val) > BLOCK_BYTES)
+            flushBlock();
+        memcpy(buffer_+offset_, &val, sizeof(val));
+        offset_ += sizeof(val);
+    }
+
+    template<typename T>
+    void save(T* const& val)
+    {
+    	// don't save pointers
+        //fwrite(&val, sizeof(val), 1, handle_);
+    }
+    
+    template<typename T>
+    void save_binary(T* ptr, size_t size)
+    {
+        while (size > BLOCK_BYTES) {
+            // Flush existing block
+            flushBlock();
+            
+            // Save large chunk
+            memcpy(buffer_, ptr, BLOCK_BYTES);
+            offset_ += BLOCK_BYTES;
+            ptr = ((char *)ptr) + BLOCK_BYTES;
+            size -= BLOCK_BYTES;
+        }
+        
+        // Save existing block if new data will make it too big
+        if (offset_+size > BLOCK_BYTES)
+            flushBlock();
+        
+        // Copy out requested data
+        memcpy(buffer_+offset_, ptr, size);
+        offset_ += size;
+    }
+
+};
+
+
+class LoadArchive : public InputArchive<LoadArchive>
+{
+    /**
+     * Based on blockStreaming_doubleBuffer code at:
+     * https://github.com/Cyan4973/lz4/blob/master/examples/blockStreaming_doubleBuffer.c
+     */
+    
+    FILE* stream_;
+    bool own_stream_;
+    char *buffer_;
+    char *ptr_;
+    
+    char *buffer_blocks_;
+    char *compressed_buffer_;
+    LZ4_streamDecode_t lz4StreamDecode_body;
+    LZ4_streamDecode_t* lz4StreamDecode;
+    size_t block_sz_;
+
+    void decompressAndLoadV10(FILE* stream)
+    {
+        buffer_ = NULL;
+        
+        // Find file size
+        size_t pos = ftell(stream);
+        fseek(stream, 0, SEEK_END);
+        size_t fileSize = ftell(stream)-pos;
+        fseek(stream, pos, SEEK_SET);
+        size_t headSz = sizeof(IndexHeaderStruct);
+
+        // Read the (compressed) file to a buffer
+        char *compBuffer = (char *)malloc(fileSize);
+        if (compBuffer == NULL) {
+            throw FLANNException("Error allocating file buffer space");
+        }
+        if (fread(compBuffer, fileSize, 1, stream) != 1) {
+            free(compBuffer);
+            throw FLANNException("Invalid index file, cannot read from disk (compressed)");
+        }
+
+        // Extract header
+        IndexHeaderStruct *head = (IndexHeaderStruct *)(compBuffer);
+        
+        // Backward compatability
+        size_t compressedSz = fileSize-headSz;
+        size_t uncompressedSz = head->first_block_size-headSz;
+        
+        // Check for compression type
+        if (head->compression != 1) {
+            free(compBuffer);
+            throw FLANNException("Compression type not supported");
+        }
+        
+        // Allocate a decompressed buffer
+        ptr_ = buffer_ = (char *)malloc(uncompressedSz+headSz);
+        if (buffer_ == NULL) {
+            free(compBuffer);
+            throw FLANNException("Error (re)allocating decompression buffer");
+        }
+        
+        // Extract body
+        size_t usedSz = LZ4_decompress_safe(compBuffer+headSz,
+                                            buffer_+headSz,
+                                            compressedSz,
+                                            uncompressedSz);
+        
+        // Check if the decompression was the expected size.
+        if (usedSz != uncompressedSz) {
+            free(compBuffer);
+            throw FLANNException("Unexpected decompression size");
+        }
+        
+        // Copy header data
+        memcpy(buffer_, compBuffer, headSz);
+        free(compBuffer);
+        
+        // Put the file pointer at the end of the data we've read
+        if (compressedSz+headSz+pos != fileSize)
+            fseek(stream, compressedSz+headSz+pos, SEEK_SET);
+        block_sz_ = uncompressedSz+headSz;
+    }
+    
+    void initBlock(FILE *stream)
+    {
+        size_t pos = ftell(stream);
+        buffer_ = NULL;
+        buffer_blocks_ = NULL;
+        compressed_buffer_ = NULL;
+        size_t headSz = sizeof(IndexHeaderStruct);
+        
+        // Read the file header to a buffer
+        IndexHeaderStruct *head = (IndexHeaderStruct *)malloc(headSz);
+        if (head == NULL) {
+            throw FLANNException("Error allocating header buffer space");
+        }
+        if (fread(head, headSz, 1, stream) != 1) {
+            free(head);
+            throw FLANNException("Invalid index file, cannot read from disk (header)");
+        }
+        
+        // Backward compatability
+        if (head->signature[13] == '1' && head->signature[15] == '0') {
+            free(head);
+            fseek(stream, pos, SEEK_SET);
+            return decompressAndLoadV10(stream);
+        }
+        
+        // Alloc the space for both buffer blocks (each block
+        // references the previous)
+        buffer_ = buffer_blocks_ = (char *)malloc(BLOCK_BYTES*2);
+        compressed_buffer_ = (char *)malloc(LZ4_COMPRESSBOUND(BLOCK_BYTES));
+        if (buffer_ == NULL || compressed_buffer_ == NULL) {
+            free(head);
+            throw FLANNException("Error allocating compression buffer");
+        }
+        
+        // Init the LZ4 stream
+        lz4StreamDecode = &lz4StreamDecode_body;
+        LZ4_setStreamDecode(lz4StreamDecode, NULL, 0);
+        
+        // Read first block
+        memcpy(buffer_, head, headSz);
+        loadBlock(buffer_+headSz, head->first_block_size, stream);
+        block_sz_ += headSz;
+        ptr_ = buffer_;
+        free(head);
+    }
+
+    void loadBlock(char* buffer_, size_t compSz, FILE* stream)
+    {
+        if(compSz >= LZ4_COMPRESSBOUND(BLOCK_BYTES)) {
+            throw FLANNException("Requested block size too large");
+        }
+        
+        // Read the block into the compressed buffer
+        if (fread(compressed_buffer_, compSz, 1, stream) != 1) {
+            throw FLANNException("Invalid index file, cannot read from disk (block)");
+        }
+        
+        // Decompress into the regular buffer
+        const int decBytes = LZ4_decompress_safe_continue(
+            lz4StreamDecode, compressed_buffer_, buffer_, compSz, BLOCK_BYTES);
+        if(decBytes <= 0) {
+            throw FLANNException("Invalid index file, cannot decompress block");
+        }
+        block_sz_ = decBytes;
+    }
+    
+    void preparePtr(size_t size)
+    {
+        // Return if the new size is less than (or eq) the size of a block
+        if (ptr_+size <= buffer_+block_sz_)
+            return;
+        
+        // Switch the buffer to the *other* block
+        if (buffer_ == buffer_blocks_)
+            buffer_ = &buffer_blocks_[BLOCK_BYTES];
+        else
+            buffer_ = buffer_blocks_;
+        
+        // Find the size of the next block
+        size_t cmpSz = 0;
+        size_t readCnt = fread(&cmpSz, sizeof(cmpSz), 1, stream_);
+        if(cmpSz <= 0 || readCnt != 1) {
+            throw FLANNException("Requested to read next block past end of file");
+        }
+        
+        // Load block & init ptr
+        loadBlock(buffer_, cmpSz, stream_);
+        ptr_ = buffer_;
+    }
+    
+    void endBlock()
+    {
+        // If not v1.0 format hack...
+        if (buffer_blocks_ != NULL) {
+            // Read the last '0' in the file
+            size_t zero = -1;
+            if (fread(&zero, sizeof(zero), 1, stream_) != 1) {
+                throw FLANNException("Invalid index file, cannot read from disk (end)");
+            }
+            if (zero != 0) {
+                throw FLANNException("Invalid index file, last block not zero length");
+            }
+        }
+        
+        // Free resources
+        if (buffer_blocks_ != NULL) {
+            free(buffer_blocks_);
+            buffer_blocks_ = NULL;
+        }
+        if (compressed_buffer_ != NULL) {
+            free(compressed_buffer_);
+            compressed_buffer_ = NULL;
+        }
+        ptr_ = NULL;
+    }
+    
+public:
+    LoadArchive(const char* filename)
+    {
+        // Open the file
+        stream_ = fopen(filename, "rb");
+        own_stream_ = true;
+
+        initBlock(stream_);
+    }
+
+    LoadArchive(FILE* stream)
+    {
+        stream_ = stream;
+        own_stream_ = false;
+
+        initBlock(stream);
+    }
+
+    ~LoadArchive()
+    {
+        endBlock();
+    	if (own_stream_) {
+    		fclose(stream_);
+    	}
+    }
+
+    template<typename T>
+    void load(T& val)
+    {
+        preparePtr(sizeof(val));
+        memcpy(&val, ptr_, sizeof(val));
+        ptr_ += sizeof(val);
+    }
+
+    template<typename T>
+    void load(T*& val)
+    {
+    	// don't load pointers
+        //fread(&val, sizeof(val), 1, handle_);
+    }
+
+    template<typename T>
+    void load_binary(T* ptr, size_t size)
+    {
+        while (size > BLOCK_BYTES) {
+            // Load next block
+            preparePtr(BLOCK_BYTES);
+            
+            // Load large chunk
+            memcpy(ptr, ptr_, BLOCK_BYTES);
+            ptr_ += BLOCK_BYTES;
+            ptr = ((char *)ptr) + BLOCK_BYTES;
+            size -= BLOCK_BYTES;
+        }
+        
+        // Load next block if needed
+        preparePtr(size);
+        
+        // Load the data
+        memcpy(ptr, ptr_, size);
+        ptr_ += size;
+    }
+};
+
+} // namespace serialization
+} // namespace flann
+#endif // SERIALIZATION_H_

corelib/src/rtflann/util/timer.h

@@ -0,0 +1,95 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_TIMER_H
+#define RTABMAP_FLANN_TIMER_H
+
+#include <time.h>
+
+
+namespace rtflann
+{
+
+/**
+ * A start-stop timer class.
+ *
+ * Can be used to time portions of code.
+ */
+class StartStopTimer
+{
+    clock_t startTime;
+
+public:
+    /**
+     * Value of the timer.
+     */
+    double value;
+
+
+    /**
+     * Constructor.
+     */
+    StartStopTimer()
+    {
+        reset();
+    }
+
+    /**
+     * Starts the timer.
+     */
+    void start()
+    {
+        startTime = clock();
+    }
+
+    /**
+     * Stops the timer and updates timer value.
+     */
+    double stop()
+    {
+        clock_t stopTime = clock();
+        value += ( (double)stopTime - startTime) / CLOCKS_PER_SEC;
+        
+        return value;
+    }
+
+    /**
+     * Resets the timer value to 0.
+     */
+    void reset()
+    {
+        value = 0;
+    }
+
+};
+
+}
+
+#endif // FLANN_TIMER_H

corelib/src/sqlite3/readme.txt

@@ -0,0 +1,3 @@
+
+Info: https://www.sqlite.org/
+License: Public domain (https://www.sqlite.org/copyright.html)

corelib/src/toro3d/readme.txt

@@ -0,0 +1,3 @@
+
+Info: https://www.openslam.org/toro.html
+License: Creative Commons (Attribution-NonCommercial-ShareAlike)

corelib/src/toro3d/transformation2.hh

@@ -187,8 +187,10 @@ struct Transformation2{
   }
 
   /** Constructor that sets the translation and rotation  **/
-  Transformation2 (const T& _theta, const Vector2<T>& trans):
+  Transformation2 (const T& _theta, const Vector2<T>& trans){
-    Transformation2(trans.x(), trans.y(), _theta){}
+	  setRotation(_theta);
+	  setTranslation(trans.x(), trans.y());
+  }
 
 
   /** Copy constructor  **/

corelib/src/util2d.cpp

@@ -42,12 +42,13 @@ namespace util2d
 
 cv::Mat disparityFromStereoImages(
 		const cv::Mat & leftImage,
-		const cv::Mat & rightImage)
+		const cv::Mat & rightImage,
+		int type)
 {
-	UASSERT(!leftImage.empty() && !rightImage.empty() &&
+	UASSERT(!leftImage.empty() && !rightImage.empty());
-			(leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1 &&
+	UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
-			leftImage.cols == rightImage.cols &&
+	UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1);
-			leftImage.rows == rightImage.rows);
+	UASSERT(type == CV_32FC1 || type == CV_16SC1);
 
 	cv::Mat leftMono;
 	if(leftImage.channels() == 3)
@@ -70,7 +71,7 @@ cv::Mat disparityFromStereoImages(
 	stereo.state->textureThreshold = 10;
 	stereo.state->speckleWindowSize = 100;
 	stereo.state->speckleRange = 4;
-	stereo(leftMono, rightImage, disparity, CV_16SC1);
+	stereo(leftMono, rightImage, disparity, type);
 #else
 	cv::Ptr<cv::StereoBM> stereo = cv::StereoBM::create();
 	stereo->setBlockSize(15);
@@ -97,10 +98,9 @@ cv::Mat disparityFromStereoImages(
 		double flowEps,
 		float maxCorrespondencesSlope)
 {
-	UASSERT(!leftImage.empty() && !rightImage.empty() &&
+	UASSERT(!leftImage.empty() && !rightImage.empty());
-			leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
+	UASSERT(leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1);
-			leftImage.cols == rightImage.cols &&
+	UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
-			leftImage.rows == rightImage.rows);
 
 	// Find features in the new left image
 	std::vector<unsigned char> status;
@@ -122,6 +122,51 @@ cv::Mat disparityFromStereoImages(
 	return disparityFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, maxCorrespondencesSlope);
 }
 
+cv::Mat depthFromDisparity(const cv::Mat & disparity,
+		float fx, float baseline,
+		int type)
+{
+	UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
+	UASSERT(type == CV_32FC1 || type == CV_16UC1);
+	cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type);
+	int countOverMax = 0;
+	for (int i = 0; i < disparity.rows; i++)
+	{
+		for (int j = 0; j < disparity.cols; j++)
+		{
+			float disparity_value = disparity.type() == CV_16SC1?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
+			if (disparity_value > 0.0f)
+			{
+				// baseline * focal / disparity
+				float d = baseline * fx / disparity_value;
+				if(d>0)
+				{
+					if(depth.type() == CV_32FC1)
+					{
+						depth.at<float>(i,j) = d;
+					}
+					else
+					{
+						if(d*1000.0f <= (float)USHRT_MAX)
+						{
+							depth.at<unsigned short>(i,j) = (unsigned short)(d*1000.0f);
+						}
+						else
+						{
+							++countOverMax;
+						}
+					}
+				}
+			}
+		}
+	}
+	if(countOverMax)
+	{
+		UWARN("Depth conversion error, %d depth values ignored because they are over the maximum depth allowed (65535 mm).", countOverMax);
+	}
+	return depth;
+}
+
 cv::Mat depthFromStereoImages(
 		const cv::Mat & leftImage,
 		const cv::Mat & rightImage,
@@ -209,6 +254,61 @@ cv::Mat depthFromStereoCorrespondences(
 	return depth;
 }
 
+cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
+{
+	UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
+	cv::Mat depth16U;
+	if(!depth32F.empty())
+	{
+		depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
+		int countOverMax = 0;
+		for(int i=0; i<depth32F.rows; ++i)
+		{
+			for(int j=0; j<depth32F.cols; ++j)
+			{
+				float depth = (depth32F.at<float>(i,j)*1000.0f);
+				unsigned short depthMM = 0;
+				if(depth > 0 && depth <= (float)USHRT_MAX)
+				{
+					depthMM = (unsigned short)depth;
+				}
+				else if(depth > (float)USHRT_MAX)
+				{
+					++countOverMax;
+				}
+				depth16U.at<unsigned short>(i, j) = depthMM;
+			}
+		}
+		if(countOverMax)
+		{
+			UWARN("Depth conversion error, %d depth values ignored because "
+				  "they are over the maximum depth allowed (65535 mm). Is the depth "
+				  "image really in meters? 32 bits images should be in meters, "
+				  "and 16 bits should be in mm.", countOverMax);
+		}
+	}
+	return depth16U;
+}
+
+cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
+{
+	UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
+	cv::Mat depth32F;
+	if(!depth16U.empty())
+	{
+		depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
+		for(int i=0; i<depth16U.rows; ++i)
+		{
+			for(int j=0; j<depth16U.cols; ++j)
+			{
+				float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
+				depth32F.at<float>(i, j) = depth;
+			}
+		}
+	}
+	return depth32F;
+}
+
 float getDepth(
 		const cv::Mat & depthImage,
 		float x, float y,

corelib/src/util3d.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UConversion.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/transforms.h>
 #include <opencv2/imgproc/imgproc.hpp>
@@ -285,10 +286,11 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 		float fx, float fy,
 		int decimation)
 {
+	UDEBUG("");
 	UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols);
 	UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
-	UASSERT(imageDepth.rows % decimation == 0);
+	UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("imageDepth.rows=%d decimation=%d", imageDepth.rows, decimation).c_str());
-	UASSERT(imageDepth.cols % decimation == 0);
+	UASSERT_MSG(imageDepth.cols % decimation == 0, uFormat("imageDepth.cols=%d decimation=%d", imageDepth.rows, decimation).c_str());
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 	if(decimation < 1)
@@ -503,14 +505,13 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		float voxelSize,
 		int samples)
 {
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 
 	if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
 	{
 		//depth
 		UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
 		int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
-		cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
 		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
 		{
 			if(sensorData.cameraModels()[i].isValid())
@@ -626,110 +627,118 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		float voxelSize,
 		int samples)
 {
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+	UASSERT(!sensorData.imageRaw().empty());
+	UASSERT((!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) ||
+			(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid()));
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 
-	if(!sensorData.imageRaw().empty())
+	if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
 	{
-		if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
+		//depth
+		UDEBUG("");
+		UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols);
+		UASSERT(sensorData.depthRaw().size() == sensorData.imageRaw().size());
+		int subImageWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
+		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
 		{
-			//depth
+			if(sensorData.cameraModels()[i].isValid())
-			UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols);
-			UASSERT(sensorData.depthRaw().size() == sensorData.imageRaw().size());
-			int subImageWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
-			cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
-			for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
 			{
-				if(sensorData.cameraModels()[i].isValid())
+				if(subImageWidth % decimation != 0 || sensorData.depthRaw().rows % decimation != 0)
 				{
-					pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
+					UWARN("Image size (%d,%d) modulus decimation (%d) is not null "
-							cv::Mat(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows)),
+						  "for the cloud creation! Setting decimation to 1...",
-							cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
+						  subImageWidth, sensorData.depthRaw().rows, decimation);
-							sensorData.cameraModels()[i].cx(),
+					decimation = 1;
-							sensorData.cameraModels()[i].cy(),
+				}
-							sensorData.cameraModels()[i].fx(),
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
-							sensorData.cameraModels()[i].fy(),
+						cv::Mat(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows)),
-							decimation);
+						cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
+						sensorData.cameraModels()[i].cx(),
+						sensorData.cameraModels()[i].cy(),
+						sensorData.cameraModels()[i].fx(),
+						sensorData.cameraModels()[i].fy(),
+						decimation);
+
+				if(tmp->size())
+				{
+					bool filtered = false;
+					if(tmp->size() && maxDepth)
+					{
+						tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
+						filtered = true;
+					}
+
+					if(tmp->size() && voxelSize)
+					{
+						tmp = util3d::voxelize(tmp, voxelSize);
+						filtered = true;
+					}
+
+					if(tmp->size() && samples)
+					{
+						tmp = util3d::sampling(tmp, samples);
+						filtered = true;
+					}
+
+					if(tmp->size() && !filtered)
+					{
+						tmp = util3d::removeNaNFromPointCloud(tmp);
+					}
 
 					if(tmp->size())
 					{
-						bool filtered = false;
+						tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
-						if(tmp->size() && maxDepth)
-						{
-							tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
-							filtered = true;
-						}
-
-						if(tmp->size() && voxelSize)
-						{
-							tmp = util3d::voxelize(tmp, voxelSize);
-							filtered = true;
-						}
-
-						if(tmp->size() && samples)
-						{
-							tmp = util3d::sampling(tmp, samples);
-							filtered = true;
-						}
-
-						if(tmp->size() && !filtered)
-						{
-							tmp = util3d::removeNaNFromPointCloud(tmp);
-						}
-
-						if(tmp->size())
-						{
-							tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
-						}
-
-						*cloud += *tmp;
 					}
+
+					*cloud += *tmp;
 				}
-				else
+			}
-				{
+			else
-					UERROR("Camera model %d is invalid", i);
+			{
-				}
+				UERROR("Camera model %d is invalid", i);
+			}
+		}
+
+		if(cloud->size() && voxelSize)
+		{
+			cloud = util3d::voxelize(cloud, voxelSize);
+		}
+	}
+	else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())
+	{
+		//stereo
+		UDEBUG("");
+		cloud = cloudFromStereoImages(sensorData.imageRaw(),
+				sensorData.rightRaw(),
+				sensorData.stereoCameraModel().left().cx(),
+				sensorData.stereoCameraModel().left().cy(),
+				sensorData.stereoCameraModel().left().fx(),
+				sensorData.stereoCameraModel().baseline(),
+				decimation);
+
+		if(cloud->size())
+		{
+			bool filtered = false;
+			if(cloud->size() && maxDepth)
+			{
+				cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
+				filtered = true;
 			}
 
 			if(cloud->size() && voxelSize)
 			{
 				cloud = util3d::voxelize(cloud, voxelSize);
+				filtered = true;
+			}
+
+			if(cloud->size() && !filtered)
+			{
+				cloud = util3d::removeNaNFromPointCloud(cloud);
 			}
-		}
-		else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())
-		{
-			//stereo
-			cloud = cloudFromStereoImages(sensorData.imageRaw(),
-					sensorData.rightRaw(),
-					sensorData.stereoCameraModel().left().cx(),
-					sensorData.stereoCameraModel().left().cy(),
-					sensorData.stereoCameraModel().left().fx(),
-					sensorData.stereoCameraModel().baseline(),
-					decimation);
 
 			if(cloud->size())
 			{
-				bool filtered = false;
+				cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
-				if(cloud->size() && maxDepth)
-				{
-					cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
-					filtered = true;
-				}
-
-				if(cloud->size() && voxelSize)
-				{
-					cloud = util3d::voxelize(cloud, voxelSize);
-					filtered = true;
-				}
-
-				if(cloud->size() && !filtered)
-				{
-					cloud = util3d::removeNaNFromPointCloud(cloud);
-				}
-
-				if(cloud->size())
-				{
-					cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
-				}
 			}
 		}
 	}
@@ -771,50 +780,6 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
 	return scan;
 }
 
-
-cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
-{
-	UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
-	cv::Mat depth16U;
-	if(!depth32F.empty())
-	{
-		depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
-		for(int i=0; i<depth32F.rows; ++i)
-		{
-			for(int j=0; j<depth32F.cols; ++j)
-			{
-				float depth = (depth32F.at<float>(i,j)*1000.0f);
-				unsigned short depthMM = 0;
-				if(depth <= (float)USHRT_MAX)
-				{
-					depthMM = (unsigned short)depth;
-				}
-				depth16U.at<unsigned short>(i, j) = depthMM;
-			}
-		}
-	}
-	return depth16U;
-}
-
-cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
-{
-	UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
-	cv::Mat depth32F;
-	if(!depth16U.empty())
-	{
-		depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
-		for(int i=0; i<depth16U.rows; ++i)
-		{
-			for(int j=0; j<depth16U.cols; ++j)
-			{
-				float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
-				depth32F.at<float>(i, j) = depth;
-			}
-		}
-	}
-	return depth32F;
-}
-
 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
 {
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
@@ -906,36 +871,6 @@ pcl::PointXYZ projectDisparityTo3D(
 	return pcl::PointXYZ(bad_point, bad_point, bad_point);
 }
 
-cv::Mat depthFromDisparity(const cv::Mat & disparity,
-		float fx, float baseline,
-		int type)
-{
-	UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
-	UASSERT(type == CV_32FC1 || type == CV_16U);
-	cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type);
-	for (int i = 0; i < disparity.rows; i++)
-	{
-		for (int j = 0; j < disparity.cols; j++)
-		{
-			float disparity_value = disparity.type() == CV_16SC1?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
-			if (disparity_value > 0.0f)
-			{
-				// baseline * focal / disparity
-				float d = baseline * fx / disparity_value;
-				if(depth.type() == CV_32FC1)
-				{
-					depth.at<float>(i,j) = d;
-				}
-				else
-				{
-					depth.at<unsigned short>(i,j) = (unsigned short)(d*1000.0f);
-				}
-			}
-		}
-	}
-	return depth;
-}
-
 pcl::PointCloud<pcl::PointXYZ>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds)
 {
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);