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

* add rvl_codec

* add support for RVL deep image compression

* DBViewer: edit depth re-using same compression format than the one in database.

* Fixing windows ci

* 💄

* missing dll export (windows)

* typo

---------

Co-authored-by: matlabbe <matlabbe@gmail.com>

CMakeLists.txt

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 21)
-SET(RTABMAP_PATCH_VERSION 9)
+SET(RTABMAP_PATCH_VERSION 10)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

corelib/include/rtabmap/core/Compression.h

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
+#include <rtabmap/core/rvl_codec.h>
 #include <rtabmap/utilite/UThread.h>
 #include <opencv2/opencv.hpp>
 
@@ -86,5 +87,9 @@ cv::Mat RTABMAP_CORE_EXPORT uncompressData(const unsigned char * bytes, unsigned
 cv::Mat RTABMAP_CORE_EXPORT compressString(const std::string & str);
 std::string RTABMAP_CORE_EXPORT uncompressString(const cv::Mat & bytes);
 
+std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const cv::Mat & bytes);
+std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const std::vector<unsigned char> & bytes);
+std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const unsigned char * bytes, size_t size);
+
 } /* namespace rtabmap */
 #endif /* COMPRESSION_H_ */

corelib/include/rtabmap/core/DBDriver.h

@@ -100,7 +100,7 @@ public:
 		int nodeId,
 		const std::vector<CameraModel> & models,
 		const std::vector<StereoCameraModel> & stereoModels);
-	void updateDepthImage(int nodeId, const cv::Mat & image);
+	void updateDepthImage(int nodeId, const cv::Mat & image, const std::string & format);
 	void updateLaserScan(int nodeId, const LaserScan & scan);
 
 public:
@@ -178,6 +178,7 @@ public:
 	void getWeight(int signatureId, int & weight) const;
 	void getLastNodeIds(std::set<int> & ids) const;
 	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false, bool ignoreIntermediateNodes = false) const;
+	void getAllOdomPoses(std::map<int, Transform> & poses, bool ignoreChildren = false, bool ignoreIntermediateNodes = false) const;
 	void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true, bool withLandmarks = false) const;
 	void getLastNodeId(int & id) const;
 	void getLastMapId(int & mapId) const;
@@ -242,7 +243,8 @@ protected:
 
 	virtual void updateDepthImageQuery(
 			int nodeId,
-			const cv::Mat & image) const = 0;
+			const cv::Mat & image,
+			const std::string & format) const = 0;
 
 	virtual void updateLaserScanQuery(
 			int nodeId,
@@ -286,6 +288,7 @@ protected:
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const = 0;
 	virtual void getLastNodeIdsQuery(std::set<int> & ids) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const = 0;
+	virtual void getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const = 0;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName="id") const = 0;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -103,7 +103,8 @@ protected:
 
 	virtual void updateDepthImageQuery(
 			int nodeId,
-			const cv::Mat & image) const;
+			const cv::Mat & image,
+			const std::string & format) const;
 
 	void updateLaserScanQuery(
 			int nodeId,
@@ -147,6 +148,7 @@ protected:
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
 	virtual void getLastNodeIdsQuery(std::set<int> & ids) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const;
+	virtual void getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName="id") const;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
@@ -172,7 +174,7 @@ private:
 	void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const;
 	void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepCalibrationUpdate(sqlite3_stmt * ppStmt, int nodeId, const std::vector<CameraModel> & models, const std::vector<StereoCameraModel> & stereoModels) const;
-	void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & imageCompressed) const;
+	void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image, const std::string & format) const;
 	void stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const LaserScan & image) const;
 	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;

corelib/include/rtabmap/core/Graph.h

@@ -115,7 +115,8 @@ Transform RTABMAP_CORE_EXPORT calcRMSE(
 		float & rotational_median,
 		float & rotational_std,
 		float & rotational_min,
-		float & rotational_max);
+		float & rotational_max,
+		bool align2D = false);
 
 void RTABMAP_CORE_EXPORT computeMaxGraphErrors(
 		const std::map<int, Transform> & poses,

corelib/include/rtabmap/core/Memory.h

@@ -303,6 +303,7 @@ private:
 	bool _notLinkedNodesKeptInDb;
 	bool _saveIntermediateNodeData;
 	std::string _rgbCompressionFormat;
+	std::string _depthCompressionFormat;
 	bool _incrementalMemory;
 	bool _localizationDataSaved;
 	bool _reduceGraph;
@@ -314,6 +315,7 @@ private:
 	bool _badSignaturesIgnored;
 	bool _mapLabelsAdded;
 	bool _depthAsMask;
+	float _maskFloorThreshold;
 	bool _stereoFromMotion;
 	unsigned int _imagePreDecimation;
 	unsigned int _imagePostDecimation;

corelib/include/rtabmap/core/Parameters.h

@@ -209,6 +209,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Mem, NotLinkedNodesKept,          bool, true,     "Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
     RTABMAP_PARAM(Mem, IntermediateNodeDataKept,    bool, false,    "Keep intermediate node data in db.");
     RTABMAP_PARAM_STR(Mem, ImageCompressionFormat,   ".jpg",        "RGB image compression format. It should be \".jpg\" or \".png\".");
+    RTABMAP_PARAM_STR(Mem, DepthCompressionFormat,   ".rvl",        "Depth image compression format for 16UC1 depth type. It should be \".png\" or \".rvl\". If depth type is 32FC1, \".png\" is used.");
     RTABMAP_PARAM(Mem, STMSize,                   unsigned int, 10, "Short-term memory size.");
     RTABMAP_PARAM(Mem, IncrementalMemory,           bool, true,     "SLAM mode, otherwise it is Localization mode.");
     RTABMAP_PARAM(Mem, LocalizationDataSaved,       bool, false,     uFormat("Save localization data during localization session (when %s=false). When enabled, the database will then also grow in localization mode. This mode would be used only for debugging purpose.", kMemIncrementalMemory().c_str()).c_str());
@@ -221,6 +222,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Mem, BadSignaturesIgnored,        bool, false,    "Bad signatures are ignored.");
     RTABMAP_PARAM(Mem, InitWMWithAllNodes,          bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
     RTABMAP_PARAM(Mem, DepthAsMask,                 bool, true,     "Use depth image as mask when extracting features for vocabulary.");
+    RTABMAP_PARAM(Mem, DepthMaskFloorThr,           float, 0.0,     uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kMemDepthAsMask().c_str()));
     RTABMAP_PARAM(Mem, StereoFromMotion,            bool, false,    uFormat("Triangulate features without depth using stereo from motion (odometry). It would be ignored if %s is true and the feature detector used supports masking.", kMemDepthAsMask().c_str()));
     RTABMAP_PARAM(Mem, ImagePreDecimation,          unsigned int, 1, uFormat("Decimation of the RGB image before visual feature detection. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.",kMemDepthAsMask().c_str()));
     RTABMAP_PARAM(Mem, ImagePostDecimation,         unsigned int, 1, uFormat("Decimation of the RGB image before saving it to database. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. Decimation is done from the original image. If set to same value than %s, data already decimated is saved (no need to re-decimate the image).", kMemImagePreDecimation().c_str()));
@@ -703,6 +705,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,     "Max depth of the features (0 means no limit).");
     RTABMAP_PARAM(Vis, MinDepth,                  float, 0,     "Min depth of the features (0 means no limit).");
     RTABMAP_PARAM(Vis, DepthAsMask,               bool,  true,  "Use depth image as mask when extracting features.");
+    RTABMAP_PARAM(Vis, DepthMaskFloorThr,         float, 0.0,    uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kVisDepthAsMask().c_str()));
     RTABMAP_PARAM_STR(Vis, RoiRatios,        "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
     RTABMAP_PARAM(Vis, SubPixWinSize,             int,   3,     "See cv::cornerSubPix().");
     RTABMAP_PARAM(Vis, SubPixIterations,          int,   0,     "See cv::cornerSubPix(). 0 disables sub pixel refining.");

corelib/include/rtabmap/core/RegistrationVis.h

@@ -101,6 +101,7 @@ private:
 	bool _guessMatchToProjection;
 	int _bundleAdjustment;
 	bool _depthAsMask;
+	float _maskFloorThreshold;
 	float _minInliersDistributionThr;
 	float _maxInliersMeanDistance;
 

corelib/include/rtabmap/core/rvl_codec.h

@@ -0,0 +1,37 @@
+// The following code is a C++ wrapper of the code presented by
+// Andrew D. Wilson in "Fast Lossless Depth Image Compression" at SIGCHI'17.
+// The original code is licensed under the MIT License.
+
+#ifndef RVL_CODEC_H_
+#define RVL_CODEC_H_
+
+#include <cstdint>
+#include "rtabmap/core/rtabmap_core_export.h"
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT RvlCodec {
+public:
+    RvlCodec();
+    // Compress input data into output. The size of output can be equal to (1.5 * numPixels + 4) in the worst case.
+    int CompressRVL(const uint16_t * input, unsigned char * output, int numPixels);
+    // Decompress input data into output. The size of output must be equal to numPixels.
+    void DecompressRVL(const unsigned char * input, uint16_t * output, int numPixels);
+
+private:
+    RvlCodec(const RvlCodec &);
+    RvlCodec & operator=(const RvlCodec &);
+
+    void EncodeVLE(int value);
+    int DecodeVLE();
+
+    int *buffer_;
+    int *pBuffer_;
+    int word_;
+    int nibblesWritten_;
+};
+
+}  // namespace rtabmap
+
+#endif  // RVL_CODEC_H_

corelib/include/rtabmap/core/util3d.h

@@ -364,6 +364,22 @@ void RTABMAP_CORE_EXPORT fillProjectedCloudHoles(
 		bool verticalDirection,
 		bool fillToBorder);
 
+/**
+ * @brief Remove values below a floor threshold in a depth image.
+ * 
+ * @param depth the depth image to filter (can be a multi-camera depth image).
+ * @param cameraModels corresponding camera model(s) to depth image, with valid 
+ *                     local transform between base frame to camera frame.
+ * @param threshold height from base frame at which pixels below it are set to 0.
+ * @param depthBelow depth image of the pixels below the floor theshold.
+ * @return cv::Mat depth image of the pixels above the floor theshold.
+ */
+cv::Mat RTABMAP_CORE_EXPORT filterFloor(
+	const cv::Mat & depth,
+	const std::vector<CameraModel> & cameraModels,
+	float threshold,
+	cv::Mat * depthBelow = 0);
+
 /**
  * For each point, return pixel of the best camera (NodeID->CameraIndex)
  * looking at it based on the policy and parameters

corelib/include/rtabmap/core/util3d_surface.h

@@ -482,6 +482,22 @@ void RTABMAP_CORE_EXPORT adjustNormalsToViewPoint(
 		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
 		float groundNormalsUp = 0.0f);
 
+void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const std::vector<int> & cameraIndices,
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		float groundNormalsUp = 0.0f);
+void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const std::vector<int> & cameraIndices,
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		float groundNormalsUp = 0.0f);
+void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const std::vector<int> & cameraIndices,
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		float groundNormalsUp = 0.0f);
+
 void RTABMAP_CORE_EXPORT adjustNormalsToViewPoints(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,

corelib/src/CMakeLists.txt

@@ -64,6 +64,8 @@ SET(SRC_FILES
     util3d_correspondences.cpp
     util3d_motion_estimation.cpp
     
+	rvl_codec.cpp
+	
     SensorData.cpp
     Graph.cpp
     Compression.cpp
@@ -643,10 +645,10 @@ IF(octomap_FOUND)
 ENDIF(octomap_FOUND)
 
 IF(grid_map_core_FOUND)
-    IF(TARGET grid_map_core)
+    IF(TARGET grid_map_core::grid_map_core)
         SET(LIBRARIES
             ${LIBRARIES}
-            grid_map_core
+            grid_map_core::grid_map_core
         )
     ELSE()
         SET(INCLUDE_DIRS

corelib/src/Compression.cpp

@@ -34,14 +34,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-// format : ".png" ".jpg" "" (empty is general)
+// format : ".jpg" ".png" ".rvl" "" (empty is general)
 CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & format) :
 	uncompressedData_(mat),
 	format_(format),
 	image_(!format.empty()),
 	compressMode_(true)
 {
-	UASSERT(format.empty() || format.compare(".png") == 0 || format.compare(".jpg") == 0);
+	UASSERT(format.empty() || format.compare(".jpg") == 0 || format.compare(".png") == 0 || format.compare(".rvl") == 0);
 }
 // assume image
 CompressionThread::CompressionThread(const cv::Mat & bytes, bool isImage) :
@@ -96,7 +96,7 @@ void CompressionThread::mainLoop()
 	this->kill();
 }
 
-// ".png" or ".jpg"
+// ".jpg" or ".png" or ".rvl"
 std::vector<unsigned char> compressImage(const cv::Mat & image, const std::string & format)
 {
 	std::vector<unsigned char> bytes;
@@ -106,7 +106,21 @@ std::vector<unsigned char> compressImage(const cv::Mat & image, const std::strin
 		{
 			//save in 8bits-4channel
 			cv::Mat bgra(image.size(), CV_8UC4, image.data);
-			cv::imencode(format, bgra, bytes);
+			cv::imencode(".png", bgra, bytes);
+		}
+		else if(format == ".rvl")
+		{
+			bytes = {'D', 'E', 'P', 'T', 'H', 'R', 'V', 'L'};
+			int numPixels = image.rows * image.cols;
+        	// In the worst case, RVL compression results in ~1.5x larger data.
+        	bytes.resize(3 * numPixels + 20);
+        	uint32_t cols = image.cols;
+        	uint32_t rows = image.rows;
+        	memcpy(&bytes[8], &cols, 4);
+        	memcpy(&bytes[12], &rows, 4);
+        	RvlCodec rvl;
+        	int compressedSize = rvl.CompressRVL(image.ptr<uint16_t>(), &bytes[16], numPixels);
+        	bytes.resize(16 + compressedSize);
 		}
 		else
 		{
@@ -116,7 +130,7 @@ std::vector<unsigned char> compressImage(const cv::Mat & image, const std::strin
 	return bytes;
 }
 
-// ".png" or ".jpg"
+// ".jpg" or ".png" or ".rvl"
 cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
 {
 	std::vector<unsigned char> bytes = compressImage(image, format);
@@ -129,21 +143,33 @@ cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
 
 cv::Mat uncompressImage(const cv::Mat & bytes)
 {
-	 cv::Mat image;
+	cv::Mat image;
 	if(!bytes.empty())
 	{
-#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
+		if (compressedDepthFormat(bytes) == ".rvl")
-		image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
-#else
-		image = cv::imdecode(bytes, -1);
-#endif
-		if(image.type() == CV_8UC4)
 		{
-			// Using clone() or copyTo() caused a memory leak !?!?
+			uint32_t cols, rows;
-			// image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+        	memcpy(&cols, &bytes.data[8], 4);
-			cv::Mat depth(image.size(), CV_32FC1);
+        	memcpy(&rows, &bytes.data[12], 4);
-			memcpy(depth.data, image.data, image.total()*image.elemSize());
+			image = cv::Mat(rows, cols, CV_16UC1);
-			image = depth;
+			RvlCodec rvl;
+        	rvl.DecompressRVL(&bytes.data[16], image.ptr<uint16_t>(), cols * rows);
+		}
+		else
+		{
+#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
+			image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
+#else
+			image = cv::imdecode(bytes, -1);
+#endif
+			if(image.type() == CV_8UC4)
+			{
+				// Using clone() or copyTo() caused a memory leak !?!?
+				// image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+				cv::Mat depth(image.size(), CV_32FC1);
+				memcpy(depth.data, image.data, image.total()*image.elemSize());
+				image = depth;
+			}
 		}
 	}
 	return image;
@@ -151,17 +177,29 @@ cv::Mat uncompressImage(const cv::Mat & bytes)
 
 cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
 {
-	 cv::Mat image;
+	cv::Mat image;
 	if(bytes.size())
 	{
-#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
+		if (compressedDepthFormat(bytes) == ".rvl")
-		image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
-#else
-		image = cv::imdecode(bytes, -1);
-#endif
-		if(image.type() == CV_8UC4)
 		{
-			image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+			uint32_t cols, rows;
+        	memcpy(&cols, &bytes[8], 4);
+        	memcpy(&rows, &bytes[12], 4);
+			image = cv::Mat(rows, cols, CV_16UC1);
+			RvlCodec rvl;
+        	rvl.DecompressRVL(&bytes[16], image.ptr<uint16_t>(), cols * rows);
+		}
+		else
+		{
+#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
+			image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
+#else
+			image = cv::imdecode(bytes, -1);
+#endif
+			if(image.type() == CV_8UC4)
+			{
+				image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+			}
 		}
 	}
 	return image;
@@ -291,4 +329,33 @@ std::string uncompressString(const cv::Mat & bytes)
 	return "";
 }
 
+std::string compressedDepthFormat(const cv::Mat & bytes)
+{
+	return compressedDepthFormat(bytes.data, bytes.rows * bytes.cols * bytes.elemSize());
+}
+std::string compressedDepthFormat(const std::vector<unsigned char> & bytes)
+{
+	return compressedDepthFormat(bytes.data(), bytes.size());
+}
+std::string compressedDepthFormat(const unsigned char * bytes, size_t size)
+{
+	std::string format;
+	if(bytes && size)
+	{
+		size_t maxlen = std::min(size, size_t(8));
+		std::vector<unsigned char> signature(maxlen);
+		memcpy(&signature[0], bytes, maxlen);
+		if (std::string(signature.begin(), signature.end()) == "DEPTHRVL")
+		{
+			format = ".rvl";
+		}
+		else
+		{
+			// Assuming png by default
+			format = ".png";
+		}
+	}
+	return format;
+}
+
 } /* namespace rtabmap */

corelib/src/DBDriver.cpp

@@ -512,12 +512,13 @@ void DBDriver::updateCalibration(int nodeId, const std::vector<CameraModel> & mo
 	_dbSafeAccessMutex.unlock();
 }
 
-void DBDriver::updateDepthImage(int nodeId, const cv::Mat & image)
+void DBDriver::updateDepthImage(int nodeId, const cv::Mat & image, const std::string & format)
 {
 	_dbSafeAccessMutex.lock();
 	this->updateDepthImageQuery(
 			nodeId,
-			image);
+			image,
+			format);
 	_dbSafeAccessMutex.unlock();
 }
 
@@ -922,6 +923,45 @@ void DBDriver::getAllNodeIds(std::set<int> & ids, bool ignoreChildren, bool igno
 	_dbSafeAccessMutex.unlock();
 }
 
+void DBDriver::getAllOdomPoses(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const
+{
+	// look in the trash
+	_trashesMutex.lock();
+	if(_trashSignatures.size())
+	{
+		for(std::map<int, Signature*>::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter)
+		{
+			bool hasNeighbors = !ignoreChildren;
+			if(ignoreChildren)
+			{
+				for(std::map<int, Link>::const_iterator nIter = sIter->second->getLinks().begin();
+						nIter!=sIter->second->getLinks().end();
+						++nIter)
+				{
+					if(nIter->second.type() == Link::kNeighbor ||
+					   nIter->second.type() == Link::kNeighborMerged)
+					{
+						hasNeighbors = true;
+						break;
+					}
+				}
+			}
+			if(hasNeighbors && (!ignoreIntermediateNodes || sIter->second->getWeight() != -1))
+			{
+				poses.insert(std::make_pair(sIter->first, sIter->second->getPose()));
+			}
+		}
+
+		std::vector<int> keys = uKeys(_trashSignatures);
+
+	}
+	_trashesMutex.unlock();
+
+	_dbSafeAccessMutex.lock();
+	this->getAllOdomPosesQuery(poses, ignoreChildren, ignoreIntermediateNodes);
+	_dbSafeAccessMutex.unlock();
+}
+
 void DBDriver::getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const
 {
 	_dbSafeAccessMutex.lock();
@@ -1139,7 +1179,7 @@ void DBDriver::addInfoAfterRun(
 					  << processMemUsed << ","
 					  << databaseMemUsed << ","
 					  << dictionarySize << ","
-					  "\"" << param.c_str() << "\");";
+					  "'" << param.c_str() << "');";
 			}
 			else
 			{
@@ -1149,7 +1189,7 @@ void DBDriver::addInfoAfterRun(
 					  << processMemUsed << ","
 					  << databaseMemUsed << ","
 					  << dictionarySize << ","
-					  "\"" << param.c_str() << "\");";
+					  "'" << param.c_str() << "');";
 			}
 		}
 		else

corelib/src/DBDriverSqlite3.cpp

@@ -2427,9 +2427,13 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 			  << "FROM Node ";
 		if(ignoreChildren)
 		{
-			query << "INNER JOIN Link ";
+			// use to_id to ignore all children (which don't have link pointing on them)
-			query << "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them)
+			// ignore self referring links
-			query << "WHERE from_id != to_id "; // ignore self referring links
+			// keep nodes without link to other nodes (map has only a single node)
+			query << "WHERE ";
+			query << "(EXISTS (select 1 from Link where Node.id=to_id and from_id != to_id) OR ";
+			query << " NOT EXISTS (select 1 from Link where id=to_id and from_id != to_id)) ";
+			
 			query << "AND weight>-9 "; //ignore invalid nodes
 			if(ignoreIntermediateNodes)
 			{
@@ -2483,6 +2487,75 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 	}
 }
 
+void DBDriverSqlite3::getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const
+{
+	if(_ppDb)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+
+		query << "SELECT DISTINCT id, pose "
+			  << "FROM Node ";
+		if(ignoreChildren)
+		{
+			query << "INNER JOIN Link ";
+			query << "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them)
+			query << "WHERE from_id != to_id "; // ignore self referring links
+			query << "AND weight>-9 "; //ignore invalid nodes
+			if(ignoreIntermediateNodes)
+			{
+				query << "AND weight!=-1 "; //ignore intermediate nodes
+			}
+		}
+		else if(ignoreIntermediateNodes)
+		{
+			query << "WHERE weight!=-1 "; //ignore intermediate nodes
+		}
+
+		query  << "ORDER BY id";
+
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		const void * data = 0;
+		int dataSize = 0;
+
+		// Process the result if one
+		rc = sqlite3_step(ppStmt);
+		while(rc == SQLITE_ROW)
+		{
+			int id = sqlite3_column_int(ppStmt, 0); // Signature Id
+			data = sqlite3_column_blob(ppStmt, 1);  // Pose
+			dataSize = sqlite3_column_bytes(ppStmt, 1);
+
+			Transform pose;
+			if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
+			{
+				memcpy(pose.data(), data, dataSize);
+				if(uStrNumCmp(_version, "0.15.2") < 0)
+				{
+					pose.normalizeRotation();
+				}
+			}
+			else if(dataSize)
+			{
+				UERROR("Error while loading pose for node %d! Setting to null...", id);
+			}
+			poses.insert(std::make_pair(id, pose));
+			rc = sqlite3_step(ppStmt);
+		}
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		ULOGGER_DEBUG("Time=%f ids=%d", timer.ticks(), (int)poses.size());
+	}
+}
+
 void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const
 {
 	links.clear();
@@ -4650,7 +4723,8 @@ void DBDriverSqlite3::updateCalibrationQuery(
 
 void DBDriverSqlite3::updateDepthImageQuery(
 		int nodeId,
-		const cv::Mat & image) const
+		const cv::Mat & image,
+		const std::string & format) const
 {
 	UDEBUG("");
 	if(_ppDb)
@@ -4669,7 +4743,8 @@ void DBDriverSqlite3::updateDepthImageQuery(
 		// Save depth
 		stepDepthUpdate(ppStmt,
 				nodeId,
-				image);
+				image,
+				format);
 
 		// Finalize (delete) the statement
 		rc = sqlite3_finalize(ppStmt);
@@ -5940,7 +6015,7 @@ std::string DBDriverSqlite3::queryStepDepthUpdate() const
 		return "UPDATE Data SET depth=? WHERE id=?;";
 	}
 }
-void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image) const
+void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image, const std::string & format) const
 {
 	if(!ppStmt)
 	{
@@ -5954,7 +6029,7 @@ void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const c
 	if(!image.empty() && (image.type()!=CV_8UC1 || image.rows > 1))
 	{
 		// compress
-		imageCompressed = compressImage2(image, ".png");
+		imageCompressed = compressImage2(image, format);
 	}
 	else
 	{

corelib/src/Features2d.cpp

@@ -1315,6 +1315,11 @@ 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;
 	cv::Mat imgRoi(image, roi);
+	cv::Mat maskRoi;
+	if(!mask.empty())
+	{
+		maskRoi = cv::Mat(mask, roi);
+	}
 #ifdef RTABMAP_CUDASIFT
 	if(gpu_)
 	{
@@ -1383,6 +1388,12 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 					//std::cout << cv::Mat(1, 128*4, CV_8UC1, desc) << std::endl;
 					continue;
 				}
+				// Ignore keypoints not in the mask
+				if(!maskRoi.empty() && maskRoi.at<unsigned char>(cudaSiftData_->h_data[i].ypos, cudaSiftData_->h_data[i].xpos) == 0)
+				{
+					continue;
+				}
+
 				//Keep track of the data, to be easier to manage the data in the next step
 				hessianMap.insert(std::pair<float, int>(cudaSiftData_->h_data[i].sharpness, i));
 			}
@@ -1412,12 +1423,6 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 	else
 #endif
 	{
-		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

corelib/src/Graph.cpp

@@ -783,7 +783,8 @@ Transform calcRMSE (
 		float & rotational_median,
 		float & rotational_std,
 		float & rotational_min,
-		float & rotational_max)
+		float & rotational_max,
+		bool align2D)
 {
 
 	translational_rmse = 0.0f;
@@ -815,8 +816,8 @@ Transform calcRMSE (
 			{
 				idFirst = iter->first;
 			}
-			cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), jter->second.z());
+			cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), align2D?0:jter->second.z());
-			cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+			cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), align2D?0:iter->second.z());
 		}
 	}
 

corelib/src/Memory.cpp

@@ -80,6 +80,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
 	_saveIntermediateNodeData(Parameters::defaultMemIntermediateNodeDataKept()),
 	_rgbCompressionFormat(Parameters::defaultMemImageCompressionFormat()),
+	_depthCompressionFormat(Parameters::defaultMemDepthCompressionFormat()),
 	_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
 	_localizationDataSaved(Parameters::defaultMemLocalizationDataSaved()),
 	_reduceGraph(Parameters::defaultMemReduceGraph()),
@@ -91,6 +92,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
 	_mapLabelsAdded(Parameters::defaultMemMapLabelsAdded()),
 	_depthAsMask(Parameters::defaultMemDepthAsMask()),
+	_maskFloorThreshold(Parameters::defaultMemDepthMaskFloorThr()),
 	_stereoFromMotion(Parameters::defaultMemStereoFromMotion()),
     _imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
 	_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
@@ -567,6 +569,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
 	Parameters::parse(params, Parameters::kMemIntermediateNodeDataKept(), _saveIntermediateNodeData);
 	Parameters::parse(params, Parameters::kMemImageCompressionFormat(), _rgbCompressionFormat);
+	Parameters::parse(params, Parameters::kMemDepthCompressionFormat(), _depthCompressionFormat);
 	Parameters::parse(params, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
 	Parameters::parse(params, Parameters::kMemGenerateIds(), _generateIds);
 	Parameters::parse(params, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
@@ -576,6 +579,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
 	Parameters::parse(params, Parameters::kMemSTMSize(), _maxStMemSize);
 	Parameters::parse(params, Parameters::kMemDepthAsMask(), _depthAsMask);
+	Parameters::parse(params, Parameters::kMemDepthMaskFloorThr(), _maskFloorThreshold);
 	Parameters::parse(params, Parameters::kMemStereoFromMotion(), _stereoFromMotion);
 	Parameters::parse(params, Parameters::kMemImagePreDecimation(), _imagePreDecimation);
 	Parameters::parse(params, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
@@ -4884,7 +4888,26 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					imageMono.cols % decimatedData.depthRaw().cols == 0 &&
 					imageMono.rows/decimatedData.depthRaw().rows == imageMono.cols/decimatedData.depthRaw().cols)
 				{
-					depthMask = util2d::interpolate(decimatedData.depthRaw(), imageMono.rows/decimatedData.depthRaw().rows, 0.1f);
+					depthMask = decimatedData.depthRaw();
+
+					if(_maskFloorThreshold != 0.0f)
+					{
+						UASSERT(!decimatedData.cameraModels().empty());
+						UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
+						if(_maskFloorThreshold<0.0f)
+						{
+							cv::Mat depthBelow;
+							util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
+							depthMask = depthBelow;
+						}
+						else
+						{
+							depthMask = util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold);
+						}
+						UDEBUG("Masking floor done.");
+					}
+
+					depthMask = util2d::interpolate(depthMask, imageMono.rows/depthMask.rows, 0.1f);
 				}
 				else
 				{
@@ -5787,10 +5810,42 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		std::vector<unsigned char> imageBytes;
 		std::vector<unsigned char> depthBytes;
 
-		if(_saveDepth16Format && !depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
+		if(!depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
 		{
-			UWARN("Save depth data to 16 bits format: depth type detected is 32FC1, use 16UC1 depth format to avoid this conversion (or set parameter \"Mem/SaveDepth16Format\"=false to use 32bits format).");
+			if(_saveDepth16Format)
-			depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
+			{
+				static bool warned = false;
+				if(!warned)
+				{
+					UWARN("Converting depth data to 16 bits format because depth type detected is 32FC1, "
+					      "feed 16UC1 depth format directly to avoid this conversion (or set parameter %s=false "
+						  "to save 32bits format). This warning is only printed once.",
+						Parameters::kMemSaveDepth16Format().c_str());
+					warned = true;
+				}
+				depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
+			}
+			else if(_depthCompressionFormat == ".rvl")
+			{
+				static bool warned = false;
+				if(!warned)
+				{
+					UWARN("%s is set to false to use 32bits format but this is not "
+					 	  "compatible with the compressed depth format chosen (%s=\"%s\"), depth "
+						  "images will be compressed in \".png\" format instead. Explicitly "
+						  "set %s to true to keep using \"%s\" format and images will be "
+						  "converted to 16bits for convenience (warning: that would "
+						  "remove all depth values over 65 meters). Explicitly set %s=\".png\" "
+						  "to suppress this warning. This warning is only printed once.",
+						Parameters::kMemSaveDepth16Format().c_str(),
+						Parameters::kMemDepthCompressionFormat().c_str(),
+						_depthCompressionFormat.c_str(),
+						Parameters::kMemSaveDepth16Format().c_str(),
+						_depthCompressionFormat.c_str(),
+						Parameters::kMemDepthCompressionFormat().c_str());
+					warned = true;
+				}
+			}
 		}
 
 		cv::Mat compressedImage;
@@ -5800,7 +5855,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		if(_compressionParallelized)
 		{
 			rtabmap::CompressionThread ctImage(image, _rgbCompressionFormat);
-			rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?std::string(".png"):_rgbCompressionFormat);
+			rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
 			rtabmap::CompressionThread ctLaserScan(laserScan.data());
 			rtabmap::CompressionThread ctUserData(data.userDataRaw());
 			if(!image.empty())
@@ -5832,7 +5887,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		else
 		{
 			compressedImage = compressImage2(image, _rgbCompressionFormat);
-			compressedDepth = compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?std::string(".png"):_rgbCompressionFormat);
+			compressedDepth = compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
 			compressedScan = compressData2(laserScan.data());
 			compressedUserData = compressData2(data.userDataRaw());
 		}

corelib/src/RegistrationVis.cpp

@@ -94,6 +94,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_guessMatchToProjection(Parameters::defaultVisCorGuessMatchToProjection()),
 		_bundleAdjustment(Parameters::defaultVisBundleAdjustment()),
 		_depthAsMask(Parameters::defaultVisDepthAsMask()),
+		_maskFloorThreshold(Parameters::defaultVisDepthMaskFloorThr()),
 		_minInliersDistributionThr(Parameters::defaultVisMinInliersDistribution()),
 		_maxInliersMeanDistance(Parameters::defaultVisMeanInliersDistance()),
 		_detectorFrom(0),
@@ -155,6 +156,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisCorGuessMatchToProjection(), _guessMatchToProjection);
 	Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
 	Parameters::parse(parameters, Parameters::kVisDepthAsMask(), _depthAsMask);
+	Parameters::parse(parameters, Parameters::kVisDepthMaskFloorThr(), _maskFloorThreshold);
 	Parameters::parse(parameters, Parameters::kVisMinInliersDistribution(), _minInliersDistributionThr);
 	Parameters::parse(parameters, Parameters::kVisMeanInliersDistance(), _maxInliersMeanDistance);
 	uInsert(_bundleParameters, parameters);
@@ -423,13 +425,32 @@ Transform RegistrationVis::computeTransformationImpl(
 						   imageFrom.cols % fromSignature.sensorData().depthRaw().cols == 0 &&
 						   imageFrom.rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
 						{
-							depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
+							depthMask = fromSignature.sensorData().depthRaw();
+
+							if(_maskFloorThreshold != 0.0f)
+							{
+								UASSERT(!fromSignature.sensorData().cameraModels().empty());
+								UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
+								if(_maskFloorThreshold<0.0f)
+								{
+									cv::Mat depthBelow;
+									util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
+									depthMask = depthBelow;
+								}
+								else
+								{
+									depthMask = util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold);
+								}
+								UDEBUG("Masking floor done.");
+							}
+
+							depthMask = util2d::interpolate(depthMask, imageFrom.rows/depthMask.rows, 0.1f);
 						}
 						else
 						{
 							UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.",
 									Parameters::kVisDepthAsMask().c_str(),
-									fromSignature.sensorData().imageRaw().rows, fromSignature.sensorData().imageRaw().cols,
+									imageFrom.rows, imageFrom.cols,
 									fromSignature.sensorData().depthRaw().rows, fromSignature.sensorData().depthRaw().cols);
 						}
 					}
@@ -770,13 +791,32 @@ Transform RegistrationVis::computeTransformationImpl(
 						   imageTo.cols % toSignature.sensorData().depthRaw().cols == 0 &&
 						   imageTo.rows/toSignature.sensorData().depthRaw().rows == imageTo.cols/toSignature.sensorData().depthRaw().cols)
 						{
-							depthMask = util2d::interpolate(toSignature.sensorData().depthRaw(), imageTo.rows/toSignature.sensorData().depthRaw().rows, 0.1f);
+							depthMask = toSignature.sensorData().depthRaw();
+
+							if(_maskFloorThreshold != 0.0f)
+							{
+								UASSERT(!toSignature.sensorData().cameraModels().empty());
+								UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
+								if(_maskFloorThreshold<0.0f)
+								{
+									cv::Mat depthBelow;
+									util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
+									depthMask = depthBelow;
+								}
+								else
+								{
+									depthMask = util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold);
+								}
+								UDEBUG("Masking floor done.");
+							}
+
+							depthMask = util2d::interpolate(depthMask, imageTo.rows/depthMask.rows, 0.1f);
 						}
 						else
 						{
 							UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.",
 									Parameters::kVisDepthAsMask().c_str(),
-									toSignature.sensorData().imageRaw().rows, toSignature.sensorData().imageRaw().cols,
+									imageTo.rows, imageTo.cols,
 									toSignature.sensorData().depthRaw().rows, toSignature.sensorData().depthRaw().cols);
 						}
 					}

corelib/src/Rtabmap.cpp

@@ -716,6 +716,36 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 
 	if(_memory)
 	{
+		bool isMemIncremental = _memory->isIncremental();
+		if(Parameters::parse(parameters, Parameters::kMemIncrementalMemory(), isMemIncremental) &&
+			isMemIncremental != _memory->isIncremental())
+		{
+			// Mode has changed from Mapping to Localization, cleanup the local graph
+			if(_memory->isGraphReduced() && _memory->isIncremental())
+			{
+				// Force reducing graph, then remove filtered nodes from the optimized poses
+				std::map<int, int> reducedIds;
+				_memory->incrementMapId(&reducedIds);
+				for(std::map<int, int>::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter)
+				{
+					_optimizedPoses.erase(iter->first);
+				}
+			}
+
+			// In both cases, we save the latest optimized graph and latest localization pose
+			_memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose);
+
+			// Mode changed from Localization to Mapping, clear local graph
+			if(!_memory->isIncremental()) {
+				_optimizedPoses.clear();
+				_lastLocalizationPose.setNull();
+				_mapCorrection.setIdentity();
+				_mapCorrectionBackup.setNull();
+				_localizationCovariance = cv::Mat();
+				_lastLocalizationNodeId = 0;
+			}
+		}
+
 		_memory->parseParameters(parameters);
 		if(_memory->isIncremental() && !_globalScanMap.empty())
 		{
@@ -1720,6 +1750,7 @@ bool Rtabmap::process(
 					_constraints.erase(--_constraints.end());
 				}
 			}
+
 			_constraints.insert(std::make_pair(tmp.from(), tmp));
 		}
 		// Localization mode stuff

corelib/src/camera/CameraStereoZed.cpp

@@ -318,12 +318,13 @@ CameraStereoZed::CameraStereoZed(
     sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_);
     sl::DEPTH_MODE qual = static_cast<sl::DEPTH_MODE>(quality_);
 
-    UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
     UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST);
 #if ZED_SDK_MAJOR_VERSION < 4
+	UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
     sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
     UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST);
 #else
+	UASSERT(res >= sl::RESOLUTION::HD4K && res < sl::RESOLUTION::LAST);
     UASSERT(sensingMode_ >= 0 && sensingMode_ < 2);
 #endif
     UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);

corelib/src/optimizer/OptimizerCeres.cpp

@@ -37,22 +37,47 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #ifdef RTABMAP_CERES
 #include <ceres/ceres.h>
+
+#if CERES_VERSION_MAJOR >= 3 || \
+    (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1)
+#include <ceres/manifold.h>
+#else
 #include <ceres/local_parameterization.h>
+#endif
+
 #include "ceres/pose_graph_2d/types.h"
 #include "ceres/pose_graph_2d/pose_graph_2d_error_term.h"
-#include "ceres/pose_graph_2d/angle_local_parameterization.h"
+#include "ceres/pose_graph_2d/angle_manifold.h"
 #include "ceres/pose_graph_3d/types.h"
 #include "ceres/pose_graph_3d/pose_graph_3d_error_term.h"
 #include "ceres/bundle/BAProblem.h"
 #include "ceres/bundle/snavely_reprojection_error.h"
 
 #if not(CERES_VERSION_MAJOR > 1 || (CERES_VERSION_MAJOR == 1 && CERES_VERSION_MINOR >= 12))
-#include "ceres/pose_graph_3d/eigen_quaternion_parameterization.h"
+#include "ceres/pose_graph_3d/eigen_quaternion_manifold.h"
 #endif
 
 #endif
 
 namespace rtabmap {
+namespace {
+
+#ifdef RTABMAP_CERES
+#if CERES_VERSION_MAJOR >= 3 || \
+    (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1)
+inline void SetCeresProblemManifold(ceres::Problem& problem, double* params,
+                                    ceres::Manifold* manifold) {
+  problem.SetManifold(params, manifold);
+#else
+inline void SetCeresProblemManifold(
+    ceres::Problem& problem, double* params,
+    ceres::LocalParameterization* parameterization) {
+  problem.SetParameterization(params, parameterization);
+#endif
+}
+#endif
+
+}  // namespace
 
 bool OptimizerCeres::available()
 {
@@ -118,8 +143,14 @@ std::map<int, Transform> OptimizerCeres::optimize(
 		}
 
 		ceres::LossFunction* loss_function = NULL;
-		ceres::LocalParameterization* angle_local_parameterization = NULL;
+#if CERES_VERSION_MAJOR >= 3 || \
-		ceres::LocalParameterization* quaternion_local_parameterization = NULL;
+    (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1)
+		ceres::Manifold* angle_local_manifold = NULL;
+		ceres::Manifold* quaternion_local_manifold = NULL;
+#else
+		ceres::LocalParameterization* angle_local_manifold = NULL;
+		ceres::LocalParameterization* quaternion_local_manifold = NULL;
+#endif
 
 		for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 		{
@@ -164,12 +195,12 @@ std::map<int, Transform> OptimizerCeres::optimize(
 						&pose_begin_iter->second.x,	&pose_begin_iter->second.y, &pose_begin_iter->second.yaw_radians,
 						&pose_end_iter->second.x, &pose_end_iter->second.y, &pose_end_iter->second.yaw_radians);
 
-					if(angle_local_parameterization == NULL)
+					if(angle_local_manifold == NULL)
 					{
-						angle_local_parameterization = ceres::examples::AngleLocalParameterization::Create();
+						angle_local_manifold = ceres::examples::AngleManfold::Create();
 					}
-					problem.SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameterization);
+					SetCeresProblemManifold(problem, &pose_begin_iter->second.yaw_radians, angle_local_manifold);
-					problem.SetParameterization(&pose_end_iter->second.yaw_radians, angle_local_parameterization);
+					SetCeresProblemManifold(problem, &pose_end_iter->second.yaw_radians, angle_local_manifold);
 				}
 				else
 				{
@@ -194,12 +225,17 @@ std::map<int, Transform> OptimizerCeres::optimize(
 					problem.AddResidualBlock(cost_function, loss_function,
 											  pose_begin_iter->second.p.data(), pose_begin_iter->second.q.coeffs().data(),
 											  pose_end_iter->second.p.data(), pose_end_iter->second.q.coeffs().data());
-					if(quaternion_local_parameterization == NULL)
+					if(quaternion_local_manifold == NULL)
 					{
-						quaternion_local_parameterization = new ceres::EigenQuaternionParameterization;
+#if CERES_VERSION_MAJOR >= 3 || \
+    (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1)
+						quaternion_local_manifold = new ceres::EigenQuaternionManifold;
+#else
+						quaternion_local_manifold = new ceres::EigenQuaternionParameterization;
+#endif
 					}
-					problem.SetParameterization(pose_begin_iter->second.q.coeffs().data(), quaternion_local_parameterization);
+					SetCeresProblemManifold(problem, pose_begin_iter->second.q.coeffs().data(), quaternion_local_manifold);
-					problem.SetParameterization(pose_end_iter->second.q.coeffs().data(), quaternion_local_parameterization);
+					SetCeresProblemManifold(problem, pose_end_iter->second.q.coeffs().data(), quaternion_local_manifold);
 				}
 			}
 			//else // not supporting pose prior and landmarks

corelib/src/optimizer/OptimizerG2O.cpp

@@ -1037,6 +1037,12 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		int it = 0;
 		UTimer timer;
 		double lastError = 0.0;
+
+		if (!optimizer.solver()->init()) {
+			UERROR("g2o: Error while initializing solver");
+			return optimizedPoses;
+		}
+
 		if(intermediateGraphes || this->epsilon() > 0.0)
 		{
 			for(int i=0; i<iterations(); ++i)
@@ -1150,7 +1156,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					}
 				}
 
-				it += optimizer.optimize(1);
+				g2o::OptimizationAlgorithm::SolverResult result = optimizer.solver()->solve(i);
+				++it;
 
 				// early stop condition
 				optimizer.computeActiveErrors();
@@ -1163,6 +1170,12 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					return optimizedPoses;
 				}
 
+				if(result == g2o::OptimizationAlgorithm::Fail)
+				{
+					UERROR("g2o: Solver failed, aborting optimization!");
+					return optimizedPoses;
+				}
+
 				double errorDelta = lastError - chi2;
 				if(i>0 && errorDelta < this->epsilon())
 				{

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -209,7 +209,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			UDEBUG("hasGPSPrior=%s", hasGPSPrior?"true":"false");
 			if(isSlam2d())
 			{
-				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:std::numeric_limits<double>::min()));
+				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:1e-9));
 				graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
 				addedPrior.push_back(ConstraintToFactor(rootId, rootId, -1));
 			}
@@ -217,7 +217,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			{
 				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(
 						(gtsam::Vector(6) <<
-								(hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), (hasGPSPrior?1e-2:std::numeric_limits<double>::min()), // roll, pitch, fixed yaw if there are no priors
+								(hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), (hasGPSPrior?1e-2:1e-9), // roll, pitch, fixed yaw if there are no priors
 								(hasGPSPrior?2:1e-2), hasGPSPrior?2:1e-2, hasGPSPrior?2:1e-2 // xyz
 								).finished());
 				graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
@@ -924,22 +924,42 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			// early stop condition
 			UDEBUG("iteration %d error =%f", i+1, error);
 			double errorDelta = lastError - error;
-			if((isam2_ || i>0) && errorDelta < this->epsilon())
+			if(this->epsilon() > 0.0 && fabs(error) > 1000000000000.0)
 			{
-				if(errorDelta < 0)
+				UWARN("Error computed (%e) is very huge and/or diverging! Aborting! "
-				{
+					   "Set %s to 0 to ignore that check and keep iterating up to %s (%d).",
-					UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
+					   error,
-				}
+					   Parameters::kOptimizerEpsilon().c_str(),
-				else
+					   Parameters::kOptimizerIterations().c_str(),
-				{
+					   this->iterations());
-					UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
+				return optimizedPoses;
-					break;
-				}
 			}
-			else if(i==0 && error < this->epsilon())
+			else
 			{
-				UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
+				if((isam2_ || i>0) && errorDelta < this->epsilon())
-				break;
+				{
+					if(errorDelta < 0)
+					{
+						UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
+					}
+					else
+					{
+						UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
+						break;
+					}
+				}
+				else if(i==0)
+				{ 
+					if(error < 0)
+					{
+						UDEBUG("Negative error?! Ignore and continue optimizing... (%f)", error);
+					}
+					else if(error < this->epsilon())
+					{
+						UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
+						break;
+					}
+				}
 			}
 			lastError = error;
 		}

corelib/src/optimizer/ceres/pose_graph_2d/angle_manifold.h

@@ -28,10 +28,16 @@
 //
 // Author: vitus@google.com (Michael Vitus)
 
-#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_
-#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_
 
-#include "ceres/local_parameterization.h"
+#if CERES_VERSION_MAJOR >= 3 || \
+    (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1)
+#include <ceres/autodiff_manifold.h>
+#include <ceres/manifold.h>
+#else
+#include <ceres/local_parameterization.h>
+#endif
 #include "normalize_angle.h"
 
 namespace ceres {
@@ -39,7 +45,39 @@ namespace examples {
 
 // Defines a local parameterization for updating the angle to be constrained in
 // [-pi to pi).
-class AngleLocalParameterization {
+
+#if CERES_VERSION_MAJOR >= 3 || \
+    (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1)
+
+// Defines a manifold for updating the angle to be constrained in [-pi to pi).
+class AngleManifold {
+ public:
+  template <typename T>
+  bool Plus(const T* x_radians,
+            const T* delta_radians,
+            T* x_plus_delta_radians) const {
+    *x_plus_delta_radians = NormalizeAngle(*x_radians + *delta_radians);
+    return true;
+  }
+
+  template <typename T>
+  bool Minus(const T* y_radians,
+             const T* x_radians,
+             T* y_minus_x_radians) const {
+    *y_minus_x_radians =
+        NormalizeAngle(*y_radians) - NormalizeAngle(*x_radians);
+
+    return true;
+  }
+
+  static ceres::Manifold* Create() {
+    return new ceres::AutoDiffManifold<AngleManifold, 1, 1>;
+  }
+};
+
+#else
+
+class AngleManfold {
  public:
 
   template <typename T>
@@ -52,12 +90,13 @@ class AngleLocalParameterization {
   }
 
   static ceres::LocalParameterization* Create() {
-    return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
+    return (new ceres::AutoDiffLocalParameterization<AngleManfold, 1, 1>);
-                                                     1, 1>);
   }
 };
 
+#endif
+
 }  // namespace examples
 }  // namespace ceres
 
-#endif  // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
+#endif  // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_

corelib/src/optimizer/ceres/pose_graph_3d/eigen_quaternion_parameterization.h

@@ -31,7 +31,7 @@
 #ifndef CERES_EXAMPLES_POSE_GRAPH_3D_EIGEN_QUATERNION_PARAMETERIZATION_H_
 #define CERES_EXAMPLES_POSE_GRAPH_3D_EIGEN_QUATERNION_PARAMETERIZATION_H_
 
-#include "ceres/local_parameterization.h"
+#include "ceres/manifold.h"
 
 namespace ceres {
 
@@ -46,7 +46,7 @@ namespace ceres {
 //
 // Plus(x, delta) = [sin(|delta|) delta / |delta|, cos(|delta|)] * x
 // with * being the quaternion multiplication operator.
-class EigenQuaternionParameterization : public ceres::LocalParameterization {
+class EigenQuaternionParameterization : public ceres::Manifold {
  public:
   virtual ~EigenQuaternionParameterization() {}
   virtual bool Plus(const double* x_ptr,

corelib/src/rvl_codec.cpp

@@ -0,0 +1,102 @@
+// The following code is a C++ wrapper of the code presented by
+// Andrew D. Wilson in "Fast Lossless Depth Image Compression" at SIGCHI'17.
+// The original code is licensed under the MIT License.
+
+#include <rtabmap/core/rvl_codec.h>
+
+namespace rtabmap
+{
+
+RvlCodec::RvlCodec() {}
+
+void RvlCodec::EncodeVLE(int value)
+{
+    do
+    {
+        int nibble = value & 0x7;  // lower 3 bits
+        if (value >>= 3)
+            nibble |= 0x8;  // more to come
+        word_ <<= 4;
+        word_ |= nibble;
+        if (++nibblesWritten_ == 8)  // output word
+        {
+            *pBuffer_++ = word_;
+            nibblesWritten_ = 0;
+            word_ = 0;
+        }
+    } while (value);
+}
+
+int RvlCodec::DecodeVLE()
+{
+    unsigned int nibble;
+    int value = 0, bits = 29;
+    do
+    {
+        if (!nibblesWritten_)
+        {
+            word_ = *pBuffer_++;  // load word
+            nibblesWritten_ = 8;
+        }
+        nibble = word_ & 0xf0000000;
+        value |= (nibble << 1) >> bits;
+        word_ <<= 4;
+        nibblesWritten_--;
+        bits -= 3;
+    } while (nibble & 0x80000000);
+    return value;
+}
+
+int RvlCodec::CompressRVL(const uint16_t * input, unsigned char * output, int numPixels)
+{
+    buffer_ = pBuffer_ = reinterpret_cast<int *>(output);
+    nibblesWritten_ = 0;
+    const uint16_t * end = input + numPixels;
+    uint16_t previous = 0;
+    while (input != end)
+    {
+        int zeros = 0, nonzeros = 0;
+        for (; (input != end) && !*input; input++, zeros++) {}
+        EncodeVLE(zeros);  // number of zeros
+        for (const uint16_t * p = input; (p != end) && *p++; nonzeros++) {}
+        EncodeVLE(nonzeros);  // number of nonzeros
+        for (int i = 0; i < nonzeros; i++)
+        {
+            uint16_t current = *input++;
+            int delta = current - previous;
+            int positive = (delta << 1) ^ (delta >> 31);
+            EncodeVLE(positive);  // nonzero value
+            previous = current;
+        }
+    }
+    if (nibblesWritten_)  // last few values
+        *pBuffer_++ = word_ << 4 * (8 - nibblesWritten_);
+    return static_cast<int>((unsigned char *)pBuffer_ - (unsigned char *)buffer_);  // num bytes
+}
+
+void RvlCodec::DecompressRVL(const unsigned char * input, uint16_t * output, int numPixels)
+{
+    buffer_ = pBuffer_ = const_cast<int *>(reinterpret_cast<const int *>(input));
+    nibblesWritten_ = 0;
+    uint16_t current, previous = 0;
+    int numPixelsToDecode = numPixels;
+    while (numPixelsToDecode)
+    {
+        int zeros = DecodeVLE();  // number of zeros
+        numPixelsToDecode -= zeros;
+        for (; zeros; zeros--) 
+            *output++ = 0;
+        int nonzeros = DecodeVLE();  // number of nonzeros
+        numPixelsToDecode -= nonzeros;
+        for (; nonzeros; nonzeros--)
+        {
+            int positive = DecodeVLE();  // nonzero value
+            int delta = (positive >> 1) ^ -(positive & 1);
+            current = previous + delta;
+            *output++ = current;
+            previous = current;
+        }
+    }
+}
+
+}  // namespace rtabmap

corelib/src/util3d.cpp

@@ -3007,6 +3007,115 @@ void fillProjectedCloudHoles(cv::Mat & registeredDepth, bool verticalDirection,
 	}
 }
 
+cv::Mat filterFloor(const cv::Mat & depth, const std::vector<CameraModel> & cameraModels, float threshold, cv::Mat * depthBelow)
+{
+	cv::Mat output = depth.clone();
+	if(depth.empty())
+	{
+		return output;
+	}
+	if(depthBelow)
+	{
+		*depthBelow = cv::Mat::zeros(output.size(), output.type());
+	}
+
+	UASSERT(!cameraModels.empty());
+	UASSERT(cameraModels[0].isValidForReprojection());
+	// Support camera model with different resolution than depth image
+	float rgbToDepthFactorX = float(cameraModels[0].imageWidth()) / float(output.cols/cameraModels.size());
+	float rgbToDepthFactorY = float(cameraModels[0].imageHeight()) / float(output.rows);
+	int depthWidth = output.cols/cameraModels.size();
+	UASSERT(depthWidth*(int)cameraModels.size() == output.cols);
+
+	// for each camera
+	for(size_t i=0; i<cameraModels.size(); ++i)
+	{
+		const CameraModel & cam = cameraModels[i];
+		UASSERT(cam.isValidForReprojection());
+		const Transform & localTransform = cam.localTransform();
+		UASSERT(!localTransform.isNull());
+		if(i>0)
+		{
+			// Make sure all models are the same resolution
+			UASSERT(cam.imageWidth() == cameraModels[i-1].imageWidth());
+			UASSERT(cam.imageHeight() == cameraModels[i-1].imageHeight());
+		}
+
+		float depthFx = cam.fx() / rgbToDepthFactorX;
+		float depthFy = cam.fy() / rgbToDepthFactorY;
+		float depthCx = cam.cx() / rgbToDepthFactorX;
+		float depthCy = cam.cy() / rgbToDepthFactorY;
+
+		cv::Mat subImage = output.colRange(cv::Range(i*depthWidth, (i+1)*depthWidth));
+		cv::Mat subImageBelow;
+		if(depthBelow)
+			subImageBelow = depthBelow->colRange(cv::Range(i*depthWidth, (i+1)*depthWidth));
+
+		for(int y=0; y<subImage.rows; ++y)
+		{
+			if(subImage.type() == CV_16UC1)
+			{
+				unsigned short * ptr = (unsigned short *)subImage.row(y).ptr();
+				unsigned short * ptrBelow = 0;
+				if(depthBelow)
+				{
+					ptrBelow = (unsigned short *)subImageBelow.row(y).ptr();
+				}
+				for(int x=0; x<subImage.cols; ++x)
+				{
+					if(ptr[x] > 0)
+					{
+						float d = float(ptr[x])/1000.0f;
+						cv::Point3f pt;
+						pt.x = (x - depthCx) * d / depthFx;
+						pt.y = (y - depthCy) * d / depthFy;
+						pt.z = d;
+						pt = util3d::transformPoint(pt, localTransform);
+						if(pt.z < threshold)
+						{
+							if(ptrBelow)
+							{
+								ptrBelow[x] = ptr[x];
+							}
+							ptr[x] = 0;
+						}
+					}
+				}
+			}
+			else // CV_32FC1
+			{
+				float * ptr = (float *)subImage.row(y).ptr();
+				float * ptrBelow = 0;
+				if(depthBelow)
+				{
+					ptrBelow = (float *)subImageBelow.row(y).ptr();
+				}
+				for(int x=0; x<subImage.cols; ++x)
+				{
+					if(ptr[x] > 0.0f)
+					{
+						float & d = ptr[x];
+						cv::Point3f pt;
+						pt.x = (x - depthCx) * d / depthFx;
+						pt.y = (y - depthCy) * d / depthFy;
+						pt.z = d;
+						pt = util3d::transformPoint(pt, localTransform);
+						if(pt.z < threshold)
+						{
+							if(ptrBelow)
+							{
+								ptrBelow[x] = ptr[x];
+							}
+							d = 0;
+						}
+					}
+				}
+			}
+		}
+	}
+	return output;
+}
+
 class ProjectionInfo {
 public:
 	ProjectionInfo():

corelib/src/util3d_filtering.cpp

@@ -694,7 +694,7 @@ typename pcl::PointCloud<PointT>::Ptr voxelizeImpl(
 
 		if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
 		{
-			UWARN("Leaf size is too small for the input dataset. Integer indices would overflow. "
+			UDEBUG("Leaf size is too small for the input dataset. Integer indices would overflow. "
 				  "We will split space to be able to voxelize (lvl=%d cloud=%d min=[%f %f %f] max=[%f %f %f] voxel=%f).",
 				  level,
 				  (int)(indices->empty()?cloud->size():indices->size()),
@@ -2155,7 +2155,7 @@ pcl::IndicesPtr normalFilteringImpl(
 		for(unsigned int i=0; i<cloud_normals->size(); ++i)
 		{
 			Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
-			if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->size()!=0?indices->at(i):i).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
+			if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->size()!=0?indices->at(i):i).z < viewpoint[2]) // some far velodyne rays on road can have normals toward ground
 			{
 				//reverse normal
 				v *= -1.0f;
@@ -2226,7 +2226,7 @@ pcl::IndicesPtr normalFilteringImpl(
 			for(unsigned int i=0; i<indices->size(); ++i)
 			{
 				Eigen::Vector4f v(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
-				if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->at(i)).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
+				if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->at(i)).z < viewpoint[2]) // some far velodyne rays on road can have normals toward ground
 				{
 					//reverse normal
 					v *= -1.0f;
@@ -2244,7 +2244,7 @@ pcl::IndicesPtr normalFilteringImpl(
 			for(unsigned int i=0; i<cloud->size(); ++i)
 			{
 				Eigen::Vector4f v(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
-				if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(i).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
+				if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(i).z < viewpoint[2]) // some far velodyne rays on road can have normals toward ground
 				{
 					//reverse normal
 					v *= -1.0f;

corelib/src/util3d_surface.cpp

@@ -3530,7 +3530,7 @@ LaserScan adjustNormalsToViewPoint(
 
 					float result = v.dot(n);
 					if(result < 0
-					 || (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
+					 || (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[2])) // some far velodyne rays on road can have normals toward ground
 					{
 						//reverse normal
 						ptr[nx] *= -1.0f;
@@ -3569,7 +3569,7 @@ void adjustNormalsToViewPointImpl(
 
 			float result = v.dot(n);
 			if(result < 0
-				|| (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && 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[2])) // some far velodyne rays on road can have normals toward ground
 			{
 				//reverse normal
 				cloud->points[i].normal_x *= -1.0f;
@@ -3625,6 +3625,67 @@ void adjustNormalsToViewPoint(
 	adjustNormalsToViewPointImpl<pcl::PointXYZINormal>(cloud, viewpoint, groundNormalsUp);
 }
 
+template<typename PointT>
+void adjustNormalsToViewPointsImpl(
+		const std::map<int, Transform> & poses,
+		const std::vector<int> & cameraIndices,
+		typename pcl::PointCloud<PointT>::Ptr & cloud,
+		float groundNormalsUp)
+{
+	if(poses.size() && cloud->size() == cameraIndices.size() && cloud->size())
+	{
+		#pragma omp parallel for
+		for(int i=0; i<(int)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))
+			{
+				const Transform & p = poses.at(cameraIndices[i]);
+				pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
+				Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
+
+				Eigen::Vector3f n(normal.x, normal.y, normal.z);
+
+				float result = v.dot(n);
+				if(result < 0 ||
+					(groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint.z)) // 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 adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const std::vector<int> & cameraIndices,
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		float groundNormalsUp)
+{
+	adjustNormalsToViewPointsImpl<pcl::PointNormal>(poses, cameraIndices, cloud, groundNormalsUp);
+}
+
+void adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const std::vector<int> & cameraIndices,
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		float groundNormalsUp)
+{
+	adjustNormalsToViewPointsImpl<pcl::PointXYZRGBNormal>(poses, cameraIndices, cloud, groundNormalsUp);
+}
+
+void adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const std::vector<int> & cameraIndices,
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		float groundNormalsUp)
+{
+	adjustNormalsToViewPointsImpl<pcl::PointXYZINormal>(poses, cameraIndices, cloud, groundNormalsUp);
+}
 
 template<typename PointT>
 void adjustNormalsToViewPointsImpl(

guilib/src/DatabaseViewer.cpp

@@ -2423,7 +2423,8 @@ void DatabaseViewer::editDepthImage()
 				UASSERT(data.depthRaw().type() == depth.type());
 				UASSERT(data.depthRaw().cols == depth.cols);
 				UASSERT(data.depthRaw().rows == depth.rows);
-				dbDriver_->updateDepthImage(id, depth);
+				std::string depthFormat = compressedDepthFormat(data.depthOrRightCompressed());
+				dbDriver_->updateDepthImage(id, depth, depthFormat);
 				this->update3dView();
 			}
 		}
@@ -2501,115 +2502,9 @@ void DatabaseViewer::exportPoses(int format)
 		return;
 	}
 
-	if(format == 5)
+	if(format == 5 && (gpsValues_.empty() || gpsPoses_.empty()))
 	{
-		if(gpsValues_.empty() || gpsPoses_.empty())
+		QMessageBox::warning(this, tr("Cannot export poses in KML format"), tr("No GPS in database?!"));
-		{
-			QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!"));
-		}
-		else
-		{
-			std::map<int, rtabmap::Transform> graph;
-			if(groundTruth)
-			{
-				graph = groundTruthPoses_;
-			}
-			else if(odometry)
-			{
-				graph = odomPoses_;
-			}
-			else
-			{
-				graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
-			}
-
-
-			//align with ground truth for more meaningful results
-			pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
-			cloud1.resize(graph.size());
-			cloud2.resize(graph.size());
-			int oi = 0;
-			int idFirst = 0;
-			for(std::map<int, Transform>::const_iterator iter=gpsPoses_.begin(); iter!=gpsPoses_.end(); ++iter)
-			{
-				std::map<int, Transform>::iterator iter2 = graph.find(iter->first);
-				if(iter2!=graph.end())
-				{
-					if(oi==0)
-					{
-						idFirst = iter->first;
-					}
-					cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-					cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
-				}
-			}
-
-			Transform t = Transform::getIdentity();
-			if(oi>5)
-			{
-				cloud1.resize(oi);
-				cloud2.resize(oi);
-
-				t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
-			}
-			else if(idFirst)
-			{
-				t = gpsPoses_.at(idFirst) * graph.at(idFirst).inverse();
-			}
-
-			std::map<int, GPS> values;
-			GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords();
-			for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
-			{
-				iter->second = t * iter->second;
-
-				GeodeticCoords coord;
-				coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin);
-				double bearing = -(iter->second.theta()*180.0/M_PI-90.0);
-				if(bearing < 0)
-				{
-					bearing += 360;
-				}
-
-				Transform p, g;
-				int w;
-				std::string l;
-				double stamp=0.0;
-				int mapId;
-				std::vector<float> v;
-				GPS gps;
-				EnvSensors sensors;
-				dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors);
-				values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0)));
-			}
-
-			QString output = pathDatabase_ + QDir::separator() + "poses.kml";
-			QString path = QFileDialog::getSaveFileName(
-					this,
-					tr("Save File"),
-					output,
-					tr("Google Earth file (*.kml)"));
-
-			if(!path.isEmpty())
-			{
-				bool saved = graph::exportGPS(path.toStdString(), values, ui_->graphViewer->getNodeColor().rgba());
-
-				if(saved)
-				{
-					QMessageBox::information(this,
-							tr("Export poses..."),
-							tr("GPS coordinates saved to \"%1\".")
-							.arg(path));
-				}
-				else
-				{
-					QMessageBox::information(this,
-							tr("Export poses..."),
-							tr("Failed to save GPS coordinates to \"%1\"!")
-							.arg(path));
-				}
-			}
-		}
 		return;
 	}
 
@@ -2618,70 +2513,132 @@ void DatabaseViewer::exportPoses(int format)
 	{
 		optimizedPoses = groundTruthPoses_;
 	}
+	else if(odometry)
+	{
+		optimizedPoses = odomPoses_;
+	}
 	else
 	{
-		if(odometry)
+		optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+	}
+
+	bool alignToGPS = 
+			(ui_->checkBox_alignPosesWithGPS->isEnabled() && 
+		     ui_->checkBox_alignPosesWithGPS->isChecked()) ||
+		    format == 5;
+
+	if(alignToGPS ||
+	   (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked()))
+	{
+		std::map<int, Transform> refPoses = groundTruthPoses_;
+		if(alignToGPS)
 		{
-			optimizedPoses = odomPoses_;
+			refPoses = gpsPoses_;
-		}
-		else
-		{
-			optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
 		}
 
-		if((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) ||
+		// Log ground truth statistics (in TUM's RGBD-SLAM format)
-		   (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked()))
+		if(refPoses.size())
 		{
-			std::map<int, Transform> refPoses = groundTruthPoses_;
+			float translational_rmse = 0.0f;
-			if(ui_->checkBox_alignPosesWithGPS->isEnabled() && 
+			float translational_mean = 0.0f;
-			   ui_->checkBox_alignPosesWithGPS->isChecked())
+			float translational_median = 0.0f;
+			float translational_std = 0.0f;
+			float translational_min = 0.0f;
+			float translational_max = 0.0f;
+			float rotational_rmse = 0.0f;
+			float rotational_mean = 0.0f;
+			float rotational_median = 0.0f;
+			float rotational_std = 0.0f;
+			float rotational_min = 0.0f;
+			float rotational_max = 0.0f;
+
+			Transform gtToMap = graph::calcRMSE(
+					refPoses,
+					optimizedPoses,
+					translational_rmse,
+					translational_mean,
+					translational_median,
+					translational_std,
+					translational_min,
+					translational_max,
+					rotational_rmse,
+					rotational_mean,
+					rotational_median,
+					rotational_std,
+					rotational_min,
+					rotational_max,
+					alignToGPS);
+
+			if(!gtToMap.isIdentity())
 			{
-				refPoses = gpsPoses_;
+				for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
-			}
-
-			// Log ground truth statistics (in TUM's RGBD-SLAM format)
-			if(refPoses.size())
-			{
-				float translational_rmse = 0.0f;
-				float translational_mean = 0.0f;
-				float translational_median = 0.0f;
-				float translational_std = 0.0f;
-				float translational_min = 0.0f;
-				float translational_max = 0.0f;
-				float rotational_rmse = 0.0f;
-				float rotational_mean = 0.0f;
-				float rotational_median = 0.0f;
-				float rotational_std = 0.0f;
-				float rotational_min = 0.0f;
-				float rotational_max = 0.0f;
-
-				Transform gtToMap = graph::calcRMSE(
-						refPoses,
-						optimizedPoses,
-						translational_rmse,
-						translational_mean,
-						translational_median,
-						translational_std,
-						translational_min,
-						translational_max,
-						rotational_rmse,
-						rotational_mean,
-						rotational_median,
-						rotational_std,
-						rotational_min,
-						rotational_max);
-
-				if(!gtToMap.isIdentity())
 				{
+					iter->second = gtToMap * iter->second;
+				}
+				if(alignToGPS && format != 5 && optimizedPoses.find(gpsValues_.begin()->first)!=optimizedPoses.end())
+				{
+					// This will make the exported first pose the GPS origin. Don't do it for KML format as is it done implicitly below.
+					int originId = gpsValues_.begin()->first;
+					Transform offset = optimizedPoses.at(originId).translation().inverse();
 					for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
 					{
-						iter->second = gtToMap * iter->second;
+						iter->second = offset * iter->second;
 					}
 				}
 			}
 		}
 	}
 
+	if(format == 5)
+	{
+		std::map<int, GPS> values;
+		GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords();
+		for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+		{
+			GeodeticCoords coord;
+			coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin);
+
+			Transform p, g;
+			int w;
+			std::string l;
+			double stamp=0.0;
+			int mapId;
+			std::vector<float> v;
+			GPS gps;
+			EnvSensors sensors;
+			dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors);
+			values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0)));
+		}
+
+		QString output = pathDatabase_ + QDir::separator() + "poses.kml";
+		QString path = QFileDialog::getSaveFileName(
+				this,
+				tr("Save File"),
+				output,
+				tr("Google Earth file (*.kml)"));
+
+		if(!path.isEmpty())
+		{
+			bool saved = graph::exportGPS(path.toStdString(), values, ui_->graphViewer->getNodeColor().rgba());
+
+			if(saved)
+			{
+				QMessageBox::information(this,
+						tr("Export poses..."),
+						tr("GPS coordinates saved to \"%1\".")
+						.arg(path));
+			}
+			else
+			{
+				QMessageBox::information(this,
+						tr("Export poses..."),
+						tr("Failed to save GPS coordinates to \"%1\"!")
+						.arg(path));
+			}
+		}
+		return;
+	}
+
 	if(optimizedPoses.size())
 	{
 		std::map<int, Transform> localTransforms;
@@ -4173,6 +4130,72 @@ void DatabaseViewer::generate3DMap()
 	else
 	{
 		optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+
+		bool alignToGPS = 
+			ui_->checkBox_alignPosesWithGPS->isEnabled() && 
+		    ui_->checkBox_alignPosesWithGPS->isChecked();
+
+		if(alignToGPS ||
+		   (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked()))
+		{
+			std::map<int, Transform> refPoses = groundTruthPoses_;
+			if(alignToGPS)
+			{
+				refPoses = gpsPoses_;
+			}
+
+			// Log ground truth statistics (in TUM's RGBD-SLAM format)
+			if(refPoses.size())
+			{
+				float translational_rmse = 0.0f;
+				float translational_mean = 0.0f;
+				float translational_median = 0.0f;
+				float translational_std = 0.0f;
+				float translational_min = 0.0f;
+				float translational_max = 0.0f;
+				float rotational_rmse = 0.0f;
+				float rotational_mean = 0.0f;
+				float rotational_median = 0.0f;
+				float rotational_std = 0.0f;
+				float rotational_min = 0.0f;
+				float rotational_max = 0.0f;
+
+				Transform gtToMap = graph::calcRMSE(
+						refPoses,
+						optimizedPoses,
+						translational_rmse,
+						translational_mean,
+						translational_median,
+						translational_std,
+						translational_min,
+						translational_max,
+						rotational_rmse,
+						rotational_mean,
+						rotational_median,
+						rotational_std,
+						rotational_min,
+						rotational_max,
+						alignToGPS);
+
+				if(!gtToMap.isIdentity())
+				{
+					for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+					{
+						iter->second = gtToMap * iter->second;
+					}
+					if(alignToGPS && optimizedPoses.find(gpsValues_.begin()->first)!=optimizedPoses.end())
+					{
+						// This will make the exported first pose the GPS origin.
+						int originId = gpsValues_.begin()->first;
+						Transform offset = optimizedPoses.at(originId).translation().inverse();
+						for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+						{
+							iter->second = offset * iter->second;
+						}
+					}
+				}
+			}
+		}
 	}
 	if(ui_->groupBox_posefiltering->isChecked())
 	{
@@ -4870,9 +4893,7 @@ void DatabaseViewer::update(int value,
 				dbDriver_->loadLinks(id, gravityLink, Link::kGravity);
 				if(!gravityLink.empty())
 				{
-					float roll,pitch,yaw;
+					Eigen::Vector3f v = gravityLink.begin()->second.transform().inverse().toEigen3f() * -Eigen::Vector3f::UnitZ();
-					gravityLink.begin()->second.transform().getEulerAngles(roll, pitch, yaw);
-					Eigen::Vector3d v = Transform(0,0,0,roll,pitch,0).toEigen3d() * -Eigen::Vector3d::UnitZ();
 					labelGravity->setText(QString("x=%1 y=%2 z=%3").arg(v[0]).arg(v[1]).arg(v[2]));
 					labelGravity->setToolTip(QString("roll=%1 pitch=%2 yaw=%3").arg(roll).arg(pitch).arg(yaw));
 				}
@@ -5098,13 +5119,8 @@ void DatabaseViewer::update(int value,
 					if(!gravityLink.empty() && ui_->checkBox_gravity_3dview->isChecked())
 					{
 						Transform gravityT = gravityLink.begin()->second.transform();
-						Eigen::Vector3f gravity(0,0,-1);
+						Eigen::Vector3f gravity =  gravityT.inverse().toEigen3f()*-Eigen::Vector3f::UnitZ();
-						if(pose.isIdentity())
+						cloudViewer_->addOrUpdateLine("gravity", pose, pose*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::yellow, true, false);
-						{
-							gravityT = gravityT.inverse();
-						}
-						gravity = (gravityT.rotation()*(pose).rotation().inverse()).toEigen3f()*gravity;
-						cloudViewer_->addOrUpdateLine("gravity", pose, (pose).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*pose.rotation().inverse(), Qt::yellow, true, false);
 					}
 
 					//add scan
@@ -6960,7 +6976,8 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 		std::map<int, rtabmap::Transform> graph = uValueAt(graphes_, value);
 
 		std::map<int, Transform> refPoses = groundTruthPoses_;
-		if(ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked())
+		bool alignToGPS = ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked();
+		if(alignToGPS)
 		{
 			refPoses = gpsPoses_;
 		}
@@ -7006,7 +7023,8 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 					rotational_median,
 					rotational_std,
 					rotational_min,
-					rotational_max);
+					rotational_max,
+					alignToGPS);
 
 			// ground truth live statistics
 			ui_->label_rmse->setNum(translational_rmse);
@@ -7024,7 +7042,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			UINFO("rotational_min=%f", rotational_min);
 			UINFO("rotational_max=%f", rotational_max);
 
-			if(((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) ||
+			if((alignToGPS ||
 				(ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked())) &&
 			   !gtToMap.isIdentity())
 			{

guilib/src/ExportCloudsDialog.cpp

@@ -816,7 +816,7 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->doubleSpinBox_gp3Mu->setValue(2.5);
 	_ui->doubleSpinBox_meshDecimationFactor->setValue(0.0);
 	_ui->spinBox_meshMaxPolygons->setValue(0);
-	_ui->doubleSpinBox_transferColorRadius->setValue(0.025);
+	_ui->doubleSpinBox_transferColorRadius->setValue(0.05);
 	_ui->checkBox_cleanMesh->setChecked(true);
 	_ui->spinBox_mesh_minClusterSize->setValue(0);
 

guilib/src/MainWindow.cpp

@@ -3094,8 +3094,8 @@ void MainWindow::updateMapCloud(
 				{
 					Transform gravityT = linkIter->second.transform();
 					Eigen::Vector3f gravity(0,0,-_preferencesDialog->getIMUGravityLength(0));
-					gravity = (gravityT.rotation()*(iter->second).rotation().inverse()).toEigen3f()*gravity;
+					gravity = gravityT.inverse().toEigen3f()*gravity;
-					_cloudViewer->addOrUpdateLine(gravityName, iter->second, (iter->second).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*iter->second.rotation().inverse(), Qt::yellow, false, false);
+					_cloudViewer->addOrUpdateLine(gravityName, iter->second, iter->second*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::yellow, false, false);
 				}
 			}
 			else if(viewerLines.find(gravityName)!=viewerLines.end())

guilib/src/ParametersToolBox.cpp

@@ -405,7 +405,8 @@ void ParametersToolBox::addParameter(QVBoxLayout * layout,
 	// set minimum for selected parameters
 	if(key.compare(Parameters::kGridMinGroundHeight().c_str()) == 0 ||
 		key.compare(Parameters::kGridMaxGroundHeight().c_str()) == 0 ||
-		key.compare(Parameters::kGridMaxObstacleHeight().c_str()) == 0)
+		key.compare(Parameters::kGridMaxObstacleHeight().c_str()) == 0 ||
+		key.compare(Parameters::kVisDepthMaskFloorThr().c_str()) == 0)
 	{
 		widget->setMinimum(-1000000.0);
 	}

guilib/src/PreferencesDialog.cpp

@@ -995,6 +995,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_checkBox_keepBinaryData->setObjectName(Parameters::kMemBinDataKept().c_str());
 	_ui->general_checkBox_saveIntermediateNodeData->setObjectName(Parameters::kMemIntermediateNodeDataKept().c_str());
 	_ui->lineEdit_rgbCompressionFormat->setObjectName(Parameters::kMemImageCompressionFormat().c_str());
+	_ui->lineEdit_depthCompressionFormat->setObjectName(Parameters::kMemDepthCompressionFormat().c_str());
 	_ui->general_checkBox_keepDescriptors->setObjectName(Parameters::kMemRawDescriptorsKept().c_str());
 	_ui->general_checkBox_saveDepth16bits->setObjectName(Parameters::kMemSaveDepth16Format().c_str());
 	_ui->general_checkBox_compressionParallelized->setObjectName(Parameters::kMemCompressionParallelized().c_str());
@@ -1057,6 +1058,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->surf_doubleSpinBox_maxDepth->setObjectName(Parameters::kKpMaxDepth().c_str());
 	_ui->surf_doubleSpinBox_minDepth->setObjectName(Parameters::kKpMinDepth().c_str());
 	_ui->checkBox_memDepthAsMask->setObjectName(Parameters::kMemDepthAsMask().c_str());
+	_ui->doubleSpinBox_memDepthMaskFloorThr->setObjectName(Parameters::kMemDepthMaskFloorThr().c_str());
 	_ui->checkBox_memStereoFromMotion->setObjectName(Parameters::kMemStereoFromMotion().c_str());
 	_ui->surf_spinBox_wordsPerImageTarget->setObjectName(Parameters::kKpMaxFeatures().c_str());
 	_ui->checkBox_kp_ssc->setObjectName(Parameters::kKpSSC().c_str());
@@ -1289,6 +1291,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_bowMaxDepth->setObjectName(Parameters::kVisMaxDepth().c_str());
 	_ui->loopClosure_bowMinDepth->setObjectName(Parameters::kVisMinDepth().c_str());
 	_ui->checkBox_visDepthAsMask->setObjectName(Parameters::kVisDepthAsMask().c_str());
+	_ui->doubleSpinBox_visDepthMaskFloorThr->setObjectName(Parameters::kVisDepthMaskFloorThr().c_str());
 	_ui->loopClosure_roi->setObjectName(Parameters::kVisRoiRatios().c_str());
 	_ui->subpix_winSize->setObjectName(Parameters::kVisSubPixWinSize().c_str());
 	_ui->subpix_iterations->setObjectName(Parameters::kVisSubPixIterations().c_str());
@@ -4869,22 +4872,46 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 				{
 					if(valueInt==1 && combo->objectName().toStdString().compare(Parameters::kOptimizerStrategy()) == 0)
 					{
-						UWARN("Trying to set \"%s\" to g2o but RTAB-Map isn't built "
+						if(Optimizer::isAvailable(Optimizer::kTypeGTSAM)) {
-							  "with g2o. Keeping default combo value: %s.",
+							UWARN("Trying to set \"%s\" to g2o but RTAB-Map isn't built "
-							  combo->objectName().toStdString().c_str(),
+								"with g2o. Falling back to GTSAM.",
-							  combo->currentText().toStdString().c_str());
+								combo->objectName().toStdString().c_str());
-						ok = false;
+							valueInt = 2;
+						}
+						else
+						{
+							UWARN("Trying to set \"%s\" to g2o but RTAB-Map isn't built "
+								"with g2o. Keeping default combo value: %s.",
+								combo->objectName().toStdString().c_str(),
+								combo->currentText().toStdString().c_str());
+							ok = false;
+						}
 					}
 				}
 				if(!Optimizer::isAvailable(Optimizer::kTypeGTSAM))
 				{
 					if(valueInt==2 && combo->objectName().toStdString().compare(Parameters::kOptimizerStrategy()) == 0)
 					{
-						UWARN("Trying to set \"%s\" to GTSAM but RTAB-Map isn't built "
+					if(
-							  "with GTSAM. Keeping default combo value: %s.",
+#ifndef RTABMAP_ORB_SLAM
-							  combo->objectName().toStdString().c_str(),
+						Optimizer::isAvailable(Optimizer::kTypeG2O)
-							  combo->currentText().toStdString().c_str());
+#else
-						ok = false;
+						true
+#endif
+						){
+							UWARN("Trying to set \"%s\" to GTSAM but RTAB-Map isn't built "
+								"with GTSAM. Falling back to g2o.",
+								combo->objectName().toStdString().c_str());
+							valueInt = 1;
+						}
+						else
+						{
+							UWARN("Trying to set \"%s\" to GTSAM but RTAB-Map isn't built "
+								"with GTSAM. Keeping default combo value: %s.",
+								combo->objectName().toStdString().c_str(),
+								combo->currentText().toStdString().c_str());
+							ok = false;
+						}
 					}
 				}
 				if(ok)

guilib/src/ui/exportCloudsDialog.ui

@@ -23,9 +23,9 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>-1328</y>
+        <y>-2995</y>
         <width>885</width>
-        <height>6169</height>
+        <height>6152</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_13">
@@ -2353,7 +2353,7 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
                <string> m</string>
               </property>
               <property name="decimals">
-               <number>2</number>
+               <number>3</number>
               </property>
               <property name="minimum">
                <double>-1.000000000000000</double>

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-269</y>
+            <y>0</y>
             <width>713</width>
             <height>4653</height>
            </rect>
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>5</number>
+                <number>9</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -10315,7 +10315,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <item row="8" column="1">
                        <widget class="QLabel" name="label_retrieved_4">
                         <property name="text">
-                         <string>Initialize the Woking Memory with all nodes from Long-Term memory, instead of only nodes of the last session. This may be useful in localization mode, where less processing time is required than in SLAM mode, so more nodes can be kept in Working Memory.</string>
+                         <string>Initialize the Working Memory with all nodes from Long-Term memory, instead of only nodes of the last session. This may be useful in localization mode, where less processing time is required than in SLAM mode, so more nodes can be kept in Working Memory.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -10666,16 +10666,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <layout class="QVBoxLayout" name="verticalLayout_60">
                        <item>
                         <layout class="QGridLayout" name="gridLayout_32" columnstretch="0,1">
-                         <item row="5" column="1">
+                         <item row="1" column="1">
-                          <widget class="QLabel" name="label_22">
+                          <widget class="QLabel" name="label_53">
-                           <property name="toolTip">
-                            <string>0 means that the response (hessian) threshold 
-used for the detector will not be adapted. 
-Otherwise, the threshold is modified to 
-generate the number of words requested.</string>
-                           </property>
                            <property name="text">
-                            <string>Maximum words per image (0=no maximum). Setting to -1 will disable features extraction, so disabling loop closure detection indirectly.</string>
+                            <string>Maximum words depth (0 means inf). Only used when a depth image is provided. Applied before &quot;Maximum words per image&quot;.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -10685,61 +10679,26 @@ generate the number of words requested.</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="7" column="1">
+                         <item row="12" column="0">
-                          <widget class="QLabel" name="label_81">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi2">
-                           <property name="text">
+                           <property name="suffix">
-                            <string>Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).</string>
+                            <string> %</string>
                            </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="11" column="1">
-                          <widget class="QLabel" name="label_99">
-                           <property name="text">
-                            <string>Top ROI ratio (0 = no change).</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="7" column="0">
-                          <widget class="QDoubleSpinBox" name="surf_doubleSpinBox_ratioBadSign">
                            <property name="decimals">
-                            <number>2</number>
+                            <number>0</number>
                            </property>
+                          </widget>
+                         </item>
+                         <item row="6" column="0">
+                          <widget class="QSpinBox" name="surf_spinBox_wordsPerImageTarget">
                            <property name="minimum">
-                            <double>0.000000000000000</double>
+                            <number>-1</number>
                            </property>
                            <property name="maximum">
-                            <double>1.000000000000000</double>
+                            <number>2000</number>
-                           </property>
-                           <property name="singleStep">
-                            <double>0.050000000000000</double>
                            </property>
                            <property name="value">
-                            <double>0.250000000000000</double>
+                            <number>500</number>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="2" column="1">
-                          <widget class="QLabel" name="label_57">
-                           <property name="text">
-                            <string>Minimum words depth. Only used when a depth image is provided. Applied before &quot;Maximum words per image&quot;.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                            </property>
                           </widget>
                          </item>
@@ -10827,17 +10786,75 @@ generate the number of words requested.</string>
                            </item>
                           </widget>
                          </item>
+                         <item row="15" column="1">
+                          <widget class="QLabel" name="label_86">
+                           <property name="text">
+                            <string>Number of columns of the grid used to extract uniformly &quot;max words / grid cells&quot; features from each cell.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="2" column="1">
+                          <widget class="QLabel" name="label_57">
+                           <property name="text">
+                            <string>Minimum words depth. Only used when a depth image is provided. Applied before &quot;Maximum words per image&quot;.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="7" column="1">
+                          <widget class="QLabel" name="label_7551">
+                           <property name="text">
+                            <string>If true, SSC (Suppression via Square Covering) is applied to limit keypoints.</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="QDoubleSpinBox" name="surf_doubleSpinBox_ratioBadSign">
+                           <property name="decimals">
+                            <number>2</number>
+                           </property>
+                           <property name="minimum">
+                            <double>0.000000000000000</double>
+                           </property>
+                           <property name="maximum">
+                            <double>1.000000000000000</double>
+                           </property>
+                           <property name="singleStep">
+                            <double>0.050000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>0.250000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="9" column="0">
                           <widget class="QLineEdit" name="lineEdit_kp_roi">
                            <property name="readOnly">
                             <bool>true</bool>
                            </property>
                           </widget>
                          </item>
-                         <item row="8" column="1">
+                         <item row="5" column="1">
-                          <widget class="QLabel" name="label_101">
+                          <widget class="QLabel" name="label_582">
                            <property name="text">
-                            <string>ROI ratios [left, right, top, bottom] between 0 and 1.</string>
+                            <string>Triangulate features without depth using stereo from motion (odometry). It would be ignored if depth as mask is checked and the feature detector used supports masking.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -10847,21 +10864,8 @@ generate the number of words requested.</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="1" column="1">
+                         <item row="13" column="0">
-                          <widget class="QLabel" name="label_53">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi3">
-                           <property name="text">
-                            <string>Maximum words depth (0 means inf). Only used when a depth image is provided. Applied before &quot;Maximum words per image&quot;.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="10" column="0">
-                          <widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi1">
                            <property name="suffix">
                             <string> %</string>
                            </property>
@@ -10870,19 +10874,6 @@ generate the number of words requested.</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="0" column="1">
-                          <widget class="QLabel" name="label_117">
-                           <property name="text">
-                            <string>Visual word type.</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="0">
                           <widget class="QDoubleSpinBox" name="surf_doubleSpinBox_maxDepth">
                            <property name="suffix">
@@ -10905,30 +10896,10 @@ generate the number of words requested.</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="5" column="0">
+                         <item row="3" column="1">
-                          <widget class="QSpinBox" name="surf_spinBox_wordsPerImageTarget">
+                          <widget class="QLabel" name="label_262">
-                           <property name="minimum">
-                            <number>-1</number>
-                           </property>
-                           <property name="maximum">
-                            <number>2000</number>
-                           </property>
-                           <property name="value">
-                            <number>500</number>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="6" column="0">
-                          <widget class="QCheckBox" name="checkBox_kp_ssc">
                            <property name="text">
-                            <string/>
+                            <string>Use depth image as mask when extracting features.</string>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="6" column="1">
-                          <widget class="QLabel" name="label_7551">
-                           <property name="text">
-                            <string>If true, SSC (Suppression via Square Covering) is applied to limit keypoints.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -10938,30 +10909,7 @@ generate the number of words requested.</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="9" column="0">
+                         <item row="11" column="1">
-                          <widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi0">
-                           <property name="suffix">
-                            <string> %</string>
-                           </property>
-                           <property name="decimals">
-                            <number>0</number>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="9" column="1">
-                          <widget class="QLabel" name="label_97">
-                           <property name="text">
-                            <string>Left ROI ratio (0 = no change).</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="10" column="1">
                           <widget class="QLabel" name="label_98">
                            <property name="text">
                             <string>Right ROI ratio (0 = no change).</string>
@@ -10974,30 +10922,10 @@ generate the number of words requested.</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="11" column="0">
-                          <widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi2">
-                           <property name="suffix">
-                            <string> %</string>
-                           </property>
-                           <property name="decimals">
-                            <number>0</number>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="12" column="0">
-                          <widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi3">
-                           <property name="suffix">
-                            <string> %</string>
-                           </property>
-                           <property name="decimals">
-                            <number>0</number>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="12" column="1">
-                          <widget class="QLabel" name="label_100">
+                          <widget class="QLabel" name="label_99">
                            <property name="text">
-                            <string>Bottom ROI ratio (0 = no change).</string>
+                            <string>Top ROI ratio (0 = no change).</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -11007,6 +10935,131 @@ generate the number of words requested.</string>
                            </property>
                           </widget>
                          </item>
+                         <item row="6" column="1">
+                          <widget class="QLabel" name="label_22">
+                           <property name="toolTip">
+                            <string>0 means that the response (hessian) threshold 
+used for the detector will not be adapted. 
+Otherwise, the threshold is modified to 
+generate the number of words requested.</string>
+                           </property>
+                           <property name="text">
+                            <string>Maximum words per image (0=no maximum). Setting to -1 will disable features extraction, so disabling loop closure detection indirectly.</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="1">
+                          <widget class="QLabel" name="label_117">
+                           <property name="text">
+                            <string>Visual word type.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="10" column="1">
+                          <widget class="QLabel" name="label_97">
+                           <property name="text">
+                            <string>Left ROI ratio (0 = no change).</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="10" column="0">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi0">
+                           <property name="suffix">
+                            <string> %</string>
+                           </property>
+                           <property name="decimals">
+                            <number>0</number>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="14" column="0">
+                          <widget class="QSpinBox" name="spinBox_KPGridRows">
+                           <property name="minimum">
+                            <number>1</number>
+                           </property>
+                           <property name="maximum">
+                            <number>99</number>
+                           </property>
+                           <property name="value">
+                            <number>1</number>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="5" column="0">
+                          <widget class="QCheckBox" name="checkBox_memStereoFromMotion">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="9" column="1">
+                          <widget class="QLabel" name="label_101">
+                           <property name="text">
+                            <string>ROI ratios [left, right, top, bottom] between 0 and 1.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="11" column="0">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi1">
+                           <property name="suffix">
+                            <string> %</string>
+                           </property>
+                           <property name="decimals">
+                            <number>0</number>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="3" column="0">
+                          <widget class="QCheckBox" name="checkBox_memDepthAsMask">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="8" column="1">
+                          <widget class="QLabel" name="label_81">
+                           <property name="text">
+                            <string>Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="7" column="0">
+                          <widget class="QCheckBox" name="checkBox_kp_ssc">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
                          <item row="2" column="0">
                           <widget class="QDoubleSpinBox" name="surf_doubleSpinBox_minDepth">
                            <property name="suffix">
@@ -11029,7 +11082,7 @@ generate the number of words requested.</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="13" column="1">
+                         <item row="14" column="1">
                           <widget class="QLabel" name="label_50">
                            <property name="text">
                             <string>Number of rows of the grid used to extract uniformly &quot;max words / grid cells&quot; features from each cell.</string>
@@ -11042,20 +11095,20 @@ generate the number of words requested.</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="13" column="0">
+                         <item row="13" column="1">
-                          <widget class="QSpinBox" name="spinBox_KPGridRows">
+                          <widget class="QLabel" name="label_100">
-                           <property name="minimum">
+                           <property name="text">
-                            <number>1</number>
+                            <string>Bottom ROI ratio (0 = no change).</string>
                            </property>
-                           <property name="maximum">
+                           <property name="wordWrap">
-                            <number>99</number>
+                            <bool>true</bool>
                            </property>
-                           <property name="value">
+                           <property name="textInteractionFlags">
-                            <number>1</number>
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                            </property>
                           </widget>
                          </item>
-                         <item row="14" column="0">
+                         <item row="15" column="0">
                           <widget class="QSpinBox" name="spinBox_KPGridCols">
                            <property name="minimum">
                             <number>1</number>
@@ -11068,43 +11121,10 @@ generate the number of words requested.</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="14" column="1">
-                          <widget class="QLabel" name="label_86">
-                           <property name="text">
-                            <string>Number of columns of the grid used to extract uniformly &quot;max words / grid cells&quot; features from each cell.</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="1">
-                          <widget class="QLabel" name="label_262">
-                           <property name="text">
-                            <string>Use depth image as mask when extracting features.</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="checkBox_memDepthAsMask">
-                           <property name="text">
-                            <string/>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="4" column="1">
-                          <widget class="QLabel" name="label_582">
+                          <widget class="QLabel" name="label_591">
                            <property name="text">
-                            <string>Triangulate features without depth using stereo from motion (odometry). It would be ignored if depth as mask is checked and the feature detector used supports masking.</string>
+                            <string>Filter floor from depth mask. 0 means disabled, negative means keeping pixels below the floor theshold instead.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -11115,9 +11135,24 @@ generate the number of words requested.</string>
                           </widget>
                          </item>
                          <item row="4" column="0">
-                          <widget class="QCheckBox" name="checkBox_memStereoFromMotion">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_memDepthMaskFloorThr">
-                           <property name="text">
+                           <property name="suffix">
-                            <string/>
+                            <string> m</string>
+                           </property>
+                           <property name="decimals">
+                            <number>2</number>
+                           </property>
+                           <property name="minimum">
+                            <double>-99.000000000000000</double>
+                           </property>
+                           <property name="maximum">
+                            <double>99.000000000000000</double>
+                           </property>
+                           <property name="singleStep">
+                            <double>0.050000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>0.000000000000000</double>
                            </property>
                           </widget>
                          </item>
@@ -11554,7 +11589,7 @@ When set to false, no new words are added to dictionary, so no more updates are
                       </property>
                      </widget>
                     </item>
-                    <item row="6" column="0">
+                    <item row="7" column="0">
                      <widget class="QComboBox" name="comboBox_dbJournalMode">
                       <item>
                        <property name="text">
@@ -11596,7 +11631,7 @@ When set to false, no new words are added to dictionary, so no more updates are
                       </property>
                      </widget>
                     </item>
-                    <item row="8" column="1">
+                    <item row="9" column="1">
                      <widget class="QLabel" name="label_120">
                       <property name="text">
                        <string>Sqlite3 temp store,
@@ -11610,7 +11645,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                       </property>
                      </widget>
                     </item>
-                    <item row="8" column="0">
+                    <item row="9" column="0">
                      <widget class="QComboBox" name="comboBox_dbTempStore">
                       <item>
                        <property name="text">
@@ -11639,7 +11674,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                       </property>
                      </widget>
                     </item>
-                    <item row="5" column="0">
+                    <item row="6" column="0">
                      <widget class="QSpinBox" name="spinBox_dbCacheSize">
                       <property name="minimum">
                        <number>10</number>
@@ -11655,7 +11690,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                       </property>
                      </widget>
                     </item>
-                    <item row="5" column="1">
+                    <item row="6" column="1">
                      <widget class="QLabel" name="label_75">
                       <property name="text">
                        <string>Sqlite3 cache size,
@@ -11669,7 +11704,7 @@ see Sqlite3 doc 'PRAGMA cache_size'.</string>
                       </property>
                      </widget>
                     </item>
-                    <item row="6" column="1">
+                    <item row="7" column="1">
                      <widget class="QLabel" name="label_77">
                       <property name="text">
                        <string>Sqlite3 journal mode,
@@ -11683,7 +11718,7 @@ see Sqlite3 doc 'PRAGMA journal_mode'.</string>
                       </property>
                      </widget>
                     </item>
-                    <item row="7" column="1">
+                    <item row="8" column="1">
                      <widget class="QLabel" name="label_89">
                       <property name="text">
                        <string>Sqlite3 synchronous,
@@ -11697,7 +11732,7 @@ see Sqlite3 doc 'PRAGMA synchronous'.</string>
                       </property>
                      </widget>
                     </item>
-                    <item row="7" column="0">
+                    <item row="8" column="0">
                      <widget class="QComboBox" name="comboBox_dbSynchronous">
                       <property name="currentIndex">
                        <number>2</number>
@@ -11749,6 +11784,22 @@ see Sqlite3 doc 'PRAGMA synchronous'.</string>
                      <widget class="QLineEdit" name="lineEdit_rgbCompressionFormat"/>
                     </item>
                     <item row="4" column="1">
+                     <widget class="QLabel" name="label_760">
+                      <property name="text">
+                       <string>Depth image compression format (should be &quot;.png&quot; or &quot;.rvl&quot;).</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_depthCompressionFormat"/>
+                    </item>
+                    <item row="5" 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>
@@ -11761,7 +11812,7 @@ see Sqlite3 doc 'PRAGMA synchronous'.</string>
                       </property>
                      </widget>
                     </item>
-                    <item row="4" column="0">
+                    <item row="5" column="0">
                      <widget class="QLineEdit" name="lineEdit_targetDatabaseVersion">
                       <property name="placeholderText">
                        <string>major.minor</string>
@@ -13887,11 +13938,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                           </widget>
                          </item>
                          <item row="3" column="0">
-                          <widget class="QLineEdit" name="lineEdit_grid_roi">
+                          <widget class="QLineEdit" name="lineEdit_grid_roi"/>
-                           <property name="readOnly">
-                            <bool>true</bool>
-                           </property>
-                          </widget>
                          </item>
                          <item row="3" column="1">
                           <widget class="QLabel" name="label_324">
@@ -22557,10 +22604,10 @@ Lower the ratio -&gt; higher the precision.</string>
                        </item>
                        <item>
                         <layout class="QGridLayout" name="gridLayout_25" columnstretch="0,1">
-                         <item row="4" column="1">
+                         <item row="6" column="1">
-                          <widget class="QLabel" name="label_237">
+                          <widget class="QLabel" name="label_450">
                            <property name="text">
-                            <string>Maximum feature depth.</string>
+                            <string>Use depth image as mask when extracting features.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -22570,49 +22617,6 @@ Lower the ratio -&gt; higher the precision.</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="7" column="1">
-                          <widget class="QLabel" name="label_261">
-                           <property name="text">
-                            <string>ROI ratios [left right top bottom] between 0 and 1.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="7" column="0">
-                          <widget class="QLineEdit" name="loopClosure_roi"/>
-                         </item>
-                         <item row="2" column="0">
-                          <widget class="QSpinBox" name="reextract_maxFeatures">
-                           <property name="maximum">
-                            <number>999999</number>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="2" column="1">
-                          <widget class="QLabel" name="label_180">
-                           <property name="text">
-                            <string>Max features extracted from the images (0 means inf).</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="checkBox_visSSC">
-                           <property name="text">
-                            <string/>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="3" column="1">
                           <widget class="QLabel" name="label_756">
                            <property name="text">
@@ -22626,6 +22630,32 @@ Lower the ratio -&gt; higher the precision.</string>
                            </property>
                           </widget>
                          </item>
+                         <item row="4" column="1">
+                          <widget class="QLabel" name="label_237">
+                           <property name="text">
+                            <string>Maximum feature depth.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="10" column="0">
+                          <widget class="QSpinBox" name="reextract_gridcols">
+                           <property name="minimum">
+                            <number>1</number>
+                           </property>
+                           <property name="maximum">
+                            <number>99</number>
+                           </property>
+                           <property name="value">
+                            <number>1</number>
+                           </property>
+                          </widget>
+                         </item>
                          <item row="5" column="1">
                           <widget class="QLabel" name="label_275">
                            <property name="text">
@@ -22639,6 +22669,79 @@ Lower the ratio -&gt; higher the precision.</string>
                            </property>
                           </widget>
                          </item>
+                         <item row="9" column="1">
+                          <widget class="QLabel" name="label_438">
+                           <property name="text">
+                            <string>Number of rows of the grid used to extract uniformly &quot;max features / grid cells&quot; features from each cell.</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="loopClosure_bowMinDepth">
+                           <property name="suffix">
+                            <string> m</string>
+                           </property>
+                           <property name="minimum">
+                            <double>0.000000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>0.100000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="3" column="0">
+                          <widget class="QCheckBox" name="checkBox_visSSC">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="6" column="0">
+                          <widget class="QCheckBox" name="checkBox_visDepthAsMask">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="10" column="1">
+                          <widget class="QLabel" name="label_439">
+                           <property name="text">
+                            <string>Number of columns of the grid used to extract uniformly &quot;max features / grid cells&quot; features from each cell.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="2" column="0">
+                          <widget class="QSpinBox" name="reextract_maxFeatures">
+                           <property name="maximum">
+                            <number>999999</number>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="2" column="1">
+                          <widget class="QLabel" name="label_180">
+                           <property name="text">
+                            <string>Max features extracted from the images (0 means inf).</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="0">
                           <widget class="QComboBox" name="vis_feature_detector">
                            <property name="currentIndex">
@@ -22729,6 +22832,16 @@ Lower the ratio -&gt; higher the precision.</string>
                            </item>
                           </widget>
                          </item>
+                         <item row="1" column="1">
+                          <widget class="QLabel" name="label_178">
+                           <property name="text">
+                            <string>Feature detector </string>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
                          <item row="4" column="0">
                           <widget class="QDoubleSpinBox" name="loopClosure_bowMaxDepth">
                            <property name="suffix">
@@ -22742,30 +22855,10 @@ Lower the ratio -&gt; higher the precision.</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="1" column="1">
-                          <widget class="QLabel" name="label_178">
-                           <property name="text">
-                            <string>Feature detector </string>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="5" column="0">
-                          <widget class="QDoubleSpinBox" name="loopClosure_bowMinDepth">
-                           <property name="suffix">
-                            <string> m</string>
-                           </property>
-                           <property name="minimum">
-                            <double>0.000000000000000</double>
-                           </property>
-                           <property name="value">
-                            <double>0.100000000000000</double>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="8" column="0">
+                          <widget class="QLineEdit" name="loopClosure_roi"/>
+                         </item>
+                         <item row="9" column="0">
                           <widget class="QSpinBox" name="reextract_gridrows">
                            <property name="minimum">
                             <number>1</number>
@@ -22779,9 +22872,9 @@ Lower the ratio -&gt; higher the precision.</string>
                           </widget>
                          </item>
                          <item row="8" column="1">
-                          <widget class="QLabel" name="label_438">
+                          <widget class="QLabel" name="label_261">
                            <property name="text">
-                            <string>Number of rows of the grid used to extract uniformly &quot;max features / grid cells&quot; features from each cell.</string>
+                            <string>ROI ratios [left right top bottom] between 0 and 1.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -22791,10 +22884,10 @@ Lower the ratio -&gt; higher the precision.</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="9" column="1">
+                         <item row="7" column="1">
-                          <widget class="QLabel" name="label_439">
+                          <widget class="QLabel" name="label_759">
                            <property name="text">
-                            <string>Number of columns of the grid used to extract uniformly &quot;max features / grid cells&quot; features from each cell.</string>
+                            <string>Filter floor from depth mask. 0 means disabled, negative means keeping pixels below the floor theshold instead.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -22804,36 +22897,19 @@ Lower the ratio -&gt; higher the precision.</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="9" column="0">
+                         <item row="7" column="0">
-                          <widget class="QSpinBox" name="reextract_gridcols">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_visDepthMaskFloorThr">
+                           <property name="suffix">
+                            <string> m</string>
+                           </property>
                            <property name="minimum">
-                            <number>1</number>
+                            <double>-99.000000000000000</double>
                            </property>
-                           <property name="maximum">
+                           <property name="singleStep">
-                            <number>99</number>
+                            <double>0.050000000000000</double>
                            </property>
                            <property name="value">
-                            <number>1</number>
+                            <double>0.000000000000000</double>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="6" column="1">
-                          <widget class="QLabel" name="label_450">
-                           <property name="text">
-                            <string>Use depth image as mask when extracting features.</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="0">
-                          <widget class="QCheckBox" name="checkBox_visDepthAsMask">
-                           <property name="text">
-                            <string/>
                            </property>
                           </widget>
                          </item>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>rtabmap</name>
-  <version>0.21.9</version>
+  <version>0.21.10</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>