package.xml: 0.20.7

* fixing slow convert after cnn

* restoring cmake

.appveyor.yml

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

.gitignore

@@ -8,3 +8,5 @@ app/android/.classpath
 app/android/.project
 app/android/AndroidManifest.xml
 app/android/res/raw/
+compile_flags.txt
+tags

CMakeLists.txt

@@ -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 5)
+SET(RTABMAP_PATCH_VERSION 7)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -429,7 +429,7 @@ IF(WITH_POINTMATCHER)
     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 REQUIRED)
+           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)
@@ -519,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)
 
@@ -626,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)
@@ -1213,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()

Version.h.in

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

app/android/jni/RTABMapApp.cpp

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

corelib/include/rtabmap/core/CameraThread.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;

corelib/include/rtabmap/core/DBDriver.h

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

corelib/include/rtabmap/core/DBDriverSqlite3.h

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

corelib/include/rtabmap/core/Features2d.h

@@ -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;
@@ -116,7 +118,10 @@ public:
 		kFeatureKaze=9,     //new 0.13.2
 		kFeatureOrbOctree=10, //new 0.19.2
 		kFeatureSuperPointTorch=11, //new 0.19.7
-		kFeatureSurfFreak=12}; //new 0.20.4
+		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){
@@ -146,6 +151,10 @@ public:
 			return "SUPERPOINT";
 		case kFeatureSurfFreak:
 			return "SURF+Freak";
+		case kFeatureGfttDaisy:
+			return "GFTT+Daisy";
+		case kFeatureSurfDaisy:
+			return "SURF+Daisy";
 		default:
 			return "Unknown";
 		}
@@ -563,6 +572,8 @@ private:
 private:
 	float scaleFactor_;
 	int nLevels_;
+	int patchSize_;
+	int edgeThreshold_;
 	int fastThreshold_;
 	int fastMinThreshold_;
 
@@ -593,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_ */

corelib/include/rtabmap/core/Graph.h

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

corelib/include/rtabmap/core/LaserScan.h

@@ -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();}

corelib/include/rtabmap/core/MarkerDetector.h

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

corelib/include/rtabmap/core/Memory.h

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

corelib/include/rtabmap/core/OctoMap.h

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

corelib/include/rtabmap/core/Parameters.h

@@ -244,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 12=SURF/FREAK.");
+    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 12=SURF/FREAK.");
+    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.");
@@ -265,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).");
@@ -300,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.");
@@ -587,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 12=SURF/FREAK.");
+    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 12=SURF/FREAK.");
+    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).");
@@ -603,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()));
@@ -643,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
@@ -660,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
@@ -753,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].");
@@ -840,4 +846,3 @@ private:
 }
 
 #endif /* PARAMETERS_H_ */
-

corelib/include/rtabmap/core/RegistrationIcp.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;
 };

corelib/include/rtabmap/core/Transform.h

@@ -98,6 +98,7 @@ public:
 
 	float theta() const;
 
+	bool isInvertible() const;
 	Transform inverse() const;
 	Transform rotation() const;
 	Transform translation() const;

corelib/include/rtabmap/core/camera/CameraImages.h

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

corelib/include/rtabmap/core/camera/CameraRGBDImages.h

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

corelib/include/rtabmap/core/camera/CameraRealSense2.h

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

corelib/include/rtabmap/core/impl/util3d_mapping.hpp

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

corelib/include/rtabmap/core/impl/util3d_surface.hpp

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

corelib/include/rtabmap/core/odometry/OdometryF2M.h

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

corelib/include/rtabmap/core/util2d.h

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

corelib/include/rtabmap/core/util3d_filtering.h

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

corelib/include/rtabmap/core/util3d_registration.h

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

corelib/include/rtabmap/core/util3d_surface.h

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

corelib/src/BayesFilter.cpp

@@ -420,13 +420,13 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
 unsigned long BayesFilter::getMemoryUsed() const
 {
 	long memoryUsage = sizeof(BayesFilter);
-	memoryUsage += _posterior.size() * (sizeof(float)+sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, float>);
+	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::_Rb_tree_node_base)) + sizeof(std::map<int, std::map<int, int> >);
+	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::_Rb_tree_node_base)) + sizeof(std::map<int, int>);
+		memoryUsage += iter->second.size() * (sizeof(int)*2+sizeof(std::map<int, int>::iterator)) + sizeof(std::map<int, int>);
 	}
 	return memoryUsage;
 }

corelib/src/CMakeLists.txt

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

corelib/src/CameraThread.cpp

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

corelib/src/DBDriver.cpp

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

corelib/src/DBDriverSqlite3.cpp

@@ -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());
 	}
@@ -5057,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
@@ -5252,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
@@ -5314,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());

corelib/src/DBReader.cpp

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

corelib/src/Features2d.cpp

@@ -509,55 +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 || type == Feature2D::kFeatureSurfFreak)
+
+  #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 ||
-	   type == Feature2D::kFeatureSurfFreak)
-	{
-		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
-	if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSurfFreak)
+  #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)
 	{
@@ -618,6 +632,12 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	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);
@@ -626,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
 
@@ -977,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
 }
@@ -986,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;
 }
@@ -1004,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...");
@@ -1022,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;
 }
 
@@ -1933,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);
 }
@@ -1948,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
@@ -2064,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;
+}
+
 }

corelib/src/FlannIndex.cpp

@@ -115,7 +115,7 @@ unsigned long FlannIndex::memoryUsed() const
 		return 0;
 	}
 	unsigned long memoryUsage = sizeof(FlannIndex);
-	memoryUsage += addedDescriptors_.size() * (sizeof(int) + sizeof(cv::Mat) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, cv::Mat>);
+	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)
 	{

corelib/src/Graph.cpp

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

corelib/src/MarkerDetector.cpp

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

corelib/src/Memory.cpp

@@ -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())
 		{
@@ -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
 					}
@@ -3513,7 +3513,7 @@ void Memory::dumpMemoryTree(const char * fileNameTree) const
 unsigned long Memory::getMemoryUsed() const
 {
 	unsigned long memoryUsage = sizeof(Memory);
-	memoryUsage += _signatures.size() * (sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Signature *>);
+	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);
@@ -3522,26 +3522,26 @@ unsigned long Memory::getMemoryUsed() const
 	{
 		memoryUsage += _vwd->getMemoryUsed();
 	}
-	memoryUsage += _stMem.size() * (sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
-	memoryUsage += _workingMem.size() * (sizeof(int)+sizeof(double)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, double>);
-	memoryUsage += _groundTruths.size() * (sizeof(int)+sizeof(Transform)+12*sizeof(float) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Transform>);
-	memoryUsage += _labels.size() * (sizeof(int)+sizeof(std::string) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::string>);
+	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::_Rb_tree_node_base)) + sizeof(std::map<int, std::set<int> >);
-	memoryUsage += _landmarksInvertedIndex.size() * (sizeof(int)+sizeof(std::set<int>) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::set<int> >);
+	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::_Rb_tree_node_base)) + sizeof(std::set<int>);
+		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::_Rb_tree_node_base)) + sizeof(std::set<int>);
+		memoryUsage+=iter->second.size()*(sizeof(int)+sizeof(std::set<int>::iterator)) + sizeof(std::set<int>);
 	}
-	memoryUsage += parameters_.size()*(sizeof(std::string)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(ParametersMap);
-	memoryUsage += sizeof(Feature2D) + _feature2D->getParameters().size()*(sizeof(std::string)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(ParametersMap);
+	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();
@@ -3660,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)
 		{
@@ -3725,6 +3729,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 				}
 				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);
@@ -4253,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 ||

corelib/src/OccupancyGrid.cpp

@@ -1535,9 +1535,9 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 unsigned long OccupancyGrid::getMemoryUsed() const
 {
 	unsigned long memoryUsage = sizeof(OccupancyGrid);
-	memoryUsage += parameters_.size()*(sizeof(std::string)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(ParametersMap);
+	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::_Rb_tree_node_base)) + sizeof(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >);
+	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();
@@ -1546,8 +1546,8 @@ unsigned long OccupancyGrid::getMemoryUsed() const
 	}
 	memoryUsage += map_.total() * map_.elemSize();
 	memoryUsage += mapInfo_.total() * mapInfo_.elemSize();
-	memoryUsage += cellCount_.size()*(sizeof(int)*3 + sizeof(std::pair<int, int>) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::pair<int, int> >);
-	memoryUsage += addedNodes_.size()*(sizeof(int) + sizeof(Transform)+ sizeof(float)*12 + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Transform>);
+	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())
 	{

corelib/src/OctoMap.cpp

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

corelib/src/Odometry.cpp

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

corelib/src/Optimizer.cpp

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

corelib/src/RegistrationIcp.cpp

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

corelib/src/Rtabmap.cpp

@@ -1071,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() &&
@@ -1238,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);
 						}
 					}
 				}
@@ -1353,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
@@ -3058,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())
@@ -3294,11 +3334,11 @@ bool Rtabmap::process(
 				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::_Rb_tree_node_base)) + sizeof(std::map<int, Transform>);
-				estimatedMemoryUsage += _constraints.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Link>);
+				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(std::_Rb_tree_node_base)) + sizeof(ParametersMap);
+				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);
 			}

corelib/src/Signature.cpp

@@ -345,11 +345,11 @@ cv::Mat Signature::getPoseCovariance() const
 unsigned long Signature::getMemoryUsed(bool withSensorData) const // Return memory usage in Bytes
 {
 	unsigned long total = sizeof(Signature);
-	total += _words.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::multimap<int, cv::KeyPoint>);
+	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::_Rb_tree_node_base)) + sizeof(std::map<int, int>);
+	total += _wordsChanged.size() * (sizeof(int)*2+sizeof(std::map<int, int>::iterator)) + sizeof(std::map<int, int>);
 	if(withSensorData)
 	{
 		total+=_sensorData.getMemoryUsed();
@@ -357,8 +357,8 @@ unsigned long Signature::getMemoryUsed(bool withSensorData) const // Return memo
 	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::_Rb_tree_node_base)) + sizeof(std::multimap<int, Link>);
-	total += _landmarks.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Link>);
+	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;
 }
 

corelib/src/Transform.cpp

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

corelib/src/VWDictionary.cpp

@@ -373,7 +373,7 @@ unsigned long VWDictionary::getMemoryUsed() const
 	memoryUsage += _dataTree.total()*_dataTree.elemSize();
 	if(!_visualWords.empty())
 	{
-		memoryUsage += _visualWords.size()*(sizeof(int) + _visualWords.rbegin()->second->getMemoryUsed() + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, VisualWord *>);
+		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)
@@ -385,12 +385,12 @@ unsigned long VWDictionary::getMemoryUsed() const
 	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::_Rb_tree_node_base)) + sizeof(std::map<int, VisualWord *>);
+		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::_Rb_tree_node_base)) + sizeof(std::map<int ,int>);
-	memoryUsage += _mapIdIndex.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int ,int>);
-	memoryUsage += _notIndexedWords.size() * (sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
-	memoryUsage += _removedIndexedWords.size() * (sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
+	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;
 }
 

corelib/src/VisualWord.cpp

@@ -72,8 +72,8 @@ int VisualWord::removeAllRef(int signatureId)
 unsigned long VisualWord::getMemoryUsed() const
 {
 	unsigned long memoryUsage = sizeof(VisualWord);
-	memoryUsage += _references.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int ,int>);
-	memoryUsage += _oldReferences.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int ,int>);
+	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;
 }

corelib/src/camera/CameraImages.cpp

@@ -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;
@@ -448,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
@@ -511,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())
 	{
@@ -558,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
 		{
@@ -585,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())
 				{
@@ -608,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)
@@ -698,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);
 				}
 			}
 
@@ -716,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.");
 					}
@@ -727,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);
@@ -742,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;

corelib/src/camera/CameraRGBDImages.cpp

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

corelib/src/camera/CameraRealSense2.cpp

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

corelib/src/imufilter/MadgwickFilter.cpp

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

corelib/src/odometry/OdometryF2M.cpp

@@ -591,14 +591,9 @@ Transform OdometryF2M::computeTransform(
 						  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;
@@ -955,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)
@@ -970,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>);
@@ -998,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
@@ -1058,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;
 											}
@@ -1071,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)
 										{
@@ -1086,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;
 									}
@@ -1100,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;
 						}
@@ -1311,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)
@@ -1357,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
@@ -1368,7 +1361,7 @@ Transform OdometryF2M::computeTransform(
 											util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
 											0,
 											0.0f,
-											LaserScan::kXYZNormal,
+											LaserScan::kXYZINormal,
 											newFramePose.translation()));
 						}
 

corelib/src/opencv/ORBextractor.cc

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

corelib/src/opencv/ORBextractor.h

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

corelib/src/optimizer/OptimizerGTSAM.cpp

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

corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h

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

corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h

@@ -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());
     }
 

corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h

@@ -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());
     }
 

corelib/src/pcl18/surface/impl/texture_mapping.hpp

@@ -1055,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)
@@ -1077,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);
@@ -1273,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))
 		{
@@ -1283,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))
 	{
@@ -1386,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;

corelib/src/pcl18/surface/texture_mapping.h

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

corelib/src/resources/backward_compatibility/DatabaseSchema_0_16_0.sql

@@ -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');
+
+

corelib/src/resources/backward_compatibility/DatabaseSchema_0_16_1.sql

@@ -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');
+
+

corelib/src/resources/backward_compatibility/DatabaseSchema_0_16_2.sql

@@ -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');
+
+

corelib/src/resources/backward_compatibility/DatabaseSchema_0_17_0.sql

@@ -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');
+
+

corelib/src/resources/backward_compatibility/DatabaseSchema_0_18_0.sql

@@ -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');
+
+

corelib/src/resources/backward_compatibility/DatabaseSchema_0_18_3.sql

@@ -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');
+
+

corelib/src/superpoint_torch/SuperPoint.cc

@@ -39,7 +39,7 @@ SuperPoint::SuperPoint()
 
         convDa(torch::nn::Conv2dOptions(c4, c5, 3).stride(1).padding(1)),
         convDb(torch::nn::Conv2dOptions(c5, d1, 1).stride(1).padding(0))
-        
+
   {
     register_module("conv1a", conv1a);
     register_module("conv1b", conv1b);
@@ -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;
@@ -289,7 +293,7 @@ void NMS(const std::vector<cv::KeyPoint> & ptsIn,
     confidence.setTo(0);
 
     for (size_t i = 0; i < pts_raw.size(); i++)
-    {   
+    {
         int uu = (int) pts_raw[i].x;
         int vv = (int) pts_raw[i].y;
 
@@ -308,7 +312,7 @@ void NMS(const std::vector<cv::KeyPoint> & ptsIn,
     cv::copyMakeBorder(grid, grid, dist_thresh, dist_thresh, dist_thresh, dist_thresh, cv::BORDER_CONSTANT, 0);
 
     for (size_t i = 0; i < pts_raw.size(); i++)
-    {   
+    {
     	// account for top left padding
         int uu = (int) pts_raw[i].x + dist_thresh;
         int vv = (int) pts_raw[i].y + dist_thresh;
@@ -356,7 +360,7 @@ void NMS(const std::vector<cv::KeyPoint> & ptsIn,
             }
         }
     }
-    
+
     if(!descriptorsIn.empty())
     {
     	UASSERT(descriptorsIn.rows == (int)ptsIn.size());

corelib/src/util2d.cpp

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

corelib/src/util3d.cpp

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

corelib/src/util3d_filtering.cpp

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

corelib/src/util3d_mapping.cpp

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

corelib/src/util3d_registration.cpp

@@ -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);
+}
+
 }
 
 }

corelib/src/util3d_surface.cpp

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

docker/jfr2018/run_euroc_datasets.sh

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

docker/jfr2018/run_kitti_datasets.sh

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

docker/jfr2018/run_tum_datasets.sh

@@ -83,6 +83,7 @@ do
        --Mem/UseOdomFeatures false \
        --Mem/BinDataKept true \
        --Rtabmap/CreateIntermediateNodes false\
+       --Vis/CorNNDR 0.6 \
        $F2F_params\
        --OdomORBSLAM2/VocPath /root/ORBvoc.txt\
        --OdomFovis/MinFeaturesForEstimate 10\

guilib/include/rtabmap/gui/CloudViewer.h

@@ -295,6 +295,8 @@ public:
 	void clearTrajectory();
 	bool isCameraAxisShown() const;
 	void setCameraAxisShown(bool shown);
+	double getCoordinateFrameScale() const;
+	void setCoordinateFrameScale(double scale);
 	bool isFrustumShown() const;
 	float getFrustumScale() const;
 	QColor getFrustumColor() const;
@@ -367,8 +369,10 @@ public:
 	void setNormalsStep(int step);
 	void setNormalsScale(float scale);
 	bool isIntensityRedColormap() const;
+	bool isIntensityRainbowColormap() const;
 	float getIntensityMax() const;
 	void setIntensityRedColormap(bool value);
+	void setIntensityRainbowColormap(bool value);
 	void setIntensityMax(float value);
 	void buildPickingLocator(bool enable);
 	const std::map<std::string, vtkSmartPointer<vtkOBBTree> > & getLocators() const {return _locators;}
@@ -412,6 +416,7 @@ private:
     QAction * _aSetTrajectorySize;
     QAction * _aClearTrajectory;
     QAction * _aShowCameraAxis;
+    QAction * _aSetFrameScale;
     QAction * _aShowFrustum;
     QAction * _aSetFrustumScale;
     QAction * _aSetFrustumColor;
@@ -422,6 +427,7 @@ private:
 	QAction * _aSetNormalsStep;
 	QAction * _aSetNormalsScale;
 	QAction * _aSetIntensityRedColormap;
+	QAction * _aSetIntensityRainbowColormap;
 	QAction * _aSetIntensityMaximum;
     QAction * _aSetBackgroundColor;
     QAction * _aSetRenderingRate;
@@ -462,6 +468,7 @@ private:
     double _renderingRate;
     vtkProp * _octomapActor;
     float _intensityAbsMax;
+    double  _coordinateFrameScale;
 };
 
 } /* namespace rtabmap */

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -249,6 +249,7 @@ public:
 	QString getSourceDevice() const;
 
 	bool isSourceDatabaseStampsUsed() const;
+	bool isSourceDatabaseStereoToDepth() const;
 	bool isSourceRGBDColorOnly() const;
 	int getIMUFilteringStrategy() const;
 	bool isDepthFilteringAvailable() const;
@@ -266,7 +267,7 @@ public:
 	double getSourceScanVoxelSize() const;
 	int getSourceScanNormalsK() const;
 	double getSourceScanNormalsRadius() const;
-	bool isSourceScanForceGroundNormalsUp() const;
+	double getSourceScanForceGroundNormalsUp() const;
 	Transform getSourceLocalTransform() const;    //Openni group
 	Transform getLaserLocalTransform() const; // directory images
 	Transform getIMULocalTransform() const; // directory images

guilib/src/CloudViewer.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/core/util2d.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/common/transforms.h>
 #include <QMenu>
@@ -130,11 +131,15 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
 		_frontfaceCulling(false),
 		_renderingRate(5.0),
 		_octomapActor(0),
-		_intensityAbsMax(0.0f)
+		_intensityAbsMax(0.0f),
+		_coordinateFrameScale(1.0)
 {
 	UDEBUG("");
 	this->setMinimumSize(200, 200);
-
+#if VTK_MAJOR_VERSION >= 8
+	vtkObject::GlobalWarningDisplayOff();
+#endif
+	
 	int argc = 0;
 	UASSERT(style!=0);
 	style->setCloudViewer(this);
@@ -264,6 +269,8 @@ void CloudViewer::createMenu()
 	_aShowCameraAxis= new QAction("Show base frame", this);
 	_aShowCameraAxis->setCheckable(true);
 	_aShowCameraAxis->setChecked(true);
+	_aSetFrameScale= new QAction("Set frame scale...", this);
+	_aShowCameraAxis->setChecked(true);
 	_aShowFrustum= new QAction("Show frustum", this);
 	_aShowFrustum->setCheckable(true);
 	_aShowFrustum->setChecked(false);
@@ -280,6 +287,9 @@ void CloudViewer::createMenu()
 	_aSetIntensityRedColormap = new QAction("Red/Yellow Colormap", this);
 	_aSetIntensityRedColormap->setCheckable(true);
 	_aSetIntensityRedColormap->setChecked(false);
+	_aSetIntensityRainbowColormap = new QAction("Rainbow Colormap", this);
+	_aSetIntensityRainbowColormap->setCheckable(true);
+	_aSetIntensityRainbowColormap->setChecked(false);
 	_aSetIntensityMaximum = new QAction("Set maximum absolute intensity...", this);
 	_aSetBackgroundColor = new QAction("Set background color...", this);	
 	_aSetRenderingRate = new QAction("Set rendering rate...", this);
@@ -338,8 +348,9 @@ void CloudViewer::createMenu()
 	normalsMenu->addAction(_aSetNormalsStep);
 	normalsMenu->addAction(_aSetNormalsScale);
 
-	QMenu * scanMenu = new QMenu("Scan", this);
+	QMenu * scanMenu = new QMenu("Scan color", this);
 	scanMenu->addAction(_aSetIntensityRedColormap);
+	scanMenu->addAction(_aSetIntensityRainbowColormap);
 	scanMenu->addAction(_aSetIntensityMaximum);
 
 	//menus
@@ -347,6 +358,7 @@ void CloudViewer::createMenu()
 	_menu->addMenu(cameraMenu);
 	_menu->addMenu(trajectoryMenu);
 	_menu->addAction(_aShowCameraAxis);
+	_menu->addAction(_aSetFrameScale);
 	_menu->addMenu(frustumMenu);
 	_menu->addMenu(gridMenu);
 	_menu->addMenu(normalsMenu);
@@ -400,12 +412,14 @@ void CloudViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("normals_scale", (double)this->getNormalsScale());
 
 	settings.setValue("intensity_red_colormap", this->isIntensityRedColormap());
+	settings.setValue("intensity_rainbow_colormap", this->isIntensityRainbowColormap());
 	settings.setValue("intensity_max", (double)this->getIntensityMax());
 
 	settings.setValue("trajectory_shown", this->isTrajectoryShown());
 	settings.setValue("trajectory_size", this->getTrajectorySize());
 
 	settings.setValue("camera_axis_shown", this->isCameraAxisShown());
+	settings.setValue("coordinate_frame_scale", this->getCoordinateFrameScale());
 
 	settings.setValue("frustum_shown", this->isFrustumShown());
 	settings.setValue("frustum_scale", this->getFrustumScale());
@@ -449,12 +463,14 @@ void CloudViewer::loadSettings(QSettings & settings, const QString & group)
 	this->setNormalsScale(settings.value("normals_scale", this->getNormalsScale()).toFloat());
 
 	this->setIntensityRedColormap(settings.value("intensity_red_colormap", this->isIntensityRedColormap()).toBool());
+	this->setIntensityRainbowColormap(settings.value("intensity_rainbow_colormap", this->isIntensityRainbowColormap()).toBool());
 	this->setIntensityMax(settings.value("intensity_max", this->getIntensityMax()).toFloat());
 
 	this->setTrajectoryShown(settings.value("trajectory_shown", this->isTrajectoryShown()).toBool());
 	this->setTrajectorySize(settings.value("trajectory_size", this->getTrajectorySize()).toUInt());
 
 	this->setCameraAxisShown(settings.value("camera_axis_shown", this->isCameraAxisShown()).toBool());
+	this->setCoordinateFrameScale(settings.value("coordinate_frame_scale", this->getCoordinateFrameScale()).toDouble());
 
 	this->setFrustumShown(settings.value("frustum_shown", this->isFrustumShown()).toBool());
 	this->setFrustumScale(settings.value("frustum_scale", this->getFrustumScale()).toDouble());
@@ -542,10 +558,10 @@ public:
 	typedef boost::shared_ptr<const PointCloudColorHandlerIntensityField > ConstPtr;
 
 	/** \brief Constructor. */
-	PointCloudColorHandlerIntensityField (const PointCloudConstPtr &cloud, float maxAbsIntensity = 0.0f, bool redYellowColormap = true) :
+	PointCloudColorHandlerIntensityField (const PointCloudConstPtr &cloud, float maxAbsIntensity = 0.0f, int colorMap = 0) :
 		pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloudColorHandler (cloud),
 		maxAbsIntensity_(maxAbsIntensity),
-		redColormap_(redYellowColormap)
+		colormap_(colorMap)
 		{
 		field_idx_  = pcl::getFieldIndex (*cloud, "intensity");
 		if (field_idx_ != -1)
@@ -562,12 +578,16 @@ public:
 	 * \return true if the operation was successful (the handler is capable and
 	 * the input cloud was given as a valid pointer), false otherwise
 	 */
-	virtual bool
-	getColor (vtkSmartPointer<vtkDataArray> &scalars) const
-	{
+#if PCL_VERSION_COMPARE(>, 1, 11, 1)
+	virtual vtkSmartPointer<vtkDataArray> getColor () const {
+		vtkSmartPointer<vtkDataArray> scalars;
+		if (!capable_ || !cloud_)
+			return scalars;
+#else
+	virtual bool getColor (vtkSmartPointer<vtkDataArray> &scalars) const {
 		if (!capable_ || !cloud_)
 			return (false);
-
+#endif
 		if (!scalars)
 			scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
 		scalars->SetNumberOfComponents (3);
@@ -632,11 +652,19 @@ public:
 			for(size_t k=0; k<j; ++k)
 			{
 				colors[k*3+0] = colors[k*3+1] = colors[k*3+2] = max>0?(unsigned char)(std::min(intensities[k]/max*255.0f, 255.0f)):255;
-				if(redColormap_)
+				if(colormap_ == 1)
 				{
 					colors[k*3+0] = 255;
 					colors[k*3+2] = 0;
 				}
+				else if(colormap_ == 2)
+				{
+					float r,g,b;
+					util2d::HSVtoRGB(&r, &g, &b, colors[k*3+0]*299.0f/255.0f, 1.0f, 1.0f);
+					colors[k*3+0] = r*255.0f;
+					colors[k*3+1] = g*255.0f;
+					colors[k*3+2] = b*255.0f;
+				}
 			}
 			reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (j);
 			reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, j*3, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
@@ -645,7 +673,11 @@ public:
 			reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (0);
 		//delete [] colors;
 		delete [] intensities;
+#if PCL_VERSION_COMPARE(>, 1, 11, 1)
+		return scalars;
+#else
 		return (true);
+#endif
 	}
 
 protected:
@@ -659,7 +691,7 @@ protected:
 
 private:
 	float maxAbsIntensity_;
-	bool redColormap_;
+	int colormap_; // 0=grayscale, 1=redYellow, 2=RainbowHSV
 };
 
 bool CloudViewer::addCloud(
@@ -724,7 +756,7 @@ bool CloudViewer::addCloud(
 		else if(hasIntensity)
 		{
 			//intensity
-			colorHandler.reset(new PointCloudColorHandlerIntensityField(binaryCloud, _intensityAbsMax, _aSetIntensityRedColormap->isChecked()));
+			colorHandler.reset(new PointCloudColorHandlerIntensityField(binaryCloud, _intensityAbsMax, _aSetIntensityRedColormap->isChecked()?1:_aSetIntensityRainbowColormap->isChecked()?2:0));
 			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
 		}
 		else if(previousColorIndex == 5)
@@ -1179,7 +1211,7 @@ bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth, bo
 							// Gradiant color on z axis
 							float H = (maxZ - pt.z())*299.0f/(maxZ-minZ);
 							float r,g,b;
-							OctoMap::HSVtoRGB(&r, &g, &b, H, 1, 1);
+							util2d::HSVtoRGB(&r, &g, &b, H, 1, 1);
 							pixel[0] = r*255.0f;
 							pixel[1] = g*255.0f;
 							pixel[2] = b*255.0f;
@@ -1542,10 +1574,10 @@ void CloudViewer::addOrUpdateCoordinate(
 	{
 		_coordinates.insert(id);
 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
-		_visualizer->addCoordinateSystem(scale, transform.toEigen3f(), id, foreground?3:2);
+		_visualizer->addCoordinateSystem(scale*_coordinateFrameScale, transform.toEigen3f(), id, foreground?3:2);
 #else
 		// Well, on older versions, just update the main coordinate
-		_visualizer->addCoordinateSystem(scale, transform.toEigen3f(), 0);
+		_visualizer->addCoordinateSystem(scale*_coordinateFrameScale, transform.toEigen3f(), 0);
 #endif
 	}
 }
@@ -2250,6 +2282,16 @@ void CloudViewer::setCameraAxisShown(bool shown)
 	_aShowCameraAxis->setChecked(shown);
 }
 
+double CloudViewer::getCoordinateFrameScale() const
+{
+	return _coordinateFrameScale;
+}
+
+void CloudViewer::setCoordinateFrameScale(double scale)
+{
+	_coordinateFrameScale = std::max(0.1, scale);
+}
+
 bool CloudViewer::isFrustumShown() const
 {
 	return _aShowFrustum->isChecked();
@@ -3296,6 +3338,10 @@ bool CloudViewer::isIntensityRedColormap() const
 {
 	return _aSetIntensityRedColormap->isChecked();
 }
+bool CloudViewer::isIntensityRainbowColormap() const
+{
+	return _aSetIntensityRainbowColormap->isChecked();
+}
 float CloudViewer::getIntensityMax() const
 {
 	return _intensityAbsMax;
@@ -3304,6 +3350,18 @@ float CloudViewer::getIntensityMax() const
 void CloudViewer::setIntensityRedColormap(bool on)
 {
 	_aSetIntensityRedColormap->setChecked(on);
+	if(on)
+	{
+		_aSetIntensityRainbowColormap->setChecked(false);
+	}
+}
+void CloudViewer::setIntensityRainbowColormap(bool on)
+{
+	_aSetIntensityRainbowColormap->setChecked(on);
+	if(on)
+	{
+		_aSetIntensityRedColormap->setChecked(false);
+	}
 }
 void CloudViewer::setIntensityMax(float value)
 {
@@ -3581,6 +3639,15 @@ void CloudViewer::handleAction(QAction * a)
 	{
 		this->setCameraAxisShown(a->isChecked());
 	}
+	else if(a == _aSetFrameScale)
+	{
+		bool ok;
+		double value = QInputDialog::getDouble(this, tr("Set frame scale"), tr("Scale"), _coordinateFrameScale, 0.1, 999.0, 1, &ok);
+		if(ok)
+		{
+			this->setCoordinateFrameScale(value);
+		}
+	}
 	else if(a == _aShowFrustum)
 	{
 		this->setFrustumShown(a->isChecked());
@@ -3669,10 +3736,14 @@ void CloudViewer::handleAction(QAction * a)
 			this->setIntensityMax(value);
 		}
 	}
-	else if(a == _aShowNormals)
+	else if(a == _aSetIntensityRedColormap)
 	{
 		this->setIntensityRedColormap(_aSetIntensityRedColormap->isChecked());
 	}
+	else if(a == _aSetIntensityRainbowColormap)
+	{
+		this->setIntensityRainbowColormap(_aSetIntensityRainbowColormap->isChecked());
+	}
 	else if(a == _aSetBackgroundColor)
 	{
 		QColor color = this->getDefaultBackgroundColor();

guilib/src/DatabaseViewer.cpp

@@ -79,6 +79,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/EditMapArea.h"
 #include "rtabmap/core/SensorData.h"
 #include "rtabmap/core/GainCompensator.h"
+#include "rtabmap/core/VisualWord.h"
 #include "rtabmap/gui/ExportDialog.h"
 #include "rtabmap/gui/EditConstraintDialog.h"
 #include "rtabmap/gui/ProgressDialog.h"
@@ -844,6 +845,7 @@ bool DatabaseViewer::openDatabase(const QString & path)
 				}
 
 				updateIds();
+				this->setWindowTitle("RTAB-Map Database Viewer - " + path + "[*]");
 				return true;
 			}
 		}
@@ -866,6 +868,7 @@ bool DatabaseViewer::openDatabase(const QString & path)
 
 bool DatabaseViewer::closeDatabase()
 {
+	this->setWindowTitle("RTAB-Map Database Viewer[*]");
 	if(dbDriver_)
 	{
 		if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size())
@@ -1999,8 +2002,28 @@ void DatabaseViewer::updateInfo()
 			{
 				ui_->textEdit_info->append(tr("Total odometry length:\t%1 m").arg(infoTotalOdom_));
 			}
+
+			int lastWordIdId = 0;
+			int wordsDim = 0;
+			int wordsType = 0;
+			dbDriver_->getLastWordId(lastWordIdId);
+			if(lastWordIdId>0)
+			{
+				std::set<int> ids;
+				ids.insert(lastWordIdId);
+				std::list<VisualWord *> vws;
+				dbDriver_->loadWords(ids, vws);
+				if(!vws.empty())
+				{
+					wordsDim = vws.front()->getDescriptor().cols;
+					wordsType = vws.front()->getDescriptor().type();
+					delete vws.front();
+					vws.clear();
+				}
+			}
+
 			ui_->textEdit_info->append(tr("Total time:\t\t%1").arg(QDateTime::fromMSecsSinceEpoch(infoTotalTime_*1000).toUTC().toString("hh:mm:ss.zzz")));
-			ui_->textEdit_info->append(tr("LTM:\t\t%1 nodes and %2 words").arg(ids_.size()).arg(dbDriver_->getTotalDictionarySize()));
+			ui_->textEdit_info->append(tr("LTM:\t\t%1 nodes and %2 words (dim=%3 type=%4)").arg(ids_.size()).arg(dbDriver_->getTotalDictionarySize()).arg(wordsDim).arg(wordsType==CV_8UC1?"8U":wordsType==CV_32FC1?"32F":uNumber2Str(wordsType).c_str()));
 			ui_->textEdit_info->append(tr("WM:\t\t%1 nodes and %2 words").arg(dbDriver_->getLastNodesSize()).arg(dbDriver_->getLastDictionarySize()));
 			ui_->textEdit_info->append(tr("Global graph:\t%1 poses and %2 links").arg(odomPoses_.size()).arg(links_.size()));
 			ui_->textEdit_info->append(tr("Ground truth:\t%1 poses").arg(groundTruthPoses_.size()));
@@ -3050,7 +3073,7 @@ void DatabaseViewer::viewOptimizedMesh()
 		return;
 	}
 
-	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
@@ -3102,7 +3125,7 @@ void DatabaseViewer::exportOptimizedMesh()
 		return;
 	}
 
-	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
@@ -3310,7 +3333,7 @@ void DatabaseViewer::updateOptimizedMesh()
 			else if(meshes.size())
 			{
 				dbDriver_->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
-				std::vector<std::vector<std::vector<unsigned int> > > polygons(1);
+				std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(1);
 				polygons.at(0) = util3d::convertPolygonsFromPCL(meshes.at(0)->polygons);
 				dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(meshes.at(0)->cloud, false).data(), polygons);
 				QMessageBox::information(this, tr("Update Optimized Mesh"), tr("Updated!"));
@@ -4040,14 +4063,21 @@ void DatabaseViewer::refineAllLinks(const QList<Link> & links)
 
 void DatabaseViewer::resetAllChanges()
 {
-	linksAdded_.clear();
-	linksRefined_.clear();
-	linksRemoved_.clear();
-	generatedLocalMaps_.clear();
-	generatedLocalMapsInfo_.clear();
-	modifiedLaserScans_.clear();
-	updateLoopClosuresSlider();
-	this->updateGraphView();
+	if(QMessageBox::question(this,
+			tr("Reset all changes"),
+			tr("You are about to reset all changes you've made so far, do you want to continue?"),
+			QMessageBox::Yes | QMessageBox::No,
+			QMessageBox::No) == QMessageBox::Yes)
+	{
+		linksAdded_.clear();
+		linksRefined_.clear();
+		linksRemoved_.clear();
+		generatedLocalMaps_.clear();
+		generatedLocalMapsInfo_.clear();
+		modifiedLaserScans_.clear();
+		updateLoopClosuresSlider();
+		this->updateGraphView();
+	}
 }
 
 void DatabaseViewer::sliderAValueChanged(int value)
@@ -5313,8 +5343,8 @@ void DatabaseViewer::editConstraint()
 				}
 				if(updated)
 				{
-					updateConstraintView();
 					this->updateGraphView();
+					updateConstraintView();
 				}
 			}
 		}
@@ -5345,8 +5375,8 @@ void DatabaseViewer::editConstraint()
 					newLink = newLink.inverse();
 				}
 				linksAdded_.insert(std::make_pair(newLink.from(), newLink));
-				updateLoopClosuresSlider(from, to);
 				this->updateGraphView();
+				updateLoopClosuresSlider(from, to);
 			}
 		}
 	}
@@ -5733,7 +5763,6 @@ void DatabaseViewer::updateConstraintView(
 		}
 
 		constraintsViewer_->removeCloud("scan2");
-		constraintsViewer_->removeCloud("scan2normals");
 		constraintsViewer_->removeGraph("scan2graph");
 		constraintsViewer_->removeCloud("scan0");
 		constraintsViewer_->removeCloud("scan1");
@@ -5741,7 +5770,8 @@ void DatabaseViewer::updateConstraintView(
 		{
 			//cloud 2d
 			if(link.type() == Link::kLocalSpaceClosure &&
-			   !link.userDataCompressed().empty())
+			   !link.userDataCompressed().empty() &&
+			   signatureTo.id()==0)
 			{
 				std::vector<int> ids;
 				cv::Mat userData = link.uncompressUserDataConst();
@@ -5827,6 +5857,8 @@ void DatabaseViewer::updateConstraintView(
 						Transform u = t * finalPoses.at(link.to()).inverse();
 						pcl::PointCloud<pcl::PointXYZ>::Ptr assembledScans(new pcl::PointCloud<pcl::PointXYZ>);
 						pcl::PointCloud<pcl::PointNormal>::Ptr assembledNormalScans(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::PointCloud<pcl::PointXYZI>::Ptr assembledIScans(new pcl::PointCloud<pcl::PointXYZI>);
+						pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledINormalScans(new pcl::PointCloud<pcl::PointXYZINormal>);
 						pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
 						for(std::map<int, Transform>::iterator iter=finalPoses.begin(); iter!=finalPoses.end(); ++iter)
 						{
@@ -5840,29 +5872,47 @@ void DatabaseViewer::updateConstraintView(
 								data.uncompressDataConst(0, 0, &scan, 0);
 								if(!scan.isEmpty())
 								{
-									if(scan.hasNormals())
+									if(scan.hasNormals() && scan.hasIntensity())
+									{
+										*assembledINormalScans += *util3d::laserScanToPointCloudINormal(scan, iter->second*scan.localTransform());
+									}
+									else if(scan.hasNormals())
 									{
 										*assembledNormalScans += *util3d::laserScanToPointCloudNormal(scan, iter->second*scan.localTransform());
 									}
+									else if(scan.hasIntensity())
+									{
+										*assembledIScans += *util3d::laserScanToPointCloudI(scan, iter->second*scan.localTransform());
+									}
 									else
 									{
 										*assembledScans += *util3d::laserScanToPointCloud(scan, iter->second*scan.localTransform());
 									}
 								}
 							}
-							graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
+							graph->push_back(util3d::transformPoint(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()), pose));
 						}
 
 						if(assembledNormalScans->size())
 						{
-							constraintsViewer_->addCloud("scan2normals", assembledNormalScans, pose, Qt::cyan);
-							constraintsViewer_->setCloudColorIndex("scan2normals", 2);
+							constraintsViewer_->addCloud("scan2", assembledNormalScans, pose, Qt::cyan);
+							constraintsViewer_->setCloudColorIndex("scan2", 2);
 						}
 						if(assembledScans->size())
 						{
 							constraintsViewer_->addCloud("scan2", assembledScans, pose, Qt::cyan);
 							constraintsViewer_->setCloudColorIndex("scan2", 2);
 						}
+						if(assembledINormalScans->size())
+						{
+							constraintsViewer_->addCloud("scan2", assembledINormalScans, pose, Qt::cyan);
+							constraintsViewer_->setCloudColorIndex("scan2", 2);
+						}
+						if(assembledIScans->size())
+						{
+							constraintsViewer_->addCloud("scan2", assembledIScans, pose, Qt::cyan);
+							constraintsViewer_->setCloudColorIndex("scan2", 2);
+						}
 						if(graph->size())
 						{
 							constraintsViewer_->addOrUpdateGraph("scan2graph", graph, Qt::cyan);
@@ -5876,13 +5926,27 @@ void DatabaseViewer::updateConstraintView(
 			constraintsViewer_->removeCloud("scan1");
 			if(!dataFrom.laserScanRaw().isEmpty())
 			{
-				if(dataFrom.laserScanRaw().hasNormals())
+				if(dataFrom.laserScanRaw().hasNormals() && dataFrom.laserScanRaw().hasIntensity())
+				{
+					pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan;
+					scan = rtabmap::util3d::laserScanToPointCloudINormal(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
+					constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
+					constraintsViewer_->setCloudColorIndex("scan0", 2);
+				}
+				else if(dataFrom.laserScanRaw().hasNormals())
 				{
 					pcl::PointCloud<pcl::PointNormal>::Ptr scan;
 					scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
 					constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
 					constraintsViewer_->setCloudColorIndex("scan0", 2);
 				}
+				else if(dataFrom.laserScanRaw().hasIntensity())
+				{
+					pcl::PointCloud<pcl::PointXYZI>::Ptr scan;
+					scan = rtabmap::util3d::laserScanToPointCloudI(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
+					constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
+					constraintsViewer_->setCloudColorIndex("scan0", 2);
+				}
 				else
 				{
 					pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
@@ -5893,13 +5957,27 @@ void DatabaseViewer::updateConstraintView(
 			}
 			if(!dataTo.laserScanRaw().isEmpty())
 			{
-				if(dataTo.laserScanRaw().hasNormals())
+				if(dataTo.laserScanRaw().hasNormals() && dataTo.laserScanRaw().hasIntensity())
+				{
+					pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan;
+					scan = rtabmap::util3d::laserScanToPointCloudINormal(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
+					constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
+					constraintsViewer_->setCloudColorIndex("scan1", 2);
+				}
+				else if(dataTo.laserScanRaw().hasNormals())
 				{
 					pcl::PointCloud<pcl::PointNormal>::Ptr scan;
 					scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
 					constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
 					constraintsViewer_->setCloudColorIndex("scan1", 2);
 				}
+				else if(dataTo.laserScanRaw().hasIntensity())
+				{
+					pcl::PointCloud<pcl::PointXYZI>::Ptr scan;
+					scan = rtabmap::util3d::laserScanToPointCloudI(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
+					constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
+					constraintsViewer_->setCloudColorIndex("scan1", 2);
+				}
 				else
 				{
 					pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
@@ -5941,6 +6019,7 @@ void DatabaseViewer::updateConstraintButtons()
 
 	if(ui_->label_type->text().toInt() == Link::kLandmark)
 	{
+		ui_->pushButton_reject->setEnabled(true);
 		return;
 	}
 
@@ -6977,7 +7056,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 			}
 		}
 	}
-	if(scanPoses.size())
+	if(scanPoses.size()>1)
 	{
 		//optimize the path's poses locally
 		Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
@@ -7035,6 +7114,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 		pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
 		pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>);
 		pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(new pcl::PointCloud<pcl::PointXYZINormal>);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledToRGBClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledToNormalRGBClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 		for(std::map<int, Transform>::const_iterator iter = filteredScanPoses.begin(); iter!=filteredScanPoses.end(); ++iter)
 		{
 			if(iter->first != currentLink.from())
@@ -7060,6 +7141,19 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 										toPoseInv * iter->second * scan.localTransform());
 							}
 						}
+						else if(scan.hasRGB())
+						{
+							if(scan.hasNormals())
+							{
+								*assembledToNormalRGBClouds += *util3d::laserScanToPointCloudRGBNormal(scan,
+										toPoseInv * iter->second * scan.localTransform());
+							}
+							else
+							{
+								*assembledToRGBClouds += *util3d::laserScanToPointCloudRGB(scan,
+										toPoseInv * iter->second * scan.localTransform());
+							}
+						}
 						else
 						{
 							if(scan.hasNormals())
@@ -7108,11 +7202,21 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 		{
 			assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
 		}
+		else if(assembledToNormalRGBClouds->size())
+		{
+			UASSERT(!fromScan.is2d());
+			assembledScan = util3d::laserScanFromPointCloud(*assembledToNormalRGBClouds);
+		}
+		else if(assembledToRGBClouds->size())
+		{
+			UASSERT(!fromScan.is2d());
+			assembledScan = util3d::laserScanFromPointCloud(*assembledToRGBClouds);
+		}
 		else
 		{
 			UWARN("Assembled scan is empty!");
 		}
-		SensorData assembledData;
+		SensorData assembledData(cv::Mat(), to);
 		// scans are in base frame but for 2d scans, set the height so that correspondences matching works
 		assembledData.setLaserScan(LaserScan(
 				assembledScan,
@@ -7121,8 +7225,9 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 				fromScan.format(),
 				fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
 
+		toS = new Signature(assembledData);
 		RegistrationIcp registrationIcp(parameters);
-		transform = registrationIcp.computeTransformation(fromS->sensorData(), assembledData, currentLink.transform(), &info);
+		transform = registrationIcp.computeTransformationMod(*fromS, *toS, currentLink.transform(), &info);
 		if(!transform.isNull())
 		{
 			// local scan matching proximity detection should have higher variance (see Rtabmap::process())
@@ -7757,22 +7862,21 @@ void DatabaseViewer::resetConstraint()
 		this->updateGraphView();
 	}
 
-	iter = rtabmap::graph::findLink(links_, from, to);
-	if(iter != links_.end())
-	{
-		this->updateConstraintView(iter->second);
-	}
-	iter = rtabmap::graph::findLink(linksAdded_, from, to);
-	if(iter != linksAdded_.end())
-	{
-		this->updateConstraintView(iter->second);
-	}
+	updateConstraintView();
 }
 
 void DatabaseViewer::rejectConstraint()
 {
 	int from = ids_.at(ui_->horizontalSlider_A->value());
 	int to = ids_.at(ui_->horizontalSlider_B->value());
+	if(ui_->label_type->text().toInt() == Link::kLandmark)
+	{
+		int position = ui_->horizontalSlider_loops->value();
+		const rtabmap::Link & link = loopLinks_.at(position);
+		from = link.from();
+		to = link.to();
+	}
+
 	if(from < to)
 	{
 		int tmp = to;
@@ -7880,7 +7984,6 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
 void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
 {
 	UDEBUG("%d %d", from, to);
-	int size = loopLinks_.size();
 	loopLinks_.clear();
 	std::multimap<int, Link> links = updateLinksWithModifications(links_);
 	int position = ui_->horizontalSlider_loops->value();
@@ -7931,7 +8034,7 @@ void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
 		{
 			ui_->horizontalSlider_loops->setValue(position);
 		}
-		else if(size != loopLinks_.size())
+		else
 		{
 			this->updateConstraintView(loopLinks_.at(position));
 		}

guilib/src/EditConstraintDialog.cpp

@@ -28,6 +28,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/EditConstraintDialog.h"
 #include "ui_editConstraintDialog.h"
 
+#ifndef M_PI
+#define M_PI       3.14159265358979323846
+#endif
+
 namespace rtabmap {
 
 EditConstraintDialog::EditConstraintDialog(const Transform & constraint, double linearSigma, double angularSigma, QWidget * parent) :

guilib/src/ExportCloudsDialog.cpp

@@ -303,6 +303,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("binary", _ui->checkBox_binary->isChecked());
 	settings.setValue("normals_k", _ui->spinBox_normalKSearch->value());
 	settings.setValue("normals_radius", _ui->doubleSpinBox_normalRadiusSearch->value());
+	settings.setValue("intensity_colormap", _ui->comboBox_intensityColormap->currentIndex());
 
 	settings.setValue("regenerate", _ui->checkBox_regenerate->isChecked());
 	settings.setValue("regenerate_decimation", _ui->spinBox_decimation->value());
@@ -371,6 +372,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("mesh_textureMaxAngle", _ui->doubleSpinBox_meshingTextureMaxAngle->value());
 	settings.setValue("mesh_textureMinCluster", _ui->spinBox_mesh_minTextureClusterSize->value());
 	settings.setValue("mesh_textureRoiRatios", _ui->lineEdit_meshingTextureRoiRatios->text());
+	settings.setValue("mesh_textureDistanceToCamPolicy", _ui->checkBox_distanceToCamPolicy->isChecked());
 	settings.setValue("mesh_textureCameraFiltering", _ui->checkBox_cameraFilter->isChecked());
 	settings.setValue("mesh_textureCameraFilteringRadius", _ui->doubleSpinBox_cameraFilterRadius->value());
 	settings.setValue("mesh_textureCameraFilteringAngle", _ui->doubleSpinBox_cameraFilterAngle->value());
@@ -439,6 +441,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->checkBox_binary->setChecked(settings.value("binary", _ui->checkBox_binary->isChecked()).toBool());
 	_ui->spinBox_normalKSearch->setValue(settings.value("normals_k", _ui->spinBox_normalKSearch->value()).toInt());
 	_ui->doubleSpinBox_normalRadiusSearch->setValue(settings.value("normals_radius", _ui->doubleSpinBox_normalRadiusSearch->value()).toDouble());
+	_ui->comboBox_intensityColormap->setCurrentIndex(settings.value("intensity_colormap", _ui->comboBox_intensityColormap->currentIndex()).toInt());
 
 	_ui->checkBox_regenerate->setChecked(settings.value("regenerate", _ui->checkBox_regenerate->isChecked()).toBool());
 	_ui->spinBox_decimation->setValue(settings.value("regenerate_decimation", _ui->spinBox_decimation->value()).toInt());
@@ -510,6 +513,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->doubleSpinBox_meshingTextureMaxAngle->setValue(settings.value("mesh_textureMaxAngle", _ui->doubleSpinBox_meshingTextureMaxAngle->value()).toDouble());
 	_ui->spinBox_mesh_minTextureClusterSize->setValue(settings.value("mesh_textureMinCluster", _ui->spinBox_mesh_minTextureClusterSize->value()).toDouble());
 	_ui->lineEdit_meshingTextureRoiRatios->setText(settings.value("mesh_textureRoiRatios", _ui->lineEdit_meshingTextureRoiRatios->text()).toString());
+	_ui->checkBox_distanceToCamPolicy->setChecked(settings.value("mesh_textureDistanceToCamPolicy", _ui->checkBox_distanceToCamPolicy->isChecked()).toBool());
 	_ui->checkBox_cameraFilter->setChecked(settings.value("mesh_textureCameraFiltering", _ui->checkBox_cameraFilter->isChecked()).toBool());
 	_ui->doubleSpinBox_cameraFilterRadius->setValue(settings.value("mesh_textureCameraFilteringRadius", _ui->doubleSpinBox_cameraFilterRadius->value()).toDouble());
 	_ui->doubleSpinBox_cameraFilterAngle->setValue(settings.value("mesh_textureCameraFilteringAngle", _ui->doubleSpinBox_cameraFilterAngle->value()).toDouble());
@@ -578,6 +582,7 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->checkBox_binary->setChecked(true);
 	_ui->spinBox_normalKSearch->setValue(20);
 	_ui->doubleSpinBox_normalRadiusSearch->setValue(0.0);
+	_ui->comboBox_intensityColormap->setCurrentIndex(0);
 
 	_ui->checkBox_regenerate->setChecked(_dbDriver!=0?true:false);
 	_ui->spinBox_decimation->setValue(1);
@@ -646,6 +651,7 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->doubleSpinBox_meshingTextureMaxAngle->setValue(0.0);
 	_ui->spinBox_mesh_minTextureClusterSize->setValue(50);
 	_ui->lineEdit_meshingTextureRoiRatios->setText("0.0 0.0 0.0 0.0");
+	_ui->checkBox_distanceToCamPolicy->setChecked(false);
 	_ui->checkBox_cameraFilter->setChecked(false);
 	_ui->doubleSpinBox_cameraFilterRadius->setValue(0);
 	_ui->doubleSpinBox_cameraFilterAngle->setValue(30);
@@ -753,6 +759,9 @@ void ExportCloudsDialog::updateReconstructionFlavor()
 			_ui->comboBox_frame->setCurrentIndex(0);
 		}
 	}
+	_ui->comboBox_intensityColormap->setVisible(!_ui->checkBox_fromDepth->isChecked() && !_ui->checkBox_binary->isEnabled());
+	_ui->comboBox_intensityColormap->setEnabled(!_ui->checkBox_fromDepth->isChecked() && !_ui->checkBox_binary->isEnabled());
+	_ui->label_intensityColormap->setVisible(!_ui->checkBox_fromDepth->isChecked() && !_ui->checkBox_binary->isEnabled());
 
 	_ui->checkBox_smoothing->setVisible(_ui->comboBox_pipeline->currentIndex() == 1);
 	_ui->checkBox_smoothing->setEnabled(_ui->comboBox_pipeline->currentIndex() == 1);
@@ -1002,6 +1011,14 @@ void ExportCloudsDialog::viewClouds(
 		viewer->setLighting(false);
 		viewer->setDefaultBackgroundColor(QColor(40, 40, 40, 255));
 		viewer->buildPickingLocator(true);
+		if(_ui->comboBox_intensityColormap->currentIndex()==1)
+		{
+			viewer->setIntensityRedColormap(true);
+		}
+		else if(_ui->comboBox_intensityColormap->currentIndex() == 2)
+		{
+			viewer->setIntensityRainbowColormap(true);
+		}
 
 		QVBoxLayout *layout = new QVBoxLayout();
 		layout->addWidget(viewer);
@@ -2404,7 +2421,7 @@ bool ExportCloudsDialog::getExportedClouds(
 					}
 					else
 					{
-						_progressDialog->appendText(tr("No polygons created for cloud %d!").arg(iter->first), Qt::darkYellow);
+						_progressDialog->appendText(tr("No polygons created for cloud %1!").arg(iter->first), Qt::darkYellow);
 						_progressDialog->setAutoClose(false);
 					}
 
@@ -2600,7 +2617,7 @@ bool ExportCloudsDialog::getExportedClouds(
 
 		// texture mesh
 		UDEBUG("texture mapping=%d", _ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked()?1:0);
-		if(!has2dScans && _ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked())
+		if(!has2dScans && !meshes.empty() && _ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked())
 		{
 			_progressDialog->appendText(tr("Texturing..."));
 			QApplication::processEvents();
@@ -2930,7 +2947,8 @@ bool ExportCloudsDialog::getExportedClouds(
 								_ui->spinBox_mesh_minTextureClusterSize->value(),
 								roiRatios,
 								&texturingState,
-								cameraPoses.size()>1?&textureVertexToPixels:0); // only get vertexToPixels if merged clouds with multi textures
+								cameraPoses.size()>1?&textureVertexToPixels:0, // only get vertexToPixels if merged clouds with multi textures
+								_ui->checkBox_distanceToCamPolicy->isChecked());
 
 						if(_canceled)
 						{
@@ -3205,6 +3223,18 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 						UERROR("Cloud %d not found in cache!", iter->first);
 					}
 				}
+				if(!cloud->empty() &&
+				   (_ui->doubleSpinBox_ceilingHeight->value() != 0.0 || _ui->doubleSpinBox_floorHeight->value() != 0.0))
+				{
+					float min = _ui->doubleSpinBox_floorHeight->value();
+					float max = _ui->doubleSpinBox_ceilingHeight->value();
+					indices = util3d::passThrough(
+							util3d::transformPointCloud(cloud, iter->second),
+							indices,
+							"z",
+							min!=0.0f&&min<max?min:std::numeric_limits<float>::lowest(),
+							max!=0.0f?max:std::numeric_limits<float>::max());
+				}
 			}
 			else if(_ui->checkBox_fromDepth->isChecked() && uContains(cachedClouds, iter->first))
 			{

guilib/src/GuiLib.qrc

@@ -40,5 +40,6 @@
         <file>images/t265.png</file>
         <file>images/sr300.png</file>
         <file>images/mynteyes.png</file>
+        <file>images/l515.png</file>
     </qresource>
 </RCC>

guilib/src/MainWindow.cpp

@@ -437,6 +437,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	connect(_ui->actionRealSense2_SR300, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
 	connect(_ui->actionRealSense2_D415, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
 	connect(_ui->actionRealSense2_D435, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
+	connect(_ui->actionRealSense2_L515, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
 	connect(_ui->actionStereoDC1394, SIGNAL(triggered()), this, SLOT(selectStereoDC1394()));
 	connect(_ui->actionStereoFlyCapture2, SIGNAL(triggered()), this, SLOT(selectStereoFlyCapture2()));
 	connect(_ui->actionStereoZed, SIGNAL(triggered()), this, SLOT(selectStereoZed()));
@@ -458,6 +459,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	_ui->actionRealSense2_SR300->setEnabled(CameraRealSense2::available());
 	_ui->actionRealSense2_D415->setEnabled(CameraRealSense2::available());
 	_ui->actionRealSense2_D435->setEnabled(CameraRealSense2::available());
+	_ui->actionRealSense2_L515->setEnabled(CameraRealSense2::available());
 	_ui->actionRealSense2_T265->setEnabled(CameraRealSense2::available());
 	_ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available());
 	_ui->actionStereoFlyCapture2->setEnabled(CameraStereoFlyCapture2::available());
@@ -1153,17 +1155,42 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 				{
 					if(!lost)
 					{
-						pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
-						cloud = util3d::laserScanToPointCloudNormal(odom.info().localScanMap, odom.info().localScanMap.localTransform());
-						bool scanAdded = _cloudViewer->getAddedClouds().contains("scanMapOdom");
-						if(!_cloudViewer->addCloud("scanMapOdom", cloud, _odometryCorrection, Qt::blue))
+						bool scanAlreadyThere = _cloudViewer->getAddedClouds().contains("scanMapOdom");
+						bool scanAdded = false;
+						if(odom.info().localScanMap.hasIntensity() && odom.info().localScanMap.hasNormals())
+						{
+							scanAdded = _cloudViewer->addCloud("scanMapOdom",
+									util3d::laserScanToPointCloudINormal(odom.info().localScanMap, odom.info().localScanMap.localTransform()),
+									_odometryCorrection, Qt::blue);
+						}
+						else if(odom.info().localScanMap.hasNormals())
+						{
+							scanAdded = _cloudViewer->addCloud("scanMapOdom",
+									util3d::laserScanToPointCloudNormal(odom.info().localScanMap, odom.info().localScanMap.localTransform()),
+									_odometryCorrection, Qt::blue);
+						}
+						else if(odom.info().localScanMap.hasIntensity())
+						{
+							scanAdded = _cloudViewer->addCloud("scanMapOdom",
+									util3d::laserScanToPointCloudI(odom.info().localScanMap, odom.info().localScanMap.localTransform()),
+									_odometryCorrection, Qt::blue);
+						}
+						else
+						{
+							scanAdded = _cloudViewer->addCloud("scanMapOdom",
+									util3d::laserScanToPointCloud(odom.info().localScanMap, odom.info().localScanMap.localTransform()),
+									_odometryCorrection, Qt::blue);
+						}
+
+
+						if(!scanAdded)
 						{
 							UERROR("Adding scanMapOdom to viewer failed!");
 						}
 						else
 						{
 							_cloudViewer->setCloudVisibility("scanMapOdom", true);
-							_cloudViewer->setCloudColorIndex("scanMapOdom", scanAdded && _preferencesDialog->getScanColorScheme(1)==0 && odom.info().localScanMap.is2d()?2:_preferencesDialog->getScanColorScheme(1));
+							_cloudViewer->setCloudColorIndex("scanMapOdom", scanAlreadyThere && _preferencesDialog->getScanColorScheme(1)==0 && odom.info().localScanMap.is2d()?2:_preferencesDialog->getScanColorScheme(1));
 							_cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
 							_cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
 						}
@@ -1184,23 +1211,58 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 								_preferencesDialog->getScanMinRange(1),
 								_preferencesDialog->getScanMaxRange(1));
 					}
+					bool scanAlreadyThere = _cloudViewer->getAddedClouds().contains("scanOdom");
+					bool scanAdded = false;
 
-					pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
-					cloud = util3d::laserScanToPointCloudNormal(scan, pose*scan.localTransform());
-					if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
+					if(odom.info().localScanMap.hasIntensity() && odom.info().localScanMap.hasNormals())
 					{
-						cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
+						pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud;
+						cloud = util3d::laserScanToPointCloudINormal(scan, pose*scan.localTransform());
+						if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
+						{
+							cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
+						}
+						scanAdded = _cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta);
+					}
+					else if(odom.info().localScanMap.hasNormals())
+					{
+						pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
+						cloud = util3d::laserScanToPointCloudNormal(scan, pose*scan.localTransform());
+						if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
+						{
+							cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
+						}
+						scanAdded = _cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta);
+					}
+					else if(odom.info().localScanMap.hasIntensity())
+					{
+						pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;
+						cloud = util3d::laserScanToPointCloudI(scan, pose*scan.localTransform());
+						if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
+						{
+							cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
+						}
+						scanAdded = _cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta);
+					}
+					else
+					{
+						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+						cloud = util3d::laserScanToPointCloud(scan, pose*scan.localTransform());
+						if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
+						{
+							cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
+						}
+						scanAdded = _cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta);
 					}
 
-					bool scanAdded = !_cloudViewer->getAddedClouds().contains("scanOdom");
-					if(!_cloudViewer->addCloud("scanOdom", cloud, _odometryCorrection, Qt::magenta))
+					if(!scanAdded)
 					{
 						UERROR("Adding scanOdom to viewer failed!");
 					}
 					else
 					{
 						_cloudViewer->setCloudVisibility("scanOdom", true);
-						_cloudViewer->setCloudColorIndex("scanOdom", scanAdded && _preferencesDialog->getScanColorScheme(1)==0 && scan.is2d()?2:_preferencesDialog->getScanColorScheme(1));
+						_cloudViewer->setCloudColorIndex("scanOdom", scanAlreadyThere && _preferencesDialog->getScanColorScheme(1)==0 && scan.is2d()?2:_preferencesDialog->getScanColorScheme(1));
 						_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
 						_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
 						scanUpdated = true;
@@ -1701,7 +1763,26 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		if(stat.getLastSignatureData().id() == stat.refImageId())
 		{
 			signature = stat.getLastSignatureData();
-			signature.sensorData().uncompressData(); // make sure data are uncompressed
+
+			// make sure data are uncompressed
+			// We don't need to uncompress images if we don't show them
+			bool uncompressImages = !signature.sensorData().imageCompressed().empty() && (
+					_ui->imageView_source->isVisible() ||
+					(_loopClosureViewer->isVisible() &&
+							!signature.sensorData().depthOrRightCompressed().empty()) ||
+					(_cloudViewer->isVisible() &&
+							_preferencesDialog->isCloudsShown(0) &&
+							!signature.sensorData().depthOrRightCompressed().empty()));
+			bool uncompressScan = !signature.sensorData().laserScanCompressed().isEmpty() && (
+					_loopClosureViewer->isVisible() ||
+					(_cloudViewer->isVisible() && _preferencesDialog->isScansShown(0)));
+			cv::Mat tmpRgb, tmpDepth, tmpG, tmpO, tmpE;
+			LaserScan tmpScan;
+			signature.sensorData().uncompressData(
+					uncompressImages?&tmpRgb:0,
+					uncompressImages?&tmpDepth:0,
+					uncompressScan?&tmpScan:0,
+					0, &tmpG, &tmpO, &tmpE);
 
 			if( uStr2Bool(_preferencesDialog->getParameter(Parameters::kMemIncrementalMemory())) &&
 				signature.getWeight()>=0) // ignore intermediate nodes for the cache
@@ -1868,7 +1949,18 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 					{
 						// uncompress after copy to avoid keeping uncompressed data in memory
 						loopSignature = iter.value();
-						loopSignature.sensorData().uncompressData();
+						bool uncompressImages = _ui->imageView_source->isVisible() ||
+							(_loopClosureViewer->isVisible() && !signature.sensorData().depthOrRightCompressed().empty());
+						bool uncompressScan = _loopClosureViewer->isVisible() && !signature.sensorData().laserScanCompressed().isEmpty();
+						if(uncompressImages || uncompressScan)
+						{
+							cv::Mat tmpRGB, tmpDepth;
+							LaserScan tmpScan;
+							loopSignature.sensorData().uncompressData(
+									uncompressImages?&tmpRGB:0,
+									uncompressImages?&tmpDepth:0,
+									uncompressScan?&tmpScan:0);
+						}
 					}
 				}
 			}
@@ -4641,6 +4733,7 @@ void MainWindow::updateSelectSourceMenu()
 	_ui->actionRealSense2_SR300->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
 	_ui->actionRealSense2_D415->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
 	_ui->actionRealSense2_D435->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
+	_ui->actionRealSense2_L515->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
 	_ui->actionStereoDC1394->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDC1394);
 	_ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFlyCapture2);
 	_ui->actionStereoZed->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoZed);
@@ -5184,7 +5277,7 @@ void MainWindow::startDetection()
 			_preferencesDialog->getSourceScanVoxelSize(),
 			_preferencesDialog->getSourceScanNormalsK(),
 			_preferencesDialog->getSourceScanNormalsRadius(),
-			_preferencesDialog->isSourceScanForceGroundNormalsUp());
+			(float)_preferencesDialog->getSourceScanForceGroundNormalsUp());
 	if(_preferencesDialog->getIMUFilteringStrategy()>0 && dynamic_cast<DBReader*>(camera) == 0)
 	{
 		_camera->enableIMUFiltering(_preferencesDialog->getIMUFilteringStrategy()-1, parameters);

guilib/src/PreferencesDialog.cpp

@@ -195,8 +195,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 #ifndef RTABMAP_NONFREE
 	_ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1);
 	_ui->comboBox_detector_strategy->setItemData(12, 0, Qt::UserRole - 1);
+	_ui->comboBox_detector_strategy->setItemData(14, 0, Qt::UserRole - 1);
 	_ui->vis_feature_detector->setItemData(0, 0, Qt::UserRole - 1);
 	_ui->vis_feature_detector->setItemData(12, 0, Qt::UserRole - 1);
+	_ui->vis_feature_detector->setItemData(14, 0, Qt::UserRole - 1);
 #endif
 
 // SIFT
@@ -213,11 +215,15 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->comboBox_detector_strategy->setItemData(5, 0, Qt::UserRole - 1);
 	_ui->comboBox_detector_strategy->setItemData(6, 0, Qt::UserRole - 1);
 	_ui->comboBox_detector_strategy->setItemData(12, 0, Qt::UserRole - 1);
+	_ui->comboBox_detector_strategy->setItemData(13, 0, Qt::UserRole - 1);
+	_ui->comboBox_detector_strategy->setItemData(14, 0, Qt::UserRole - 1);
 	_ui->vis_feature_detector->setItemData(3, 0, Qt::UserRole - 1);
 	_ui->vis_feature_detector->setItemData(4, 0, Qt::UserRole - 1);
 	_ui->vis_feature_detector->setItemData(5, 0, Qt::UserRole - 1);
 	_ui->vis_feature_detector->setItemData(6, 0, Qt::UserRole - 1);
 	_ui->vis_feature_detector->setItemData(12, 0, Qt::UserRole - 1);
+	_ui->vis_feature_detector->setItemData(13, 0, Qt::UserRole - 1);
+	_ui->vis_feature_detector->setItemData(14, 0, Qt::UserRole - 1);
 #endif
 
 #ifndef RTABMAP_ORB_OCTREE
@@ -242,7 +248,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->groupBox_fast_opencv2->setEnabled(false);
 #else
 	_ui->comboBox_detector_strategy->setItemData(9, 0, Qt::UserRole - 1); // No KAZE
+	_ui->comboBox_detector_strategy->setItemData(13, 0, Qt::UserRole - 1); // No DAISY
+	_ui->comboBox_detector_strategy->setItemData(14, 0, Qt::UserRole - 1); // No DAISY
 	_ui->vis_feature_detector->setItemData(9, 0, Qt::UserRole - 1); // No KAZE
+	_ui->vis_feature_detector->setItemData(13, 0, Qt::UserRole - 1); // No DAISY
+	_ui->vis_feature_detector->setItemData(14, 0, Qt::UserRole - 1); // No DAISY
 #endif
 
 	_ui->comboBox_cameraImages_odomFormat->setItemData(4, 0, Qt::UserRole - 1);
@@ -635,6 +645,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->source_spinBox_databaseStopId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_database_cameraIndex, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->source_checkBox_stereoToDepthDB, SIGNAL(toggled(bool)), _ui->checkbox_stereo_depthGenerated, SLOT(setChecked(bool)));
+	connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(toggled(bool)), _ui->source_checkBox_stereoToDepthDB, SLOT(setChecked(bool)));
 
 	//openni group
 	_ui->stackedWidget_rgbd->setCurrentIndex(_ui->comboBox_cameraRGBD->currentIndex());
@@ -670,6 +682,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->spinBox_rs2_width, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_rs2_height, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_rs2_rate, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkbox_rs2_globalTimeStync, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_rs2_dualMode, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_rs2_dualModeExtrinsics, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_rs2_jsonFile, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -685,6 +698,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->toolButton_cameraImages_gt, SIGNAL(clicked()), this, SLOT(selectSourceImagesPathGt()));
 	connect(_ui->lineEdit_cameraRGBDImages_path_rgb, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraRGBDImages_path_depth, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkBox_cameraImages_configForEachFrame, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_cameraImages_timestamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_cameraImages_syncTimeStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_cameraRGBDImages_scale, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
@@ -780,7 +794,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->doubleSpinBox_source_scanVoxelSize, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_source_scanNormalsK, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_source_scanNormalsRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->checkBox_source_scanForceGroundNormalsUp, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->doubleSpinBox_source_scanNormalsForceGroundUp, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 
 
 	//Rtabmap basic
@@ -867,6 +881,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->comboBox_dbJournalMode->setObjectName(Parameters::kDbSqlite3JournalMode().c_str());
 	_ui->comboBox_dbSynchronous->setObjectName(Parameters::kDbSqlite3Synchronous().c_str());
 	_ui->comboBox_dbTempStore->setObjectName(Parameters::kDbSqlite3TempStore().c_str());
+	_ui->lineEdit_targetDatabaseVersion->setObjectName(Parameters::kDbTargetVersion().c_str());
+
 
 	// Create hypotheses
 	_ui->general_doubleSpinBox_hardThr->setObjectName(Parameters::kRtabmapLoopThr().c_str());
@@ -1116,7 +1132,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kIcpPointToPlane().c_str());
 	_ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kIcpPointToPlaneK().c_str());
 	_ui->loopClosure_icpPointToPlaneNormalsRadius->setObjectName(Parameters::kIcpPointToPlaneRadius().c_str());
+	_ui->loopClosure_icpPointToPlaneGroundNormalsUp->setObjectName(Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
 	_ui->loopClosure_icpPointToPlaneNormalsMinComplexity->setObjectName(Parameters::kIcpPointToPlaneMinComplexity().c_str());
+	_ui->loopClosure_icpPointToPlaneLowComplexityStrategy->setObjectName(Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
 
 	_ui->groupBox_libpointmatcher->setObjectName(Parameters::kIcpPM().c_str());
 	_ui->lineEdit_IcpPMConfigPath->setObjectName(Parameters::kIcpPMConfig().c_str());
@@ -1124,6 +1142,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->doubleSpinBox_icpPMOutlierRatio->setObjectName(Parameters::kIcpPMOutlierRatio().c_str());
 	_ui->spinBox_icpPMMatcherKnn->setObjectName(Parameters::kIcpPMMatcherKnn().c_str());
 	_ui->doubleSpinBox_icpPMMatcherEpsilon->setObjectName(Parameters::kIcpPMMatcherEpsilon().c_str());
+	_ui->loopClosure_icpPMMatcherIntensity->setObjectName(Parameters::kIcpPMMatcherIntensity().c_str());
 
 	// Occupancy grid
 	_ui->groupBox_grid_3d->setObjectName(Parameters::kGrid3D().c_str());
@@ -1367,6 +1386,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->ArucoMaxDepthError->setObjectName(Parameters::kMarkerMaxDepthError().c_str());
 	_ui->ArucoVarianceLinear->setObjectName(Parameters::kMarkerVarianceLinear().c_str());
 	_ui->ArucoVarianceAngular->setObjectName(Parameters::kMarkerVarianceAngular().c_str());
+	_ui->ArucoMarkerRangeMin->setObjectName(Parameters::kMarkerMinRange().c_str());
+	_ui->ArucoMarkerRangeMax->setObjectName(Parameters::kMarkerMaxRange().c_str());
 	_ui->ArucoCornerRefinementMethod->setObjectName(Parameters::kMarkerCornerRefinementMethod().c_str());
 
 	// IMU filter
@@ -1887,6 +1908,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->spinBox_rs2_width->setValue(848);
 		_ui->spinBox_rs2_height->setValue(480);
 		_ui->spinBox_rs2_rate->setValue(60);
+		_ui->checkbox_rs2_globalTimeStync->setChecked(true);
 		_ui->checkbox_rs2_dualMode->setChecked(false);
 		_ui->lineEdit_rs2_dualModeExtrinsics->setText("0.009 0.021 0.027 0 -0.018 0.005");
 		_ui->lineEdit_rs2_jsonFile->clear();
@@ -1934,6 +1956,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->spinBox_stereoMyntEye_contrast->setValue(116);
 		_ui->spinBox_stereoMyntEye_irControl->setValue(0);
 
+		_ui->checkBox_cameraImages_configForEachFrame->setChecked(false);
 		_ui->checkBox_cameraImages_timestamps->setChecked(false);
 		_ui->checkBox_cameraImages_syncTimeStamps->setChecked(true);
 		_ui->lineEdit_cameraImages_timestamps->setText("");
@@ -1965,7 +1988,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->doubleSpinBox_source_scanVoxelSize->setValue(0.0f);
 		_ui->spinBox_source_scanNormalsK->setValue(0);
 		_ui->doubleSpinBox_source_scanNormalsRadius->setValue(0.0);
-		_ui->checkBox_source_scanForceGroundNormalsUp->setChecked(false);
+		_ui->doubleSpinBox_source_scanNormalsForceGroundUp->setValue(0);
 
 		_ui->groupBox_depthFromScan->setChecked(false);
 		_ui->groupBox_depthFromScan_fillHoles->setChecked(true);
@@ -2341,6 +2364,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->spinBox_rs2_width->setValue(settings.value("width", _ui->spinBox_rs2_width->value()).toInt());
 	_ui->spinBox_rs2_height->setValue(settings.value("height", _ui->spinBox_rs2_height->value()).toInt());
 	_ui->spinBox_rs2_rate->setValue(settings.value("rate", _ui->spinBox_rs2_rate->value()).toInt());
+	_ui->checkbox_rs2_globalTimeStync->setChecked(settings.value("global_time_sync", _ui->checkbox_rs2_globalTimeStync->isChecked()).toBool());
 	_ui->checkbox_rs2_dualMode->setChecked(settings.value("dual_mode", _ui->checkbox_rs2_dualMode->isChecked()).toBool());
 	_ui->lineEdit_rs2_dualModeExtrinsics->setText(settings.value("dual_mode_extrinsics", _ui->lineEdit_rs2_dualModeExtrinsics->text()).toString());
 	_ui->lineEdit_rs2_jsonFile->setText(settings.value("json_preset", _ui->lineEdit_rs2_jsonFile->text()).toString());
@@ -2399,6 +2423,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->source_images_spinBox_maxFrames->setValue(settings.value("maxFrames",_ui->source_images_spinBox_maxFrames->value()).toInt());
 	_ui->comboBox_cameraImages_bayerMode->setCurrentIndex(settings.value("bayerMode",_ui->comboBox_cameraImages_bayerMode->currentIndex()).toInt());
 
+	_ui->checkBox_cameraImages_configForEachFrame->setChecked(settings.value("config_each_frame",_ui->checkBox_cameraImages_configForEachFrame->isChecked()).toBool());
 	_ui->checkBox_cameraImages_timestamps->setChecked(settings.value("filenames_as_stamps",_ui->checkBox_cameraImages_timestamps->isChecked()).toBool());
 	_ui->checkBox_cameraImages_syncTimeStamps->setChecked(settings.value("sync_stamps",_ui->checkBox_cameraImages_syncTimeStamps->isChecked()).toBool());
 	_ui->lineEdit_cameraImages_timestamps->setText(settings.value("stamps", _ui->lineEdit_cameraImages_timestamps->text()).toString());
@@ -2444,7 +2469,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->doubleSpinBox_source_scanVoxelSize->setValue(settings.value("voxelSize", _ui->doubleSpinBox_source_scanVoxelSize->value()).toDouble());
 	_ui->spinBox_source_scanNormalsK->setValue(settings.value("normalsK", _ui->spinBox_source_scanNormalsK->value()).toInt());
 	_ui->doubleSpinBox_source_scanNormalsRadius->setValue(settings.value("normalsRadius", _ui->doubleSpinBox_source_scanNormalsRadius->value()).toDouble());
-	_ui->checkBox_source_scanForceGroundNormalsUp->setChecked(settings.value("normalsUp", _ui->checkBox_source_scanForceGroundNormalsUp->isChecked()).toBool());
+	_ui->doubleSpinBox_source_scanNormalsForceGroundUp->setValue(settings.value("normalsUpF", _ui->doubleSpinBox_source_scanNormalsForceGroundUp->value()).toDouble());
 	settings.endGroup();//Scan
 
 	settings.beginGroup("DepthFromScan");
@@ -2822,6 +2847,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("width",                  _ui->spinBox_rs2_width->value());
 	settings.setValue("height",                 _ui->spinBox_rs2_height->value());
 	settings.setValue("rate",                   _ui->spinBox_rs2_rate->value());
+	settings.setValue("global_time_sync",       _ui->checkbox_rs2_globalTimeStync->isChecked());
 	settings.setValue("dual_mode",              _ui->checkbox_rs2_dualMode->isChecked());
 	settings.setValue("dual_mode_extrinsics",   _ui->lineEdit_rs2_dualModeExtrinsics->text());
 	settings.setValue("json_preset",            _ui->lineEdit_rs2_jsonFile->text());
@@ -2879,6 +2905,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("startPos", 		_ui->source_images_spinBox_startPos->value());
 	settings.setValue("maxFrames", 		_ui->source_images_spinBox_maxFrames->value());
 	settings.setValue("bayerMode", 	    _ui->comboBox_cameraImages_bayerMode->currentIndex());
+	settings.setValue("config_each_frame", _ui->checkBox_cameraImages_configForEachFrame->isChecked());
 	settings.setValue("filenames_as_stamps", _ui->checkBox_cameraImages_timestamps->isChecked());
 	settings.setValue("sync_stamps",    _ui->checkBox_cameraImages_syncTimeStamps->isChecked());
 	settings.setValue("stamps",              _ui->lineEdit_cameraImages_timestamps->text());
@@ -2923,7 +2950,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("voxelSize", 			_ui->doubleSpinBox_source_scanVoxelSize->value());
 	settings.setValue("normalsK", 			_ui->spinBox_source_scanNormalsK->value());
 	settings.setValue("normalsRadius", 		_ui->doubleSpinBox_source_scanNormalsRadius->value());
-	settings.setValue("normalsUp", 	        _ui->checkBox_source_scanForceGroundNormalsUp->isChecked());
+	settings.setValue("normalsUpF", 	    _ui->doubleSpinBox_source_scanNormalsForceGroundUp->value());
 	settings.endGroup();
 
 	settings.beginGroup("DepthFromScan");
@@ -3281,9 +3308,6 @@ void PreferencesDialog::showEvent ( QShowEvent * event )
 		_ui->groupBox_source0->setEnabled(false);
 		_ui->groupBox_odometry1->setEnabled(false);
 
-		_ui->checkBox_useOdomFeatures->setChecked(false);
-		_ui->checkBox_useOdomFeatures->setEnabled(false);
-
 		this->setWindowTitle(tr("Preferences [Monitoring mode]"));
 	}
 	else
@@ -3295,8 +3319,6 @@ void PreferencesDialog::showEvent ( QShowEvent * event )
 		_ui->groupBox_source0->setEnabled(true);
 		_ui->groupBox_odometry1->setEnabled(true);
 
-		_ui->checkBox_useOdomFeatures->setEnabled(true);
-
 		this->setWindowTitle(tr("Preferences"));
 	}
 
@@ -5562,9 +5584,9 @@ double PreferencesDialog::getSourceScanNormalsRadius() const
 {
 	return _ui->doubleSpinBox_source_scanNormalsRadius->value();
 }
-bool PreferencesDialog::isSourceScanForceGroundNormalsUp() const
+double PreferencesDialog::getSourceScanForceGroundNormalsUp() const
 {
-	return _ui->checkBox_source_scanForceGroundNormalsUp->isChecked();
+	return _ui->doubleSpinBox_source_scanNormalsForceGroundUp->value();
 }
 
 Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
@@ -5716,6 +5738,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 				((CameraRealSense2*)camera)->setEmitterEnabled(_ui->checkbox_rs2_emitter->isChecked());
 				((CameraRealSense2*)camera)->setIRFormat(_ui->checkbox_rs2_irMode->isChecked(), _ui->checkbox_rs2_irDepth->isChecked());
 				((CameraRealSense2*)camera)->setResolution(_ui->spinBox_rs2_width->value(), _ui->spinBox_rs2_height->value(), _ui->spinBox_rs2_rate->value());
+				((CameraRealSense2*)camera)->setGlobalTimeSync(_ui->checkbox_rs2_globalTimeStync->isChecked());
 				((CameraRealSense2*)camera)->setDualMode(_ui->checkbox_rs2_dualMode->isChecked(), Transform::fromString(_ui->lineEdit_rs2_dualModeExtrinsics->text().toStdString()));
 				((CameraRealSense2*)camera)->setJsonConfig(_ui->lineEdit_rs2_jsonFile->text().toStdString());
 			}
@@ -5776,6 +5799,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 				_ui->checkBox_cameraImages_timestamps->isChecked(),
 				_ui->lineEdit_cameraImages_timestamps->text().toStdString(),
 				_ui->checkBox_cameraImages_syncTimeStamps->isChecked());
+		((CameraRGBDImages*)camera)->setConfigForEachFrame(_ui->checkBox_cameraImages_configForEachFrame->isChecked());
 	}
 	else if(driver == kSrcDC1394)
 	{
@@ -5821,6 +5845,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 				_ui->checkBox_cameraImages_timestamps->isChecked(),
 				_ui->lineEdit_cameraImages_timestamps->text().toStdString(),
 				_ui->checkBox_cameraImages_syncTimeStamps->isChecked());
+		((CameraRGBDImages*)camera)->setConfigForEachFrame(_ui->checkBox_cameraImages_configForEachFrame->isChecked());
 
 	}
 	else if (driver == kSrcStereoUsb)
@@ -5959,6 +5984,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 				_ui->checkBox_cameraImages_timestamps->isChecked(),
 				_ui->lineEdit_cameraImages_timestamps->text().toStdString(),
 				_ui->checkBox_cameraImages_syncTimeStamps->isChecked());
+		((CameraRGBDImages*)camera)->setConfigForEachFrame(_ui->checkBox_cameraImages_configForEachFrame->isChecked());
 	}
 	else if(driver == kSrcDatabase)
 	{
@@ -6242,7 +6268,7 @@ void PreferencesDialog::testOdometry()
 			_ui->doubleSpinBox_source_scanVoxelSize->value(),
 			_ui->spinBox_source_scanNormalsK->value(),
 			_ui->doubleSpinBox_source_scanNormalsRadius->value(),
-			_ui->checkBox_source_scanForceGroundNormalsUp->isChecked());
+			(float)_ui->doubleSpinBox_source_scanNormalsForceGroundUp->value());
 	if(_ui->comboBox_imuFilter_strategy->currentIndex()>0 && dynamic_cast<DBReader*>(camera) == 0)
 	{
 		cameraThread.enableIMUFiltering(_ui->comboBox_imuFilter_strategy->currentIndex()-1, this->getAllParameters());
@@ -6313,7 +6339,7 @@ void PreferencesDialog::testCamera()
 				_ui->doubleSpinBox_source_scanVoxelSize->value(),
 				_ui->spinBox_source_scanNormalsK->value(),
 				_ui->doubleSpinBox_source_scanNormalsRadius->value(),
-				_ui->checkBox_source_scanForceGroundNormalsUp->isChecked());
+				(float)_ui->doubleSpinBox_source_scanNormalsForceGroundUp->value());
 		if(_ui->comboBox_imuFilter_strategy->currentIndex()>0 && dynamic_cast<DBReader*>(camera) == 0)
 		{
 			cameraThread.enableIMUFiltering(_ui->comboBox_imuFilter_strategy->currentIndex()-1, this->getAllParameters());

guilib/src/ui/exportCloudsDialog.ui

@@ -23,9 +23,9 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>-2339</y>
+        <y>0</y>
         <width>780</width>
-        <height>4823</height>
+        <height>4975</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_13">
@@ -52,21 +52,21 @@
            </property>
           </widget>
          </item>
-         <item row="9" column="0">
+         <item row="10" column="0">
           <widget class="QCheckBox" name="checkBox_regenerate">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="10" column="0">
+         <item row="11" column="0">
           <widget class="QCheckBox" name="checkBox_filtering">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="9" column="1">
+         <item row="10" column="1">
           <widget class="QLabel" name="label_regenerate">
            <property name="text">
             <string>Regenerate clouds. This can be used to regenerate the point clouds at higher density than those used for online visualization.</string>
@@ -76,7 +76,7 @@
            </property>
           </widget>
          </item>
-         <item row="12" column="0">
+         <item row="13" column="0">
           <widget class="QCheckBox" name="checkBox_gainCompensation">
            <property name="text">
             <string/>
@@ -103,7 +103,7 @@
            </property>
           </widget>
          </item>
-         <item row="12" column="1">
+         <item row="13" column="1">
           <widget class="QLabel" name="label_gainCompensation">
            <property name="text">
             <string>Gain compensation. Normalize brightness of images.</string>
@@ -133,7 +133,7 @@
            </property>
           </widget>
          </item>
-         <item row="13" column="1">
+         <item row="14" column="1">
           <widget class="QLabel" name="label_binaryFile_12">
            <property name="text">
             <string>Meshing.</string>
@@ -143,14 +143,14 @@
            </property>
           </widget>
          </item>
-         <item row="11" column="0">
+         <item row="12" column="0">
           <widget class="QCheckBox" name="checkBox_smoothing">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="10" column="1">
+         <item row="11" column="1">
           <widget class="QLabel" name="label_binaryFile_9">
            <property name="text">
             <string>Cloud filtering. Remove sparse points that are far from surfaces.</string>
@@ -160,7 +160,7 @@
            </property>
           </widget>
          </item>
-         <item row="13" column="0">
+         <item row="14" column="0">
           <widget class="QCheckBox" name="checkBox_meshing">
            <property name="text">
             <string/>
@@ -177,7 +177,7 @@
            </property>
           </widget>
          </item>
-         <item row="11" column="1">
+         <item row="12" column="1">
           <widget class="QLabel" name="label_smoothing">
            <property name="text">
             <string>Cloud smoothing using Moving Least Squares algorithm (MLS).</string>
@@ -294,6 +294,35 @@
            </property>
           </widget>
          </item>
+         <item row="9" column="1">
+          <widget class="QLabel" name="label_intensityColormap">
+           <property name="text">
+            <string>Intensity colormap.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="9" column="0">
+          <widget class="QComboBox" name="comboBox_intensityColormap">
+           <item>
+            <property name="text">
+             <string>GrayScale</string>
+            </property>
+           </item>
+           <item>
+            <property name="text">
+             <string>RedYellow</string>
+            </property>
+           </item>
+           <item>
+            <property name="text">
+             <string>Rainbow</string>
+            </property>
+           </item>
+          </widget>
+         </item>
         </layout>
        </item>
        <item>
@@ -397,14 +426,14 @@
          <layout class="QVBoxLayout" name="verticalLayout_14">
           <item>
            <layout class="QGridLayout" name="gridLayout" columnstretch="0,0,1">
-            <item row="6" column="0">
+            <item row="8" column="0">
              <widget class="QToolButton" name="toolButton_distortionModel">
               <property name="text">
                <string>...</string>
               </property>
              </widget>
             </item>
-            <item row="4" column="1">
+            <item row="6" column="1">
              <widget class="QSpinBox" name="spinBox_fillDepthHoles">
               <property name="suffix">
                <string> pixels</string>
@@ -430,7 +459,7 @@
                <string> m</string>
               </property>
               <property name="decimals">
-               <number>1</number>
+               <number>2</number>
               </property>
               <property name="maximum">
                <double>100.000000000000000</double>
@@ -453,10 +482,10 @@
               </property>
              </widget>
             </item>
-            <item row="6" column="1">
+            <item row="8" column="1">
              <widget class="QLineEdit" name="lineEdit_distortionModel"/>
             </item>
-            <item row="5" column="1">
+            <item row="7" column="1">
              <widget class="QSpinBox" name="spinBox_fillDepthHolesError">
               <property name="suffix">
                <string> %</string>
@@ -483,7 +512,7 @@
               </property>
              </widget>
             </item>
-            <item row="4" column="2">
+            <item row="6" column="2">
              <widget class="QLabel" name="label_197">
               <property name="text">
                <string>Fill depth hole size (0 means no fill).</string>
@@ -502,7 +531,7 @@
                <string> m</string>
               </property>
               <property name="decimals">
-               <number>1</number>
+               <number>2</number>
               </property>
               <property name="maximum">
                <double>100.000000000000000</double>
@@ -515,7 +544,7 @@
               </property>
              </widget>
             </item>
-            <item row="6" column="2">
+            <item row="8" column="2">
              <widget class="QLabel" name="label_134">
               <property name="text">
                <string>Path to a depth distortion model to apply (output from depth calibration).</string>
@@ -525,7 +554,7 @@
               </property>
              </widget>
             </item>
-            <item row="5" column="2">
+            <item row="7" column="2">
              <widget class="QLabel" name="label_198">
               <property name="text">
                <string>Fill depth hole error.</string>
@@ -538,7 +567,7 @@
               </property>
              </widget>
             </item>
-            <item row="3" column="2">
+            <item row="5" column="2">
              <widget class="QLabel" name="label_135">
               <property name="text">
                <string>ROI ratios [left, right, top, bottom] between 0 and 1. Only generate 3D points for pixels inside the region of interest (RGB image).</string>
@@ -548,17 +577,17 @@
               </property>
              </widget>
             </item>
-            <item row="3" column="1">
+            <item row="5" column="1">
              <widget class="QLineEdit" name="lineEdit_roiRatios"/>
             </item>
-            <item row="7" column="1">
+            <item row="9" column="1">
              <widget class="QCheckBox" name="checkBox_bilateral">
               <property name="text">
                <string/>
               </property>
              </widget>
             </item>
-            <item row="7" column="2">
+            <item row="9" column="2">
              <widget class="QLabel" name="label_136">
               <property name="text">
                <string>Bilateral filtering of the depth image. Reduce noise in depth images.</string>
@@ -568,7 +597,7 @@
               </property>
              </widget>
             </item>
-            <item row="8" column="2">
+            <item row="10" column="2">
              <widget class="QLabel" name="label_137">
               <property name="text">
                <string>Cloud subtraction. Superposed points from different nodes are filtered.</string>
@@ -578,13 +607,71 @@
               </property>
              </widget>
             </item>
-            <item row="8" column="1">
+            <item row="10" column="1">
              <widget class="QCheckBox" name="checkBox_subtraction">
               <property name="text">
                <string/>
               </property>
              </widget>
             </item>
+            <item row="3" column="2">
+             <widget class="QLabel" name="label_140">
+              <property name="text">
+               <string>Ceiling filtering height.</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="4" column="2">
+             <widget class="QLabel" name="label_141">
+              <property name="text">
+               <string>Floor filtering height.</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="3" column="1">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_ceilingHeight">
+              <property name="suffix">
+               <string> m</string>
+              </property>
+              <property name="decimals">
+               <number>2</number>
+              </property>
+              <property name="maximum">
+               <double>100.000000000000000</double>
+              </property>
+              <property name="singleStep">
+               <double>0.100000000000000</double>
+              </property>
+              <property name="value">
+               <double>0.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+            <item row="4" column="1">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_floorHeight">
+              <property name="suffix">
+               <string> m</string>
+              </property>
+              <property name="decimals">
+               <number>2</number>
+              </property>
+              <property name="maximum">
+               <double>100.000000000000000</double>
+              </property>
+              <property name="singleStep">
+               <double>0.100000000000000</double>
+              </property>
+              <property name="value">
+               <double>0.000000000000000</double>
+              </property>
+             </widget>
+            </item>
            </layout>
           </item>
           <item>
@@ -1595,7 +1682,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
             <layout class="QVBoxLayout" name="verticalLayout_16">
              <item>
               <layout class="QGridLayout" name="gridLayout_15" columnstretch="0,1">
-               <item row="12" column="0">
+               <item row="13" column="0">
                 <widget class="QSpinBox" name="spinBox_textureBrightnessContrastRatioHigh">
                  <property name="suffix">
                   <string> %</string>
@@ -1608,7 +1695,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </property>
                 </widget>
                </item>
-               <item row="12" column="1">
+               <item row="13" column="1">
                 <widget class="QLabel" name="label_meshingTextureSize_6">
                  <property name="text">
                   <string>Brightness and contrast balance high ratio. Only used when textures are merged.</string>
@@ -1618,7 +1705,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </property>
                 </widget>
                </item>
-               <item row="10" column="0">
+               <item row="11" column="0">
                 <widget class="QComboBox" name="comboBox_blendingDecimation">
                  <item>
                   <property name="text">
@@ -1662,14 +1749,14 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </item>
                 </widget>
                </item>
-               <item row="9" column="0">
+               <item row="10" column="0">
                 <widget class="QCheckBox" name="checkBox_blending">
                  <property name="text">
                   <string/>
                  </property>
                 </widget>
                </item>
-               <item row="10" column="1">
+               <item row="11" column="1">
                 <widget class="QLabel" name="label_exposureFusion_3">
                  <property name="text">
                   <string>Blending decimation. </string>
@@ -1699,7 +1786,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </property>
                 </widget>
                </item>
-               <item row="9" column="1">
+               <item row="10" column="1">
                 <widget class="QLabel" name="label_exposureFusion_2">
                  <property name="text">
                   <string>Blending. Only used with dense reconstruction flavor and if clouds are assembled.</string>
@@ -1741,7 +1828,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </property>
                 </widget>
                </item>
-               <item row="11" column="1">
+               <item row="12" column="1">
                 <widget class="QLabel" name="label_meshingTextureSize_5">
                  <property name="text">
                   <string>Brightness and contrast balance low ratio. Only used when textures are merged.</string>
@@ -1824,7 +1911,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </item>
                 </widget>
                </item>
-               <item row="14" column="0">
+               <item row="15" column="0">
                 <widget class="QCheckBox" name="checkBox_exposureFusion">
                  <property name="text">
                   <string/>
@@ -1854,14 +1941,14 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </property>
                 </widget>
                </item>
-               <item row="8" column="0">
+               <item row="9" column="0">
                 <widget class="QCheckBox" name="checkBox_cameraFilter">
                  <property name="text">
                   <string/>
                  </property>
                 </widget>
                </item>
-               <item row="11" column="0">
+               <item row="12" column="0">
                 <widget class="QSpinBox" name="spinBox_textureBrightnessContrastRatioLow">
                  <property name="suffix">
                   <string> %</string>
@@ -1884,7 +1971,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </property>
                 </widget>
                </item>
-               <item row="8" column="1">
+               <item row="9" column="1">
                 <widget class="QLabel" name="label_meshingTextureSize_3">
                  <property name="text">
                   <string>Camera filtering. Some criteria to select which cameras are used for texturing.</string>
@@ -1894,7 +1981,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </property>
                 </widget>
                </item>
-               <item row="14" column="1">
+               <item row="15" column="1">
                 <widget class="QLabel" name="label_exposureFusion">
                  <property name="text">
                   <string>Exposure fusion. Only used when textures are merged and brightness/contrast balance ratios are used (merging original texture + 1 for each ratios).</string>
@@ -1978,7 +2065,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </property>
                 </widget>
                </item>
-               <item row="15" column="1">
+               <item row="16" column="1">
                 <widget class="QLabel" name="label_multiband">
                  <property name="text">
                   <string>MultiBand texturing. Only available on export and RTAB-Map should be built with AliceVision support.</string>
@@ -1988,13 +2075,30 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </property>
                 </widget>
                </item>
-               <item row="15" column="0">
+               <item row="16" column="0">
                 <widget class="QCheckBox" name="checkBox_multiband">
                  <property name="text">
                   <string/>
                  </property>
                 </widget>
                </item>
+               <item row="8" column="1">
+                <widget class="QLabel" name="label_meshingTextureSize_11">
+                 <property name="text">
+                  <string>Distance to camera policy. The closest camera from a polygon is used to texture the polygon. If disabled, the camera for which the polygon projection is the closest of the image center is used to texture the polygon.</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="8" column="0">
+                <widget class="QCheckBox" name="checkBox_distanceToCamPolicy">
+                 <property name="text">
+                  <string/>
+                 </property>
+                </widget>
+               </item>
               </layout>
              </item>
              <item>

guilib/src/ui/mainWindow.ui

@@ -228,6 +228,16 @@
        </property>
        <addaction name="actionRealSense2_SR300"/>
       </widget>
+      <widget class="QMenu" name="menuRealSense_L515">
+       <property name="title">
+        <string>RealSense L515</string>
+       </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/l515.png</normaloff>:/images/l515.png</iconset>
+       </property>
+       <addaction name="actionRealSense2_L515"/>
+      </widget>
       <addaction name="menuKinect_for_Xbox_360"/>
       <addaction name="menuXtion_PRO_LIVE"/>
       <addaction name="menuSense_3D_scanner"/>
@@ -238,6 +248,7 @@
       <addaction name="menuRealSense_SR300"/>
       <addaction name="menuRealSense_D415"/>
       <addaction name="menuRealSense_D435"/>
+      <addaction name="menuRealSense_L515"/>
      </widget>
      <widget class="QMenu" name="menuStereo_camera">
       <property name="title">
@@ -1617,6 +1628,14 @@
     <string>MYNT EYE S SDK</string>
    </property>
   </action>
+  <action name="actionRealSense2_L515">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>RealSense2</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-1179</y>
+            <y>0</y>
             <width>686</width>
-            <height>3236</height>
+            <height>3286</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>21</number>
+                <number>10</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -3064,7 +3064,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                       <item row="5" column="1">
                        <widget class="QLabel" name="label_36">
                         <property name="text">
-                         <string>Image decimation. RGB/Mono and depth images will be resized according to this value (size*1/decimation). Note that if depth images are captured, decimation should be a multiple of the depth image size.</string>
+                         <string>Image decimation. RGB/Mono and depth images will be resized according to this value (size*1/decimation). Note that if depth images are captured, decimation should be a multiple of the depth image size. If depth images are smaller than RGB images, the decimation is first applied on RGB, if the resulting RGB image is still bigger than depth image, the depth is not decimated.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -3220,7 +3220,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_rgbd">
                              <property name="currentIndex">
-                              <number>10</number>
+                              <number>9</number>
                              </property>
                              <widget class="QWidget" name="page_32">
                               <layout class="QVBoxLayout" name="verticalLayout_63">
@@ -4173,10 +4173,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                   <string>RealSense2</string>
                                  </property>
                                  <layout class="QGridLayout" name="gridLayout_100" columnstretch="0,0,1">
-                                  <item row="8" column="1">
+                                  <item row="9" column="1">
                                    <widget class="QLineEdit" name="lineEdit_rs2_jsonFile"/>
                                   </item>
-                                  <item row="6" column="2">
+                                  <item row="7" column="2">
                                    <widget class="QLabel" name="label_565">
                                     <property name="text">
                                      <string>Dual Mode (D400+T265): Odometry is computed by T265 and RGB-D frames are from D400.</string>
@@ -4184,6 +4184,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     <property name="wordWrap">
                                      <bool>true</bool>
                                     </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
                                    </widget>
                                   </item>
                                   <item row="3" column="2">
@@ -4194,6 +4197,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     <property name="wordWrap">
                                      <bool>true</bool>
                                     </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
                                    </widget>
                                   </item>
                                   <item row="4" column="1">
@@ -4234,6 +4240,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     <property name="wordWrap">
                                      <bool>true</bool>
                                     </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
                                    </widget>
                                   </item>
                                   <item row="1" column="1">
@@ -4254,6 +4263,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     <property name="wordWrap">
                                      <bool>true</bool>
                                     </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
                                    </widget>
                                   </item>
                                   <item row="3" column="1">
@@ -4274,9 +4286,12 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     <property name="wordWrap">
                                      <bool>true</bool>
                                     </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
                                    </widget>
                                   </item>
-                                  <item row="9" column="1">
+                                  <item row="10" column="1">
                                    <spacer name="verticalSpacer_71">
                                     <property name="orientation">
                                      <enum>Qt::Vertical</enum>
@@ -4297,6 +4312,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     <property name="wordWrap">
                                      <bool>true</bool>
                                     </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
                                    </widget>
                                   </item>
                                   <item row="2" column="2">
@@ -4307,6 +4325,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     <property name="wordWrap">
                                      <bool>true</bool>
                                     </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
                                    </widget>
                                   </item>
                                   <item row="2" column="1">
@@ -4319,7 +4340,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="6" column="1">
+                                  <item row="7" column="1">
                                    <widget class="QCheckBox" name="checkbox_rs2_dualMode">
                                     <property name="text">
                                      <string/>
@@ -4329,7 +4350,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="7" column="1">
+                                  <item row="8" column="1">
                                    <widget class="QLineEdit" name="lineEdit_rs2_dualModeExtrinsics">
                                     <property name="toolTip">
                                      <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /gray_camera = 0 0 1 -1 0 0 0 -1 0&lt;br/&gt;KITTI: /base_link to /color_camera = 0 0 1 0 -1 0 0 -0.06 0 -1 0 0&lt;br/&gt;KITTI: /base_footprint to /gray_camera = 0 0 1 0 -1 0 0 0 0 -1 0 1.67&lt;br/&gt;KITTI: /base_footprint to /color_camera = 0 0 1 0 -1 0 0 -0.06 0 -1 0 1.67&lt;/p&gt;&lt;p&gt;EuRoC MAV: /base_link to /cam0 = T_BS*T_SC0 = -0.0257742 0.00375623 0.999661 0.00981073 -0.999557 -0.0149672 -0.0257155 0.064677 0.0148655 -0.999881 0.00414038 -0.0216401&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -4339,7 +4360,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="7" column="2">
+                                  <item row="8" column="2">
                                    <widget class="QLabel" name="label_566">
                                     <property name="text">
                                      <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Dual Mode extrinsics (T265's pose frame to D400's left IR camera). Default extrinsics match the 3D printed bracket &lt;a href=&quot; https://www.intelrealsense.com/depth-and-tracking-combined-get-started/&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;here&lt;/span&gt;&lt;/a&gt; (&lt;a href=&quot;https://github.com/IntelRealSense/realsense-ros/blob/occupancy-mapping/realsense2_camera/meshes/mount_t265_d435.stl&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;stl&lt;/span&gt;&lt;/a&gt;).&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -4350,9 +4371,12 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     <property name="openExternalLinks">
                                      <bool>true</bool>
                                     </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
                                    </widget>
                                   </item>
-                                  <item row="8" column="2">
+                                  <item row="9" column="2">
                                    <widget class="QLabel" name="label_571">
                                     <property name="text">
                                      <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;D400 Series Visual Presets. See this &lt;a href=&quot;https://github.com/IntelRealSense/librealsense/wiki/D400-Series-Visual-Presets&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;page&lt;/span&gt;&lt;/a&gt;.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -4363,15 +4387,41 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     <property name="openExternalLinks">
                                      <bool>true</bool>
                                     </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
                                    </widget>
                                   </item>
-                                  <item row="8" column="0">
+                                  <item row="9" column="0">
                                    <widget class="QToolButton" name="toolButton_rs2_jsonFile">
                                     <property name="text">
                                      <string>...</string>
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="6" column="2">
+                                   <widget class="QLabel" name="label_606">
+                                    <property name="text">
+                                     <string>Global time sync. This will make sure IMU data is interpolated at image timestamp, otherwise latest received IMU is synchronized to current frame. Set to off if your firmware of your camera cannot synchronize timestamps.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="6" column="1">
+                                   <widget class="QCheckBox" name="checkbox_rs2_globalTimeStync">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                    <property name="checked">
+                                     <bool>false</bool>
+                                    </property>
+                                   </widget>
+                                  </item>
                                  </layout>
                                 </widget>
                                </item>
@@ -5877,6 +5927,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
+                           <item row="1" column="0">
+                            <widget class="QCheckBox" name="source_checkBox_ignoreOdometry">
+                             <property name="text">
+                              <string/>
+                             </property>
+                            </widget>
+                           </item>
                            <item row="0" column="2">
                             <widget class="QToolButton" name="toolButton_dbViewer">
                              <property name="toolTip">
@@ -5891,13 +5948,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
-                           <item row="1" column="0">
-                            <widget class="QCheckBox" name="source_checkBox_ignoreOdometry">
-                             <property name="text">
-                              <string/>
-                             </property>
-                            </widget>
-                           </item>
                            <item row="0" column="0">
                             <widget class="QToolButton" name="source_database_toolButton_selectSource">
                              <property name="text">
@@ -5908,7 +5958,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item row="0" column="1">
                             <widget class="QLineEdit" name="source_database_lineEdit_path"/>
                            </item>
-                           <item row="8" column="1">
+                           <item row="9" column="1">
                             <spacer name="verticalSpacer_35">
                              <property name="orientation">
                               <enum>Qt::Vertical</enum>
@@ -6040,6 +6090,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
+                           <item row="8" column="1">
+                            <widget class="QLabel" name="label_557">
+                             <property name="text">
+                              <string>If the database contains stereo data, generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab.</string>
+                             </property>
+                             <property name="wordWrap">
+                              <bool>true</bool>
+                             </property>
+                             <property name="textInteractionFlags">
+                              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="8" column="0">
+                            <widget class="QCheckBox" name="source_checkBox_stereoToDepthDB">
+                             <property name="text">
+                              <string/>
+                             </property>
+                            </widget>
+                           </item>
                           </layout>
                          </widget>
                         </item>
@@ -6067,7 +6137,20 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        </property>
                        <item>
                         <layout class="QGridLayout" name="gridLayout_68" columnstretch="0,0,1">
-                         <item row="7" column="2">
+                         <item row="8" column="2">
+                          <widget class="QLabel" name="label_288">
+                           <property name="text">
+                            <string>Ground truth file. Select the correct format below.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="9" column="2">
                           <widget class="QLabel" name="label_289">
                            <property name="text">
                             <string>Ground truth format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images.</string>
@@ -6093,21 +6176,21 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="6" column="0">
+                         <item row="8" column="0">
                           <widget class="QToolButton" name="toolButton_cameraImages_gt">
                            <property name="text">
                             <string>...</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="4" column="1">
+                         <item row="6" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_odom">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="9" column="2">
+                         <item row="11" column="2">
                           <widget class="QLabel" name="label_293">
                            <property name="text">
                             <string>Path to directory containing optional laser scans (*.pcd, *.ply, *.bin [KITTI format]). The directory should have the same size has the images directory. </string>
@@ -6120,7 +6203,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="4" column="2">
+                         <item row="6" column="2">
                           <widget class="QLabel" name="label_348">
                            <property name="text">
                             <string>Odometry file. Select the correct format below.</string>
@@ -6133,7 +6216,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="1" column="2">
+                         <item row="2" column="2">
                           <widget class="QLabel" name="label_255">
                            <property name="text">
                             <string>Use file names as timestamps. Format is epoch time. Example: &quot;1305031102.175304.png&quot;.</string>
@@ -6146,55 +6229,42 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="0">
+                         <item row="4" column="0">
                           <widget class="QToolButton" name="toolButton_cameraImages_timestamps">
                            <property name="text">
                             <string>...</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="1">
+                         <item row="4" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_timestamps">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="6" column="1">
+                         <item row="8" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_gt">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="1">
+                         <item row="3" column="1">
                           <widget class="QCheckBox" name="checkBox_cameraImages_syncTimeStamps">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="6" column="2">
-                          <widget class="QLabel" name="label_288">
-                           <property name="text">
-                            <string>Ground truth file. Select the correct format below.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="1" column="1">
+                         <item row="2" column="1">
                           <widget class="QCheckBox" name="checkBox_cameraImages_timestamps">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="11" column="2">
+                         <item row="13" column="2">
                           <widget class="QLabel" name="label_292">
                            <property name="text">
                             <string>Maximum laser scan points.</string>
@@ -6207,7 +6277,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="2">
+                         <item row="3" column="2">
                           <widget class="QLabel" name="label_256">
                            <property name="text">
                             <string>Synchronize capture rate with timestamps.</string>
@@ -6220,14 +6290,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="9" column="1">
+                         <item row="11" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_path_scans">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="2">
+                         <item row="4" column="2">
                           <widget class="QLabel" name="label_251">
                            <property name="text">
                             <string>Timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if &quot;Use file names as timestamps&quot; above is checked. </string>
@@ -6240,7 +6310,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="11" column="1">
+                         <item row="13" column="1">
                           <widget class="QSpinBox" name="spinBox_cameraImages_max_scan_pts">
                            <property name="toolTip">
                             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -6250,7 +6320,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="7" column="1">
+                         <item row="9" column="1">
                           <widget class="QComboBox" name="comboBox_cameraImages_gtFormat">
                            <property name="toolTip">
                             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Raw Format (3 values): x y z&lt;br/&gt;Raw Format (6 values): x y z roll pitch yaw&lt;br/&gt;Raw Format (7 values): x y z qx qy qz qw&lt;br/&gt;Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;RGBD-SLAM (stamp tx ty tz qx qy qz qw)&lt;br/&gt;KITTI (stamp + 12 values transform)&lt;br/&gt;TORO&lt;br/&gt;g2o&lt;br/&gt;NewCollege (stamp x y)&lt;br/&gt;Malaga Urban (GPS)&lt;br/&gt;St Lucia Stereo (INS)&lt;br/&gt;EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -6315,14 +6385,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </item>
                           </widget>
                          </item>
-                         <item row="9" column="0">
+                         <item row="11" column="0">
                           <widget class="QToolButton" name="toolButton_cameraImages_path_scans">
                            <property name="text">
                             <string>...</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="10" column="1">
+                         <item row="12" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_laser_transform">
                            <property name="toolTip">
                             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0&lt;br/&gt;KITTI: /base_footprint to /scan = -0.27 0 1.75 0 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -6332,7 +6402,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="10" column="2">
+                         <item row="12" column="2">
                           <widget class="QLabel" name="label_294">
                            <property name="text">
                             <string>Local transform from /base_link to /scan_link. Mouse over the box to show formats.</string>
@@ -6377,7 +6447,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </item>
                           </widget>
                          </item>
-                         <item row="5" column="2">
+                         <item row="7" column="2">
                           <widget class="QLabel" name="label_349">
                            <property name="text">
                             <string>Odometry format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images.</string>
@@ -6390,7 +6460,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="5" column="1">
+                         <item row="7" column="1">
                           <widget class="QComboBox" name="comboBox_cameraImages_odomFormat">
                            <property name="toolTip">
                             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Raw Format (3 values): x y z&lt;br/&gt;Raw Format (6 values): x y z roll pitch yaw&lt;br/&gt;Raw Format (7 values): x y z qx qy qz qw&lt;br/&gt;Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;RGBD-SLAM (stamp tx ty tz qx qy qz qw)&lt;br/&gt;KITTI (stamp + 12 values transform)&lt;br/&gt;TORO&lt;br/&gt;g2o&lt;br/&gt;NewCollege (stamp x y)&lt;br/&gt;Malaga Urban (GPS)&lt;br/&gt;St Lucia Stereo (INS)&lt;br/&gt;EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -6455,14 +6525,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </item>
                           </widget>
                          </item>
-                         <item row="4" column="0">
+                         <item row="6" column="0">
                           <widget class="QToolButton" name="toolButton_cameraImages_odom">
                            <property name="text">
                             <string>...</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="8" column="2">
+                         <item row="10" column="2">
                           <widget class="QLabel" name="label_443">
                            <property name="text">
                             <string>Max time difference between data and corresponding pose for format with stamps. If delay is over this threshold, the pose won't be set on data loaded. This is used when odometry and/or ground truth files are set.</string>
@@ -6475,7 +6545,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="8" column="1">
+                         <item row="10" column="1">
                           <widget class="QDoubleSpinBox" name="doubleSpinBox_maxPoseTimeDiff">
                            <property name="suffix">
                             <string> s</string>
@@ -6494,7 +6564,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="13" column="2">
+                         <item row="15" column="2">
                           <widget class="QLabel" name="label_464">
                            <property name="text">
                             <string>Local transform from /base_link to /imu_link. Mouse over the box to show formats.</string>
@@ -6507,7 +6577,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="12" column="2">
+                         <item row="14" column="2">
                           <widget class="QLabel" name="label_463">
                            <property name="text">
                             <string>Path to file containing optional IMU data (*.csv [EuRoC format]).</string>
@@ -6520,21 +6590,21 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="12" column="0">
+                         <item row="14" column="0">
                           <widget class="QToolButton" name="toolButton_cameraImages_path_imu">
                            <property name="text">
                             <string>...</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="12" column="1">
+                         <item row="14" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_path_imu">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="13" column="1">
+                         <item row="15" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_imu_transform">
                            <property name="toolTip">
                             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;EuRoC: /base_link to /imu = 0 0 1 0 -1 0 1 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -6544,7 +6614,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="14" column="2">
+                         <item row="16" column="2">
                           <widget class="QLabel" name="label_465">
                            <property name="text">
                             <string>IMU Rate. To synchronize capture rate with IMU timestamps, set to 0. This can be set a little over the actual IMU rate to keep up with camera capture rate if images are dropped by odometry.</string>
@@ -6557,7 +6627,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="14" column="1">
+                         <item row="16" column="1">
                           <widget class="QSpinBox" name="spinBox_cameraImages_max_imu_rate">
                            <property name="toolTip">
                             <string>EuRoC: 200 Hz -&gt; 250 Hz</string>
@@ -6567,6 +6637,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
+                         <item row="1" column="2">
+                          <widget class="QLabel" name="label_605">
+                           <property name="text">
+                            <string>Load config file for each frame (intrinsics, pose and stamp). Config files should be in the same directory than RGB frames and they should have the same name than the corresponding frame file. Currently supporting only 3DScannerApp for iOS export config format (JSON).</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="1" column="1">
+                          <widget class="QCheckBox" name="checkBox_cameraImages_configForEachFrame">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                        <item>
@@ -7227,7 +7317,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                          <item row="7" column="1">
                           <widget class="QLabel" name="label_453">
                            <property name="text">
-                            <string>Force ground normals to be all upward. Useful to make sure Velodyne's scans on ground have all normals upward.</string>
+                            <string>Force ground normals to be all upward (e.g., 0.8). Useful to make sure Velodyne's scans on ground have all normals upward. 0 means disabled.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -7237,13 +7327,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="7" column="0">
-                          <widget class="QCheckBox" name="checkBox_source_scanForceGroundNormalsUp">
-                           <property name="text">
-                            <string/>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="3" column="0">
                           <widget class="QDoubleSpinBox" name="doubleSpinBox_source_scanRangeMax">
                            <property name="suffix">
@@ -7300,6 +7383,19 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
+                         <item row="7" column="0">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_source_scanNormalsForceGroundUp">
+                           <property name="toolTip">
+                            <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                           </property>
+                           <property name="maximum">
+                            <double>1.000000000000000</double>
+                           </property>
+                           <property name="singleStep">
+                            <double>0.010000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>
@@ -8969,6 +9065,16 @@ generate the number of words requested.</string>
                              <string>SURF+FREAK</string>
                             </property>
                            </item>
+                           <item>
+                            <property name="text">
+                             <string>GFTT+DAISY</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>SURF+DAISY</string>
+                            </property>
+                           </item>
                           </widget>
                          </item>
                          <item row="7" column="0">
@@ -9708,7 +9814,7 @@ When set to false, no new words are added to dictionary, so no more updates are
                       </property>
                      </widget>
                     </item>
-                    <item row="5" column="0">
+                    <item row="6" column="0">
                      <widget class="QComboBox" name="comboBox_dbJournalMode">
                       <item>
                        <property name="text">
@@ -9750,7 +9856,7 @@ When set to false, no new words are added to dictionary, so no more updates are
                       </property>
                      </widget>
                     </item>
-                    <item row="7" column="1">
+                    <item row="8" column="1">
                      <widget class="QLabel" name="label_120">
                       <property name="text">
                        <string>Sqlite3 temp store,
@@ -9764,7 +9870,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                       </property>
                      </widget>
                     </item>
-                    <item row="7" column="0">
+                    <item row="8" column="0">
                      <widget class="QComboBox" name="comboBox_dbTempStore">
                       <item>
                        <property name="text">
@@ -9793,7 +9899,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                       </property>
                      </widget>
                     </item>
-                    <item row="4" column="0">
+                    <item row="5" column="0">
                      <widget class="QSpinBox" name="spinBox_dbCacheSize">
                       <property name="minimum">
                        <number>10</number>
@@ -9809,7 +9915,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                       </property>
                      </widget>
                     </item>
-                    <item row="4" column="1">
+                    <item row="5" column="1">
                      <widget class="QLabel" name="label_75">
                       <property name="text">
                        <string>Sqlite3 cache size,
@@ -9823,7 +9929,7 @@ see Sqlite3 doc 'PRAGMA cache_size'.</string>
                       </property>
                      </widget>
                     </item>
-                    <item row="5" column="1">
+                    <item row="6" column="1">
                      <widget class="QLabel" name="label_77">
                       <property name="text">
                        <string>Sqlite3 journal mode,
@@ -9837,7 +9943,7 @@ see Sqlite3 doc 'PRAGMA journal_mode'.</string>
                       </property>
                      </widget>
                     </item>
-                    <item row="6" column="1">
+                    <item row="7" column="1">
                      <widget class="QLabel" name="label_89">
                       <property name="text">
                        <string>Sqlite3 synchronous,
@@ -9851,7 +9957,7 @@ see Sqlite3 doc 'PRAGMA synchronous'.</string>
                       </property>
                      </widget>
                     </item>
-                    <item row="6" column="0">
+                    <item row="7" column="0">
                      <widget class="QComboBox" name="comboBox_dbSynchronous">
                       <property name="currentIndex">
                        <number>2</number>
@@ -9902,6 +10008,26 @@ see Sqlite3 doc 'PRAGMA synchronous'.</string>
                     <item row="3" column="0">
                      <widget class="QLineEdit" name="lineEdit_rgbCompressionFormat"/>
                     </item>
+                    <item row="4" column="1">
+                     <widget class="QLabel" name="label_610">
+                      <property name="text">
+                       <string>Target database version for backward compatibility purpose. Only Major and minor versions are used and should be set (e.g., &quot;0.19&quot; vs &quot;0.20&quot; or &quot;1.0&quot; vs &quot;2.0&quot;). Patch version is ignored (e.g., &quot;0.20.1&quot; and &quot;0.20.3&quot; will generate a &quot;0.20&quot; database).</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                      <property name="textInteractionFlags">
+                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="4" column="0">
+                     <widget class="QLineEdit" name="lineEdit_targetDatabaseVersion">
+                      <property name="placeholderText">
+                       <string>major.minor</string>
+                      </property>
+                     </widget>
+                    </item>
                    </layout>
                   </widget>
                  </item>
@@ -12550,6 +12676,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                    <layout class="QVBoxLayout" name="verticalLayout_136">
                     <item>
                      <layout class="QGridLayout" name="gridLayout_63" columnstretch="0,1">
+                      <item row="4" column="1">
+                       <widget class="QLabel" name="label_space2_9">
+                        <property name="text">
+                         <string>Angular variance to set on marker detections. Set to &gt;=9999 to use only position (xyz) constraint in graph optimization.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="3" column="0">
                        <widget class="QDoubleSpinBox" name="ArucoVarianceLinear">
                         <property name="suffix">
@@ -12608,19 +12747,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="4" column="1">
-                       <widget class="QLabel" name="label_space2_9">
-                        <property name="text">
-                         <string>Angular variance to set on marker detections. Set to &gt;=9999 to use only position (xyz) constraint in graph optimization.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="0" column="0">
                        <widget class="QCheckBox" name="RGBDMarkerDetection">
                         <property name="text">
@@ -12701,6 +12827,76 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="5" column="1">
+                       <widget class="QLabel" name="label_space2_13">
+                        <property name="text">
+                         <string>Minimum detection range (0=disabled).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="6" column="1">
+                       <widget class="QLabel" name="label_space2_14">
+                        <property name="text">
+                         <string>Maximum detection range (0=unlimited).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="5" column="0">
+                       <widget class="QDoubleSpinBox" name="ArucoMarkerRangeMin">
+                        <property name="suffix">
+                         <string> m</string>
+                        </property>
+                        <property name="decimals">
+                         <number>2</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>999.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.000000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="6" column="0">
+                       <widget class="QDoubleSpinBox" name="ArucoMarkerRangeMax">
+                        <property name="suffix">
+                         <string> m</string>
+                        </property>
+                        <property name="decimals">
+                         <number>2</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>999.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.000000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
@@ -18249,6 +18445,16 @@ Lower the ratio -&gt; higher the precision.</string>
                              <string>SURF+FREAK</string>
                             </property>
                            </item>
+                           <item>
+                            <property name="text">
+                             <string>GFTT+DAISY</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>SURF+DAISY</string>
+                            </property>
+                           </item>
                           </widget>
                          </item>
                          <item row="3" column="0">
@@ -18857,10 +19063,10 @@ Lower the ratio -&gt; higher the precision.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="13" column="1">
+                      <item row="14" column="1">
                        <widget class="QLabel" name="label_429">
                         <property name="text">
-                         <string>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.</string>
+                         <string>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 PointToPlane is enabled.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -18870,7 +19076,7 @@ Lower the ratio -&gt; higher the precision.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="13" column="0">
+                      <item row="14" column="0">
                        <widget class="QDoubleSpinBox" name="loopClosure_icpPointToPlaneNormalsMinComplexity">
                         <property name="decimals">
                          <number>3</number>
@@ -18950,6 +19156,64 @@ Lower the ratio -&gt; higher the precision.</string>
                         </property>
                        </widget>
                       </item>
+                      <item row="15" column="1">
+                       <widget class="QLabel" name="label_607">
+                        <property name="text">
+                         <string>Stuctural complexity strategy. If structural complexity is below the minimum complexity threshold: 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 &quot;as is&quot; the transform computed by PointToPoint.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="13" column="0">
+                       <widget class="QDoubleSpinBox" name="loopClosure_icpPointToPlaneGroundNormalsUp">
+                        <property name="decimals">
+                         <number>2</number>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="13" column="1">
+                       <widget class="QLabel" name="label_608">
+                        <property name="text">
+                         <string>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.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="15" column="0">
+                       <widget class="QComboBox" name="loopClosure_icpPointToPlaneLowComplexityStrategy">
+                        <item>
+                         <property name="text">
+                          <string>Reject</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>Constrained PointToPoint</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>PointToPoint</string>
+                         </property>
+                        </item>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
@@ -19096,6 +19360,26 @@ Lower the ratio -&gt; higher the precision.</string>
                            </property>
                           </widget>
                          </item>
+                         <item row="3" column="1">
+                          <widget class="QLabel" name="label_609">
+                           <property name="text">
+                            <string>KDTreeMatcher:  among nearest neighbors, keep only the one with the most similar intensity. This only work with knn&gt;1.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="3" column="0">
+                          <widget class="QCheckBox" name="loopClosure_icpPMMatcherIntensity">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.20.2</version>
+  <version>0.20.7</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -12,9 +12,8 @@
 
   <buildtool_depend>cmake</buildtool_depend>
   
-  <build_depend>libvtk-qt</build_depend>
   <build_depend>qt_gui_cpp</build_depend> <!-- libqt4-dev or libqt5-dev -->
-  <build_depend>libpcl-all-dev</build_depend>
+  <build_depend>libpcl-all-dev</build_depend> <!--  include libvtk-qt -->
   <build_depend>libsqlite3-dev</build_depend>
   <build_depend>zlib</build_depend>
   <build_depend>libfreenect-dev</build_depend>
@@ -26,9 +25,8 @@
   <build_depend>octomap</build_depend>
   <build_depend>libg2o</build_depend>
   
-  <run_depend>libvtk-qt</run_depend>
   <run_depend>qt_gui_cpp</run_depend>
-  <run_depend>libpcl-all-dev</run_depend>
+  <run_depend>libpcl-all-dev</run_depend> <!--  include libvtk-qt -->
   <run_depend>libsqlite3-dev</run_depend>
   <run_depend>zlib</run_depend>
   <run_depend>libfreenect-dev</run_depend>

tools/DataRecorder/main.cpp

@@ -308,6 +308,7 @@ int main (int argc, char * argv[])
 		UFATAL("Camera driver (%d) not found!", driver);
 	}
 	cam = new CameraThread(camera);
+	cam->enableIMUFiltering();
 
 	DataRecorder recorder;
 

tools/Export/main.cpp

@@ -55,17 +55,22 @@ void showUsage()
 			"    --texture_size  #     Texture size (default 4096).\n"
 			"    --texture_count #     Maximum textures generated (default 1).\n"
 			"    --texture_range #     Maximum camera range for texturing a polygon (default 0 meters: no limit).\n"
+			"    --texture_d2c         Distance to camera policy.\n"
 			"    --ba                  Do global bundle adjustment before assembling the clouds.\n"
-			"    --no_gain             Disable gain compensation when texturing.\n"
+			"    --gain          #     Gain compensation value (default 1, set 0 to disable).\n"
+			"    --gain_gray           Do gain estimation compensation on gray channel only (default RGB channels).\n"
 			"    --no_blending         Disable blending when texturing.\n"
 			"    --no_clean            Disable cleaning colorless polygons.\n"
+			"    --low_gain      #     Low brightness gain 0-100 (default 0).\n"
+			"    --high_gain     #     High brightness gain 0-100 (default 10).\n"
 			"    --multiband           Enable multiband texturing (AliceVision dependency required).\n"
 			"    --poisson_depth #     Set Poisson depth for mesh reconstruction.\n"
 			"    --max_polygons  #     Maximum polygons when creating a mesh (default 500000, set 0 for no limit).\n"
-			"    --max_range     #     Maximum range of the created clouds (default 4 m).\n"
-			"    --decimation    #     Depth image decimation before creating the clouds (default 4).\n"
-			"    --voxel         #     Voxel size of the created clouds (default 0.01 m).\n"
-			"    --color_radius  #     Radius used to colorize polygons (default 0.05 m, set 0 for nearest color).\n"
+			"    --max_range     #     Maximum range of the created clouds (default 4 m, 0 m with --scan).\n"
+			"    --decimation    #     Depth image decimation before creating the clouds (default 4, 1 with --scan).\n"
+			"    --voxel         #     Voxel size of the created clouds (default 0.01 m, 0 m with --scan).\n"
+			"    --color_radius  #     Radius used to colorize polygons (default 0.05 m, 0 m with --scan). Set 0 for nearest color.\n"
+			"    --scan                Use laser scan for the point cloud.\n"
 			"    --save_in_db          Save resulting assembled point cloud or mesh in the database.\n"
 			"\n%s", Parameters::showUsage());
 	;
@@ -85,21 +90,26 @@ int main(int argc, char * argv[])
 	bool mesh = false;
 	bool texture = false;
 	bool ba = false;
-	bool doGainCompensation = true;
+	bool doGainCompensationRGB = true;
+	float gainValue = 1;
 	bool doBlending = true;
 	bool doClean = true;
 	int poissonDepth = 0;
 	int maxPolygons = 500000;
-	int decimation = 4;
-	float maxRange = 4.0f;
-	float voxelSize = 0.01f;
+	int decimation = -1;
+	float maxRange = -1.0f;
+	float voxelSize = -1.0f;
 	int textureSize = 4096;
 	int textureCount = 1;
 	int textureRange = 0;
+	bool distanceToCamPolicy = false;
 	bool multiband = false;
-	float colorRadius = 0.05;
+	float colorRadius = -1.0f;
+	bool cloudFromScan = false;
 	bool saveInDb = false;
-	for(int i=1; i<argc-1; ++i)
+	int lowBrightnessGain = 0;
+	int highBrightnessGain = 10;
+	for(int i=1; i<argc; ++i)
 	{
 		if(std::strcmp(argv[i], "--help") == 0)
 		{
@@ -150,13 +160,30 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(std::strcmp(argv[i], "--texture_d2c") == 0)
+		{
+			distanceToCamPolicy = true;
+		}
 		else if(std::strcmp(argv[i], "--ba") == 0)
 		{
 			ba = true;
 		}
-		else if(std::strcmp(argv[i], "--no_gain") == 0)
+		else if(std::strcmp(argv[i], "--gain_gray") == 0)
 		{
-			doGainCompensation = false;
+			doGainCompensationRGB = false;
+		}
+		else if(std::strcmp(argv[i], "--gain") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				gainValue = uStr2Float(argv[i]);
+				UASSERT(gainValue>0.0f);
+			}
+			else
+			{
+				showUsage();
+			}
 		}
 		else if(std::strcmp(argv[i], "--no_blending") == 0)
 		{
@@ -246,10 +273,55 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(std::strcmp(argv[i], "--scan") == 0)
+		{
+			cloudFromScan = true;
+		}
 		else if(std::strcmp(argv[i], "--save_in_db") == 0)
 		{
 			saveInDb = true;
 		}
+		else if(std::strcmp(argv[i], "--low_gain") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				lowBrightnessGain = uStr2Int(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--high_gain") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				highBrightnessGain = uStr2Int(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+	}
+
+	if(decimation < 1)
+	{
+		decimation = cloudFromScan?1:4;
+	}
+	if(maxRange < 0)
+	{
+		maxRange = cloudFromScan?0:4;
+	}
+	if(voxelSize < 0.0f)
+	{
+		voxelSize = cloudFromScan?0:0.01f;
+	}
+	if(colorRadius < 0.0f)
+	{
+		colorRadius = cloudFromScan?0:0.05f;
 	}
 
 	if(saveInDb)
@@ -305,7 +377,13 @@ int main(int argc, char * argv[])
 	std::multimap<int, Link> links;
 	printf("Optimizing the map...\n");
 	rtabmap.getGraph(optimizedPoses, links, true, true, &nodes, true, true, true, true);
-	printf("Optimizing the map... done (%fs).\n", timer.ticks());
+	printf("Optimizing the map... done (%fs, poses=%d).\n", timer.ticks(), (int)optimizedPoses.size());
+
+	if(optimizedPoses.empty())
+	{
+		printf("The optimized graph is empty!? Aborting...\n");
+		return -1;
+	}
 
 	std::string outputDirectory = UDirectory::getDir(dbPath);
 	std::string baseName = uSplit(UFile::getName(dbPath), '.').front();
@@ -340,27 +418,49 @@ int main(int argc, char * argv[])
 		Signature node = nodes.find(iter->first)->second;
 
 		// uncompress data
-		node.sensorData().uncompressData();
 		std::vector<CameraModel> models = node.sensorData().cameraModels();
-		cv::Mat depth = node.sensorData().depthRaw();
+		cv::Mat depth;
 
 		pcl::IndicesPtr indices(new std::vector<int>);
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
-				node.sensorData(),
-				decimation,      // image decimation before creating the clouds
-				maxRange,        // maximum depth of the cloud
-				0.0f,
-				indices.get());
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+		if(cloudFromScan)
+		{
+			cv::Mat tmpDepth;
+			LaserScan scan;
+			node.sensorData().uncompressData(0, texture&&!node.sensorData().depthOrRightCompressed().empty()?&tmpDepth:0, &scan);
+			if(decimation>1 || maxRange)
+			{
+				scan = util3d::commonFiltering(scan, decimation, 0, maxRange);
+			}
+			cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
+		}
+		else
+		{
+			cv::Mat tmpRGB;
+			node.sensorData().uncompressData(&tmpRGB, &depth);
+			cloud = util3d::cloudRGBFromSensorData(
+					node.sensorData(),
+					decimation,      // image decimation before creating the clouds
+					maxRange,        // maximum depth of the cloud
+					0.0f,
+					indices.get());
+		}
+		if(voxelSize>0.0f)
+		{
+			cloud = rtabmap::util3d::voxelize(cloud, indices, voxelSize);
+		}
+		cloud = rtabmap::util3d::transformPointCloud(cloud, iter->second);
 
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-		transformedCloud = rtabmap::util3d::voxelize(cloud, indices, voxelSize);
-		transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second);
-
-		Eigen::Vector3f viewpoint( iter->second.x(),  iter->second.y(),  iter->second.z());
-		pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(transformedCloud, 10, 0.0f, viewpoint);
+		Eigen::Vector3f viewpoint(iter->second.x(), iter->second.y(), iter->second.z());
+		if(cloudFromScan)
+		{
+			Transform lidarViewpoint = iter->second * node.sensorData().laserScanRaw().localTransform();
+			viewpoint = Eigen::Vector3f(iter->second.x(),  iter->second.y(),  iter->second.z());
+		}
+		pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(cloud, 20, 0.0f, viewpoint);
 
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-		pcl::concatenateFields(*transformedCloud, *normals, *cloudWithNormals);
+		pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
 
 		if(mergedClouds->size() == 0)
 		{
@@ -398,8 +498,11 @@ int main(int argc, char * argv[])
 
 		if(!(mesh || texture))
 		{
-			printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", 0.01f, (int)mergedClouds->size());
-			mergedClouds = util3d::voxelize(mergedClouds, voxelSize);
+			if(voxelSize>0.0f)
+			{
+				printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", voxelSize, (int)mergedClouds->size());
+				mergedClouds = util3d::voxelize(mergedClouds, voxelSize);
+			}
 
 			if(saveInDb)
 			{
@@ -460,7 +563,7 @@ int main(int argc, char * argv[])
 					if(saveInDb)
 					{
 						printf("Saving mesh in db...\n");
-						std::vector<std::vector<std::vector<unsigned int> > > polygons;
+						std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
 						polygons.push_back(util3d::convertPolygonsFromPCL(mesh->polygons));
 						driver->saveOptimizedMesh(
 								util3d::laserScanFromPointCloud(mesh->cloud, false).data(),
@@ -490,7 +593,8 @@ int main(int argc, char * argv[])
 							multiband?0:50, // Min polygons in camera view to be textured by this camera
 							std::vector<float>(),
 							0,
-							&vertexToPixels);
+							&vertexToPixels,
+							distanceToCamPolicy);
 					printf("Texturing... done (%fs).\n", timer.ticks());
 
 					// Remove occluded polygons (polygons with no texture)
@@ -523,9 +627,9 @@ int main(int argc, char * argv[])
 								textureSize,
 								multiband?1:textureCount, // to get contrast values based on all images in multiband mode
 								vertexToPixels,
-								doGainCompensation, 1.0f, true,
+								gainValue>0.0f, gainValue, doGainCompensationRGB,
 								doBlending, 0,
-								0, 10, // low-high brightness/contrast balance
+								lowBrightnessGain, highBrightnessGain, // low-high brightness/contrast balance
 								false, // exposure fusion
 								0,     // state
 								0,     // blank value (0=black)
@@ -600,7 +704,8 @@ int main(int argc, char * argv[])
 									"jpg",
 									gains,
 									blendingGains,
-									contrastValues))
+									contrastValues,
+									doGainCompensationRGB))
 							{
 								printf("MultiBand texturing...done (%fs).\n", timer.ticks());
 							}

tools/Info/main.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <signal.h>
 
 #include <rtabmap/core/DBDriver.h>
+#include <rtabmap/core/VisualWord.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include "rtabmap/utilite/UFile.h"
 #include "rtabmap/utilite/UStl.h"
@@ -329,8 +330,27 @@ int main(int argc, char * argv[])
 			sessionsInOptGraphStr << *iter << "(" << mapIdsLinkedToLastGraph.count(*iter) << ")";
 		}
 
+		int lastWordIdId = 0;
+		int wordsDim = 0;
+		int wordsType = 0;
+		driver->getLastWordId(lastWordIdId);
+		if(lastWordIdId>0)
+		{
+			std::set<int> ids;
+			ids.insert(lastWordIdId);
+			std::list<VisualWord *> vws;
+			driver->loadWords(ids, vws);
+			if(!vws.empty())
+			{
+				wordsDim = vws.front()->getDescriptor().cols;
+				wordsType = vws.front()->getDescriptor().type();
+				delete vws.front();
+				vws.clear();
+			}
+		}
+
 		std::cout << (uFormat("%s%fs\n", pad("Total time:").c_str(), infoTotalTime));
-		std::cout << (uFormat("%s%d nodes and %d words\n", pad("LTM:").c_str(), (int)ids.size(), driver->getTotalDictionarySize()));
+		std::cout << (uFormat("%s%d nodes and %d words (dim=%d type=%s)\n", pad("LTM:").c_str(), (int)ids.size(), driver->getTotalDictionarySize(), wordsDim, wordsType==CV_8UC1?"8U":wordsType==CV_32FC1?"32F":uNumber2Str(wordsType).c_str()));
 		std::cout << (uFormat("%s%d nodes and %d words\n", pad("WM:").c_str(), driver->getLastNodesSize(), driver->getLastDictionarySize()));
 		std::cout << (uFormat("%s%d poses and %d links\n", pad("Global graph:").c_str(), odomPoses, links.size()));
 		std::cout << (uFormat("%s%d poses\n", pad("Optimized graph:").c_str(), (int)optimizedPoses.size(), links.size()));

tools/Matcher/main.cpp

@@ -63,6 +63,7 @@ void showUsage()
 			"   rtabmap-matcher --Vis/FeatureType 11 --SuperPoint/ModelPath \"superpoint.pt\" --Vis/CorNNType 6 --PyMatcher/Path \"~/SuperGluePretrainedNetwork/rtabmap_superglue.py\" from.png to.png\n"
 			"   rtabmap-matcher --Vis/FeatureType 1 --Vis/CorNNType 6 --PyMatcher/Path \"~/OANet/demo/rtabmap_oanet.py\" --PyMatcher/Model \"~/OANet/model/gl3d/sift-4000/model_best.pth\" from.png to.png\n"
 			"   rtabmap-matcher --calibration calib.yaml --from_depth from_depth.png --to_depth to_depth.png from.png to.png\n"
+			"   rtabmap-matcher --calibration calibFrom.yaml --calibration_to calibTo.yaml --from_depth from_depth.png --to_depth to_depth.png from.png to.png\n"
 			"   rtabmap-matcher --calibration calib.yaml --Vis/FeatureType 2 --Vis/MaxFeatures 10000 --Vis/CorNNType 7 from.png to.png\n"
 			"\n"
 			"Note: Use \"Vis/\" parameters for feature stuff.\n"
@@ -71,7 +72,11 @@ void showUsage()
 			"                                        fake one is created from image's\n"
 			"                                        size (which may not be optimal).\n"
 			"                                        Required if from_depth option is set.\n"
-			"                                        Assuming same calibration for both images.\n"
+			"                                        Assuming same calibration for both images\n"
+			"                                        if --calibration_to is not set.\n"
+			"   --calibration_to \"calibration.yaml\" Calibration file for \"to\" image. If not set,\n"
+			"                                           the same calibration of --calibration option is\n"
+			"                                           used for \"to\" image.\n"
 			"   --from_depth \"from_depth.png\"    Depth or right image file of the first image.\n"
 			"                                        If not set, 2D->2D estimation is done by \n"
 			"                                        default. For 3D->2D estimation, from_depth\n"
@@ -98,6 +103,7 @@ int main(int argc, char * argv[])
 	std::string fromDepthPath;
 	std::string toDepthPath;
 	std::string calibrationPath;
+	std::string calibrationToPath;
 	for(int i=1; i<argc-2; ++i)
 	{
 		if(strcmp(argv[i], "--from_depth") == 0)
@@ -136,6 +142,18 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(strcmp(argv[i], "--calibration_to") == 0)
+		{
+			++i;
+			if(i<argc-2)
+			{
+				calibrationToPath = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+		}
 		else if(strcmp(argv[i], "--help") == 0)
 		{
 			showUsage();
@@ -144,6 +162,10 @@ int main(int argc, char * argv[])
 
 	printf("Options\n");
 	printf("  --calibration = \"%s\"\n", calibrationPath.c_str());
+	if(!calibrationToPath.empty())
+	{
+		printf("  --calibration_to = \"%s\"\n", calibrationToPath.c_str());
+	}
 	printf("  --from_depth  = \"%s\"\n", fromDepthPath.c_str());
 	printf("  --to_depth    = \"%s\"\n", toDepthPath.c_str());
 
@@ -197,6 +219,8 @@ int main(int argc, char * argv[])
 
 		CameraModel model;
 		StereoCameraModel stereoModel;
+		CameraModel modelTo;
+		StereoCameraModel stereoModelTo;
 		if(!fromDepth.empty())
 		{
 			if(fromDepth.type() != CV_8UC1)
@@ -206,6 +230,10 @@ int main(int argc, char * argv[])
 					printf("Failed to load calibration file \"%s\"!\n", calibrationPath.c_str());
 					exit(-1);
 				}
+				if(calibrationToPath.empty())
+				{
+					modelTo = model;
+				}
 			}
 			else // fromDepth.type() == CV_8UC1
 			{
@@ -214,6 +242,29 @@ int main(int argc, char * argv[])
 					printf("Failed to load calibration file \"%s\"!\n", calibrationPath.c_str());
 					exit(-1);
 				}
+				if(calibrationToPath.empty())
+				{
+					stereoModelTo = stereoModel;
+				}
+			}
+			if(!calibrationToPath.empty())
+			{
+				if(toDepth.empty() || toDepth.type() != CV_8UC1)
+				{
+					if(!modelTo.load(UDirectory::getDir(calibrationToPath), uSplit(UFile::getName(calibrationToPath), '.').front()))
+					{
+						printf("Failed to load calibration file \"%s\"!\n", calibrationToPath.c_str());
+						exit(-1);
+					}
+				}
+				else // toDepth.type() == CV_8UC1
+				{
+					if(!stereoModelTo.load(UDirectory::getDir(calibrationToPath), uSplit(UFile::getName(calibrationToPath), '.').front()))
+					{
+						printf("Failed to load calibration file \"%s\"!\n", calibrationToPath.c_str());
+						exit(-1);
+					}
+				}
 			}
 		}
 		else if(!calibrationPath.empty())
@@ -223,13 +274,29 @@ int main(int argc, char * argv[])
 				printf("Failed to load calibration file \"%s\"!\n", calibrationPath.c_str());
 				exit(-1);
 			}
+			if(!calibrationToPath.empty())
+			{
+				if(!modelTo.load(UDirectory::getDir(calibrationToPath), uSplit(UFile::getName(calibrationToPath), '.').front()))
+				{
+					printf("Failed to load calibration file \"%s\"!\n", calibrationToPath.c_str());
+					exit(-1);
+				}
+			}
+			else
+			{
+				modelTo = model;
+			}
 		}
 		else
 		{
-			printf("Using fake calibration model (image size=%dx%d): fx=%d fy=%d cx=%d cy=%d\n",
+			printf("Using fake calibration model \"from\" (image size=%dx%d): fx=%d fy=%d cx=%d cy=%d\n",
 					imageFrom.cols, imageFrom.rows, imageFrom.cols/2, imageFrom.cols/2, imageFrom.cols/2, imageFrom.rows/2);
 			model = CameraModel(imageFrom.cols/2, imageFrom.cols/2, imageFrom.cols/2, imageFrom.rows/2); // Fake model
 			model.setImageSize(imageFrom.size());
+			printf("Using fake calibration model \"to\" (image size=%dx%d): fx=%d fy=%d cx=%d cy=%d\n",
+					imageTo.cols, imageTo.rows, imageTo.cols/2, imageTo.cols/2, imageTo.cols/2, imageTo.rows/2);
+			modelTo = CameraModel(imageTo.cols/2, imageTo.cols/2, imageTo.cols/2, imageTo.rows/2); // Fake model
+			modelTo.setImageSize(imageTo.size());
 		}
 
 		Signature dataFrom;
@@ -238,13 +305,13 @@ int main(int argc, char * argv[])
 		{
 			printf("Mono calibration model detected.\n");
 			dataFrom = SensorData(imageFrom, fromDepth, model, 1);
-			dataTo = SensorData(imageTo, toDepth, model, 2);
+			dataTo = SensorData(imageTo, toDepth, modelTo, 2);
 		}
 		else //stereo
 		{
 			printf("Stereo calibration model detected.\n");
 			dataFrom = SensorData(imageFrom, fromDepth, stereoModel, 1);
-			dataTo = SensorData(imageTo, toDepth, stereoModel, 2);
+			dataTo = SensorData(imageTo, toDepth, stereoModelTo, 2);
 		}
 
 		//////////////////

tools/Reprocess/main.cpp

@@ -460,6 +460,8 @@ int main(int argc, char * argv[])
 	}
 
 	ParametersMap parameters = dbDriver->getLastParameters();
+	std::string targetVersion = dbDriver->getDatabaseVersion();
+	parameters.insert(ParametersPair(Parameters::kDbTargetVersion(), targetVersion));
 	if(parameters.empty())
 	{
 		printf("WARNING: Failed getting parameters from database, reprocessing will be done with default parameters! Database version may be too old (%s).\n", dbDriver->getDatabaseVersion().c_str());
@@ -498,6 +500,7 @@ int main(int argc, char * argv[])
 
 	bool incrementalMemory = Parameters::defaultMemIncrementalMemory();
 	Parameters::parse(parameters, Parameters::kMemIncrementalMemory(), incrementalMemory);
+	Parameters::parse(parameters, Parameters::kDbTargetVersion(), targetVersion);
 
 	int totalIds = 0;
 	std::set<int> ids;
@@ -539,6 +542,10 @@ int main(int argc, char * argv[])
 
 	std::string workingDirectory = UDirectory::getDir(outputDatabasePath);
 	printf("Set working directory to \"%s\".\n", workingDirectory.c_str());
+	if(!targetVersion.empty())
+	{
+		printf("Target database version: \"%s\" (set explicitly --%s \"\" to output with latest version.\n", targetVersion.c_str(), Parameters::kDbTargetVersion().c_str());
+	}
 	uInsert(parameters, ParametersPair(Parameters::kRtabmapWorkingDirectory(), workingDirectory));
 	uInsert(parameters, ParametersPair(Parameters::kRtabmapPublishStats(), "true")); // to log status below