package.xml: 0.20.7

package.xml: removed libvtk-qt as there are conflicts on noetic with libpcl-dev (which is using vtk7)
RegIcp: don't assert if normals cannot be computed properly before doing complexity check, just reject the transform
2026-09-08 12:30:20 +08:00 · 2020-12-12 19:05:06 -05:00 · 2020-12-12 17:32:30 -05:00 · 2020-12-12 16:53:07 -05:00 · 2020-12-11 17:22:29 -05:00 · 2020-12-11 17:05:37 -05:00
143 changed files with 7653 additions and 2993 deletions
@@ -35,7 +35,7 @@ install:
  # OpenCV
  #- ps: wget 'http://kent.dl.sourceforge.net/project/opencvlibrary/opencv-win/3.3.1/opencv-3.3.1-vc14.exe' -outfile opencv-3.3.1-vc14.exe
  #- cmd: opencv-3.3.1-vc14.exe -o"C:\Program Files" -y
-  - ps: wget 'http://kent.dl.sourceforge.net/project/opencvlibrary/opencv-win/2.4.13/opencv-2.4.13.6-vc14.exe' -outfile opencv-2.4.13.6-vc14.exe
+  - ps: $url = "https://downloads.sourceforge.net/project/opencvlibrary/opencv-win/2.4.13/opencv-2.4.13.6-vc14.exe?r=&ts="+([int64](([datetime]::UtcNow)-(get-date "1/1/1970")).TotalSeconds) ; wget $url -outfile opencv-2.4.13.6-vc14.exe
  - cmd: opencv-2.4.13.6-vc14.exe -o"C:\Program Files" -y
  - ECHO "Installed OpenCV:"
  - ps: "ls \"C:/Program Files/opencv/build\""
@@ -115,6 +115,20 @@ install:
  - cmd: yaml-cpp.exe -o"C:\Program Files" -y
  - ECHO "Installed yaml-cpp:"
  - ps: "ls \"C:/Program Files/yaml-cpp\""
+  # RealSense2
+  - ps: wget 'https://github.com/IntelRealSense/librealsense/releases/download/v2.40.0/Intel.RealSense.SDK-WIN10-2.40.0.2482.exe' -outfile realsense2.exe
+  - cmd: realsense2.exe /VERYSILENT
+  - ECHO "Installed RealSense2:"
+  - ps: "ls \"C:/Program Files (x86)/Intel RealSense SDK 2.0\""
+  - set PATH=%PATH%;C:\Program Files (x86)\Intel RealSense SDK 2.0\bin\x64
+  - set RealSense2_ROOT_DIR=C:\Program Files (x86)\Intel RealSense SDK 2.0
+  # Kinect 4 Azure
+  - ps: wget 'https://download.microsoft.com/download/3/d/6/3d6d9e99-a251-4cf3-8c6a-8e108e960b4b/Azure%20Kinect%20SDK%201.4.1.exe' -outfile azure.exe
+  - cmd: azure.exe /quiet
+  - ECHO "Installed Kinect For Azure:"
+  - ps: "ls \"C:/Program Files/Azure Kinect SDK v1.4.1\""
+  - set PATH=%PATH%;C:\Program Files\Azure Kinect SDK v1.4.1\tools
+  - set K4A_ROOT_DIR=C:\Program Files\Azure Kinect SDK v1.4.1
  
 before_build:
  - cd c:\projects\rtabmap\build
@@ -8,3 +8,5 @@ app/android/.classpath
 app/android/.project
 app/android/AndroidManifest.xml
 app/android/res/raw/
+compile_flags.txt
+tags
@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 20)
-SET(RTABMAP_PATCH_VERSION 3)
+SET(RTABMAP_PATCH_VERSION 7)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -61,7 +61,7 @@ ELSE ()
 ENDIF()

 if(POLICY CMP0020)
-    cmake_policy(SET CMP0020 OLD)
+    cmake_policy(SET CMP0020 NEW)
 endif()
 if(POLICY CMP0043)
    cmake_policy(SET CMP0043 OLD)
@@ -427,6 +427,10 @@ ENDIF(WITH_CVSBA)
 IF(WITH_POINTMATCHER)
    find_package(libpointmatcher QUIET)
    IF(libpointmatcher_FOUND)
+       find_package(Boost COMPONENTS thread filesystem system program_options date_time REQUIRED)
+       if (Boost_MINOR_VERSION GREATER 47)
+           find_package(Boost COMPONENTS thread filesystem system program_options date_time chrono timer REQUIRED)
+       endif (Boost_MINOR_VERSION GREATER 47)
       MESSAGE(STATUS "Found libpointmatcher: ${libpointmatcher_INCLUDE_DIRS}")
    ENDIF(libpointmatcher_FOUND)
 ENDIF(WITH_POINTMATCHER)
@@ -515,9 +519,9 @@ IF(WITH_ALICE_VISION)
    IF(AliceVision_FOUND)
        SET(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/local/lib/cmake/modules")
        find_package(Geogram REQUIRED QUIET)
-        # Make sure the two following lines are also commented in AliceVision to avoid Eigen memory alignment error
-        #add_definitions("-DEIGEN_DONT_ALIGN_STATICALLY=1")
-        #add_definitions("-DEIGEN_DONT_VECTORIZE=1")
+        add_definitions("-DRTABMAP_ALICE_VISION_MAJOR=${AliceVision_VERSION_MAJOR}")
+        add_definitions("-DRTABMAP_ALICE_VISION_MINOR=${AliceVision_VERSION_MINOR}")
+        add_definitions("-DRTABMAP_ALICE_VISION_PATCH=${AliceVision_VERSION_PATCH}")
    ENDIF(AliceVision_FOUND)
 ENDIF(WITH_ALICE_VISION)

@@ -622,7 +626,8 @@ ELSEIF(G2O_FOUND OR
       okvis_FOUND OR 
       open_chisel_FOUND OR 
       msckf_vio_FOUND OR 
-       vins_FOUND)
+       vins_FOUND OR 
+       libpointmatcher_FOUND)
   #Newest versions require std11
   IF(NOT MSVC)
     include(CheckCXXCompilerFlag)
@@ -1209,7 +1214,7 @@ MESSAGE(STATUS "  With OpenChisel           = NO (open_chisel not found)")
 ENDIF()

 IF(AliceVision_FOUND)
-MESSAGE(STATUS "  With AliceVision          = YES (License: MPLv2)")
+MESSAGE(STATUS "  With AliceVision ${AliceVision_VERSION}    = YES (License: MPLv2)")
 ELSEIF(NOT WITH_ALICE_VISION)
 MESSAGE(STATUS "  With AliceVision          = NO (WITH_ALICE_VISION=OFF)")
 ELSE()
@@ -76,6 +76,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@PYMATCHER@#define RTABMAP_PYMATCHER
@MADGWICK@#define RTABMAP_MADGWICK

+#include <pcl/pcl_config.h>
+
+#if PCL_VERSION_COMPARE(>, 1, 11, 1)
+#include <pcl/types.h>
+#define RTABMAP_PCL_INDEX pcl::index_t
+#elif PCL_VERSION_COMPARE(>=, 1, 10, 0)
+#define RTABMAP_PCL_INDEX std::uint32_t
+#else
+#include <pcl/pcl_macros.h>
+#define RTABMAP_PCL_INDEX pcl::uint32_t
+#endif

 #endif /* VERSION_H_ */

@@ -308,7 +308,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 		optRefPose_ = 0;
 	}
 	cv::Mat cloudMat;
-	std::vector<std::vector<std::vector<unsigned int> > > polygons;
+	std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 	std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
 #else
@@ -2902,7 +2902,7 @@ bool RTABMapApp::exportMesh(
 						pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 						pcl::fromPCLPointCloud2(polygonMesh->cloud, *cloud);
 						cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false)); // for database
-						std::vector<std::vector<std::vector<unsigned int> > > polygons(1);
+						std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(1);
 						polygons[0].resize(polygonMesh->polygons.size());
 						for(unsigned int p=0; p<polygonMesh->polygons.size(); ++p)
 						{
@@ -2921,7 +2921,7 @@ bool RTABMapApp::exportMesh(
 					cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false)); // for database

 					// save in database
-					std::vector<std::vector<std::vector<unsigned int> > > polygons(textureMesh->tex_polygons.size());
+					std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(textureMesh->tex_polygons.size());
 					for(unsigned int t=0; t<textureMesh->tex_polygons.size(); ++t)
 					{
 						polygons[t].resize(textureMesh->tex_polygons[t].size());
@@ -3114,7 +3114,7 @@ bool RTABMapApp::postExportation(bool visualize)
 	{
 		visualizingMesh_ = false;
 		cv::Mat cloudMat;
-		std::vector<std::vector<std::vector<unsigned int> > > polygons;
+		std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 		std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
 #else
@@ -3171,7 +3171,7 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str
 	pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh);
 	pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
 	cv::Mat cloudMat;
-	std::vector<std::vector<std::vector<unsigned int> > > polygons;
+	std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 	std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
 #else
@@ -3186,7 +3186,7 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str
 			LOGI("writeExportedMesh: Found optimized mesh!");
 			if(textures.empty())
 			{
-				polygonMesh = rtabmap::util3d::assemblePolygonMesh(cloudMat, polygons.size() == 1?polygons[0]:std::vector<std::vector<unsigned int> >());
+				polygonMesh = rtabmap::util3d::assemblePolygonMesh(cloudMat, polygons.size() == 1?polygons[0]:std::vector<std::vector<RTABMAP_PCL_INDEX> >());
 			}
 			else
 			{
@@ -61,6 +61,8 @@ public:

 	cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids);

+	unsigned long getMemoryUsed() const;
+
 private:
 	cv::Mat updatePrediction(const cv::Mat & oldPrediction,
 			const Memory * memory,
@@ -158,5 +158,7 @@ private:
 	Transform localTransform_;
 };

+RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const CameraModel& model);
+
 } /* namespace rtabmap */
 #endif /* CAMERAMODEL_H_ */
@@ -72,6 +72,15 @@ public:
 	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap());
 	void disableIMUFiltering();

+	RTABMAP_DEPRECATED(void setScanParameters(
+			bool fromDepth,
+			int downsampleStep, // decimation of the depth image in case the scan is from depth image
+			float rangeMin,
+			float rangeMax,
+			float voxelSize,
+			int normalsK,
+			int normalsRadius,
+			bool forceGroundNormalsUp) , "Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.");
 	void setScanParameters(
 			bool fromDepth,
 			int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
@@ -80,17 +89,7 @@ public:
 			float voxelSize = 0.0f,
 			int normalsK = 0,
 			int normalsRadius = 0.0f,
-			bool forceGroundNormalsUp = false)
-	{
-		_scanFromDepth = fromDepth;
-		_scanDownsampleStep=downsampleStep;
-		_scanRangeMin = rangeMin;
-		_scanRangeMax = rangeMax;
-		_scanVoxelSize = voxelSize;
-		_scanNormalsK = normalsK;
-		_scanNormalsRadius = normalsRadius;
-		_scanForceGroundNormalsUp = forceGroundNormalsUp;
-	}
+			float groundNormalsUp = 0.0f);

 	void postUpdate(SensorData * data, CameraInfo * info = 0) const;

@@ -119,7 +118,7 @@ private:
 	float _scanVoxelSize;
 	int _scanNormalsK;
 	float _scanNormalsRadius;
-	bool _scanForceGroundNormalsUp;
+	float _scanForceGroundNormalsUp;
 	StereoDense * _stereoDense;
 	clams::DiscreteDepthDistortionModel * _distortionModel;
 	bool _bilateralFiltering;
@@ -70,6 +70,7 @@ public:
 	virtual void parseParameters(const ParametersMap & parameters);
 	virtual bool isInMemory() const {return _url.empty();}
 	const std::string & getUrl() const {return _url;}
+	const std::string & getTargetVersion() const {return _targetVersion;}

 	void beginTransaction() const;
 	void commit() const;
@@ -109,7 +110,7 @@ public:
 	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
 	void saveOptimizedMesh(
 			const cv::Mat & cloud,
-			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
+			const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons = std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > >(),      // Textures -> polygons -> vertices
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
 #else
@@ -117,7 +118,7 @@ public:
 #endif
 			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size);
 	cv::Mat loadOptimizedMesh(
-			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
+			std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons = 0,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
 #else
@@ -131,7 +132,7 @@ public:
 	bool openConnection(const std::string & url, bool overwritten = false);
 	void closeConnection(bool save = true, const std::string & outputUrl = "");
 	bool isConnected() const;
-	long getMemoryUsed() const; // In bytes
+	unsigned long getMemoryUsed() const; // In bytes
 	std::string getDatabaseVersion() const;
 	long getNodesMemoryUsed() const;
 	long getLinksMemoryUsed() const;
@@ -188,7 +189,7 @@ protected:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "") = 0;
 	virtual bool isConnectedQuery() const = 0;
-	virtual long getMemoryUsedQuery() const = 0; // In bytes
+	virtual unsigned long getMemoryUsedQuery() const = 0; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const = 0;
 	virtual long getNodesMemoryUsedQuery() const = 0;
 	virtual long getLinksMemoryUsedQuery() const = 0;
@@ -247,7 +248,7 @@ protected:
 	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const = 0;
 	virtual void saveOptimizedMeshQuery(
 				const cv::Mat & cloud,
-				const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+				const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 				const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
 #else
@@ -255,7 +256,7 @@ protected:
 #endif
 				const cv::Mat & textures) const = 0;
 	virtual cv::Mat loadOptimizedMeshQuery(
-				std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+				std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 				std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
 #else
@@ -300,6 +301,7 @@ private:
 	USemaphore _addSem;
 	double _emptyTrashesTime;
 	std::string _url;
+	std::string _targetVersion;
 	bool _timestampUpdate;
 };

@@ -54,7 +54,7 @@ protected:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
 	virtual bool isConnectedQuery() const;
-	virtual long getMemoryUsedQuery() const; // In bytes
+	virtual unsigned long getMemoryUsedQuery() const; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const;
 	virtual long getNodesMemoryUsedQuery() const;
 	virtual long getLinksMemoryUsedQuery() const;
@@ -113,7 +113,7 @@ protected:
 	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const;
 	virtual void saveOptimizedMeshQuery(
 			const cv::Mat & cloud,
-			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+			const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
 #else
@@ -121,7 +121,7 @@ protected:
 #endif
 			const cv::Mat & textures) const;
 	virtual cv::Mat loadOptimizedMeshQuery(
-			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+			std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
 #else
@@ -189,7 +189,7 @@ protected:
 	std::string _version;

 private:
-	long _memoryUsedEstimate;
+	unsigned long _memoryUsedEstimate;
 	bool _dbInMemory;
 	unsigned int _cacheSize;
 	int _journalMode;
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 #include "rtabmap/core/Parameters.h"
+#include "rtabmap/utilite/UStl.h"
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <pcl/point_cloud.h>
@@ -91,41 +92,133 @@ public:
 	 * if a=[1 2 3 4 6], b=[1 2 4 5 6], results= [(1,1) (2,2) (4,4) (6,6)]
 	 * realPairsCount = 4
 	 */
+	template<typename T>
 	static int findPairs(
-			const std::map<int, cv::KeyPoint> & wordsA,
-			const std::map<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-			bool ignoreNegativeIds = true);
+			const std::map<int, T> & wordsA,
+			const std::map<int, T> & wordsB,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
+			bool ignoreNegativeIds = true)
+	{
+		int realPairsCount = 0;
+		pairs.clear();
+		for(typename std::map<int, T>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && i->first>=0))
+			{
+				std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
+				if(ptB != wordsB.end())
+				{
+					pairs.push_back(std::pair<int, std::pair<T, T> >(i->first, std::make_pair(i->second, ptB->second)));
+					++realPairsCount;
+				}
+			}
+		}
+		return realPairsCount;
+	}

 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
 	 * realPairsCount = 5
 	 */
+	template<typename T>
 	static int findPairs(
-			const std::multimap<int, cv::KeyPoint> & wordsA,
-			const std::multimap<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-			bool ignoreNegativeIds = true);
+			const std::multimap<int, T> & wordsA,
+			const std::multimap<int, T> & wordsB,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
+			bool ignoreNegativeIds = true)
+	{
+		const std::list<int> & ids = uUniqueKeys(wordsA);
+		typename std::multimap<int, T>::const_iterator iterA;
+		typename std::multimap<int, T>::const_iterator iterB;
+		pairs.clear();
+		int realPairsCount = 0;
+		for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && *i >= 0))
+			{
+				iterA = wordsA.find(*i);
+				iterB = wordsB.find(*i);
+				while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
+				{
+					pairs.push_back(std::pair<int, std::pair<T, T> >(*i, std::make_pair((*iterA).second, (*iterB).second)));
+					++iterA;
+					++iterB;
+					++realPairsCount;
+				}
+			}
+		}
+		return realPairsCount;
+	}

 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
 	 * realPairsCount = 5
 	 */
+	template<typename T>
 	static int findPairsUnique(
-			const std::multimap<int, cv::KeyPoint> & wordsA,
-			const std::multimap<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-			bool ignoreNegativeIds = true);
+			const std::multimap<int, T> & wordsA,
+			const std::multimap<int, T> & wordsB,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
+			bool ignoreNegativeIds = true)
+	{
+		const std::list<int> & ids = uUniqueKeys(wordsA);
+		int realPairsCount = 0;
+		pairs.clear();
+		for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && *i>=0))
+			{
+				std::list<T> ptsA = uValues(wordsA, *i);
+				std::list<T> ptsB = uValues(wordsB, *i);
+				if(ptsA.size() == 1 && ptsB.size() == 1)
+				{
+					pairs.push_back(std::pair<int, std::pair<T, T> >(*i, std::pair<T, T>(ptsA.front(), ptsB.front())));
+					++realPairsCount;
+				}
+				else if(ptsA.size()>1 && ptsB.size()>1)
+				{
+					// just update the count
+					realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+				}
+			}
+		}
+		return realPairsCount;
+	}

 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
 	 * realPairsCount = 5
 	 */
+	template<typename T>
 	static int findPairsAll(
-			const std::multimap<int, cv::KeyPoint> & wordsA,
-			const std::multimap<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-			bool ignoreNegativeIds = true);
+			const std::multimap<int, T> & wordsA,
+			const std::multimap<int, T> & wordsB,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
+			bool ignoreNegativeIds = true)
+	{
+		const std::list<int> & ids = uUniqueKeys(wordsA);
+		pairs.clear();
+		int realPairsCount = 0;;
+		for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && *iter>=0))
+			{
+				std::list<T> ptsA = uValues(wordsA, *iter);
+				std::list<T> ptsB = uValues(wordsB, *iter);
+
+				realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+
+				for(typename std::list<T>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
+				{
+					for(typename std::list<T>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
+					{
+						pairs.push_back(std::pair<int, std::pair<T, T> >(*iter, std::pair<T, T>(*jter, *kter)));
+					}
+				}
+			}
+		}
+		return realPairsCount;
+	}

 	static cv::Mat linearLSTriangulation(
 			cv::Point3d u,    //homogenous image point (u,v,1)
@@ -61,6 +61,7 @@ typedef cv::gpu::FAST_GPU CV_FAST_GPU;
 namespace cv{
 namespace xfeatures2d {
 class FREAK;
+class DAISY;
 class BriefDescriptorExtractor;
 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 class SIFT;
@@ -81,6 +82,7 @@ typedef cv::SIFT CV_SIFT; // SIFT is back in features2d since 4.4.0 / 3.4.11
 typedef cv::xfeatures2d::SURF CV_SURF;
 typedef cv::FastFeatureDetector CV_FAST;
 typedef cv::xfeatures2d::FREAK CV_FREAK;
+typedef cv::xfeatures2d::DAISY CV_DAISY;
 typedef cv::GFTTDetector CV_GFTT;
 typedef cv::xfeatures2d::BriefDescriptorExtractor CV_BRIEF;
 typedef cv::BRISK CV_BRISK;
@@ -115,7 +117,11 @@ public:
 		kFeatureGfttOrb=8,  //new 0.10.11
 		kFeatureKaze=9,     //new 0.13.2
 		kFeatureOrbOctree=10, //new 0.19.2
-		kFeatureSuperPointTorch=11}; //new 0.19.7
+		kFeatureSuperPointTorch=11, //new 0.19.7
+		kFeatureSurfFreak=12, //new 0.20.4
+		kFeatureGfttDaisy=13, //new 0.20.6
+		kFeatureSurfDaisy=14};  //new 0.20.6
+
 	static std::string typeName(Type type)
 	{
 		switch(type){
@@ -143,6 +149,12 @@ public:
 			return "ORB-OCTREE";
 		case kFeatureSuperPointTorch:
 			return "SUPERPOINT";
+		case kFeatureSurfFreak:
+			return "SURF+Freak";
+		case kFeatureGfttDaisy:
+			return "GFTT+Daisy";
+		case kFeatureSurfDaisy:
+			return "SURF+Daisy";
 		default:
 			return "Unknown";
 		}
@@ -455,6 +467,28 @@ private:
 	cv::Ptr<CV_FREAK> _freak;
 };

+//SURF_FREAK
+class RTABMAP_EXP SURF_FREAK : public SURF
+{
+public:
+	SURF_FREAK(const ParametersMap & parameters = ParametersMap());
+	virtual ~SURF_FREAK();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureSurfFreak;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	bool orientationNormalized_;
+	bool scaleNormalized_;
+	float patternScale_;
+	int nOctaves_;
+
+	cv::Ptr<CV_FREAK> _freak;
+};
+
 //GFTT_ORB
 class RTABMAP_EXP GFTT_ORB : public GFTT
 {
@@ -538,6 +572,8 @@ private:
 private:
 	float scaleFactor_;
 	int nLevels_;
+	int patchSize_;
+	int edgeThreshold_;
 	int fastThreshold_;
 	int fastMinThreshold_;

@@ -568,6 +604,54 @@ private:
 	bool cuda_;
 };

+//GFTT_DAISY
+class RTABMAP_EXP GFTT_DAISY : public GFTT
+{
+public:
+	GFTT_DAISY(const ParametersMap & parameters = ParametersMap());
+	virtual ~GFTT_DAISY();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureGfttDaisy;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	bool orientationNormalized_;
+	bool scaleNormalized_;
+	float patternScale_;
+	int nOctaves_;
+
+#if CV_MAJOR_VERSION > 2
+	cv::Ptr<CV_DAISY> _daisy;
+#endif
+};
+
+//SURF_DAISY
+class RTABMAP_EXP SURF_DAISY : public SURF
+{
+public:
+	SURF_DAISY(const ParametersMap & parameters = ParametersMap());
+	virtual ~SURF_DAISY();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureSurfDaisy;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	bool orientationNormalized_;
+	bool scaleNormalized_;
+	float patternScale_;
+	int nOctaves_;
+
+#if CV_MAJOR_VERSION > 2
+	cv::Ptr<CV_DAISY> _daisy;
+#endif
+};
+
 }

 #endif /* FEATURES2D_H_ */
@@ -43,8 +43,8 @@ public:
 	void release();
 	unsigned int indexedFeatures() const;

-	// return KB
-	unsigned int memoryUsed() const;
+	// return Bytes
+	unsigned long memoryUsed() const;

 	// Note that useDistanceL1 doesn't have any effect if LSH is used
 	void buildLinearIndex(
@@ -74,7 +74,7 @@ public:
 	int featuresType() const {return featuresType_;}
 	int featuresDim() const {return featuresDim_;}

-	unsigned int addPoints(const cv::Mat & features);
+	std::vector<unsigned int> addPoints(const cv::Mat & features);

 	void removePoint(unsigned int index);

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Link.h>
 #include <rtabmap/core/GPS.h>
+#include <rtabmap/core/CameraModel.h>

 namespace rtabmap {
 class Memory;
@@ -55,10 +56,10 @@ namespace graph {

 bool RTABMAP_EXP importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
+		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
-		std::map<int, double> * stamps = 0); // optional for format 1
+		std::map<int, double> * stamps = 0); // optional for format 1 and 9

 bool RTABMAP_EXP exportGPS(
 		const std::string & filePath,
@@ -155,12 +156,20 @@ std::list<Link> RTABMAP_EXP findLinks(

 std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
 		const std::multimap<int, Link> & links);
+/**
+ * Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
+ */
 std::multimap<int, Link> RTABMAP_EXP filterLinks(
 		const std::multimap<int, Link> & links,
-		Link::Type filteredType);
+		Link::Type filteredType,
+		bool inverted = false);
+/**
+ * Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
+ */
 std::map<int, Link> RTABMAP_EXP filterLinks(
 		const std::map<int, Link> & links,
-		Link::Type filteredType);
+		Link::Type filteredType,
+		bool inverted = false);

 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
 std::map<int, Transform> RTABMAP_EXP frustumPosesFiltering(
@@ -255,11 +264,26 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
 		float linearVelocity = 0.0f,   // m/sec
 		float angularVelocity = 0.0f); // rad/sec

+/**
+ * Get the nearest node of the target pose
+ * @param nodes the nodes to search for
+ * @param targetPose the target pose to search around
+ * @param distance squared distance of the nearest node found (optional)
+ * @return the node id.
+ */
 int RTABMAP_EXP findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
-		const rtabmap::Transform & targetPose);
+		const rtabmap::Transform & targetPose,
+		float * distance = 0);

-std::vector<int> RTABMAP_EXP findNearestNodes(
+/**
+ * Get K nearest nodes of the target pose
+ * @param nodes the nodes to search for
+ * @param targetPose the target pose to search around
+ * @param k number of nearest neighbors to search for
+ * @return the node ids with squared distance to target pose.
+ */
+std::map<int, float> RTABMAP_EXP findNearestNodes(
 		const std::map<int, rtabmap::Transform> & nodes,
 		const rtabmap::Transform & targetPose,
 		int k);
@@ -275,11 +299,20 @@ std::map<int, float> RTABMAP_EXP getNodesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
 		float radius);
+std::map<int, float> RTABMAP_EXP getNodesInRadius(
+		const Transform & targetPose,
+		const std::map<int, Transform> & nodes,
+		float radius);
 std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
 		float radius,
 		float angle = 0.0f);
+std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
+		const Transform & targetPose,
+		const std::map<int, Transform> & nodes,
+		float radius,
+		float angle = 0.0f);

 float RTABMAP_EXP computePathLength(
 		const std::vector<std::pair<int, Transform> > & path,
@@ -97,6 +97,7 @@ public:
 	float angleIncrement() const {return angleIncrement_;}
 	Transform localTransform() const {return localTransform_;}

+	bool empty() const {return data_.empty();}
 	bool isEmpty() const {return data_.empty();}
 	int size() const {return data_.cols;}
 	int dataType() const {return data_.type();}
@@ -50,6 +50,8 @@ private:
 	cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
 	float markerLength_;
 	float maxDepthError_;
+	float maxRange_;
+	float minRange_;
 	int dictionaryId_;
 	cv::Ptr<cv::aruco::Dictionary> dictionary_;
 #endif
@@ -102,7 +102,7 @@ public:
 	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
 	void saveOptimizedMesh(
 			const cv::Mat & cloud,
-			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
+			const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons = std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > >(),      // Textures -> polygons -> vertices
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
 #else
@@ -110,7 +110,7 @@ public:
 #endif
 			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size)
 	cv::Mat loadOptimizedMesh(
-			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
+			std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons = 0,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
 #else
@@ -199,9 +199,10 @@ public:
 	cv::Mat getImageCompressed(int signatureId) const;
 	SensorData getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const;
 	void getNodeWordsAndGlobalDescriptors(int nodeId,
-			std::multimap<int, cv::KeyPoint> & words,
-			std::multimap<int, cv::Point3f> & words3,
-			std::multimap<int, cv::Mat> & wordsDescriptors,
+			std::multimap<int, int> & words,
+			std::vector<cv::KeyPoint> & wordsKpts,
+			std::vector<cv::Point3f> & words3,
+			cv::Mat & wordsDescriptors,
 			std::vector<GlobalDescriptor> & globalDescriptors) const;
 	void getNodeCalibration(int nodeId,
 			std::vector<CameraModel> & models,
@@ -225,6 +226,7 @@ public:
 	virtual void dumpMemory(std::string directory) const;
 	virtual void dumpSignatures(const char * fileNameSign, bool words3D) const;
 	void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
+	unsigned long getMemoryUsed() const; //Bytes

 	void generateGraph(const std::string & fileName, const std::set<int> & ids = std::set<int>());

@@ -104,6 +104,8 @@ public:
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}

+	unsigned long getMemoryUsed() const;
+
 private:
 	ParametersMap parameters_;
 	int cloudDecimation_;
@@ -168,9 +168,6 @@ class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase <RtabmapColorOcTr
  };

 class RTABMAP_EXP OctoMap {
-public:
-	static void HSVtoRGB(float *r, float *g, float *b, float h, float s, float v);
-
 public:
 	OctoMap(const ParametersMap & parameters);
 	OctoMap(float cellSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false, float updateError=0.01f);
@@ -236,6 +236,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Kp, IncrementalDictionary,    bool, true,   "");
    RTABMAP_PARAM(Kp, IncrementalFlann,         bool, true,   uFormat("When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary increases of the factor \"%s\" in size).", kKpFlannRebalancingFactor().c_str()));
    RTABMAP_PARAM(Kp, FlannRebalancingFactor,   float, 2.0,   uFormat("Factor used when rebuilding the incremental FLANN index (see \"%s\"). Set <=1 to disable.", kKpIncrementalFlann().c_str()));
+    RTABMAP_PARAM(Kp, ByteToFloat,              bool, false,  uFormat("For %s=1, binary descriptors are converted to float by converting each byte to float instead of converting each bit to float. When converting bytes instead of bits, less memory is used and search is faster at the cost of slightly less accurate matching.", kKpNNStrategy().c_str()));
    RTABMAP_PARAM(Kp, MaxDepth,                 float, 0,     "Filter extracted keypoints by depth (0=inf).");
    RTABMAP_PARAM(Kp, MinDepth,                 float, 0,     "Filter extracted keypoints by depth.");
    RTABMAP_PARAM(Kp, MaxFeatures,              int, 500,     "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
@@ -243,9 +244,9 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Kp, NndrRatio,                float, 0.8,   "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
 #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
    // OpenCV>2 without xFeatures2D module doesn't have BRIEF
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 8,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 8,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY");
 #else
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY");
 #endif
    RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed,      bool, true,   "Use of the td-idf strategy to compute the likelihood.");
    RTABMAP_PARAM(Kp, Parallelized,             bool, true,   "If the dictionary update and signature creation were parallelized.");
@@ -264,6 +265,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(DbSqlite3, JournalMode,  int, 3,           "0=DELETE, 1=TRUNCATE, 2=PERSIST, 3=MEMORY, 4=OFF (see sqlite3 doc : \"PRAGMA journal_mode\")");
    RTABMAP_PARAM(DbSqlite3, Synchronous,  int, 0,           "0=OFF, 1=NORMAL, 2=FULL (see sqlite3 doc : \"PRAGMA synchronous\")");
    RTABMAP_PARAM(DbSqlite3, TempStore,    int, 2,           "0=DEFAULT, 1=FILE, 2=MEMORY (see sqlite3 doc : \"PRAGMA temp_store\")");
+    RTABMAP_PARAM_STR(Db, TargetVersion,   "",               "Target database version for backward compatibility purpose. Only Major and minor versions are used and should be set (e.g., 0.19 vs 0.20 or 1.0 vs 2.0). Patch version is ignored (e.g., 0.20.1 and 0.20.3 will generate a 0.20 database).");

    // Keypoints descriptors/detectors
    RTABMAP_PARAM(SURF, Extended,          bool, false,  "Extended descriptor flag (true - use extended 128-element descriptors; false - use 64-element descriptors).");
@@ -299,9 +301,9 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false,  "");
    RTABMAP_PARAM(GFTT, K,                 double, 0.04, "");

-    RTABMAP_PARAM(ORB, ScaleFactor,   float, 1.2,  "Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.");
-    RTABMAP_PARAM(ORB, NLevels,       int, 8,      "The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).");
-    RTABMAP_PARAM(ORB, EdgeThreshold, int, 31,     "This is size of the border where the features are not detected. It should roughly match the patchSize parameter.");
+    RTABMAP_PARAM(ORB, ScaleFactor,   float, 2,  "Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.");
+    RTABMAP_PARAM(ORB, NLevels,       int, 3,      "The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).");
+    RTABMAP_PARAM(ORB, EdgeThreshold, int, 19,     "This is size of the border where the features are not detected. It should roughly match the patchSize parameter.");
    RTABMAP_PARAM(ORB, FirstLevel,    int, 0,      "It should be 0 in the current implementation.");
    RTABMAP_PARAM(ORB, WTA_K,         int, 2,      "The number of points that produce each element of the oriented BRIEF descriptor. The default value 2 means the BRIEF where we take a random point pair and compare their brightnesses, so we get 0/1 response. Other possible values are 3 and 4. For example, 3 means that we take 3 random points (of course, those point coordinates are random, but they are generated from the pre-defined seed, so each element of BRIEF descriptor is computed deterministically from the pixel rectangle), find point of maximum brightness and output index of the winner (0, 1 or 2). Such output will occupy 2 bits, and therefore it will need a special variant of Hamming distance, denoted as NORM_HAMMING2 (2 bits per bin). When WTA_K=4, we take 4 random points to compute each bin (that will also occupy 2 bits with possible values 0, 1, 2 or 3).");
    RTABMAP_PARAM(ORB, ScoreType,     int, 0,      "The default HARRIS_SCORE=0 means that Harris algorithm is used to rank features (the score is written to KeyPoint::score and is used to retain best nfeatures features); FAST_SCORE=1 is alternative value of the parameter that produces slightly less stable keypoints, but it is a little faster to compute.");
@@ -586,9 +588,9 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Vis, Iterations,               int, 300,      "Maximum iterations to compute the transform.");
 #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
    // OpenCV>2 without xFeatures2D module doesn't have BRIEF
-    RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
+    RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY");
 #else
-    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
+    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY");
 #endif
    RTABMAP_PARAM(Vis, MaxFeatures,              int, 1000,   "0 no limits.");
    RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,   "Max depth of the features (0 means no limit).");
@@ -602,7 +604,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Vis, GridCols,                 int, 1,      uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
    RTABMAP_PARAM(Vis, CorType,                  int, 0,      "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
    RTABMAP_PARAM(Vis, CorNNType,                int, 1,    uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4, BruteForceCrossCheck=5, SuperGlue=6, GMS=7. Used for features matching approach.", kVisCorType().c_str()));
-    RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.6,  uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.8,  uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str()));
    RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 20,     uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
    RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool, false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str()));
    RTABMAP_PARAM(Vis, CorFlowWinSize,           int, 16,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
@@ -642,13 +644,15 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Icp, Epsilon,                   float, 0,     "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
    RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.1,   "Ratio of matching correspondences to accept the transform.");
 #ifdef RTABMAP_POINTMATCHER
-    RTABMAP_PARAM(Icp, PointToPlane,              bool, true,   "Use point to plane ICP.");
+    RTABMAP_PARAM(Icp, PointToPlane,                bool, true,   "Use point to plane ICP.");
 #else
-    RTABMAP_PARAM(Icp, PointToPlane,              bool, false,  "Use point to plane ICP.");
+    RTABMAP_PARAM(Icp, PointToPlane,                bool, false,  "Use point to plane ICP.");
 #endif
-    RTABMAP_PARAM(Icp, PointToPlaneK,             int, 5,       "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
-    RTABMAP_PARAM(Icp, PointToPlaneRadius,        float, 1.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
-    RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.02,  "Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.");
+    RTABMAP_PARAM(Icp, PointToPlaneK,               int, 5,       "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneRadius,          float, 1.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneGroundNormalsUp, float, 0.0,   "Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans.");
+    RTABMAP_PARAM(Icp, PointToPlaneMinComplexity,   float, 0.02,  uFormat("Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when %s=true.", kIcpPointToPlaneLowComplexityStrategy().c_str(), kIcpPointToPlane().c_str()));
+    RTABMAP_PARAM(Icp, PointToPlaneLowComplexityStrategy, int, 1, uFormat("If structural complexity is below %s: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept \"as is\" the transform computed by PointToPoint.", kIcpPointToPlaneMinComplexity().c_str()));

    // libpointmatcher
 #ifdef RTABMAP_POINTMATCHER
@@ -659,6 +663,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM_STR(Icp, PMConfig,             "",            uFormat("Configuration file (*.yaml) used by libpointmatcher. Note that data filters set for libpointmatcher are done after filtering done by rtabmap (i.e., %s, %s), so make sure to disable those in rtabmap if you want to use only those from libpointmatcher. Parameters %s, %s and %s are also ignored if configuration file is set.", kIcpVoxelSize().c_str(), kIcpDownsamplingStep().c_str(), kIcpIterations().c_str(), kIcpEpsilon().c_str(), kIcpMaxCorrespondenceDistance().c_str()).c_str());
    RTABMAP_PARAM(Icp, PMMatcherKnn,             int, 1,        "KDTreeMatcher/knn: number of nearest neighbors to consider it the reference. For convenience when configuration file is not set.");
    RTABMAP_PARAM(Icp, PMMatcherEpsilon,         float, 0.0,    "KDTreeMatcher/epsilon: approximation to use for the nearest-neighbor search. For convenience when configuration file is not set.");
+    RTABMAP_PARAM(Icp, PMMatcherIntensity,       bool, false,   uFormat("KDTreeMatcher:  among nearest neighbors, keep only the one with the most similar intensity. This only work with %s>1.", kIcpPMMatcherKnn().c_str()));
    RTABMAP_PARAM(Icp, PMOutlierRatio,           float, 0.95,   "TrimmedDistOutlierFilter/ratio: For convenience when configuration file is not set. For kinect-like point cloud, use 0.65.");

    // Stereo disparity
@@ -752,6 +757,8 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Marker, VarianceLinear,         float, 0.001, "Linear variance to set on marker detections.");
    RTABMAP_PARAM(Marker, VarianceAngular,        float, 0.01,  "Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization.");
    RTABMAP_PARAM(Marker, CornerRefinementMethod, int,   0,     "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
+    RTABMAP_PARAM(Marker, MaxRange,               float, 0.0,    "Maximum range in which markers will be detected. <=0 for unlimited range.");
+    RTABMAP_PARAM(Marker, MinRange,               float, 0.0,    "Miniminum range in which markers will be detected. <=0 for unlimited range.");

    RTABMAP_PARAM(ImuFilter, MadgwickGain,                  double, 0.1,  "Gain of the filter. Higher values lead to faster convergence but more noise. Lower values lead to slower convergence but smoother signal, belongs in [0, 1].");
    RTABMAP_PARAM(ImuFilter, MadgwickZeta,                  double, 0.0,  "Gyro drift gain (approx. rad/s), belongs in [-1, 1].");
@@ -839,4 +846,3 @@ private:
 }

 #endif /* PARAMETERS_H_ */
-
@@ -69,11 +69,14 @@ private:
 	bool _pointToPlane;
 	int _pointToPlaneK;
 	float _pointToPlaneRadius;
+	float _pointToPlaneGroundNormalsUp;
 	float _pointToPlaneMinComplexity;
+	int _pointToPlaneLowComplexityStrategy;
 	bool _libpointmatcher;
 	std::string _libpointmatcherConfig;
 	int _libpointmatcherKnn;
 	float _libpointmatcherEpsilon;
+	bool _libpointmatcherIntensity;
 	float _libpointmatcherOutlierRatio;
 	void * _libpointmatcherICP;
 };
@@ -75,6 +75,7 @@ public:

 	// RegistrationVis
 	int inliers;
+	float inliersRatio;
 	float inliersMeanDistance;
 	float inliersDistribution;
 	std::vector<int> inliersIDs;
@@ -136,11 +136,9 @@ public:
 	std::map<int, int> getWeights() const;
 	int getTotalMemSize() const;
 	double getLastProcessTime() const {return _lastProcessTime;};
-	std::multimap<int, cv::KeyPoint> getWords(int locationId) const;
 	bool isInSTM(int locationId) const;
 	bool isIDsGenerated() const;
 	const Statistics & getStatistics() const;
-	//bool getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const;
 	const std::map<int, Transform> & getLocalOptimizedPoses() const {return _optimizedPoses;}
 	const std::multimap<int, Link> & getLocalConstraints() const {return _constraints;}
 	Transform getPose(int locationId) const;
@@ -198,6 +196,8 @@ public:
 			bool withGrid = false,
 			bool withWords = true,
 			bool withGlobalDescriptors = true) const;
+	std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius); // If radius=0, RGBD/LocalRadius is used. Can return landmarks.
+	std::map<int, Transform> getNodesInRadius(int nodeId, float radius); // If nodeId==0, return poses around latest node. If radius=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
 	int detectMoreLoopClosures(
 			float clusterRadius = 0.5f,
 			float clusterAngle = M_PI/6.0f,
@@ -310,6 +310,7 @@ private:
 	double _lastProcessTime;
 	bool _someNodesHaveBeenTransferred;
 	float _distanceTravelled;
+	float _distanceTravelledSinceLastLocalization;
 	bool _optimizeFromGraphEndChanged;

 	// Abstract classes containing all loop closure
@@ -275,7 +275,7 @@ public:
 	void setLandmarks(const Landmarks & landmarks) {_landmarks = landmarks;}
 	const Landmarks & landmarks() const {return _landmarks;}

-	long getMemoryUsed() const; // Return memory usage in Bytes
+	unsigned long getMemoryUsed() const; // Return memory usage in Bytes
 	/**
 	 * Clear compressed rgb/depth (left/right) images, compressed laser scan and compressed user data.
 	 * Raw data are kept is set.
@@ -104,19 +104,18 @@ public:

 	//visual words stuff
 	void removeAllWords();
-	void removeWord(int wordId);
 	void changeWordsRef(int oldWordId, int activeWordId);
-	void setWords(const std::multimap<int, cv::KeyPoint> & words);
+	void setWords(const std::multimap<int, int> & words, const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> & words3, const cv::Mat & descriptors);
 	bool isEnabled() const {return _enabled;}
 	void setEnabled(bool enabled) {_enabled = enabled;}
-	const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
+	const std::multimap<int, int> & getWords() const {return _words;}
+	const std::vector<cv::KeyPoint> & getWordsKpts() const {return _wordsKpts;}
 	int getInvalidWordsCount() const {return _invalidWordsCount;}
 	const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
-	const std::multimap<int, cv::Mat> & getWordsDescriptors() const {return _wordsDescriptors;}
-	void setWordsDescriptors(const std::multimap<int, cv::Mat> & descriptors) {_wordsDescriptors = descriptors;}
+	const cv::Mat & getWordsDescriptors() const {return _wordsDescriptors;}
+	void setWordsDescriptors(const cv::Mat & descriptors);

 	//metric stuff
-	void setWords3(const std::multimap<int, cv::Point3f> & words3) {_words3 = words3;}
 	void setPose(const Transform & pose) {_pose = pose;}
 	void setGroundTruthPose(const Transform & pose) {_groundTruthPose = pose;}
 	void setVelocity(float vx, float vy, float vz, float vroll, float vpitch, float vyaw) {
@@ -129,7 +128,7 @@ public:
 		_velocity[5]=vyaw;
 	}

-	const std::multimap<int, cv::Point3f> & getWords3() const {return _words3;}
+	const std::vector<cv::Point3f> & getWords3() const {return _words3;}
 	const Transform & getPose() const {return _pose;}
 	cv::Mat getPoseCovariance() const;
 	const Transform & getGroundTruthPose() const {return _groundTruthPose;}
@@ -138,7 +137,7 @@ public:
 	SensorData & sensorData() {return _sensorData;}
 	const SensorData & sensorData() const {return _sensorData;}

-	long getMemoryUsed(bool withSensorData=true) const; // Return memory usage in Bytes
+	unsigned long getMemoryUsed(bool withSensorData=true) const; // Return memory usage in Bytes

 private:
 	int _id;
@@ -155,9 +154,10 @@ private:
 	// Contains all words (Some can be duplicates -> if a word appears 2
 	// times in the signature, it will be 2 times in this list)
 	// Words match with the CvSeq keypoints and descriptors
-	std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
-	std::multimap<int, cv::Point3f> _words3; // word <id, point> // in base_link frame (localTransform applied))
-	std::multimap<int, cv::Mat> _wordsDescriptors;
+	std::multimap<int, int> _words; // word <id, keypoint index>
+	std::vector<cv::KeyPoint> _wordsKpts;
+	std::vector<cv::Point3f> _words3; // in base_link frame (localTransform applied))
+	cv::Mat _wordsDescriptors;
 	std::map<int, int> _wordsChanged; // <oldId, newId>
 	bool _enabled;
 	int _invalidWordsCount;
@@ -65,7 +65,9 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Map_id,);
 	RTABMAP_STATS(Loop, Visual_words,);
 	RTABMAP_STATS(Loop, Visual_inliers,);
+	RTABMAP_STATS(Loop, Visual_inliers_ratio,);
 	RTABMAP_STATS(Loop, Visual_matches,);
+	RTABMAP_STATS(Loop, Distance_since_last_loc,);
 	RTABMAP_STATS(Loop, Last_id,);
 	RTABMAP_STATS(Loop, Optimization_max_error, m);
 	RTABMAP_STATS(Loop, Optimization_max_error_ratio, );
@@ -148,6 +150,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Odometry_variance_lin,);
 	RTABMAP_STATS(Memory, Distance_travelled, m);
 	RTABMAP_STATS(Memory, RAM_usage, MB);
+	RTABMAP_STATS(Memory, RAM_estimated, MB);
 	RTABMAP_STATS(Memory, Triangulated_points, );

 	RTABMAP_STATS(Timing, Memory_update, ms);
@@ -170,6 +173,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Timing, Joining_trash, ms);
 	RTABMAP_STATS(Timing, Emptying_trash, ms);
 	RTABMAP_STATS(Timing, Finalizing_statistics, ms);
+	RTABMAP_STATS(Timing, RAM_estimation, ms);

 	RTABMAP_STATS(TimingMem, Pre_update, ms);
 	RTABMAP_STATS(TimingMem, Signature_creation, ms);
@@ -98,6 +98,7 @@ public:

 	float theta() const;

+	bool isInvertible() const;
 	Transform inverse() const;
 	Transform rotation() const;
 	Transform translation() const;
@@ -100,8 +100,9 @@ public:
 	int getLastIndexedWordId() const;
 	int getTotalActiveReferences() const {return _totalActiveReferences;}
 	unsigned int getIndexedWordsCount() const;
-	unsigned int getIndexMemoryUsed() const;
-	void setNNStrategy(NNStrategy strategy);
+	unsigned int getIndexMemoryUsed() const; // KB
+	unsigned long getMemoryUsed() const; //Bytes
+	bool setNNStrategy(NNStrategy strategy); // Return true if the search tree has been re-initialized
 	bool isIncremental() const {return _incrementalDictionary;}
 	bool isIncrementalFlann() const {return _incrementalFlann;}
 	void setIncrementalDictionary();
@@ -117,8 +118,8 @@ public:
 	void deleteUnusedWords();

 public:
-	static cv::Mat convertBinTo32F(const cv::Mat & descriptorsIn);
-	static cv::Mat convert32FToBin(const cv::Mat & descriptorsIn);
+	static cv::Mat convertBinTo32F(const cv::Mat & descriptorsIn, bool byteToFloat = true);
+	static cv::Mat convert32FToBin(const cv::Mat & descriptorsIn, bool byteToFloat = true);

 protected:
 	int getNextId();
@@ -131,6 +132,7 @@ private:
 	bool _incrementalDictionary;
 	bool _incrementalFlann;
 	float _rebalancingFactor;
+	bool _byteToFloat;
 	float _nndrRatio;
 	std::string _dictionaryPath; // a pre-computed dictionary (.txt or .db)
 	std::string _newDictionaryPath; // a pre-computed dictionary (.txt or .db)
@@ -43,6 +43,7 @@ public:

 	void addRef(int signatureId);
 	int removeAllRef(int signatureId);
+	unsigned long getMemoryUsed() const;

 	int getTotalReferences() const {return _totalReferences;}
 	int id() const {return _id;}
@@ -74,6 +74,11 @@ public:
 		_syncImageRateWithStamps = syncImageRateWithStamps;
 	}

+	void setConfigForEachFrame(bool value)
+	{
+		_hasConfigForEachFrame = value;
+	}
+
 	void setScanPath(
 			const std::string & dir,
 			int maxScanPts = 0,
@@ -116,12 +121,14 @@ public:

 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
+
+private:
 	bool readPoses(
-			std::list<Transform> & outputPoses,
-			std::list<double> & stamps,
-			const std::string & filePath,
-			int format,
-			double maxTimeDiff) const;
+		std::list<Transform> & outputPoses,
+		std::list<double> & stamps,
+		const std::string & filePath,
+		int format,
+		double maxTimeDiff) const;

 private:
 	std::string _path;
@@ -151,6 +158,7 @@ private:
 	bool _depthFromScanFillHolesFromBorder;

 	bool _filenamesAreTimestamps;
+	bool _hasConfigForEachFrame;
 	std::string _timestampsPath;
 	bool _syncImageRateWithStamps;

@@ -162,8 +170,10 @@ private:

 	std::list<double> _stamps;
 	std::list<Transform> odometry_;
+	std::list<cv::Mat> covariances_;
 	std::list<Transform> groundTruth_;
 	CameraModel _model;
+	std::list<CameraModel> _models;

 	UTimer _captureTimer;
 	double _captureDelay;
@@ -35,7 +35,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UTimer.h"

 #ifdef RTABMAP_K4A
- #include <k4a/k4atypes.h>
+#include <k4a/k4atypes.h>
+#include <k4arecord/playback.h>
 #endif

 namespace rtabmap
@@ -72,15 +73,14 @@ private:
 private:

 #ifdef RTABMAP_K4A
-	k4a_device_t device_;
+	k4a_device_t deviceHandle_;
 	k4a_device_configuration_t config_;
 	k4a_calibration_t calibration_;
-	k4a_transformation_t transformation_;
-	k4a_capture_t capture_;
+	k4a_transformation_t transformationHandle_;
+	k4a_capture_t captureHandle_;
+	k4a_playback_t playbackHandle_;
 	std::string serial_number_;

-	void* playbackHandle_;
-	void* transformationHandle_;
 	CameraModel model_;
 	int deviceId_;
 	std::string fileName_;
@@ -48,8 +48,6 @@ public:
 	virtual ~CameraRGBDImages();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;

 	virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);cameraDepth_.setStartIndex(index);} // negative means last
 	virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);cameraDepth_.setMaxFrames(value);}
@@ -75,6 +75,7 @@ public:
 	void setEmitterEnabled(bool enabled);
 	void setIRFormat(bool enabled, bool useDepthInsteadOfRightImage);
 	void setResolution(int width, int height, int fps = 30);
+	void setGlobalTimeSync(bool enabled);
 	void publishInterIMU(bool enabled);
 	void setDualMode(bool enabled, const Transform & extrinsics);
 	void setJsonConfig(const std::string & json);
@@ -93,7 +94,7 @@ private:
 			Transform & pose,
 			unsigned int & poseConfidence,
 			IMU & imu,
-			int maxWaitTimeMs = 35) const;
+			int maxWaitTimeMs = 35);
 #endif

 protected:
@@ -121,6 +122,7 @@ private:
 	UMutex imuMutex_;
 	double lastImuStamp_;
 	bool clockSyncWarningShown_;
+	bool imuGlobalSyncWarningShown_;

 	bool emitterEnabled_;
 	bool ir_;
@@ -130,11 +132,13 @@ private:
 	int cameraWidth_;
 	int cameraHeight_;
 	int cameraFps_;
+	bool globalTimeSync_;
 	bool publishInterIMU_;
 	bool dualMode_;
 	Transform dualExtrinsics_;
 	std::string jsonConfig_;
 	bool closing_;
+	bool isL500_;

 	static Transform realsense2PoseRotation_;
 	static Transform realsense2PoseRotationInv_;
@@ -97,28 +97,35 @@ void segmentObstaclesFromGround(
 			// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
 			if(clusteredFlatSurfaces.size())
 			{
-				Eigen::Vector4f min,max;
+				Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax;
 				if(maxGroundHeight != 0.0f)
 				{
 					// Search for biggest surface under max ground height
 					size_t points = 0;
+					biggestFlatSurfaceIndex = -1;
 					for(size_t i=0;i<clusteredFlatSurfaces.size();++i)
 					{
+						Eigen::Vector4f min,max;
 						pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(i), min, max);
 						if(min[2]<maxGroundHeight && clusteredFlatSurfaces.size() > points)
 						{
 							points = clusteredFlatSurfaces.at(i)->size();
 							biggestFlatSurfaceIndex = i;
+							biggestSurfaceMin = min;
+							biggestSurfaceMax = max;
 						}
 					}
 				}
 				else
 				{
-					pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
+					pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), biggestSurfaceMin, biggestSurfaceMax);
+				}
+				if(biggestFlatSurfaceIndex>=0)
+				{
+					ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
 				}
-				ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);

-				if(!ground->empty() && (maxGroundHeight == 0.0f || min[2] < maxGroundHeight))
+				if(!ground->empty() && (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight))
 				{
 					for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
 					{
@@ -126,7 +133,7 @@ void segmentObstaclesFromGround(
 						{
 							Eigen::Vector4f centroid(0,0,0,1);
 							pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
-							if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight || centroid[2] <= max[2]) // epsilon
+							if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight || centroid[2] <= biggestSurfaceMax[2]) // epsilon
 							{
 								ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
 							}
@@ -9,6 +9,7 @@
 #define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_

 #include <pcl/search/kdtree.h>
+#include <pcl/conversions.h>
 #include <rtabmap/utilite/UConversion.h>

 namespace rtabmap {
@@ -78,7 +78,7 @@ private:
 	Signature * map_;
 	Signature * lastFrame_;
 	int lastFrameOldestNewId_;
-	std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
+	std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
 	bool initGravity_;

 	std::map<int, std::map<int, FeatureBA> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>
@@ -154,6 +154,8 @@ cv::Mat RTABMAP_EXP brightnessAndContrastAuto(
 cv::Mat RTABMAP_EXP exposureFusion(
 	const std::vector<cv::Mat> & images);

+void RTABMAP_EXP HSVtoRGB( float *r, float *g, float *b, float h, float s, float v );
+
 } // namespace util3d
 } // namespace rtabmap

@@ -56,7 +56,16 @@ LaserScan RTABMAP_EXP commonFiltering(
 		float voxelSize = 0.0f,
 		int normalK = 0,
 		float normalRadius = 0.0f,
-		bool forceGroundNormalsUp = false);
+		float groundNormalsUp = 0.0f);
+RTABMAP_DEPRECATED(LaserScan RTABMAP_EXP commonFiltering(
+		const LaserScan & scan,
+		int downsamplingStep,
+		float rangeMin,
+		float rangeMax,
+		float voxelSize,
+		int normalK,
+		float normalRadius,
+		bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp=0.8, otherwise set groundNormalsUp=0.0.");

 LaserScan RTABMAP_EXP rangeFiltering(
 		const LaserScan & scan,
@@ -288,6 +297,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cropBox(
 		const Eigen::Vector4f & max,
 		const Transform & transform = Transform::getIdentity(),
 		bool negative = false);
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const Eigen::Vector4f & min,
@@ -451,6 +466,12 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
 /**
 * For convenience.
 */
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
+		float radiusSearch,
+		float maxAngle = M_PI/4.0f,
+		int minNeighborsInRadius = 1);
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
@@ -467,6 +488,14 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP subtractFiltering(
 * @param radiusSearch the radius in meter.
 * @return the indices of the points satisfying the parameters.
 */
+pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
+		const pcl::IndicesPtr & substractIndices,
+		float radiusSearch,
+		float maxAngle = M_PI/4.0f,
+		int minNeighborsInRadius = 1);
 pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
@@ -66,12 +66,25 @@ void RTABMAP_EXP computeVarianceAndCorrespondences(
 		double maxCorrespondenceAngle, // <=0 means that we don't care about normal angle difference
 		double & variance,
 		int & correspondencesOut);
+void RTABMAP_EXP computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double maxCorrespondenceAngle, // <=0 means that we don't care about normal angle difference
+		double & variance,
+		int & correspondencesOut);
 void RTABMAP_EXP computeVarianceAndCorrespondences(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
 		double maxCorrespondenceDistance,
 		double & variance,
 		int & correspondencesOut);
+void RTABMAP_EXP computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double & variance,
+		int & correspondencesOut);

 Transform RTABMAP_EXP icp(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
@@ -82,6 +95,15 @@ Transform RTABMAP_EXP icp(
 		pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
 		float epsilon = 0.0f,
 		bool icp2D = false);
+Transform RTABMAP_EXP icp(
+		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_source,
+		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_target,
+		double maxCorrespondenceDistance,
+		int maximumIterations,
+		bool & hasConverged,
+		pcl::PointCloud<pcl::PointXYZI> & cloud_source_registered,
+		float epsilon = 0.0f,
+		bool icp2D = false);

 Transform RTABMAP_EXP icpPointToPlane(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
@@ -92,6 +114,15 @@ Transform RTABMAP_EXP icpPointToPlane(
 		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
 		float epsilon = 0.0f,
 		bool icp2D = false);
+Transform RTABMAP_EXP icpPointToPlane(
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_source,
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_target,
+		double maxCorrespondenceDistance,
+		int maximumIterations,
+		bool & hasConverged,
+		pcl::PointCloud<pcl::PointXYZINormal> & cloud_source_registered,
+		float epsilon = 0.0f,
+		bool icp2D = false);

 } // namespace util3d
 } // namespace rtabmap
@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/CameraModel.h>
 #include <rtabmap/core/ProgressState.h>
 #include <rtabmap/core/LaserScan.h>
+#include <rtabmap/core/Version.h>
 #include <set>
 #include <list>

@@ -148,7 +149,8 @@ pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
 		int minClusterSize = 50, // minimum size of polygons clusters textured
 		const std::vector<float> & roiRatios = std::vector<float>(), // [left, right, top, bottom] region of interest (in ratios) of the image projected.
 		const ProgressState * state = 0,
-		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0);
+		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0, // For each point, we have a list of cameras with corresponding pixel in it. Beware that the camera ids don't correspond to pose ids, they are indexes from 0 to total camera models and texture's materials.
+		bool distanceToCamPolicy = false);
 pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
 		const pcl::PolygonMesh::Ptr & mesh,
 		const std::map<int, Transform> & poses,
@@ -160,7 +162,8 @@ pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
 		int minClusterSize = 50, // minimum size of polygons clusters textured
 		const std::vector<float> & roiRatios = std::vector<float>(), // [left, right, top, bottom] region of interest (in ratios) of the image projected.
 		const ProgressState * state = 0,
-		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0);
+		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0, // For each point, we have a list of cameras with corresponding pixel in it. Beware that the camera ids don't correspond to pose ids, they are indexes from 0 to total camera models and texture's materials.
+		bool distanceToCamPolicy = false);

 /**
 * Remove not textured polygon clusters. If minClusterSize<0, only the largest cluster is kept.
@@ -175,18 +178,18 @@ pcl::TextureMesh::Ptr RTABMAP_EXP concatenateTextureMeshes(
 void RTABMAP_EXP concatenateTextureMaterials(
 		pcl::TextureMesh & mesh, const cv::Size & imageSize, int textureSize, int maxTextures, float & scale, std::vector<bool> * materialsKept=0);

-std::vector<std::vector<unsigned int> > RTABMAP_EXP convertPolygonsFromPCL(
+std::vector<std::vector<RTABMAP_PCL_INDEX> > RTABMAP_EXP convertPolygonsFromPCL(
 		const std::vector<pcl::Vertices> & polygons);
-std::vector<std::vector<std::vector<unsigned int> > > RTABMAP_EXP convertPolygonsFromPCL(
+std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > RTABMAP_EXP convertPolygonsFromPCL(
 		const std::vector<std::vector<pcl::Vertices> > & polygons);
 std::vector<pcl::Vertices> RTABMAP_EXP convertPolygonsToPCL(
-		const std::vector<std::vector<unsigned int> > & polygons);
+		const std::vector<std::vector<RTABMAP_PCL_INDEX> > & polygons);
 std::vector<std::vector<pcl::Vertices> > RTABMAP_EXP convertPolygonsToPCL(
-		const std::vector<std::vector<std::vector<unsigned int> > > & tex_polygons);
+		const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & tex_polygons);

 pcl::TextureMesh::Ptr RTABMAP_EXP assembleTextureMesh(
 		const cv::Mat & cloudMat,
-		const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+		const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 		const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
 #else
@@ -197,7 +200,7 @@ pcl::TextureMesh::Ptr RTABMAP_EXP assembleTextureMesh(

 pcl::PolygonMesh::Ptr RTABMAP_EXP assemblePolygonMesh(
 		const cv::Mat & cloudMat,
-		const std::vector<std::vector<unsigned int> > & polygons);
+		const std::vector<std::vector<RTABMAP_PCL_INDEX> > & polygons);

 /**
 * Merge all textures in the mesh into "textureCount" textures of size "textureSize".
@@ -263,8 +266,9 @@ bool RTABMAP_EXP multiBandTexturing(
 		int textureSize = 8192,
 		const std::string & textureFormat = "jpg",    // png, jpg
 		const std::map<int, std::map<int, cv::Vec4d> > & gains = std::map<int, std::map<int, cv::Vec4d> >(),       // optional output of util3d::mergeTextures()
-		const std::map<int, std::map<int, cv::Mat> > & blendingGains = std::map<int, std::map<int, cv::Mat> >(),    // optional output of util3d::mergeTextures()
-		const std::pair<float, float> & contrastValues = std::pair<float, float>(0,0));               // optional output of util3d::mergeTextures()
+		const std::map<int, std::map<int, cv::Mat> > & blendingGains = std::map<int, std::map<int, cv::Mat> >(),   // optional output of util3d::mergeTextures()
+		const std::pair<float, float> & contrastValues = std::pair<float, float>(0,0),                             // optional output of util3d::mergeTextures()
+		bool gainRGB = true);

 cv::Mat RTABMAP_EXP computeNormals(
 		const cv::Mat & laserScan,
@@ -354,6 +358,12 @@ float RTABMAP_EXP computeNormalsComplexity(
 		bool is2d = false,
 		cv::Mat * pcaEigenVectors = 0,
 		cv::Mat * pcaEigenValues = 0);
+float RTABMAP_EXP computeNormalsComplexity(
+		const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
+		const Transform & t = Transform::getIdentity(),
+		bool is2d = false,
+		cv::Mat * pcaEigenVectors = 0,
+		cv::Mat * pcaEigenValues = 0);
 float RTABMAP_EXP computeNormalsComplexity(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
 		const Transform & t = Transform::getIdentity(),
@@ -383,18 +393,39 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
 		float dilationVoxelSize = 1.0f,  // VOXEL_GRID_DILATION
 		int dilationIterations = 0);     // VOXEL_GRID_DILATION

-LaserScan RTABMAP_EXP adjustNormalsToViewPoint(
+RTABMAP_DEPRECATED(LaserScan RTABMAP_EXP adjustNormalsToViewPoint(
 		const LaserScan & scan,
 		const Eigen::Vector3f & viewpoint,
-		bool forceGroundNormalsUp);
+		bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f.");
+LaserScan RTABMAP_EXP adjustNormalsToViewPoint(
+		const LaserScan & scan,
+		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
+		float groundNormalsUp = 0.0f);
+RTABMAP_DEPRECATED(void RTABMAP_EXP adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f.");
 void RTABMAP_EXP adjustNormalsToViewPoint(
 		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
 		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
-		bool forceGroundNormalsUp = false);
+		float groundNormalsUp = 0.0f);
+RTABMAP_DEPRECATED(void RTABMAP_EXP adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f.");
 void RTABMAP_EXP adjustNormalsToViewPoint(
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
-		bool forceGroundNormalsUp = false);
+		float groundNormalsUp = 0.0f);
+RTABMAP_DEPRECATED(void RTABMAP_EXP adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f.");
+void RTABMAP_EXP adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
+		float groundNormalsUp = 0.0f);
+
 void RTABMAP_EXP adjustNormalsToViewPoints(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
@@ -417,6 +417,20 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
 	return prediction;
 }

+unsigned long BayesFilter::getMemoryUsed() const
+{
+	long memoryUsage = sizeof(BayesFilter);
+	memoryUsage += _posterior.size() * (sizeof(float)+sizeof(int)+sizeof(std::map<int, float>::iterator)) + sizeof(std::map<int, float>);
+	memoryUsage += _prediction.total() * _prediction.elemSize();
+	memoryUsage += _predictionLC.size() * sizeof(double);
+	memoryUsage += _neighborsIndex.size() * (sizeof(int)+sizeof(std::map<int, int>)+sizeof(std::map<int, std::map<int, int> >::iterator)) + sizeof(std::map<int, std::map<int, int> >);
+	for(std::map<int, std::map<int, int> >::const_iterator iter=_neighborsIndex.begin(); iter!=_neighborsIndex.end(); ++iter)
+	{
+		memoryUsage += iter->second.size() * (sizeof(int)*2+sizeof(std::map<int, int>::iterator)) + sizeof(std::map<int, int>);
+	}
+	return memoryUsage;
+}
+
 void BayesFilter::normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const
 {
 	UASSERT(index < (unsigned int)prediction.rows && index < (unsigned int)prediction.cols);
@@ -592,18 +592,24 @@ ENDIF(WITH_MADGWICK)
 ####################################
 CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql.in ${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql)

-SET(R
+SET(RESOURCES
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_3.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_0.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_17_0.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_16_2.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_16_1.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_16_0.sql
 )

-#replace semicolons by spaces
-foreach(arg ${R})
-   set(RESOURCES "${RESOURCES}" "${arg}")
-endforeach(arg ${R})
+foreach(arg ${RESOURCES})
+   get_filename_component(filename ${arg} NAME)
+   string(REPLACE "." "_" output ${filename})
+   set(RESOURCES_HEADERS "${RESOURCES_HEADERS}" "${CMAKE_CURRENT_BINARY_DIR}/${output}.h")
+endforeach(arg ${RESOURCES})

-SET(RESOURCES_HEADERS 
-	${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h 
-)
+MESSAGE(STATUS "RESOURCES = ${RESOURCES}")
+MESSAGE(STATUS "RESOURCES_HEADERS = ${RESOURCES_HEADERS}")

 IF(ANDROID)

@@ -618,14 +624,14 @@ IF(ANDROID)
      OUTPUT ${RESOURCES_HEADERS}
      COMMAND ${RTABMAP_RES_TOOL} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
      COMMENT "[Creating resources]"
-      DEPENDS ${R}
+      DEPENDS ${RESOURCES}
   )
 ELSE()
   ADD_CUSTOM_COMMAND(
      OUTPUT ${RESOURCES_HEADERS}
      COMMAND ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/rtabmap-res_tool -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
      COMMENT "[Creating resources]"
-      DEPENDS ${R} res_tool
+      DEPENDS ${RESOURCES} res_tool
   )
 ENDIF()

@@ -765,4 +765,16 @@ bool CameraModel::inFrame(int u, int v) const
 	return uIsInBounds(u, 0, imageWidth()) && uIsInBounds(v, 0, imageHeight());
 }

+std::ostream& operator<<(std::ostream& os, const CameraModel& model)
+{
+	os << "Name: " << model.name() << std::endl
+	   << "Size: " << model.imageWidth() << "x" << model.imageHeight() << std::endl
+	   << "K= " << model.K_raw() << std::endl
+	   << "D= " << model.D_raw() << std::endl
+	   << "R= " << model.R() << std::endl
+	   << "P= " << model.P() << std::endl
+	   << "LocalTransform= " << model.localTransform();
+	return os;
+}
+
 } /* namespace rtabmap */
@@ -129,6 +129,39 @@ void CameraThread::disableIMUFiltering()
 	_imuFilter = 0;
 }

+void CameraThread::setScanParameters(
+	bool fromDepth,
+	int downsampleStep,
+	float rangeMin,
+	float rangeMax,
+	float voxelSize,
+	int normalsK,
+	int normalsRadius,
+	bool forceGroundNormalsUp)
+{
+	setScanParameters(fromDepth, downsampleStep, rangeMin, rangeMax, voxelSize, normalsK, normalsRadius, forceGroundNormalsUp?0.8f:0.0f);
+}
+
+void CameraThread::setScanParameters(
+			bool fromDepth,
+			int downsampleStep, // decimation of the depth image in case the scan is from depth image
+			float rangeMin,
+			float rangeMax,
+			float voxelSize,
+			int normalsK,
+			int normalsRadius,
+			float groundNormalsUp)
+{
+	_scanFromDepth = fromDepth;
+	_scanDownsampleStep=downsampleStep;
+	_scanRangeMin = rangeMin;
+	_scanRangeMax = rangeMax;
+	_scanVoxelSize = voxelSize;
+	_scanNormalsK = normalsK;
+	_scanNormalsRadius = normalsRadius;
+	_scanForceGroundNormalsUp = groundNormalsUp;
+}
+
 void CameraThread::mainLoopBegin()
 {
 	ULogger::registerCurrentThread("Camera");
@@ -237,7 +270,26 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 		else
 		{
 			cv::Mat image = util2d::decimate(data.imageRaw(), _imageDecimation);
-			cv::Mat depthOrRight = util2d::decimate(data.depthOrRightRaw(), _imageDecimation);
+
+			int depthDecimation = _imageDecimation;
+			if(data.depthOrRightRaw().rows <= image.rows || data.depthOrRightRaw().cols <= image.cols)
+			{
+				depthDecimation = 1;
+			}
+			else
+			{
+				depthDecimation = 2;
+				while(data.depthOrRightRaw().rows / depthDecimation > image.rows ||
+					  data.depthOrRightRaw().cols / depthDecimation > image.cols ||
+					  data.depthOrRightRaw().rows % depthDecimation != 0 ||
+					  data.depthOrRightRaw().cols % depthDecimation != 0)
+				{
+					++depthDecimation;
+				}
+				UDEBUG("depthDecimation=%d", depthDecimation);
+			}
+			cv::Mat depthOrRight = util2d::decimate(data.depthOrRightRaw(), depthDecimation);
+
 			std::vector<CameraModel> models = data.cameraModels();
 			for(unsigned int i=0; i<models.size(); ++i)
 			{
@@ -40,7 +40,6 @@ namespace rtabmap {

 DBDriver * DBDriver::create(const ParametersMap & parameters)
 {
-	// well, we only have Sqlite3 database type for now :P
 	return new DBDriverSqlite3(parameters);
 }

@@ -59,6 +58,7 @@ DBDriver::~DBDriver()

 void DBDriver::parseParameters(const ParametersMap & parameters)
 {
+	Parameters::parse(parameters, Parameters::kDbTargetVersion(), _targetVersion);
 }

 void DBDriver::closeConnection(bool save, const std::string & outputUrl)
@@ -107,9 +107,9 @@ bool DBDriver::isConnected() const
 }

 // In bytes
-long DBDriver::getMemoryUsed() const
+unsigned long DBDriver::getMemoryUsed() const
 {
-	long bytes;
+	unsigned long bytes;
 	_dbSafeAccessMutex.lock();
 	bytes = getMemoryUsedQuery();
 	_dbSafeAccessMutex.unlock();
@@ -1209,7 +1209,7 @@ cv::Mat DBDriver::load2DMap(float & xMin, float & yMin, float & cellSize) const

 void DBDriver::saveOptimizedMesh(
 			const cv::Mat & cloud,
-			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+			const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
 #else
@@ -1223,7 +1223,7 @@ void DBDriver::saveOptimizedMesh(
 }

 cv::Mat DBDriver::loadOptimizedMesh(
-				std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+				std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 				std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
 #else
@@ -34,6 +34,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/Compression.h"
 #include "DatabaseSchema_sql.h"
+#include "DatabaseSchema_0_18_3_sql.h"
+#include "DatabaseSchema_0_18_0_sql.h"
+#include "DatabaseSchema_0_17_0_sql.h"
+#include "DatabaseSchema_0_16_2_sql.h"
+#include "DatabaseSchema_0_16_1_sql.h"
+#include "DatabaseSchema_0_16_0_sql.h"
+
+
 #include <set>

 #include "rtabmap/utilite/UtiLite.h"
@@ -383,6 +391,34 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 		}
 		// Create the database
 		std::string schema = DATABASESCHEMA_SQL;
+		std::string targetVersion = this->getTargetVersion();
+		if(!targetVersion.empty())
+		{
+			// search for schema with version <= target version
+			std::vector<std::pair<std::string, std::string> > schemas;
+			schemas.push_back(std::make_pair("0.16.0", DATABASESCHEMA_0_16_0_SQL));
+			schemas.push_back(std::make_pair("0.16.1", DATABASESCHEMA_0_16_1_SQL));
+			schemas.push_back(std::make_pair("0.16.2", DATABASESCHEMA_0_16_2_SQL));
+			schemas.push_back(std::make_pair("0.17.0", DATABASESCHEMA_0_17_0_SQL));
+			schemas.push_back(std::make_pair("0.18.0", DATABASESCHEMA_0_18_0_SQL));
+			schemas.push_back(std::make_pair("0.18.3", DATABASESCHEMA_0_18_3_SQL));
+			schemas.push_back(std::make_pair(uNumber2Str(RTABMAP_VERSION_MAJOR)+"."+uNumber2Str(RTABMAP_VERSION_MINOR), DATABASESCHEMA_SQL));
+			for(size_t i=0; i<schemas.size(); ++i)
+			{
+				if(uStrNumCmp(targetVersion, schemas[i].first) < 0)
+				{
+					if(i==0)
+					{
+						UERROR("Cannot create database with target version \"%s\" (not implemented), using latest version.", targetVersion.c_str());
+					}
+					break;
+				}
+				else
+				{
+					schema = schemas[i].second;
+				}
+			}
+		}
 		schema = uHex2Str(schema);
 		this->executeNoResultQuery(schema.c_str());
 	}
@@ -486,7 +522,7 @@ void DBDriverSqlite3::executeNoResultQuery(const std::string & sql) const
 	}
 }

-long DBDriverSqlite3::getMemoryUsedQuery() const
+unsigned long DBDriverSqlite3::getMemoryUsedQuery() const
 {
 	if(_dbInMemory)
 	{
@@ -2377,6 +2413,7 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 			query << "INNER JOIN Link ";
 			query << "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them)
 			query << "WHERE from_id != to_id "; // ignore self referring links
+			query << "AND weight>-9 "; //ignore invalid nodes
 		}

 		if(ignoreBadSignatures)
@@ -3078,9 +3115,10 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			const void * descriptor = 0;
 			int dRealSize = 0;
 			cv::KeyPoint kpt;
-			std::multimap<int, cv::KeyPoint> visualWords;
-			std::multimap<int, cv::Point3f> visualWords3;
-			std::multimap<int, cv::Mat> descriptors;
+			std::multimap<int, int> visualWords;
+			std::vector<cv::KeyPoint> visualWordsKpts;
+			std::vector<cv::Point3f> visualWords3;
+			cv::Mat descriptors;
 			bool allWords3NaN = true;
 			cv::Point3f depth(0,0,0);

@@ -3130,8 +3168,9 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 					depth.z = sqlite3_column_double(ppStmt, index++);
 				}

-				visualWords.insert(visualWords.end(), std::make_pair(visualWordId, kpt));
-				visualWords3.insert(visualWords3.end(), std::make_pair(visualWordId, depth));
+				visualWordsKpts.push_back(kpt);
+				visualWords.insert(visualWords.end(), std::make_pair(visualWordId, visualWordsKpts.size()-1));
+				visualWords3.push_back(depth);

 				if(allWords3NaN && util3d::isFinite(depth))
 				{
@@ -3164,7 +3203,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<

 						memcpy(d.data, descriptor, dRealSize);

-						descriptors.insert(descriptors.end(), std::make_pair(visualWordId, d));
+						descriptors.push_back(d);
 					}
 				}

@@ -3178,13 +3217,12 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			}
 			else
 			{
-				(*iter)->setWords(visualWords);
-				if(!allWords3NaN)
+				if(allWords3NaN)
 				{
-					(*iter)->setWords3(visualWords3);
+					visualWords3.clear();
 				}
-				(*iter)->setWordsDescriptors(descriptors);
-				ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.size(), (*iter)->id());
+				(*iter)->setWords(visualWords, visualWordsKpts, visualWords3, descriptors);
+				ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id());
 			}

 			//reset
@@ -3622,6 +3660,7 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
 		int descriptorSize;
 		const void * descriptor;
 		int dRealSize;
+		unsigned long dRealSizeTotal = 0;
 		for(std::set<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
 		{
 			// bind id
@@ -3654,6 +3693,7 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
 				}

 				memcpy(d.data, descriptor, dRealSize);
+				dRealSizeTotal+=dRealSize;
 				VisualWord * vw = new VisualWord(*iter, d);
 				if(vw)
 				{
@@ -3675,7 +3715,7 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
 		rc = sqlite3_finalize(ppStmt);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());

-		ULOGGER_DEBUG("Time=%fs", timer.ticks());
+		UDEBUG("Time=%fs (%d words, %lu MB)", timer.ticks(), (int)vws.size(), dRealSizeTotal/1000000);

 		if(wordIds.size() != loaded.size())
 		{
@@ -4272,30 +4312,25 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
 		float nanFloat = std::numeric_limits<float>::quiet_NaN ();
 		for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
 		{
+			UASSERT((*i)->getWords().size() == (*i)->getWordsKpts().size());
 			UASSERT((*i)->getWords3().empty() || (*i)->getWords().size() == (*i)->getWords3().size());
-			UASSERT((*i)->getWordsDescriptors().empty() || (*i)->getWords().size() == (*i)->getWordsDescriptors().size());
+			UASSERT((*i)->getWordsDescriptors().empty() || (int)(*i)->getWords().size() == (*i)->getWordsDescriptors().rows);

-			std::multimap<int, cv::Point3f>::const_iterator p=(*i)->getWords3().begin();
-			std::multimap<int, cv::Mat>::const_iterator d=(*i)->getWordsDescriptors().begin();
-			for(std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
+			for(std::multimap<int, int>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
 			{
 				cv::Point3f pt(nanFloat,nanFloat,nanFloat);
-				if(p!=(*i)->getWords3().end())
+				if(!(*i)->getWords3().empty())
 				{
-					UASSERT(w->first == p->first); // must be same id!
-					pt = p->second;
-					++p;
+					pt = (*i)->getWords3()[w->second];
 				}

 				cv::Mat descriptor;
-				if(d!=(*i)->getWordsDescriptors().end())
+				if(!(*i)->getWordsDescriptors().empty())
 				{
-					UASSERT(w->first == d->first); // must be same id!
-					descriptor = d->second;
-					++d;
+					descriptor = (*i)->getWordsDescriptors().row(w->second);
 				}

-				stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, pt, descriptor);
+				stepKeypoint(ppStmt, (*i)->id(), w->first, (*i)->getWordsKpts()[w->second], pt, descriptor);
 			}
 		}
 		// Finalize (delete) the statement
@@ -5058,7 +5093,7 @@ cv::Mat DBDriverSqlite3::load2DMapQuery(float & xMin, float & yMin, float & cell

 void DBDriverSqlite3::saveOptimizedMeshQuery(
 			const cv::Mat & cloud,
-			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+			const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
 #else
@@ -5253,7 +5288,7 @@ void DBDriverSqlite3::saveOptimizedMeshQuery(
 }

 cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
-			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+			std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
 #else
@@ -5315,7 +5350,7 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
 					for(int t=0; t<serializedPolygons.cols; ++t)
 					{
 						UASSERT(serializedPolygons.at<int>(t) > 0);
-						std::vector<std::vector<unsigned int> > materialPolygons(serializedPolygons.at<int>(t), std::vector<unsigned int>(polygonSize));
+						std::vector<std::vector<RTABMAP_PCL_INDEX> > materialPolygons(serializedPolygons.at<int>(t), std::vector<RTABMAP_PCL_INDEX>(polygonSize));
 						++t;
 						UASSERT(t < serializedPolygons.cols);
 						UDEBUG("materialPolygons=%d", (int)materialPolygons.size());
@@ -47,16 +47,16 @@ DBReader::DBReader(const std::string & databasePath,
 				   bool odometryIgnored,
 				   bool ignoreGoalDelay,
 				   bool goalsIgnored,
-				   int stopId,
+				   int startId,
 				   int cameraIndex,
-				   int endId) :
+				   int stopId) :
 	Camera(frameRate),
 	_paths(uSplit(databasePath, ';')),
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
 	_goalsIgnored(goalsIgnored),
-	_startId(stopId),
-	_stopId(endId),
+	_startId(startId),
+	_stopId(stopId),
 	_cameraIndex(cameraIndex),
 	_dbDriver(0),
 	_currentId(_ids.end()),
@@ -76,16 +76,16 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   bool odometryIgnored,
 				   bool ignoreGoalDelay,
 				   bool goalsIgnored,
-				   int stopId,
+				   int startId,
 				   int cameraIndex,
-				   int endId) :
+				   int stopId) :
 	Camera(frameRate),
   _paths(databasePaths),
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
 	_goalsIgnored(goalsIgnored),
-	_startId(stopId),
-	_stopId(endId),
+	_startId(startId),
+	_stopId(stopId),
 	_cameraIndex(cameraIndex),
 	_dbDriver(0),
 	_currentId(_ids.end()),
@@ -510,23 +510,9 @@ SensorData DBReader::getNextData(CameraInfo * info)
 					data.gps().stamp()!=0.0?1:0,
 					gravityTransform.isNull()?0:1);

-			cv::Mat descriptors;
-			if(!s->getWordsDescriptors().empty())
-			{
-				descriptors = cv::Mat(
-					s->getWordsDescriptors().size(),
-					s->getWordsDescriptors().begin()->second.cols,
-					s->getWordsDescriptors().begin()->second.type());
-				int i=0;
-				for(std::multimap<int, cv::Mat>::const_iterator iter=s->getWordsDescriptors().begin();
-					iter!=s->getWordsDescriptors().end();
-					++iter, ++i)
-				{
-					iter->second.copyTo(descriptors.row(i));
-				}
-			}
-			std::vector<cv::KeyPoint> keypoints = uValues(s->getWords());
-			std::vector<cv::Point3f> keypoints3D = uValues(s->getWords3());
+			cv::Mat descriptors = s->getWordsDescriptors().clone();
+			const std::vector<cv::KeyPoint> & keypoints = s->getWordsKpts();
+			const std::vector<cv::Point3f> & keypoints3D = s->getWords3();
 			if(!keypoints.empty() &&
 			   (keypoints3D.empty() || keypoints.size() == keypoints3D.size()) &&
 			   (descriptors.empty() || (int)keypoints.size() == descriptors.rows))
@@ -70,15 +70,21 @@ bool EpipolarGeometry::check(const Signature * ssA, const Signature * ssB)
 	}
 	ULOGGER_DEBUG("id(%d,%d)", ssA->id(), ssB->id());

-	std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
+	std::list<std::pair<int, std::pair<int, int> > > pairsId;

-	findPairsUnique(ssA->getWords(), ssB->getWords(), pairs);
+	findPairsUnique(ssA->getWords(), ssB->getWords(), pairsId);

-	if((int)pairs.size()<_matchCountMinAccepted)
+	if((int)pairsId.size()<_matchCountMinAccepted)
 	{
 		return false;
 	}

+	std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
+	for(std::list<std::pair<int, std::pair<int, int> > >::iterator iter = pairsId.begin(); iter!=pairsId.end(); ++iter)
+	{
+		pairs.push_back(std::make_pair(iter->first, std::make_pair(ssA->getWordsKpts()[iter->second.first], ssB->getWordsKpts()[iter->second.second])));
+	}
+
 	std::vector<uchar> status;
 	cv::Mat f = findFFromWords(pairs, status, _ransacParam1, _ransacParam2);

@@ -406,136 +412,6 @@ cv::Mat EpipolarGeometry::findFFromCalibratedStereoCameras(double fx, double fy,
 	return K.inv().t()*E*K.inv();
 }

-/**
- * if a=[1 2 3 4 6], b=[1 2 4 5 6], results= [(1,1) (2,2) (4,4) (6,6)]
- * realPairsCount = 4
- */
-int EpipolarGeometry::findPairs(
-		const std::map<int, cv::KeyPoint> & wordsA,
-		const std::map<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-		bool ignoreInvalidIds)
-{
-	int realPairsCount = 0;
-	pairs.clear();
-	for(std::map<int, cv::KeyPoint>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
-	{
-		if(!ignoreInvalidIds || (ignoreInvalidIds && i->first>=0))
-		{
-			std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
-			if(ptB != wordsB.end())
-			{
-				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(i->first, std::pair<cv::KeyPoint, cv::KeyPoint>(i->second, ptB->second)));
-				++realPairsCount;
-			}
-		}
-	}
-	return realPairsCount;
-}
-
-/**
- * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
- * realPairsCount = 5
- */
-int EpipolarGeometry::findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
-		const std::multimap<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-		bool ignoreInvalidIds)
-{
-	const std::list<int> & ids = uUniqueKeys(wordsA);
-	std::multimap<int, cv::KeyPoint>::const_iterator iterA;
-	std::multimap<int, cv::KeyPoint>::const_iterator iterB;
-	pairs.clear();
-	int realPairsCount = 0;
-	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
-	{
-		if(!ignoreInvalidIds || (ignoreInvalidIds && *i >= 0))
-		{
-			iterA = wordsA.find(*i);
-			iterB = wordsB.find(*i);
-			while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
-			{
-				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>((*iterA).second, (*iterB).second)));
-				++iterA;
-				++iterB;
-				++realPairsCount;
-			}
-		}
-	}
-	return realPairsCount;
-}
-
-/**
- * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
- * realPairsCount = 5
- */
-int EpipolarGeometry::findPairsUnique(
-		const std::multimap<int, cv::KeyPoint> & wordsA,
-		const std::multimap<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-		bool ignoreInvalidIds)
-{
-	const std::list<int> & ids = uUniqueKeys(wordsA);
-	int realPairsCount = 0;
-	pairs.clear();
-	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
-	{
-		if(!ignoreInvalidIds || (ignoreInvalidIds && *i>=0))
-		{
-			std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
-			std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
-			if(ptsA.size() == 1 && ptsB.size() == 1)
-			{
-				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>(ptsA.front(), ptsB.front())));
-				++realPairsCount;
-			}
-			else if(ptsA.size()>1 && ptsB.size()>1)
-			{
-				// just update the count
-				realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
-			}
-		}
-	}
-	return realPairsCount;
-}
-
-/**
- * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
- * realPairsCount = 5
- */
-int EpipolarGeometry::findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
-		const std::multimap<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-		bool ignoreInvalidIds)
-{
-	UTimer timer;
-	timer.start();
-	const std::list<int> & ids = uUniqueKeys(wordsA);
-	pairs.clear();
-	int realPairsCount = 0;;
-	for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
-	{
-		if(!ignoreInvalidIds || (ignoreInvalidIds && *iter>=0))
-		{
-			std::list<cv::KeyPoint> ptsA = uValues(wordsA, *iter);
-			std::list<cv::KeyPoint> ptsB = uValues(wordsB, *iter);
-
-			realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
-
-			for(std::list<cv::KeyPoint>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
-			{
-				for(std::list<cv::KeyPoint>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
-				{
-					pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*iter, std::pair<cv::KeyPoint, cv::KeyPoint>(*jter, *kter)));
-				}
-			}
-		}
-	}
-	ULOGGER_DEBUG("time = %f", timer.ticks());
-	return realPairsCount;
-}
-
-

 /**
 source = SfM toy library: https://github.com/royshil/SfM-Toy-Library
@@ -509,54 +509,69 @@ Feature2D * Feature2D::create(const ParametersMap & parameters)
 Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parameters)
 {

+// NONFREE checks
 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
-#ifndef RTABMAP_NONFREE
-	if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift)
+
+  #ifndef RTABMAP_NONFREE
+	if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift || type == Feature2D::kFeatureSurfFreak || type == Feature2D::kFeatureSurfDaisy)
 	{
-#if CV_MAJOR_VERSION < 3
+    #if CV_MAJOR_VERSION < 3
 		UWARN("SURF and SIFT features cannot be used because OpenCV was not built with nonfree module. GFTT/ORB is used instead.");
-#else
+    #else
 		UWARN("SURF and SIFT features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.");
-#endif
+    #endif
 		type = Feature2D::kFeatureGfttOrb;
 	}
-#if CV_MAJOR_VERSION == 3
-	if(type == Feature2D::kFeatureFastBrief ||
-	   type == Feature2D::kFeatureFastFreak ||
-	   type == Feature2D::kFeatureGfttBrief ||
-	   type == Feature2D::kFeatureGfttFreak)
-	{
-		UWARN("BRIEF and FREAK features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.");
-		type = Feature2D::kFeatureGfttOrb;
-	}
-#endif
-#endif
+  #endif

 #else // >= 4.4.0 >= 3.4.11

-#ifndef RTABMAP_NONFREE
+  #ifndef RTABMAP_NONFREE
 	if(type == Feature2D::kFeatureSurf)
 	{
 		UWARN("SURF features cannot be used because OpenCV was not built with nonfree module. SIFT is used instead.");
 		type = Feature2D::kFeatureSift;
 	}
-#endif
+	else if(type == Feature2D::kFeatureSurfFreak || type == Feature2D::kFeatureSurfDaisy)
+	{
+		UWARN("SURF detector cannot be used because OpenCV was not built with nonfree module. GFTT/ORB is used instead.");
+		type = Feature2D::kFeatureGfttOrb;
+	}
+  #endif

 #endif // >= 4.4.0 >= 3.4.11

-#if CV_MAJOR_VERSION < 3
+#if !defined(HAVE_OPENCV_XFEATURES2D) && CV_MAJOR_VERSION >= 3
+	if(type == Feature2D::kFeatureFastBrief ||
+	   type == Feature2D::kFeatureFastFreak ||
+	   type == Feature2D::kFeatureGfttBrief ||
+	   type == Feature2D::kFeatureGfttFreak ||
+	   type == Feature2D::kFeatureSurfFreak ||
+	   type == Feature2D::kFeatureGfttDaisy ||
+	   type == Feature2D::kFeatureSurfDaisy)
+	{
+		UWARN("BRIEF, FREAK and DAISY features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.");
+		type = Feature2D::kFeatureGfttOrb;
+	}
+#elif CV_MAJOR_VERSION < 3
 	if(type == Feature2D::kFeatureKaze)
 	{
-#ifdef RTABMAP_NONFREE
+  #ifdef RTABMAP_NONFREE
 		UWARN("KAZE detector/descriptor can be used only with OpenCV3. SURF is used instead.");
 		type = Feature2D::kFeatureSurf;
-#else
+  #else
 		UWARN("KAZE detector/descriptor can be used only with OpenCV3. GFTT/ORB is used instead.");
 		type = Feature2D::kFeatureGfttOrb;
-#endif
+  #endif
+	}
+	if(type == Feature2D::kFeatureGfttDaisy || type == Feature2D::kFeatureSurfDaisy)
+	{
+		UWARN("DAISY detector/descriptor can be used only with OpenCV3. GFTT/BRIEF is used instead.");
+		type = Feature2D::kFeatureGfttBrief;
 	}
 #endif

+
 #ifndef RTABMAP_ORB_OCTREE
 	if(type == Feature2D::kFeatureOrbOctree)
 	{
@@ -614,6 +629,15 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 		feature2D = new SuperPointTorch(parameters);
 		break;
 #endif
+	case Feature2D::kFeatureSurfFreak:
+		feature2D = new SURF_FREAK(parameters);
+		break;
+	case Feature2D::kFeatureGfttDaisy:
+		feature2D = new GFTT_DAISY(parameters);
+		break;
+	case Feature2D::kFeatureSurfDaisy:
+		feature2D = new SURF_DAISY(parameters);
+		break;
 #ifdef RTABMAP_NONFREE
 	default:
 		feature2D = new SURF(parameters);
@@ -622,7 +646,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 #else
 	default:
 		feature2D = new ORB(parameters);
-		type = Feature2D::kFeatureOrb;
+		type = Feature2D::kFeatureGfttOrb;
 		break;
 #endif

@@ -973,7 +997,7 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 #else
 	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
-#else
+#else // >=4.4, >=3.4.11
 	_sift = CV_SIFT::create(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
 #endif
 }
@@ -982,16 +1006,20 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
-#if defined(RTABMAP_NONFREE) || CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 3)
 	cv::Mat imgRoi(image, roi);
 	cv::Mat maskRoi;
 	if(!mask.empty())
 	{
 		maskRoi = cv::Mat(mask, roi);
 	}
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
+#ifdef RTABMAP_NONFREE
 	_sift->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
 #else
 	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
+#endif
+#else // >=4.4, >=3.4.11
+	_sift->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
 #endif
 	return keypoints;
 }
@@ -1000,9 +1028,15 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
-#if defined(RTABMAP_NONFREE) || CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 3)
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
+#ifdef RTABMAP_NONFREE
 	_sift->compute(image, keypoints, descriptors);
-
+#else
+	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
+#endif
+#else // >=4.4, >=3.4.11
+	_sift->compute(image, keypoints, descriptors);
+#endif
 	if( rootSIFT_ && !descriptors.empty())
 	{
 		UDEBUG("Performing RootSIFT...");
@@ -1018,10 +1052,6 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 			cv::sqrt(descriptors.row(i), descriptors.row(i));
 		}
 	}
-
-#else
-	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
-#endif
 	return descriptors;
 }

@@ -1724,6 +1754,59 @@ cv::Mat GFTT_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<c
 	return descriptors;
 }

+//////////////////////////
+//SURF-FREAK
+//////////////////////////
+SURF_FREAK::SURF_FREAK(const ParametersMap & parameters) :
+	SURF(parameters),
+	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
+	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
+	patternScale_(Parameters::defaultFREAKPatternScale()),
+	nOctaves_(Parameters::defaultFREAKNOctaves())
+{
+	parseParameters(parameters);
+}
+
+SURF_FREAK::~SURF_FREAK()
+{
+}
+
+void SURF_FREAK::parseParameters(const ParametersMap & parameters)
+{
+	SURF::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
+	Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
+	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
+	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
+
+#if CV_MAJOR_VERSION < 3
+	_freak = cv::Ptr<CV_FREAK>(new CV_FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_));
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_freak = CV_FREAK::create(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
+#endif
+#endif
+}
+
+cv::Mat SURF_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	cv::Mat descriptors;
+#if CV_MAJOR_VERSION < 3
+	_freak->compute(image, keypoints, descriptors);
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_freak->compute(image, keypoints, descriptors);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
+#endif
+#endif
+	return descriptors;
+}
+
 //////////////////////////
 //GFTT-ORB
 //////////////////////////
@@ -1876,7 +1959,10 @@ cv::Mat KAZE::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 ORBOctree::ORBOctree(const ParametersMap & parameters) :
 		scaleFactor_(Parameters::defaultORBScaleFactor()),
 		nLevels_(Parameters::defaultORBNLevels()),
-		fastThreshold_(Parameters::defaultFASTThreshold())
+		patchSize_(Parameters::defaultORBPatchSize()),
+		edgeThreshold_(Parameters::defaultORBEdgeThreshold()),
+		fastThreshold_(Parameters::defaultFASTThreshold()),
+		fastMinThreshold_(Parameters::defaultFASTMinThreshold())
 {
 	parseParameters(parameters);
 }
@@ -1891,12 +1977,14 @@ void ORBOctree::parseParameters(const ParametersMap & parameters)

 	Parameters::parse(parameters, Parameters::kORBScaleFactor(), scaleFactor_);
 	Parameters::parse(parameters, Parameters::kORBNLevels(), nLevels_);
+	Parameters::parse(parameters, Parameters::kORBPatchSize(), patchSize_);
+	Parameters::parse(parameters, Parameters::kORBEdgeThreshold(), edgeThreshold_);

 	Parameters::parse(parameters, Parameters::kFASTThreshold(), fastThreshold_);
 	Parameters::parse(parameters, Parameters::kFASTMinThreshold(), fastMinThreshold_);

 #ifdef RTABMAP_ORB_OCTREE
-	_orb = cv::Ptr<ORBextractor>(new ORBextractor(this->getMaxFeatures(), scaleFactor_, nLevels_, fastThreshold_, fastMinThreshold_));
+	_orb = cv::Ptr<ORBextractor>(new ORBextractor(this->getMaxFeatures(), scaleFactor_, nLevels_, fastThreshold_, fastMinThreshold_, patchSize_, edgeThreshold_));
 #else
 	UWARN("RTAB-Map is not built with ORB OcTree option enabled so ORB OcTree feature cannot be used!");
 #endif
@@ -2007,4 +2095,95 @@ cv::Mat SuperPointTorch::generateDescriptorsImpl(const cv::Mat & image, std::vec
 #endif
 }

+
+//////////////////////////
+//GFTT-DAISY
+//////////////////////////
+GFTT_DAISY::GFTT_DAISY(const ParametersMap & parameters) :
+	GFTT(parameters),
+	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
+	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
+	patternScale_(Parameters::defaultFREAKPatternScale()),
+	nOctaves_(Parameters::defaultFREAKNOctaves())
+{
+	parseParameters(parameters);
+}
+
+GFTT_DAISY::~GFTT_DAISY()
+{
+}
+
+void GFTT_DAISY::parseParameters(const ParametersMap & parameters)
+{
+	GFTT::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
+	Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
+	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
+	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
+
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_daisy = CV_DAISY::create();
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so DAISY cannot be used!");
+#endif
+}
+
+cv::Mat GFTT_DAISY::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	cv::Mat descriptors;
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_daisy->compute(image, keypoints, descriptors);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so DAISY cannot be used!");
+#endif
+	return descriptors;
+}
+
+//////////////////////////
+//SURF-DAISY
+//////////////////////////
+SURF_DAISY::SURF_DAISY(const ParametersMap & parameters) :
+	SURF(parameters),
+	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
+	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
+	patternScale_(Parameters::defaultFREAKPatternScale()),
+	nOctaves_(Parameters::defaultFREAKNOctaves())
+{
+	parseParameters(parameters);
+}
+
+SURF_DAISY::~SURF_DAISY()
+{
+}
+
+void SURF_DAISY::parseParameters(const ParametersMap & parameters)
+{
+	SURF::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
+	Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
+	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
+	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
+
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_daisy = CV_DAISY::create();
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so DAISY cannot be used!");
+#endif
+}
+
+cv::Mat SURF_DAISY::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	cv::Mat descriptors;
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_daisy->compute(image, keypoints, descriptors);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so DAISY cannot be used!");
+#endif
+	return descriptors;
+}
+
 }
@@ -107,32 +107,36 @@ unsigned int FlannIndex::indexedFeatures() const
 	}
 }

-// return KB
-unsigned int FlannIndex::memoryUsed() const
+// return Bytes
+unsigned long FlannIndex::memoryUsed() const
 {
 	if(!index_)
 	{
 		return 0;
 	}
+	unsigned long memoryUsage = sizeof(FlannIndex);
+	memoryUsage += addedDescriptors_.size() * (sizeof(int) + sizeof(cv::Mat) + sizeof(std::map<int, cv::Mat>::iterator)) + sizeof(std::map<int, cv::Mat>);
+	memoryUsage += sizeof(std::list<int>) + removedIndexes_.size() * sizeof(int);
 	if(featuresType_ == CV_8UC1)
 	{
-		return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->usedMemory()/1000;
+		memoryUsage += ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->usedMemory();
 	}
 	else
 	{
 		if(useDistanceL1_)
 		{
-			return ((const rtflann::Index<rtflann::L1<float> >*)index_)->usedMemory()/1000;
+			memoryUsage += ((const rtflann::Index<rtflann::L1<float> >*)index_)->usedMemory();
 		}
 		else if(featuresDim_ <= 3)
 		{
-			return ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->usedMemory()/1000;
+			memoryUsage += ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->usedMemory();
 		}
 		else
 		{
-			return ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory()/1000;
+			memoryUsage += ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory();
 		}
 	}
+	return memoryUsage;
 }

 void FlannIndex::buildLinearIndex(
@@ -177,10 +181,21 @@ void FlannIndex::buildLinearIndex(
 		}
 	}

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

@@ -227,10 +242,21 @@ void FlannIndex::buildKDTreeIndex(
 		}
 	}

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

@@ -278,10 +304,21 @@ void FlannIndex::buildKDTreeSingleIndex(
 		}
 	}

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

@@ -305,10 +342,21 @@ void FlannIndex::buildLSHIndex(
 	index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, rtflann::LshIndexParams(12, 20, 2));
 	((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();

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

@@ -317,12 +365,12 @@ bool FlannIndex::isBuilt()
 	return index_!=0;
 }

-unsigned int FlannIndex::addPoints(const cv::Mat & features)
+std::vector<unsigned int> FlannIndex::addPoints(const cv::Mat & features)
 {
 	if(!index_)
 	{
 		UERROR("Flann index not yet created!");
-		return 0;
+		return std::vector<unsigned int>();
 	}
 	UASSERT(features.type() == featuresType_);
 	UASSERT(features.cols == featuresDim_);
@@ -401,11 +449,16 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
 		removedIndexes_.clear();
 	}

-	addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+	// incremental FLANN: we should add all headers separately in case we remove
+	// some indexes (to keep underlying matrix data allocated)
+	std::vector<unsigned int> indexes;
+	for(int i=0; i<features.rows; ++i)
+	{
+		indexes.push_back(nextIndex_);
+		addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
+	}

-	int r = nextIndex_;
-	nextIndex_ += features.rows;
-	return r;
+	return indexes;
 }

 void FlannIndex::removePoint(unsigned int index)
@@ -169,7 +169,7 @@ bool exportPoses(

 bool importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAC
+		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints, // optional for formats 3 and 4
 		std::map<int, double> * stamps) // optional for format 1 and 9
@@ -1129,19 +1129,22 @@ std::multimap<int, Link> filterDuplicateLinks(

 std::multimap<int, Link> filterLinks(
 		const std::multimap<int, Link> & links,
-		Link::Type filteredType)
+		Link::Type filteredType,
+		bool inverted)
 {
 	std::multimap<int, Link> output;
 	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 	{
 		if(filteredType == Link::kSelfRefLink)
 		{
-			if(iter->second.from() != iter->second.to())
+			if((!inverted && iter->second.from() != iter->second.to())||
+			   (inverted && iter->second.from() == iter->second.to()))
 			{
 				output.insert(*iter);
 			}
 		}
-		else if(iter->second.type() != filteredType)
+		else if((!inverted && iter->second.type() != filteredType)||
+				(inverted && iter->second.type() == filteredType))
 		{
 			output.insert(*iter);
 		}
@@ -1151,19 +1154,22 @@ std::multimap<int, Link> filterLinks(

 std::map<int, Link> filterLinks(
 		const std::map<int, Link> & links,
-		Link::Type filteredType)
+		Link::Type filteredType,
+		bool inverted)
 {
 	std::map<int, Link> output;
 	for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 	{
 		if(filteredType == Link::kSelfRefLink)
 		{
-			if(iter->second.from() != iter->second.to())
+			if((!inverted && iter->second.from() != iter->second.to())||
+			   (inverted && iter->second.from() == iter->second.to()))
 			{
 				output.insert(*iter);
 			}
 		}
-		else if(iter->second.type() != filteredType)
+		else if((!inverted && iter->second.type() != filteredType)||
+				(inverted && iter->second.type() == filteredType))
 		{
 			output.insert(*iter);
 		}
@@ -2054,23 +2060,28 @@ std::list<std::pair<int, Transform> > computePath(

 int findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
-		const rtabmap::Transform & targetPose)
+		const rtabmap::Transform & targetPose,
+		float * distance)
 {
 	int id = 0;
-	std::vector<int> nearestNodes = findNearestNodes(nodes, targetPose, 1);
-	if(nearestNodes.size())
+	std::map<int, float> nearestNodes = findNearestNodes(nodes, targetPose, 1);
+	if(!nearestNodes.empty())
 	{
-		id = nearestNodes[0];
+		id = nearestNodes.begin()->first;
+		if(distance)
+		{
+			*distance = nearestNodes.begin()->second;
+		}
 	}
 	return id;
 }

-std::vector<int> findNearestNodes(
+std::map<int, float> findNearestNodes(
 		const std::map<int, rtabmap::Transform> & nodes,
 		const rtabmap::Transform & targetPose,
 		int k)
 {
-	std::vector<int> nearestIds;
+	std::map<int, float> nearestIds;
 	if(nodes.size() && !targetPose.isNull())
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -2090,10 +2101,9 @@ std::vector<int> findNearestNodes(
 		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
 		kdTree->nearestKSearch(pt, k, ind, dist);

-		nearestIds.resize(ind.size());
 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
-			nearestIds[i] = ids[ind[i]];
+			nearestIds.insert(std::make_pair(ids[ind[i]], dist[i]));
 		}
 	}
 	return nearestIds;
@@ -2106,8 +2116,21 @@ std::map<int, float> getNodesInRadius(
 		float radius)
 {
 	UASSERT(uContains(nodes, nodeId));
+
+	std::map<int, Transform> nodesMinusTarget = nodes;
+	Transform targetPose = nodes.at(nodeId);
+	nodesMinusTarget.erase(nodeId);
+	return getNodesInRadius(targetPose, nodesMinusTarget, radius);
+}
+
+// return <id, sqrd distance>, excluding query
+std::map<int, float> getNodesInRadius(
+		const Transform & targetPose,
+		const std::map<int, Transform> & nodes,
+		float radius)
+{
 	std::map<int, float> foundNodes;
-	if(nodes.size() <= 1)
+	if(nodes.empty())
 	{
 		return foundNodes;
 	}
@@ -2118,26 +2141,21 @@ std::map<int, float> getNodesInRadius(
 	int oi = 0;
 	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
 	{
-		if(iter->first != nodeId)
-		{
-			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-			UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
-			ids[oi] = iter->first;
-			++oi;
-		}
+		(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+		UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
+		ids[oi] = iter->first;
+		++oi;
 	}
 	cloud->resize(oi);
 	ids.resize(oi);

-	Transform fromT = nodes.at(nodeId);
-
 	if(cloud->size())
 	{
 		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
 		kdTree->setInputCloud(cloud);
 		std::vector<int> ind;
 		std::vector<float> sqrdDist;
-		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
+		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
 		kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
@@ -2159,8 +2177,21 @@ std::map<int, Transform> getPosesInRadius(
 		float angle)
 {
 	UASSERT(uContains(nodes, nodeId));
+
+	std::map<int, Transform> nodesMinusTarget = nodes;
+	Transform targetPose = nodes.at(nodeId);
+	nodesMinusTarget.erase(nodeId);
+	return getPosesInRadius(targetPose, nodesMinusTarget, radius, angle);
+}
+// return <id, Transform>, excluding query
+std::map<int, Transform> getPosesInRadius(
+		const Transform & targetPose,
+		const std::map<int, Transform> & nodes,
+		float radius,
+		float angle)
+{
 	std::map<int, Transform> foundNodes;
-	if(nodes.size() <= 1)
+	if(nodes.empty())
 	{
 		return foundNodes;
 	}
@@ -2171,29 +2202,24 @@ std::map<int, Transform> getPosesInRadius(
 	int oi = 0;
 	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
 	{
-		if(iter->first != nodeId)
-		{
-			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-			UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
-			ids[oi] = iter->first;
-			++oi;
-		}
+		(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+		UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
+		ids[oi] = iter->first;
+		++oi;
 	}
 	cloud->resize(oi);
 	ids.resize(oi);

-	Transform fromT = nodes.at(nodeId);
-
 	if(cloud->size())
 	{
 		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
 		kdTree->setInputCloud(cloud);
 		std::vector<int> ind;
 		std::vector<float> sqrdDist;
-		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
+		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
 		kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);

-		Eigen::Vector3f vA = fromT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
+		Eigen::Vector3f vA = targetPose.toEigen3f().linear()*Eigen::Vector3f(1,0,0);

 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
@@ -36,6 +36,8 @@ MarkerDetector::MarkerDetector(const ParametersMap & parameters)
 #ifdef HAVE_OPENCV_ARUCO
 	markerLength_ = Parameters::defaultMarkerLength();
 	maxDepthError_ = Parameters::defaultMarkerMaxDepthError();
+	maxRange_ = Parameters::defaultMarkerMaxRange();
+	minRange_ = Parameters::defaultMarkerMinRange();
 	dictionaryId_ = Parameters::defaultMarkerDictionary();
 #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
 	detectorParams_ = cv::aruco::DetectorParameters::create();
@@ -87,6 +89,8 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)

 	Parameters::parse(parameters, Parameters::kMarkerLength(), markerLength_);
 	Parameters::parse(parameters, Parameters::kMarkerMaxDepthError(), maxDepthError_);
+	Parameters::parse(parameters, Parameters::kMarkerMaxRange(), maxRange_);
+	Parameters::parse(parameters, Parameters::kMarkerMinRange(), minRange_);
 	Parameters::parse(parameters, Parameters::kMarkerDictionary(), dictionaryId_);
 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
 	if(dictionaryId_ >= 17)
@@ -191,15 +195,20 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
 				}
 			}

-			cv::Mat R;
-			cv::Rodrigues(rvecs[i], R);
-			Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvecs[i].val[0],
-						   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvecs[i].val[1],
-						   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvecs[i].val[2]);
-
-			Transform pose = model.localTransform() * t;
-			detections.insert(std::make_pair(ids[i], pose));
-			UDEBUG("Marker %d detected at %s (%s)", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str());
+			// Limit the detection range to be between the min / max range.
+			// If the ranges are -1, allow any detection within that direction.
+			if((maxRange_ <= 0 || tvecs[i].val[2] < maxRange_) &&
+				(minRange_ <= 0 || tvecs[i].val[2] > minRange_))
+			{
+				cv::Mat R;
+				cv::Rodrigues(rvecs[i], R);
+				Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvecs[i].val[0],
+								 R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvecs[i].val[1],
+								 R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvecs[i].val[2]);
+				Transform pose = model.localTransform() * t;
+				detections.insert(std::make_pair(ids[i], pose));
+				UDEBUG("Marker %d detected at %s (%s)", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str());
+			}
 		}
 		if(markerLength_ == 0)
 		{
@@ -261,4 +270,3 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam


 } /* namespace rtabmap */
-
@@ -362,7 +362,7 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			const std::map<int, Signature *> & signatures = this->getSignatures();
 			for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
 			{
-				const std::multimap<int, cv::KeyPoint> & words = i->second->getWords();
+				const std::multimap<int, int> & words = i->second->getWords();
 				std::list<int> keys = uUniqueKeys(words);
 				for(std::list<int>::iterator iter=keys.begin(); iter!=keys.end(); ++iter)
 				{
@@ -413,11 +413,11 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			Signature * s = this->_getSignature(i->first);
 			UASSERT(s != 0);

-			const std::multimap<int, cv::KeyPoint> & words = s->getWords();
+			const std::multimap<int, int> & words = s->getWords();
 			if(words.size())
 			{
 				UDEBUG("node=%d, word references=%d", s->id(), words.size());
-				for(std::multimap<int, cv::KeyPoint>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
+				for(std::multimap<int, int>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
 				{
 					if(iter->first > 0)
 					{
@@ -886,7 +886,7 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
 	// add signature on top of the short-term memory
 	if(signature)
 	{
-		UDEBUG("adding %d", signature->id());
+		UDEBUG("adding %d (pose=%s)", signature->id(), signature->getPose().prettyPrint().c_str());
 		// Update neighbors
 		if(_stMem.size())
 		{
@@ -1009,7 +1009,7 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance

 		if(_vwd)
 		{
-			UDEBUG("%d words ref for the signature %d", signature->getWords().size(), signature->id());
+			UDEBUG("%d words ref for the signature %d (weight=%d)", signature->getWords().size(), signature->id(), signature->getWeight());
 		}
 		if(signature->getWords().size())
 		{
@@ -1131,7 +1131,7 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
 					}
 				}

-				this->moveToTrash(s, _notLinkedNodesKeptInDb);
+				this->moveToTrash(s, false);
 				s = 0;
 			}
 		}
@@ -1536,7 +1536,7 @@ std::map<int, float> Memory::getNeighborsIdRadius(
 	nextMargin.insert(signatureId);
 	int m = 0;
 	Transform referential = optimizedPoses.at(signatureId);
-	UASSERT(!referential.isNull());
+	UASSERT_MSG(!referential.isNull(), uFormat("signatureId=%d", signatureId).c_str());
 	float radiusSqrd = radius*radius;
 	while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size())
 	{
@@ -2083,7 +2083,7 @@ cv::Mat Memory::load2DMap(float & xMin, float & yMin, float & cellSize) const

 void Memory::saveOptimizedMesh(
 		const cv::Mat & cloud,
-		const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+		const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 		const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
 #else
@@ -2098,7 +2098,7 @@ void Memory::saveOptimizedMesh(
 }

 cv::Mat Memory::loadOptimizedMesh(
-			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+			std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
 #else
@@ -2357,7 +2357,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 					}

 					// child
-					if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id())
+					if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id() && s->getWeight()>0)
 					{
 						sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight
 					}
@@ -2368,7 +2368,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 			}
 			s->removeLinks(true); // remove all links, but keep self referring link
 			s->removeLandmarks(); // remove all landmarks
-			s->setWeight(0);
+			s->setWeight(-9); // invalid
 			s->setLabel(""); // reset label
 		}
 		else
@@ -2753,20 +2753,16 @@ Transform Memory::computeTransform(
 		if(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())
 		{
 			UDEBUG("");
-			tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
-			tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
-			tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
+			tmpFrom.removeAllWords();
 			tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
-			tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
-			tmpTo.setWords3(std::multimap<int, cv::Point3f>());
-			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+			tmpTo.removeAllWords();
 			tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 		}
 		else if(useKnownCorrespondencesIfPossible)
 		{
 			// This will make RegistrationVis bypassing the correspondences computation
-			tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
-			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+			tmpFrom.setWordsDescriptors(cv::Mat());
+			tmpTo.setWordsDescriptors(cv::Mat());
 		}

 		bool isNeighborRefining = fromS.getLinks().find(toS.id()) != fromS.getLinks().end() && fromS.getLinks().find(toS.id())->second.type() == Link::kNeighbor;
@@ -2795,23 +2791,28 @@ Transform Memory::computeTransform(
 			   !tmpTo.getWords().empty() &&
 			   !tmpFrom.getWordsDescriptors().empty() &&
 			   !tmpFrom.getWords().empty() &&
-			   !tmpFrom.getWords3().empty())
+			   !tmpFrom.getWords3().empty() &&
+			   fromS.hasLink(0, Link::kNeighbor)) // If doesn't have neighbors, skip bundle
 		{
-			std::multimap<int, cv::Point3f> words3DMap;
-			std::multimap<int, cv::KeyPoint> wordsMap;
-			std::multimap<int, cv::Mat> wordsDescriptorsMap;
+			std::multimap<int, int> words;
+			std::vector<cv::Point3f> words3DMap;
+			std::vector<cv::KeyPoint> wordsMap;
+			cv::Mat wordsDescriptorsMap;

 			const std::multimap<int, Link> & links = fromS.getLinks();
+			if(!fromS.getWords3().empty())
 			{
-				const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(fromS.getWords3());
-				UDEBUG("fromS.getWords3()=%d  uniques=%d", (int)fromS.getWords3().size(), (int)words3.size());
-				for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
+				const std::map<int, int> & wordsFrom = uMultimapToMapUnique(fromS.getWords());
+				UDEBUG("fromS.getWords()=%d  uniques=%d", (int)fromS.getWords().size(), (int)wordsFrom.size());
+				for(std::map<int, int>::const_iterator jter=wordsFrom.begin(); jter!=wordsFrom.end(); ++jter)
 				{
-					if(util3d::isFinite(jter->second))
+					const cv::Point3f & pt = fromS.getWords3()[jter->second];
+					if(util3d::isFinite(pt))
 					{
-						words3DMap.insert(*jter);
-						wordsMap.insert(*fromS.getWords().find(jter->first));
-						wordsDescriptorsMap.insert(*fromS.getWordsDescriptors().find(jter->first));
+						words.insert(std::make_pair(jter->first, words.size()));
+						words3DMap.push_back(pt);
+						wordsMap.push_back(fromS.getWordsKpts()[jter->second]);
+						wordsDescriptorsMap.push_back(fromS.getWordsDescriptors().row(jter->second));
 					}
 				}
 			}
@@ -2820,21 +2821,23 @@ Transform Memory::computeTransform(
 			for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 			{
 				int id = iter->first;
-				if(id != fromS.id())
+				if(id != fromS.id() && iter->second.type() == Link::kNeighbor) // assemble only neighbors for the local feature map
 				{
 					const Signature * s = this->getSignature(id);
-					if(s)
+					if(s && !s->getWords3().empty())
 					{
-						const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(s->getWords3());
-						for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
+						const std::map<int, int> & wordsTo = uMultimapToMapUnique(s->getWords());
+						for(std::map<int, int>::const_iterator jter=wordsTo.begin(); jter!=wordsTo.end(); ++jter)
 						{
+							const cv::Point3f & pt = s->getWords3()[jter->second];
 							if( jter->first > 0 &&
-								util3d::isFinite(jter->second) &&
-								words3DMap.find(jter->first) == words3DMap.end())
+								util3d::isFinite(pt) &&
+								words.find(jter->first) == words.end())
 							{
-								words3DMap.insert(std::make_pair(jter->first, util3d::transformPoint(jter->second, iter->second.transform())));
-								wordsMap.insert(*s->getWords().find(jter->first));
-								wordsDescriptorsMap.insert(*s->getWordsDescriptors().find(jter->first));
+								words.insert(words.end(), std::make_pair(jter->first, words.size()));
+								words3DMap.push_back(util3d::transformPoint(pt, iter->second.transform()));
+								wordsMap.push_back(s->getWordsKpts()[jter->second]);
+								wordsDescriptorsMap.push_back(s->getWordsDescriptors().row(jter->second));
 							}
 						}
 					}
@@ -2842,24 +2845,29 @@ Transform Memory::computeTransform(
 			}
 			UDEBUG("words3DMap=%d", (int)words3DMap.size());
 			Signature tmpFrom2(fromS.id());
-			tmpFrom2.setWords3(words3DMap);
-			tmpFrom2.setWords(wordsMap);
-			tmpFrom2.setWordsDescriptors(wordsDescriptorsMap);
+			tmpFrom2.setWords(words, wordsMap, words3DMap, wordsDescriptorsMap);

 			transform = _registrationPipeline->computeTransformationMod(tmpFrom2, tmpTo, guess, info);

-			if(!transform.isNull() && info)
+			if(!transform.isNull() && info && !tmpFrom2.getWords3().empty())
 			{
-				std::map<int, cv::Point3f> points3DMap = uMultimapToMapUnique(tmpFrom2.getWords3());
+				std::map<int, cv::Point3f> points3DMap;
+				std::map<int, int> wordsMap = uMultimapToMapUnique(tmpFrom2.getWords());
+				for(std::map<int, int>::iterator iter=wordsMap.begin(); iter!=wordsMap.end(); ++iter)
+				{
+					points3DMap.insert(std::make_pair(iter->first, tmpFrom2.getWords3()[iter->second]));
+				}
 				std::map<int, Transform> bundlePoses;
 				std::multimap<int, Link> bundleLinks;
 				std::map<int, CameraModel> bundleModels;
 				std::map<int, std::map<int, FeatureBA> > wordReferences;

 				std::multimap<int, Link> links = fromS.getLinks();
+				links = graph::filterLinks(links, Link::kNeighbor, true); // assemble only neighbors for the local feature map
 				links.insert(std::make_pair(toS.id(), Link(fromS.id(), toS.id(), Link::kGlobalClosure, transform, info->covariance.inv())));
 				links.insert(std::make_pair(fromS.id(), Link()));

+				int totalWordReferences = 0;
 				for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
 				{
 					int id = iter->first;
@@ -2912,33 +2920,41 @@ Transform Memory::computeTransform(
 								bundlePoses.insert(std::make_pair(id, iter->second.transform()));
 							}

-							const std::map<int,cv::KeyPoint> & words = uMultimapToMapUnique(s->getWords());
-							for(std::map<int, cv::KeyPoint>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
+							const std::map<int,int> & words = uMultimapToMapUnique(s->getWords());
+							for(std::map<int, int>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
 							{
 								if(points3DMap.find(jter->first)!=points3DMap.end() &&
-									(id == tmpTo.id() || jter->first > 0))
+										(id == tmpTo.id() || jter->first > 0)) // Since we added negative words of "from", only accept matches with current frame
 								{
-									std::multimap<int, cv::Point3f>::const_iterator kter = s->getWords3().find(jter->first);
-									cv::Point3f pt3d = util3d::transformPoint(kter->second, invLocalTransform);
+									//get depth
+									float d = 0.0f;
+									if( !s->getWords3().empty() &&
+										util3d::isFinite(s->getWords3()[jter->second]))
+									{
+										//move back point in camera frame (to get depth along z)
+										d = util3d::transformPoint(s->getWords3()[jter->second], invLocalTransform).z;
+									}
 									wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
-									wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(jter->second, pt3d.z)));
+									wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(s->getWordsKpts()[jter->second], d)));
+									++totalWordReferences;
 								}
 							}
 						}
 					}
 				}

+
 				UDEBUG("sba...start");
 				// set root negative to fix all other poses
 				std::set<int> sbaOutliers;
 				UTimer bundleTimer;
-				OptimizerG2O sba;
+				OptimizerG2O sba(parameters_);
 				sba.setIterations(5);
 				UTimer bundleTime;
 				bundlePoses = sba.optimizeBA(-toS.id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
 				UDEBUG("sba...end");

-				UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime.ticks(), (int)bundlePoses.size(), (int)wordReferences.size(), (int)sbaOutliers.size());
+				UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime.ticks(), (int)bundlePoses.size(), totalWordReferences, (int)sbaOutliers.size());

 				UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
 				if(!bundlePoses.rbegin()->second.isNull())
@@ -2980,36 +2996,6 @@ Transform Memory::computeTransform(
 		{
 			transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
 		}
-
-		if(!transform.isNull() &&
-			fromS.sensorData().cameraModels().size()<=1 &&
-			toS.sensorData().cameraModels().size()<=1)
-		{
-			UDEBUG("");
-			// verify if it is a 180 degree transform, well verify > 90
-			float x,y,z, roll,pitch,yaw;
-			if(guess.isNull())
-			{
-				transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-			}
-			else
-			{
-				Transform guessError = guess.inverse() * transform;
-				guessError.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-			}
-			if(fabs(pitch) > CV_PI/2 ||
-			   fabs(yaw) > CV_PI/2)
-			{
-				transform.setNull();
-				std::string msg = uFormat("Too large rotation detected! (pitch=%f, yaw=%f) max is %f",
-						roll, pitch, yaw, CV_PI/2);
-				UINFO(msg.c_str());
-				if(info)
-				{
-					info->rejectedMsg = msg;
-				}
-			}
-		}
 	}
 	return transform;
 }
@@ -3436,21 +3422,25 @@ void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
 			{
 				if(words3D)
 				{
-					const std::multimap<int, cv::Point3f> & ref = ss->getWords3();
-					for(std::multimap<int, cv::Point3f>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
+					if(!ss->getWords3().empty())
 					{
-						//show only valid point according to current parameters
-						if(pcl::isFinite(jter->second) &&
-						   (jter->second.x != 0 || jter->second.y != 0 || jter->second.z != 0))
+						const std::multimap<int, int> & ref = ss->getWords();
+						for(std::multimap<int, int>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
 						{
-							fprintf(foutSign, "%d ", (*jter).first);
+							const cv::Point3f & pt = ss->getWords3()[jter->second];
+							//show only valid point according to current parameters
+							if(pcl::isFinite(pt) &&
+							   (pt.x != 0 || pt.y != 0 || pt.z != 0))
+							{
+								fprintf(foutSign, "%d ", (*jter).first);
+							}
 						}
 					}
 				}
 				else
 				{
-					const std::multimap<int, cv::KeyPoint> & ref = ss->getWords();
-					for(std::multimap<int, cv::KeyPoint>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
+					const std::multimap<int, int> & ref = ss->getWords();
+					for(std::multimap<int, int>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
 					{
 						fprintf(foutSign, "%d ", (*jter).first);
 					}
@@ -3520,6 +3510,47 @@ void Memory::dumpMemoryTree(const char * fileNameTree) const

 }

+unsigned long Memory::getMemoryUsed() const
+{
+	unsigned long memoryUsage = sizeof(Memory);
+	memoryUsage += _signatures.size() * (sizeof(int)+sizeof(std::map<int, Signature *>::iterator)) + sizeof(std::map<int, Signature *>);
+	for(std::map<int, Signature*>::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
+	{
+		memoryUsage += iter->second->getMemoryUsed(true);
+	}
+	if(_vwd)
+	{
+		memoryUsage += _vwd->getMemoryUsed();
+	}
+	memoryUsage += _stMem.size() * (sizeof(int)+sizeof(std::set<int>::iterator)) + sizeof(std::set<int>);
+	memoryUsage += _workingMem.size() * (sizeof(int)+sizeof(double)+sizeof(std::map<int, double>::iterator)) + sizeof(std::map<int, double>);
+	memoryUsage += _groundTruths.size() * (sizeof(int)+sizeof(Transform)+12*sizeof(float) + sizeof(std::map<int, Transform>::iterator)) + sizeof(std::map<int, Transform>);
+	memoryUsage += _labels.size() * (sizeof(int)+sizeof(std::string) + sizeof(std::map<int, std::string>::iterator)) + sizeof(std::map<int, std::string>);
+	for(std::map<int, std::string>::const_iterator iter=_labels.begin(); iter!=_labels.end(); ++iter)
+	{
+		memoryUsage+=iter->second.size();
+	}
+	memoryUsage += _landmarksIndex.size() * (sizeof(int)+sizeof(std::set<int>) + sizeof(std::map<int, std::set<int> >::iterator)) + sizeof(std::map<int, std::set<int> >);
+	memoryUsage += _landmarksInvertedIndex.size() * (sizeof(int)+sizeof(std::set<int>) + sizeof(std::map<int, std::set<int> >::iterator)) + sizeof(std::map<int, std::set<int> >);
+	for(std::map<int, std::set<int>>::const_iterator iter=_landmarksIndex.begin(); iter!=_landmarksIndex.end(); ++iter)
+	{
+		memoryUsage+=iter->second.size()*(sizeof(int)+sizeof(std::set<int>::iterator)) + sizeof(std::set<int>);
+	}
+	for(std::map<int, std::set<int>>::const_iterator iter=_landmarksInvertedIndex.begin(); iter!=_landmarksInvertedIndex.end(); ++iter)
+	{
+		memoryUsage+=iter->second.size()*(sizeof(int)+sizeof(std::set<int>::iterator)) + sizeof(std::set<int>);
+	}
+	memoryUsage += parameters_.size()*(sizeof(std::string)*2+sizeof(ParametersMap::iterator)) + sizeof(ParametersMap);
+	memoryUsage += sizeof(Feature2D) + _feature2D->getParameters().size()*(sizeof(std::string)*2+sizeof(ParametersMap::iterator)) + sizeof(ParametersMap);
+	memoryUsage += sizeof(Registration);
+	memoryUsage += sizeof(RegistrationIcp);
+	memoryUsage += _occupancy->getMemoryUsed();
+	memoryUsage += sizeof(MarkerDetector);
+	memoryUsage += sizeof(DBDriver);
+
+	return memoryUsage;
+}
+
 void Memory::rehearsal(Signature * signature, Statistics * stats)
 {
 	UTimer timer;
@@ -3629,6 +3660,10 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 		{
 			fullMerge = newS->hasLink(oldS->id()) && newS->getLinks().begin()->second.transform().isNull();
 		}
+		UDEBUG("fullMerge=%s intermediateMerge=%s _idUpdatedToNewOneRehearsal=%s",
+				fullMerge?"true":"false",
+				intermediateMerge?"true":"false",
+				_idUpdatedToNewOneRehearsal?"true":"false");

 		if(fullMerge)
 		{
@@ -3679,6 +3714,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 				{
 					_lastGlobalLoopClosureId = newS->id();
 				}
+				oldS->setWeight(-9);
 			}
 			else
 			{
@@ -3691,7 +3727,9 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 				{
 					_lastSignature = oldS;
 				}
+				newS->setWeight(-9);
 			}
+			UDEBUG("New weights: %d->%d %d->%d", oldS->id(), oldS->getWeight(), newS->id(), oldS->getWeight());

 			// remove location
 			moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, _notLinkedNodesKeptInDb);
@@ -3911,9 +3949,10 @@ SensorData Memory::getNodeData(int locationId, bool images, bool scan, bool user
 }

 void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
-		std::multimap<int, cv::KeyPoint> & words,
-		std::multimap<int, cv::Point3f> & words3,
-		std::multimap<int, cv::Mat> & wordsDescriptors,
+		std::multimap<int, int> & words,
+		std::vector<cv::KeyPoint> & wordsKpts,
+		std::vector<cv::Point3f> & words3,
+		cv::Mat & wordsDescriptors,
 		std::vector<GlobalDescriptor> & globalDescriptors) const
 {
 	//UDEBUG("nodeId=%d", nodeId);
@@ -3921,6 +3960,7 @@ void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
 	if(s)
 	{
 		words = s->getWords();
+		wordsKpts = s->getWordsKpts();
 		words3 = s->getWords3();
 		wordsDescriptors = s->getWordsDescriptors();
 		globalDescriptors = s->sensorData().globalDescriptors();
@@ -3936,6 +3976,7 @@ void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
 		if(signatures.size())
 		{
 			words = signatures.front()->getWords();
+			wordsKpts = signatures.front()->getWordsKpts();
 			words3 = signatures.front()->getWords3();
 			wordsDescriptors = signatures.front()->getWordsDescriptors();
 			globalDescriptors = signatures.front()->sensorData().globalDescriptors();
@@ -4005,7 +4046,7 @@ void Memory::copyData(const Signature * from, Signature * to)
 	{
 		// words 2d
 		this->disableWordsRef(to->id());
-		to->setWords(from->getWords());
+		to->setWords(from->getWords(), from->getWordsKpts(), from->getWords3(), from->getWordsDescriptors());
 		std::list<int> id;
 		id.push_back(to->id());
 		this->enableWordsRef(id);
@@ -4022,8 +4063,6 @@ void Memory::copyData(const Signature * from, Signature * to)
 		to->sensorData().setId(to->id());

 		to->setPose(from->getPose());
-		to->setWords3(from->getWords3());
-		to->setWordsDescriptors(from->getWordsDescriptors());
 	}
 	else
 	{
@@ -4219,6 +4258,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	int preDecimation = 1;
 	std::vector<cv::Point3f> keypoints3D;
 	SensorData decimatedData;
+	UDEBUG("Received kpts=%d kpts3D=%d, descriptors=%d _useOdometryFeatures=%s",
+			(int)data.keypoints().size(), (int)data.keypoints3D().size(), data.descriptors().rows, _useOdometryFeatures?"true":"false");
 	if(!_useOdometryFeatures ||
 		data.keypoints().empty() ||
 		(int)data.keypoints().size() != data.descriptors().rows ||
@@ -4480,7 +4521,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						keypoints3D[i] = data.keypoints3D()[keypoints[i].class_id];
 					}
 				}
-				else if(keypoints.size() == data.keypoints3D().size())
+				else if(useProvided3dPoints && keypoints.size() == data.keypoints3D().size())
 				{
 					UDEBUG("Using provided 3d points (%d)", (int)data.keypoints3D().size());
 					keypoints3D = data.keypoints3D();
@@ -4679,9 +4720,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		UDEBUG("id %d is a bad signature", id);
 	}

-	std::multimap<int, cv::KeyPoint> words;
-	std::multimap<int, cv::Point3f> words3D;
-	std::multimap<int, cv::Mat> wordsDescriptors;
+	std::multimap<int, int> words;
+	std::vector<cv::KeyPoint> wordsKpts;
+	std::vector<cv::Point3f> words3D;
+	cv::Mat wordsDescriptors;
 	int words3DValid = 0;
 	if(wordIds.size() > 0)
 	{
@@ -4701,11 +4743,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				kpt.size *= decimationRatio;
 				kpt.octave += log2value;
 			}
-			words.insert(std::pair<int, cv::KeyPoint>(*iter, kpt));
+			words.insert(std::make_pair(*iter, words.size()));
+			wordsKpts.push_back(kpt);

 			if(keypoints3D.size())
 			{
-				words3D.insert(std::pair<int, cv::Point3f>(*iter, keypoints3D.at(i)));
+				words3D.push_back(keypoints3D.at(i));
 				if(util3d::isFinite(keypoints3D.at(i)))
 				{
 					++words3DValid;
@@ -4713,7 +4756,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			}
 			if(_rawDescriptorsKept)
 			{
-				wordsDescriptors.insert(std::pair<int, cv::Mat>(*iter, descriptors.row(i).clone()));
+				wordsDescriptors.push_back(descriptors.row(i));
 			}
 		}
 	}
@@ -4833,18 +4876,32 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor

 			Signature cpPrevious(2);
 			// IDs should be unique so that registration doesn't override them
-			std::map<int, cv::KeyPoint> uniqueWords = uMultimapToMapUnique(previousS->getWords());
-			std::map<int, cv::Mat> uniqueWordsDescriptors = uMultimapToMapUnique(previousS->getWordsDescriptors());
+			std::map<int, int> uniqueWordsOld = uMultimapToMapUnique(previousS->getWords());
+			std::vector<cv::KeyPoint> uniqueWordsKpts;
+			cv::Mat uniqueWordsDescriptors;
+			std::multimap<int, int> uniqueWords;
+			for(std::map<int, int>::iterator iter=uniqueWordsOld.begin(); iter!=uniqueWordsOld.end(); ++iter)
+			{
+				uniqueWords.insert(std::make_pair(iter->first, uniqueWords.size()));
+				uniqueWordsKpts.push_back(previousS->getWordsKpts()[iter->second]);
+				uniqueWordsDescriptors.push_back(previousS->getWordsDescriptors().row(iter->second));
+			}
 			cpPrevious.sensorData().setCameraModels(previousS->sensorData().cameraModels());
-			cpPrevious.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
-			cpPrevious.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
+			cpPrevious.setWords(uniqueWords, uniqueWordsKpts, std::vector<cv::Point3f>(), uniqueWordsDescriptors);
 			Signature cpCurrent(1);
-			uniqueWords = uMultimapToMapUnique(words);
-			uniqueWordsDescriptors = uMultimapToMapUnique(wordsDescriptors);
+			uniqueWordsOld = uMultimapToMapUnique(words);
+			uniqueWordsKpts.clear();
+			uniqueWordsDescriptors = cv::Mat();
+			uniqueWords.clear();
+			for(std::map<int, int>::iterator iter=uniqueWordsOld.begin(); iter!=uniqueWordsOld.end(); ++iter)
+			{
+				uniqueWords.insert(std::make_pair(iter->first, uniqueWords.size()));
+				uniqueWordsKpts.push_back(wordsKpts[iter->second]);
+				uniqueWordsDescriptors.push_back(wordsDescriptors.row(iter->second));
+			}
 			cpCurrent.sensorData().setCameraModels(cameraModels);
 			// This will force comparing descriptors between both images directly
-			cpCurrent.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
-			cpCurrent.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
+			cpCurrent.setWords(uniqueWords, uniqueWordsKpts, std::vector<cv::Point3f>(), uniqueWordsDescriptors);

 			// The following is used only to re-estimate the correspondences, the returned transform is ignored
 			Transform tmpt;
@@ -4862,9 +4919,21 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			UDEBUG("t=%s", tmpt.prettyPrint().c_str());

 			// compute 3D words by epipolar geometry with the previous signature using odometry motion
+			std::map<int, int> currentUniqueWords = uMultimapToMapUnique(cpCurrent.getWords());
+			std::map<int, int> previousUniqueWords = uMultimapToMapUnique(cpPrevious.getWords());
+			std::map<int, cv::KeyPoint> currentWords;
+			std::map<int, cv::KeyPoint> previousWords;
+			for(std::map<int, int>::iterator iter=currentUniqueWords.begin(); iter!=currentUniqueWords.end(); ++iter)
+			{
+				currentWords.insert(std::make_pair(iter->first, cpCurrent.getWordsKpts()[iter->second]));
+			}
+			for(std::map<int, int>::iterator iter=previousUniqueWords.begin(); iter!=previousUniqueWords.end(); ++iter)
+			{
+				previousWords.insert(std::make_pair(iter->first, cpPrevious.getWordsKpts()[iter->second]));
+			}
 			std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
-					uMultimapToMapUnique(cpCurrent.getWords()),
-					uMultimapToMapUnique(cpPrevious.getWords()),
+					currentWords,
+					previousWords,
 					cameraModels[0],
 					cameraTransform);

@@ -4875,32 +4944,26 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			UASSERT(words3D.size() == 0 || words.size() == words3D.size());
 			bool words3DWasEmpty = words3D.empty();
 			int added3DPointsWithoutDepth = 0;
-			for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+			for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 			{
 				std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
-				std::multimap<int, cv::Point3f>::iterator iter3D = words3D.find(iter->first);
-				if(iter3D == words3D.end())
+				if(words3DWasEmpty)
 				{
 					if(jter != inliers.end())
 					{
-						words3D.insert(std::make_pair(iter->first, jter->second));
+						words3D.push_back(jter->second);
 						++added3DPointsWithoutDepth;
 					}
 					else
 					{
-						words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
+						words3D.push_back(cv::Point3f(bad_point,bad_point,bad_point));
 					}
 				}
-				else if(!util3d::isFinite(iter3D->second) && jter != inliers.end())
+				else if(!util3d::isFinite(words3D[iter->second]) && jter != inliers.end())
 				{
-					iter3D->second = jter->second;
+					words3D[iter->second] = jter->second;
 					++added3DPointsWithoutDepth;
 				}
-				else if(words3DWasEmpty && jter == inliers.end())
-				{
-					// duplicate
-					words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
-				}
 			}
 			UDEBUG("added3DPointsWithoutDepth=%d", added3DPointsWithoutDepth);
 			if(stats) stats->addStatistic(Statistics::kMemoryTriangulated_points(), (float)added3DPointsWithoutDepth);
@@ -5153,9 +5216,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						compressedUserData));
 	}

-	s->setWords(words);
-	s->setWords3(words3D);
-	s->setWordsDescriptors(wordsDescriptors);
+	s->setWords(words, wordsKpts, words3D, wordsDescriptors);

 	// set raw data
 	if(!cameraModels.empty())
@@ -5320,7 +5381,7 @@ void Memory::disableWordsRef(int signatureId)
 	Signature * ss = this->_getSignature(signatureId);
 	if(ss && ss->isEnabled())
 	{
-		const std::multimap<int, cv::KeyPoint> & words = ss->getWords();
+		const std::multimap<int, int> & words = ss->getWords();
 		const std::list<int> & keys = uUniqueKeys(words);
 		int count = _vwd->getTotalActiveReferences();
 		// First remove all references
@@ -1532,4 +1532,36 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 	return updated;
 }

+unsigned long OccupancyGrid::getMemoryUsed() const
+{
+	unsigned long memoryUsage = sizeof(OccupancyGrid);
+	memoryUsage += parameters_.size()*(sizeof(std::string)*2+sizeof(ParametersMap::iterator)) + sizeof(ParametersMap);
+
+	memoryUsage += cache_.size()*(sizeof(int) + sizeof(std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat>) + sizeof(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator)) + sizeof(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >);
+	for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::const_iterator iter=cache_.begin(); iter!=cache_.end(); ++iter)
+	{
+		memoryUsage += iter->second.first.first.total() * iter->second.first.first.elemSize();
+		memoryUsage += iter->second.first.second.total() * iter->second.first.second.elemSize();
+		memoryUsage += iter->second.second.total() * iter->second.second.elemSize();
+	}
+	memoryUsage += map_.total() * map_.elemSize();
+	memoryUsage += mapInfo_.total() * mapInfo_.elemSize();
+	memoryUsage += cellCount_.size()*(sizeof(int)*3 + sizeof(std::pair<int, int>) + sizeof(std::map<int, std::pair<int, int> >::iterator)) + sizeof(std::map<int, std::pair<int, int> >);
+	memoryUsage += addedNodes_.size()*(sizeof(int) + sizeof(Transform)+ sizeof(float)*12 + sizeof(std::map<int, Transform>::iterator)) + sizeof(std::map<int, Transform>);
+
+	if(assembledGround_.get())
+	{
+		memoryUsage += assembledGround_->points.size() * sizeof(pcl::PointXYZRGB);
+	}
+	if(assembledObstacles_.get())
+	{
+		memoryUsage += assembledObstacles_->points.size() * sizeof(pcl::PointXYZRGB);
+	}
+	if(assembledEmptyCells_.get())
+	{
+		memoryUsage += assembledEmptyCells_->points.size() * sizeof(pcl::PointXYZRGB);
+	}
+	return memoryUsage;
+}
+
 }
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util3d_mapping.h>
+#include <rtabmap/core/util2d.h>
 #include <pcl/common/transforms.h>

 namespace rtabmap {
@@ -886,55 +887,6 @@ void OctoMap::updateMinMax(const octomap::point3d & point)
 	}
 }

-void OctoMap::HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
-{
-	int i;
-	float f, p, q, t;
-	if( s == 0 ) {
-		// achromatic (grey)
-		*r = *g = *b = v;
-		return;
-	}
-	h /= 60;			// sector 0 to 5
-	i = floor( h );
-	f = h - i;			// factorial part of h
-	p = v * ( 1 - s );
-	q = v * ( 1 - s * f );
-	t = v * ( 1 - s * ( 1 - f ) );
-	switch( i ) {
-		case 0:
-			*r = v;
-			*g = t;
-			*b = p;
-			break;
-		case 1:
-			*r = q;
-			*g = v;
-			*b = p;
-			break;
-		case 2:
-			*r = p;
-			*g = v;
-			*b = t;
-			break;
-		case 3:
-			*r = p;
-			*g = q;
-			*b = v;
-			break;
-		case 4:
-			*r = t;
-			*g = p;
-			*b = v;
-			break;
-		default:		// case 5:
-			*r = v;
-			*g = p;
-			*b = q;
-			break;
-	}
-}
-
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 		unsigned int treeDepth,
 		std::vector<int> * obstacleIndices,
@@ -1003,7 +955,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 				// Gradiant color on z axis
 				float H = (maxZ - pt.z())*299.0f/(maxZ-minZ);
 				float r,g,b;
-				HSVtoRGB(&r, &g, &b, H, 1, 1);
+				util2d::HSVtoRGB(&r, &g, &b, H, 1, 1);
 				(*cloud)[oi].r = r*255.0f;
 				(*cloud)[oi].g = g*255.0f;
 				(*cloud)[oi].b = b*255.0f;
@@ -308,8 +308,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			}
 			if(stereoModel_.isRectificationMapInitialized())
 			{
-				data.setImageRaw(stereoModel_.left().rectifyImage(data.imageRaw()));
-				data.setDepthOrRightRaw(stereoModel_.right().rectifyImage(data.rightRaw()));
+				data.setStereoImage(
+						stereoModel_.left().rectifyImage(data.imageRaw()),
+						stereoModel_.right().rectifyImage(data.rightRaw()),
+						stereoModel_,
+						false);
 			}
 		}
 		else
@@ -101,16 +101,16 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame
 	}
 	if(!OptimizerG2O::available() && type == Optimizer::kTypeG2O)
 	{
-		if(OptimizerTORO::available())
-		{
-			UWARN("g2o optimizer not available. TORO will be used instead.");
-					type = Optimizer::kTypeTORO;
-		}
-		else if(OptimizerGTSAM::available())
+		if(OptimizerGTSAM::available())
 		{
 			UWARN("g2o optimizer not available. GTSAM will be used instead.");
 					type = Optimizer::kTypeGTSAM;
 		}
+		else if(OptimizerTORO::available())
+		{
+			UWARN("g2o optimizer not available. TORO will be used instead.");
+					type = Optimizer::kTypeTORO;
+		}
 		else if(OptimizerCeres::available())
 		{
 			UWARN("g2o optimizer not available. ceres will be used instead.");
@@ -119,16 +119,16 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame
 	}
 	if(!OptimizerGTSAM::available() && type == Optimizer::kTypeGTSAM)
 	{
-		if(OptimizerTORO::available())
-		{
-			UWARN("GTSAM optimizer not available. TORO will be used instead.");
-					type = Optimizer::kTypeTORO;
-		}
-		else if(OptimizerG2O::available())
+		if(OptimizerG2O::available())
 		{
 			UWARN("GTSAM optimizer not available. g2o will be used instead.");
 					type = Optimizer::kTypeG2O;
 		}
+		else if(OptimizerTORO::available())
+		{
+			UWARN("GTSAM optimizer not available. TORO will be used instead.");
+					type = Optimizer::kTypeTORO;
+		}
 		else if(OptimizerCeres::available())
 		{
 			UWARN("GTSAM optimizer not available. ceres will be used instead.");
@@ -137,26 +137,11 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame
 	}
 	if(!OptimizerCVSBA::available() && type == Optimizer::kTypeCVSBA)
 	{
-		if(OptimizerTORO::available())
-		{
-			UWARN("CVSBA optimizer not available. TORO will be used instead.");
-					type = Optimizer::kTypeTORO;
-		}
-		else if(OptimizerGTSAM::available())
-		{
-			UWARN("CVSBA optimizer not available. GTSAM will be used instead.");
-					type = Optimizer::kTypeGTSAM;
-		}
-		else if(OptimizerG2O::available())
+		if(OptimizerG2O::available())
 		{
 			UWARN("CVSBA optimizer not available. g2o will be used instead.");
 					type = Optimizer::kTypeG2O;
 		}
-		else if(OptimizerCeres::available())
-		{
-			UWARN("CVSBA optimizer not available. ceres will be used instead.");
-					type = Optimizer::kTypeCeres;
-		}
 	}
 	if(!OptimizerCeres::available() && type == Optimizer::kTypeCeres)
 	{
@@ -617,8 +602,8 @@ void Optimizer::computeBACorrespondences(

 					if(!rematchFeatures)
 					{
-						sFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
-						sTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+						sFrom.setWordsDescriptors(cv::Mat());
+						sTo.setWordsDescriptors(cv::Mat());
 					}

 					RegistrationInfo info;
@@ -633,13 +618,13 @@ void Optimizer::computeBACorrespondences(
 							// set descriptors for the output
 							if(sFrom.getWords().size() &&
 							   sFrom.getWordsDescriptors().empty() &&
-							   sFrom.getWords().size() == signatures.at(link.from()).getWordsDescriptors().size())
+							   (int)sFrom.getWords().size() == signatures.at(link.from()).getWordsDescriptors().rows)
 							{
 								sFrom.setWordsDescriptors(signatures.at(link.from()).getWordsDescriptors());
 							}
 							if(sTo.getWords().size() &&
 							   sTo.getWordsDescriptors().empty() &&
-							   sTo.getWords().size() == signatures.at(link.to()).getWordsDescriptors().size())
+							   (int)sTo.getWords().size() == signatures.at(link.to()).getWordsDescriptors().rows)
 							{
 								sTo.setWordsDescriptors(signatures.at(link.to()).getWordsDescriptors());
 							}
@@ -649,11 +634,13 @@ void Optimizer::computeBACorrespondences(
 						UASSERT(!pose.isNull());
 						for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
 						{
-							cv::Point3f p = sFrom.getWords3().lower_bound(info.inliersIDs[i])->second;
+							int indexFrom = sFrom.getWords().lower_bound(info.inliersIDs[i])->second;
+							cv::Point3f p = sFrom.getWords3()[indexFrom];
 							if(p.x > 0.0f) // make sure the point is valid
 							{
-								cv::KeyPoint ptFrom = sFrom.getWords().lower_bound(info.inliersIDs[i])->second;
-								cv::KeyPoint ptTo = sTo.getWords().lower_bound(info.inliersIDs[i])->second;
+								cv::KeyPoint ptFrom = sFrom.getWordsKpts()[indexFrom];
+								int indexTo = sTo.getWords().lower_bound(info.inliersIDs[i])->second;
+								cv::KeyPoint ptTo = sTo.getWordsKpts()[indexTo];

 								int wordId = -1;

@@ -692,10 +679,10 @@ void Optimizer::computeBACorrespondences(
 								if(!fromAlreadyAdded)
 								{
 									cv::Mat descriptorFrom;
-									if(sFrom.getWordsDescriptors().size())
+									if(!sFrom.getWordsDescriptors().empty())
 									{
-										UASSERT(sFrom.getWordsDescriptors().find(info.inliersIDs[i]) != sFrom.getWordsDescriptors().end());
-										descriptorFrom = sFrom.getWordsDescriptors().lower_bound(info.inliersIDs[i])->second;
+										UASSERT(indexFrom < sFrom.getWordsDescriptors().rows);
+										descriptorFrom = sFrom.getWordsDescriptors().row(indexFrom);
 									}
 									wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), FeatureBA(ptFrom, p.x, descriptorFrom)));
 									frameToWordMap.insert(std::make_pair(sFrom.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
@@ -705,17 +692,20 @@ void Optimizer::computeBACorrespondences(
 								if(!toAlreadyAdded)
 								{
 									cv::Mat descriptorTo;
-									if(sTo.getWordsDescriptors().size())
+									if(!sTo.getWordsDescriptors().empty())
 									{
-										UASSERT(sTo.getWordsDescriptors().find(info.inliersIDs[i]) != sTo.getWordsDescriptors().end());
-										descriptorTo = sTo.getWordsDescriptors().lower_bound(info.inliersIDs[i])->second;
+										UASSERT(indexTo < sTo.getWordsDescriptors().rows);
+										descriptorTo = sTo.getWordsDescriptors().row(indexTo);
 									}
 									float depth = 0.0f;
-									std::multimap<int, cv::Point3f>::const_iterator iterTo = sTo.getWords3().lower_bound(info.inliersIDs[i]);
-									if( iterTo!=sTo.getWords3().end() &&
-										iterTo->second.x > 0)
+									if(!sTo.getWords3().empty())
 									{
-										depth = iterTo->second.x;
+										UASSERT(indexTo < (int)sTo.getWords3().size());
+										const cv::Point3f & pt = sTo.getWords3()[indexTo];
+										if( pt.x > 0)
+										{
+											depth = pt.x;
+										}
 									}
 									wordReferences.at(wordId).insert(std::make_pair(sTo.id(), FeatureBA(ptTo, depth, descriptorTo)));
 									frameToWordMap.insert(std::make_pair(sTo.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
@@ -706,6 +706,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With K4A:";
+#ifdef RTABMAP_K4A
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With DC1394:";
 #ifdef RTABMAP_DC1394
@@ -133,6 +133,18 @@ bool databaseRecovery(
 			*errorMsg = uFormat("Failed renaming database file from \"%s\" to \"%s\". Is it opened by another app?", UFile::getName(databasePath).c_str(), UFile::getName(backupPath).c_str());
 		return false;
 	}
+
+	bool incrementalMemory = true;
+	Parameters::parse(parameters, Parameters::kMemIncrementalMemory(), incrementalMemory);
+	if(!incrementalMemory)
+	{
+		if(progressState)
+		{
+			progressState->callback("Database is in localization mode, setting it to mapping mode to recover...");
+		}
+		uInsert(parameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true"));
+	}
+
 	Rtabmap rtabmap;
 	rtabmap.init(parameters, databasePath);

@@ -38,10 +38,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <pcl/conversions.h>
 #include <pcl/common/pca.h>
+#include <pcl/common/io.h>

 #ifdef RTABMAP_POINTMATCHER
 #include <fstream>
 #include "pointmatcher/PointMatcher.h"
+#include "nabo/nabo.h"
 typedef PointMatcher<float> PM;
 typedef PM::DataPoints DP;

@@ -181,6 +183,7 @@ DP laserScanToDP(const rtabmap::LaserScan & scan)
 		descLabels.push_back(Label("intensity", 1));
 	}

+
 	// create cloud
 	DP cloud(featLabels, descLabels, scan.size());
 	cloud.getFeatureViewByName("pad").setConstant(1);
@@ -359,6 +362,110 @@ typename PointMatcher<T>::TransformationParameters eigenMatrixToDim(const typena
 	return out;
 }

+template<typename T>
+struct KDTreeMatcherIntensity : public PointMatcher<T>::Matcher
+{
+	typedef PointMatcherSupport::Parametrizable Parametrizable;
+	typedef PointMatcherSupport::Parametrizable P;
+	typedef Parametrizable::Parameters Parameters;
+	typedef Parametrizable::ParameterDoc ParameterDoc;
+	typedef Parametrizable::ParametersDoc ParametersDoc;
+
+	typedef typename Nabo::NearestNeighbourSearch<T> NNS;
+	typedef typename NNS::SearchType NNSearchType;
+
+	typedef typename PointMatcher<T>::DataPoints DataPoints;
+	typedef typename PointMatcher<T>::Matcher Matcher;
+	typedef typename PointMatcher<T>::Matches Matches;
+	typedef typename PointMatcher<T>::Matrix Matrix;
+
+	inline static const std::string description()
+	{
+		return "This matcher matches a point from the reading to its closest neighbors in the reference.";
+	}
+	inline static const ParametersDoc availableParameters()
+	{
+		return {
+			{"knn", "number of nearest neighbors to consider it the reference", "1", "1", "2147483647", &P::Comp<unsigned>},
+			{"epsilon", "approximation to use for the nearest-neighbor search", "0", "0", "inf", &P::Comp<T>},
+			{"searchType", "Nabo search type. 0: brute force, check distance to every point in the data (very slow), 1: kd-tree with linear heap, good for small knn (~up to 30) and 2: kd-tree with tree heap, good for large knn (~from 30)", "1", "0", "2", &P::Comp<unsigned>},
+			{"maxDist", "maximum distance to consider for neighbors", "inf", "0", "inf", &P::Comp<T>}
+		};
+	}
+
+	const int knn;
+	const T epsilon;
+	const NNSearchType searchType;
+	const T maxDist;
+
+protected:
+	std::shared_ptr<NNS> featureNNS;
+	Matrix filteredReferenceIntensity;
+
+public:
+	KDTreeMatcherIntensity(const Parameters& params = Parameters()) :
+		PointMatcher<T>::Matcher("KDTreeMatcherIntensity", KDTreeMatcherIntensity::availableParameters(), params),
+		knn(Parametrizable::get<int>("knn")),
+		epsilon(Parametrizable::get<T>("epsilon")),
+		searchType(NNSearchType(Parametrizable::get<int>("searchType"))),
+		maxDist(Parametrizable::get<T>("maxDist"))
+	{
+		UINFO("* KDTreeMatcherIntensity: initialized with knn=%d, epsilon=%f, searchType=%d and maxDist=%f", knn, epsilon, searchType, maxDist);
+	}
+	virtual ~KDTreeMatcherIntensity() {}
+	virtual void init(const DataPoints& filteredReference)
+	{
+		// build and populate NNS
+		if(knn>1)
+		{
+			filteredReferenceIntensity = filteredReference.getDescriptorCopyByName("intensity");
+		}
+		else
+		{
+			UWARN("KDTreeMatcherIntensity: knn is not over 1 (%d), intensity re-ordering will be ignored.", knn);
+		}
+		featureNNS.reset( NNS::create(filteredReference.features, filteredReference.features.rows() - 1, searchType, NNS::TOUCH_STATISTICS));
+	}
+	virtual PM::Matches findClosests(const DP& filteredReading)
+	{
+		const int pointsCount(filteredReading.features.cols());
+		Matches matches(
+			typename Matches::Dists(knn, pointsCount),
+			typename Matches::Ids(knn, pointsCount)
+		);
+
+		const BOOST_AUTO(filteredReadingIntensity, filteredReading.getDescriptorViewByName("intensity"));
+
+		static_assert(NNS::InvalidIndex == PM::Matches::InvalidId, "");
+		static_assert(NNS::InvalidValue == PM::Matches::InvalidDist, "");
+		this->visitCounter += featureNNS->knn(filteredReading.features, matches.ids, matches.dists, knn, epsilon, NNS::ALLOW_SELF_MATCH, maxDist);
+
+		if(knn > 1)
+		{
+			Matches matchesOrderedByIntensity(
+				typename Matches::Dists(1, pointsCount),
+				typename Matches::Ids(1, pointsCount)
+			);
+			#pragma omp parallel for
+			for (int i = 0; i < pointsCount; ++i)
+			{
+				float minDistance = std::numeric_limits<float>::max();
+				for(int k=0; k<knn && k<filteredReferenceIntensity.rows(); ++k)
+				{
+					float distIntensity = fabs(filteredReadingIntensity(0,i) - filteredReferenceIntensity(0, matches.ids.coeff(k, i)));
+					if(distIntensity < minDistance)
+					{
+						matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(k, i);
+						matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(k, i);
+						minDistance = distIntensity;
+					}
+				}
+			}
+			matches = matchesOrderedByIntensity;
+		}
+		return matches;
+	}
+};
 #endif

 namespace rtabmap {
@@ -378,11 +485,14 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
 	_pointToPlane(Parameters::defaultIcpPointToPlane()),
 	_pointToPlaneK(Parameters::defaultIcpPointToPlaneK()),
 	_pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
+	_pointToPlaneGroundNormalsUp(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
 	_pointToPlaneMinComplexity(Parameters::defaultIcpPointToPlaneMinComplexity()),
+	_pointToPlaneLowComplexityStrategy(Parameters::defaultIcpPointToPlaneLowComplexityStrategy()),
 	_libpointmatcher(Parameters::defaultIcpPM()),
 	_libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
 	_libpointmatcherKnn(Parameters::defaultIcpPMMatcherKnn()),
 	_libpointmatcherEpsilon(Parameters::defaultIcpPMMatcherEpsilon()),
+	_libpointmatcherIntensity(Parameters::defaultIcpPMMatcherIntensity()),
 	_libpointmatcherOutlierRatio(Parameters::defaultIcpPMOutlierRatio()),
 	_libpointmatcherICP(0)
 {
@@ -413,7 +523,10 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), _pointToPlaneRadius);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlaneGroundNormalsUp(), _pointToPlaneGroundNormalsUp);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneMinComplexity(), _pointToPlaneMinComplexity);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlaneLowComplexityStrategy(), _pointToPlaneLowComplexityStrategy);
+	UASSERT(_pointToPlaneGroundNormalsUp >= 0.0f && _pointToPlaneGroundNormalsUp <= 1.0f);
 	UASSERT(_pointToPlaneMinComplexity >= 0.0f && _pointToPlaneMinComplexity <= 1.0f);

 	Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher);
@@ -421,6 +534,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kIcpPMOutlierRatio(), _libpointmatcherOutlierRatio);
 	Parameters::parse(parameters, Parameters::kIcpPMMatcherKnn(), _libpointmatcherKnn);
 	Parameters::parse(parameters, Parameters::kIcpPMMatcherEpsilon(), _libpointmatcherEpsilon);
+	Parameters::parse(parameters, Parameters::kIcpPMMatcherIntensity(), _libpointmatcherIntensity);

 #ifndef RTABMAP_POINTMATCHER
 	if(_libpointmatcher)
@@ -473,11 +587,19 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 			params["maxDist"] = uNumber2Str(_maxCorrespondenceDistance);
 			params["knn"] = uNumber2Str(_libpointmatcherKnn);
 			params["epsilon"] = uNumber2Str(_libpointmatcherEpsilon);
+
+			if(_libpointmatcherIntensity)
+			{
+				icp->matcher.reset(new KDTreeMatcherIntensity<float>(params));
+			}
+			else
+			{
 #if POINTMATCHER_VERSION_INT >= 10300
-			icp->matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
+				icp->matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
 #else
-			icp->matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
+				icp->matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
 #endif
+			}
 			params.clear();

 			params["ratio"] = uNumber2Str(_libpointmatcherOutlierRatio);
@@ -590,6 +712,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 			double variance = 1.0;
 			bool transformComputed = false;
 			bool tooLowComplexityForPlaneToPlane = false;
+			float secondEigenValue = 1.0f;
 			cv::Mat complexityVectors;

 			if( _pointToPlane &&
@@ -609,19 +732,29 @@ Transform RegistrationIcp::computeTransformationImpl(
 				if(complexity < _pointToPlaneMinComplexity)
 				{
 					tooLowComplexityForPlaneToPlane = true;
-					complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
-
-					UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)||
-							(complexityVectors.rows == 3 && complexityVectors.cols == 3));
-					UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). "
-						  "PointToPoint is done instead, orientation is still optimized but translation will be limited to "
-						  "direction of normals (%s: %s).",
-						  fromComplexity, toComplexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str(),
-						  fromComplexity<toComplexity?"From":"To",
-						  complexityVectors.rows==2?
-								  uFormat("n=%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1)).c_str():
-								  uFormat("n1=%f,%f,%f n2=%f,%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2), complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2)).c_str());
+					if(complexity > 0.0f)
+					{
+						complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;

+						UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)||
+								(complexityVectors.rows == 3 && complexityVectors.cols == 3));
+						secondEigenValue = complexityValuesFrom.at<float>(1,0)<complexityValuesTo.at<float>(1,0)?complexityValuesFrom.at<float>(1,0):complexityValuesTo.at<float>(1,0);
+						UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). Second eigen value=%f. "
+							  "PointToPoint is done instead, orientation is still optimized but translation will be limited to "
+							  "direction of normals (%s: %s).",
+							  fromComplexity, toComplexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str(),
+							  secondEigenValue,
+							  fromComplexity<toComplexity?"From":"To",
+							  complexityVectors.rows==2?
+									  uFormat("n=%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1)).c_str():
+									  secondEigenValue<_pointToPlaneMinComplexity?
+									  uFormat("n=%f,%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2)).c_str():
+									  uFormat("n1=%f,%f,%f n2=%f,%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2), complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2)).c_str());
+					}
+					else
+					{
+						UWARN("ICP PointToPlane ignored as structural complexity cannot be computed (from=%f to=%f)!? PointToPoint is done instead.", fromComplexity, toComplexity);
+					}
 					if(ULogger::level() == ULogger::kDebug)
 					{
 						std::cout << "complexityVectorsFrom = " << std::endl << complexityVectorsFrom << std::endl;
@@ -632,15 +765,15 @@ Transform RegistrationIcp::computeTransformationImpl(
 				}
 				else
 				{
-					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromScan.localTransform());
-					pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
+					pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudINormal(fromScan, fromScan.localTransform());
+					pcl::PointCloud<pcl::PointXYZINormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudINormal(toScan, guess * toScan.localTransform());

 					fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
 					toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);

 					if(fromCloudNormals->size() > 2 && toCloudNormals->size() > 2)
 					{
-						pcl::PCA<pcl::PointNormal> pca;
+						pcl::PCA<pcl::PointXYZINormal> pca;
 						pca.setInputCloud(fromCloudNormals);
 						Eigen::Vector3f valuesFrom = pca.getEigenValues();
 						pca.setInputCloud(toCloudNormals);
@@ -656,7 +789,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 					}

 					UDEBUG("Conversion time = %f s", timer.ticks());
-					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
+					pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointXYZINormal>());
 #ifdef RTABMAP_POINTMATCHER
 					if(_libpointmatcher)
 					{
@@ -721,13 +854,13 @@ Transform RegistrationIcp::computeTransformationImpl(
 			int maxLaserScansTo = toScan.maxPoints();
 			if(!transformComputed)
 			{
-				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromScan.localTransform());
-				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toScan.localTransform());
+				pcl::PointCloud<pcl::PointXYZI>::Ptr fromCloud = util3d::laserScanToPointCloudI(fromScan, fromScan.localTransform());
+				pcl::PointCloud<pcl::PointXYZI>::Ptr toCloud = util3d::laserScanToPointCloudI(toScan, guess * toScan.localTransform());
 				UDEBUG("Conversion time = %f s", timer.ticks());

 				if(fromCloud->size() > 2 && toCloud->size() > 2)
 				{
-					pcl::PCA<pcl::PointXYZ> pca;
+					pcl::PCA<pcl::PointXYZI> pca;
 					pca.setInputCloud(fromCloud);
 					Eigen::Vector3f valuesFrom = pca.getEigenValues();
 					pca.setInputCloud(toCloud);
@@ -740,10 +873,11 @@ Transform RegistrationIcp::computeTransformationImpl(
 					{
 						info.icpStructuralDistribution = sqrt(valuesTo[0]/toCloud->size());
 					}
+					UDEBUG("Computed icpStructuralDistribution %f s",timer.ticks());
 				}

-				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
-				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
+				pcl::PointCloud<pcl::PointXYZI>::Ptr fromCloudFiltered = fromCloud;
+				pcl::PointCloud<pcl::PointXYZI>::Ptr toCloudFiltered = toCloud;
 				if(_voxelSize > 0.0f)
 				{
 					float pointsBeforeFiltering = (float)fromCloudFiltered->size();
@@ -767,7 +901,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 							timer.ticks());
 				}

-				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
+				pcl::PointCloud<pcl::PointXYZI>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZI>());
 				if(_pointToPlane && // ICP Point To Plane
 					!tooLowComplexityForPlaneToPlane && // if previously rejected above
 					!((fromScan.is2d()|| toScan.is2d()) && !_libpointmatcher)) // PCL crashes if 2D
@@ -834,18 +968,24 @@ Transform RegistrationIcp::computeTransformationImpl(
 					if(complexity < _pointToPlaneMinComplexity)
 					{
 						tooLowComplexityForPlaneToPlane = true;
-						complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
-						UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)||
-								(complexityVectors.rows == 3 && complexityVectors.cols == 3));
-						UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). "
-							  "PointToPoint is done instead, orientation is still optimized but translation will be limited to "
-							  "direction of normals (%s: %s).",
-							  fromComplexity, toComplexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str(),
-							  fromComplexity<toComplexity?"From":"To",
-							  complexityVectors.rows==2?
-									  uFormat("n=%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1)).c_str():
-									  uFormat("n1=%f,%f,%f n2=%f,%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2), complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2)).c_str());
-
+						if(complexity > 0.0f)
+						{
+							complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
+							UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)||
+									(complexityVectors.rows == 3 && complexityVectors.cols == 3));
+							UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). "
+								  "PointToPoint is done instead, orientation is still optimized but translation will be limited to "
+								  "direction of normals (%s: %s).",
+								  fromComplexity, toComplexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str(),
+								  fromComplexity<toComplexity?"From":"To",
+								  complexityVectors.rows==2?
+										  uFormat("n=%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1)).c_str():
+										  uFormat("n1=%f,%f,%f n2=%f,%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2), complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2)).c_str());
+						}
+						else
+						{
+							UWARN("ICP PointToPlane ignored as structural complexity cannot be computed (from=%f to=%f)!? PointToPoint is done instead.", fromComplexity, toComplexity);
+						}
 						if(ULogger::level() == ULogger::kDebug)
 						{
 							std::cout << "complexityVectorsFrom = " << std::endl << complexityVectorsFrom << std::endl;
@@ -856,16 +996,31 @@ Transform RegistrationIcp::computeTransformationImpl(
 					}
 					else
 					{
-						pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointXYZINormal>);
 						pcl::concatenateFields(*fromCloudFiltered, *normalsFrom, *fromCloudNormals);

-						pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::PointCloud<pcl::PointXYZINormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointXYZINormal>);
 						pcl::concatenateFields(*toCloudFiltered, *normalsTo, *toCloudNormals);

 						std::vector<int> indices;
 						toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
 						fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);

+						if(!fromCloudNormals->empty() && !fromScan.is2d() && _pointToPlaneGroundNormalsUp>0.0f)
+						{
+							util3d::adjustNormalsToViewPoint(fromCloudNormals,
+									Eigen::Vector3f(fromScan.localTransform().x(),fromScan.localTransform().y(),fromScan.localTransform().z()+10),
+									_pointToPlaneGroundNormalsUp);
+						}
+						if(!toCloudNormals->empty() && !toScan.is2d() && _pointToPlaneGroundNormalsUp>0.0f)
+						{
+							Transform toT = guess * toScan.localTransform();
+							Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z()+10);
+							util3d::adjustNormalsToViewPoint(toCloudNormals,
+									viewpointTo,
+									_pointToPlaneGroundNormalsUp);
+						}
+
 						// update output scans
 						if(fromScan.is2d())
 						{
@@ -874,7 +1029,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 											util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
 											maxLaserScansFrom,
 											fromScan.rangeMax(),
-											LaserScan::kXYNormal,
+											LaserScan::kXYINormal,
 											fromScan.localTransform()));
 						}
 						else
@@ -884,7 +1039,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 											util3d::laserScanFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
 											maxLaserScansFrom,
 											fromScan.rangeMax(),
-											LaserScan::kXYZNormal,
+											LaserScan::kXYZINormal,
 											fromScan.localTransform()));
 						}
 						if(toScan.is2d())
@@ -894,7 +1049,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 											util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
 											maxLaserScansTo,
 											toScan.rangeMax(),
-											LaserScan::kXYNormal,
+											LaserScan::kXYINormal,
 											toScan.localTransform()));
 						}
 						else
@@ -904,7 +1059,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 											util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
 											maxLaserScansTo,
 											toScan.rangeMax(),
-											LaserScan::kXYZNormal,
+											LaserScan::kXYZINormal,
 											toScan.localTransform()));
 						}
 						UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());
@@ -913,7 +1068,7 @@ Transform RegistrationIcp::computeTransformationImpl(

 						if(toCloudNormals->size() && fromCloudNormals->size())
 						{
-							pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
+							pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointXYZINormal>());

 #ifdef RTABMAP_POINTMATCHER
 							if(_libpointmatcher)
@@ -993,7 +1148,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 											util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
 											maxLaserScansFrom,
 											fromScan.rangeMax(),
-											LaserScan::kXY,
+											LaserScan::kXYI,
 											fromScan.localTransform()));
 						}
 						else
@@ -1003,7 +1158,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 											util3d::laserScanFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
 											maxLaserScansFrom,
 											fromScan.rangeMax(),
-											LaserScan::kXYZ,
+											LaserScan::kXYZI,
 											fromScan.localTransform()));
 						}
 						if(toScan.is2d())
@@ -1013,7 +1168,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 											util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
 											maxLaserScansTo,
 											toScan.rangeMax(),
-											LaserScan::kXY,
+											LaserScan::kXYI,
 											toScan.localTransform()));
 						}
 						else
@@ -1023,7 +1178,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 											util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
 											maxLaserScansTo,
 											toScan.rangeMax(),
-											LaserScan::kXYZ,
+											LaserScan::kXYZI,
 											toScan.localTransform()));
 						}
 						fromScan = fromSignature.sensorData().laserScanRaw();
@@ -1116,70 +1271,86 @@ Transform RegistrationIcp::computeTransformationImpl(

 					if(!icpT.isNull() && hasConverged)
 					{
-						if(tooLowComplexityForPlaneToPlane)
+						if(tooLowComplexityForPlaneToPlane && _pointToPlaneLowComplexityStrategy<2)
 						{
-							Transform guessInv = guess.inverse();
-							Transform t = guessInv * icpT.inverse() * guess;
-							Eigen::Vector3f v(t.x(), t.y(), t.z());
-							if(complexityVectors.cols == 2)
+							if(complexityVectors.empty() || _pointToPlaneLowComplexityStrategy == 0)
 							{
-								// limit translation in direction of the first eigen vector
-								Eigen::Vector3f n(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), 0.0f);
-								float a = v.dot(n);
-								Eigen::Vector3f vp = n*a;
-								UWARN("Normals low complexity: Limiting translation from (%f,%f) to (%f,%f)",
-										v[0], v[1], vp[0], vp[1]);
-								v= vp;
+								msg = uFormat("Rejecting transform because too low complexity (%s=0)", Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
+								icpT.setNull();
+								UWARN(msg.c_str());
 							}
-							else if(complexityVectors.rows == 3)
+							else //if(_pointToPlaneLowComplexityStrategy == 1)
 							{
-								// limit translation in direction of the first and second eigen vectors
-								Eigen::Vector3f n1(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2));
-								Eigen::Vector3f n2(complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2));
-								float a = v.dot(n1);
-								float b = v.dot(n2);
-								Eigen::Vector3f vp = n1*a;
-								vp += n2*b;
-								UWARN("Normals low complexity: Limiting translation from (%f,%f,%f) to (%f,%f,%f)",
-										v[0], v[1], v[2], vp[0], vp[1], vp[2]);
-								v = vp;
-							}
-							else
-							{
-								UWARN("not supposed to be here!");
-								v = Eigen::Vector3f(0,0,0);
-							}
-							float roll, pitch, yaw;
-							t.getEulerAngles(roll, pitch, yaw);
-							t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
-							icpT = guess * t.inverse() * guessInv;
+								Transform guessInv = guess.inverse();
+								Transform t = guessInv * icpT.inverse() * guess;
+								Eigen::Vector3f v(t.x(), t.y(), t.z());
+								if(complexityVectors.cols == 2)
+								{
+									// limit translation in direction of the first eigen vector
+									Eigen::Vector3f n(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), 0.0f);
+									float a = v.dot(n);
+									Eigen::Vector3f vp = n*a;
+									UWARN("Normals low complexity: Limiting translation from (%f,%f) to (%f,%f)",
+											v[0], v[1], vp[0], vp[1]);
+									v= vp;
+								}
+								else if(complexityVectors.rows == 3)
+								{
+									// limit translation in direction of the first and second eigen vectors
+									Eigen::Vector3f n1(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2));
+									Eigen::Vector3f n2(complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2));
+									float a = v.dot(n1);
+									float b = v.dot(n2);
+									Eigen::Vector3f vp = n1*a;
+									if(secondEigenValue >= _pointToPlaneMinComplexity)
+									{
+										vp += n2*b;
+									}
+									UWARN("Normals low complexity: Limiting translation from (%f,%f,%f) to (%f,%f,%f)",
+											v[0], v[1], v[2], vp[0], vp[1], vp[2]);
+									v = vp;
+								}
+								else
+								{
+									UWARN("not supposed to be here!");
+									v = Eigen::Vector3f(0,0,0);
+								}
+								float roll, pitch, yaw;
+								t.getEulerAngles(roll, pitch, yaw);
+								t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
+								icpT = guess * t.inverse() * guessInv;

-							if(fromScan.hasNormals() && toScan.hasNormals())
-							{
-								// we were using normals, so compute correspondences using normals
-								pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
-								pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
+								if(fromScan.hasNormals() && toScan.hasNormals())
+								{
+									// we were using normals, so compute correspondences using normals
+									pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudINormal(fromScan, icpT * fromScan.localTransform());
+									pcl::PointCloud<pcl::PointXYZINormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudINormal(toScan, guess * toScan.localTransform());

-								util3d::computeVarianceAndCorrespondences(
-										fromCloudNormalsRegistered,
-										toCloudNormals,
-										_maxCorrespondenceDistance,
-										_maxRotation,
-										variance,
-										correspondences);
-							}
-							else
-							{
-								util3d::computeVarianceAndCorrespondences(
-										fromCloudRegistered,
-										toCloudFiltered,
-										_maxCorrespondenceDistance,
-										variance,
-										correspondences);
+									util3d::computeVarianceAndCorrespondences(
+											fromCloudNormalsRegistered,
+											toCloudNormals,
+											_maxCorrespondenceDistance,
+											_maxRotation,
+											variance,
+											correspondences);
+								}
+								else
+								{
+									util3d::computeVarianceAndCorrespondences(
+											fromCloudRegistered,
+											toCloudFiltered,
+											_maxCorrespondenceDistance,
+											variance,
+											correspondences);
+								}
 							}
 						}
 						else
 						{
+							if(tooLowComplexityForPlaneToPlane)
+							{
+								UWARN("Even if complexity is low , PointToPoint transformation is accepted \"as is\" (%s=2)", Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
+							}
 							util3d::computeVarianceAndCorrespondences(
 									fromCloudRegistered,
 									toCloudFiltered,
@@ -1254,6 +1425,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 					else
 					{
 						info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
+						info.covariance(cv::Range(3,6),cv::Range(3,6))/=10.0; //orientation error
 					}
 					info.icpInliersRatio = correspondencesRatio;
 					info.icpCorrespondences = correspondences;
@@ -213,6 +213,10 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	{
 		uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), parameters.at(Parameters::kVisCorNNDR())));
 	}
+	if(uContains(parameters, Parameters::kKpByteToFloat()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kKpByteToFloat(), parameters.at(Parameters::kKpByteToFloat())));
+	}
 	if(uContains(parameters, Parameters::kVisFeatureType()))
 	{
 		uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
@@ -299,7 +303,7 @@ Transform RegistrationVis::computeTransformationImpl(
 			fromSignature.id(),
 			(int)fromSignature.getWords().size(),
 			(int)fromSignature.getWords3().size(),
-			(int)fromSignature.getWordsDescriptors().size(),
+			(int)fromSignature.getWordsDescriptors().rows,
 			(int)fromSignature.sensorData().keypoints().size(),
 			(int)fromSignature.sensorData().keypoints3D().size(),
 			fromSignature.sensorData().descriptors().rows,
@@ -312,7 +316,7 @@ Transform RegistrationVis::computeTransformationImpl(
 			toSignature.id(),
 			(int)toSignature.getWords().size(),
 			(int)toSignature.getWords3().size(),
-			(int)toSignature.getWordsDescriptors().size(),
+			(int)toSignature.getWordsDescriptors().rows,
 			(int)toSignature.sensorData().keypoints().size(),
 			(int)toSignature.sensorData().keypoints3D().size(),
 			toSignature.sensorData().descriptors().rows,
@@ -345,16 +349,16 @@ Transform RegistrationVis::computeTransformationImpl(
 				fromSignature.getWords3().empty() ||
 				(fromSignature.getWords().size() == fromSignature.getWords3().size()));
 		UASSERT((int)fromSignature.sensorData().keypoints().size() == fromSignature.sensorData().descriptors().rows ||
-				fromSignature.getWords().size() == fromSignature.getWordsDescriptors().size() ||
-				fromSignature.sensorData().descriptors().rows == 0 ||
-				fromSignature.getWordsDescriptors().size() == 0);
+				(int)fromSignature.getWords().size() == fromSignature.getWordsDescriptors().rows ||
+				fromSignature.sensorData().descriptors().empty() ||
+				fromSignature.getWordsDescriptors().empty() == 0);
 		UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
 				(toSignature.getWords().size() && toSignature.getWords3().empty())||
 				(toSignature.getWords().size() == toSignature.getWords3().size()));
 		UASSERT((int)toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
-				toSignature.getWords().size() == toSignature.getWordsDescriptors().size() ||
-				toSignature.sensorData().descriptors().rows == 0 ||
-				toSignature.getWordsDescriptors().size() == 0);
+				(int)toSignature.getWords().size() == toSignature.getWordsDescriptors().rows ||
+				toSignature.sensorData().descriptors().empty() ||
+				toSignature.getWordsDescriptors().empty());
 		UASSERT(fromSignature.sensorData().imageRaw().empty() ||
 				fromSignature.sensorData().imageRaw().type() == CV_8UC1 ||
 				fromSignature.sensorData().imageRaw().type() == CV_8UC3);
@@ -367,6 +371,7 @@ Transform RegistrationVis::computeTransformationImpl(
 		cv::Mat imageTo = toSignature.sensorData().imageRaw();

 		std::vector<int> orignalWordsFromIds;
+		int kptsFromSource = 0;
 		if(fromSignature.getWords().empty())
 		{
 			if(fromSignature.sensorData().keypoints().empty())
@@ -399,22 +404,26 @@ Transform RegistrationVis::computeTransformationImpl(
 			else
 			{
 				kptsFrom = fromSignature.sensorData().keypoints();
+				kptsFromSource = 1;
 			}
 		}
 		else
 		{
-			kptsFrom.resize(fromSignature.getWords().size());
+			kptsFromSource = 2;
 			orignalWordsFromIds.resize(fromSignature.getWords().size());
 			int i=0;
 			bool allUniques = true;
-			for(std::multimap<int, cv::KeyPoint>::const_iterator iter=fromSignature.getWords().begin(); iter!=fromSignature.getWords().end(); ++iter)
+			int previousIdAdded = 0;
+			kptsFrom = fromSignature.getWordsKpts();
+			for(std::multimap<int, int>::const_iterator iter=fromSignature.getWords().begin(); iter!=fromSignature.getWords().end(); ++iter)
 			{
-				kptsFrom[i] = iter->second;
-				orignalWordsFromIds[i] = iter->first;
-				if(i>0 && iter->first==orignalWordsFromIds[i-1])
+				UASSERT(iter->second>=0 && iter->second<(int)orignalWordsFromIds.size());
+				orignalWordsFromIds[iter->second] = iter->first;
+				if(i>0 && iter->first==previousIdAdded)
 				{
 					allUniques = false;
 				}
+				previousIdAdded = iter->first;
 				++i;
 			}
 			if(!allUniques)
@@ -424,12 +433,14 @@ Transform RegistrationVis::computeTransformationImpl(
 			}
 		}

-		std::multimap<int, cv::KeyPoint> wordsFrom;
-		std::multimap<int, cv::KeyPoint> wordsTo;
-		std::multimap<int, cv::Point3f> words3From;
-		std::multimap<int, cv::Point3f> words3To;
-		std::multimap<int, cv::Mat> wordsDescFrom;
-		std::multimap<int, cv::Mat> wordsDescTo;
+		std::multimap<int, int> wordsFrom;
+		std::multimap<int, int> wordsTo;
+		std::vector<cv::KeyPoint> wordsKptsFrom;
+		std::vector<cv::KeyPoint> wordsKptsTo;
+		std::vector<cv::Point3f> words3From;
+		std::vector<cv::Point3f> words3To;
+		cv::Mat wordsDescFrom;
+		cv::Mat wordsDescTo;
 		if(_correspondencesApproach == 1) //Optical Flow
 		{
 			UDEBUG("");
@@ -450,7 +461,7 @@ Transform RegistrationVis::computeTransformationImpl(
 			std::vector<cv::Point3f> kptsFrom3D;
 			if(kptsFrom.size() == fromSignature.getWords3().size())
 			{
-				kptsFrom3D = uValues(fromSignature.getWords3());
+				kptsFrom3D = fromSignature.getWords3();
 			}
 			else if(kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
 			{
@@ -540,13 +551,16 @@ Transform RegistrationVis::computeTransformationImpl(
 				UASSERT(kptsTo3D.size() == 0 || kptsTo.size() == kptsTo3D.size());
 				for(unsigned int i=0; i< kptsFrom3DKept.size(); ++i)
 				{
-					int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
-					wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
-					words3From.insert(std::make_pair(id, kptsFrom3DKept[i]));
-					wordsTo.insert(std::make_pair(id, kptsTo[i]));
-					if(kptsTo3D.size())
+					int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
+					wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
+					wordsKptsFrom.push_back(kptsFrom[i]);
+					words3From.push_back(kptsFrom3DKept[i]);
+
+					wordsTo.insert(wordsTo.end(), std::make_pair(id, wordsTo.size()));
+					wordsKptsTo.push_back(kptsTo[i]);
+					if(!kptsTo3D.empty())
 					{
-						words3To.insert(std::make_pair(id, kptsTo3D[i]));
+						words3To.push_back(kptsTo3D[i]);
 					}
 				}
 				toSignature.sensorData().setFeatures(kptsTo, kptsTo3D, cv::Mat());
@@ -562,9 +576,10 @@ Transform RegistrationVis::computeTransformationImpl(
 				{
 					if(util3d::isFinite(kptsFrom3D[i]))
 					{
-						int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
-						wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
-						words3From.insert(std::make_pair(id, kptsFrom3D[i]));
+						int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
+						wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
+						wordsKptsFrom.push_back(kptsFrom[i]);
+						words3From.push_back(kptsFrom3D[i]);
 					}
 				}
 				toSignature.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
@@ -576,6 +591,7 @@ Transform RegistrationVis::computeTransformationImpl(
 		{
 			UDEBUG("");
 			std::vector<cv::KeyPoint> kptsTo;
+			int kptsToSource = 0;
 			if(toSignature.getWords().empty())
 			{
 				if(toSignature.sensorData().keypoints().empty() &&
@@ -606,33 +622,28 @@ Transform RegistrationVis::computeTransformationImpl(
 				else
 				{
 					kptsTo = toSignature.sensorData().keypoints();
+					kptsToSource = 1;
 				}
 			}
 			else
 			{
-				kptsTo = uValues(toSignature.getWords());
+				kptsTo = toSignature.getWordsKpts();
+				kptsToSource = 2;
 			}

 			// extract descriptors
-			UDEBUG("kptsFrom=%d", (int)kptsFrom.size());
-			UDEBUG("kptsTo=%d", (int)kptsTo.size());
+			UDEBUG("kptsFrom=%d kptsFromSource=%d", (int)kptsFrom.size(), kptsFromSource);
+			UDEBUG("kptsTo=%d kptsToSource=%d", (int)kptsTo.size(), kptsToSource);
 			cv::Mat descriptorsFrom;
-			if(fromSignature.getWordsDescriptors().size() &&
-					((kptsFrom.empty() && fromSignature.getWordsDescriptors().size()) ||
-					 fromSignature.getWordsDescriptors().size() == kptsFrom.size()))
+			if(kptsFromSource == 2 &&
+				fromSignature.getWordsDescriptors().rows &&
+				((kptsFrom.empty() && fromSignature.getWordsDescriptors().rows) ||
+				 fromSignature.getWordsDescriptors().rows == (int)kptsFrom.size()))
 			{
-				descriptorsFrom = cv::Mat(fromSignature.getWordsDescriptors().size(),
-						fromSignature.getWordsDescriptors().begin()->second.cols,
-						fromSignature.getWordsDescriptors().begin()->second.type());
-				int i=0;
-				for(std::multimap<int, cv::Mat>::const_iterator iter=fromSignature.getWordsDescriptors().begin();
-					iter!=fromSignature.getWordsDescriptors().end();
-					++iter, ++i)
-				{
-					iter->second.copyTo(descriptorsFrom.row(i));
-				}
+				descriptorsFrom = fromSignature.getWordsDescriptors();
 			}
-			else if(fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
+			else if(kptsFromSource == 1 &&
+					fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
 			{
 				descriptorsFrom = fromSignature.sensorData().descriptors();
 			}
@@ -652,20 +663,13 @@ Transform RegistrationVis::computeTransformationImpl(
 			cv::Mat descriptorsTo;
 			if(kptsTo.size())
 			{
-				if(toSignature.getWordsDescriptors().size() == kptsTo.size())
+				if(kptsToSource == 2 &&
+					toSignature.getWordsDescriptors().rows == (int)kptsTo.size())
 				{
-					descriptorsTo = cv::Mat(toSignature.getWordsDescriptors().size(),
-							toSignature.getWordsDescriptors().begin()->second.cols,
-							toSignature.getWordsDescriptors().begin()->second.type());
-					int i=0;
-					for(std::multimap<int, cv::Mat>::const_iterator iter=toSignature.getWordsDescriptors().begin();
-						iter!=toSignature.getWordsDescriptors().end();
-						++iter, ++i)
-					{
-						iter->second.copyTo(descriptorsTo.row(i));
-					}
+					descriptorsTo = toSignature.getWordsDescriptors();
 				}
-				else if(toSignature.sensorData().descriptors().rows == (int)kptsTo.size())
+				else if(kptsToSource == 1 &&
+						toSignature.sensorData().descriptors().rows == (int)kptsTo.size())
 				{
 					descriptorsTo = toSignature.sensorData().descriptors();
 				}
@@ -685,11 +689,13 @@ Transform RegistrationVis::computeTransformationImpl(
 			// create 3D keypoints
 			std::vector<cv::Point3f> kptsFrom3D;
 			std::vector<cv::Point3f> kptsTo3D;
-			if(kptsFrom.size() == fromSignature.getWords3().size())
+			if(kptsFromSource == 2 &&
+				kptsFrom.size() == fromSignature.getWords3().size())
 			{
-				kptsFrom3D = uValues(fromSignature.getWords3());
+				kptsFrom3D = fromSignature.getWords3();
 			}
-			else if(kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
+			else if(kptsFromSource == 1 &&
+					kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
 			{
 				kptsFrom3D = fromSignature.sensorData().keypoints3D();
 			}
@@ -720,11 +726,12 @@ Transform RegistrationVis::computeTransformationImpl(
 				_detectorFrom->filterKeypointsByDepth(kptsFrom, descriptorsFrom, kptsFrom3D, _detectorFrom->getMinDepth(), _detectorFrom->getMaxDepth());
 			}

-			if(kptsTo.size() == toSignature.getWords3().size())
+			if(kptsToSource == 2 && kptsTo.size() == toSignature.getWords3().size())
 			{
-				kptsTo3D = uValues(toSignature.getWords3());
+				kptsTo3D = toSignature.getWords3();
 			}
-			else if(kptsTo.size() == toSignature.sensorData().keypoints3D().size())
+			else if(kptsToSource == 1 &&
+					kptsTo.size() == toSignature.sensorData().keypoints3D().size())
 			{
 				kptsTo3D = toSignature.sensorData().keypoints3D();
 			}
@@ -854,7 +861,7 @@ Transform RegistrationVis::computeTransformationImpl(
 							UDEBUG("radius search done for guess");

 							// Process results (Nearest Neighbor Distance Ratio)
-							int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
+							int newToId = !orignalWordsFromIds.empty()?fromSignature.getWords().rbegin()->first+1:descriptorsFrom.rows;
 							std::map<int,int> addedWordsFrom; //<id, index>
 							std::map<int, int> duplicates; //<fromId, toId>
 							int newWords = 0;
@@ -908,7 +915,7 @@ Transform RegistrationVis::computeTransformationImpl(
 								if(matchedIndex >= 0)
 								{
 									matchedIndex = projectedIndexToDescIndex[matchedIndex];
-									int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndex]:matchedIndex;
+									int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndex]:matchedIndex;

 									if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
 									{
@@ -919,29 +926,32 @@ Transform RegistrationVis::computeTransformationImpl(
 									{
 										addedWordsFrom.insert(std::make_pair(matchedIndex, id));

-										if(kptsFrom.size())
+										wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
+										if(!kptsFrom.empty())
 										{
-											wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndex]));
+											wordsKptsFrom.push_back(kptsFrom[matchedIndex]);
 										}
-										words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndex]));
-										wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndex)));
+										words3From.push_back(kptsFrom3D[matchedIndex]);
+										wordsDescFrom.push_back(descriptorsFrom.row(matchedIndex));
 									}

-									wordsTo.insert(std::make_pair(id, kptsTo[i]));
-									wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(i)));
-									if(kptsTo3D.size())
+									wordsTo.insert(wordsTo.end(), std::make_pair(id, wordsTo.size()));
+									wordsKptsTo.push_back(kptsTo[i]);
+									wordsDescTo.push_back(descriptorsTo.row(i));
+									if(!kptsTo3D.empty())
 									{
-										words3To.insert(std::make_pair(id, kptsTo3D[i]));
+										words3To.push_back(kptsTo3D[i]);
 									}
 								}
 								else
 								{
 									// gen fake ids
-									wordsTo.insert(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
-									wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
-									if(kptsTo3D.size())
+									wordsTo.insert(wordsTo.end(), std::make_pair(newToId, wordsTo.size()));
+									wordsKptsTo.push_back(kptsTo[i]);
+									wordsDescTo.push_back(descriptorsTo.row(i));
+									if(!kptsTo3D.empty())
 									{
-										words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
+										words3To.push_back(kptsTo3D[i]);
 									}

 									++newToId;
@@ -958,10 +968,11 @@ Transform RegistrationVis::computeTransformationImpl(
 							{
 								if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
 								{
-									int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
-									wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[i]));
-									wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(i)));
-									words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[i]));
+									int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
+									wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
+									wordsKptsFrom.push_back(kptsFrom[i]);
+									wordsDescFrom.push_back(descriptorsFrom.row(i));
+									words3From.push_back(kptsFrom3D[i]);

 									++addWordsFromNotMatched;
 								}
@@ -1003,7 +1014,7 @@ Transform RegistrationVis::computeTransformationImpl(

 								if(indices[i].size())
 								{
-									info.projectedIDs.push_back(orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom);
+									info.projectedIDs.push_back(!orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom);
 								}

 								if(util3d::isFinite(kptsFrom3D[matchedIndexFrom]))
@@ -1054,26 +1065,28 @@ Transform RegistrationVis::computeTransformationImpl(
 										matchedIndexTo = indices[i].at(0);
 									}

-									int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
+									int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
 									addedWordsFrom.insert(addedWordsFrom.end(), matchedIndexFrom);

-									if(kptsFrom.size())
+									wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
+									if(!kptsFrom.empty())
 									{
-										wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[matchedIndexFrom]));
+										wordsKptsFrom.push_back(kptsFrom[matchedIndexFrom]);
 									}
-									words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[matchedIndexFrom]));
-									wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(matchedIndexFrom)));
+									words3From.push_back(kptsFrom3D[matchedIndexFrom]);
+									wordsDescFrom.push_back(descriptorsFrom.row(matchedIndexFrom));

 									if(	matchedIndexTo >= 0 &&
 										addedWordsTo.find(matchedIndexTo) == addedWordsTo.end())
 									{
 										addedWordsTo.insert(matchedIndexTo);

-										wordsTo.insert(wordsTo.end(), std::make_pair(id, kptsTo[matchedIndexTo]));
-										wordsDescTo.insert(wordsDescTo.end(), std::make_pair(id, descriptorsTo.row(matchedIndexTo)));
-										if(kptsTo3D.size())
+										wordsTo.insert(wordsTo.end(), std::make_pair(id, wordsTo.size()));
+										wordsKptsTo.push_back(kptsTo[matchedIndexTo]);
+										wordsDescTo.push_back(descriptorsTo.row(matchedIndexTo));
+										if(!kptsTo3D.empty())
 										{
-											words3To.insert(words3To.end(), std::make_pair(id, kptsTo3D[matchedIndexTo]));
+											words3To.push_back(kptsTo3D[matchedIndexTo]);
 										}
 									}
 								}
@@ -1085,23 +1098,25 @@ Transform RegistrationVis::computeTransformationImpl(
 							{
 								if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
 								{
-									int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
-									wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[i]));
-									wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(i)));
-									words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[i]));
+									int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
+									wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
+									wordsKptsFrom.push_back(kptsFrom[i]);
+									wordsDescFrom.push_back(descriptorsFrom.row(i));
+									words3From.push_back(kptsFrom3D[i]);
 								}
 							}

-							int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
+							int newToId = !orignalWordsFromIds.empty()?fromSignature.getWords().rbegin()->first+1:descriptorsFrom.rows;
 							for(unsigned int i = 0; i < kptsTo.size(); ++i)
 							{
 								if(addedWordsTo.find(i) == addedWordsTo.end())
 								{
-									wordsTo.insert(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
-									wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
-									if(kptsTo3D.size())
+									wordsTo.insert(wordsTo.end(), std::make_pair(newToId, wordsTo.size()));
+									wordsKptsTo.push_back(kptsTo[i]);
+									wordsDescTo.push_back(descriptorsTo.row(i));
+									if(!kptsTo3D.empty())
 									{
-										words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
+										words3To.push_back(kptsTo3D[i]);
 									}
 									++newToId;
 								}
@@ -1267,15 +1282,16 @@ Transform RegistrationVis::computeTransformationImpl(
 					{
 						if(fromWordIdsSet.count(*iter) == 1)
 						{
-							if (kptsFrom.size())
+							wordsFrom.insert(wordsFrom.end(), std::make_pair(*iter, wordsFrom.size()));
+							if (!kptsFrom.empty())
 							{
-								wordsFrom.insert(std::make_pair(*iter, kptsFrom[i]));
+								wordsKptsFrom.push_back(kptsFrom[i]);
 							}
-							if(kptsFrom3D.size())
+							if(!kptsFrom3D.empty())
 							{
-								words3From.insert(std::make_pair(*iter, kptsFrom3D[i]));
+								words3From.push_back(kptsFrom3D[i]);
 							}
-							wordsDescFrom.insert(std::make_pair(*iter, descriptorsFrom.row(i)));
+							wordsDescFrom.push_back(descriptorsFrom.row(i));
 						}
 						++i;
 					}
@@ -1287,11 +1303,12 @@ Transform RegistrationVis::computeTransformationImpl(
 					{
 						if(toWordIdsSet.count(*iter) == 1)
 						{
-							wordsTo.insert(std::make_pair(*iter, kptsTo[i]));
-							wordsDescTo.insert(std::make_pair(*iter, descriptorsTo.row(i)));
-							if(kptsTo3D.size())
+							wordsTo.insert(wordsTo.end(), std::make_pair(*iter, wordsTo.size()));
+							wordsKptsTo.push_back(kptsTo[i]);
+							wordsDescTo.push_back(descriptorsTo.row(i));
+							if(!kptsTo3D.empty())
 							{
-								words3To.insert(std::make_pair(*iter, kptsTo3D[i]));
+								words3To.push_back(kptsTo3D[i]);
 							}
 						}
 						++i;
@@ -1304,21 +1321,19 @@ Transform RegistrationVis::computeTransformationImpl(
 				UASSERT(kptsFrom3D.empty() || int(kptsFrom3D.size()) == descriptorsFrom.rows);
 				for(int i=0; i<descriptorsFrom.rows; ++i)
 				{
-					wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
-					wordsDescFrom.insert(std::make_pair(i, descriptorsFrom.row(i)));
-					if(kptsFrom3D.size())
+					wordsFrom.insert(wordsFrom.end(), std::make_pair(i, wordsFrom.size()));
+					wordsKptsFrom.push_back(kptsFrom[i]);
+					wordsDescFrom.push_back(descriptorsFrom.row(i));
+					if(!kptsFrom3D.empty())
 					{
-						words3From.insert(std::make_pair(i, kptsFrom3D[i]));
+						words3From.push_back(kptsFrom3D[i]);
 					}
 				}
 			}
 		}
-		fromSignature.setWords(wordsFrom);
-		fromSignature.setWords3(words3From);
-		fromSignature.setWordsDescriptors(wordsDescFrom);
-		toSignature.setWords(wordsTo);
-		toSignature.setWords3(words3To);
-		toSignature.setWordsDescriptors(wordsDescTo);
+
+		fromSignature.setWords(wordsFrom, wordsKptsFrom, words3From, wordsDescFrom);
+		toSignature.setWords(wordsTo, wordsKptsTo, words3To, wordsDescTo);
 	}

 	/////////////////////
@@ -1372,14 +1387,31 @@ Transform RegistrationVis::computeTransformationImpl(
 					Transform cameraTransform;
 					double variance = 1.0f;
 					std::vector<int> matchesV;
+					std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
+					std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
+					std::map<int, cv::KeyPoint> wordsA;
+					std::map<int, cv::Point3f> words3A;
+					std::map<int, cv::KeyPoint> wordsB;
+					for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
+					{
+						wordsA.insert(std::make_pair(iter->first, signatureA->getWordsKpts()[iter->second]));
+						if(!signatureA->getWords3().empty())
+						{
+							words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
+						}
+					}
+					for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
+					{
+						wordsB.insert(std::make_pair(iter->first, signatureB->getWordsKpts()[iter->second]));
+					}
 					std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
-							uMultimapToMapUnique(signatureA->getWords()),
-							uMultimapToMapUnique(signatureB->getWords()),
+							wordsA,
+							wordsB,
 							cameraModel,
 							cameraTransform,
 							_PnPReprojError,
 							0.99f,
-							uMultimapToMapUnique(signatureA->getWords3()), // for scale estimation
+							words3A, // for scale estimation
 							&variance,
 							&matchesV);
 					covariances[dir] *= variance;
@@ -1455,9 +1487,26 @@ Transform RegistrationVis::computeTransformationImpl(

 						std::vector<int> inliersV;
 						std::vector<int> matchesV;
+						std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
+						std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
+						std::map<int, cv::Point3f> words3A;
+						std::map<int, cv::Point3f> words3B;
+						std::map<int, cv::KeyPoint> wordsB;
+						for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
+						{
+							words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
+						}
+						for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
+						{
+							wordsB.insert(std::make_pair(iter->first, signatureB->getWordsKpts()[iter->second]));
+							if(!signatureB->getWords3().empty())
+							{
+								words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
+							}
+						}
 						transforms[dir] = util3d::estimateMotion3DTo2D(
-								uMultimapToMapUnique(signatureA->getWords3()),
-								uMultimapToMapUnique(signatureB->getWords()),
+								words3A,
+								wordsB,
 								cameraModel,
 								_minInliers,
 								_iterations,
@@ -1465,7 +1514,7 @@ Transform RegistrationVis::computeTransformationImpl(
 								_PnPFlags,
 								_PnPRefineIterations,
 								dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
-								uMultimapToMapUnique(signatureB->getWords3()),
+								words3B,
 								&covariances[dir],
 								&matchesV,
 								&inliersV);
@@ -1501,9 +1550,21 @@ Transform RegistrationVis::computeTransformationImpl(
 				{
 					std::vector<int> inliersV;
 					std::vector<int> matchesV;
+					std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
+					std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
+					std::map<int, cv::Point3f> words3A;
+					std::map<int, cv::Point3f> words3B;
+					for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
+					{
+						words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
+					}
+					for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
+					{
+						words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
+					}
 					transforms[dir] = util3d::estimateMotion3DTo3D(
-							uMultimapToMapUnique(signatureA->getWords3()),
-							uMultimapToMapUnique(signatureB->getWords3()),
+							words3A,
+							words3B,
 							_minInliers,
 							_inlierDistance,
 							_iterations,
@@ -1671,27 +1732,37 @@ Transform RegistrationVis::computeTransformationImpl(
 			models.insert(std::make_pair(2, cameraModelTo));

 			std::map<int, std::map<int, FeatureBA> > wordReferences;
+			std::set<int> sbaOutliers;
 			for(unsigned int i=0; i<allInliers.size(); ++i)
 			{
 				int wordId = allInliers[i];
-				const cv::Point3f & pt3D = fromSignature.getWords3().find(wordId)->second;
+				int indexFrom = fromSignature.getWords().find(wordId)->second;
+				const cv::Point3f & pt3D = fromSignature.getWords3()[indexFrom];
+				if(!util3d::isFinite(pt3D))
+				{
+					UASSERT_MSG(!_forwardEstimateOnly, uFormat("3D point %d is not finite!?", wordId).c_str());
+					sbaOutliers.insert(wordId);
+					continue;
+				}
+
 				points3DMap.insert(std::make_pair(wordId, pt3D));

 				std::map<int, FeatureBA> ptMap;
-				if(fromSignature.getWords().size() && cameraModelFrom.isValidForProjection())
+				if(!fromSignature.getWordsKpts().empty() && cameraModelFrom.isValidForProjection())
 				{
 					float depthFrom = util3d::transformPoint(pt3D, invLocalTransformFrom).z;
-					const cv::KeyPoint & kpt = fromSignature.getWords().find(wordId)->second;
+					const cv::KeyPoint & kpt = fromSignature.getWordsKpts()[indexFrom];
 					ptMap.insert(std::make_pair(1,FeatureBA(kpt, depthFrom)));
 				}
-				if(toSignature.getWords().size() && cameraModelTo.isValidForProjection())
+				if(!toSignature.getWordsKpts().empty() && cameraModelTo.isValidForProjection())
 				{
+					int indexTo = toSignature.getWords().find(wordId)->second;
 					float depthTo = 0.0f;
-					if(toSignature.getWords3().find(wordId) != toSignature.getWords3().end())
+					if(!toSignature.getWords3().empty())
 					{
-						depthTo = util3d::transformPoint(toSignature.getWords3().find(wordId)->second, invLocalTransformTo).z;
+						depthTo = util3d::transformPoint(toSignature.getWords3()[indexTo], invLocalTransformTo).z;
 					}
-					const cv::KeyPoint & kpt = toSignature.getWords().find(wordId)->second;
+					const cv::KeyPoint & kpt = toSignature.getWordsKpts()[indexTo];
 					ptMap.insert(std::make_pair(2,FeatureBA(kpt, depthTo)));
 				}

@@ -1704,7 +1775,6 @@ Transform RegistrationVis::computeTransformationImpl(
 				//}
 			}

-			std::set<int> sbaOutliers;
 			optimizedPoses = sba->optimizeBA(1, poses, links, models, points3DMap, wordReferences, &sbaOutliers);
 			delete sba;

@@ -1830,24 +1900,25 @@ Transform RegistrationVis::computeTransformationImpl(
 			{
 				if(_maxInliersMeanDistance>0.0f)
 				{
-					std::multimap<int, cv::Point3f>::const_iterator words3Iter = fromSignature.getWords3().find(allInliers[i]);
-					if(words3Iter != fromSignature.getWords3().end())
+					std::multimap<int, int>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
+					if(wordsIter != fromSignature.getWords().end() && !fromSignature.getWords3().empty())
 					{
-						if(uIsFinite(words3Iter->second.x))
+						const cv::Point3f & pt = fromSignature.getWords3()[wordsIter->second];
+						if(uIsFinite(pt.x))
 						{
-							cv::Point3f pt = util3d::transformPoint(words3Iter->second, transformInv);
-							distances.push_back(pt.x);
+							distances.push_back(util3d::transformPoint(pt, transformInv).x);
 						}
 					}
 				}

 				if(!pcaData.empty())
 				{
-					std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
-					UASSERT(wordsIter != fromSignature.getWords().end());
+					std::multimap<int, int>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
+					UASSERT(wordsIter != fromSignature.getWords().end() && !fromSignature.getWordsKpts().empty());
 					float * ptr = pcaData.ptr<float>(i, 0);
-					ptr[0] = (wordsIter->second.pt.x-cx) / w;
-					ptr[1] = (wordsIter->second.pt.y-cy) / h;
+					const cv::KeyPoint & kpt = fromSignature.getWordsKpts()[wordsIter->second];
+					ptr[0] = (kpt.pt.x-cx) / w;
+					ptr[1] = (kpt.pt.y-cy) / h;
 				}
 			}

@@ -1888,6 +1959,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	}

 	info.inliers = inliersCount;
+	info.inliersRatio = !toSignature.getWords().empty()?float(inliersCount)/float(toSignature.getWords().size()):0;
 	info.matches = matchesCount;
 	info.rejectedMsg = msg;
 	info.covariance = covariance;
@@ -134,6 +134,7 @@ Rtabmap::Rtabmap() :
 	_lastProcessTime(0.0),
 	_someNodesHaveBeenTransferred(false),
 	_distanceTravelled(0.0f),
+	_distanceTravelledSinceLastLocalization(0.0f),
 	_optimizeFromGraphEndChanged(false),
 	_epipolarGeometry(0),
 	_bayesFilter(0),
@@ -350,6 +351,17 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 		std::map<int, Transform> tmp;
 		// Get just the links
 		_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true);
+
+		// Initialize Bayes' prediction matrix
+		UTimer time;
+		std::map<int, float> likelihood;
+		likelihood.insert(std::make_pair(Memory::kIdVirtual, 1));
+		for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
+		{
+			likelihood.insert(std::make_pair(iter->first, 0));
+		}
+		_bayesFilter->computePosterior(_memory, likelihood);
+		UINFO("Time initializing Bayes' prediction with %ld nodes: %fs", _optimizedPoses.size(), time.ticks());
 	}
 	else
 	{
@@ -393,6 +405,7 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	_odomCacheConstraints.clear();
 	_odomCorrectionAcc = std::vector<float>(6,0);
 	_distanceTravelled = 0.0f;
+	_distanceTravelledSinceLastLocalization = 0.0f;
 	_optimizeFromGraphEndChanged = false;
 	this->clearPath(0);
 	_gpsGeocentricCache.clear();
@@ -419,6 +432,16 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	{
 		if(databaseSaved)
 		{
+			if(_memory->isGraphReduced() && _memory->isIncremental())
+			{
+				// Force reducing graph, then remove filtered nodes from the optimized poses
+				std::map<int, int> reducedIds;
+				_memory->incrementMapId(&reducedIds);
+				for(std::map<int, int>::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter)
+				{
+					_optimizedPoses.erase(iter->first);
+				}
+			}
 			_memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose);
 		}
 		_memory->close(databaseSaved, true, ouputDatabasePath);
@@ -655,19 +678,6 @@ int Rtabmap::getTotalMemSize() const
 	return 0;
 }

-std::multimap<int, cv::KeyPoint> Rtabmap::getWords(int locationId) const
-{
-	if(_memory)
-	{
-		const Signature * s = _memory->getSignature(locationId);
-		if(s)
-		{
-			return s->getWords();
-		}
-	}
-	return std::multimap<int, cv::KeyPoint>();
-}
-
 bool Rtabmap::isInSTM(int locationId) const
 {
 	if(_memory)
@@ -690,25 +700,7 @@ const Statistics & Rtabmap::getStatistics() const
 {
 	return statistics_;
 }
-/*
-bool Rtabmap::getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const
-{
-	if(_memory)
-	{
-		const Signature * s = _memory->getSignature(locationId);
-		if(s && _optimizedPoses.find(s->id()) != _optimizedPoses.end())
-		{
-			rgb = s->getImage();
-			depth = s->getDepth();
-			depthConstant = s->getDepthConstant();
-			pose = _optimizedPoses.at(s->id());
-			localTransform = s->getLocalTransform();
-			return true;
-		}
-	}
-	return false;
-}
-*/
+
 Transform Rtabmap::getPose(int locationId) const
 {
 	return uValue(_optimizedPoses, locationId, Transform());
@@ -751,6 +743,8 @@ int Rtabmap::triggerNewMap()
 		_odomCachePoses.clear();
 		_odomCacheConstraints.clear();
 		_odomCorrectionAcc = std::vector<float>(6,0);
+		_distanceTravelled = 0.0f;
+		_distanceTravelledSinceLastLocalization = 0.0f;

 		if(!_memory->isIncremental())
 		{
@@ -917,6 +911,7 @@ void Rtabmap::resetMemory()
 	_odomCacheConstraints.clear();
 	_odomCorrectionAcc = std::vector<float>(6,0);
 	_distanceTravelled = 0.0f;
+	_distanceTravelledSinceLastLocalization = 0.0f;
 	_optimizeFromGraphEndChanged = false;
 	this->clearPath(0);

@@ -1076,6 +1071,43 @@ bool Rtabmap::process(
 	bool fakeOdom = false;
 	if(_rgbdSlamMode)
 	{
+		if(!odomPose.isNull())
+		{
+			// this will make sure that all inverse operations will work!
+			if(!odomPose.isInvertible())
+			{
+				UWARN("Input odometry is not invertible! pose = %s\n"
+						"[%f %f %f %f;\n"
+						" %f %f %f %f;\n"
+						" %f %f %f %f;\n"
+						" 0 0 0 1]\n"
+						"Trying to normalize rotation to see if it makes it invertible...",
+						odomPose.prettyPrint().c_str(),
+						odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(),
+						odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(),
+						odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34());
+				odomPose.normalizeRotation();
+				UASSERT_MSG(odomPose.isInvertible(), uFormat("Odometry pose is not invertible!\n"
+						"[%f %f %f %f;\n"
+						" %f %f %f %f;\n"
+						" %f %f %f %f;\n"
+						" 0 0 0 1]", odomPose.prettyPrint().c_str(),
+						odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(),
+						odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(),
+						odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34()).c_str());
+				UWARN("Normalizing rotation succeeded! fixed pose = %s\n"
+						"[%f %f %f %f;\n"
+						" %f %f %f %f;\n"
+						" %f %f %f %f;\n"
+						" 0 0 0 1]\n"
+						"If the resulting rotation is very different from original one, try to fix the odometry or TF.",
+						odomPose.prettyPrint().c_str(),
+						odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(),
+						odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(),
+						odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34());
+			}
+		}
+
 		if(!_memory->isIncremental() &&
 			!odomPose.isNull() &&
 			_optimizedPoses.size() &&
@@ -1243,6 +1275,7 @@ bool Rtabmap::process(
 							// This will disable global loop closure detection, only retrieval will be done.
 							// The location will also be deleted at the end.
 							smallDisplacement = true;
+							UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
 						}
 					}
 				}
@@ -1358,7 +1391,9 @@ bool Rtabmap::process(
 			}
 			else
 			{
-				UWARN("Neighbor link refining is activated but there are intermediate nodes, aborting refining...");
+				UWARN("Neighbor link refining is activated but there are intermediate nodes (%d=%d %d=%d), aborting refining...",
+						signature->id(), signature->getWeight(), oldS->id(), oldS->getWeight());
+				newPose = _mapCorrection * signature->getPose();
 			}
 		}
 		else
@@ -1380,6 +1415,9 @@ bool Rtabmap::process(
 				_constraints.insert(std::make_pair(iter->first, iter->second.inverse()));
 			}
 		}
+
+		float distanceTravelledOld = _distanceTravelled;
+
 		// only in mapping mode we add a neighbor link
 		if(signature->getLinks().size() &&
 		   signature->getLinks().begin()->second.type() == Link::kNeighbor)
@@ -1452,6 +1490,7 @@ bool Rtabmap::process(
 				}
 			}
 		}
+		_distanceTravelledSinceLastLocalization += _distanceTravelled - distanceTravelledOld;

 		//============================================================
 		// Reduced graph
@@ -2184,6 +2223,7 @@ bool Rtabmap::process(
 	// Landmark
 	//============================================================
 	int landmarkDetected = 0;
+	bool rejectedLandmark = false;
 	std::set<int> landmarkDetectedNodesRef;
 	if(!signature->getLandmarks().empty())
 	{
@@ -2205,6 +2245,7 @@ bool Rtabmap::process(
 	//============================================================
 	std::list<std::pair<int, int> > loopClosureLinksAdded;
 	int loopClosureVisualInliers = 0; // for statistics
+	float loopClosureVisualInliersRatio = 0.0f;
 	int loopClosureVisualMatches = 0;
 	float loopClosureLinearVariance = 0.0f;
 	float loopClosureAngularVariance = 0.0f;
@@ -2356,6 +2397,7 @@ bool Rtabmap::process(
 									lastProximitySpaceClosureId = nearestId;

 									loopClosureVisualInliers = info.inliers;
+									loopClosureVisualInliersRatio = info.inliersRatio;
 									loopClosureVisualMatches = info.matches;

 									loopClosureLinearVariance = 1.0/information.at<double>(0,0);
@@ -2559,6 +2601,7 @@ bool Rtabmap::process(
 			loopClosureVisualInliersDistribution = info.inliersDistribution;

 			loopClosureVisualInliers = info.inliers;
+			loopClosureVisualInliersRatio = info.inliersRatio;
 			loopClosureVisualMatches = info.matches;
 			rejectedGlobalLoopClosure = transform.isNull();
 			if(rejectedGlobalLoopClosure)
@@ -2911,6 +2954,7 @@ bool Rtabmap::process(
 				_loopClosureHypothesis.first = 0;
 				lastProximitySpaceClosureId = 0;
 				rejectedGlobalLoopClosure = true;
+				rejectedLandmark = true;
 			}
 		}
 		else
@@ -2946,6 +2990,7 @@ bool Rtabmap::process(
 				_loopClosureHypothesis.first = 0;
 				lastProximitySpaceClosureId = 0;
 				rejectedGlobalLoopClosure = true;
+				rejectedLandmark = true;
 			}
 			else if(_memory->isIncremental() &&
 			  _optimizationMaxError > 0.0f &&
@@ -3031,6 +3076,7 @@ bool Rtabmap::process(
 					_loopClosureHypothesis.first = 0;
 					lastProximitySpaceClosureId = 0;
 					rejectedGlobalLoopClosure = true;
+					rejectedLandmark = true;
 				}
 			}

@@ -3052,7 +3098,7 @@ bool Rtabmap::process(
 		previousMapCorrection = _mapCorrection;
 		_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
 		_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
-		if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
+		if(_mapCorrection.getNormSquared() > 0.1f && _optimizeFromGraphEnd)
 		{
 			bool hasPrior = signature->hasLink(signature->id());
 			if(!_graphOptimizer->priorsIgnored())
@@ -3137,6 +3183,7 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kLoopReactivate_id(), retrievalId);
 			statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
+			statistics_.addStatistic(Statistics::kLoopVisual_inliers_ratio(), loopClosureVisualInliersRatio);
 			statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches);
 			statistics_.addStatistic(Statistics::kLoopLinear_variance(), loopClosureLinearVariance);
 			statistics_.addStatistic(Statistics::kLoopAngular_variance(), loopClosureAngularVariance);
@@ -3160,7 +3207,9 @@ bool Rtabmap::process(
 			statistics_.setProximityDetectionId(lastProximitySpaceClosureId);
 			statistics_.setProximityDetectionMapId(_memory->getMapId(lastProximitySpaceClosureId));

-			statistics_.addStatistic(Statistics::kLoopId(), _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId);
+			int loopId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId;
+			statistics_.addStatistic(Statistics::kLoopId(), loopId);
+			statistics_.addStatistic(Statistics::kLoopMap_id(), (loopId>0 && sLoop)?sLoop->mapId():-1);

 			float x,y,z,roll,pitch,yaw;
 			if(_loopClosureHypothesis.first || lastProximitySpaceClosureId)
@@ -3172,8 +3221,9 @@ bool Rtabmap::process(
 				UINFO("Set loop closure transform = %s", loopIter->second.transform().prettyPrint().c_str());
 				statistics_.setLoopClosureTransform(loopIter->second.transform());

-				statistics_.addStatistic(Statistics::kLoopMap_id(), sLoop->mapId());
 				statistics_.addStatistic(Statistics::kLoopVisual_words(), sLoop->getWords().size());
+				statistics_.addStatistic(Statistics::kLoopDistance_since_last_loc(), _distanceTravelledSinceLastLocalization);
+				_distanceTravelledSinceLastLocalization = 0.0f;

 				// if ground truth exists, compute localization error
 				if(!sLoop->getGroundTruthPose().isNull() && !signature->getGroundTruthPose().isNull())
@@ -3281,7 +3331,16 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kMemoryFast_movement(), tooFastMovement?1.0f:0);
 			if(_publishRAMUsage)
 			{
+				UTimer ramTimer;
 				statistics_.addStatistic(Statistics::kMemoryRAM_usage(), UProcessInfo::getMemoryUsage()/(1024*1024));
+				long estimatedMemoryUsage = sizeof(Rtabmap);
+				estimatedMemoryUsage += _optimizedPoses.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(std::map<int, Transform>::iterator)) + sizeof(std::map<int, Transform>);
+				estimatedMemoryUsage += _constraints.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double) + sizeof(std::map<int, Link>::iterator)) + sizeof(std::map<int, Link>);
+				estimatedMemoryUsage += _memory->getMemoryUsed();
+				estimatedMemoryUsage += _bayesFilter->getMemoryUsed();
+				estimatedMemoryUsage += _parameters.size()*(sizeof(std::string)*2+sizeof(ParametersMap::iterator)) + sizeof(ParametersMap);
+				statistics_.addStatistic(Statistics::kMemoryRAM_estimated(), (float)(estimatedMemoryUsage/(1024*1024)));//MB
+				statistics_.addStatistic(Statistics::kTimingRAM_estimation(), ramTimer.ticks()*1000);
 			}

 			if(_publishLikelihood || _publishPdf)
@@ -3346,7 +3405,7 @@ bool Rtabmap::process(
 		if(_startNewMapOnLoopClosure &&
 			_memory->isIncremental() &&              // only in mapping mode
 			graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0 &&      // alone in the current map
-			(landmarkDetected == 0 || rejectedGlobalLoopClosure) &&      // if we re not seeing a landmark from a previous map
+			(landmarkDetected == 0 || rejectedLandmark) &&      // if we re not seeing a landmark from a previous map
 			_memory->getWorkingMem().size()>=2)       // The working memory should not be empty (beside virtual signature)
 		{
 			UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
@@ -4430,18 +4489,30 @@ Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userDat
 				groundTruth,
 				data);

+		std::multimap<int, Link> links = _memory->getLinks(id, true, true);
+		for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
+		{
+			if(iter->second.type() == Link::kLandmark)
+			{
+				s.addLandmark(iter->second);
+			}
+			else
+			{
+				s.addLink(iter->second);
+			}
+		}
+
 		if(withWords || withGlobalDescriptors)
 		{
-			std::multimap<int, cv::KeyPoint> words;
-			std::multimap<int, cv::Point3f> words3;
-			std::multimap<int, cv::Mat> wordsDescriptors;
+			std::multimap<int, int> words;
+			std::vector<cv::KeyPoint> wordsKpts;
+			std::vector<cv::Point3f> words3;
+			cv::Mat wordsDescriptors;
 			std::vector<rtabmap::GlobalDescriptor> globalDescriptors;
-			_memory->getNodeWordsAndGlobalDescriptors(id, words, words3, wordsDescriptors, globalDescriptors);
+			_memory->getNodeWordsAndGlobalDescriptors(id, words, wordsKpts, words3, wordsDescriptors, globalDescriptors);
 			if(withWords)
 			{
-				s.setWords(words);
-				s.setWords3(words3);
-				s.setWordsDescriptors(wordsDescriptors);
+				s.setWords(words, wordsKpts, words3, wordsDescriptors);
 			}
 			if(withGlobalDescriptors)
 			{
@@ -4535,6 +4606,26 @@ void Rtabmap::getGraph(
 	}
 }

+std::map<int, Transform> Rtabmap::getNodesInRadius(const Transform & pose, float radius)
+{
+	return graph::getPosesInRadius(pose, _optimizedPoses, radius<=0?_localRadius:radius);
+}
+
+std::map<int, Transform> Rtabmap::getNodesInRadius(int nodeId, float radius)
+{
+	UDEBUG("nodeId=%d, radius=%f", nodeId, radius);
+	std::map<int, Transform> nearNodes;
+	if(nodeId==0 && !_optimizedPoses.empty())
+	{
+		nodeId = _optimizedPoses.rbegin()->first;
+	}
+	if(_optimizedPoses.find(nodeId) != _optimizedPoses.end())
+	{
+		nearNodes = graph::getPosesInRadius(nodeId, _optimizedPoses, radius<=0?_localRadius:radius);
+	}
+	return nearNodes;
+}
+
 int Rtabmap::detectMoreLoopClosures(
 		float clusterRadius,
 		float clusterAngle,
@@ -4567,7 +4658,7 @@ int Rtabmap::detectMoreLoopClosures(
 	std::map<int, Transform> posesToCheckLoopClosures;
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> links;
-	std::map<int, Signature> signatures;
+	std::map<int, Signature> signatures; // some signatures may be in LTM, get them all
 	this->getGraph(poses, links, true, true, &signatures);

 	std::map<int, int> mapIds;
@@ -4605,7 +4696,7 @@ int Rtabmap::detectMoreLoopClosures(

 			int from = iter->first;
 			int to = iter->second;
-			if(iter->first < iter->second)
+			if(from > to)
 			{
 				from = iter->second;
 				to = iter->first;
@@ -4641,9 +4732,15 @@ int Rtabmap::detectMoreLoopClosures(
 						UASSERT(signatures.find(from) != signatures.end());
 						UASSERT(signatures.find(to) != signatures.end());

+						Transform guess;
+						if(_proximityOdomGuess && uContains(poses, from) && uContains(poses, to))
+						{
+							guess = poses.at(from).inverse() * poses.at(to);
+						}
+
 						RegistrationInfo info;
 						// use signatures instead of IDs because some signatures may not be in WM
-						Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), Transform(), &info);
+						Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), guess, &info);

 						if(!t.isNull())
 						{
@@ -763,9 +763,10 @@ void SensorData::setFeatures(const std::vector<cv::KeyPoint> & keypoints, const
 	_descriptors = descriptors;
 }

-long SensorData::getMemoryUsed() const // Return memory usage in Bytes
+unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 {
-	return _imageCompressed.total()*_imageCompressed.elemSize() +
+	return sizeof(SensorData) +
+			_imageCompressed.total()*_imageCompressed.elemSize() +
 			_imageRaw.total()*_imageRaw.elemSize() +
 			_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize() +
 			_depthOrRightRaw.total()*_depthOrRightRaw.elemSize() +
@@ -779,8 +780,8 @@ long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 			_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize()+
 			_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize() +
 			_emptyCellsRaw.total()*_emptyCellsRaw.elemSize()+
-			_keypoints.size() * sizeof(float) * 7 +
-			_keypoints3D.size() * sizeof(float)*3 +
+			_keypoints.size() * sizeof(cv::KeyPoint) +
+			_keypoints3D.size() * sizeof(cv::Point3f) +
 			_descriptors.total()*_descriptors.elemSize();
 }

@@ -132,11 +132,24 @@ bool Signature::hasLink(int idTo, Link::Type type) const
 	{
 		return _links.find(idTo) != _links.end();
 	}
-	for(std::multimap<int, Link>::const_iterator iter=_links.find(idTo); iter!=_links.end() && iter->first == idTo; ++iter)
+	if(idTo==0)
 	{
-		if(type == iter->second.type())
+		for(std::multimap<int, Link>::const_iterator iter=_links.begin(); iter!=_links.end(); ++iter)
 		{
-			return true;
+			if(type == iter->second.type())
+			{
+				return true;
+			}
+		}
+	}
+	else
+	{
+		for(std::multimap<int, Link>::const_iterator iter=_links.find(idTo); iter!=_links.end() && iter->first == idTo; ++iter)
+		{
+			if(type == iter->second.type())
+			{
+				return true;
+			}
 		}
 	}
 	return false;
@@ -209,11 +222,11 @@ void Signature::removeVirtualLinks()
 float Signature::compareTo(const Signature & s) const
 {
 	float similarity = 0.0f;
-	const std::multimap<int, cv::KeyPoint> & words = s.getWords();
+	const std::multimap<int, int> & words = s.getWords();

 	if(!s.isBadSignature() && !this->isBadSignature())
 	{
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
+		std::list<std::pair<int, std::pair<int, int> > > pairs;
 		int totalWords = ((int)_words.size()-_invalidWordsCount)>((int)words.size()-s.getInvalidWordsCount())?((int)_words.size()-_invalidWordsCount):((int)words.size()-s.getInvalidWordsCount());
 		UASSERT(totalWords > 0);
 		EpipolarGeometry::findPairs(words, _words, pairs);
@@ -225,11 +238,9 @@ float Signature::compareTo(const Signature & s) const

 void Signature::changeWordsRef(int oldWordId, int activeWordId)
 {
-	std::list<cv::KeyPoint> kps = uValues(_words, oldWordId);
-	if(kps.size())
+	std::list<int> words = uValues(_words, oldWordId);
+	if(words.size())
 	{
-		std::list<cv::Point3f> pts = uValues(_words3, oldWordId);
-		std::list<cv::Mat> descriptors = uValues(_wordsDescriptors, oldWordId);
 		if(oldWordId<=0)
 		{
 			_invalidWordsCount-=(int)_words.erase(oldWordId);
@@ -239,37 +250,41 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
 		{
 			_words.erase(oldWordId);
 		}
-		_words3.erase(oldWordId);
-		_wordsDescriptors.erase(oldWordId);
+
 		_wordsChanged.insert(std::make_pair(oldWordId, activeWordId));
-		for(std::list<cv::KeyPoint>::const_iterator iter=kps.begin(); iter!=kps.end(); ++iter)
+		for(std::list<int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 		{
-			_words.insert(std::pair<int, cv::KeyPoint>(activeWordId, (*iter)));
-		}
-		for(std::list<cv::Point3f>::const_iterator iter=pts.begin(); iter!=pts.end(); ++iter)
-		{
-			_words3.insert(std::pair<int, cv::Point3f>(activeWordId, (*iter)));
-		}
-		for(std::list<cv::Mat>::const_iterator iter=descriptors.begin(); iter!=descriptors.end(); ++iter)
-		{
-			_wordsDescriptors.insert(std::pair<int, cv::Mat>(activeWordId, (*iter)));
+			_words.insert(std::pair<int, int>(activeWordId, (*iter)));
 		}
 	}
 }

-void Signature::setWords(const std::multimap<int, cv::KeyPoint> & words)
+void Signature::setWords(const std::multimap<int, int> & words,
+		const std::vector<cv::KeyPoint> & keypoints,
+		const std::vector<cv::Point3f> & points,
+		const cv::Mat & descriptors)
 {
+	UASSERT_MSG(descriptors.empty() || descriptors.rows == (int)words.size(), uFormat("words=%d, descriptors=%d", (int)words.size(), descriptors.rows).c_str());
+	UASSERT_MSG(points.empty() || points.size() == words.size(),  uFormat("words=%d, points=%d", (int)words.size(), (int)points.size()).c_str());
+	UASSERT_MSG(keypoints.empty() || keypoints.size() == words.size(),  uFormat("words=%d, descriptors=%d", (int)words.size(), (int)keypoints.size()).c_str());
+	UASSERT(words.empty() || !keypoints.empty() || !points.empty() || !descriptors.empty());
+
+	_invalidWordsCount = 0;
+	for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+	{
+		if(iter->first<=0)
+		{
+			++_invalidWordsCount;
+		}
+		// make sure indexes are all valid!
+		UASSERT_MSG(iter->second >=0 && iter->second < (int)words.size(), uFormat("iter->second=%d words.size()=%d", iter->second, (int)words.size()).c_str());
+	}
+
 	_enabled = false;
 	_words = words;
-	_invalidWordsCount = 0;
-	for(std::multimap<int, cv::KeyPoint>::iterator iter=_words.begin(); iter!=_words.end(); ++iter)
-	{
-		if(iter->first>0)
-		{
-			break;
-		}
-		++_invalidWordsCount;
-	}
+	_wordsKpts = keypoints;
+	_words3 = points;
+	_wordsDescriptors = descriptors.clone();
 }

 bool Signature::isBadSignature() const
@@ -280,24 +295,30 @@ bool Signature::isBadSignature() const
 void Signature::removeAllWords()
 {
 	_words.clear();
+	_wordsKpts.clear();
 	_words3.clear();
-	_wordsDescriptors.clear();
+	_wordsDescriptors = cv::Mat();
 	_invalidWordsCount = 0;
 }

-void Signature::removeWord(int wordId)
+void Signature::setWordsDescriptors(const cv::Mat & descriptors)
 {
-	if(wordId<=0)
+	if(descriptors.empty())
 	{
-		_invalidWordsCount-=(int)_words.erase(wordId);
-		UASSERT(_invalidWordsCount>=0);
+		if(_wordsKpts.empty() && _words3.empty())
+		{
+			removeAllWords();
+		}
+		else
+		{
+			_wordsDescriptors = cv::Mat();
+		}
 	}
 	else
 	{
-		_words.erase(wordId);
+		UASSERT(descriptors.rows == (int)_words.size());
+		_wordsDescriptors = descriptors.clone();
 	}
-	_words3.erase(wordId);
-	_wordsDescriptors.clear();
 }

 cv::Mat Signature::getPoseCovariance() const
@@ -321,19 +342,23 @@ cv::Mat Signature::getPoseCovariance() const
 	return covariance;
 }

-long Signature::getMemoryUsed(bool withSensorData) const // Return memory usage in Bytes
+unsigned long Signature::getMemoryUsed(bool withSensorData) const // Return memory usage in Bytes
 {
-	long total =  _words.size() * sizeof(float) * 8 +
-				  _words3.size() * sizeof(float) * 4;
-	if(!_wordsDescriptors.empty())
-	{
-		total += _wordsDescriptors.size() * sizeof(int);
-		total += _wordsDescriptors.size() * _wordsDescriptors.begin()->second.total() * _wordsDescriptors.begin()->second.elemSize();
-	}
+	unsigned long total = sizeof(Signature);
+	total += _words.size() * (sizeof(int)*2+sizeof(std::multimap<int, cv::KeyPoint>::iterator)) + sizeof(std::multimap<int, cv::KeyPoint>);
+	total += _wordsKpts.size() * sizeof(cv::KeyPoint) + sizeof(std::vector<cv::KeyPoint>);
+	total += _words3.size() * sizeof(cv::Point3f) + sizeof(std::vector<cv::Point3f>);
+	total += _wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat);
+	total += _wordsChanged.size() * (sizeof(int)*2+sizeof(std::map<int, int>::iterator)) + sizeof(std::map<int, int>);
 	if(withSensorData)
 	{
 		total+=_sensorData.getMemoryUsed();
 	}
+	total += _pose.size() * (sizeof(Transform) + sizeof(float)*12);
+	total += _groundTruthPose.size() * (sizeof(Transform) + sizeof(float)*12);
+	total += _velocity.size() * sizeof(float);
+	total += _links.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double)+sizeof(std::multimap<int, Link>::iterator)) + sizeof(std::multimap<int, Link>);
+	total += _landmarks.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double)+sizeof(std::map<int, Link>::iterator)) + sizeof(std::map<int, Link>);
 	return total;
 }

@@ -166,9 +166,30 @@ float Transform::theta() const
 	return yaw;
 }

+bool Transform::isInvertible() const
+{
+	bool invertible = false;
+	Eigen::Matrix4f inverse;
+	Eigen::Matrix4f::RealScalar det;
+	toEigen4f().computeInverseAndDetWithCheck(inverse, det, invertible);
+	return invertible;
+}
+
 Transform Transform::inverse() const
 {
-	return fromEigen4f(toEigen4f().inverse());
+	bool invertible = false;
+	Eigen::Matrix4f inverse;
+	Eigen::Matrix4f::RealScalar det;
+	toEigen4f().computeInverseAndDetWithCheck(inverse, det, invertible);
+	UASSERT_MSG(invertible, uFormat("This transform is not invertible! %s \n"
+			"[%f %f %f %f;\n"
+			" %f %f %f %f;\n"
+			" %f %f %f %f;\n"
+			" 0 0 0 1]", prettyPrint().c_str(),
+			r11(), r12(), r13(), o14(),
+			r21(), r22(), r23(), o24(),
+			r31(), r32(), r33(), o34()).c_str());
+	return fromEigen4f(inverse);
 }

 Transform Transform::rotation() const
@@ -311,14 +332,9 @@ bool Transform::operator!=(const Transform & t) const

 std::ostream& operator<<(std::ostream& os, const Transform& s)
 {
-	for(int i = 0; i < 3; ++i)
-	{
-		for(int j = 0; j < 4; ++j)
-		{
-			os << std::left << std::setw(12) << s.data()[i*4 + j] << " ";
-		}
-		os << std::endl;
-	}
+	os << "[" << s.data()[0] << ", " << s.data()[1] << ", " << s.data()[2] << ", " << s.data()[3] << ";" << std::endl
+	   << " " << s.data()[4] << ", " << s.data()[5] << ", " << s.data()[6] << ", " << s.data()[7] << ";" << std::endl
+	   << " " << s.data()[8] << ", " << s.data()[9] << ", " << s.data()[10]<< ", " << s.data()[11] << "]";
 	return os;
 }

@@ -65,6 +65,7 @@ VWDictionary::VWDictionary(const ParametersMap & parameters) :
 	_incrementalDictionary(Parameters::defaultKpIncrementalDictionary()),
 	_incrementalFlann(Parameters::defaultKpIncrementalFlann()),
 	_rebalancingFactor(Parameters::defaultKpFlannRebalancingFactor()),
+	_byteToFloat(Parameters::defaultKpByteToFloat()),
 	_nndrRatio(Parameters::defaultKpNndrRatio()),
 	_newDictionaryPath(Parameters::defaultKpDictionaryPath()),
 	_newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()),
@@ -90,6 +91,8 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether);
 	Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann);
 	Parameters::parse(parameters, Parameters::kKpFlannRebalancingFactor(), _rebalancingFactor);
+	bool byteToFloat = _byteToFloat;
+	Parameters::parse(parameters, Parameters::kKpByteToFloat(), _byteToFloat);

 	UASSERT_MSG(_nndrRatio > 0.0f, uFormat("String=%s value=%f", uContains(parameters, Parameters::kKpNndrRatio())?parameters.at(Parameters::kKpNndrRatio()).c_str():"", _nndrRatio).c_str());

@@ -104,10 +107,19 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 	}

 	// Verifying hypotheses strategy
+	bool treeUpdated = false;
 	if((iter=parameters.find(Parameters::kKpNNStrategy())) != parameters.end())
 	{
 		NNStrategy nnStrategy = (NNStrategy)std::atoi((*iter).second.c_str());
-		this->setNNStrategy(nnStrategy);
+		treeUpdated = this->setNNStrategy(nnStrategy);
+	}
+	if(!treeUpdated && byteToFloat!=_byteToFloat && _strategy == kNNFlannKdTree)
+	{
+		UINFO("KDTree: Binary to Float conversion approach has changed, re-initialize kd-tree.");
+		_dataTree = cv::Mat();
+		_notIndexedWords = uKeysSet(_visualWords);
+		_removedIndexedWords.clear();
+		this->update();
 	}

 	if(incrementalDictionary)
@@ -277,7 +289,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 	_newDictionaryPath = dictionaryPath;
 }

-void VWDictionary::setNNStrategy(NNStrategy strategy)
+bool VWDictionary::setNNStrategy(NNStrategy strategy)
 {
 #if CV_MAJOR_VERSION < 3
 #ifdef HAVE_OPENCV_GPU
@@ -319,11 +331,17 @@ void VWDictionary::setNNStrategy(NNStrategy strategy)
 	_strategy = strategy;
 	if(update)
 	{
+		if(_notIndexedWords.size() != _visualWords.size() || !_dataTree.empty())
+		{
+			UINFO("Nearest neighbor strategy has changed, re-initialize search tree.");
+		}
 		_dataTree = cv::Mat();
 		_notIndexedWords = uKeysSet(_visualWords);
 		_removedIndexedWords.clear();
 		this->update();
+		return true;
 	}
+	return false;
 }

 int VWDictionary::getLastIndexedWordId() const
@@ -348,59 +366,103 @@ unsigned int VWDictionary::getIndexMemoryUsed() const
 	return _flannIndex->memoryUsed();
 }

-cv::Mat VWDictionary::convertBinTo32F(const cv::Mat & descriptorsIn)
+unsigned long VWDictionary::getMemoryUsed() const
 {
-	// Old approach
-	//cv::Mat descriptorsOut;
-	//descriptorsIn.convertTo(descriptorsOut, CV_32F);
-	//return descriptorsOut;
-
-	// New approach
-	UASSERT(descriptorsIn.type() == CV_8UC1);
-	cv::Mat descriptorsOut(descriptorsIn.rows, descriptorsIn.cols*8, CV_32FC1);
-	for(int i=0; i<descriptorsIn.rows; ++i)
+	long memoryUsage = sizeof(VWDictionary);
+	memoryUsage += getIndexMemoryUsed();
+	memoryUsage += _dataTree.total()*_dataTree.elemSize();
+	if(!_visualWords.empty())
 	{
-		const unsigned char * ptrIn = descriptorsIn.ptr(i);
-		float * ptrOut = descriptorsOut.ptr<float>(i);
-		for(int j=0; j<descriptorsIn.cols; ++j)
+		memoryUsage += _visualWords.size()*(sizeof(int) + _visualWords.rbegin()->second->getMemoryUsed() + sizeof(std::map<int, VisualWord *>::iterator)) + sizeof(std::map<int, VisualWord *>);
+		if(_dataTree.empty() &&
+			_visualWords.begin()->second->getDescriptor().type() == CV_8U &&
+			_strategy == kNNFlannKdTree)
 		{
-			int jo = j*8;
-			ptrOut[jo] = (ptrIn[j] & 1) == 1?1.0f:0.0f;
-			ptrOut[jo+1] = (ptrIn[j] & (1<<1)) != 0?1.0f:0.0f;
-			ptrOut[jo+2] = (ptrIn[j] & (1<<2)) != 0?1.0f:0.0f;
-			ptrOut[jo+3] = (ptrIn[j] & (1<<3)) != 0?1.0f:0.0f;
-			ptrOut[jo+4] = (ptrIn[j] & (1<<4)) != 0?1.0f:0.0f;
-			ptrOut[jo+5] = (ptrIn[j] & (1<<5)) != 0?1.0f:0.0f;
-			ptrOut[jo+6] = (ptrIn[j] & (1<<6)) != 0?1.0f:0.0f;
-			ptrOut[jo+7] = (ptrIn[j] & (1<<7)) != 0?1.0f:0.0f;
+			// Binary descriptors were converted to float, and not included in _dataTree
+			memoryUsage += _visualWords.size() * _visualWords.begin()->second->getDescriptor().total() * sizeof(float) * (_byteToFloat?1:8);
 		}
 	}
-	return descriptorsOut;
+	if(!_unusedWords.empty())
+	{
+		// they are the same words than in _visualWords, so just add the pointer size
+		memoryUsage += _unusedWords.size()*(sizeof(int) + sizeof(VisualWord *)+sizeof(std::map<int, VisualWord *>::iterator)) + sizeof(std::map<int, VisualWord *>);
+	}
+	memoryUsage += _mapIndexId.size() * (sizeof(int)*2+sizeof(std::map<int ,int>::iterator)) + sizeof(std::map<int ,int>);
+	memoryUsage += _mapIdIndex.size() * (sizeof(int)*2+sizeof(std::map<int ,int>::iterator)) + sizeof(std::map<int ,int>);
+	memoryUsage += _notIndexedWords.size() * (sizeof(int)+sizeof(std::set<int>::iterator)) + sizeof(std::set<int>);
+	memoryUsage += _removedIndexedWords.size() * (sizeof(int)+sizeof(std::set<int>::iterator)) + sizeof(std::set<int>);
+	return memoryUsage;
 }

-cv::Mat VWDictionary::convert32FToBin(const cv::Mat & descriptorsIn)
+cv::Mat VWDictionary::convertBinTo32F(const cv::Mat & descriptorsIn, bool byteToFloat)
 {
-	UASSERT(descriptorsIn.type() == CV_32FC1 && descriptorsIn.cols % 8 == 0);
-	cv::Mat descriptorsOut(descriptorsIn.rows, descriptorsIn.cols/8, CV_8UC1);
-	for(int i=0; i<descriptorsIn.rows; ++i)
+	if(byteToFloat)
 	{
-		const float * ptrIn = descriptorsIn.ptr<float>(i);
-		unsigned char * ptrOut = descriptorsOut.ptr(i);
-		for(int j=0; j<descriptorsOut.cols; ++j)
+		// Old approach
+		cv::Mat descriptorsOut;
+		descriptorsIn.convertTo(descriptorsOut, CV_32F);
+		return descriptorsOut;
+	}
+	else
+	{
+		// New approach
+		UASSERT(descriptorsIn.type() == CV_8UC1);
+		cv::Mat descriptorsOut(descriptorsIn.rows, descriptorsIn.cols*8, CV_32FC1);
+		for(int i=0; i<descriptorsIn.rows; ++i)
 		{
-			int jo = j*8;
-			ptrOut[j] =
-					(unsigned char)(ptrIn[jo] == 0?0:1) |
-					(ptrIn[jo+1] == 0?0:(1<<1)) |
-					(ptrIn[jo+2] == 0?0:(1<<2)) |
-					(ptrIn[jo+3] == 0?0:(1<<3)) |
-					(ptrIn[jo+4] == 0?0:(1<<4)) |
-					(ptrIn[jo+5] == 0?0:(1<<5)) |
-					(ptrIn[jo+6] == 0?0:(1<<6)) |
-					(ptrIn[jo+7] == 0?0:(1<<7));
+			const unsigned char * ptrIn = descriptorsIn.ptr(i);
+			float * ptrOut = descriptorsOut.ptr<float>(i);
+			for(int j=0; j<descriptorsIn.cols; ++j)
+			{
+				int jo = j*8;
+				ptrOut[jo] = (ptrIn[j] & 1) == 1?1.0f:0.0f;
+				ptrOut[jo+1] = (ptrIn[j] & (1<<1)) != 0?1.0f:0.0f;
+				ptrOut[jo+2] = (ptrIn[j] & (1<<2)) != 0?1.0f:0.0f;
+				ptrOut[jo+3] = (ptrIn[j] & (1<<3)) != 0?1.0f:0.0f;
+				ptrOut[jo+4] = (ptrIn[j] & (1<<4)) != 0?1.0f:0.0f;
+				ptrOut[jo+5] = (ptrIn[j] & (1<<5)) != 0?1.0f:0.0f;
+				ptrOut[jo+6] = (ptrIn[j] & (1<<6)) != 0?1.0f:0.0f;
+				ptrOut[jo+7] = (ptrIn[j] & (1<<7)) != 0?1.0f:0.0f;
+			}
 		}
+		return descriptorsOut;
+	}
+}
+
+cv::Mat VWDictionary::convert32FToBin(const cv::Mat & descriptorsIn, bool byteToFloat)
+{
+	if(byteToFloat)
+	{
+		// Old approach
+		cv::Mat descriptorsOut;
+		descriptorsIn.convertTo(descriptorsOut, CV_8UC1);
+		return descriptorsOut;
+	}
+	else
+	{
+		// New approach
+		UASSERT(descriptorsIn.type() == CV_32FC1 && descriptorsIn.cols % 8 == 0);
+		cv::Mat descriptorsOut(descriptorsIn.rows, descriptorsIn.cols/8, CV_8UC1);
+		for(int i=0; i<descriptorsIn.rows; ++i)
+		{
+			const float * ptrIn = descriptorsIn.ptr<float>(i);
+			unsigned char * ptrOut = descriptorsOut.ptr(i);
+			for(int j=0; j<descriptorsOut.cols; ++j)
+			{
+				int jo = j*8;
+				ptrOut[j] =
+						(unsigned char)(ptrIn[jo] == 0?0:1) |
+						(ptrIn[jo+1] == 0?0:(1<<1)) |
+						(ptrIn[jo+2] == 0?0:(1<<2)) |
+						(ptrIn[jo+3] == 0?0:(1<<3)) |
+						(ptrIn[jo+4] == 0?0:(1<<4)) |
+						(ptrIn[jo+5] == 0?0:(1<<5)) |
+						(ptrIn[jo+6] == 0?0:(1<<6)) |
+						(ptrIn[jo+7] == 0?0:(1<<7));
+			}
+		}
+		return descriptorsOut;
 	}
-	return descriptorsOut;
 }

 void VWDictionary::update()
@@ -451,7 +513,7 @@ void VWDictionary::update()
 						useDistanceL1_ = true;
 						if(_strategy == kNNFlannKdTree)
 						{
-							descriptor = convertBinTo32F(w->getDescriptor());
+							descriptor = convertBinTo32F(w->getDescriptor(), _byteToFloat);
 						}
 						else
 						{
@@ -490,7 +552,8 @@ void VWDictionary::update()
 					{
 						UASSERT(descriptor.cols == _flannIndex->featuresDim());
 						UASSERT(descriptor.type() == _flannIndex->featuresType());
-						index = _flannIndex->addPoints(descriptor);
+						UASSERT(descriptor.rows == 1);
+						index = _flannIndex->addPoints(descriptor).front();
 					}
 					std::pair<std::map<int, int>::iterator, bool> inserted;
 					inserted = _mapIndexId.insert(std::pair<int, int>(index, w->id()));
@@ -543,7 +606,10 @@ void VWDictionary::update()
 					if(_strategy == kNNFlannKdTree)
 					{
 						type = CV_32F;
-						dim *= 8;
+						if(!_byteToFloat)
+						{
+							dim *= 8;
+						}
 					}
 					else
 					{
@@ -568,7 +634,7 @@ void VWDictionary::update()
 					{
 						if(_strategy == kNNFlannKdTree)
 						{
-							descriptor = convertBinTo32F(iter->second->getDescriptor());
+							descriptor = convertBinTo32F(iter->second->getDescriptor(), _byteToFloat);
 						}
 						else
 						{
@@ -594,15 +660,15 @@ void VWDictionary::update()
 				switch(_strategy)
 				{
 				case kNNFlannNaive:
-					_flannIndex->buildLinearIndex(_dataTree, useDistanceL1_, _rebalancingFactor);
+					_flannIndex->buildLinearIndex(_dataTree, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
 					break;
 				case kNNFlannKdTree:
 					UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
-					_flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_, _rebalancingFactor);
+					_flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
 					break;
 				case kNNFlannLSH:
 					UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
-					_flannIndex->buildLSHIndex(_dataTree, 12, 20, 2, _rebalancingFactor);
+					_flannIndex->buildLSHIndex(_dataTree, 12, 20, 2, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
 					break;
 				default:
 					break;
@@ -736,7 +802,7 @@ std::list<int> VWDictionary::addNewWords(
 		useDistanceL1_ = true;
 		if(_strategy == kNNFlannKdTree)
 		{
-			descriptors = convertBinTo32F(descriptorsIn);
+			descriptors = convertBinTo32F(descriptorsIn, _byteToFloat);
 		}
 		else
 		{
@@ -1074,7 +1140,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 		{
 			if(_strategy == kNNFlannKdTree)
 			{
-				query = convertBinTo32F(queryIn);
+				query = convertBinTo32F(queryIn, _byteToFloat);
 			}
 			else
 			{
@@ -1202,7 +1268,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 				{
 					if(_strategy == kNNFlannKdTree)
 					{
-						descriptor = convertBinTo32F(vw->getDescriptor());
+						descriptor = convertBinTo32F(vw->getDescriptor(), _byteToFloat);
 					}
 					else
 					{
@@ -69,4 +69,13 @@ int VisualWord::removeAllRef(int signatureId)
 	return removed;
 }

+unsigned long VisualWord::getMemoryUsed() const
+{
+	unsigned long memoryUsage = sizeof(VisualWord);
+	memoryUsage += _references.size() * (sizeof(int)*2+sizeof(std::map<int ,int>::iterator)) + sizeof(std::map<int ,int>);
+	memoryUsage += _oldReferences.size() * (sizeof(int)*2+sizeof(std::map<int ,int>::iterator)) + sizeof(std::map<int ,int>);
+	memoryUsage += _descriptor.total() * _descriptor.elemSize();
+	return memoryUsage;
+}
+
 } // namespace rtabmap
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UThreadC.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
@@ -58,6 +59,7 @@ CameraImages::CameraImages() :
 		_depthFromScanFillHoles(1),
 		_depthFromScanFillHolesFromBorder(false),
 		_filenamesAreTimestamps(false),
+		_hasConfigForEachFrame(false),
 		_syncImageRateWithStamps(true),
 		_odometryFormat(0),
 		_groundTruthFormat(0),
@@ -87,6 +89,7 @@ CameraImages::CameraImages(const std::string & path,
 	_depthFromScanFillHoles(1),
 	_depthFromScanFillHolesFromBorder(false),
 	_filenamesAreTimestamps(false),
+	_hasConfigForEachFrame(false),
 	_syncImageRateWithStamps(true),
 	_odometryFormat(0),
 	_groundTruthFormat(0),
@@ -111,6 +114,9 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	_countScan = 0;
 	_captureDelay = 0.0;
 	_framesPublished=0;
+	_model = cameraModel();
+	_models.clear();
+	covariances_.clear();

 	UDEBUG("");
 	if(_dir)
@@ -213,7 +219,108 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	groundTruth_.clear();
 	if(success)
 	{
-		if(_filenamesAreTimestamps)
+		if(_hasConfigForEachFrame)
+		{
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && CV_MAJOR_VERSION < 2)
+			UDirectory dirJson(_path, "yaml xml");
+#else
+			UDirectory dirJson(_path, "yaml xml json");
+#endif
+			if(dirJson.getFileNames().size() == _dir->getFileNames().size())
+			{
+				bool modelsWarned = false;
+				bool firstFrame = true;
+				for(std::list<std::string>::const_iterator iter=dirJson.getFileNames().begin(); iter!=dirJson.getFileNames().end() && success; ++iter)
+				{
+					// Assuming 3DScannerApp(iOS) format (only this one supported...)
+					std::string filePath = _path+"/"+*iter;
+					cv::FileStorage fs(filePath, 0);
+					cv::FileNode poseNode = fs["cameraPoseARFrame"];
+					cv::FileNode timeNode = fs["time"];
+					cv::FileNode intrinsicsNode = fs["intrinsics"];
+					if(poseNode.isNone() || poseNode.size() != 16)
+					{
+						UERROR("Failed reading \"cameraPoseARFrame\" parameter, it should have 16 values (file=%s)", filePath.c_str());
+						success = false;
+						break;
+					}
+					else if(timeNode.isNone() || !timeNode.isReal())
+					{
+						UERROR("Failed reading \"time\" parameter (file=%s)", filePath.c_str());
+						success = false;
+						break;
+					}
+					else if(intrinsicsNode.isNone() || intrinsicsNode.size()!=9)
+					{
+						UERROR("Failed reading \"intrinsics\" parameter (file=%s)", filePath.c_str());
+						success = false;
+						break;
+					}
+					else
+					{
+						_stamps.push_back((double)timeNode);
+						if(_model.isValidForProjection() && !modelsWarned)
+						{
+							UWARN("Camera model loaded for each frame is overridden by "
+									"general calibration file provided. Remove general calibration "
+									"file to use camera model of each frame. This warning will "
+									"be shown only one time.");
+							modelsWarned = true;
+						}
+						else
+						{
+							_models.push_back(CameraModel(
+									(double)intrinsicsNode[0], //fx
+									(double)intrinsicsNode[4], //fy
+									(double)intrinsicsNode[2], //cx
+									(double)intrinsicsNode[5], //cy
+									CameraModel::opticalRotation()));
+						}
+						// we need to rotate from opengl world to rtabmap world
+						Transform pose(
+								(float)poseNode[0], (float)poseNode[1], (float)poseNode[2], (float)poseNode[3],
+								(float)poseNode[4], (float)poseNode[5], (float)poseNode[6], (float)poseNode[7],
+								(float)poseNode[8], (float)poseNode[9], (float)poseNode[10], (float)poseNode[11]);
+						pose =  Transform::rtabmap_T_opengl() * pose * Transform::opengl_T_rtabmap();
+						odometry_.push_back(pose);
+						// linear cov = 0.0001
+						cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
+						if(!firstFrame)
+						{
+							// angular cov = 0.000001
+							covariance.at<double>(3,3) *= 0.01;
+							covariance.at<double>(4,4) *= 0.01;
+							covariance.at<double>(5,5) *= 0.01;
+						}
+						firstFrame = false;
+						covariances_.push_back(covariance);
+					}
+				}
+				if(!success)
+				{
+					odometry_.clear();
+					_stamps.clear();
+					_models.clear();
+					covariances_.clear();
+				}
+			}
+			else
+			{
+				std::string opencv32warn;
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && CV_MAJOR_VERSION < 2)
+				opencv32warn = " RTAB-Map is currently built with OpenCV < 3.2, only xml and yaml files are supported (not json).";
+#endif
+				UERROR("Parameter \"Config for each frame\" is true, but the "
+						"number of config files (%d) is not equal to number "
+						"of images (%d) in this directory \"%s\".%s",
+						(int)dirJson.getFileNames().size(),
+						(int)_dir->getFileNames().size(),
+						_path.c_str(),
+						opencv32warn.c_str());
+				success = false;
+			}
+		}
+		else if(_filenamesAreTimestamps)
 		{
 			const std::list<std::string> & filenames = _dir->getFileNames();
 			for(std::list<std::string>::const_iterator iter=filenames.begin(); iter!=filenames.end(); ++iter)
@@ -316,7 +423,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 			}
 		}

-		if(success && _odometryPath.size())
+		if(success && _odometryPath.size() && odometry_.empty())
 		{
 			success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat, _maxPoseTimeDiff);
 		}
@@ -332,7 +439,12 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	return success;
 }

-bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<double> & inOutStamps, const std::string & filePath, int format, double maxTimeDiff) const
+bool CameraImages::readPoses(
+		std::list<Transform> & outputPoses,
+		std::list<double> & inOutStamps,
+		const std::string & filePath,
+		int format,
+		double maxTimeDiff) const
 {
 	outputPoses.clear();
 	std::map<int, Transform> poses;
@@ -436,6 +548,11 @@ bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<doubl
 			UERROR("With Karlsruhe format, timestamps (%d) and poses (%d) should match!", (int)stamps.size(), (int)poses.size());
 			return false;
 		}
+		else if(!outputPoses.empty() && inOutStamps.empty() && stamps.empty())
+		{
+			UERROR("Timestamps are empty (poses=%d)! Forgot the set a timestamp file?", (int)outputPoses.size());
+			return false;
+		}
 	}
 	UASSERT_MSG(outputPoses.size() == inOutStamps.size(), uFormat("%d vs %d", (int)outputPoses.size(), (int)inOutStamps.size()).c_str());
 	return true;
@@ -443,7 +560,7 @@ bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<doubl

 bool CameraImages::isCalibrated() const
 {
-	return _model.isValidForProjection();
+	return _model.isValidForProjection() || (_models.size() && _models.front().isValidForProjection());
 }

 std::string CameraImages::getSerial() const
@@ -506,8 +623,10 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 	LaserScan scan(cv::Mat(), _scanMaxPts, 0, LaserScan::kUnknown, _scanLocalTransform);
 	double stamp = UTimer::now();
 	Transform odometryPose;
+	cv::Mat covariance;
 	Transform groundTruthPose;
 	cv::Mat depthFromScan;
+	CameraModel model = _model;
 	UDEBUG("");
 	if(_dir->isValid())
 	{
@@ -553,17 +672,27 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 				{
 					_captureDelay = _stamps.front() - stamp;
 				}
-				if(odometry_.size())
+			}
+			if(odometry_.size())
+			{
+				odometryPose = odometry_.front();
+				odometry_.pop_front();
+				if(covariances_.size())
 				{
-					odometryPose = odometry_.front();
-					odometry_.pop_front();
-				}
-				if(groundTruth_.size())
-				{
-					groundTruthPose = groundTruth_.front();
-					groundTruth_.pop_front();
+					covariance = covariances_.front();
+					covariances_.pop_front();
 				}
 			}
+			if(groundTruth_.size())
+			{
+				groundTruthPose = groundTruth_.front();
+				groundTruth_.pop_front();
+			}
+			if(_models.size() && !model.isValidForProjection())
+			{
+				model = _models.front();
+				_models.pop_front();
+			}
 		}
 		else
 		{
@@ -580,17 +709,27 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 					{
 						_captureDelay = _stamps.front() - stamp;
 					}
-					if(odometry_.size())
+				}
+				if(odometry_.size())
+				{
+					odometryPose = odometry_.front();
+					odometry_.pop_front();
+					if(covariances_.size())
 					{
-						odometryPose = odometry_.front();
-						odometry_.pop_front();
-					}
-					if(groundTruth_.size())
-					{
-						groundTruthPose = groundTruth_.front();
-						groundTruth_.pop_front();
+						covariance = covariances_.front();
+						covariances_.pop_front();
 					}
 				}
+				if(groundTruth_.size())
+				{
+					groundTruthPose = groundTruth_.front();
+					groundTruth_.pop_front();
+				}
+				if(_models.size() && !model.isValidForProjection())
+				{
+					model = _models.front();
+					_models.pop_front();
+				}

 				while(_count++ < _startAt && (fileName = _dir->getNextFileName()).size())
 				{
@@ -603,17 +742,27 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 						{
 							_captureDelay = _stamps.front() - stamp;
 						}
-						if(odometry_.size())
+					}
+					if(odometry_.size())
+					{
+						odometryPose = odometry_.front();
+						odometry_.pop_front();
+						if(covariances_.size())
 						{
-							odometryPose = odometry_.front();
-							odometry_.pop_front();
-						}
-						if(groundTruth_.size())
-						{
-							groundTruthPose = groundTruth_.front();
-							groundTruth_.pop_front();
+							covariance = covariances_.front();
+							covariances_.pop_front();
 						}
 					}
+					if(groundTruth_.size())
+					{
+						groundTruthPose = groundTruth_.front();
+						groundTruth_.pop_front();
+					}
+					if(_models.size() && !model.isValidForProjection())
+					{
+						model = _models.front();
+						_models.pop_front();
+					}
 				}
 			}
 			if(_scanDir)
@@ -693,12 +842,11 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 							UWARN("Error debayering images: \"%s\". Please set bayer mode to -1 if images are not bayered!", e.what());
 						}
 					}
-
 				}

-				if(!img.empty() && _model.isValidForRectification() && _rectifyImages)
+				if(!img.empty() && model.isValidForRectification() && _rectifyImages)
 				{
-					img = _model.rectifyImage(img);
+					img = model.rectifyImage(img);
 				}
 			}

@@ -711,7 +859,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 				if(_depthFromScan && !img.empty())
 				{
 					UDEBUG("Computing depth from scan...");
-					if(!_model.isValidForProjection())
+					if(!model.isValidForProjection())
 					{
 						UWARN("Depth from laser scan: Camera model should be valid.");
 					}
@@ -722,7 +870,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 					else
 					{
 						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
-						depthFromScan = util3d::projectCloudToCamera(img.size(), _model.K(), cloud, _model.localTransform());
+						depthFromScan = util3d::projectCloudToCamera(img.size(), model.K(), cloud, model.localTransform());
 						if(_depthFromScanFillHoles!=0)
 						{
 							util3d::fillProjectedCloudHoles(depthFromScan, _depthFromScanFillHoles>0, _depthFromScanFillHolesFromBorder);
@@ -737,18 +885,18 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 		UWARN("Directory is not set, camera must be initialized.");
 	}

-	if(_model.imageHeight() == 0 || _model.imageWidth() == 0)
+	if(model.imageHeight() == 0 || model.imageWidth() == 0)
 	{
-		_model.setImageSize(img.size());
+		model.setImageSize(img.size());
 	}

-	SensorData data(scan, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
+	SensorData data(scan, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, model, this->getNextSeqID(), stamp);
 	data.setGroundTruth(groundTruthPose);

 	if(info && !odometryPose.isNull())
 	{
 		info->odomPose = odometryPose;
-		info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1); // Note that with TORO and g2o file formats, we could get the covariance
+		info->odomCovariance = covariance.empty()?cv::Mat::eye(6,6,CV_64FC1):covariance; // Note that with TORO and g2o file formats, we could get the covariance
 	}

 	return data;
@@ -55,12 +55,11 @@ CameraK4A::CameraK4A(
 			Camera(imageRate, localTransform)
 #ifdef RTABMAP_K4A
 			,
-			device_(NULL),
+			deviceHandle_(NULL),
 			config_(K4A_DEVICE_CONFIG_INIT_DISABLE_ALL),
-			transformation_(NULL),
-			capture_(NULL),
-			playbackHandle_(NULL),
 			transformationHandle_(NULL),
+			captureHandle_(NULL),
+			playbackHandle_(NULL),
 			deviceId_(deviceId),
 			rgb_resolution_(0),
 			framerate_(2),
@@ -78,11 +77,10 @@ CameraK4A::CameraK4A(
 			Camera(imageRate, localTransform)
 #ifdef RTABMAP_K4A
 			,
-			device_(NULL),
-			transformation_(NULL),
-			capture_(NULL),
-			playbackHandle_(NULL),
+			deviceHandle_(NULL),
 			transformationHandle_(NULL),
+			captureHandle_(NULL),
+			playbackHandle_(NULL),
 			deviceId_(-1),
 			fileName_(fileName),
 			rgb_resolution_(0),
@@ -102,37 +100,25 @@ CameraK4A::~CameraK4A()
 void CameraK4A::close()
 {
 #ifdef RTABMAP_K4A
-	if (!fileName_.empty())
+	if (playbackHandle_ != NULL)
 	{
-		if (playbackHandle_ != NULL)
-		{
-			k4a_playback_close((k4a_playback_t)playbackHandle_);
-			playbackHandle_ = NULL;
-		}
-
-		if (transformationHandle_ != NULL)
-		{
-			k4a_transformation_destroy((k4a_transformation_t)transformationHandle_);
-			transformationHandle_ = NULL;
-		}
+		k4a_playback_close((k4a_playback_t)playbackHandle_);
+		playbackHandle_ = NULL;
 	}
-	else
+	else if (deviceHandle_ != NULL)
 	{
-		if (device_ != NULL)
-		{
-			k4a_device_stop_imu(device_);
+		k4a_device_stop_imu(deviceHandle_);

-			if (transformation_ != NULL)
-			{
-				k4a_transformation_destroy(transformation_);
-				transformation_ = NULL;
-			}
+		k4a_device_stop_cameras(deviceHandle_);
+		k4a_device_close(deviceHandle_);
+		deviceHandle_ = NULL;
+		config_ = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
+	}

-			k4a_device_stop_cameras(device_);
-			k4a_device_close(device_);
-			device_ = NULL;
-			config_ = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
-		}
+	if (transformationHandle_ != NULL)
+	{
+		k4a_transformation_destroy((k4a_transformation_t)transformationHandle_);
+		transformationHandle_ = NULL;
 	}
 #endif
 }
@@ -171,50 +157,17 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 		uint64_t recording_length = k4a_playback_get_last_timestamp_usec((k4a_playback_t)playbackHandle_);
 		UINFO("Recording is %lld seconds long", recording_length / 1000000);

-		k4a_calibration_t calibration;
-		if (k4a_playback_get_calibration((k4a_playback_t)playbackHandle_, &calibration))
+		if (k4a_playback_get_calibration((k4a_playback_t)playbackHandle_, &calibration_))
 		{
 			UERROR("Failed to get calibration");
 			close();
 			return false;
 		}
-		if (ir_)
-		{
-			model_ = CameraModel(
-					calibration.depth_camera_calibration.intrinsics.parameters.param.fx,
-					calibration.depth_camera_calibration.intrinsics.parameters.param.fy,
-					calibration.depth_camera_calibration.intrinsics.parameters.param.cx,
-					calibration.depth_camera_calibration.intrinsics.parameters.param.cy,
-					this->getLocalTransform(),
-					0,
-					cv::Size(calibration.depth_camera_calibration.resolution_width, calibration.depth_camera_calibration.resolution_height));
-		}
-		else
-		{
-			model_ = CameraModel(
-					calibration.color_camera_calibration.intrinsics.parameters.param.fx,
-					calibration.color_camera_calibration.intrinsics.parameters.param.fy,
-					calibration.color_camera_calibration.intrinsics.parameters.param.cx,
-					calibration.color_camera_calibration.intrinsics.parameters.param.cy,
-					this->getLocalTransform(),
-					0,
-					cv::Size(calibration.color_camera_calibration.resolution_width, calibration.color_camera_calibration.resolution_height));

-			transformationHandle_ = k4a_transformation_create(&calibration);
-		}
-
-		k4a_record_configuration_t config;
-
-		if (k4a_playback_get_record_configuration((k4a_playback_t)playbackHandle_, &config))
-		{
-			UERROR("Failed to getting recording configuration");
-			close();
-			return false;
-		}
 	}
 	else if (deviceId_ >= 0)
 	{
-		if(device_!=NULL)
+		if(deviceHandle_!=NULL)
 		{
 			this->close();
 		}
@@ -265,7 +218,7 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 		UINFO("CameraK4A found %d k4a device(s) attached", device_count);

 		// Open the first plugged in Kinect device
-		if (K4A_FAILED(k4a_device_open(deviceId_, &device_)))
+		if (K4A_FAILED(k4a_device_open(deviceId_, &deviceHandle_)))
 		{
 			UERROR("Failed to open k4a device!");
 			return false;
@@ -273,18 +226,18 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &

 		// Get the size of the serial number
 		size_t serial_size = 0;
-		k4a_device_get_serialnum(device_, NULL, &serial_size);
+		k4a_device_get_serialnum(deviceHandle_, NULL, &serial_size);

 		// Allocate memory for the serial, then acquire it
 		char *serial = (char*)(malloc(serial_size));
-		k4a_device_get_serialnum(device_, serial, &serial_size);
+		k4a_device_get_serialnum(deviceHandle_, serial, &serial_size);
 		serial_number_.assign(serial, serial_size);
 		free(serial);

 		UINFO("Opened K4A device: %s", serial_number_.c_str());

 		// Start the camera with the given configuration
-		if (K4A_FAILED(k4a_device_start_cameras(device_, &config_)))
+		if (K4A_FAILED(k4a_device_start_cameras(deviceHandle_, &config_)))
 		{
 			UERROR("Failed to start cameras!");
 			close();
@@ -293,59 +246,121 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &

 		UINFO("K4A camera started successfully");

-		if (K4A_FAILED(k4a_device_get_calibration(device_, config_.depth_mode, config_.color_resolution, &calibration_)))
+		if (K4A_FAILED(k4a_device_get_calibration(deviceHandle_, config_.depth_mode, config_.color_resolution, &calibration_)))
 		{
 			UERROR("k4a_device_get_calibration() failed!");
 			close();
 			return false;
 		}
+	}
+	else
+	{
+		UERROR("k4a_device_get_calibration() no file and no valid device id!");
+		return false;
+	}

-		if (ir_)
+	if (ir_)
+	{
+		cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
+		K.at<double>(0,0) = calibration_.depth_camera_calibration.intrinsics.parameters.param.fx;
+		K.at<double>(1,1) = calibration_.depth_camera_calibration.intrinsics.parameters.param.fy;
+		K.at<double>(0,2) = calibration_.depth_camera_calibration.intrinsics.parameters.param.cx;
+		K.at<double>(1,2) = calibration_.depth_camera_calibration.intrinsics.parameters.param.cy;
+		cv::Mat D = cv::Mat::eye(1, 8, CV_64FC1);
+		D.at<double>(0,0) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k1;
+		D.at<double>(0,1) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k2;
+		D.at<double>(0,2) = calibration_.depth_camera_calibration.intrinsics.parameters.param.p1;
+		D.at<double>(0,3) = calibration_.depth_camera_calibration.intrinsics.parameters.param.p2;
+		D.at<double>(0,4) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k3;
+		D.at<double>(0,5) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k4;
+		D.at<double>(0,6) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k5;
+		D.at<double>(0,7) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k6;
+		cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+		cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
+		P.at<double>(0,0) = K.at<double>(0,0);
+		P.at<double>(1,1) = K.at<double>(1,1);
+		P.at<double>(0,2) = K.at<double>(0,2);
+		P.at<double>(1,2) = K.at<double>(1,2);
+		model_ = CameraModel(
+				"k4a_ir",
+				cv::Size(calibration_.depth_camera_calibration.resolution_width, calibration_.depth_camera_calibration.resolution_height),
+				K,D,R,P,
+				this->getLocalTransform());
+		UASSERT(model_.isValidForRectification());
+		model_.initRectificationMap();
+	}
+	else
+	{
+		cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
+		K.at<double>(0,0) = calibration_.color_camera_calibration.intrinsics.parameters.param.fx;
+		K.at<double>(1,1) = calibration_.color_camera_calibration.intrinsics.parameters.param.fy;
+		K.at<double>(0,2) = calibration_.color_camera_calibration.intrinsics.parameters.param.cx;
+		K.at<double>(1,2) = calibration_.color_camera_calibration.intrinsics.parameters.param.cy;
+		cv::Mat D = cv::Mat::eye(1, 8, CV_64FC1);
+		D.at<double>(0,0) = calibration_.color_camera_calibration.intrinsics.parameters.param.k1;
+		D.at<double>(0,1) = calibration_.color_camera_calibration.intrinsics.parameters.param.k2;
+		D.at<double>(0,2) = calibration_.color_camera_calibration.intrinsics.parameters.param.p1;
+		D.at<double>(0,3) = calibration_.color_camera_calibration.intrinsics.parameters.param.p2;
+		D.at<double>(0,4) = calibration_.color_camera_calibration.intrinsics.parameters.param.k3;
+		D.at<double>(0,5) = calibration_.color_camera_calibration.intrinsics.parameters.param.k4;
+		D.at<double>(0,6) = calibration_.color_camera_calibration.intrinsics.parameters.param.k5;
+		D.at<double>(0,7) = calibration_.color_camera_calibration.intrinsics.parameters.param.k6;
+		cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+		cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
+		P.at<double>(0,0) = K.at<double>(0,0);
+		P.at<double>(1,1) = K.at<double>(1,1);
+		P.at<double>(0,2) = K.at<double>(0,2);
+		P.at<double>(1,2) = K.at<double>(1,2);
+		model_ = CameraModel(
+				"k4a_color",
+				cv::Size(calibration_.color_camera_calibration.resolution_width, calibration_.color_camera_calibration.resolution_height),
+				K,D,R,P,
+				this->getLocalTransform());
+	}
+
+	if (ULogger::level() <= ULogger::kInfo)
+	{
+		UINFO("K4A calibration:");
+		std::cout << model_ << std::endl;
+	}
+
+	transformationHandle_ = k4a_transformation_create(&calibration_);
+
+	// Get imu transform
+	k4a_calibration_extrinsics_t* imu_extrinsics;
+	if(ir_)
+	{
+		imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_DEPTH];
+	}
+	else
+	{
+		imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_COLOR];
+	}
+	imuLocalTransform_ = Transform(
+		imu_extrinsics->rotation[0], imu_extrinsics->rotation[1], imu_extrinsics->rotation[2], imu_extrinsics->translation[0] / 1000.0f,
+		imu_extrinsics->rotation[3], imu_extrinsics->rotation[4], imu_extrinsics->rotation[5], imu_extrinsics->translation[1] / 1000.0f,
+		imu_extrinsics->rotation[6], imu_extrinsics->rotation[7], imu_extrinsics->rotation[8], imu_extrinsics->translation[2] / 1000.0f);
+
+	UINFO("camera to imu=%s", imuLocalTransform_.prettyPrint().c_str());
+	UINFO("base to camera=%s", this->getLocalTransform().prettyPrint().c_str());
+	imuLocalTransform_ = this->getLocalTransform()*imuLocalTransform_;
+	UINFO("base to imu=%s", imuLocalTransform_.prettyPrint().c_str());
+
+
+	// Start playback or camera
+	if (!fileName_.empty())
+	{
+		k4a_record_configuration_t config;
+		if (k4a_playback_get_record_configuration((k4a_playback_t)playbackHandle_, &config))
 		{
-			model_ = CameraModel(
-					calibration_.depth_camera_calibration.intrinsics.parameters.param.fx,
-					calibration_.depth_camera_calibration.intrinsics.parameters.param.fy,
-					calibration_.depth_camera_calibration.intrinsics.parameters.param.cx,
-					calibration_.depth_camera_calibration.intrinsics.parameters.param.cy,
-					this->getLocalTransform(),
-					0,
-					cv::Size(calibration_.depth_camera_calibration.resolution_width, calibration_.depth_camera_calibration.resolution_height));
+			UERROR("Failed to getting recording configuration");
+			close();
+			return false;
 		}
-		else
-		{
-			model_ = CameraModel(
-					calibration_.color_camera_calibration.intrinsics.parameters.param.fx,
-					calibration_.color_camera_calibration.intrinsics.parameters.param.fy,
-					calibration_.color_camera_calibration.intrinsics.parameters.param.cx,
-					calibration_.color_camera_calibration.intrinsics.parameters.param.cy,
-					this->getLocalTransform(),
-					0,
-					cv::Size(calibration_.color_camera_calibration.resolution_width, calibration_.color_camera_calibration.resolution_height));
-		}
-
-		transformation_ = k4a_transformation_create(&calibration_);
-
-		// Get imu transform
-		k4a_calibration_extrinsics_t* imu_extrinsics;
-		if(ir_)
-		{
-			imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_DEPTH];
-		}
-		else
-		{
-			imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_COLOR];
-		}
-		imuLocalTransform_ = Transform(
-			imu_extrinsics->rotation[0], imu_extrinsics->rotation[1], imu_extrinsics->rotation[2], imu_extrinsics->translation[0] / 1000.0f,
-			imu_extrinsics->rotation[3], imu_extrinsics->rotation[4], imu_extrinsics->rotation[5], imu_extrinsics->translation[1] / 1000.0f,
-			imu_extrinsics->rotation[6], imu_extrinsics->rotation[7], imu_extrinsics->rotation[8], imu_extrinsics->translation[2] / 1000.0f);
-
-		UINFO("camera to imu=%s", imuLocalTransform_.prettyPrint().c_str());
-		UINFO("base to camera=%s", this->getLocalTransform().prettyPrint().c_str());
-		imuLocalTransform_ = this->getLocalTransform()*imuLocalTransform_;
-		UINFO("base to imu=%s", imuLocalTransform_.prettyPrint().c_str());
-
-		if (K4A_FAILED(k4a_device_start_imu(device_)))
+	}
+	else
+	{
+		if (K4A_FAILED(k4a_device_start_imu(deviceHandle_)))
 		{
 			UERROR("Failed to start K4A IMU");
 			close();
@@ -355,9 +370,9 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 		UINFO("K4a IMU started successfully");

 		// Get an initial capture to put the camera in the right state
-		if (K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_capture(device_, &capture_, K4A_WAIT_INFINITE))
+		if (K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_capture(deviceHandle_, &captureHandle_, K4A_WAIT_INFINITE))
 		{
-			k4a_capture_release(capture_);
+			k4a_capture_release(captureHandle_);
 			return true;
 		}

@@ -395,197 +410,23 @@ SensorData CameraK4A::captureImage(CameraInfo * info)

 #ifdef RTABMAP_K4A

-	if (playbackHandle_ != NULL)
+	k4a_image_t ir_image_ = NULL;
+	k4a_image_t rgb_image_ = NULL;
+	k4a_imu_sample_t imu_sample_;
+
+	double t = UTimer::now();
+
+	bool captured = false;
+	if(playbackHandle_)
 	{
-		k4a_capture_t capture = NULL;
-		k4a_stream_result_t result = K4A_STREAM_RESULT_SUCCEEDED;
-
-		// wait to get all frames
-		UTimer time;
-		while (result == K4A_STREAM_RESULT_SUCCEEDED && time.elapsed() < 5.0)
+		k4a_stream_result_t result = K4A_STREAM_RESULT_FAILED;
+		while((UTimer::now()-t < 0.1) &&
+				(K4A_STREAM_RESULT_SUCCEEDED != (result=k4a_playback_get_next_capture(playbackHandle_, &captureHandle_)) ||
+				((ir_ && (ir_image_=k4a_capture_get_ir_image(captureHandle_)) == NULL) || (!ir_ && (rgb_image_=k4a_capture_get_color_image(captureHandle_)) == NULL))))
 		{
-			result = k4a_playback_get_next_capture((k4a_playback_t)playbackHandle_, &capture);
-
-			if (result == K4A_STREAM_RESULT_SUCCEEDED)
-			{
-				cv::Mat bgrCV;
-				cv::Mat depthCV;
-				double stamp = 0;
-
-				// Process capture here
-				if (ir_)
-				{
-					k4a_image_t ir = k4a_capture_get_ir_image(capture);
-					if (ir != NULL)
-					{
-						/*UDEBUG("ir res:%4dx%4d stride:%5d format:%d stamp=%f",
-							k4a_image_get_height_pixels(ir),
-							k4a_image_get_width_pixels(ir),
-							k4a_image_get_stride_bytes(ir),
-							k4a_image_get_format(ir),
-							double(k4a_image_get_timestamp_usec(ir)) / 1000000.0);*/
-
-						UASSERT(k4a_image_get_format(ir) == K4A_IMAGE_FORMAT_IR16);
-
-						cv::Mat bgrCV16(k4a_image_get_height_pixels(ir), k4a_image_get_width_pixels(ir), CV_16UC1, (void*)k4a_image_get_buffer(ir));
-						bgrCV16.convertTo(bgrCV, CV_8U);
-
-						// Release the image
-						k4a_image_release(ir);
-					}
-				}
-				else
-				{
-					k4a_image_t color = k4a_capture_get_color_image(capture);
-					if (color != NULL)
-					{
-						/*UDEBUG("Color res:%4dx%4d stride:%5d format:%d stamp=%f",
-							k4a_image_get_height_pixels(color),
-							k4a_image_get_width_pixels(color),
-							k4a_image_get_stride_bytes(color),
-							k4a_image_get_format(color),
-							double(k4a_image_get_timestamp_usec(color)) / 1000000.0);*/
-
-						UASSERT(k4a_image_get_format(color) == K4A_IMAGE_FORMAT_COLOR_MJPG || k4a_image_get_format(color) == K4A_IMAGE_FORMAT_COLOR_BGRA32);
-
-						if (k4a_image_get_format(color) == K4A_IMAGE_FORMAT_COLOR_MJPG)
-						{
-							bgrCV = uncompressImage(cv::Mat(1, (int)k4a_image_get_size(color), CV_8UC1, (void*)k4a_image_get_buffer(color)));
-							//UDEBUG("Uncompressed = %d %d %d", bgrCV.rows, bgrCV.cols, bgrCV.channels());
-						}
-						else
-						{
-							cv::Mat bgra(k4a_image_get_height_pixels(color), k4a_image_get_width_pixels(color), CV_8UC4, (void*)k4a_image_get_buffer(color));
-							cv::cvtColor(bgra, bgrCV, CV_BGRA2BGR);
-						}
-
-						// Release the image
-						k4a_image_release(color);
-					}
-				}
-
-				if (!bgrCV.empty())
-				{
-					k4a_image_t depth = k4a_capture_get_depth_image(capture);
-					if (depth != NULL)
-					{
-						/*UDEBUG("Depth16 res:%4dx%4d stride:%5d format:%d stamp=%f",
-							k4a_image_get_height_pixels(depth),
-							k4a_image_get_width_pixels(depth),
-							k4a_image_get_stride_bytes(depth),
-							k4a_image_get_format(depth),
-							double(k4a_image_get_timestamp_usec(depth)) / 1000000.0);*/
-
-						UASSERT(k4a_image_get_format(depth) == K4A_IMAGE_FORMAT_DEPTH16);
-
-						stamp = ((double)k4a_image_get_timestamp_usec(depth)) / 1000000;
-
-						if (ir_)
-						{
-							depthCV = cv::Mat(k4a_image_get_height_pixels(depth), k4a_image_get_width_pixels(depth), CV_16UC1, (void*)k4a_image_get_buffer(depth)).clone();
-						}
-						else
-						{
-							k4a_image_t transformedDepth;
-							if (k4a_image_create(k4a_image_get_format(depth), bgrCV.cols, bgrCV.rows, bgrCV.cols * 2, &transformedDepth) == K4A_RESULT_SUCCEEDED)
-							{
-								if (k4a_transformation_depth_image_to_color_camera((k4a_transformation_t)transformationHandle_, depth, transformedDepth) == K4A_RESULT_SUCCEEDED)
-								{
-									depthCV = cv::Mat(k4a_image_get_height_pixels(transformedDepth), k4a_image_get_width_pixels(transformedDepth), CV_16UC1, (void*)k4a_image_get_buffer(transformedDepth)).clone();
-								}
-								else
-								{
-									UERROR("Failed registration!");
-								}
-								k4a_image_release(transformedDepth);
-							}
-							else
-							{
-								UERROR("Failed allocating depth registered! (%d %d %d)", bgrCV.cols, bgrCV.rows, bgrCV.cols * 2);
-							}
-							
-						}
-
-						// Release the image
-						k4a_image_release(depth);
-					}
-				}
-
-				k4a_capture_release(capture);
-
-				IMU imu;
-				// FIXME: local imu transform missing
-				/*k4a_imu_sample_t imuSample;
-				if (k4a_playback_get_next_imu_sample((k4a_playback_t)playbackHandle_, &imuSample) == K4A_STREAM_RESULT_SUCCEEDED)
-				{
-					// K4A IMU Co-ordinates
-					//  x+ = "backwards"
-					//  y+ = "left"
-					//  z+ = "down"
-					//
-					// ROS Standard co-ordinates:
-					//  x+ = "forward"
-					//  y+ = "left"
-					//  z+ = "up"
-					//
-					// Remap K4A IMU to ROS co-ordinate system:
-					// ROS_X+ = K4A_X-
-					// ROS_Y+ = K4A_Y+
-					// ROS_Z+ = K4A_Z-
-
-					imu = IMU(
-						cv::Vec3d(-1*imuSample.gyro_sample.xyz.x, imuSample.gyro_sample.xyz.y, -1 * imuSample.gyro_sample.xyz.z),
-						cv::Mat::eye(3, 3, CV_64FC1),
-						cv::Vec3d(-1 * imuSample.acc_sample.xyz.x, imuSample.acc_sample.xyz.y, -1 * imuSample.acc_sample.xyz.z),
-						cv::Mat::eye(3, 3, CV_64FC1),
-						Transform::getIdentity());
-				}*/
-
-				if (!bgrCV.empty() && !depthCV.empty())
-				{
-					data = SensorData(bgrCV, depthCV, model_, this->getNextSeqID(), stamp);
-					data.setIMU(imu);
-
-					// Frame rate
-					if (this->getImageRate() < 0.0f)
-					{
-						if (stamp == 0)
-						{
-							UWARN("The option to use mkv stamps is set (framerate<0), but there are no stamps saved in the file! Aborting...");
-						}
-						else if (previousStamp_ > 0)
-						{
-							float ratio = -this->getImageRate();
-							int sleepTime = 1000.0*(stamp - previousStamp_) / ratio - 1000.0*timer_.getElapsedTime();
-							if (sleepTime > 10000)
-							{
-								UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
-										sleepTime / 1000, previousStamp_, stamp);
-								sleepTime = 10000;
-							}
-							if (sleepTime > 2)
-							{
-								uSleep(sleepTime - 2);
-							}
-
-							// Add precision at the cost of a small overhead
-							while (timer_.getElapsedTime() < (stamp - previousStamp_) / ratio - 0.000001)
-							{
-								//
-							}
-
-							double slept = timer_.getElapsedTime();
-							timer_.start();
-							UDEBUG("slept=%fs vs target=%fs (ratio=%f)", slept, (stamp - previousStamp_) / ratio, ratio);
-						}
-						previousStamp_ = stamp;
-					}
-
-					break;
-				}
-			}
+			k4a_capture_release(captureHandle_);
+			// the first frame may be null, just retry for 1 second
 		}
-
 		if (result == K4A_STREAM_RESULT_EOF)
 		{
 			// End of file reached
@@ -595,112 +436,138 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
 		{
 			UERROR("Failed to read entire recording");
 		}
+		captured = result == K4A_STREAM_RESULT_SUCCEEDED;
 	}
-	else
+	else // device
 	{
-		k4a_image_t ir_image_ = NULL;
-		k4a_image_t rgb_image_ = NULL;
-		k4a_imu_sample_t imu_sample_;
-
-		double t = UTimer::now();
 		k4a_wait_result_t result = K4A_WAIT_RESULT_FAILED;
 		while((UTimer::now()-t < 5.0) &&
-				(K4A_WAIT_RESULT_SUCCEEDED != (result=k4a_device_get_capture(device_, &capture_, K4A_WAIT_INFINITE)) ||
-				((ir_ && (ir_image_=k4a_capture_get_ir_image(capture_)) == NULL) || (!ir_ && (rgb_image_=k4a_capture_get_color_image(capture_)) == NULL))))
+				(K4A_WAIT_RESULT_SUCCEEDED != (result=k4a_device_get_capture(deviceHandle_, &captureHandle_, K4A_WAIT_INFINITE)) ||
+				((ir_ && (ir_image_=k4a_capture_get_ir_image(captureHandle_)) == NULL) || (!ir_ && (rgb_image_=k4a_capture_get_color_image(captureHandle_)) == NULL))))
 		{
-			k4a_capture_release(capture_);
+			k4a_capture_release(captureHandle_);
 			// the first frame may be null, just retry for 5 seconds
 		}
+		captured = result == K4A_WAIT_RESULT_SUCCEEDED;
+	}

-		if (result == K4A_WAIT_RESULT_SUCCEEDED && (rgb_image_!=NULL || ir_image_!=NULL))
+	if (captured && (rgb_image_!=NULL || ir_image_!=NULL))
+	{
+		cv::Mat bgrCV;
+		cv::Mat depthCV;
+		IMU imu;
+
+		if (ir_image_ != NULL)
 		{
-			cv::Mat bgrCV;
-			cv::Mat depthCV;
-			IMU imu;
+			// Convert IR image
+			cv::Mat bgrCV16(k4a_image_get_height_pixels(ir_image_),
+					k4a_image_get_width_pixels(ir_image_),
+					CV_16UC1,
+					(void*)k4a_image_get_buffer(ir_image_));

-			if (ir_image_ != NULL)
+			bgrCV16.convertTo(bgrCV, CV_8U);
+
+			bgrCV = model_.rectifyImage(bgrCV);
+
+			// Release the image
+			k4a_image_release(ir_image_);
+		}
+		else
+		{
+			// Convert RGB image
+			if (k4a_image_get_format(rgb_image_) == K4A_IMAGE_FORMAT_COLOR_MJPG)
 			{
-				// Convert IR image
-				cv::Mat bgrCV16(k4a_image_get_height_pixels(ir_image_),
-						k4a_image_get_width_pixels(ir_image_),
-						CV_16UC1,
-						(void*)k4a_image_get_buffer(ir_image_));
-
-				bgrCV16.convertTo(bgrCV, CV_8U);
-
-				// Release the image
-				k4a_image_release(ir_image_);
+				bgrCV = uncompressImage(cv::Mat(1, (int)k4a_image_get_size(rgb_image_),
+						CV_8UC1,
+						(void*)k4a_image_get_buffer(rgb_image_)));
 			}
 			else
 			{
-				// Convert RGB image
-				if (k4a_image_get_format(rgb_image_) == K4A_IMAGE_FORMAT_COLOR_MJPG)
+				cv::Mat bgra(k4a_image_get_height_pixels(rgb_image_),
+						k4a_image_get_width_pixels(rgb_image_),
+						CV_8UC4,
+						(void*)k4a_image_get_buffer(rgb_image_));
+
+				cv::cvtColor(bgra, bgrCV, CV_BGRA2BGR);
+			}
+
+			// Release the image
+			k4a_image_release(rgb_image_);
+		}
+
+		double stamp = UTimer::now();
+		if(!bgrCV.empty())
+		{
+			// Retrieve depth image from capture
+			k4a_image_t depth_image_ = k4a_capture_get_depth_image(captureHandle_);
+
+			if (depth_image_ != NULL)
+			{
+				stamp = ((double)k4a_image_get_timestamp_usec(depth_image_)) / 1000000;
+
+				if (ir_)
 				{
-					bgrCV = uncompressImage(cv::Mat(1, (int)k4a_image_get_size(rgb_image_),
-							CV_8UC1,
-							(void*)k4a_image_get_buffer(rgb_image_)));
+					depthCV = cv::Mat(k4a_image_get_height_pixels(depth_image_),
+							k4a_image_get_width_pixels(depth_image_),
+							CV_16UC1,
+							(void*)k4a_image_get_buffer(depth_image_));
+
+					depthCV = model_.rectifyDepth(depthCV);
 				}
 				else
 				{
-					cv::Mat bgra(k4a_image_get_height_pixels(rgb_image_),
-							k4a_image_get_width_pixels(rgb_image_),
-							CV_8UC4,
-							(void*)k4a_image_get_buffer(rgb_image_));
-
-					cv::cvtColor(bgra, bgrCV, CV_BGRA2BGR);
-				}
-
-				// Release the image
-				k4a_image_release(rgb_image_);
-			}
-
-			if(!bgrCV.empty())
-			{
-				// Retrieve depth image from capture
-				k4a_image_t depth_image_ = k4a_capture_get_depth_image(capture_);
-
-				if (depth_image_ != NULL)
-				{
-					if (ir_)
+					k4a_image_t transformedDepth = NULL;
+					if (k4a_image_create(k4a_image_get_format(depth_image_),
+							bgrCV.cols, bgrCV.rows, bgrCV.cols * 2, &transformedDepth) == K4A_RESULT_SUCCEEDED)
 					{
-						depthCV = cv::Mat(k4a_image_get_height_pixels(depth_image_),
-								k4a_image_get_width_pixels(depth_image_),
-								CV_16UC1,
-								(void*)k4a_image_get_buffer(depth_image_)).clone();
-					}
-					else
-					{
-						k4a_image_t transformedDepth = NULL;
-						if (k4a_image_create(k4a_image_get_format(depth_image_),
-								bgrCV.cols, bgrCV.rows, bgrCV.cols * 2, &transformedDepth) == K4A_RESULT_SUCCEEDED)
+						if(k4a_transformation_depth_image_to_color_camera(transformationHandle_, depth_image_, transformedDepth) == K4A_RESULT_SUCCEEDED)
 						{
-							if(k4a_transformation_depth_image_to_color_camera(transformation_, depth_image_, transformedDepth) == K4A_RESULT_SUCCEEDED)
-							{
-								depthCV = cv::Mat(k4a_image_get_height_pixels(transformedDepth),
-										k4a_image_get_width_pixels(transformedDepth),
-										CV_16UC1,
-										(void*)k4a_image_get_buffer(transformedDepth)).clone();
-							}
-							else
-							{
-								UERROR("K4A failed to register depth image");
-							}
-
-							k4a_image_release(transformedDepth);
+							depthCV = cv::Mat(k4a_image_get_height_pixels(transformedDepth),
+									k4a_image_get_width_pixels(transformedDepth),
+									CV_16UC1,
+									(void*)k4a_image_get_buffer(transformedDepth)).clone();
 						}
 						else
 						{
-							UERROR("K4A failed to allocate registered depth image");
+							UERROR("K4A failed to register depth image");
 						}
+
+						k4a_image_release(transformedDepth);
+					}
+					else
+					{
+						UERROR("K4A failed to allocate registered depth image");
 					}
-					k4a_image_release(depth_image_);
 				}
+				k4a_image_release(depth_image_);
 			}
+		}

-			k4a_capture_release(capture_);
+		k4a_capture_release(captureHandle_);

+		if(playbackHandle_)
+		{
 			// Get IMU sample, clear buffer
-			if(K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_imu_sample(device_, &imu_sample_, 60))
+			// FIXME: not tested, uncomment when tested.
+			k4a_playback_seek_timestamp(playbackHandle_, stamp* 1000000+1, K4A_PLAYBACK_SEEK_BEGIN);
+			if(K4A_STREAM_RESULT_SUCCEEDED == k4a_playback_get_previous_imu_sample(playbackHandle_, &imu_sample_))
+			{
+				double stmp = ((double)imu_sample_.acc_timestamp_usec) / 1000000;
+				imu = IMU(cv::Vec3d(imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, imu_sample_.gyro_sample.xyz.z),
+						cv::Mat::eye(3, 3, CV_64FC1),
+						cv::Vec3d(imu_sample_.acc_sample.xyz.x, imu_sample_.acc_sample.xyz.y, imu_sample_.acc_sample.xyz.z),
+						cv::Mat::eye(3, 3, CV_64FC1),
+						imuLocalTransform_);
+			}
+			else
+			{
+				UWARN("IMU data NULL");
+			}
+		}
+		else
+		{
+			// Get IMU sample, clear buffer
+			if(K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_imu_sample(deviceHandle_, &imu_sample_, 60))
 			{
 				imu = IMU(cv::Vec3d(imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, imu_sample_.gyro_sample.xyz.z),
 						cv::Mat::eye(3, 3, CV_64FC1),
@@ -712,13 +579,51 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
 			{
 				UERROR("IMU data NULL");
 			}
+		}

-			// Relay the data to rtabmap
-			if (!bgrCV.empty() && !depthCV.empty())
+		// Relay the data to rtabmap
+		if (!bgrCV.empty() && !depthCV.empty())
+		{
+			data = SensorData(bgrCV, depthCV, model_, this->getNextSeqID(), stamp);
+			if(!imu.empty())
 			{
-				data = SensorData(bgrCV, depthCV, model_, this->getNextSeqID(), UTimer::now());
 				data.setIMU(imu);
 			}
+
+			// Frame rate
+			if (playbackHandle_ && this->getImageRate() < 0.0f)
+			{
+				if (stamp == 0)
+				{
+					UWARN("The option to use mkv stamps is set (framerate<0), but there are no stamps saved in the file! Aborting...");
+				}
+				else if (previousStamp_ > 0)
+				{
+					float ratio = -this->getImageRate();
+					int sleepTime = 1000.0*(stamp - previousStamp_) / ratio - 1000.0*timer_.getElapsedTime();
+					if (sleepTime > 10000)
+					{
+						UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
+								sleepTime / 1000, previousStamp_, stamp);
+						sleepTime = 10000;
+					}
+					if (sleepTime > 2)
+					{
+						uSleep(sleepTime - 2);
+					}
+
+					// Add precision at the cost of a small overhead
+					while (timer_.getElapsedTime() < (stamp - previousStamp_) / ratio - 0.000001)
+					{
+						//
+					}
+
+					double slept = timer_.getElapsedTime();
+					timer_.start();
+					UDEBUG("slept=%fs vs target=%fs (ratio=%f)", slept, (stamp - previousStamp_) / ratio, ratio);
+				}
+				previousStamp_ = stamp;
+			}
 		}
 	}
 #else
@@ -70,16 +70,6 @@ bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::st
 	return success;
 }

-bool CameraRGBDImages::isCalibrated() const
-{
-	return this->cameraModel().isValidForProjection();
-}
-
-std::string CameraRGBDImages::getSerial() const
-{
-	return this->cameraModel().name();
-}
-
 SensorData CameraRGBDImages::captureImage(CameraInfo * info)
 {
 	SensorData data;
@@ -68,6 +68,7 @@ CameraRealSense2::CameraRealSense2(
 	depthToRGBExtrinsics_(new rs2_extrinsics),
 	lastImuStamp_(0.0),
 	clockSyncWarningShown_(false),
+	imuGlobalSyncWarningShown_(false),
 	emitterEnabled_(true),
 	ir_(false),
 	irDepth_(true),
@@ -76,9 +77,11 @@ CameraRealSense2::CameraRealSense2(
 	cameraWidth_(640),
 	cameraHeight_(480),
 	cameraFps_(30),
+	globalTimeSync_(true),
 	publishInterIMU_(false),
 	dualMode_(false),
-	closing_(false)
+	closing_(false),
+	isL500_(false)
 #endif
 {
 	UDEBUG("");
@@ -229,7 +232,7 @@ void CameraRealSense2::getPoseAndIMU(
 		Transform & pose,
 		unsigned int & poseConfidence,
 		IMU & imu,
-		int maxWaitTimeMs) const
+		int maxWaitTimeMs)
 {
 	pose.setNull();
 	imu = IMU();
@@ -296,15 +299,18 @@ void CameraRealSense2::getPoseAndIMU(
 	cv::Vec3d acc;
 	{
 		imuMutex_.lock();
-		int waitTry = 0;
-		while(maxWaitTimeMs > 0 && accBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
+		if(globalTimeSync_)
 		{
-			imuMutex_.unlock();
-			++waitTry;
-			uSleep(1);
-			imuMutex_.lock();
+			int waitTry = 0;
+			while(maxWaitTimeMs > 0 && accBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
+			{
+				imuMutex_.unlock();
+				++waitTry;
+				uSleep(1);
+				imuMutex_.lock();
+			}
 		}
-		if(accBuffer_.rbegin()->first < stamp)
+		if(globalTimeSync_ && accBuffer_.rbegin()->first < stamp)
 		{
 			if(maxWaitTimeMs>0)
 			{
@@ -340,16 +346,34 @@ void CameraRealSense2::getPoseAndIMU(
 			}
 			else
 			{
-				if(stamp < iterA->first)
+				if(!imuGlobalSyncWarningShown_)
 				{
-					UWARN("Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
+					if(stamp < iterA->first)
+					{
+						UWARN("Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
+					}
+					else
+					{
+						UWARN("Could not find acc data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
+					}
+				}
+				if(!globalTimeSync_)
+				{
+					if(!imuGlobalSyncWarningShown_)
+					{
+						UWARN("As globalTimeSync option is off, the received gyro and accelerometer will be re-stamped with image time. This message is only shown once.");
+						imuGlobalSyncWarningShown_ = true;
+					}
+					std::map<double, cv::Vec3f>::const_reverse_iterator iterC = accBuffer_.rbegin();
+					acc[0] = iterC->second[0];
+					acc[1] = iterC->second[1];
+					acc[2] = iterC->second[2];
 				}
 				else
 				{
-					UWARN("Could not find acc data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
+					imuMutex_.unlock();
+					return;
 				}
-				imuMutex_.unlock();
-				return;
 			}
 		}
 		imuMutex_.unlock();
@@ -359,15 +383,18 @@ void CameraRealSense2::getPoseAndIMU(
 	cv::Vec3d gyro;
 	{
 		imuMutex_.lock();
-		int waitTry = 0;
-		while(maxWaitTimeMs>0 && gyroBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
+		if(globalTimeSync_)
 		{
-			imuMutex_.unlock();
-			++waitTry;
-			uSleep(1);
-			imuMutex_.lock();
+			int waitTry = 0;
+			while(maxWaitTimeMs>0 && gyroBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
+			{
+				imuMutex_.unlock();
+				++waitTry;
+				uSleep(1);
+				imuMutex_.lock();
+			}
 		}
-		if(gyroBuffer_.rbegin()->first < stamp)
+		if(globalTimeSync_ && gyroBuffer_.rbegin()->first < stamp)
 		{
 			if(maxWaitTimeMs>0)
 			{
@@ -403,16 +430,34 @@ void CameraRealSense2::getPoseAndIMU(
 			}
 			else
 			{
-				if(stamp < iterA->first)
+				if(!imuGlobalSyncWarningShown_)
 				{
-					UWARN("Could not find gyro data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
+					if(stamp < iterA->first)
+					{
+						UWARN("Could not find gyro data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
+					}
+					else
+					{
+						UWARN("Could not find gyro data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
+					}
+				}
+				if(!globalTimeSync_)
+				{
+					if(!imuGlobalSyncWarningShown_)
+					{
+						UWARN("As globalTimeSync option is off, the latest received gyro and accelerometer will be re-stamped with image time. This message is only shown once.");
+						imuGlobalSyncWarningShown_ = true;
+					}
+					std::map<double, cv::Vec3f>::const_reverse_iterator iterC = gyroBuffer_.rbegin();
+					gyro[0] = iterC->second[0];
+					gyro[1] = iterC->second[1];
+					gyro[2] = iterC->second[2];
 				}
 				else
 				{
-					UWARN("Could not find gyro data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
+					imuMutex_.unlock();
+					return;
 				}
-				imuMutex_.unlock();
-				return;
 			}
 		}
 		imuMutex_.unlock();
@@ -435,6 +480,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		dev_[i] = 0;
 	}
 	clockSyncWarningShown_ = false;
+	imuGlobalSyncWarningShown_ = false;

 	auto list = ctx_->query_devices();
 	if (0 == list.size())
@@ -560,6 +606,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 	UINFO("Device Sensors: ");
 	std::vector<rs2::sensor> sensors(2); //0=rgb 1=depth 2=(pose in dualMode_)
 	bool stereo = false;
+	isL500_ = false;
 	for(auto&& elem : dev_sensors)
 	{
 		std::string module_name = elem.get_info(RS2_CAMERA_INFO_NAME);
@@ -604,6 +651,11 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			sensors.back().set_option(rs2_option::RS2_OPTION_ENABLE_POSE_JUMPING, 0);
 			sensors.back().set_option(rs2_option::RS2_OPTION_ENABLE_RELOCALIZATION, 0);
 		}
+		else if ("L500 Depth Sensor" == module_name)
+		{
+			sensors[1] = elem;
+			isL500_ = true;
+		}
 		else
 		{
 			UERROR("Module Name \"%s\" isn't supported!", module_name.c_str());
@@ -649,10 +701,38 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			auto video_profile = profile.as<rs2::video_stream_profile>();
 			if(!stereo)
 			{
+				if(isL500_ &&
+						(video_profile.width()  == 640 &&
+						 video_profile.height() == 480 &&
+						 video_profile.fps()    == 30))
+				{
+					if( i==0 // rgb
+						&& video_profile.format() == RS2_FORMAT_RGB8 && video_profile.stream_type() == RS2_STREAM_COLOR)
+					{
+						auto intrinsic = video_profile.get_intrinsics();
+						profilesPerSensor[i].push_back(profile);
+						rgbBuffer_ = cv::Mat(cv::Size(video_profile.width(), video_profile.height()), CV_8UC3, cv::Scalar(0, 0, 0));
+						model_ = CameraModel(camera_name, intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy, this->getLocalTransform(), 0, cv::Size(intrinsic.width, intrinsic.height));
+						rgbStreamProfile = profile;
+						*rgbIntrinsics_ = intrinsic;
+						added = true;
+					}
+					else if( i==1 // depth
+						 && video_profile.format() == RS2_FORMAT_Z16 && video_profile.stream_type() == RS2_STREAM_DEPTH)
+					{
+						auto intrinsic = video_profile.get_intrinsics();
+						profilesPerSensor[i].push_back(profile);
+						depthBuffer_ = cv::Mat(cv::Size(video_profile.width(), video_profile.height()), CV_16UC1, cv::Scalar(0));
+						depthStreamProfile = profile;
+						*depthIntrinsics_ = intrinsic;
+						added = true;
+					}
+				}
 				//D400 series:
-				if (video_profile.width()  == cameraWidth_ &&
-					video_profile.height() == cameraHeight_ &&
-					video_profile.fps()    == cameraFps_)
+				else if (!isL500_ &&
+						(video_profile.width()  == cameraWidth_ &&
+						 video_profile.height() == cameraHeight_ &&
+						 video_profile.fps()    == cameraFps_))
 				{
 					auto intrinsic = video_profile.get_intrinsics();

@@ -964,26 +1044,33 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			 {
 				 auto video_profile = profilesPerSensor[i][j].as<rs2::video_stream_profile>();
 				 UINFO("Opening: %s %d %d %d %d %s type=%d", rs2_format_to_string(
-					 video_profile.format()),
-					 video_profile.width(),
-					 video_profile.height(),
-					 video_profile.fps(),
-					 video_profile.stream_index(),
-					 video_profile.stream_name().c_str(),
-					 video_profile.stream_type());
+						 video_profile.format()),
+						 video_profile.width(),
+						 video_profile.height(),
+						 video_profile.fps(),
+						 video_profile.stream_index(),
+						 video_profile.stream_name().c_str(),
+						 video_profile.stream_type());
+			 }
+			 if(globalTimeSync_ && sensors[i].supports(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED))
+			 {
+				 float value = sensors[i].get_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED);
+				 UINFO("Set RS2_OPTION_GLOBAL_TIME_ENABLED=1 (was %f) for sensor %d", value, (int)i);
+				 sensors[i].set_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED, 1);
 			 }
 			 sensors[i].open(profilesPerSensor[i]);
 			 if(sensors[i].is<rs2::depth_sensor>())
 			 {
 				 auto depth_sensor = sensors[i].as<rs2::depth_sensor>();
 				 depth_scale_meters_ = depth_sensor.get_depth_scale();
+				 UINFO("Depth scale %f for sensor %d", depth_scale_meters_, (int)i);
 			 }
 			 sensors[i].start(multiple_message_callback_function);
 		 }
 	 }

-	uSleep(1000); // ignore the first frames
-	UINFO("Enabling streams...done!");
+	 uSleep(1000); // ignore the first frames
+	 UINFO("Enabling streams...done!");

 	return true;

@@ -1046,6 +1133,13 @@ void CameraRealSense2::setResolution(int width, int height, int fps)
 #endif
 }

+void CameraRealSense2::setGlobalTimeSync(bool enabled)
+{
+#ifdef RTABMAP_REALSENSE2
+	globalTimeSync_ = enabled;
+#endif
+}
+
 void CameraRealSense2::publishInterIMU(bool enabled)
 {
 #ifdef RTABMAP_REALSENSE2
@@ -1100,12 +1194,15 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 	try{
 		auto frameset = syncer_->wait_for_frames(5000);
 		UTimer timer;
-		while (frameset.size() != 2 && timer.elapsed() < 2.0)
+		int desiredFramesetSize = 2;
+		if(isL500_ && globalTimeSync_)
+			desiredFramesetSize = 3;
+		while ((int)frameset.size() != desiredFramesetSize && timer.elapsed() < 2.0)
 		{
 			// maybe there is a latency with the USB, try again in 100 ms (for the next 2 seconds)
 			frameset = syncer_->wait_for_frames(100);
 		}
-		if (frameset.size() == 2)
+		if ((int)frameset.size() == desiredFramesetSize)
 		{
 			double now = UTimer::now();
 			bool is_rgb_arrived = false;
@@ -1125,7 +1222,15 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 				auto stream_type = f.get_profile().stream_type();
 				if (stream_type == RS2_STREAM_COLOR || stream_type == RS2_STREAM_INFRARED)
 				{
-					if(ir_ && !irDepth_)
+					if(isL500_)
+					{
+						if(stream_type == RS2_STREAM_COLOR)
+						{
+							rgb_frame = f;
+							is_rgb_arrived = true;
+						}
+					} 
+					else if(ir_ && !irDepth_)
 					{
 						//stereo D435
 						if(!is_depth_arrived)
@@ -1183,7 +1288,6 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)

 			if(is_rgb_arrived && is_depth_arrived)
 			{
-				auto from_image_frame = depth_frame.as<rs2::video_frame>();
 				cv::Mat depth;
 				if(ir_)
 				{
@@ -1195,6 +1299,19 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 					rs2::frameset processed = frameset.apply_filter(align);
 					rs2::depth_frame aligned_depth_frame = processed.get_depth_frame();
 					depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)aligned_depth_frame.get_data()).clone();
+					if(depth_scale_meters_ != 0.001f)
+					{ // convert to mm
+						if(depth.type() ==  CV_16UC1)
+						{
+							float scale = depth_scale_meters_ / 0.001f;
+							uint16_t *p = depth.ptr<uint16_t>();
+							int buffSize = depth.rows * depth.cols;
+							#pragma omp parallel for
+							for(int i = 0; i < buffSize; ++i) {
+								p[i] *= scale;
+							}
+						}
+					}
 				}

 				cv::Mat rgb = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data());
@@ -1305,9 +1422,13 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 				lastImuStamp_ = imuStamp;
 			}
 		}
+		else if(isL500_ && globalTimeSync_)
+		{
+			UERROR("Missing frames (received %d, needed=%d). L500 camera is used and global time sync is enabled, try disabling global time sync for the RealSense2 driver.", (int)frameset.size(), desiredFramesetSize);
+		}
 		else
 		{
-			UERROR("Missing frames (received %d)", (int)frameset.size());
+			UERROR("Missing frames (received %d, needed=%d)", (int)frameset.size(), desiredFramesetSize);
 		}
 	}
 	catch(const std::exception& ex)
@@ -310,9 +310,11 @@ void MadgwickFilter::updateImpl(
 	  A[0] = ax;
 	  A[1] = ay;
 	  A[2] = az;
-	computeOrientation(A,orientation);
-	reset(orientation.x(), orientation.y(), orientation.z(), orientation.w());
-	initialized_ = true;
+	if(computeOrientation(A,orientation))
+	{
+		reset(orientation.x(), orientation.y(), orientation.z(), orientation.w());
+		initialized_ = true;
+	}
 	return;
  }

@@ -137,9 +137,7 @@ Transform OdometryF2F::computeTransform(
 		{
 			tmpRefFrame = refFrame_;
 			// reset matches, but keep already extracted features in newFrame.sensorData()
-			newFrame.setWords(std::multimap<int, cv::KeyPoint>());
-			newFrame.setWords3(std::multimap<int, cv::Point3f>());
-			newFrame.setWordsDescriptors(std::multimap<int, cv::Mat>());
+			newFrame.removeAllWords();
 			UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
 			// If optical flow is used, switch temporary to feature matching
 			int visCorTypeBackup = Parameters::defaultVisCorType();
@@ -176,18 +174,18 @@ Transform OdometryF2F::computeTransform(

 		if(info && this->isInfoDataFilled())
 		{
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
+			std::list<std::pair<int, std::pair<int, int> > > pairs;
 			EpipolarGeometry::findPairsUnique(tmpRefFrame.getWords(), newFrame.getWords(), pairs);
 			info->refCorners.resize(pairs.size());
 			info->newCorners.resize(pairs.size());
 			std::map<int, int> idToIndex;
 			int i=0;
-			for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin();
+			for(std::list<std::pair<int, std::pair<int, int> > >::iterator iter=pairs.begin();
 				iter!=pairs.end();
 				++iter)
 			{
-				info->refCorners[i] = iter->second.first.pt;
-				info->newCorners[i] = iter->second.second.pt;
+				info->refCorners[i] = tmpRefFrame.getWordsKpts()[iter->second.first].pt;
+				info->newCorners[i] = newFrame.getWordsKpts()[iter->second.second].pt;
 				idToIndex.insert(std::make_pair(iter->first, i));
 				++i;
 			}
@@ -199,12 +197,21 @@ Transform OdometryF2F::computeTransform(
 			}

 			Transform t = this->getPose()*motionSinceLastKeyFrame.inverse();
-			for(std::multimap<int, cv::Point3f>::const_iterator iter=tmpRefFrame.getWords3().begin(); iter!=tmpRefFrame.getWords3().end(); ++iter)
+			if(!tmpRefFrame.getWords3().empty())
 			{
-				info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t)));
+				for(std::multimap<int, int>::const_iterator iter=tmpRefFrame.getWords().begin(); iter!=tmpRefFrame.getWords().end(); ++iter)
+				{
+					info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(tmpRefFrame.getWords3()[iter->second], t)));
+				}
 			}
 			info->localMapSize = tmpRefFrame.getWords3().size();
-			info->words = newFrame.getWords();
+			if(!newFrame.getWordsKpts().empty())
+			{
+				for(std::multimap<int, int>::const_iterator iter=newFrame.getWords().begin(); iter!=newFrame.getWords().end(); ++iter)
+				{
+					info->words.insert(std::make_pair(iter->first, newFrame.getWordsKpts()[iter->second]));
+				}
+			}

 			info->localScanMapSize = tmpRefFrame.sensorData().laserScanRaw().size();

@@ -232,7 +239,7 @@ Transform OdometryF2F::computeTransform(
 			(registrationPipeline_->isScanRequired() && (scanKeyFrameThr_ == 0.0f || regInfo.icpInliersRatio <= scanKeyFrameThr_)))
 		{
 			UDEBUG("Update key frame");
-			int features = newFrame.getWordsDescriptors().size();
+			int features = newFrame.getWordsDescriptors().rows;
 			if(registrationPipeline_->isImageRequired() && features == 0)
 			{
 				newFrame = Signature(data);
@@ -251,9 +258,7 @@ Transform OdometryF2F::computeTransform(
 			{
 				refFrame_ = newFrame;

-				refFrame_.setWords(std::multimap<int, cv::KeyPoint>());
-				refFrame_.setWords3(std::multimap<int, cv::Point3f>());
-				refFrame_.setWordsDescriptors(std::multimap<int, cv::Mat>());
+				refFrame_.removeAllWords();

 				//reset motion
 				lastKeyFramePose_.setNull();
@@ -133,6 +133,27 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	}
 	uInsert(bundleParameters, ParametersPair(Parameters::kVisCorType(), uNumber2Str(corType)));

+	int estType = Parameters::defaultVisEstimationType();
+	Parameters::parse(parameters, Parameters::kVisEstimationType(), estType);
+	if(estType > 1)
+	{
+		UWARN("%s=%d is not supported by OdometryF2M, using 2D->3D approach instead (type=1).",
+				Parameters::kVisEstimationType().c_str(),
+				estType);
+		estType = 1;
+	}
+	uInsert(bundleParameters, ParametersPair(Parameters::kVisEstimationType(), uNumber2Str(estType)));
+
+	bool forwardEst = Parameters::defaultVisForwardEstOnly();
+	Parameters::parse(parameters, Parameters::kVisForwardEstOnly(), forwardEst);
+	if(!forwardEst)
+	{
+		UWARN("%s=false is not supported by OdometryF2M, setting to true.",
+				Parameters::kVisForwardEstOnly().c_str());
+		forwardEst = true;
+	}
+	uInsert(bundleParameters, ParametersPair(Parameters::kVisForwardEstOnly(), uBool2Str(forwardEst)));
+
 	regPipeline_ = Registration::create(bundleParameters);
 	if(bundleAdjustment_>0 && regPipeline_->isScanRequired())
 	{
@@ -272,9 +293,7 @@ Transform OdometryF2M::computeTransform(
 			{
 				tmpMap = *map_;
 				// reset matches, but keep already extracted features in lastFrame_->sensorData()
-				lastFrame_->setWords(std::multimap<int, cv::KeyPoint>());
-				lastFrame_->setWords3(std::multimap<int, cv::Point3f>());
-				lastFrame_->setWordsDescriptors(std::multimap<int, cv::Mat>());
+				lastFrame_->removeAllWords();

 				points3DMap.clear();
 				bundlePoses.clear();
@@ -393,11 +412,9 @@ Transform OdometryF2M::computeTransform(
 								int wordId =regInfo.inliersIDs[i];

 								// 3D point
-								std::multimap<int, cv::Point3f>::const_iterator iter3D = tmpMap.getWords3().find(wordId);
-								UASSERT(iter3D!=tmpMap.getWords3().end());
-								points3DMap.insert(*iter3D);
-
-								std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
+								std::multimap<int, int>::const_iterator iter3D = tmpMap.getWords().find(wordId);
+								UASSERT(iter3D!=tmpMap.getWords().end() && !tmpMap.getWords3().empty());
+								points3DMap.insert(std::make_pair(wordId, tmpMap.getWords3()[iter3D->second]));

 								// all other references
 								std::map<int, std::map<int, FeatureBA> >::iterator refIter = bundleWordReferences_.find(wordId);
@@ -427,12 +444,19 @@ Transform OdometryF2M::computeTransform(
 									}
 								}

+								std::multimap<int, int>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
 								if(iter2D!=lastFrame_->getWords().end())
 								{
-									UASSERT(lastFrame_->getWords3().find(wordId) != lastFrame_->getWords3().end());
-									//move back point in camera frame (to get depth along z)
-									cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(wordId)->second, invLocalTransform);
-									references.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, pt3d.z)));
+									UASSERT(!lastFrame_->getWordsKpts().empty());
+									//get depth
+									float d = 0.0f;
+									if( !lastFrame_->getWords3().empty() &&
+										util3d::isFinite(lastFrame_->getWords3()[iter2D->second]))
+									{
+										//move back point in camera frame (to get depth along z)
+										d = util3d::transformPoint(lastFrame_->getWords3()[iter2D->second], invLocalTransform).z;
+									}
+									references.insert(std::make_pair(lastFrame_->id(), FeatureBA(lastFrame_->getWordsKpts()[iter2D->second], d)));
 								}
 								wordReferences.insert(std::make_pair(wordId, references));

@@ -557,23 +581,19 @@ Transform OdometryF2M::computeTransform(

 				// fields to update
 				LaserScan mapScan = tmpMap.sensorData().laserScanRaw();
-				std::multimap<int, cv::KeyPoint> mapWords = tmpMap.getWords();
-				std::multimap<int, cv::Point3f> mapPoints = tmpMap.getWords3();
-				std::multimap<int, cv::Mat> mapDescriptors = tmpMap.getWordsDescriptors();
+				std::multimap<int, int> mapWords = tmpMap.getWords();
+				std::vector<cv::KeyPoint> mapWordsKpts = tmpMap.getWordsKpts();
+				std::vector<cv::Point3f> mapPoints = tmpMap.getWords3();
+				cv::Mat mapDescriptors = tmpMap.getWordsDescriptors();

 				bool addVisualKeyFrame = regPipeline_->isImageRequired() &&
 						 (keyFrameThr_ == 0.0f ||
 						  visKeyFrameThr_ == 0 ||
 						  float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->getWords().size())) ||
 						  regInfo.inliers <= visKeyFrameThr_);
-				float minComplexity = Parameters::defaultIcpPointToPlaneMinComplexity();
-				bool p2n = Parameters::defaultIcpPointToPlane();
-				Parameters::parse(parameters_, Parameters::kIcpPointToPlane(), p2n);
-				Parameters::parse(parameters_, Parameters::kIcpPointToPlaneMinComplexity(), minComplexity);
-				bool addGeometricKeyFrame =
-					regPipeline_->isScanRequired() &&
-					(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_) &&
-					(addVisualKeyFrame || !p2n || regInfo.icpStructuralComplexity>=minComplexity);
+
+				bool addGeometricKeyFrame = regPipeline_->isScanRequired() &&
+					(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_);

 				addKeyFrame = false;//bundleLinks.rbegin()->second.transform().getNorm() > 5.0f*0.075f;
 				addKeyFrame = addKeyFrame || addVisualKeyFrame || addGeometricKeyFrame;
@@ -590,8 +610,9 @@ Transform OdometryF2M::computeTransform(

 					// update local map
 					UASSERT(mapWords.size() == mapPoints.size());
-					UASSERT(mapPoints.size() == mapDescriptors.size());
-					UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
+					UASSERT(mapWords.size() == mapWordsKpts.size());
+					UASSERT((int)mapPoints.size() == mapDescriptors.rows);
+					UASSERT_MSG(lastFrame_->getWordsDescriptors().rows == (int)lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().rows, (int)lastFrame_->getWords3().size()).c_str());

 					std::map<int, int>::iterator iterBundlePosesRef = bundlePoseReferences_.end();
 					if(bundleAdjustment_>0)
@@ -613,17 +634,15 @@ Transform OdometryF2M::computeTransform(
 						// update local map 3D points (if bundle adjustment was done)
 						for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
 						{
-							UASSERT(mapPoints.count(iter->first) == 1);
-							//UDEBUG("Updated %d (%f,%f,%f) -> (%f,%f,%f)", iter->first, mapPoints.find(origin)->second.x, mapPoints.find(origin)->second.y, mapPoints.find(origin)->second.z, iter->second.x, iter->second.y, iter->second.z);
-							mapPoints.find(iter->first)->second = iter->second;
+							UASSERT(mapWords.count(iter->first) == 1);
+							//UDEBUG("Updated %d (%f,%f,%f) -> (%f,%f,%f)", iter->first, mapPoints[mapWords.find(iter->first)->second].x, mapPoints[mapWords.find(iter->first)->second].y, mapPoints[mapWords.find(iter->first)->second].z, iter->second.x, iter->second.y, iter->second.z);
+							mapPoints[mapWords.find(iter->first)->second] = iter->second;
 						}
 					}

 					// sort by feature response
 					std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > > newIds;
 					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
-					std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().begin();
-					std::multimap<int, cv::Mat>::const_iterator iterDesc = lastFrame_->getWordsDescriptors().begin();
 					UDEBUG("new frame words3=%d", (int)lastFrame_->getWords3().size());
 					std::set<int> seenStatusUpdated;
 					Transform invLocalTransform;
@@ -648,11 +667,11 @@ Transform OdometryF2M::computeTransform(
 					if(!visDepthAsMask && validDepthRatio_ < 1.0f)
 					{
 						int ptsWithDepth = 0;
-						for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
+						for (std::vector<cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
 							iter != lastFrame_->getWords3().end();
 							++iter)
 						{
-							if(util3d::isFinite(iter->second))
+							if(util3d::isFinite(*iter))
 							{
 								++ptsWithDepth;
 							}
@@ -666,27 +685,29 @@ Transform OdometryF2M::computeTransform(
 						}
 					}

-					for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D, ++iterDesc)
+					for(std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter)
 					{
-						if(mapPoints.find(iter->first) == mapPoints.end()) // Point not in map
+						const cv::Point3f & pt = lastFrame_->getWords3()[iter->second];
+						const cv::KeyPoint & kpt = lastFrame_->getWordsKpts()[iter->second];
+						if(mapWords.find(iter->first) == mapWords.end()) // Point not in map
 						{
-							if(util3d::isFinite(iter->second) || addPointsWithoutDepth)
+							if(util3d::isFinite(pt) || addPointsWithoutDepth)
 							{
 								newIds.insert(
-										std::make_pair(iter2D->second.response>0?1.0f/iter2D->second.response:0.0f,
+										std::make_pair(kpt.response>0?1.0f/kpt.response:0.0f,
 												std::make_pair(iter->first,
-														std::make_pair(iter2D->second,
-																std::make_pair(iter->second, iterDesc->second)))));
+														std::make_pair(kpt,
+																std::make_pair(pt, lastFrame_->getWordsDescriptors().row(iter->second))))));
 							}
 						}
 						else if(bundleAdjustment_>0)
 						{
 							if(lastFrame_->getWords().count(iter->first) == 1)
 							{
-								std::multimap<int, cv::KeyPoint>::iterator iterKpts = mapWords.find(iter->first);
-								if(iterKpts!=mapWords.end())
+								std::multimap<int, int>::iterator iterKpts = mapWords.find(iter->first);
+								if(iterKpts!=mapWords.end() && !mapWordsKpts.empty())
 								{
-									iterKpts->second.octave = iter2D->second.octave;
+									mapWordsKpts[iterKpts->second].octave = kpt.octave;
 								}

 								UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
@@ -694,19 +715,19 @@ Transform OdometryF2M::computeTransform(

 								//move back point in camera frame (to get depth along z)
 								float depth = 0.0f;
-								if(util3d::isFinite(iter->second))
+								if(util3d::isFinite(pt))
 								{
-									depth = util3d::transformPoint(iter->second, invLocalTransform).z;
+									depth = util3d::transformPoint(pt, invLocalTransform).z;
 								}
 								if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
 								{
 									std::map<int, FeatureBA> framePt;
-									framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, depth)));
+									framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth)));
 									bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
 								}
 								else
 								{
-									bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, depth)));
+									bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth)));
 								}
 							}
 						}
@@ -747,7 +768,8 @@ Transform OdometryF2M::computeTransform(
 								}
 							}

-							mapWords.insert(std::make_pair(iter->second.first, iter->second.second.first));
+							mapWords.insert(mapWords.end(), std::make_pair(iter->second.first, mapWords.size()));
+							mapWordsKpts.push_back(iter->second.second.first);
 							cv::Point3f pt = iter->second.second.second.first;
 							if(!util3d::isFinite(pt))
 							{
@@ -783,8 +805,8 @@ Transform OdometryF2M::computeTransform(
 								float scaleInf = (0.05 * model.fx()) / 0.01;
 								pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), model.localTransform()); // in base_link frame
 							}
-							mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(pt, newFramePose)));
-							mapDescriptors.insert(std::make_pair(iter->second.first, iter->second.second.second.second));
+							mapPoints.push_back(util3d::transformPoint(pt, newFramePose));
+							mapDescriptors.push_back(iter->second.second.second.second);
 							if(lastFrameOldestNewId_ > iter->second.first)
 							{
 								lastFrameOldestNewId_ = iter->second.first;
@@ -794,7 +816,7 @@ Transform OdometryF2M::computeTransform(
 					}

 					// remove words in map if max size is reached
-					if((int)mapPoints.size() > maximumMapSize_)
+					if((int)mapWords.size() > maximumMapSize_)
 					{
 						// remove oldest outliers first
 						std::set<int> inliers(regInfo.inliersIDs.begin(), regInfo.inliersIDs.end());
@@ -813,7 +835,7 @@ Transform OdometryF2M::computeTransform(
 							ids.resize(regInfo.matchesIDs.size()+oi);
 							UDEBUG("projected added=%d/%d minLastFrameId=%d", oi, (int)regInfo.projectedIDs.size(), lastFrameOldestNewId);
 						}
-						for(unsigned int i=0; i<ids.size() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size(); ++i)
+						for(unsigned int i=0; i<ids.size() && (int)mapWords.size() > maximumMapSize_ && mapWords.size() >= newIds.size(); ++i)
 						{
 							int id = ids.at(i);
 							if(inliers.find(id) == inliers.end())
@@ -831,18 +853,14 @@ Transform OdometryF2M::computeTransform(
 									bundleWordReferences_.erase(iterRef);
 								}

-								mapPoints.erase(id);
-								mapDescriptors.erase(id);
 								mapWords.erase(id);
 								++removed;
 							}
 						}

 						// remove oldest first
-						std::multimap<int, cv::Mat>::iterator iterMapDescriptors = mapDescriptors.begin();
-						std::multimap<int, cv::KeyPoint>::iterator iterMapWords = mapWords.begin();
-						for(std::multimap<int, cv::Point3f>::iterator iter = mapPoints.begin();
-							iter!=mapPoints.end() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size();)
+						for(std::multimap<int, int>::iterator iter = mapWords.begin();
+							iter!=mapWords.end() && (int)mapWords.size() > maximumMapSize_ && mapWords.size() >= newIds.size();)
 						{
 							if(inliers.find(iter->first) == inliers.end())
 							{
@@ -859,19 +877,36 @@ Transform OdometryF2M::computeTransform(
 									bundleWordReferences_.erase(iterRef);
 								}

-								mapPoints.erase(iter++);
-								mapDescriptors.erase(iterMapDescriptors++);
-								mapWords.erase(iterMapWords++);
+								mapWords.erase(iter++);
 								++removed;
 							}
 							else
 							{
 								++iter;
-								++iterMapDescriptors;
-								++iterMapWords;
 							}
 						}

+						if(mapWords.size() != mapPoints.size())
+						{
+							UDEBUG("Remove points");
+							std::vector<cv::KeyPoint> mapWordsKptsClean(mapWords.size());
+							std::vector<cv::Point3f> mapPointsClean(mapWords.size());
+							cv::Mat mapDescriptorsClean(mapWords.size(), mapDescriptors.cols, mapDescriptors.type());
+							int index = 0;
+							for(std::multimap<int, int>::iterator iter = mapWords.begin(); iter!=mapWords.end(); ++iter, ++index)
+							{
+								mapWordsKptsClean[index] = mapWordsKpts[iter->second];
+								mapPointsClean[index] = mapPoints[iter->second];
+								mapDescriptors.row(iter->second).copyTo(mapDescriptorsClean.row(index));
+								iter->second = index;
+							}
+							mapWordsKpts = mapWordsKptsClean;
+							mapWordsKptsClean.clear();
+							mapPoints = mapPointsClean;
+							mapPointsClean.clear();
+							mapDescriptors = mapDescriptorsClean;
+						}
+
 						Link * previousLink = 0;
 						for(std::map<int, int>::iterator iter=bundlePoseReferences_.begin(); iter!=bundlePoseReferences_.end();)
 						{
@@ -915,14 +950,13 @@ Transform OdometryF2M::computeTransform(

 					if(lastFrame_->sensorData().laserScanRaw().size())
 					{
-						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan, tmpMap.sensorData().laserScanRaw().localTransform());
+						pcl::PointCloud<pcl::PointXYZINormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudINormal(mapScan, tmpMap.sensorData().laserScanRaw().localTransform());
 						Transform viewpoint =  newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform();
-						pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals (new pcl::PointCloud<pcl::PointNormal>());
+						pcl::PointCloud<pcl::PointXYZINormal>::Ptr frameCloudNormals (new pcl::PointCloud<pcl::PointXYZINormal>());
 						
 						if(scanMapMaxRange_ > 0)
 						{
-							frameCloudNormals = util3d::laserScanToPointCloudNormal(
-									lastFrame_->sensorData().laserScanRaw());
+							frameCloudNormals = util3d::laserScanToPointCloudINormal(lastFrame_->sensorData().laserScanRaw());
 							frameCloudNormals = util3d::cropBox(frameCloudNormals,
 									Eigen::Vector4f(-scanMapMaxRange_ / 2, -scanMapMaxRange_ / 2,-scanMapMaxRange_ / 2, 0),
 									Eigen::Vector4f(scanMapMaxRange_ / 2,scanMapMaxRange_ / 2,scanMapMaxRange_ / 2, 0)
@@ -930,8 +964,7 @@ Transform OdometryF2M::computeTransform(
 							frameCloudNormals = util3d::transformPointCloud(frameCloudNormals, viewpoint);
 						} else
 						{
-							frameCloudNormals = util3d::laserScanToPointCloudNormal(
-									lastFrame_->sensorData().laserScanRaw(), viewpoint);
+							frameCloudNormals = util3d::laserScanToPointCloudINormal(lastFrame_->sensorData().laserScanRaw(), viewpoint);
 						}
 						
 						pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
@@ -958,7 +991,7 @@ Transform OdometryF2M::computeTransform(
 							if (scanMapMaxRange_ > 0) {
 								// Copying new points to tmp cloud
 								// These are the points that have no overlap between mapScan and lastFrame
-								pcl::PointCloud<pcl::PointNormal> tmp;
+								pcl::PointCloud<pcl::PointXYZINormal> tmp;
 								pcl::copyPointCloud(*frameCloudNormals, *frameCloudNormalsIndices, tmp);

 								if (int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_) // 20 000 points
@@ -1018,7 +1051,7 @@ Transform OdometryF2M::computeTransform(
 										{
 											if(scansBuffer_[i].second->size())
 											{
-												pcl::PointCloud<pcl::PointNormal> tmp;
+												pcl::PointCloud<pcl::PointXYZINormal> tmp;
 												pcl::copyPointCloud(*scansBuffer_[i].first, *scansBuffer_[i].second, tmp);
 												*mapCloudNormals += tmp;
 											}
@@ -1031,7 +1064,7 @@ Transform OdometryF2M::computeTransform(
 									// remove old clouds
 									if(i > 0)
 									{
-										std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
+										std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
 										int oi = 0;
 										for(; i<(int)scansBuffer_.size(); ++i)
 										{
@@ -1046,7 +1079,7 @@ Transform OdometryF2M::computeTransform(
 									// just append the last cloud
 									if(scansBuffer_.back().second->size())
 									{
-										pcl::PointCloud<pcl::PointNormal> tmp;
+										pcl::PointCloud<pcl::PointXYZINormal> tmp;
 										pcl::copyPointCloud(*scansBuffer_.back().first, *scansBuffer_.back().second, tmp);
 										*mapCloudNormals += tmp;
 									}
@@ -1060,12 +1093,12 @@ Transform OdometryF2M::computeTransform(
 							if(mapScan.is2d())
 							{
 								Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
-								mapScan = LaserScan(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYNormal);
+								mapScan = LaserScan(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYINormal);
 							}
 							else
 							{
 								Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
-								mapScan = LaserScan(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYZNormal);
+								mapScan = LaserScan(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYZINormal);
 							}
 							modified=true;
 						}
@@ -1099,9 +1132,7 @@ Transform OdometryF2M::computeTransform(
 										newFramePose.translation()));
 					}

-					map_->setWords(mapWords);
-					map_->setWords3(mapPoints);
-				 	map_->setWordsDescriptors(mapDescriptors);
+					map_->setWords(mapWords, mapWordsKpts, mapPoints, mapDescriptors);
 				}
 			}

@@ -1112,7 +1143,14 @@ Transform OdometryF2M::computeTransform(
 				info->localScanMapSize = tmpMap.sensorData().laserScanRaw().size();
 				if(this->isInfoDataFilled())
 				{
-					info->localMap = uMultimapToMap(tmpMap.getWords3());
+					info->localMap.clear();
+					if(!tmpMap.getWords3().empty())
+					{
+						for(std::multimap<int, int>::const_iterator iter=tmpMap.getWords().begin(); iter!=tmpMap.getWords().end(); ++iter)
+						{
+							info->localMap.insert(std::make_pair(iter->first, tmpMap.getWords3()[iter->second]));
+						}
+					}
 					info->localScanMap = tmpMap.sensorData().laserScanRaw();
 				}
 			}
@@ -1139,11 +1177,12 @@ Transform OdometryF2M::computeTransform(
 			if(regPipeline_->isImageRequired())
 			{
 				int ptsWithDepth = 0;
-				for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
-					iter != lastFrame_->getWords3().end();
+				for (std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin();
+					iter != lastFrame_->getWords().end();
 					++iter)
 				{
-					if(util3d::isFinite(iter->second))
+					if(!lastFrame_->getWords3().empty() && 
+					   util3d::isFinite(lastFrame_->getWords3()[iter->second]))
 					{
 						++ptsWithDepth;
 					}
@@ -1153,26 +1192,29 @@ Transform OdometryF2M::computeTransform(
 				{
 					frameValid = true;
 					// update local map
-					UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
+					UASSERT_MSG(lastFrame_->getWordsDescriptors().rows == (int)lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().rows, (int)lastFrame_->getWords3().size()).c_str());
 					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());

-					std::multimap<int, cv::KeyPoint> words;
-					std::multimap<int, cv::Point3f> transformedPoints;
+					std::multimap<int, int> words;
+					std::vector<cv::KeyPoint> wordsKpts;
+					std::vector<cv::Point3f> transformedPoints;
 					std::multimap<int, int> mapPointWeights;
-					std::multimap<int, cv::Mat> descriptors;
-					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
-					std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
-					std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
-					for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
-						iter != lastFrame_->getWords3().end();
-						++iter, ++descIter, ++wordsIter)
+					cv::Mat descriptors;
+					if(!lastFrame_->getWords3().empty())
 					{
-						if (util3d::isFinite(iter->second))
+						for (std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin();
+							iter != lastFrame_->getWords().end();
+							++iter)
 						{
-							words.insert(*wordsIter);
-							transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
-							mapPointWeights.insert(std::make_pair(iter->first, 0));
-							descriptors.insert(*descIter);
+							const cv::Point3f & pt = lastFrame_->getWords3()[iter->second];
+							if (util3d::isFinite(pt))
+							{
+								words.insert(words.end(), std::make_pair(iter->first, words.size()));
+								wordsKpts.push_back(lastFrame_->getWordsKpts()[iter->second]);
+								transformedPoints.push_back(util3d::transformPoint(pt, newFramePose));
+								mapPointWeights.insert(std::make_pair(iter->first, 0));
+								descriptors.push_back(lastFrame_->getWordsDescriptors().row(iter->second));
+							}
 						}
 					}

@@ -1193,25 +1235,29 @@ Transform OdometryF2M::computeTransform(
 						}

 						// update bundleWordReferences_: used for bundle adjustment
-						for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+						if(!wordsKpts.empty())
 						{
-							if(words.count(iter->first) == 1)
+							for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 							{
-								UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
-								std::map<int, FeatureBA> framePt;
-
-								//get depth
-								float d = 0.0f;
-								if(lastFrame_->getWords3().count(iter->first) == 1 &&
-									util3d::isFinite(lastFrame_->getWords3().find(iter->first)->second))
+								if(words.count(iter->first) == 1)
 								{
-									//move back point in camera frame (to get depth along z)
-									d = util3d::transformPoint(lastFrame_->getWords3().find(iter->first)->second, invLocalTransform).z;
+									UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
+									std::map<int, FeatureBA> framePt;
+
+									//get depth
+									float d = 0.0f;
+									if(lastFrame_->getWords().count(iter->first) == 1 &&
+									  !lastFrame_->getWords3().empty() &&
+										util3d::isFinite(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second]))
+									{
+										//move back point in camera frame (to get depth along z)
+										d = util3d::transformPoint(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second], invLocalTransform).z;
+									}
+
+
+									framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(wordsKpts[iter->second], d)));
+									bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
 								}
-
-
-								framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second, d)));
-								bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
 							}
 						}

@@ -1246,9 +1292,7 @@ Transform OdometryF2M::computeTransform(
 						}
 					}

-					map_->setWords(words);
-					map_->setWords3(transformedPoints);
-					map_->setWordsDescriptors(descriptors);
+					map_->setWords(words, wordsKpts, transformedPoints, descriptors);
 					addKeyFrame = true;
 				}
 				else
@@ -1260,7 +1304,7 @@ Transform OdometryF2M::computeTransform(
 			{
 				if (lastFrame_->sensorData().laserScanRaw().size())
 				{
-					pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform());
+					pcl::PointCloud<pcl::PointXYZINormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudINormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform());

 					double complexity = 0.0;;
 					if(!frameValid)
@@ -1271,9 +1315,17 @@ Transform OdometryF2M::computeTransform(
 						Parameters::parse(parameters_, Parameters::kIcpPointToPlaneMinComplexity(), minComplexity);
 						if(p2n && minComplexity>0.0f)
 						{
-							complexity = util3d::computeNormalsComplexity(*mapCloudNormals, Transform::getIdentity(), lastFrame_->sensorData().laserScanRaw().is2d());
-							if(complexity > minComplexity)
+							if(lastFrame_->sensorData().laserScanRaw().hasNormals())
 							{
+								complexity = util3d::computeNormalsComplexity(*mapCloudNormals, Transform::getIdentity(), lastFrame_->sensorData().laserScanRaw().is2d());
+								if(complexity > minComplexity)
+								{
+									frameValid = true;
+								}
+							}
+							else
+							{
+								UWARN("Input raw scan doesn't have normals, complexity check on first frame is not done.");
 								frameValid = true;
 							}
 						}
@@ -1298,7 +1350,7 @@ Transform OdometryF2M::computeTransform(
 											util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint),
 											0,
 											0.0f,
-											LaserScan::kXYNormal,
+											LaserScan::kXYINormal,
 											Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
 						}
 						else
@@ -1309,7 +1361,7 @@ Transform OdometryF2M::computeTransform(
 											util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
 											0,
 											0.0f,
-											LaserScan::kXYZNormal,
+											LaserScan::kXYZINormal,
 											newFramePose.translation()));
 						}

@@ -1339,7 +1391,14 @@ Transform OdometryF2M::computeTransform(

 				if(this->isInfoDataFilled())
 				{
-					info->localMap = uMultimapToMap(map_->getWords3());
+					info->localMap.clear();
+					if(!map_->getWords3().empty())
+					{
+						for(std::multimap<int, int>::const_iterator iter=map_->getWords().begin(); iter!=map_->getWords().end(); ++iter)
+						{
+							info->localMap.insert(std::make_pair(iter->first, map_->getWords3()[iter->second]));
+						}
+					}
 					info->localScanMap = map_->sensorData().laserScanRaw();
 				}
 			}
@@ -1352,7 +1411,14 @@ Transform OdometryF2M::computeTransform(
 		{
 			if(regPipeline_->isImageRequired())
 			{
-				info->words = lastFrame_->getWords();
+				info->words.clear();
+				if(!lastFrame_->getWordsKpts().empty())
+				{
+					for(std::multimap<int, int>::const_iterator iter=lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter)
+					{
+						info->words.insert(std::make_pair(iter->first, lastFrame_->getWordsKpts()[iter->second]));
+					}
+				}
 			}
 		}
 	}
@@ -283,15 +283,15 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 						newCorners[oi] = imagePoints[i];
 						if(localMap_.count(ids[i]) == 1)
 						{
-							if(prevS->getWords().count(ids[i]) == 1)
+							if(prevS->getWords().count(ids[i]) == 1 && !prevS->getWordsKpts().empty())
 							{
 								// set guess if unique
-								refCorners[oi] = prevS->getWords().find(ids[i])->second.pt;
+								refCorners[oi] = prevS->getWordsKpts()[prevS->getWords().find(ids[i])->second].pt;
 							}
-							if(newS->getWords().count(ids[i]) == 1)
+							if(newS->getWords().count(ids[i]) == 1 && !newS->getWordsKpts().empty())
 							{
 								// set guess if unique
-								newCorners[oi] = newS->getWords().find(ids[i])->second.pt;
+								newCorners[oi] = newS->getWordsKpts()[newS->getWords().find(ids[i])->second].pt;
 							}
 						}
 						objectPointsTmp[oi] = objectPoints[i];
@@ -338,9 +338,9 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 						if(this->isInfoDataFilled() && info)
 						{
 							cv::KeyPoint kpt;
-							if(newS->getWords().count(matches[i]) == 1)
+							if(newS->getWords().count(matches[i]) == 1 && !newS->getWordsKpts().empty())
 							{
-								kpt = newS->getWords().find(matches[i])->second;
+								kpt = newS->getWordsKpts()[newS->getWords().find(matches[i])->second];
 							}
 							kpt.pt = newCorners[i];
 							info->words.insert(std::make_pair(matches[i], kpt));
@@ -437,9 +437,9 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 							for(std::set<int>::iterator iter = memory_->getStMem().begin(); iter!=memory_->getStMem().end(); ++iter)
 							{
 								const Signature * s = memory_->getSignature(*iter);
-								for(std::multimap<int, cv::KeyPoint>::const_iterator jter=s->getWords().begin(); jter!=s->getWords().end(); ++jter)
+								for(std::multimap<int, int>::const_iterator jter=s->getWords().begin(); jter!=s->getWords().end(); ++jter)
 								{
-									if(s->getWords().count(jter->first) == 1 && localMap_.find(jter->first)!=localMap_.end())
+									if(s->getWords().count(jter->first) == 1 && localMap_.find(jter->first)!=localMap_.end() && !s->getWordsKpts().empty())
 									{
 										if(wordReferences.find(jter->first)==wordReferences.end())
 										{
@@ -451,7 +451,8 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 										{
 											depth = keyFrameWords3D_.at(s->id()).at(jter->first).x;
 										}
-										wordReferences.at(jter->first).insert(std::make_pair(s->id(), FeatureBA(jter->second, depth, cv::Mat())));
+										const cv::KeyPoint & kpts = s->getWordsKpts()[jter->second];
+										wordReferences.at(jter->first).insert(std::make_pair(s->id(), FeatureBA(kpts, depth, cv::Mat())));
 									}
 								}
 							}
@@ -502,9 +503,21 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 							}
 							else if(float(inliers)/float(imagePoints.size()) < keyFrameThr_)
 							{
+								std::map<int, int> uniqueWordsPrevious = uMultimapToMapUnique(previousS->getWords());
+								std::map<int, int> uniqueWordsNew = uMultimapToMapUnique(newS->getWords());
+								std::map<int, cv::KeyPoint> wordsPrevious;
+								std::map<int, cv::KeyPoint> wordsNew;
+								for(std::map<int, int>::iterator iter=uniqueWordsPrevious.begin(); iter!=uniqueWordsPrevious.end(); ++iter)
+								{
+									wordsPrevious.insert(std::make_pair(iter->first, previousS->getWordsKpts()[iter->second]));
+								}
+								for(std::map<int, int>::iterator iter=uniqueWordsNew.begin(); iter!=uniqueWordsNew.end(); ++iter)
+								{
+									wordsNew.insert(std::make_pair(iter->first, newS->getWordsKpts()[iter->second]));
+								}
 								std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
-										uMultimapToMapUnique(previousS->getWords()),
-										uMultimapToMapUnique(newS->getWords()),
+										wordsPrevious,
+										wordsNew,
 										cameraModel,
 										cameraTransform,
 										fundMatrixReprojError_,
@@ -626,9 +639,9 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 		int ii=0;
 		for(std::map<int, cv::Point2f>::iterator iter=firstFrameGuessCorners_.begin(); iter!=firstFrameGuessCorners_.end(); ++iter)
 		{
-			std::multimap<int, cv::KeyPoint>::const_iterator jter=refS->getWords().find(iter->first);
-			UASSERT(jter != refS->getWords().end());
-			refCorners[ii] = jter->second.pt;
+			std::multimap<int, int>::const_iterator jter=refS->getWords().find(iter->first);
+			UASSERT(jter != refS->getWords().end() && !refS->getWordsKpts().empty());
+			refCorners[ii] = refS->getWordsKpts()[jter->second].pt;
 			refCornersGuess[ii] = iter->second;
 			cornerIds[ii] = iter->first;
 			++ii;
@@ -800,14 +813,15 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 			// generate kpts
 		if(memory_->update(SensorData(data)))
 		{
-			const std::multimap<int, cv::KeyPoint> & words = memory_->getLastWorkingSignature()->getWords();
-			if((int)words.size() > minInliers_)
+			const Signature * s = memory_->getLastWorkingSignature();
+			const std::multimap<int, int> & words = s->getWords();
+			if((int)words.size() > minInliers_ && !s->getWordsKpts().empty())
 			{
-				for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+				for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 				{
 					if(words.count(iter->first) == 1)
 					{
-						firstFrameGuessCorners_.insert(std::make_pair(iter->first, iter->second.pt));
+						firstFrameGuessCorners_.insert(std::make_pair(iter->first, s->getWordsKpts()[iter->second].pt));
 					}
 				}
 			}
@@ -80,24 +80,19 @@ using namespace std;
 namespace rtabmap
 {

-const int PATCH_SIZE = 31;
-const int HALF_PATCH_SIZE = 15;
-const int EDGE_THRESHOLD = 19;
-
-
-static float IC_Angle(const Mat& image, Point2f pt,  const vector<int> & u_max)
+static float IC_Angle(const Mat& image, Point2f pt,  const vector<int> & u_max, int halfPatchSize)
 {
    int m_01 = 0, m_10 = 0;

    const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));

    // Treat the center line differently, v=0
-    for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
+    for (int u = -halfPatchSize; u <= halfPatchSize; ++u)
        m_10 += u * center[u];

    // Go line by line in the circuI853lar patch
    int step = (int)image.step1();
-    for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
+    for (int v = 1; v <= halfPatchSize; ++v)
    {
        // Proceed over the two lines
        int v_sum = 0;
@@ -419,10 +414,11 @@ static int bit_pattern_31_[256*4] =
 };

 ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels,
-         int _iniThFAST, int _minThFAST):
+         int _iniThFAST, int _minThFAST, int _patchSize, int _edgeThreshold):
    nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),
-    iniThFAST(_iniThFAST), minThFAST(_minThFAST)
+    iniThFAST(_iniThFAST), minThFAST(_minThFAST), patchSize(_patchSize), edgeThreshold(_edgeThreshold)
 {
+	halfPatchSize = patchSize/2;
    mvScaleFactor.resize(nlevels);
    mvLevelSigma2.resize(nlevels);
    mvScaleFactor[0]=1.0f;
@@ -462,16 +458,16 @@ ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels,

    //This is for orientation
    // pre-compute the end of a row in a circular patch
-    umax.resize(HALF_PATCH_SIZE + 1);
+    umax.resize(halfPatchSize + 1);

-    int v, v0, vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1);
-    int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2);
-    const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE;
+    int v, v0, vmax = cvFloor(float(halfPatchSize) * sqrt(2.f) / 2 + 1);
+    int vmin = cvCeil(float(halfPatchSize) * sqrt(2.f) / 2);
+    const double hp2 = halfPatchSize*halfPatchSize;
    for (v = 0; v <= vmax; ++v)
        umax[v] = cvRound(sqrt(hp2 - v * v));

    // Make sure we are symmetric
-    for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v)
+    for (v = halfPatchSize, v0 = 0; v >= vmin; --v)
    {
        while (umax[v0] == umax[v0 + 1])
            ++v0;
@@ -480,12 +476,12 @@ ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels,
    }
 }

-static void computeOrientation(const Mat& image, vector<KeyPoint>& keypoints, const vector<int>& umax)
+static void computeOrientation(const Mat& image, vector<KeyPoint>& keypoints, const vector<int>& umax, int halfPatchSize)
 {
    for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
         keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
    {
-        keypoint->angle = IC_Angle(image, keypoint->pt, umax);
+        keypoint->angle = IC_Angle(image, keypoint->pt, umax, halfPatchSize);
    }
 }

@@ -781,10 +777,10 @@ void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoin

    for (int level = 0; level < nlevels; ++level)
    {
-        const int minBorderX = EDGE_THRESHOLD-3;
+        const int minBorderX = edgeThreshold-3;
        const int minBorderY = minBorderX;
-        const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;
-        const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;
+        const int maxBorderX = mvImagePyramid[level].cols-edgeThreshold+3;
+        const int maxBorderY = mvImagePyramid[level].rows-edgeThreshold+3;

        vector<cv::KeyPoint> vToDistributeKeys;
        vToDistributeKeys.reserve(nfeatures*10);
@@ -845,7 +841,7 @@ void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoin
        keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX,
                                      minBorderY, maxBorderY,mnFeaturesPerLevel[level], level);

-        const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level];
+        const int scaledPatchSize = patchSize*mvScaleFactor[level];

        // Add border to coordinates and scale information
        const int nkps = keypoints.size();
@@ -860,7 +856,7 @@ void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoin

    // compute orientations
    for (int level = 0; level < nlevels; ++level)
-        computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
+        computeOrientation(mvImagePyramid[level], allKeypoints[level], umax, halfPatchSize);
 }

 void ORBextractor::ComputeKeyPointsOld(std::vector<std::vector<KeyPoint> > &allKeypoints)
@@ -876,10 +872,10 @@ void ORBextractor::ComputeKeyPointsOld(std::vector<std::vector<KeyPoint> > &allK
        const int levelCols = sqrt((float)nDesiredFeatures/(5*imageRatio));
        const int levelRows = imageRatio*levelCols;

-        const int minBorderX = EDGE_THRESHOLD;
+        const int minBorderX = edgeThreshold;
        const int minBorderY = minBorderX;
-        const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD;
-        const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD;
+        const int maxBorderX = mvImagePyramid[level].cols-edgeThreshold;
+        const int maxBorderY = mvImagePyramid[level].rows-edgeThreshold;

        const int W = maxBorderX - minBorderX;
        const int H = maxBorderY - minBorderY;
@@ -1006,7 +1002,7 @@ void ORBextractor::ComputeKeyPointsOld(std::vector<std::vector<KeyPoint> > &allK
        vector<KeyPoint> & keypoints = allKeypoints[level];
        keypoints.reserve(nDesiredFeatures*2);

-        const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level];
+        const int scaledPatchSize = patchSize*mvScaleFactor[level];

        // Retain by score and transform coordinates
        for(int i=0; i<levelRows; i++)
@@ -1039,7 +1035,7 @@ void ORBextractor::ComputeKeyPointsOld(std::vector<std::vector<KeyPoint> > &allK

    // and compute orientations
    for (int level = 0; level < nlevels; ++level)
-        computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
+        computeOrientation(mvImagePyramid[level], allKeypoints[level], umax, halfPatchSize);
 }

 static void computeDescriptors(const Mat& image, vector<KeyPoint>& keypoints, Mat& descriptors,
@@ -1121,21 +1117,21 @@ void ORBextractor::ComputePyramid(cv::Mat image)
    {
        float scale = mvInvScaleFactor[level];
        Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));
-        Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2);
+        Size wholeSize(sz.width + edgeThreshold*2, sz.height + edgeThreshold*2);
        Mat temp(wholeSize, image.type()), masktemp;
-        mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));
+        mvImagePyramid[level] = temp(Rect(edgeThreshold, edgeThreshold, sz.width, sz.height));

        // Compute the resized image
        if( level != 0 )
        {
            resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, INTER_LINEAR);

-            copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
+            copyMakeBorder(mvImagePyramid[level], temp, edgeThreshold, edgeThreshold, edgeThreshold, edgeThreshold,
                           BORDER_REFLECT_101+BORDER_ISOLATED);            
        }
        else
        {
-            copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
+            copyMakeBorder(image, temp, edgeThreshold, edgeThreshold, edgeThreshold, edgeThreshold,
                           BORDER_REFLECT_101);            
        }
    }
@@ -57,7 +57,7 @@ public:
    enum {HARRIS_SCORE=0, FAST_SCORE=1 };

    ORBextractor(int nfeatures, float scaleFactor, int nlevels,
-                 int iniThFAST, int minThFAST);
+                 int iniThFAST, int minThFAST, int patchSize, int edgeThreshold);

    ~ORBextractor(){}

@@ -107,6 +107,10 @@ protected:
    int nlevels;
    int iniThFAST;
    int minThFAST;
+    int patchSize;
+    int edgeThreshold;
+    int halfPatchSize;
+

    std::vector<int> mnFeaturesPerLevel;

@@ -1496,6 +1496,11 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			if(points3DMap.find(id) != points3DMap.end())
 			{
 				cv::Point3f pt3d = points3DMap.at(id);
+				if(!util3d::isFinite(pt3d))
+				{
+					UWARN("Ignoring 3D point %d because it has nan value(s)!", id);
+					continue;
+				}
 				g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();

 				vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
@@ -1522,7 +1527,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						const FeatureBA & pt = jter->second;
 						double depth = pt.depth;

-						//UDEBUG("Added observation pt=%d to cam=%d (%f,%f) depth=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth);
+						//UDEBUG("Added observation pt=%d to cam=%d (%d,%d) depth=%f", vpt3d->id()-stepVertexId, camId, (int)pt.kpt.pt.x, (int)pt.kpt.pt.y, depth);

 						g2o::OptimizableGraph::Edge * e;
 						double baseline = 0.0;
@@ -1568,9 +1573,9 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 							if(baseline > 0.0)
 							{
 								UDEBUG("Stereo camera model detected but current "
-										"observation (pt=%d to cam=%d) has null depth (%f m), adding "
+										"observation (pt=%d to cam=%d, kpt=[%d,%d]) has null depth (%f m), adding "
 										"mono observation instead.",
-										vpt3d->id()-stepVertexId, camId, depth);
+										vpt3d->id()-stepVertexId, camId, (int)pt.kpt.pt.x, (int)pt.kpt.pt.y, depth);
 							}
 							// mono edge
 #ifdef RTABMAP_ORB_SLAM2
@@ -107,12 +107,14 @@ std::map<int, Transform> OptimizerGTSAM::optimize(

 		// detect if there is a global pose prior set, if so remove rootId
 		bool gpsPriorOnly = false;
+		bool hasPriorPoses = false;
 		if(!priorsIgnored())
 		{
 			for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 			{
 				if(iter->second.from() == iter->second.to() && iter->second.type() == Link::kPosePrior)
 				{
+					hasPriorPoses = true;
 					if ((isSlam2d() && 1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999) ||
 						(1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) < 9999.0 &&
 						 1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) < 9999.0 &&
@@ -136,17 +138,18 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		{
 			UASSERT(uContains(poses, rootId));
 			const Transform & initialPose = poses.at(rootId);
+			UDEBUG("hasPriorPoses=%s, gpsPriorOnly=%s", hasPriorPoses?"true":"false", gpsPriorOnly?"true":"false");
 			if(isSlam2d())
 			{
-				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, 0.01));
+				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasPriorPoses?1e-2:std::numeric_limits<double>::min()));
 				graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
 			}
 			else
 			{
 				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(
 						(gtsam::Vector(6) <<
-								(gpsPriorOnly?2:1e-2), gpsPriorOnly?2:1e-2, gpsPriorOnly?2:1e-2,
-								1e-2, 1e-2, 1e-2
+								1e-2, 1e-2, hasPriorPoses?1e-2:std::numeric_limits<double>::min(), // roll, pitch, fixed yaw if there are no priors
+								(gpsPriorOnly?2:1e-2), gpsPriorOnly?2:1e-2, gpsPriorOnly?2:1e-2 // xyz
 								).finished());
 				graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
 			}
@@ -146,7 +146,14 @@ std::map<int, Transform> OptimizerTORO::optimize(
 						UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
 					}
 				}
-				//else // not supporting pose prior and landmarks
+				else if(id1 == id2)
+				{
+					UWARN("TORO optimizer doesn't support prior or gravity links, use GTSAM or g2o optimizers (see parameter %s). Link %d ignored...", Parameters::kOptimizerStrategy().c_str(), id1);
+				}
+				else if(id1 < 0 || id2 < 0)
+				{
+					UWARN("TORO optimizer doesn't support landmark links, use GTSAM or g2o optimizers (see parameter %s). Link %d->%d ignored...", Parameters::kOptimizerStrategy().c_str(), id1, id2);
+				}
 			}
 		}
 		else
@@ -178,7 +185,14 @@ std::map<int, Transform> OptimizerTORO::optimize(
 						UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
 					}
 				}
-				//else // not supporting pose prior and landmarks
+				else if(id1 == id2)
+				{
+					UWARN("TORO optimizer doesn't support prior or gravity links, use GTSAM or g2o optimizers (see parameter %s). Link %d ignored...", Parameters::kOptimizerStrategy().c_str(), id1);
+				}
+				else if(id1 < 0 || id2 < 0)
+				{
+					UWARN("TORO optimizer doesn't support landmark links, use GTSAM or g2o optimizers (see parameter %s). Link %d->%d ignored...", Parameters::kOptimizerStrategy().c_str(), id1, id2);
+				}
 			}
 		}
 		UDEBUG("buildMST... root=%d", rootId);
@@ -10,6 +10,16 @@

 #include <gtsam/linear/NoiseModel.h>
 #include <Eigen/Eigen>
+#include <gtsam/config.h>
+
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1)
+namespace gtsam {
+gtsam::Matrix inverse(const gtsam::Matrix & matrix)
+{
+	return matrix.inverse();
+}
+}
+#endif

 namespace vertigo {

@@ -39,7 +49,11 @@ namespace vertigo {
          double nu1 = 1.0/sqrt(gtsam::inverse(info1).determinant());
          double l1 = nu1 * exp(-0.5*m1);

+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1)
+          double m2 = nullHypothesisModel->squaredMahalanobisDistance(error);
+#else
          double m2 = nullHypothesisModel->distance(error);
+#endif
          gtsam::noiseModel::Gaussian::shared_ptr g2 = nullHypothesisModel;
          gtsam::Matrix info2(g2->R().transpose()*g2->R());
          double nu2 = 1.0/sqrt(gtsam::inverse(info2).determinant());
@@ -78,8 +78,8 @@ namespace vertigo {
    inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
        boost::optional<gtsam::Matrix&> H1=boost::none,
        boost::optional<gtsam::Matrix&> H2=boost::none) const {
-      if(H1) *H1 = -gtsam::eye(1);
-      if(H2) *H2 = gtsam::eye(1);
+      if(H1) *H1 = -gtsam::Matrix::Identity(1, 1);
+      if(H2) *H2 = gtsam::Matrix::Identity(1, 1);
      return SwitchVariableLinear(l2.value() - value());
    }

@@ -78,8 +78,8 @@ namespace vertigo {
    inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2,
        boost::optional<gtsam::Matrix&> H1=boost::none,
        boost::optional<gtsam::Matrix&> H2=boost::none) const {
-      if(H1) *H1 = -gtsam::eye(1);
-      if(H2) *H2 = gtsam::eye(1);
+      if(H1) *H1 = -gtsam::Matrix::Identity(1, 1);
+      if(H2) *H2 = gtsam::Matrix::Identity(1, 1);
      return SwitchVariableSigmoid(l2.value() - value());
    }

@@ -41,6 +41,7 @@
 #include <pcl/common/distances.h>
 #include <pcl18/surface/texture_mapping.h>
 #include <pcl/search/octree.h>
+#include <pcl/common/common.h> // for getAngle3D

 ///////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointInT> std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >
@@ -1054,7 +1055,8 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 		pcl::TextureMesh &mesh,
 		const pcl::texture_mapping::CameraVector &cameras,
 		const rtabmap::ProgressState * state,
-		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels)
+		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels,
+		bool distanceToCamPolicy)
 {

 	if (mesh.tex_polygons.size () != 1)
@@ -1076,7 +1078,14 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 	std::vector<std::map<int, FaceInfo > > visibleFaces(cameras.size());
 	std::vector<Eigen::Affine3f> invCamTransform(cameras.size());
 	std::vector<std::list<int> > faceCameras(faces.size());
-	UINFO("Precompute visible faces per cam (%d faces, %d cams)", (int)faces.size(), (int)cameras.size());
+	std::string msg = uFormat("Computing visible faces per cam (%d faces, %d cams)", (int)faces.size(), (int)cameras.size());
+	UINFO(msg.c_str());
+	if(state && !state->callback(msg))
+	{
+		//cancelled!
+		UWARN("Texturing cancelled!");
+		return false;
+	}
 	for (unsigned int current_cam = 0; current_cam < cameras.size(); ++current_cam)
 	{
 		UDEBUG("Texture camera %d...", current_cam);
@@ -1272,7 +1281,7 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 			}
 		}

-		std::string msg = uFormat("Processed camera %d/%d: %d occluded and %d spurious polygons out of %d", (int)current_cam+1, (int)cameras.size(), (int)occludedFaces.size(), clusterFaces, (int)visibilityIndices.size());
+		msg = uFormat("Processed camera %d/%d: %d occluded and %d spurious polygons out of %d", (int)current_cam+1, (int)cameras.size(), (int)occludedFaces.size(), clusterFaces, (int)visibilityIndices.size());
 		UINFO(msg.c_str());
 		if(state && !state->callback(msg))
 		{
@@ -1282,7 +1291,7 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 		}
 	}

-	std::string msg = uFormat("Texturing %d polygons...", (int)faces.size());
+	msg = uFormat("Texturing %d polygons...", (int)faces.size());
 	UINFO(msg.c_str());
 	if(state && !state->callback(msg))
 	{
@@ -1385,10 +1394,15 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (

 				//UDEBUG("Process polygon %d cam =%d distanceToCam=%f", idx_face, current_cam, distanceToCam);

-				if(distanceToCenter <= smallestWeight || (!depthSet && currentDepthSet))
+				float distance = distanceToCenter;
+				if(distanceToCamPolicy)
+				{
+					distance = distanceToCam;
+				}
+				if(distance <= smallestWeight || (!depthSet && currentDepthSet))
 				{
 					cameraIndex = current_cam;
-					smallestWeight = distanceToCenter;
+					smallestWeight = distance;
 					uv_coords[0] = iter->second.uv_coord1;
 					uv_coords[1] = iter->second.uv_coord2;
 					uv_coords[2] = iter->second.uv_coord3;
@@ -43,6 +43,7 @@
 #include <pcl/surface/reconstruction.h>
 #include <pcl/common/transforms.h>
 #include <pcl/TextureMesh.h>
+#include <pcl/octree/octree.h>
 #include <rtabmap/core/ProgressState.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
@@ -366,7 +367,8 @@ namespace pcl
      textureMeshwithMultipleCameras2 (pcl::TextureMesh &mesh,
                                      const pcl::texture_mapping::CameraVector &cameras,
 									  const rtabmap::ProgressState * callback = 0,
-									  std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0);
+									  std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0,
+									  bool distanceToCamPolicy = false);

    protected:
      /** \brief mesh scale control. */
@@ -0,0 +1,169 @@
+-- *******************************************************************
+--  DatabaseSchema: Script for creating the database
+--   Usage:
+--       $ sqlite3 LTM.db < DatabaseSchema.sql
+--
+-- *******************************************************************
+
+-- *******************************************************************
+-- CLEAN
+-- *******************************************************************
+/*DROP TABLE Node;*/
+
+-- *******************************************************************
+-- CREATE
+-- *******************************************************************
+CREATE TABLE Node (
+	id INTEGER NOT NULL,
+	map_id INTEGER NOT NULL,
+	weight INTEGER,
+	stamp FLOAT,
+	pose BLOB,                -- 3x4 float
+	ground_truth_pose BLOB,   -- 3x4 float
+	velocity BLOB,            -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s
+	label TEXT,
+	gps BLOB,                 -- 1x6 double: stamp, longitude (DD), latitude (DD), altitude (m), accuracy (m), bearing (North 0->360 deg clockwise)
+	
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Data (
+	id INTEGER NOT NULL,
+	image BLOB,               -- compressed image (Grayscale or RGB)
+	depth BLOB,               -- compressed image (Depth or Right image)
+	calibration BLOB,         -- fx, fy, cx, cy, [baseline,] width, height, local_transform
+	
+	scan BLOB,                -- compressed data (Laser scan)
+	scan_info BLOB,           -- scan_max_pts, scan_max_range, local_transform
+	
+	ground_cells BLOB,        -- compressed data (occupancy grid)
+	obstacle_cells BLOB,      -- compressed data (occupancy grid)
+	empty_cells BLOB,         -- compressed data (occupancy grid)
+	cell_size FLOAT,
+	view_point_x FLOAT,
+	view_point_y FLOAT,
+	view_point_z FLOAT,
+	
+	user_data BLOB,           -- compressed data (User data)
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Link (
+	from_id INTEGER NOT NULL,
+	to_id INTEGER NOT NULL,
+	type INTEGER NOT NULL,    -- neighbor=0, loop=1, child=2
+	information_matrix BLOB NOT NULL, -- 6x6 double (inverse covariance)
+	transform BLOB,           -- 3x4 float
+	user_data BLOB,           -- compressed data (User data)
+	FOREIGN KEY (from_id) REFERENCES Node(id),
+	FOREIGN KEY (to_id) REFERENCES Node(id)
+);
+
+-- 
+CREATE TABLE Word (
+	id INTEGER NOT NULL,
+	descriptor_size INTEGER NOT NULL,
+	descriptor BLOB NOT NULL,
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Feature (
+	node_id INTEGER NOT NULL,
+	word_id INTEGER NOT NULL,
+	pos_x FLOAT NOT NULL,
+	pos_y FLOAT NOT NULL,
+	size INTEGER NOT NULL,
+	dir FLOAT NOT NULL,
+	response FLOAT NOT NULL,
+	octave INTEGER NOT NULL,
+	depth_x FLOAT,
+	depth_y FLOAT,
+	depth_z FLOAT,
+	descriptor_size INTEGER,
+	descriptor BLOB,
+	FOREIGN KEY (node_id) REFERENCES Node(id)
+);
+
+CREATE TABLE Info (
+	STM_size INTEGER,
+	last_sign_added INTEGER,
+	process_mem_used INTEGER,
+	database_mem_used INTEGER,
+	dictionary_size INTEGER,
+	parameters TEXT,
+	time_enter DATE
+);
+
+CREATE TABLE Statistics (
+	id INTEGER NOT NULL,
+	stamp FLOAT,
+	data BLOB,	
+	FOREIGN KEY (id) REFERENCES Node(id)
+);
+
+CREATE TABLE Admin (
+	version TEXT,
+	preview_image BLOB,      -- compressed image
+	
+	opt_cloud BLOB,          -- compressed data
+	opt_ids BLOB,            -- Node ids used to generate the optimized cloud/mesh
+	opt_poses BLOB,          -- compressed N*3x4 float 
+	opt_polygons_size INTEGER, -- e.g., 3
+	opt_polygons BLOB,       -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
+	opt_tex_coords BLOB,     -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
+	opt_tex_materials BLOB,  -- compressed image
+	opt_map BLOB,            -- compressed CV_8SC1 occupancy grid
+	opt_map_x_min FLOAT,
+	opt_map_y_min FLOAT, 
+
+	time_enter DATE
+);
+
+-- *******************************************************************
+-- TRIGGERS
+-- *******************************************************************
+CREATE TRIGGER insert_Feature BEFORE INSERT ON Feature 
+WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key constraint failed in Feature table');
+END;
+
+ --   Creating a trigger for time_enter
+CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Data_timeEnter AFTER INSERT ON Data
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word
+BEGIN
+ UPDATE Word SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Info_timeEnter AFTER INSERT ON Info
+BEGIN
+ UPDATE Info SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+-- *******************************************************************
+-- INDEXES
+-- *******************************************************************
+CREATE UNIQUE INDEX IDX_Node_id on Node (id);
+CREATE INDEX IDX_Feature_node_id on Feature (node_id);
+CREATE INDEX IDX_Link_from_id on Link (from_id);
+CREATE UNIQUE INDEX IDX_node_label on Node (label);
+CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id);
+
+-- *******************************************************************
+-- VERSION
+-- *******************************************************************
+INSERT INTO Admin(version) VALUES('0.16.0');
+
+
@@ -0,0 +1,169 @@
+-- *******************************************************************
+--  DatabaseSchema: Script for creating the database
+--   Usage:
+--       $ sqlite3 LTM.db < DatabaseSchema.sql
+--
+-- *******************************************************************
+
+-- *******************************************************************
+-- CLEAN
+-- *******************************************************************
+/*DROP TABLE Node;*/
+
+-- *******************************************************************
+-- CREATE
+-- *******************************************************************
+CREATE TABLE Node (
+	id INTEGER NOT NULL,
+	map_id INTEGER NOT NULL,
+	weight INTEGER,
+	stamp FLOAT,
+	pose BLOB,                -- 3x4 float
+	ground_truth_pose BLOB,   -- 3x4 float
+	velocity BLOB,            -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s
+	label TEXT,
+	gps BLOB,                 -- 1x6 double: stamp, longitude (DD), latitude (DD), altitude (m), accuracy (m), bearing (North 0->360 deg clockwise)
+	
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Data (
+	id INTEGER NOT NULL,
+	image BLOB,               -- compressed image (Grayscale or RGB)
+	depth BLOB,               -- compressed image (Depth or Right image)
+	calibration BLOB,         -- fx, fy, cx, cy, [baseline,] width, height, local_transform
+	
+	scan BLOB,                -- compressed data (Laser scan)
+	scan_info BLOB,           -- scan_max_pts, scan_max_range, scan_format, local_transform
+	
+	ground_cells BLOB,        -- compressed data (occupancy grid)
+	obstacle_cells BLOB,      -- compressed data (occupancy grid)
+	empty_cells BLOB,         -- compressed data (occupancy grid)
+	cell_size FLOAT,
+	view_point_x FLOAT,
+	view_point_y FLOAT,
+	view_point_z FLOAT,
+	
+	user_data BLOB,           -- compressed data (User data)
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Link (
+	from_id INTEGER NOT NULL,
+	to_id INTEGER NOT NULL,
+	type INTEGER NOT NULL,    -- neighbor=0, loop=1, child=2
+	information_matrix BLOB NOT NULL, -- 6x6 double (inverse covariance)
+	transform BLOB,           -- 3x4 float
+	user_data BLOB,           -- compressed data (User data)
+	FOREIGN KEY (from_id) REFERENCES Node(id),
+	FOREIGN KEY (to_id) REFERENCES Node(id)
+);
+
+-- 
+CREATE TABLE Word (
+	id INTEGER NOT NULL,
+	descriptor_size INTEGER NOT NULL,
+	descriptor BLOB NOT NULL,
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Feature (
+	node_id INTEGER NOT NULL,
+	word_id INTEGER NOT NULL,
+	pos_x FLOAT NOT NULL,
+	pos_y FLOAT NOT NULL,
+	size INTEGER NOT NULL,
+	dir FLOAT NOT NULL,
+	response FLOAT NOT NULL,
+	octave INTEGER NOT NULL,
+	depth_x FLOAT,
+	depth_y FLOAT,
+	depth_z FLOAT,
+	descriptor_size INTEGER,
+	descriptor BLOB,
+	FOREIGN KEY (node_id) REFERENCES Node(id)
+);
+
+CREATE TABLE Info (
+	STM_size INTEGER,
+	last_sign_added INTEGER,
+	process_mem_used INTEGER,
+	database_mem_used INTEGER,
+	dictionary_size INTEGER,
+	parameters TEXT,
+	time_enter DATE
+);
+
+CREATE TABLE Statistics (
+	id INTEGER NOT NULL,
+	stamp FLOAT,
+	data BLOB,	
+	FOREIGN KEY (id) REFERENCES Node(id)
+);
+
+CREATE TABLE Admin (
+	version TEXT,
+	preview_image BLOB,      -- compressed image
+	
+	opt_cloud BLOB,          -- compressed data
+	opt_ids BLOB,            -- Node ids used to generate the optimized cloud/mesh
+	opt_poses BLOB,          -- compressed N*3x4 float 
+	opt_polygons_size INTEGER, -- e.g., 3
+	opt_polygons BLOB,       -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
+	opt_tex_coords BLOB,     -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
+	opt_tex_materials BLOB,  -- compressed image
+	opt_map BLOB,            -- compressed CV_8SC1 occupancy grid
+	opt_map_x_min FLOAT,
+	opt_map_y_min FLOAT, 
+
+	time_enter DATE
+);
+
+-- *******************************************************************
+-- TRIGGERS
+-- *******************************************************************
+CREATE TRIGGER insert_Feature BEFORE INSERT ON Feature 
+WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key constraint failed in Feature table');
+END;
+
+ --   Creating a trigger for time_enter
+CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Data_timeEnter AFTER INSERT ON Data
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word
+BEGIN
+ UPDATE Word SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Info_timeEnter AFTER INSERT ON Info
+BEGIN
+ UPDATE Info SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+-- *******************************************************************
+-- INDEXES
+-- *******************************************************************
+CREATE UNIQUE INDEX IDX_Node_id on Node (id);
+CREATE INDEX IDX_Feature_node_id on Feature (node_id);
+CREATE INDEX IDX_Link_from_id on Link (from_id);
+CREATE UNIQUE INDEX IDX_node_label on Node (label);
+CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id);
+
+-- *******************************************************************
+-- VERSION
+-- *******************************************************************
+INSERT INTO Admin(version) VALUES('0.16.1');
+
+
@@ -0,0 +1,170 @@
+-- *******************************************************************
+--  DatabaseSchema: Script for creating the database
+--   Usage:
+--       $ sqlite3 LTM.db < DatabaseSchema.sql
+--
+-- *******************************************************************
+
+-- *******************************************************************
+-- CLEAN
+-- *******************************************************************
+/*DROP TABLE Node;*/
+
+-- *******************************************************************
+-- CREATE
+-- *******************************************************************
+CREATE TABLE Node (
+	id INTEGER NOT NULL,
+	map_id INTEGER NOT NULL,
+	weight INTEGER,
+	stamp FLOAT,
+	pose BLOB,                -- 3x4 float
+	ground_truth_pose BLOB,   -- 3x4 float
+	velocity BLOB,            -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s
+	label TEXT,
+	gps BLOB,                 -- 1x6 double: stamp, longitude (DD), latitude (DD), altitude (m), accuracy (m), bearing (North 0->360 deg clockwise)
+	
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Data (
+	id INTEGER NOT NULL,
+	image BLOB,               -- compressed image (Grayscale or RGB)
+	depth BLOB,               -- compressed image (Depth or Right image)
+	calibration BLOB,         -- fx, fy, cx, cy, [baseline,] width, height, local_transform
+	
+	scan BLOB,                -- compressed data (Laser scan)
+	scan_info BLOB,           -- scan_max_pts, scan_max_range, scan_format, local_transform
+	
+	ground_cells BLOB,        -- compressed data (occupancy grid)
+	obstacle_cells BLOB,      -- compressed data (occupancy grid)
+	empty_cells BLOB,         -- compressed data (occupancy grid)
+	cell_size FLOAT,
+	view_point_x FLOAT,
+	view_point_y FLOAT,
+	view_point_z FLOAT,
+	
+	user_data BLOB,           -- compressed data (User data)
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Link (
+	from_id INTEGER NOT NULL,
+	to_id INTEGER NOT NULL,
+	type INTEGER NOT NULL,    -- neighbor=0, loop=1, child=2
+	information_matrix BLOB NOT NULL, -- 6x6 double (inverse covariance)
+	transform BLOB,           -- 3x4 float
+	user_data BLOB,           -- compressed data (User data)
+	FOREIGN KEY (from_id) REFERENCES Node(id),
+	FOREIGN KEY (to_id) REFERENCES Node(id)
+);
+
+-- 
+CREATE TABLE Word (
+	id INTEGER NOT NULL,
+	descriptor_size INTEGER NOT NULL,
+	descriptor BLOB NOT NULL,
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Feature (
+	node_id INTEGER NOT NULL,
+	word_id INTEGER NOT NULL,
+	pos_x FLOAT NOT NULL,
+	pos_y FLOAT NOT NULL,
+	size INTEGER NOT NULL,
+	dir FLOAT NOT NULL,
+	response FLOAT NOT NULL,
+	octave INTEGER NOT NULL,
+	depth_x FLOAT,
+	depth_y FLOAT,
+	depth_z FLOAT,
+	descriptor_size INTEGER,
+	descriptor BLOB,
+	FOREIGN KEY (node_id) REFERENCES Node(id)
+);
+
+CREATE TABLE Info (
+	STM_size INTEGER,
+	last_sign_added INTEGER,
+	process_mem_used INTEGER,
+	database_mem_used INTEGER,
+	dictionary_size INTEGER,
+	parameters TEXT,
+	time_enter DATE
+);
+
+CREATE TABLE Statistics (
+	id INTEGER NOT NULL,
+	stamp FLOAT,
+	data BLOB,              -- compressed string
+	wm_state BLOB,	        -- compressed data
+	FOREIGN KEY (id) REFERENCES Node(id)
+);
+
+CREATE TABLE Admin (
+	version TEXT,
+	preview_image BLOB,      -- compressed image
+	
+	opt_cloud BLOB,          -- compressed data
+	opt_ids BLOB,            -- Node ids used to generate the optimized cloud/mesh
+	opt_poses BLOB,          -- compressed N*3x4 float 
+	opt_polygons_size INTEGER, -- e.g., 3
+	opt_polygons BLOB,       -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
+	opt_tex_coords BLOB,     -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
+	opt_tex_materials BLOB,  -- compressed image
+	opt_map BLOB,            -- compressed CV_8SC1 occupancy grid
+	opt_map_x_min FLOAT,
+	opt_map_y_min FLOAT, 
+
+	time_enter DATE
+);
+
+-- *******************************************************************
+-- TRIGGERS
+-- *******************************************************************
+CREATE TRIGGER insert_Feature BEFORE INSERT ON Feature 
+WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key constraint failed in Feature table');
+END;
+
+ --   Creating a trigger for time_enter
+CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Data_timeEnter AFTER INSERT ON Data
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word
+BEGIN
+ UPDATE Word SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Info_timeEnter AFTER INSERT ON Info
+BEGIN
+ UPDATE Info SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+-- *******************************************************************
+-- INDEXES
+-- *******************************************************************
+CREATE UNIQUE INDEX IDX_Node_id on Node (id);
+CREATE INDEX IDX_Feature_node_id on Feature (node_id);
+CREATE INDEX IDX_Link_from_id on Link (from_id);
+CREATE UNIQUE INDEX IDX_node_label on Node (label);
+CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id);
+
+-- *******************************************************************
+-- VERSION
+-- *******************************************************************
+INSERT INTO Admin(version) VALUES('0.16.2');
+
+
@@ -0,0 +1,172 @@
+-- *******************************************************************
+--  DatabaseSchema: Script for creating the database
+--   Usage:
+--       $ sqlite3 LTM.db < DatabaseSchema.sql
+--
+-- *******************************************************************
+
+-- *******************************************************************
+-- CLEAN
+-- *******************************************************************
+/*DROP TABLE Node;*/
+
+-- *******************************************************************
+-- CREATE
+-- *******************************************************************
+CREATE TABLE Node (
+	id INTEGER NOT NULL,
+	map_id INTEGER NOT NULL,
+	weight INTEGER,
+	stamp FLOAT,
+	pose BLOB,                -- 3x4 float
+	ground_truth_pose BLOB,   -- 3x4 float
+	velocity BLOB,            -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s
+	label TEXT,
+	gps BLOB,                 -- 1x6 double: stamp, longitude (DD), latitude (DD), altitude (m), accuracy (m), bearing (North 0->360 deg clockwise)
+	
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Data (
+	id INTEGER NOT NULL,
+	image BLOB,               -- compressed image (Grayscale or RGB)
+	depth BLOB,               -- compressed image (Depth or Right image)
+	calibration BLOB,         -- fx, fy, cx, cy, [baseline,] width, height, local_transform
+	
+	scan BLOB,                -- compressed data (Laser scan)
+	scan_info BLOB,           -- scan_max_pts, scan_max_range, scan_format, local_transform
+	
+	ground_cells BLOB,        -- compressed data (occupancy grid)
+	obstacle_cells BLOB,      -- compressed data (occupancy grid)
+	empty_cells BLOB,         -- compressed data (occupancy grid)
+	cell_size FLOAT,
+	view_point_x FLOAT,
+	view_point_y FLOAT,
+	view_point_z FLOAT,
+	
+	user_data BLOB,           -- compressed data (User data)
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Link (
+	from_id INTEGER NOT NULL,
+	to_id INTEGER NOT NULL,
+	type INTEGER NOT NULL,    -- neighbor=0, loop=1, child=2
+	information_matrix BLOB NOT NULL, -- 6x6 double (inverse covariance)
+	transform BLOB,           -- 3x4 float
+	user_data BLOB,           -- compressed data (User data)
+	FOREIGN KEY (from_id) REFERENCES Node(id),
+	FOREIGN KEY (to_id) REFERENCES Node(id)
+);
+
+-- 
+CREATE TABLE Word (
+	id INTEGER NOT NULL,
+	descriptor_size INTEGER NOT NULL,
+	descriptor BLOB NOT NULL,
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Feature (
+	node_id INTEGER NOT NULL,
+	word_id INTEGER NOT NULL,
+	pos_x FLOAT NOT NULL,
+	pos_y FLOAT NOT NULL,
+	size INTEGER NOT NULL,
+	dir FLOAT NOT NULL,
+	response FLOAT NOT NULL,
+	octave INTEGER NOT NULL,
+	depth_x FLOAT,
+	depth_y FLOAT,
+	depth_z FLOAT,
+	descriptor_size INTEGER,
+	descriptor BLOB,
+	FOREIGN KEY (node_id) REFERENCES Node(id)
+);
+
+CREATE TABLE Info (
+	STM_size INTEGER,
+	last_sign_added INTEGER,
+	process_mem_used INTEGER,
+	database_mem_used INTEGER,
+	dictionary_size INTEGER,
+	parameters TEXT,
+	time_enter DATE
+);
+
+CREATE TABLE Statistics (
+	id INTEGER NOT NULL,
+	stamp FLOAT,
+	data BLOB,              -- compressed string
+	wm_state BLOB,	        -- compressed data
+	FOREIGN KEY (id) REFERENCES Node(id)
+);
+
+CREATE TABLE Admin (
+	version TEXT,
+	preview_image BLOB,      -- compressed image
+	
+	opt_cloud BLOB,          -- compressed data
+	opt_ids BLOB,            -- Node ids used to generate the optimized cloud/mesh
+	opt_poses BLOB,          -- compressed N*3x4 float 
+	opt_last_localization BLOB, -- 3x4 float
+	opt_polygons_size INTEGER, -- e.g., 3
+	opt_polygons BLOB,       -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
+	opt_tex_coords BLOB,     -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
+	opt_tex_materials BLOB,  -- compressed image
+	opt_map BLOB,            -- compressed CV_8SC1 occupancy grid
+	opt_map_x_min FLOAT,
+	opt_map_y_min FLOAT, 
+	opt_map_resolution FLOAT, 
+
+	time_enter DATE
+);
+
+-- *******************************************************************
+-- TRIGGERS
+-- *******************************************************************
+CREATE TRIGGER insert_Feature BEFORE INSERT ON Feature 
+WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key constraint failed in Feature table');
+END;
+
+ --   Creating a trigger for time_enter
+CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Data_timeEnter AFTER INSERT ON Data
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word
+BEGIN
+ UPDATE Word SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Info_timeEnter AFTER INSERT ON Info
+BEGIN
+ UPDATE Info SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+-- *******************************************************************
+-- INDEXES
+-- *******************************************************************
+CREATE UNIQUE INDEX IDX_Node_id on Node (id);
+CREATE INDEX IDX_Feature_node_id on Feature (node_id);
+CREATE INDEX IDX_Link_from_id on Link (from_id);
+CREATE UNIQUE INDEX IDX_node_label on Node (label);
+CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id);
+
+-- *******************************************************************
+-- VERSION
+-- *******************************************************************
+INSERT INTO Admin(version) VALUES('0.17.0');
+
+
@@ -0,0 +1,181 @@
+-- *******************************************************************
+--  DatabaseSchema: Script for creating the database
+--   Usage:
+--       $ sqlite3 LTM.db < DatabaseSchema.sql
+--
+-- *******************************************************************
+
+-- *******************************************************************
+-- CLEAN
+-- *******************************************************************
+/*DROP TABLE Node;*/
+
+-- *******************************************************************
+-- CREATE
+-- *******************************************************************
+CREATE TABLE Node (
+	id INTEGER NOT NULL,
+	map_id INTEGER NOT NULL,
+	weight INTEGER,
+	stamp FLOAT,
+	pose BLOB,                -- 3x4 float
+	ground_truth_pose BLOB,   -- 3x4 float
+	velocity BLOB,            -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s
+	label TEXT,
+	gps BLOB,                 -- 1x6 double: stamp, longitude (DD), latitude (DD), altitude (m), accuracy (m), bearing (North 0->360 deg clockwise)
+	env_sensors BLOB,         -- Variable 3xdouble: (sensorId1, value, stamp, sensorId2, value, stamp, ...)
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Data (
+	id INTEGER NOT NULL,
+	image BLOB,               -- compressed image (Grayscale or RGB)
+	depth BLOB,               -- compressed image (Depth or Right image)
+	calibration BLOB,         -- fx, fy, cx, cy, [baseline,] width, height, local_transform
+	
+	scan BLOB,                -- compressed data (Laser scan)
+	scan_info BLOB,           -- scan_max_pts, scan_max_range, scan_format, local_transform
+	
+	ground_cells BLOB,        -- compressed data (occupancy grid)
+	obstacle_cells BLOB,      -- compressed data (occupancy grid)
+	empty_cells BLOB,         -- compressed data (occupancy grid)
+	cell_size FLOAT,
+	view_point_x FLOAT,
+	view_point_y FLOAT,
+	view_point_z FLOAT,
+	
+	user_data BLOB,           -- compressed data (User data)
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Link (
+	from_id INTEGER NOT NULL,
+	to_id INTEGER NOT NULL,
+	type INTEGER NOT NULL,    -- neighbor=0, loop=1, child=2
+	information_matrix BLOB NOT NULL, -- 6x6 double (inverse covariance)
+	transform BLOB,           -- 3x4 float
+	user_data BLOB,           -- compressed data (User data)
+	FOREIGN KEY (from_id) REFERENCES Node(id),
+	FOREIGN KEY (to_id) REFERENCES Node(id)
+);
+
+-- 
+CREATE TABLE Word (
+	id INTEGER NOT NULL,
+	descriptor_size INTEGER NOT NULL,
+	descriptor BLOB NOT NULL,
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Feature (
+	node_id INTEGER NOT NULL,
+	word_id INTEGER NOT NULL,
+	pos_x FLOAT NOT NULL,
+	pos_y FLOAT NOT NULL,
+	size INTEGER NOT NULL,
+	dir FLOAT NOT NULL,
+	response FLOAT NOT NULL,
+	octave INTEGER NOT NULL,
+	depth_x FLOAT,
+	depth_y FLOAT,
+	depth_z FLOAT,
+	descriptor_size INTEGER,
+	descriptor BLOB,
+	FOREIGN KEY (node_id) REFERENCES Node(id)
+);
+
+--
+CREATE TABLE Tag (
+	node_id INTEGER NOT NULL,
+	tag_id INTEGER NOT NULL,
+	stamp FLOAT NOT NULL,
+	transform BLOB NOT NULL,     -- 3x4 float, /base_link -> /tag_frame
+	FOREIGN KEY (node_id) REFERENCES Node(id)
+);
+
+CREATE TABLE Info (
+	STM_size INTEGER,
+	last_sign_added INTEGER,
+	process_mem_used INTEGER,
+	database_mem_used INTEGER,
+	dictionary_size INTEGER,
+	parameters TEXT,
+	time_enter DATE
+);
+
+CREATE TABLE Statistics (
+	id INTEGER NOT NULL,
+	stamp FLOAT,
+	data BLOB,              -- compressed string
+	wm_state BLOB,	        -- compressed data
+	FOREIGN KEY (id) REFERENCES Node(id)
+);
+
+CREATE TABLE Admin (
+	version TEXT,
+	preview_image BLOB,      -- compressed image
+	
+	opt_cloud BLOB,          -- compressed data
+	opt_ids BLOB,            -- Node ids used to generate the optimized cloud/mesh
+	opt_poses BLOB,          -- compressed N*3x4 float 
+	opt_last_localization BLOB, -- 3x4 float
+	opt_polygons_size INTEGER, -- e.g., 3
+	opt_polygons BLOB,       -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
+	opt_tex_coords BLOB,     -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
+	opt_tex_materials BLOB,  -- compressed image
+	opt_map BLOB,            -- compressed CV_8SC1 occupancy grid
+	opt_map_x_min FLOAT,
+	opt_map_y_min FLOAT, 
+	opt_map_resolution FLOAT, 
+
+	time_enter DATE
+);
+
+-- *******************************************************************
+-- TRIGGERS
+-- *******************************************************************
+CREATE TRIGGER insert_Feature BEFORE INSERT ON Feature 
+WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key constraint failed in Feature table');
+END;
+
+ --   Creating a trigger for time_enter
+CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Data_timeEnter AFTER INSERT ON Data
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word
+BEGIN
+ UPDATE Word SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Info_timeEnter AFTER INSERT ON Info
+BEGIN
+ UPDATE Info SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+-- *******************************************************************
+-- INDEXES
+-- *******************************************************************
+CREATE UNIQUE INDEX IDX_Node_id on Node (id);
+CREATE INDEX IDX_Feature_node_id on Feature (node_id);
+CREATE INDEX IDX_Link_from_id on Link (from_id);
+CREATE UNIQUE INDEX IDX_node_label on Node (label);
+CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id);
+
+-- *******************************************************************
+-- VERSION
+-- *******************************************************************
+INSERT INTO Admin(version) VALUES('0.18.0');
+
+
@@ -0,0 +1,181 @@
+-- *******************************************************************
+--  DatabaseSchema: Script for creating the database
+--   Usage:
+--       $ sqlite3 LTM.db < DatabaseSchema.sql
+--
+-- *******************************************************************
+
+-- *******************************************************************
+-- CLEAN
+-- *******************************************************************
+/*DROP TABLE Node;*/
+
+-- *******************************************************************
+-- CREATE
+-- *******************************************************************
+CREATE TABLE Node (
+	id INTEGER NOT NULL,
+	map_id INTEGER NOT NULL,
+	weight INTEGER,
+	stamp FLOAT,
+	pose BLOB,                -- 3x4 float
+	ground_truth_pose BLOB,   -- 3x4 float
+	velocity BLOB,            -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s
+	label TEXT,
+	gps BLOB,                 -- 1x6 double: stamp, longitude (DD), latitude (DD), altitude (m), accuracy (m), bearing (North 0->360 deg clockwise)
+	env_sensors BLOB,         -- Variable 3xdouble: (sensorId1, value, stamp, sensorId2, value, stamp, ...)
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Data (
+	id INTEGER NOT NULL,
+	image BLOB,               -- compressed image (Grayscale or RGB)
+	depth BLOB,               -- compressed image (Depth or Right image)
+	calibration BLOB,         -- fx, fy, cx, cy, [baseline,] width, height, local_transform
+	
+	scan BLOB,                -- compressed data (Laser scan)
+	scan_info BLOB,           -- scan_max_pts, scan_max_range, scan_format, local_transform
+	
+	ground_cells BLOB,        -- compressed data (occupancy grid)
+	obstacle_cells BLOB,      -- compressed data (occupancy grid)
+	empty_cells BLOB,         -- compressed data (occupancy grid)
+	cell_size FLOAT,
+	view_point_x FLOAT,
+	view_point_y FLOAT,
+	view_point_z FLOAT,
+	
+	user_data BLOB,           -- compressed data (User data)
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Link (
+	from_id INTEGER NOT NULL,
+	to_id INTEGER NOT NULL,
+	type INTEGER NOT NULL,    -- neighbor=0, loop=1, child=2
+	information_matrix BLOB NOT NULL, -- 6x6 double (inverse covariance)
+	transform BLOB,           -- 3x4 float
+	user_data BLOB,           -- compressed data (User data)
+	FOREIGN KEY (from_id) REFERENCES Node(id),
+	FOREIGN KEY (to_id) REFERENCES Node(id)
+);
+
+-- 
+CREATE TABLE Word (
+	id INTEGER NOT NULL,
+	descriptor_size INTEGER NOT NULL,
+	descriptor BLOB NOT NULL,
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Feature (
+	node_id INTEGER NOT NULL,
+	word_id INTEGER NOT NULL,
+	pos_x FLOAT NOT NULL,
+	pos_y FLOAT NOT NULL,
+	size INTEGER NOT NULL,
+	dir FLOAT NOT NULL,
+	response FLOAT NOT NULL,
+	octave INTEGER NOT NULL,
+	depth_x FLOAT,
+	depth_y FLOAT,
+	depth_z FLOAT,
+	descriptor_size INTEGER,
+	descriptor BLOB,
+	FOREIGN KEY (node_id) REFERENCES Node(id)
+);
+
+--
+CREATE TABLE Tag (
+	node_id INTEGER NOT NULL,
+	tag_id INTEGER NOT NULL,
+	stamp FLOAT NOT NULL,
+	transform BLOB NOT NULL,     -- 3x4 float, /base_link -> /tag_frame
+	FOREIGN KEY (node_id) REFERENCES Node(id)
+);
+
+CREATE TABLE Info (
+	STM_size INTEGER,
+	last_sign_added INTEGER,
+	process_mem_used INTEGER,
+	database_mem_used INTEGER,
+	dictionary_size INTEGER,
+	parameters TEXT,
+	time_enter DATE
+);
+
+CREATE TABLE Statistics (
+	id INTEGER NOT NULL,
+	stamp FLOAT,
+	data BLOB,              -- compressed string
+	wm_state BLOB,	        -- compressed data
+	FOREIGN KEY (id) REFERENCES Node(id)
+);
+
+CREATE TABLE Admin (
+	version TEXT,
+	preview_image BLOB,      -- compressed image
+	
+	opt_cloud BLOB,          -- compressed data
+	opt_ids BLOB,            -- Node ids used to generate the optimized cloud/mesh
+	opt_poses BLOB,          -- compressed N*3x4 float 
+	opt_last_localization BLOB, -- 3x4 float
+	opt_polygons_size INTEGER, -- e.g., 3
+	opt_polygons BLOB,       -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
+	opt_tex_coords BLOB,     -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
+	opt_tex_materials BLOB,  -- compressed image
+	opt_map BLOB,            -- compressed CV_8SC1 occupancy grid
+	opt_map_x_min FLOAT,
+	opt_map_y_min FLOAT, 
+	opt_map_resolution FLOAT, 
+
+	time_enter DATE
+);
+
+-- *******************************************************************
+-- TRIGGERS
+-- *******************************************************************
+CREATE TRIGGER insert_Feature BEFORE INSERT ON Feature 
+WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key constraint failed in Feature table');
+END;
+
+ --   Creating a trigger for time_enter
+CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Data_timeEnter AFTER INSERT ON Data
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word
+BEGIN
+ UPDATE Word SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Info_timeEnter AFTER INSERT ON Info
+BEGIN
+ UPDATE Info SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+-- *******************************************************************
+-- INDEXES
+-- *******************************************************************
+CREATE UNIQUE INDEX IDX_Node_id on Node (id);
+CREATE INDEX IDX_Feature_node_id on Feature (node_id);
+CREATE INDEX IDX_Link_from_id on Link (from_id);
+CREATE UNIQUE INDEX IDX_node_label on Node (label);
+CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id);
+
+-- *******************************************************************
+-- VERSION
+-- *******************************************************************
+INSERT INTO Admin(version) VALUES('0.18.3');
+
+
@@ -168,12 +168,16 @@ std::vector<cv::KeyPoint> SPDetector::detect(const cv::Mat &img, const cv::Mat &
 		auto kpts = (prob_ > threshold_);
 		kpts = torch::nonzero(kpts);  // [n_keypoints, 2]  (y, x)

+    //convert back to cpu if in gpu
+    auto kpts_cpu = kpts.to(torch::kCPU);
+    auto prob_cpu = prob_.to(torch::kCPU);
+
 		std::vector<cv::KeyPoint> keypoints_no_nms;
-		for (int i = 0; i < kpts.size(0); i++) {
-			if(mask.empty() || mask.at<unsigned char>(kpts[i][0].item<int>(), kpts[i][1].item<int>()) != 0)
+		for (int i = 0; i < kpts_cpu.size(0); i++) {
+			if(mask.empty() || mask.at<unsigned char>(kpts_cpu[i][0].item<int>(), kpts_cpu[i][1].item<int>()) != 0)
 			{
-				float response = prob_[kpts[i][0]][kpts[i][1]].item<float>();
-				keypoints_no_nms.push_back(cv::KeyPoint(kpts[i][1].item<float>(), kpts[i][0].item<float>(), 8, -1, response));
+				float response = prob_cpu[kpts_cpu[i][0]][kpts_cpu[i][1]].item<float>();
+				keypoints_no_nms.push_back(cv::KeyPoint(kpts_cpu[i][1].item<float>(), kpts_cpu[i][0].item<float>(), 8, -1, response));
 			}
 		}

@@ -183,7 +187,7 @@ std::vector<cv::KeyPoint> SPDetector::detect(const cv::Mat &img, const cv::Mat &
 			for (size_t i = 0; i < keypoints_no_nms.size(); i++) {
 				int x = keypoints_no_nms[i].pt.x;
 				int y = keypoints_no_nms[i].pt.y;
-				conf.at<float>(i, 0) = prob_[y][x].item<float>();
+				conf.at<float>(i, 0) = prob_cpu[y][x].item<float>();
 			}

 			int border = 0;
@@ -2044,6 +2044,55 @@ cv::Mat exposureFusion(const std::vector<cv::Mat> & images)
 	return fusion;
 }

+void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
+{
+	int i;
+	float f, p, q, t;
+	if( s == 0 ) {
+		// achromatic (grey)
+		*r = *g = *b = v;
+		return;
+	}
+	h /= 60;			// sector 0 to 5
+	i = floor( h );
+	f = h - i;			// factorial part of h
+	p = v * ( 1 - s );
+	q = v * ( 1 - s * f );
+	t = v * ( 1 - s * ( 1 - f ) );
+	switch( i ) {
+		case 0:
+			*r = v;
+			*g = t;
+			*b = p;
+			break;
+		case 1:
+			*r = q;
+			*g = v;
+			*b = p;
+			break;
+		case 2:
+			*r = p;
+			*g = v;
+			*b = t;
+			break;
+		case 3:
+			*r = p;
+			*g = q;
+			*b = v;
+			break;
+		case 4:
+			*r = t;
+			*g = p;
+			*b = v;
+			break;
+		default:		// case 5:
+			*r = v;
+			*g = p;
+			*b = q;
+			break;
+	}
+}
+
 }

 }
@@ -1389,7 +1389,7 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filter
 		}
 	}

-	UASSERT(cloud.data.size()/cloud.point_step == cloud.height*cloud.width);
+	UASSERT(cloud.data.size()/cloud.point_step == (uint32_t)cloud.height*cloud.width);
 	cv::Mat laserScan = cv::Mat(1, (int)cloud.data.size()/cloud.point_step, CV_32FC(LaserScan::channels(format)));

 	bool transformValid = !transform.isNull() && !transform.isIdentity();
@@ -1399,10 +1399,10 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filter
 		transformRot = transform.rotation();
 	}
 	int oi=0;
-	for (uint32_t row = 0; row < cloud.height; ++row)
+	for (uint32_t row = 0; row < (uint32_t)cloud.height; ++row)
 	{
 		const uint8_t* row_data = &cloud.data[row * cloud.row_step];
-		for (uint32_t col = 0; col < cloud.width; ++col)
+		for (uint32_t col = 0; col < (uint32_t)cloud.width; ++col)
 		{
 			const uint8_t* msg_data = row_data + col * cloud.point_step;

@@ -2918,10 +2918,10 @@ cv::Mat projectCloudToCamera(
 	int count = 0;
 	if(field_map.size() == 1)
 	{
-		for (uint32_t row = 0; row < laserScan->height; ++row)
+		for (uint32_t row = 0; row < (uint32_t)laserScan->height; ++row)
 		{
 			const uint8_t* row_data = &laserScan->data[row * laserScan->row_step];
-			for (uint32_t col = 0; col < laserScan->width; ++col)
+			for (uint32_t col = 0; col < (uint32_t)laserScan->width; ++col)
 			{
 				const uint8_t* msg_data = row_data + col * laserScan->point_step;
 				pcl::PointXYZ ptScan;
@@ -79,11 +79,11 @@ LaserScan commonFiltering(
 		float voxelSize,
 		int normalK,
 		float normalRadius,
-		bool forceGroundNormalsUp)
+		float groundNormalsUp)
 {
 	LaserScan scan = scanIn;
-	UDEBUG("scan size=%d format=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f",
-			scan.size(), (int)scan.format(), downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius);
+	UDEBUG("scan size=%d format=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f, groundNormalsUp=%f",
+			scan.size(), (int)scan.format(), downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius, groundNormalsUp);
 	if(!scan.isEmpty())
 	{
 		// combined downsampling and range filtering step
@@ -293,14 +293,27 @@ LaserScan commonFiltering(
 			}
 		}

-		if(scan.size() && !scan.is2d() && scan.hasNormals() && forceGroundNormalsUp)
+		if(scan.size() && !scan.is2d() && scan.hasNormals() && groundNormalsUp>0.0f)
 		{
-			scan = util3d::adjustNormalsToViewPoint(scan, Eigen::Vector3f(0,0,0), forceGroundNormalsUp);
+			scan = util3d::adjustNormalsToViewPoint(scan, Eigen::Vector3f(0,0,10), groundNormalsUp);
 		}
 	}
 	return scan;
 }

+LaserScan commonFiltering(
+		const LaserScan & scanIn,
+		int downsamplingStep,
+		float rangeMin,
+		float rangeMax,
+		float voxelSize,
+		int normalK,
+		float normalRadius,
+		bool forceGroundNormalsUp)
+{
+	return commonFiltering(scanIn, downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius, forceGroundNormalsUp?0.8f:0.0f);
+}
+
 LaserScan rangeFiltering(
 		const LaserScan & scan,
 		float rangeMin,
@@ -769,6 +782,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cropBox(const pcl::PointCloud<pcl::PointX
 {
 	return cropBoxImpl<pcl::PointXYZRGB>(cloud, min, max, transform, negative);
 }
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr cropBox(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
+{
+	return cropBoxImpl<pcl::PointXYZINormal>(cloud, min, max, transform, negative);
+}
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cropBox(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
 {
 	return cropBoxImpl<pcl::PointXYZRGBNormal>(cloud, min, max, transform, negative);
@@ -1083,6 +1100,19 @@ pcl::PointCloud<pcl::PointNormal>::Ptr subtractFiltering(
 	pcl::copyPointCloud(*cloud, *indicesOut, *out);
 	return out;
 }
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
+		float radiusSearch,
+		float maxAngle,
+		int minNeighborsInRadius)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
+	pcl::PointCloud<pcl::PointXYZINormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZINormal>);
+	pcl::copyPointCloud(*cloud, *indicesOut, *out);
+	return out;
+}
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
@@ -1237,6 +1267,17 @@ pcl::IndicesPtr subtractFiltering(
 {
 	return subtractFilteringImpl<pcl::PointNormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
 }
+pcl::IndicesPtr subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
+		const pcl::IndicesPtr & substractIndices,
+		float radiusSearch,
+		float maxAngle,
+		int minNeighborsInRadius)
+{
+	return subtractFilteringImpl<pcl::PointXYZINormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
+}
 pcl::IndicesPtr subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
@@ -80,8 +80,8 @@ void occupancy2DFromLaserScan(
 }

 void occupancy2DFromLaserScan(
-		const cv::Mat & scanHit,
-		const cv::Mat & scanNoHit,
+		const cv::Mat & scanHitIn,
+		const cv::Mat & scanNoHitIn,
 		const cv::Point3f & viewpoint,
 		cv::Mat & empty,
 		cv::Mat & occupied,
@@ -89,10 +89,41 @@ void occupancy2DFromLaserScan(
 		bool unknownSpaceFilled,
 		float scanMaxRange)
 {
-	if(scanHit.empty() && scanNoHit.empty())
+	if(scanHitIn.empty() && scanNoHitIn.empty())
 	{
 		return;
 	}
+	cv::Mat scanHit;
+	cv::Mat scanNoHit;
+	// keep only XY channels
+	if(scanHitIn.channels()>2)
+	{
+		std::vector<cv::Mat> channels;
+		cv::split(scanHitIn,channels);
+		while(channels.size()>2)
+		{
+			channels.pop_back();
+		}
+		cv::merge(channels,scanHit);
+	}
+	else
+	{
+		scanHit = scanHitIn.clone(); // will be returned in occupied matrix
+	}
+	if(scanNoHitIn.channels()>2)
+	{
+		std::vector<cv::Mat> channels;
+		cv::split(scanNoHitIn,channels);
+		while(channels.size()>2)
+		{
+			channels.pop_back();
+		}
+		cv::merge(channels,scanNoHit);
+	}
+	else
+	{
+		scanNoHit = scanHitIn;
+	}

 	std::map<int, Transform> poses;
 	poses.insert(std::make_pair(1, Transform::getIdentity()));
@@ -136,11 +167,11 @@ void occupancy2DFromLaserScan(
 	// copy directly obstacles precise positions
 	if(scanMaxRange > cellSize)
 	{
-		occupied = util3d::rangeFiltering(LaserScan::backwardCompatibility(scanHit), 0.0f, scanMaxRange).data().clone();
+		occupied = util3d::rangeFiltering(LaserScan::backwardCompatibility(scanHit), 0.0f, scanMaxRange).data();
 	}
 	else
 	{
-		occupied = scanHit.clone();
+		occupied = scanHit;
 	}
 }

@@ -237,9 +237,10 @@ Transform transformFromXYZCorrespondences(
 	return Transform();
 }

-void computeVarianceAndCorrespondences(
-		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
-		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudB,
+template<typename PointNormalT>
+void computeVarianceAndCorrespondencesImpl(
+		const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloudA,
+		const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloudB,
 		double maxCorrespondenceDistance,
 		double maxCorrespondenceAngle,
 		double & variance,
@@ -247,10 +248,10 @@ void computeVarianceAndCorrespondences(
 {
 	variance = 1;
 	correspondencesOut = 0;
-	pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
-	est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
-	const pcl::PointCloud<pcl::PointNormal>::ConstPtr & target = cloudA->size()>cloudB->size()?cloudA:cloudB;
-	const pcl::PointCloud<pcl::PointNormal>::ConstPtr & source = cloudA->size()>cloudB->size()?cloudB:cloudA;
+	typename pcl::registration::CorrespondenceEstimation<PointNormalT, PointNormalT>::Ptr est;
+	est.reset(new pcl::registration::CorrespondenceEstimation<PointNormalT, PointNormalT>);
+	const typename pcl::PointCloud<PointNormalT>::ConstPtr & target = cloudA->size()>cloudB->size()?cloudA:cloudB;
+	const typename pcl::PointCloud<PointNormalT>::ConstPtr & source = cloudA->size()>cloudB->size()?cloudB:cloudA;
 	est->setInputTarget(target);
 	est->setInputSource(source);
 	pcl::Correspondences correspondences;
@@ -299,16 +300,39 @@ void computeVarianceAndCorrespondences(
 }

 void computeVarianceAndCorrespondences(
-		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
-		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double maxCorrespondenceAngle,
+		double & variance,
+		int & correspondencesOut)
+{
+	computeVarianceAndCorrespondencesImpl<pcl::PointNormal>(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut);
+}
+
+void computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double maxCorrespondenceAngle,
+		double & variance,
+		int & correspondencesOut)
+{
+	computeVarianceAndCorrespondencesImpl<pcl::PointXYZINormal>(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut);
+}
+
+template<typename PointT>
+void computeVarianceAndCorrespondencesImpl(
+		const typename pcl::PointCloud<PointT>::ConstPtr & cloudA,
+		const typename pcl::PointCloud<PointT>::ConstPtr & cloudB,
 		double maxCorrespondenceDistance,
 		double & variance,
 		int & correspondencesOut)
 {
 	variance = 1;
 	correspondencesOut = 0;
-	pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
-	est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
+	typename pcl::registration::CorrespondenceEstimation<PointT, PointT>::Ptr est;
+	est.reset(new pcl::registration::CorrespondenceEstimation<PointT, PointT>);
 	est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
 	est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
 	pcl::Correspondences correspondences;
@@ -331,25 +355,46 @@ void computeVarianceAndCorrespondences(
 	correspondencesOut = (int)correspondences.size();
 }

+void computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double & variance,
+		int & correspondencesOut)
+{
+	computeVarianceAndCorrespondencesImpl<pcl::PointXYZ>(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut);
+}
+
+void computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double & variance,
+		int & correspondencesOut)
+{
+	computeVarianceAndCorrespondencesImpl<pcl::PointXYZI>(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut);
+}
+
 // return transform from source to target (All points must be finite!!!)
-Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
-			  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
+template<typename PointT>
+Transform icpImpl(const typename pcl::PointCloud<PointT>::ConstPtr & cloud_source,
+			  const typename pcl::PointCloud<PointT>::ConstPtr & cloud_target,
 			  double maxCorrespondenceDistance,
 			  int maximumIterations,
 			  bool & hasConverged,
-			  pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
+			  pcl::PointCloud<PointT> & cloud_source_registered,
 			  float epsilon,
 			  bool icp2D)
 {
-	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
+	pcl::IterativeClosestPoint<PointT, PointT> icp;
 	// Set the input source and target
 	icp.setInputTarget (cloud_target);
 	icp.setInputSource (cloud_source);

 	if(icp2D)
 	{
-		pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
-		est.reset(new pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>);
+		typename pcl::registration::TransformationEstimation2D<PointT, PointT>::Ptr est;
+		est.reset(new pcl::registration::TransformationEstimation2D<PointT, PointT>);
 		icp.setTransformationEstimation(est);
 	}

@@ -369,24 +414,51 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 	return Transform::fromEigen4f(icp.getFinalTransformation());
 }

+// return transform from source to target (All points must be finite!!!)
+Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
+			  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
+			  double maxCorrespondenceDistance,
+			  int maximumIterations,
+			  bool & hasConverged,
+			  pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
+			  float epsilon,
+			  bool icp2D)
+{
+	return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
+}
+
+// return transform from source to target (All points must be finite!!!)
+Transform icp(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_source,
+			  const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_target,
+			  double maxCorrespondenceDistance,
+			  int maximumIterations,
+			  bool & hasConverged,
+			  pcl::PointCloud<pcl::PointXYZI> & cloud_source_registered,
+			  float epsilon,
+			  bool icp2D)
+{
+	return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
+}
+
 // return transform from source to target (All points/normals must be finite!!!)
-Transform icpPointToPlane(
-		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
-		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
+template<typename PointNormalT>
+Transform icpPointToPlaneImpl(
+		const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloud_source,
+		const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloud_target,
 		double maxCorrespondenceDistance,
 		int maximumIterations,
 		bool & hasConverged,
-		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
+		pcl::PointCloud<PointNormalT> & cloud_source_registered,
 		float epsilon,
 		bool icp2D)
 {
-	pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
+	pcl::IterativeClosestPoint<PointNormalT, PointNormalT> icp;
 	// Set the input source and target
 	icp.setInputTarget (cloud_target);
 	icp.setInputSource (cloud_source);

-	pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>::Ptr est;
-	est.reset(new pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>);
+	typename pcl::registration::TransformationEstimationPointToPlaneLLS<PointNormalT, PointNormalT>::Ptr est;
+	est.reset(new pcl::registration::TransformationEstimationPointToPlaneLLS<PointNormalT, PointNormalT>);
 	icp.setTransformationEstimation(est);

 	// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
@@ -413,6 +485,33 @@ Transform icpPointToPlane(
 	return t;
 }

+// return transform from source to target (All points/normals must be finite!!!)
+Transform icpPointToPlane(
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
+		double maxCorrespondenceDistance,
+		int maximumIterations,
+		bool & hasConverged,
+		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
+		float epsilon,
+		bool icp2D)
+{
+	return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
+}
+// return transform from source to target (All points/normals must be finite!!!)
+Transform icpPointToPlane(
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_source,
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_target,
+		double maxCorrespondenceDistance,
+		int maximumIterations,
+		bool & hasConverged,
+		pcl::PointCloud<pcl::PointXYZINormal> & cloud_source_registered,
+		float epsilon,
+		bool icp2D)
+{
+	return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
+}
+
 }

 }
@@ -62,6 +62,9 @@ using namespace aliceVision;
 #include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
 #endif

+#if PCL_VERSION_COMPARE(>, 1, 11, 1)
+#include <pcl/types.h>
+#endif
 #if PCL_VERSION_COMPARE(<, 1, 8, 0)
 #include "pcl18/surface/organized_fast_mesh.h"
 #else
@@ -676,7 +679,8 @@ pcl::TextureMesh::Ptr createTextureMesh(
 		int minClusterSize,
 		const std::vector<float> & roiRatios,
 		const ProgressState * state,
-		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels)
+		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels,
+		bool distanceToCamPolicy)
 {
 	std::map<int, std::vector<CameraModel> > cameraSubModels;
 	for(std::map<int, CameraModel>::const_iterator iter=cameraModels.begin(); iter!=cameraModels.end(); ++iter)
@@ -697,7 +701,8 @@ pcl::TextureMesh::Ptr createTextureMesh(
 			minClusterSize,
 			roiRatios,
 			state,
-			vertexToPixels);
+			vertexToPixels,
+			distanceToCamPolicy);
 }

 pcl::TextureMesh::Ptr createTextureMesh(
@@ -711,7 +716,8 @@ pcl::TextureMesh::Ptr createTextureMesh(
 		int minClusterSize,
 		const std::vector<float> & roiRatios,
 		const ProgressState * state,
-		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels)
+		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels,
+		bool distanceToCamPolicy)
 {
 	UASSERT(mesh->polygons.size());
 	pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
@@ -776,7 +782,7 @@ pcl::TextureMesh::Ptr createTextureMesh(
 		tm.setMaxDepthError(maxDepthError);
 	}
 	tm.setMinClusterSize(minClusterSize);
-	if(tm.textureMeshwithMultipleCameras2(*textureMesh, cameras, state, vertexToPixels))
+	if(tm.textureMeshwithMultipleCameras2(*textureMesh, cameras, state, vertexToPixels, distanceToCamPolicy))
 	{
 		// compute normals for the mesh if not already here
 		bool hasNormals = false;
@@ -1214,18 +1220,18 @@ void concatenateTextureMaterials(pcl::TextureMesh & mesh, const cv::Size & image
 	}
 }

-std::vector<std::vector<unsigned int> > convertPolygonsFromPCL(const std::vector<pcl::Vertices> & polygons)
+std::vector<std::vector<RTABMAP_PCL_INDEX> > convertPolygonsFromPCL(const std::vector<pcl::Vertices> & polygons)
 {
-	std::vector<std::vector<unsigned int> > polygonsOut(polygons.size());
+	std::vector<std::vector<RTABMAP_PCL_INDEX> > polygonsOut(polygons.size());
 	for(unsigned int p=0; p<polygons.size(); ++p)
 	{
 		polygonsOut[p] = polygons[p].vertices;
 	}
 	return polygonsOut;
 }
-std::vector<std::vector<std::vector<unsigned int> > > convertPolygonsFromPCL(const std::vector<std::vector<pcl::Vertices> > & tex_polygons)
+std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > convertPolygonsFromPCL(const std::vector<std::vector<pcl::Vertices> > & tex_polygons)
 {
-	std::vector<std::vector<std::vector<unsigned int> > > polygonsOut(tex_polygons.size());
+	std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygonsOut(tex_polygons.size());
 	for(unsigned int t=0; t<tex_polygons.size(); ++t)
 	{
 		polygonsOut[t].resize(tex_polygons[t].size());
@@ -1236,7 +1242,7 @@ std::vector<std::vector<std::vector<unsigned int> > > convertPolygonsFromPCL(con
 	}
 	return polygonsOut;
 }
-std::vector<pcl::Vertices> convertPolygonsToPCL(const std::vector<std::vector<unsigned int> > & polygons)
+std::vector<pcl::Vertices> convertPolygonsToPCL(const std::vector<std::vector<RTABMAP_PCL_INDEX> > & polygons)
 {
 	std::vector<pcl::Vertices> polygonsOut(polygons.size());
 	for(unsigned int p=0; p<polygons.size(); ++p)
@@ -1245,7 +1251,7 @@ std::vector<pcl::Vertices> convertPolygonsToPCL(const std::vector<std::vector<un
 	}
 	return polygonsOut;
 }
-std::vector<std::vector<pcl::Vertices> > convertPolygonsToPCL(const std::vector<std::vector<std::vector<unsigned int> > > & tex_polygons)
+std::vector<std::vector<pcl::Vertices> > convertPolygonsToPCL(const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & tex_polygons)
 {
 	std::vector<std::vector<pcl::Vertices> > polygonsOut(tex_polygons.size());
 	for(unsigned int t=0; t<tex_polygons.size(); ++t)
@@ -1261,7 +1267,7 @@ std::vector<std::vector<pcl::Vertices> > convertPolygonsToPCL(const std::vector<

 pcl::TextureMesh::Ptr assembleTextureMesh(
 		const cv::Mat & cloudMat,
-		const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+		const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 		const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
 #else
@@ -1384,7 +1390,7 @@ pcl::TextureMesh::Ptr assembleTextureMesh(

 pcl::PolygonMesh::Ptr assemblePolygonMesh(
 		const cv::Mat & cloudMat,
-		const std::vector<std::vector<unsigned int> > & polygons)
+		const std::vector<std::vector<RTABMAP_PCL_INDEX> > & polygons)
 {
 	pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh);

@@ -1506,7 +1512,7 @@ cv::Mat mergeTextures(
 	cv::Mat globalTextures;
 	if(mesh.tex_materials.size() > 1)
 	{
-		std::vector<std::pair<int, int> > textures(mesh.tex_materials.size(), std::pair<int, int>(-1,-1));
+		std::vector<std::pair<int, int> > textures(mesh.tex_materials.size(), std::pair<int, int>(-1,0));
 		cv::Size imageSize;
 		const int imageType=CV_8UC3;

@@ -2194,7 +2200,7 @@ void fixTextureMeshForVisualization(pcl::TextureMesh & textureMesh)
 				for(unsigned int j=0; j<vertices.vertices.size(); ++j)
 				{
 					UASSERT(oi < newCloud->size());
-					UASSERT_MSG(vertices.vertices[j] < originalCloud->size(), uFormat("%d vs %d", vertices.vertices[j], (int)originalCloud->size()).c_str());
+					UASSERT_MSG((size_t)vertices.vertices[j] < originalCloud->size(), uFormat("%d vs %d", vertices.vertices[j], (int)originalCloud->size()).c_str());
 					newCloud->at(oi) = originalCloud->at(vertices.vertices[j]);
 					vertices.vertices[j] = oi; // new vertex index
 					++oi;
@@ -2219,7 +2225,8 @@ bool multiBandTexturing(
 		const std::string & textureFormat,
 		const std::map<int, std::map<int, cv::Vec4d> > & gains,       // optional output of util3d::mergeTextures()
 		const std::map<int, std::map<int, cv::Mat> > & blendingGains, // optional output of util3d::mergeTextures()
-		const std::pair<float, float> & contrastValues)               // optional output of util3d::mergeTextures()
+		const std::pair<float, float> & contrastValues,               // optional output of util3d::mergeTextures()
+		bool gainRGB)
 {
 #ifdef RTABMAP_ALICE_VISION
 	if(ULogger::level() == ULogger::kDebug)
@@ -2245,17 +2252,30 @@ bool multiBandTexturing(
 	UASSERT(vertexToPixels.size() == cloud2.size());
 	UINFO("Input mesh: %d points %d polygons", (int)cloud2.size(), (int)polygons.size());
 	mesh::Texturing texturing;
+#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
+	texturing.mesh = new mesh::Mesh();
+	texturing.mesh->pts.resize(cloud2.size());
+	texturing.mesh->pointsVisibilities.resize(cloud2.size());
+#else
 	texturing.me = new mesh::Mesh();
 	texturing.me->pts = new StaticVector<Point3d>(cloud2.size());
 	texturing.pointsVisibilities = new mesh::PointsVisibility();
 	texturing.pointsVisibilities->reserve(cloud2.size());
+#endif
 	texturing.texParams.textureSide = 8192;
 	texturing.texParams.downscale = 8192/textureSize;

-	std::vector<int> camIndexToId(uKeys(cameraModels));
 	for(size_t i=0;i<cloud2.size();++i)
 	{
 		pcl::PointXYZRGB pt = cloud2.at(i);
+#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
+		texturing.mesh->pointsVisibilities[i].reserve(vertexToPixels[i].size());
+		for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[i].begin(); iter!=vertexToPixels[i].end();++iter)
+		{
+			texturing.mesh->pointsVisibilities[i].push_back(iter->first);
+		}
+		texturing.mesh->pts[i] = Point3d(pt.x, pt.y, pt.z);
+#else
 		mesh::PointVisibility* pointVisibility = new mesh::PointVisibility();
 		pointVisibility->reserve(vertexToPixels[i].size());
 		for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[i].begin(); iter!=vertexToPixels[i].end();++iter)
@@ -2264,13 +2284,24 @@ bool multiBandTexturing(
 		}
 		texturing.pointsVisibilities->push_back(pointVisibility);
 		(*texturing.me->pts)[i] = Point3d(pt.x, pt.y, pt.z);
+#endif
 	}

+#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
+	texturing.mesh->tris.resize(polygons.size());
+	texturing.mesh->trisMtlIds().resize(polygons.size());
+#else
 	texturing.me->tris = new StaticVector<mesh::Mesh::triangle>(polygons.size());
+#endif
 	for(size_t i=0;i<polygons.size();++i)
 	{
 		UASSERT(polygons[i].vertices.size() == 3);
+#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
+		texturing.mesh->trisMtlIds()[i] = -1;
+		texturing.mesh->tris[i] = mesh::Mesh::triangle(
+#else
 		(*texturing.me->tris)[i] = mesh::Mesh::triangle(
+#endif
 				polygons[i].vertices[0],
 				polygons[i].vertices[1],
 				polygons[i].vertices[2]);
@@ -2278,8 +2309,10 @@ bool multiBandTexturing(
 	UTimer timer;
 	std::string outputDirectory = UDirectory::getDir(outputOBJPath);
 	std::string tmpImageDirectory = outputDirectory+"/rtabmap_tmp_textures";
+	UDirectory::removeDir(tmpImageDirectory);
 	UDirectory::makeDir(tmpImageDirectory);
 	UINFO("Temporary saving images in directory \"%s\"...", tmpImageDirectory.c_str());
+	int viewId = 0;
 	for(std::map<int, Transform>::const_iterator iter = cameraPoses.lower_bound(1); iter!=cameraPoses.end(); ++iter)
 	{
 		int camId = iter->first;
@@ -2335,11 +2368,6 @@ bool multiBandTexturing(
 			UERROR("No camera models found for camera %d. Aborting multiband texturing...", iter->first);
 			return false;
 		}
-		else if(models.size() != 1)
-		{
-			UERROR("Unwrapping not supporting multi-camera yet... ignoring %d. Aborting multiband texturing...", iter->first);
-			return false;
-		}
 		if(image.empty())
 		{
 			UERROR("No image found for camera %d. Aborting multiband texturing...", iter->first);
@@ -2355,60 +2383,73 @@ bool multiBandTexturing(
 			image = image.clone();
 		}

-		UASSERT(models.size() == 1);
-		const CameraModel & model = models[0];
-		Transform t = iter->second * model.localTransform();
-		Eigen::Matrix<double, 3, 4> m = (t.inverse()).toEigen3d().matrix().block<3,4>(0, 0);
-		sfmData::CameraPose pose(geometry::Pose3(m), true);
-		sfmData.setAbsolutePose((IndexT)camId, pose);
-		cv::Size imageSize = model.imageSize();
-		if(imageSize.height == 0)
+		for(size_t i=0; i<models.size(); ++i)
 		{
-			// backward compatibility
-			imageSize.height = image.rows;
-			imageSize.width = image.cols;
+			const CameraModel & model = models.at(i);
+			cv::Size imageSize = model.imageSize();
+			if(imageSize.height == 0)
+			{
+				// backward compatibility
+				imageSize.height = image.rows;
+				imageSize.width = image.cols;
+			}
+			UASSERT(image.cols % imageSize.width == 0);
+			cv::Mat imageRoi = image.colRange(i*imageSize.width, (i+1)*imageSize.width);
+			if(gains.find(camId) != gains.end() &&
+			   gains.at(camId).find(i) != gains.at(camId).end())
+			{
+				const cv::Vec4d & g = gains.at(camId).at(i);
+				std::vector<cv::Mat> channels;
+				cv::split(imageRoi, channels);
+
+				// assuming BGR
+				cv::multiply(channels[0], g.val[gainRGB?3:0], channels[0]);
+				cv::multiply(channels[1], g.val[gainRGB?2:0], channels[1]);
+				cv::multiply(channels[2], g.val[gainRGB?1:0], channels[2]);
+
+				cv::Mat output;
+				cv::merge(channels, output);
+				imageRoi = output;
+			}
+
+			if(blendingGains.find(camId) != blendingGains.end() &&
+			   blendingGains.at(camId).find(i) != blendingGains.at(camId).end())
+			{
+				cv::Mat g = blendingGains.at(camId).at(i);
+				cv::Mat dst;
+				cv::blur(g, dst, cv::Size(3,3));
+				cv::Mat gResized;
+				cv::resize(dst, gResized, imageRoi.size(), 0, 0, cv::INTER_LINEAR);
+				cv::Mat output;
+				cv::multiply(imageRoi, gResized, output, 1.0, CV_8UC3);
+				imageRoi = output;
+			}
+
+			Transform t = iter->second * model.localTransform();
+			Eigen::Matrix<double, 3, 4> m = (t.inverse()).toEigen3d().matrix().block<3,4>(0, 0);
+			sfmData::CameraPose pose(geometry::Pose3(m), true);
+			sfmData.setAbsolutePose((IndexT)viewId, pose);
+
+			std::shared_ptr<camera::IntrinsicBase> camPtr = std::make_shared<camera::Pinhole>(
+					imageSize.width, imageSize.height, model.fx(), model.cx(), model.cy());
+			sfmData.intrinsics.insert(std::make_pair((IndexT)viewId, camPtr));
+
+			std::string imagePath = tmpImageDirectory+uFormat("/%d.jpg", viewId);
+
+			cv::imwrite(imagePath, imageRoi);
+
+			std::shared_ptr<sfmData::View> viewPtr = std::make_shared<sfmData::View>(
+					imagePath,
+					(IndexT)viewId,
+					(IndexT)viewId,
+					(IndexT)viewId,
+					imageSize.width,
+					imageSize.height);
+			sfmData.views.insert(std::make_pair((IndexT)viewId, viewPtr));
+			++viewId;
 		}
-		std::shared_ptr<camera::IntrinsicBase> camPtr(new camera::Pinhole(imageSize.width, imageSize.height, model.fx(), model.cx(), model.cy()));
-		sfmData.intrinsics.insert(std::make_pair((IndexT)camId, camPtr));
-
-		std::string imagePath = tmpImageDirectory+uFormat("/%d.jpg", camId);
-
-		if(gains.find(camId) != gains.end())
-		{
-			UASSERT(gains.at(camId).size() == 1);
-			const cv::Vec4d & g = gains.at(camId).begin()->second;
-			std::vector<cv::Mat> channels;
-			cv::split(image, channels);
-
-			// assuming BGR
-			cv::multiply(channels[0], g.val[3], channels[0]);
-			cv::multiply(channels[1], g.val[2], channels[1]);
-			cv::multiply(channels[2], g.val[1], channels[2]);
-
-			cv::merge(channels, image);
-		}
-		if(blendingGains.find(camId) != blendingGains.end())
-		{
-			UASSERT(blendingGains.at(camId).size() == 1);
-			cv::Mat g = blendingGains.at(camId).begin()->second;
-			cv::Mat dst;
-			cv::blur(g, dst, cv::Size(3,3));
-			cv::Mat gResized;
-			cv::resize(dst, gResized, image.size(), 0, 0, cv::INTER_LINEAR);
-			cv::multiply(image, gResized, image, 1.0, CV_8UC3);
-		}
-		cv::imwrite(imagePath, image);
-
-		sfmData.views.insert(std::make_pair((IndexT)camId,
-				new sfmData::View(
-						imagePath,
-						(IndexT)camId,
-						(IndexT)camId,
-						(IndexT)camId,
-						imageSize.width,
-						imageSize.height)));
 	}
-	UINFO("Temporary saving images in directory \"%s\"... done. %fs", tmpImageDirectory.c_str(), timer.ticks());
+	UINFO("Temporary saving images in directory \"%s\"... done (%d images). %fs", tmpImageDirectory.c_str(), viewId, (int)cameraPoses.size(), timer.ticks());

 	mvsUtils::MultiViewParams mp(sfmData);

@@ -2455,6 +2496,7 @@ bool multiBandTexturing(
 			{
 				UASSERT(img.channels() == 3);
 				// Re-use same contrast values with all images
+				UINFO("Apply contrast values %f %f", contrastValues.first, contrastValues.second);
 				img.convertTo(img, -1, contrastValues.first, contrastValues.second);
 			}
 			std::string newName = *iter;
@@ -2479,6 +2521,10 @@ bool multiBandTexturing(
 	fo.close();
 	UINFO("Rename/convert textures... done. %fs", timer.ticks());

+#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
+	sfmData.clear();
+#endif
+
 	return true;
 #else
 	UERROR("Cannot unwrap texture mesh. RTAB-Map is not built with Alice Vision support! Returning false.");
@@ -2972,7 +3018,7 @@ float computeNormalsComplexity(
 			{
 				*pcaEigenValues = pca_analysis.eigenvalues;
 			}
-
+			UASSERT((is2d && pca_analysis.eigenvalues.total()>=2) || (!is2d && pca_analysis.eigenvalues.total()>=3));
 			// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
 			return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
 		}
@@ -3096,6 +3142,62 @@ float computeNormalsComplexity(
 	return 0.0f;
 }

+float computeNormalsComplexity(
+		const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
+		const Transform & t,
+		bool is2d,
+		cv::Mat * pcaEigenVectors,
+		cv::Mat * pcaEigenValues)
+{
+	 //Construct a buffer used by the pca analysis
+	int sz = static_cast<int>(cloud.size()*2);
+	cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
+	int oi = 0;
+	bool doTransform = false;
+	Transform tn;
+	if(!t.isIdentity())
+	{
+		tn = t.rotation();
+		doTransform = true;
+	}
+	for (unsigned int i = 0; i < cloud.size(); ++i)
+	{
+		const pcl::PointXYZINormal & pt = cloud.at(i);
+		cv::Point3f n(pt.normal_x, pt.normal_y, pt.normal_z);
+		if(doTransform)
+		{
+			n = util3d::transformPoint(n, tn);
+		}
+		if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
+		{
+			float * ptr = data_normals.ptr<float>(oi++, 0);
+			ptr[0] = n.x;
+			ptr[1] = n.y;
+			if(!is2d)
+			{
+				ptr[2] = n.z;
+			}
+		}
+	}
+	if(oi>1)
+	{
+		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
+
+		if(pcaEigenVectors)
+		{
+			*pcaEigenVectors = pca_analysis.eigenvectors;
+		}
+		if(pcaEigenValues)
+		{
+			*pcaEigenValues = pca_analysis.eigenvalues;
+		}
+
+		// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
+		return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
+	}
+	return 0.0f;
+}
+
 float computeNormalsComplexity(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
 		const Transform & t,
@@ -3255,6 +3357,13 @@ LaserScan adjustNormalsToViewPoint(
 		const LaserScan & scan,
 		const Eigen::Vector3f & viewpoint,
 		bool forceGroundNormalsUp)
+{
+	return adjustNormalsToViewPoint(scan, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
+}
+LaserScan adjustNormalsToViewPoint(
+		const LaserScan & scan,
+		const Eigen::Vector3f & viewpoint,
+		float groundNormalsUp)
 {
 	if(scan.size() && !scan.is2d() && scan.hasNormals())
 	{
@@ -3262,6 +3371,7 @@ LaserScan adjustNormalsToViewPoint(
 		int ny = nx+1;
 		int nz = ny+1;
 		cv::Mat output = scan.data().clone();
+		#pragma omp parallel for
 		for(int i=0; i<scan.size(); ++i)
 		{
 			float * ptr = output.ptr<float>(0, i);
@@ -3272,7 +3382,7 @@ LaserScan adjustNormalsToViewPoint(

 				float result = v.dot(n);
 				if(result < 0
-				 || (forceGroundNormalsUp && ptr[nz] < -0.8 && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
+				 || (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
 				{
 					//reverse normal
 					ptr[nx] *= -1.0f;
@@ -3293,10 +3403,11 @@ LaserScan adjustNormalsToViewPoint(
 	return scan;
 }

-void adjustNormalsToViewPoint(
-		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+template<typename PointNormalT>
+void adjustNormalsToViewPointImpl(
+		typename pcl::PointCloud<PointNormalT>::Ptr & cloud,
 		const Eigen::Vector3f & viewpoint,
-		bool forceGroundNormalsUp)
+		float groundNormalsUp)
 {
 	for(unsigned int i=0; i<cloud->size(); ++i)
 	{
@@ -3308,7 +3419,7 @@ void adjustNormalsToViewPoint(

 			float result = v.dot(n);
 			if(result < 0
-			 || (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
+				|| (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
 			{
 				//reverse normal
 				cloud->points[i].normal_x *= -1.0f;
@@ -3319,30 +3430,49 @@ void adjustNormalsToViewPoint(
 	}
 }

+void adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		bool forceGroundNormalsUp)
+{
+	adjustNormalsToViewPoint(cloud, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
+}
+void adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		float groundNormalsUp)
+{
+	adjustNormalsToViewPointImpl<pcl::PointNormal>(cloud, viewpoint, groundNormalsUp);
+}
+
 void adjustNormalsToViewPoint(
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const Eigen::Vector3f & viewpoint,
 		bool forceGroundNormalsUp)
 {
-	for(unsigned int i=0; i<cloud->size(); ++i)
-	{
-		pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
-		if(pcl::isFinite(normal))
-		{
-			Eigen::Vector3f v = viewpoint - cloud->points[i].getVector3fMap();
-			Eigen::Vector3f n(normal.x, normal.y, normal.z);
+	adjustNormalsToViewPoint(cloud, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
+}
+void adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		float groundNormalsUp)
+{
+	adjustNormalsToViewPointImpl<pcl::PointXYZRGBNormal>(cloud, viewpoint, groundNormalsUp);
+}

-			float result = v.dot(n);
-			if(result < 0
-				|| (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
-			{
-				//reverse normal
-				cloud->points[i].normal_x *= -1.0f;
-				cloud->points[i].normal_y *= -1.0f;
-				cloud->points[i].normal_z *= -1.0f;
-			}
-		}
-	}
+void adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		bool forceGroundNormalsUp)
+{
+	adjustNormalsToViewPoint(cloud, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
+}
+void adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		float groundNormalsUp)
+{
+	adjustNormalsToViewPointImpl<pcl::PointXYZINormal>(cloud, viewpoint, groundNormalsUp);
 }

 void adjustNormalsToViewPoints(
@@ -138,6 +138,7 @@ do
       --Mem/UseOdomFeatures false \
       --Mem/BinDataKept false \
       --Rtabmap/CreateIntermediateNodes false\
+       --Vis/CorNNDR 0.6 \
       $V203_params\
       $F2F_params\
       --OdomORBSLAM2/VocPath /root/ORBvoc.txt\
@@ -124,6 +124,7 @@ do
       --Mem/STMSize 30\
       --Mem/UseOdomFeatures false \
       --Mem/BinDataKept false \
+       --Vis/CorNNDR 0.6 \
       $SCAN \
       --gt $KITTI_ROOT_PATH"/devkit/cpp/data/odometry/poses/$d.txt"\
       --output "$KITTI_RESULTS_PATH/$d"\
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
matlabbe	839045a538	package.xml: 0.20.7	2020-12-12 19:05:06 -05:00
matlabbe	a824945816	package.xml: removed libvtk-qt as there are conflicts on noetic with libpcl-dev (which is using vtk7)	2020-12-12 17:32:30 -05:00
matlabbe	1acf8ff193	RegIcp: don't assert if normals cannot be computed properly before doing complexity check, just reject the transform	2020-12-12 16:53:07 -05:00
matlabbe	51826c9a97	RegIcp: fixed KDTreeMatcherIntensity build error on Mac.	2020-12-11 17:22:29 -05:00
matlabbe	7a5d04062d	vtk8: disabled vtkOutputWindow	2020-12-11 17:05:37 -05:00
matlabbe	049238dd59	Gui: added L515 icon	2020-12-11 09:56:17 -05:00
42Max42	3271723034	superpoint, fixing slow convert after cnn (#659 ) * fixing slow convert after cnn * restoring cmake	2020-12-11 09:27:18 -05:00
matlabbe	da8e76ffed	rtabmap-matcher: added --calibration_to option (to use a different calibration file for the second image).	2020-12-05 13:21:58 -05:00
matlabbe	3131474102	Added Db/TargetVersion parameter (#652 )	2020-12-03 15:45:40 -05:00
matlabbe	3ffc8ce73f	DBViewer: enabled Reject button on landmark links	2020-11-30 23:17:47 -05:00
matlabbe	ee44adeb1f	fixed #648 (build error without OctoMap dependency)	2020-11-30 12:33:08 -05:00
matlabbe	96c1c81e22	fixed build with older libpointmatcher versions <10300	2020-11-29 19:30:30 -05:00
matlabbe	0022b6b993	ExportCloudsDialog: added colormap option when showing scans	2020-11-28 18:02:54 -05:00
matlabbe	721e046f5d	fixed build	2020-11-28 17:36:42 -05:00
matlabbe	d733029565	Increased version to 0.20.7. OdometryF2M: added support for intensity field, removed ignored key frames when there is low scan complexity. RegistrationIcp: added Icp/PMMatcherIntensity, Icp/PointToPlaneGroundNormalsUp and Icp/PointToPlaneLowComplexityStrategy parameters. Rtabmap: when graph optimized from end, increased optimization error before warning that resulting map correction is not identity (this could happen with GTSAM as the root is not perfectly fixed). CloudViewer: added coordinate frame scaling option, added rainbow colormap option for scan intensity. DBViewer: fixed local proximity merged scans not shown modified after refining those links, show intensity, fixed constraints view not updated after rejecting a link. MainWindow: added intesity support with odometry scans.	2020-11-28 17:28:34 -05:00
matlabbe	7859313beb	AppVeyor: updated realsense2 sdk to 2.40. CameraRealSense2: When GlobalTimeSync option is off, don't wait 35 ms for imu (and fails), just take the latest one directly (https://github.com/introlab/rtabmap/issues/614#issuecomment-732244439 ).	2020-11-23 11:36:27 -05:00
matlabbe	80199f23b5	RegIcp: complexity, checking if second eigen value is also under Icp/PointToPlaneMinComplexity to limit to only one axis. DbViewer: fixed refine link with scans having RGB channel.	2020-11-22 19:32:01 -05:00
matlabbe	bce7ae6acd	Fixed GTSAM reference frame yaw drift over time when gravity links are used	2020-11-22 16:40:30 -05:00
matlabbe	f88845e138	fixed #643	2020-11-22 14:29:05 -05:00
matlabbe	bdc7be40b4	Fix for previous commit https://github.com/introlab/rtabmap/commit/e4cb59b74d69887fd67700420d408db5a6850663 (otherwise imu are ignored if globalTimeSync is off)	2020-11-22 12:38:21 -05:00
matlabbe	e4cb59b74d	Added suggestion from https://github.com/introlab/rtabmap/issues/614#issuecomment-731769818	2020-11-22 12:32:39 -05:00
matlabbe	dab407e5b9	fixed https://github.com/introlab/rtabmap/commit/ad44b65a28c0bb16add80a3d0afb9c721f84ba68#commitcomment-44427459	2020-11-22 11:10:35 -05:00
matlabbe	98a499b603	Optimizer: fall back on g2o or gtsam first if one or the other is not available (instead of going TORO). ExportClouds: added ceiling and floor filtering options.	2020-11-22 01:23:08 -05:00
matlabbe	f5d7dc2814	PreferencesDialog: Added Marker/MaxRange and Marker/MinRange parameters to UI.	2020-11-21 17:23:41 -05:00
matlabbe	ab1aa5578a	Fixed build	2020-11-21 17:12:09 -05:00
matlabbe	ccbdb586da	DbViewer: Added datbabase path to window title	2020-11-21 16:37:40 -05:00
David Molina John Paul Soliva Tim Fronsee	f467f2af7f	Added Marker/MaxRange and Marker/MinRange parameters (#630 ) * ADD 3 meter limit for marker detections * ADD Marker/MaxRange and Marker/MinRange parameters for controlling marker detection ADD ctags ignore Co-authored-by: John Paul Soliva <soliva@seaos.co.jp> Co-authored-by: Tim Fronsee <tfronsee21@gmail.com>	2020-11-21 16:18:22 -05:00
matlabbe	7c4d2bbdf4	DbViewer: before resetting all changes, added a confirmation message box!	2020-11-21 16:15:27 -05:00
matlabbe	fb206b4f1e	DbViewer: fixed scan disappearing after editing constraint	2020-11-21 14:35:29 -05:00
matlabbe	ddecefbb9c	💄	2020-11-21 12:34:17 -05:00
matlabbe	ad44b65a28	Allow partial support for AliceVision v2.3.0 (see #564 for remaining issues)	2020-11-20 13:42:56 -05:00
matlabbe	fdaaa6ccfa	Fixed bug L500 gyro/acc not detected (#629 )	2020-11-19 13:49:10 -05:00
matlabbe	34e1af7e22	RealSense2: added error message for L515 if resolution is not 640x480 30 fps (#629 )	2020-11-19 10:38:15 -05:00
matlabbe	54e2688a1d	Fixed build with pcl > 1.11.1 (#641 )	2020-11-14 16:52:01 -05:00
matlabbe	f9abcf9e35	fixed opencv2 build	2020-11-14 14:41:37 -05:00
matlabbe	01eb57f293	CameraImages: added configForEachFrame option (added to GUI too). CameraThread: for decimation, if depth is smaller than RGB, RGB is decimated first and if the resulting RGB image is smaller than the original depth, we then decimate the depth.	2020-11-14 13:39:12 -05:00
matlabbe	7be22d1b67	Added check to make sure input odometry poses are invertible. Source/DB: added stereo to depth option.	2020-11-06 21:37:59 -05:00
matlabbe	4b527f9c36	Update .appveyor.yml	2020-11-06 18:56:32 -05:00
matlabbe	4d965c2089	Update .appveyor.yml	2020-11-06 18:18:06 -05:00
matlabbe	47cbd633c3	Update .appveyor.yml	2020-11-06 18:11:29 -05:00
matlabbe	b95537a680	Added c++11 definition when latest libpointmatcher is found	2020-11-06 16:37:29 -05:00
matlabbe	e102243f0e	Fixed weight=-8 bug when moving rehearsed node to trash	2020-11-06 14:47:10 -05:00
matlabbe	e7a2f206a0	Gui: fixed warning scan not found when uncompressing data. Don't disable Mem/UseOdomFeatures checkbox anymore in monitoring mode (ROS).	2020-11-05 15:50:05 -05:00
matlabbe	d04b1a13be	multiband: add multi-camera support. rtabmap-export: updated options (now supporting creating point cloud from scans).	2020-11-04 14:02:18 -05:00
matlabbe	92b1dabf1c	MainWindow: avoid uncompressing images/scans if they are not shown	2020-11-03 16:12:43 -05:00
matlabbe	e269067d4c	Texturing: add distanceToCamPolicy option	2020-11-03 16:11:37 -05:00
matlabbe	596cd10b69	Fixed -lBoost::timer not found on ubuntu18.04/arm64 (#587 )	2020-11-01 13:08:55 -05:00
matlabbe	6a730b51c7	GUI-Preferences: disabled Daisy feature option on OpenCV 2	2020-11-01 13:01:23 -05:00
matlabbe	4ecf37a3ab	fixed build with OpenCV 2.4	2020-11-01 12:49:21 -05:00
matlabbe	99275fba1d	Added Daisy descriptor. ORB: updated default parameters. Making ORBOctree using ORB parameters. Updated Vis/CorNNDR default from 0.6 to 0.8 (increase number of matches with binary descriptors, increase slightly feature matching time with float descriptors). Note that jfr2018 scripts have been updated to use old value 0.6. rtabmap-info: show descriptor dimension and type.	2020-11-01 11:28:59 -05:00
matlabbe	25c2a51ee3	fixed windows build	2020-10-29 18:03:36 -04:00
matlabbe	600484e12c	Update .appveyor.yml #624	2020-10-29 11:52:25 -04:00
matlabbe	8878d9fcdf	Update .appveyor.yml #624	2020-10-29 11:44:31 -04:00
matlabbe	72e1649cdd	DataRecorder: added imu filtering by default	2020-10-22 18:02:24 -04:00
matlabbe	afbc0edbd6	Local occupancy grid: fixed empty obstacles with scans having intensity channel when Grid/RangeMax is used.	2020-10-21 15:38:19 -04:00
matlabbe	fbc30042c4	Fixed biggest index/min/max when Grid/MaxGroundHeight is set (http://official-rtab-map-forum.67519.x6.nabble.com/Comparison-between-realsense-D435-vs-T265-vs-T265-D435-dual-setup-td6456i20.html )	2020-10-18 15:56:13 -04:00
matlabbe	a4da1e14b4	SIFT: fixed SIFT not extracted with OpenCV >=4.4 and >=3.4.11 when nonfree is false	2020-10-17 20:34:24 -04:00
matlabbe	3047b7da6b	CameraRealSense2: update for L515 support	2020-10-17 19:57:27 -04:00
matlabbe	bbb3c56008	Fixed android build	2020-10-16 14:55:50 -04:00
matlabbe	f71f00277c	DbViewer::AddConstraint: don't ask ofr manual constraint if already aborted previous question (when Reg/Strategy=1).	2020-10-12 17:41:06 -04:00
matlabbe	0e5ec91280	DbViewer: added option to ignore landmarks. Show landmark id in Constraints View.	2020-10-12 15:29:20 -04:00
matlabbe	3aae79270f	Output an error if a timestamp file is missing instead of asserting #613	2020-10-12 14:30:57 -04:00
matlabbe	1b4a992b55	Merge branch 'master' of https://github.com/introlab/rtabmap into devel	2020-10-12 13:16:08 -04:00
matlabbe	384b120d2e	Fixed landmark ignored when Rtabmap/StartNewMapOnLoopClosure is true and a loop closure is rejected (before graph optimization)	2020-10-12 13:05:14 -04:00
matlabbe	304c365ae6	`Kp/ByteToFloat`: Changed default of false Based on http://official-rtab-map-forum.67519.x6.nabble.com/ZED2-rtabmap-loop-closure-and-twisted-map-tp6986p7002.html	2020-10-12 11:04:59 -04:00
matlabbe	d5572d03ad	DbViewer: added checkbox to enable Add for nodes not linked to graph	2020-10-09 12:15:33 -04:00
matlabbe	6d552b7873	DbViewer: fixed depth of features not shown	2020-10-09 11:10:48 -04:00
matlabbe	32eb266c79	fixed CMP0020 cmake warning	2020-10-09 11:10:21 -04:00
matlabbe	2cb509c825	DbViewer: manual constraints can now be added (http://official-rtab-map-forum.67519.x6.nabble.com/Manually-adding-modifying-node-constraints-td6983.html )	2020-10-07 15:41:13 -04:00
matlabbe	168c87b5ba	Added statitic Timing/RAM_estimation/. Updated VWDictionary::getMemoryUsed().	2020-10-05 22:11:28 -04:00
matlabbe	a80e062bef	Fixed build error in Trusty	2020-10-05 18:17:18 -04:00
matlabbe	9e321971ef	Removed debug log	2020-10-05 17:49:26 -04:00
matlabbe	8260733b11	fixed detectMoreLoopClosures when RGBD/LocalBundleOnLoopClosure is true	2020-10-05 17:47:13 -04:00
matlabbe	bbccbd63e4	Increased version to 0.20.5 Refactored how features are stored in Signature (significative memory optimization, causing major refactoring in Memory, RegistrationVis, OdometryF2M) FLANN: optimized memory usage when Kp/IncrementalFlann is false Added memory usage functions Added statistics Loop/Visual_inliers_ratio/ and Memory/RAM_estimated/MB EpipolarGeometry: templated findPairs functions graph::filterLinks: added inverted option LocalBundleOnLoopClosure: Force to use only neighbor links MainWindow: fixed max depth filtering for map's features Rtabmap::getSignatureCopy() fixed links not returned Added UPlot::getAllCurveDataAsText() function. DbViewer: fixed features not rendered in right view when failing ro refine a constraint report: added --export and --export_prefix options (to export figures data)	2020-10-05 17:34:32 -04:00
matlabbe	bedc771fa4	Rtabmap: added getNodesInRadius() public functions	2020-09-28 12:03:42 -04:00
matlabbe	933ac736f1	Increased version 0.20.4. Added parameter Kp/ByteToFloat (default true to use less memory with kdtree and binary descriptors). Memory/Sqlite3: Setting weight to -9 for invalid nodes (to make sure they are not reloaded from database, to fix a graph reduced issue). Added 12-SURF/FREAK detector approach. rtabmap-info: added number of nodes in each sessions.	2020-09-28 10:29:14 -04:00
matlabbe	c5158cade5	Fixed loop closure rejected if the camera on the robot changes orientation accordingly to base frame (http://official-rtab-map-forum.67519.x6.nabble.com/Mapping-with-multiple-stereocameras-localization-with-multiple-monochrome-monocular-cameras-tp6647p6953.html ). The reason why that check was there was because ICP flipping 180 deg in some cases, but should be already detected with that commit https://github.com/introlab/rtabmap/commit/7a1cf84b0845f430fdaf0800c0960c9795a73ec4#diff-b65a61c23197f7ff77a877f62b630b2cR283 .	2020-09-27 14:38:36 -04:00
matlabbe	eef0a23b1b	Reprocess: added --skip option (to skip frames when reprocessing). Rtabmap: always save Loop/Map_id/ stat. UPlot: export NaN insted of NA for invalid values (compatible with octave). GraphViewer and ImageView: added support for PDF output format, also changed default directory to User's Pictures standard folder.	2020-09-21 19:32:48 -04:00
matlabbe	3d33370e4c	DbViewer: fixed error log when showing landmarks in Constraints view.	2020-09-15 17:12:00 -04:00
matlabbe	4e6e404951	Fixed "[Setjac] infinite jac" error when Vis/ForwardEstOnly is false. OptimizerG2O: Added a check to ignore invalid 3d points.	2020-09-15 15:18:49 -04:00
matlabbe	29368ebdb3	report: fixed database order in localization results	2020-09-15 14:06:23 -04:00
matlabbe	a24211583d	Rtabmap: init Bayes prediction when loading a database	2020-09-11 10:55:33 -04:00
matlabbe	d50b33a7b8	Preferences: updated warnings for free sift (#596 )	2020-09-10 17:56:49 -04:00
matlabbe	a689d8a23f	report: fixed how localization stats are split against localization sessions only	2020-09-10 14:09:27 -04:00
matlabbe	bd80811ea1	Stats: added Loop/Distance_since_last_loc	2020-09-10 14:07:53 -04:00
matlabbe	cabf03af44	Memory: fixed keypoints3d wrongly copied when not using odom features	2020-09-08 21:24:47 -04:00
matlabbe	f93cdc31a6	Gui/source/K4A: added more description	2020-09-04 15:12:05 -04:00
matlabbe	e0973fea92	CameraK4A: added imu support with playback. Fixed windows build error.	2020-09-04 14:10:32 -04:00
matlabbe	49cb470b8d	CameraK4A: refactored to unify playback and real device code. Fixed ir option (with rectification).	2020-09-04 12:48:18 -04:00
matlabbe	d2784095a3	Added K4A status on --version. About: added Mynteye status	2020-09-04 10:39:15 -04:00
matlabbe	1db17dd118	rtabmap-report: added loc_delay option. Db info: added unused space	2020-09-03 15:23:48 -04:00
matlabbe	23d8353540	Rtabmap::detectMoreLoopClosures, use optimized graph as guess if RGBD/ProximityOdomGuess is true. Reprocess: added options to generate scan from depth image and/or pre-process input scans.	2020-08-28 16:24:07 -04:00
matlabbe	96a628877c	reprocess: when merging databases, return number of sessions merged when program exists. export: added --save_in_db option.	2020-08-28 12:29:16 -04:00
matlabbe	24052a6ebe	fixed pcl::getAngle3D not found error on latest pcl version (https://github.com/PointCloudLibrary/pcl/commit/6df3e602a72ea16657f901c9a6911d95b263ba08#diff-8e08415b9972a447d115dbca3f8fa0a1 )	2020-08-25 10:56:30 -04:00
matlabbe	775c9318d3	Fixed Too large rotation detected message	2020-08-21 15:57:28 -04:00
matlabbe	f263d560b4	OdomF2M: fixed complexity null for first scan is not having normals already	2020-08-21 14:32:08 -04:00
matlabbe	729f96f467	TORO: added warnings when prior or landmark links are detected	2020-08-21 11:26:22 -04:00
matlabbe	8d63e9eae1	Fixed 'cannot find -lBoost::thread' build error when building only with libpointmatcher dependency	2020-08-14 12:25:03 -04:00
matlabbe	39f68c44c6	Recovery: set back database in mapping mode after being saved in localization mode	2020-08-03 13:46:42 -04:00