g2o vcpkg latest issue

Export: added gps/gt export options
add support for RVL depth image compression (#1409 )
2026-09-08 12:30:20 +08:00 · 2024-12-24 16:42:26 -08:00 · 2024-12-22 11:33:41 -08:00 · 2024-12-15 18:35:05 -08:00 · 2024-12-15 17:51:15 -08:00 · 2024-12-15 13:08:15 -08:00
59 changed files with 3422 additions and 1500 deletions
@@ -4,5 +4,6 @@
        "vscode": {
          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"]
        }
-      }
+      },
+    "runArgs": ["--privileged", "--network=host"]
 }
@@ -1 +1,2 @@
 build/*
+build_*
@@ -23,20 +23,15 @@ jobs:
    strategy:
      fail-fast: false
      matrix:
-        ros_distribution: [ noetic, humble, iron]
+        ros_distribution: [ noetic, humble ]
        include:
        - ros_distribution: 'noetic'
          os: ubuntu-20.04
        - ros_distribution: 'humble'
          os: ubuntu-22.04
-        - ros_distribution: 'iron'
-          os: ubuntu-22.04
-        # Currently CI has some errors on setup-ros with this OS, disabling for now
-        #- ros_distribution: 'jazzy'
-        #  os: ubuntu-24.04

    steps:   
-    - uses: ros-tooling/setup-ros@v0.6
+    - uses: ros-tooling/setup-ros@v0.7
      with:
        required-ros-distributions: ${{ matrix.ros_distribution }}
    
@@ -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 8)
+SET(RTABMAP_PATCH_VERSION 9)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -61,7 +61,7 @@ IF(WIN32 AND NOT MINGW)
    ADD_DEFINITIONS("-DNOMINMAX")
    ADD_DEFINITIONS("-wd4100 -wd4127 -wd4150 -wd4191 -wd4242 -wd4244 -wd4251 -wd4305 -wd4365 -wd4512 -wd4514 -wd4548 -wd4571 -wd4619 -wd4625 -wd4626 -wd4628 -wd4668 -wd4710 -wd4711 -wd4738 -wd4820 -wd4946 -wd4986")
 ELSE ()
-    ADD_DEFINITIONS( "-Wall" )
+    ADD_DEFINITIONS( "-Wall -Wtype-limits" )
    ADD_DEFINITIONS("-Wno-unknown-pragmas")
 ENDIF()

@@ -797,9 +797,21 @@ ENDIF(WITH_VINS)

 IF(WITH_OPENVINS)
    FIND_PACKAGE(ov_msckf QUIET)
-    IF(ov_msckf_FOUND)
-       MESSAGE(STATUS "Found ov_msckf: ${ov_msckf_INCLUDE_DIRS}")
-    ENDIF(ov_msckf_FOUND)
+    # On ROS2, the indirect includes and libraries
+    # are not forwarded by ov_msckf target, append them manually
+    FIND_PACKAGE(ov_core QUIET)
+    FIND_PACKAGE(ov_init QUIET)
+    IF(ov_msckf_FOUND AND ov_core_FOUND AND ov_init_FOUND)
+      SET(ov_msckf_INCLUDE_DIRS
+         ${ov_msckf_INCLUDE_DIRS}
+         ${ov_core_INCLUDE_DIRS}
+         ${ov_init_INCLUDE_DIRS})
+      SET(ov_msckf_LIBRARIES
+         ${ov_msckf_LIBRARIES}
+         ${ov_core_LIBRARIES}
+         ${ov_init_LIBRARIES})
+      MESSAGE(STATUS "Found OpenVINS: ${ov_msckf_INCLUDE_DIRS}")
+    ENDIF()
 ENDIF(WITH_OPENVINS)

 IF(WITH_FASTCV)
@@ -917,10 +929,22 @@ ENDIF()
 IF(NOT G2O_FOUND)
   SET(G2O "//")
   SET(G2O_CPP_CONF "//")
+   SET(G2O_SRC_DIR "//")
 ELSE()
   IF(NOT G2O_CPP11)
     SET(G2O_CPP_CONF "//")
   ENDIF(NOT G2O_CPP11)
+   IF(NOT G2O_INCLUDE_DIRS)
+     get_target_property(G2O_INCLUDE_DIRS g2o::core INTERFACE_INCLUDE_DIRECTORIES)
+   ENDIF()
+   FIND_FILE(G2O_CONFIG_H g2o/config.h
+      PATHS ${G2O_INCLUDE_DIRS}
+      NO_DEFAULT_PATH)
+   FILE(READ ${G2O_CONFIG_H} TMPTXT)
+   STRING(FIND "${TMPTXT}" "G2O_SRC_DIR" matchres)
+   IF(${matchres} EQUAL -1)
+      SET(G2O_SRC_DIR "//")
+   ENDIF(${matchres} EQUAL -1)
 ENDIF()
 IF(NOT GTSAM_FOUND)
   SET(GTSAM "//")
@@ -41,6 +41,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@TORO@#define RTABMAP_TORO
@G2O@#define RTABMAP_G2O
@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11 @G2O_CPP11@
+// vcpkg removes G2O_SRC_DIR
+@G2O_SRC_DIR@#ifndef G2O_SRC_DIR
+@G2O_SRC_DIR@#define G2O_SRC_DIR
+@G2O_SRC_DIR@#endif
@GTSAM@#define RTABMAP_GTSAM
@CERES@#define RTABMAP_CERES
@MRPT@#define RTABMAP_MRPT
@@ -1775,19 +1775,19 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			long currentTime = System.currentTimeMillis()/1000;
 			if(loopClosureId > 0)
 			{
-				if (mToast != null  && mToast.getView().isShown()) mToast.cancel();
+				if (mToast != null) mToast.cancel();
 				mToast.setText(String.format("Loop closure detected! (%d/%d inliers)", inliers, matches));
 				mToast.show();
 			}
 			else if(landmarkDetected != 0)
 			{
-				if (mToast != null  && mToast.getView().isShown()) mToast.cancel();
+				if (mToast != null) mToast.cancel();
 				mToast.setText(String.format("Marker %d detected!", landmarkDetected));
 				mToast.show();
 			}
 			else if(rejected > 0)
 			{
-				if (mToast != null  && mToast.getView().isShown()) mToast.cancel();
+				if (mToast != null) mToast.cancel();
 				if(inliers >= Integer.parseInt(mMinInliers))
 				{
 					if(optimizationMaxError > 0.0f)
@@ -1809,7 +1809,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			{
 				if(currentTime - mLastFastMovementNotificationStamp > 3)
 				{
-					if (mToast != null  && mToast.getView().isShown()) mToast.cancel();
+					if (mToast != null) mToast.cancel();
 					mToast.setText("Move slower... blurry images are not added to map (\"Settings->Mapping...->Maximum Motion Speed\" is enabled).");
 					mToast.show();
 				}
@@ -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_ */
@@ -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;
@@ -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;
@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <list>
+#include <numeric>
 #include "rtabmap/core/Parameters.h"
 #include "rtabmap/core/SensorData.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;
@@ -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.");
@@ -101,6 +101,7 @@ private:
 	bool _guessMatchToProjection;
 	int _bundleAdjustment;
 	bool _depthAsMask;
+	float _maskFloorThreshold;
 	float _minInliersDistributionThr;
 	float _maxInliersMeanDistance;

@@ -69,6 +69,7 @@ public:
 	virtual std::string getSerial() const;

 	void setResolution(int width, int height) {_width=width, _height=height;}
+	void setFOURCC(const std::string & fourcc) { _fourcc = fourcc; }

 protected:
 	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
@@ -86,6 +87,7 @@ private:
 	int usbDevice2_;
 	int _width;
 	int _height;
+	std::string _fourcc;
 };

 } // namespace rtabmap
@@ -62,6 +62,7 @@ public:
 	 * has been loaded, thus resolution from calibration is used.
 	 * */
 	void setResolution(int width, int height) {_width=width, _height=height;}
+	void setFOURCC(const std::string & fourcc) { _fourcc = fourcc; }

 protected:
 	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
@@ -79,6 +80,7 @@ private:
 	std::string _guid;
 	int _width;
 	int _height;
+	std::string _fourcc;

 	CameraModel _model;
 };
@@ -219,7 +219,7 @@ private:
 	bool hasColor_;
 	float rangeMax_;
 	bool rayTracing_;
-    unsigned int emptyFloodFillDepth_;
+  unsigned int emptyFloodFillDepth_;
 };

 } /* namespace rtabmap */
@@ -175,15 +175,51 @@ typename pcl::PointCloud<PointT>::Ptr LocalGridMaker::segmentCloud(
 					noiseFilteringMinNeighbors_);
 			if(groundIndices->size())
 			{
+				pcl::IndicesPtr farIndices;
+				if(rangeMax_!=0)
+				{
+					// Don't filter points farther than maximum range, in case we want to ray trace empty space
+					pcl::IndicesPtr closeIndices;
+					rtabmap::util3d::rangeSplitFiltering(cloud, groundIndices, rangeMax_, closeIndices, farIndices);
+					groundIndices = closeIndices;
+				}
 				groundIndices = rtabmap::util3d::radiusFiltering(cloud, groundIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
+				if(farIndices.get())
+				{
+					groundIndices = rtabmap::util3d::concatenate(groundIndices, farIndices);
+				}
 			}
 			if(obstaclesIndices->size())
 			{
+				pcl::IndicesPtr farIndices;
+				if(rangeMax_!=0)
+				{
+					// Don't filter points farther than maximum range, in case we want to ray trace empty space
+					pcl::IndicesPtr closeIndices;
+					rtabmap::util3d::rangeSplitFiltering(cloud, obstaclesIndices, rangeMax_, closeIndices, farIndices);
+					obstaclesIndices = closeIndices;
+				}
 				obstaclesIndices = rtabmap::util3d::radiusFiltering(cloud, obstaclesIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
+				if(farIndices.get())
+				{
+					obstaclesIndices = rtabmap::util3d::concatenate(obstaclesIndices, farIndices);
+				}
 			}
 			if(flatObstacles && (*flatObstacles)->size())
 			{
+				pcl::IndicesPtr farIndices;
+				if(rangeMax_!=0)
+				{
+					// Don't filter points farther than maximum range, in case we want to ray trace empty space
+					pcl::IndicesPtr closeIndices;
+					rtabmap::util3d::rangeSplitFiltering(cloud, *flatObstacles, rangeMax_, closeIndices, farIndices);
+					*flatObstacles = closeIndices;
+				}
 				*flatObstacles = rtabmap::util3d::radiusFiltering(cloud, *flatObstacles, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
+				if(farIndices.get())
+				{
+					*flatObstacles = rtabmap::util3d::concatenate(*flatObstacles, farIndices);
+				}
 			}
 			UDEBUG("Radius filtering end (%ld ground %ld obstacles)",
 					groundIndices->size(),
@@ -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_
@@ -165,7 +165,7 @@ void RTABMAP_CORE_EXPORT NMS(
 		int border, int dist_thresh, int img_width, int img_height);

 std::vector<int> RTABMAP_CORE_EXPORT SSC(
-	const std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, float tolerance, int cols, int rows);
+	const std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, float tolerance, int cols, int rows, const std::vector<int> & indx = {});

 /**
 * @brief Rotate images and camera model so that the top of the image is up.
@@ -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
@@ -73,6 +73,52 @@ LaserScan RTABMAP_CORE_EXPORT rangeFiltering(
 		float rangeMin,
 		float rangeMax);

+pcl::IndicesPtr RTABMAP_CORE_EXPORT rangeFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax);
+pcl::IndicesPtr RTABMAP_CORE_EXPORT rangeFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax);
+pcl::IndicesPtr RTABMAP_CORE_EXPORT rangeFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax);
+pcl::IndicesPtr RTABMAP_CORE_EXPORT rangeFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax);
+
+void RTABMAP_CORE_EXPORT rangeSplitFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range, 
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices);
+void RTABMAP_CORE_EXPORT rangeSplitFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range,
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices);
+void RTABMAP_CORE_EXPORT rangeSplitFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range,
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices);
+void RTABMAP_CORE_EXPORT rangeSplitFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range,
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices);
+
 LaserScan RTABMAP_CORE_EXPORT downsample(
 		const LaserScan & cloud,
 		int step);
@@ -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,
@@ -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
@@ -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)
-		image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
-#else
-		image = cv::imdecode(bytes, -1);
-#endif
-		if(image.type() == CV_8UC4)
+		if (compressedDepthFormat(bytes) == ".rvl")
 		{
-			// 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;
+			uint32_t cols, rows;
+        	memcpy(&cols, &bytes.data[8], 4);
+        	memcpy(&rows, &bytes.data[12], 4);
+			image = cv::Mat(rows, cols, CV_16UC1);
+			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)
-		image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
-#else
-		image = cv::imdecode(bytes, -1);
-#endif
-		if(image.type() == CV_8UC4)
+		if (compressedDepthFormat(bytes) == ".rvl")
 		{
-			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 */
@@ -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();
@@ -2483,6 +2483,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 +4719,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 +4739,8 @@ void DBDriverSqlite3::updateDepthImageQuery(
 		// Save depth
 		stepDepthUpdate(ppStmt,
 				nodeId,
-				image);
+				image,
+				format);

 		// Finalize (delete) the statement
 		rc = sqlite3_finalize(ppStmt);
@@ -5940,7 +6011,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 +6025,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
 	{
@@ -301,8 +301,24 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vecto
 		if(ssc)
 		{
 			ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
+
+			// Sorting keypoints by deacreasing order of strength
+			std::vector<float> responseVector;
+			for (unsigned int i = 0; i < keypoints.size(); i++)
+			{
+				responseVector.push_back(keypoints[i].response);
+			}
+			std::vector<int> indx(responseVector.size());
+			std::iota(std::begin(indx), std::end(indx), 0);
+
+#if CV_MAJOR_VERSION >= 4
+			cv::sortIdx(responseVector, indx, cv::SORT_DESCENDING);
+#else
+			cv::sortIdx(responseVector, indx, CV_SORT_DESCENDING);
+#endif
+
 			static constexpr float tolerance = 0.1;
-			auto ResultVec = util2d::SSC(keypoints, maxKeypoints, tolerance, imageSize.width, imageSize.height);
+			auto ResultVec = util2d::SSC(keypoints, maxKeypoints, tolerance, imageSize.width, imageSize.height, indx);
 			removed = keypoints.size()-ResultVec.size();
 			// retrieve final keypoints
 			kptsTmp.resize(ResultVec.size());
@@ -401,8 +417,24 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
 		if(ssc)
 		{
 			ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
+
+			// Sorting keypoints by deacreasing order of strength
+			std::vector<float> responseVector;
+			for (unsigned int i = 0; i < keypoints.size(); i++)
+			{
+				responseVector.push_back(keypoints[i].response);
+			}
+			std::vector<int> indx(responseVector.size());
+			std::iota(std::begin(indx), std::end(indx), 0);
+
+#if CV_MAJOR_VERSION >= 4
+			cv::sortIdx(responseVector, indx, cv::SORT_DESCENDING);
+#else
+			cv::sortIdx(responseVector, indx, CV_SORT_DESCENDING);
+#endif
+
 			static constexpr float tolerance = 0.1;
-			auto ResultVec = util2d::SSC(keypoints, maxKeypoints, tolerance, imageSize.width, imageSize.height);
+			auto ResultVec = util2d::SSC(keypoints, maxKeypoints, tolerance, imageSize.width, imageSize.height, indx);
 			removed = keypoints.size()-ResultVec.size();
 			for(unsigned int k=0; k<ResultVec.size(); ++k)
 			{
@@ -1239,10 +1271,17 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 	if(gpu_)
 	{
 #ifdef RTABMAP_CUDASIFT
-		UDEBUG("Init SiftData");
-		if(cudaSiftData_ == 0) {
-			cudaSiftData_ = new SiftData();
-			InitSiftData(*cudaSiftData_, 8192, true, true);
+		// Check if there is a cuda device
+		if(InitCuda(0, ULogger::level() == ULogger::kDebug)) {
+			UDEBUG("Init SiftData");
+			if(cudaSiftData_ == 0) {
+				cudaSiftData_ = new SiftData();
+				InitSiftData(*cudaSiftData_, 8192, true, true);
+			}
+		}
+		else{
+			UWARN("No cuda device(s) detected, CudaSift is not available! Using SIFT CPU version instead.");
+			gpu_ = false;
 		}
 #else
 		UWARN("RTAB-Map is not built with CudaSift so %s cannot be used!", Parameters::kSIFTGpu().c_str());
@@ -1276,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_)
 	{
@@ -1344,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));
 			}
@@ -1373,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
@@ -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).");
-			depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
+			if(_saveDepth16Format)
+			{
+				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());
 		}
@@ -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);
 						}
 					}
@@ -597,8 +618,9 @@ Transform RegistrationVis::computeTransformationImpl(
 							// Start from camera having the reference corner first (in case there is overlap between the cameras)
 							int startIndex = cornersFrom[i].x/subImageWidth;
 							UASSERT(startIndex < nCameras);
-							for(int c=startIndex; (c+1)%nCameras != 0; ++c)
+							for(int ci=0; ci < nCameras; ++ci)
 							{
+								int c = (ci+startIndex) % nCameras;
 								const CameraModel & model = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[c]:toSignature.sensorData().stereoCameraModels()[c].left();
 								cv::Point3f ptsInCamFrame = util3d::transformPoint(kptsFrom3D[i], inverseTransforms[c]);
 								if(ptsInCamFrame.z > 0)
@@ -769,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);
 						}
 					}
@@ -4753,7 +4753,7 @@ void Rtabmap::setMemoryThreshold(int maxMemoryAllowed)
 {
 	//must be positive, 0 mean inf memory allowed (no memory limit)
 	_maxMemoryAllowed = maxMemoryAllowed;
-	if(_maxMemoryAllowed < 0)
+	if(maxMemoryAllowed < 0)
 	{
 		ULOGGER_WARN("maxMemoryAllowed < 0, then setting it to 0 (inf).");
 		_maxMemoryAllowed = 0;
@@ -45,7 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>

-#include <pcl/io/io.h>
+#include <pcl/common/io.h>

 namespace rtabmap
 {
@@ -199,14 +199,39 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
 	{
 		if(stereoModel_.isValidForProjection())
 		{
+			if(_width > 0 && _height > 0 && (_width!=stereoModel_.left().imageWidth() || _height != stereoModel_.left().imageHeight()))
+			{
+				UWARN("Desired resolution of %dx%d is set but calibration has "
+				      "been loaded with resolution %dx%d, using calibration resolution.",
+					  _width, _height,
+					  stereoModel_.left().imageWidth(), stereoModel_.left().imageHeight());
+			}
+
 			if(capture_.isOpened())
 			{
-				capture_.set(CV_CAP_PROP_FRAME_WIDTH, stereoModel_.left().imageWidth()*(capture2_.isOpened()?1:2));
-				capture_.set(CV_CAP_PROP_FRAME_HEIGHT, stereoModel_.left().imageHeight());
+				bool resolutionSet = false;
+				resolutionSet = capture_.set(CV_CAP_PROP_FRAME_WIDTH, stereoModel_.left().imageWidth()*(capture2_.isOpened()?1:2));
+				resolutionSet = resolutionSet && capture_.set(CV_CAP_PROP_FRAME_HEIGHT, stereoModel_.left().imageHeight());
 				if(capture2_.isOpened())
 				{
-					capture2_.set(CV_CAP_PROP_FRAME_WIDTH, stereoModel_.right().imageWidth());
-					capture2_.set(CV_CAP_PROP_FRAME_HEIGHT, stereoModel_.right().imageHeight());
+					resolutionSet = resolutionSet && capture2_.set(CV_CAP_PROP_FRAME_WIDTH, stereoModel_.right().imageWidth());
+					resolutionSet = resolutionSet && capture2_.set(CV_CAP_PROP_FRAME_HEIGHT, stereoModel_.right().imageHeight());
+				}
+
+				// Check if the resolution was set successfully
+				int actualWidth = int(capture_.get(CV_CAP_PROP_FRAME_WIDTH));
+				int actualHeight = int(capture_.get(CV_CAP_PROP_FRAME_HEIGHT));
+				if(!resolutionSet ||
+				   actualWidth != stereoModel_.left().imageWidth()*(capture2_.isOpened()?1:2) ||
+				   actualHeight != stereoModel_.left().imageHeight())
+				{
+					UERROR("Calibration resolution (%dx%d) cannot be set to camera driver, "
+					      "actual resolution is %dx%d. You would have to re-calibrate with one "
+						  "supported format by your camera. "
+						  "Do \"v4l2-ctl --list-formats-ext\" to list all supported "
+						  "formats by your camera. For side-by-side format, you should set listed width/2.",
+						  stereoModel_.left().imageWidth(), stereoModel_.left().imageHeight(),
+						  actualWidth/(capture2_.isOpened()?1:2), actualHeight);
 				}
 			}
 		}
@@ -214,13 +239,92 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
 		{
 			if(capture_.isOpened())
 			{
-				capture_.set(CV_CAP_PROP_FRAME_WIDTH, _width*(capture2_.isOpened()?1:2));
-				capture_.set(CV_CAP_PROP_FRAME_HEIGHT, _height);
+				bool resolutionSet = false;
+				resolutionSet = capture_.set(CV_CAP_PROP_FRAME_WIDTH, _width*(capture2_.isOpened()?1:2));
+				resolutionSet = resolutionSet && capture_.set(CV_CAP_PROP_FRAME_HEIGHT, _height);
 				if(capture2_.isOpened())
 				{
-					capture2_.set(CV_CAP_PROP_FRAME_WIDTH, _width);
-					capture2_.set(CV_CAP_PROP_FRAME_HEIGHT, _height);
+					resolutionSet = resolutionSet && capture2_.set(CV_CAP_PROP_FRAME_WIDTH, _width);
+					resolutionSet = resolutionSet && capture2_.set(CV_CAP_PROP_FRAME_HEIGHT, _height);
 				}
+
+				// Check if the resolution was set successfully
+				int actualWidth = int(capture_.get(CV_CAP_PROP_FRAME_WIDTH));
+				int actualHeight = int(capture_.get(CV_CAP_PROP_FRAME_HEIGHT));
+				if(!resolutionSet ||
+				   actualWidth != _width*(capture2_.isOpened()?1:2) ||
+				   actualHeight != _height)
+				{
+					UWARN("Desired resolution (%dx%d) cannot be set to camera driver, "
+					      "actual resolution is %dx%d. "
+						  "Do \"v4l2-ctl --list-formats-ext\" to list all supported "
+						  "formats by your camera. For side-by-side format, you should set listed width/2.",
+						  _width, _height,
+						  actualWidth/(capture2_.isOpened()?1:2), actualHeight);
+				}
+			}
+		}
+
+		// Set FPS
+		if (this->getFrameRate() > 0)
+		{
+			bool fpsSupported = false;
+			fpsSupported = capture_.set(CV_CAP_PROP_FPS, this->getFrameRate());
+			if (capture2_.isOpened())
+			{
+				fpsSupported = fpsSupported && capture2_.set(CV_CAP_PROP_FPS, this->getFrameRate());
+			}
+			if(fpsSupported)
+			{
+				// Check if the FPS was set successfully
+				double actualFPS = capture_.get(cv::CAP_PROP_FPS);
+				
+				if(fabs(actualFPS - this->getFrameRate()) < 0.01)
+				{
+					this->setFrameRate(0);
+				}
+				else
+				{
+					UWARN("Desired FPS (%f Hz) cannot be set to camera driver, "
+					      "actual FPS is %f Hz. We will throttle to lowest FPS. "
+						  "Do \"v4l2-ctl --list-formats-ext\" to list all supported "
+						  "formats by your camera.",
+						  this->getFrameRate(), actualFPS);
+					if(this->getFrameRate() > actualFPS)
+					{
+						this->setFrameRate(0);
+					}
+				}
+			}
+		}
+
+		// Set FOURCC
+		if (!_fourcc.empty())
+		{
+			if(_fourcc.size() == 4)
+			{
+				std::string fourccUpperCase = uToUpperCase(_fourcc);
+				int fourcc = cv::VideoWriter::fourcc(fourccUpperCase.at(0), fourccUpperCase.at(1), fourccUpperCase.at(2), fourccUpperCase.at(3));
+				bool fourccSupported = false;
+				fourccSupported = capture_.set(CV_CAP_PROP_FOURCC, fourcc);
+				if (capture2_.isOpened())
+				{
+					fourccSupported = fourccSupported && capture2_.set(CV_CAP_PROP_FOURCC, fourcc);
+				}
+
+				// Check if the FOURCC was set successfully
+				int actualFourcc = int(capture_.get(CV_CAP_PROP_FOURCC));
+
+				if(!fourccSupported || actualFourcc != fourcc)
+				{
+					UWARN("Camera doesn't support provided FOURCC \"%s\". "
+						  "Do \"v4l2-ctl --list-formats-ext\" to list all supported "
+						  "formats by your camera.", fourccUpperCase.c_str());
+				}
+			}
+			else
+			{
+				UERROR("FOURCC parameter should be 4 characters, current value is \"%s\"", _fourcc.c_str());
 			}
 		}
 	}
@@ -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);
@@ -134,13 +134,101 @@ bool CameraVideo::init(const std::string & calibrationFolder, const std::string
 		{
 			if(_model.isValidForProjection())
 			{
-				_capture.set(CV_CAP_PROP_FRAME_WIDTH, _model.imageWidth());
-				_capture.set(CV_CAP_PROP_FRAME_HEIGHT, _model.imageHeight());
+				if(_width > 0 && _height > 0 && (_width!=_model.imageWidth() || _height != _model.imageHeight()))
+				{
+					UWARN("Desired resolution of %dx%d is set but calibration has "
+						"been loaded with resolution %dx%d, using calibration resolution.",
+						_width, _height,
+						_model.imageWidth(), _model.imageHeight());
+				}
+
+				bool resolutionSet = false;
+				resolutionSet = _capture.set(CV_CAP_PROP_FRAME_WIDTH, _model.imageWidth());
+				resolutionSet = resolutionSet && _capture.set(CV_CAP_PROP_FRAME_HEIGHT, _model.imageHeight());
+
+				// Check if the resolution was set successfully
+				int actualWidth = int(_capture.get(CV_CAP_PROP_FRAME_WIDTH));
+				int actualHeight = int(_capture.get(CV_CAP_PROP_FRAME_HEIGHT));
+				if(!resolutionSet ||
+				   actualWidth != _model.imageWidth() ||
+				   actualHeight != _model.imageHeight())
+				{
+					UERROR("Calibration resolution (%dx%d) cannot be set to camera driver, "
+					      "actual resolution is %dx%d. You would have to re-calibrate with one "
+						  "supported format by your camera. "
+						  "Do \"v4l2-ctl --list-formats-ext\" to list all supported "
+						  "formats by your camera.",
+						  _model.imageWidth(), _model.imageHeight(),
+						  actualWidth, actualHeight);
+				}
 			}
 			else if(_width > 0 && _height > 0)
 			{
-				_capture.set(CV_CAP_PROP_FRAME_WIDTH, _width);
-				_capture.set(CV_CAP_PROP_FRAME_HEIGHT, _height);
+				int resolutionSet = false;
+				resolutionSet = _capture.set(CV_CAP_PROP_FRAME_WIDTH, _width);
+				resolutionSet = resolutionSet && _capture.set(CV_CAP_PROP_FRAME_HEIGHT, _height);
+
+				// Check if the resolution was set successfully
+				int actualWidth = int(_capture.get(CV_CAP_PROP_FRAME_WIDTH));
+				int actualHeight = int(_capture.get(CV_CAP_PROP_FRAME_HEIGHT));
+				if(!resolutionSet || actualWidth != _width || actualHeight != _height)
+				{
+					UWARN("Desired resolution (%dx%d) cannot be set to camera driver, "
+					      "actual resolution is %dx%d. "
+						  "Do \"v4l2-ctl --list-formats-ext\" to list all supported "
+						  "formats by your camera.",
+						  _width, _height, actualWidth, actualHeight);
+				}
+			}
+
+			// Set FPS
+			if (this->getFrameRate() > 0 && _capture.set(CV_CAP_PROP_FPS, this->getFrameRate()))
+			{
+				// Check if the FPS was set successfully
+				double actualFPS = _capture.get(cv::CAP_PROP_FPS);
+				
+				if(fabs(actualFPS - this->getFrameRate()) < 0.01)
+				{
+					this->setFrameRate(0);
+				}
+				else
+				{
+					UWARN("Desired FPS (%f Hz) cannot be set to camera driver, "
+						"actual FPS is %f Hz. We will throttle to lowest FPS. "
+						"Do \"v4l2-ctl --list-formats-ext\" to list all supported "
+						"formats by your camera.",
+						this->getFrameRate(), actualFPS);
+					if(this->getFrameRate() > actualFPS)
+					{
+						this->setFrameRate(0);
+					}
+				}
+			}
+
+			// Set FOURCC
+			if (!_fourcc.empty())
+			{
+				if(_fourcc.size() == 4)
+				{
+					std::string fourccUpperCase = uToUpperCase(_fourcc);
+					int fourcc = cv::VideoWriter::fourcc(fourccUpperCase.at(0), fourccUpperCase.at(1), fourccUpperCase.at(2), fourccUpperCase.at(3));
+					
+					bool fourccSupported = _capture.set(CV_CAP_PROP_FOURCC, fourcc);
+
+					// Check if the FOURCC was set successfully
+					int actualFourcc = int(_capture.get(CV_CAP_PROP_FOURCC));
+
+					if(!fourccSupported || actualFourcc != fourcc)
+					{
+						UWARN("Camera doesn't support provided FOURCC \"%s\". "
+							"Do \"v4l2-ctl --list-formats-ext\" to list all supported "
+							"formats by your camera.", fourccUpperCase.c_str());
+					}
+				}
+				else
+				{
+					UERROR("FOURCC parameter should be 4 characters, current value is \"%s\"", _fourcc.c_str());
+				}
 			}
 		}
 		if(_rectifyImages && !_model.isValidForRectification())
@@ -66,7 +66,7 @@ void OccupancyGrid::setMap(const cv::Mat & map, float xMin, float yMin, float ce
 		{
 			for(int j=0; j<map_.cols; ++j)
 			{
-				const char value = map_.at<char>(i,j);
+				const char value = map_.at<signed char>(i,j);
 				float * info = mapInfo_.ptr<float>(i,j);
 				if(value == 0)
 				{
@@ -116,15 +116,15 @@ cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
 				const float * info = mapInfo_.ptr<float>(i, j);
 				if(info[3] == 0.0f)
 				{
-					map.at<char>(i, j) = -1; // unknown
+					map.at<signed char>(i, j) = -1; // unknown
 				}
 				else if(info[3] >= occThr)
 				{
-					map.at<char>(i, j) = 100; // unknown
+					map.at<signed char>(i, j) = 100; // unknown
 				}
 				else
 				{
-					map.at<char>(i, j) = 0; // empty
+					map.at<signed char>(i, j) = 0; // empty
 				}
 			}
 		}
@@ -155,11 +155,11 @@ cv::Mat OccupancyGrid::getProbMap(float & xMin, float & yMin) const
 				const float * info = mapInfo_.ptr<float>(i, j);
 				if(info[3] == 0.0f)
 				{
-					map.at<char>(i, j) = -1; // unknown
+					map.at<signed char>(i, j) = -1; // unknown
 				}
 				else
 				{
-					map.at<char>(i, j) = char(probability(info[3])*100.0f); // empty
+					map.at<signed char>(i, j) = char(probability(info[3])*100.0f); // empty
 				}
 			}
 		}
@@ -447,7 +447,7 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
 							UASSERT_MSG(pt.y >=0 && pt.y < map.rows && pt.x >= 0 && pt.x < map.cols,
 									uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
 											kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1).c_str());
-							char & value = map.at<char>(pt.y, pt.x);
+							signed char & value = map.at<signed char>(pt.y, pt.x);
 							if(value != -2)
 							{
 								float * info = mapInfo.ptr<float>(pt.y, pt.x);
@@ -523,7 +523,7 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
 							for(int j=ptBegin.y; j<ptEnd.y; ++j)
 							{
 								UASSERT(j < map.rows && i < map.cols);
-								char & value = map.at<char>(j, i);
+								signed char & value = map.at<signed char>(j, i);
 								float * info = mapInfo.ptr<float>(j, i);
 								int nodeId = (int)info[0];
 								if(value != -1)
@@ -573,7 +573,7 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
 							UASSERT_MSG(pt.y>=0 && pt.y < map.rows && pt.x>=0 && pt.x < map.cols,
 										uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
 												kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1).c_str());
-							char & value = map.at<char>(pt.y, pt.x);
+							signed char & value = map.at<signed char>(pt.y, pt.x);
 							if(value != -2)
 							{
 								float * info = mapInfo.ptr<float>(pt.y, pt.x);
@@ -638,7 +638,7 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
 				{
 					for(int j=1; j<map.cols-1; ++j)
 					{
-						char & value = map.at<char>(i, j);
+						signed char & value = map.at<signed char>(i, j);
 						if(value == -2)
 						{
 							value = 0;
@@ -320,7 +320,7 @@ OctoMap::OctoMap(const LocalGridCache * cache, const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);

 	Parameters::parse(parameters, Parameters::kGridGlobalFloodFillDepth(), emptyFloodFillDepth_);
-	UASSERT(emptyFloodFillDepth_>=0 && emptyFloodFillDepth_<=16);
+	UASSERT(emptyFloodFillDepth_<=16);

 	UDEBUG("rangeMax_           =%f", rangeMax_);
 	UDEBUG("rayTracing_         =%s", rayTracing_?"true":"false");
@@ -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;
-		ceres::LocalParameterization* quaternion_local_parameterization = NULL;
+#if CERES_VERSION_MAJOR >= 3 || \
+    (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);
-					problem.SetParameterization(&pose_end_iter->second.yaw_radians, angle_local_parameterization);
+					SetCeresProblemManifold(problem, &pose_begin_iter->second.yaw_radians, angle_local_manifold);
+					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);
-					problem.SetParameterization(pose_end_iter->second.q.coeffs().data(), quaternion_local_parameterization);
+					SetCeresProblemManifold(problem, pose_begin_iter->second.q.coeffs().data(), quaternion_local_manifold);
+					SetCeresProblemManifold(problem, pose_end_iter->second.q.coeffs().data(), quaternion_local_manifold);
 				}
 			}
 			//else // not supporting pose prior and landmarks
@@ -28,10 +28,11 @@
 //
 // Author: vitus@google.com (Michael Vitus)

-#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
-#define 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_MANIFOLD_H_

-#include "ceres/local_parameterization.h"
+#include "ceres/autodiff_manifold.h"
+#include "ceres/manifold.h"
 #include "normalize_angle.h"

 namespace ceres {
@@ -39,7 +40,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 +85,13 @@ class AngleLocalParameterization {
  }

  static ceres::LocalParameterization* Create() {
-    return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
-                                                     1, 1>);
+    return (new ceres::AutoDiffLocalParameterization<AngleManfold, 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_
@@ -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,
@@ -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
@@ -2203,8 +2203,10 @@ void NMS(
 }

 std::vector<int> SSC(
-	const std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, float tolerance, int cols, int rows)
+	const std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, float tolerance, int cols, int rows, const std::vector<int> & indx)
 {
+	bool useIndx = keypoints.size() == indx.size();
+
 	// several temp expression variables to simplify solution equation
 	int exp1 = rows + cols + 2*maxKeypoints;
 	long long exp2 = ((long long)4*cols + (long long)4*maxKeypoints + (long long)4*rows*maxKeypoints + (long long)rows*rows + (long long)cols*cols - (long long)2*rows*cols + (long long)4*rows*cols*maxKeypoints);
@@ -2241,27 +2243,20 @@ std::vector<int> SSC(
 		double c = (double)width / 2.0; // initializing Grid
 		int numCellCols = floor(cols / c);
 		int numCellRows = floor(rows / c);
-		std::vector<std::vector<bool>> coveredVec(numCellRows+1, std::vector<bool>(numCellCols+1, false));
+		cv::Mat coveredMask = cv::Mat::zeros(numCellRows + 1, numCellCols + 1, CV_8UC1);

 		for(unsigned int i=0; i<keypoints.size(); ++i)
 		{
-			int row = floor(keypoints[i].pt.y / c); // get position of the cell current point is located at
-			int col = floor(keypoints[i].pt.x / c);
-			if(coveredVec[row][col] == false) // if the cell is not covered
+			int row = floor(keypoints[useIndx?indx[i]:i].pt.y / c); // get position of the cell current point is located at
+			int col = floor(keypoints[useIndx?indx[i]:i].pt.x / c);
+			if(!coveredMask.at<uchar>(row, col)) // if the cell is not covered
 			{
-				result.push_back(i);
+				result.push_back(useIndx?indx[i]:i);
 				int rowMin = ((row - floor(width / c)) >= 0) ? (row - floor(width / c)) : 0; // get range which current radius is covering
 				int rowMax = ((row + floor(width / c)) <= numCellRows) ? (row + floor(width / c)) : numCellRows;
 				int colMin = ((col - floor(width / c)) >= 0) ? (col - floor(width / c)) : 0;
 				int colMax = ((col + floor(width / c)) <= numCellCols) ? (col + floor(width / c)) : numCellCols;
-				for(int rowToCov=rowMin; rowToCov<=rowMax; ++rowToCov)
-				{
-					for(int colToCov=colMin; colToCov<=colMax; ++colToCov)
-					{
-						if(!coveredVec[rowToCov][colToCov])
-							coveredVec[rowToCov][colToCov] = true; // cover cells within the square bounding box with width
-					}
-				}
+				coveredMask(cv::Range(rowMin, rowMax + 1), cv::Range(colMin, colMax + 1)) = 255; // cover cells within the square bounding box with width
 			}
 		}

@@ -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():
@@ -395,6 +395,154 @@ LaserScan rangeFiltering(
 	return scan;
 }

+template<typename PointT>
+pcl::IndicesPtr rangeFilteringImpl(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax)
+{
+	UASSERT(rangeMin >=0.0f && rangeMax>=0.0f);
+	int size = indices->empty()?cloud->size():indices->size();
+	pcl::IndicesPtr output(new std::vector<int>());
+	output->reserve(size);
+	if(!cloud->empty())
+	{
+		if(rangeMin > 0.0f || rangeMax > 0.0f)
+		{
+			float rangeMinSqrd = rangeMin * rangeMin;
+			float rangeMaxSqrd = rangeMax * rangeMax;
+			for(int i=0; i<size; ++i)
+			{
+				int index = indices->empty()?i:indices->at(i);
+				const PointT & pt = cloud->at(index);
+				float r = pt.x*pt.x + pt.y*pt.y + pt.z*pt.z;
+
+				if(rangeMin > 0.0f && r < rangeMinSqrd)
+				{
+					continue;
+				}
+				if(rangeMax > 0.0f && r > rangeMaxSqrd)
+				{
+					continue;
+				}
+
+				output->push_back(index);
+			}
+		}
+		else
+		{
+			*output = *indices;
+		}
+	}
+
+	return output;
+}
+
+pcl::IndicesPtr rangeFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax)
+{
+	return rangeFilteringImpl<pcl::PointXYZ>(cloud, indices, rangeMin, rangeMax);
+}
+pcl::IndicesPtr rangeFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax)
+{
+	return rangeFilteringImpl<pcl::PointXYZRGB>(cloud, indices, rangeMin, rangeMax);
+}
+pcl::IndicesPtr rangeFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax)
+{
+	return rangeFilteringImpl<pcl::PointNormal>(cloud, indices, rangeMin, rangeMax);
+}
+pcl::IndicesPtr rangeFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax)
+{
+	return rangeFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, rangeMin, rangeMax);
+}
+
+template<typename PointT>
+void rangeSplitFilteringImpl(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range,
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices)
+{
+	int size = indices->empty()?cloud->size():indices->size();
+	closeIndices.reset(new std::vector<int>());
+	farIndices.reset(new std::vector<int>());
+	closeIndices->reserve(size);
+	farIndices->reserve(size);
+	if(!cloud->empty())
+	{
+		float rangeSqrd = range * range;
+		for(int i=0; i<size; ++i)
+		{
+			int index = indices->empty()?i:indices->at(i);
+			const PointT & pt = cloud->at(index);
+			float r = pt.x*pt.x + pt.y*pt.y + pt.z*pt.z;
+
+			if(r < rangeSqrd)
+			{
+				closeIndices->push_back(index);
+			}
+			else
+			{
+				farIndices->push_back(index);
+			}
+		}
+	}
+}
+
+void rangeSplitFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range,
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices)
+{
+	rangeSplitFilteringImpl<pcl::PointXYZ>(cloud, indices, range, closeIndices, farIndices);
+}
+void rangeSplitFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range,
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices)
+{
+	rangeSplitFilteringImpl<pcl::PointXYZRGB>(cloud, indices, range, closeIndices, farIndices);
+}
+void rangeSplitFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range,
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices)
+{
+	rangeSplitFilteringImpl<pcl::PointNormal>(cloud, indices, range, closeIndices, farIndices);
+}
+void rangeSplitFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range,
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices)
+{
+	rangeSplitFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, range, closeIndices, farIndices);
+}
+
 LaserScan downsample(
 		const LaserScan & scan,
 		int step)
@@ -2007,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;
@@ -2078,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;
@@ -2096,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;
@@ -359,7 +359,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 						cv::Point2i pt((ptf[0]-xMin)/cellSize, (ptf[1]-yMin)/cellSize);
 						UASSERT_MSG(pt.y>0 && pt.y<map.rows && pt.x>0 && pt.x<map.cols,
 								uFormat("id=%d, map min=(%f, %f) max=(%f,%f) map=%dx%d pt=(%d,%d)", kter->first, xMin, yMin, xMax, yMax, map.cols, map.rows, pt.x, pt.y).c_str());
-						char & value = map.at<char>(pt.y, pt.x);
+						signed char & value = map.at<signed char>(pt.y, pt.x);
 						if(value != -2)
 						{
 							value = 0; // free space
@@ -385,7 +385,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 					{
 						for(int j=ptBegin.y; j<ptEnd.y; ++j)
 						{
-							map.at<char>(j, i) = -2; // free space (footprint)
+							map.at<signed char>(j, i) = -2; // free space (footprint)
 						}
 					}
 				}
@@ -398,7 +398,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 						cv::Point2i pt((ptf[0]-xMin)/cellSize, (ptf[1]-yMin)/cellSize);
 						UASSERT_MSG(pt.y>0 && pt.y<map.rows && pt.x>0 && pt.x<map.cols,
 								uFormat("id=%d: map min=(%f, %f) max=(%f,%f) map=%dx%d pt=(%d,%d)", kter->first, xMin, yMin, xMax, yMax, map.cols, map.rows, pt.x, pt.y).c_str());
-						char & value = map.at<char>(pt.y, pt.x);
+						signed char & value = map.at<signed char>(pt.y, pt.x);
 						if(value != -2)
 						{
 							value = 100; // obstacles
@@ -416,44 +416,44 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 			{
 				for(int j=0; j<map.cols; ++j)
 				{
-					if(map.at<char>(i, j) == -2)
+					if(map.at<signed char>(i, j) == -2)
 					{
-						updatedMap.at<char>(i, j) = 0;
+						updatedMap.at<signed char>(i, j) = 0;
 					}

 					if(i >=2 && i<map.rows-2 && j>=2 && j<map.cols-2)
 					{
-						if(map.at<char>(i, j) == -1 &&
-							map.at<char>(i+1, j) != -1 &&
-							map.at<char>(i-1, j) != -1 &&
-							map.at<char>(i, j+1) != -1 &&
-							map.at<char>(i, j-1) != -1)
+						if(map.at<signed char>(i, j) == -1 &&
+							map.at<signed char>(i+1, j) != -1 &&
+							map.at<signed char>(i-1, j) != -1 &&
+							map.at<signed char>(i, j+1) != -1 &&
+							map.at<signed char>(i, j-1) != -1)
 						{
-							updatedMap.at<char>(i, j) = 0;
+							updatedMap.at<signed char>(i, j) = 0;
 						}
-						else if(map.at<char>(i, j) == 100)
+						else if(map.at<signed char>(i, j) == 100)
 						{
 							// obstacle/empty/unknown -> remove empty
 							// unknown/empty/obstacle -> remove empty
-							if((map.at<char>(i-1, j) == 0 || map.at<char>(i-1, j) == -2) &&
-								map.at<char>(i-2, j) == -1)
+							if((map.at<signed char>(i-1, j) == 0 || map.at<signed char>(i-1, j) == -2) &&
+								map.at<signed char>(i-2, j) == -1)
 							{
-								updatedMap.at<char>(i-1, j) = -1;
+								updatedMap.at<signed char>(i-1, j) = -1;
 							}
-							else if((map.at<char>(i+1, j) == 0 || map.at<char>(i+1, j) == -2) &&
-									map.at<char>(i+2, j) == -1)
+							else if((map.at<signed char>(i+1, j) == 0 || map.at<signed char>(i+1, j) == -2) &&
+									map.at<signed char>(i+2, j) == -1)
 							{
-								updatedMap.at<char>(i+1, j) = -1;
+								updatedMap.at<signed char>(i+1, j) = -1;
 							}
-							if((map.at<char>(i, j-1) == 0 || map.at<char>(i, j-1) == -2) &&
-								map.at<char>(i, j-2) == -1)
+							if((map.at<signed char>(i, j-1) == 0 || map.at<signed char>(i, j-1) == -2) &&
+								map.at<signed char>(i, j-2) == -1)
 							{
-								updatedMap.at<char>(i, j-1) = -1;
+								updatedMap.at<signed char>(i, j-1) = -1;
 							}
-							else if((map.at<char>(i, j+1) == 0 || map.at<char>(i, j+1) == -2) &&
-									map.at<char>(i, j+2) == -1)
+							else if((map.at<signed char>(i, j+1) == 0 || map.at<signed char>(i, j+1) == -2) &&
+									map.at<signed char>(i, j+2) == -1)
 							{
-								updatedMap.at<char>(i, j+1) = -1;
+								updatedMap.at<signed char>(i, j+1) = -1;
 							}

 							if(erode)
@@ -461,18 +461,18 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 								obstacleIndices.push_back(std::make_pair(i, j));
 							}
 						}
-						else if(map.at<char>(i, j) == 0)
+						else if(map.at<signed char>(i, j) == 0)
 						{
 							// obstacle/empty/obstacle -> remove empty
-							if(map.at<char>(i-1, j) == 100 &&
-								map.at<char>(i+1, j) == 100)
+							if(map.at<signed char>(i-1, j) == 100 &&
+								map.at<signed char>(i+1, j) == 100)
 							{
-								updatedMap.at<char>(i, j) = -1;
+								updatedMap.at<signed char>(i, j) = -1;
 							}
-							else if(map.at<char>(i, j-1) == 100 &&
-								map.at<char>(i, j+1) == 100)
+							else if(map.at<signed char>(i, j-1) == 100 &&
+								map.at<signed char>(i, j+1) == 100)
 							{
-								updatedMap.at<char>(i, j) = -1;
+								updatedMap.at<signed char>(i, j) = -1;
 							}
 						}
 					}
@@ -490,16 +490,16 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 				{
 					int i = iter->first;
 					int j = iter->second;
-					int touchEmpty = (map.at<char>(i+1, j) == 0?1:0) +
-						(map.at<char>(i-1, j) == 0?1:0) +
-						(map.at<char>(i, j+1) == 0?1:0) +
-						(map.at<char>(i, j-1) == 0?1:0);
-					if(touchEmpty>=3 && map.at<char>(i+1, j) != -1 &&
-						map.at<char>(i-1, j) != -1 &&
-						map.at<char>(i, j+1) != -1 &&
-						map.at<char>(i, j-1) != -1)
+					int touchEmpty = (map.at<signed char>(i+1, j) == 0?1:0) +
+						(map.at<signed char>(i-1, j) == 0?1:0) +
+						(map.at<signed char>(i, j+1) == 0?1:0) +
+						(map.at<signed char>(i, j-1) == 0?1:0);
+					if(touchEmpty>=3 && map.at<signed char>(i+1, j) != -1 &&
+						map.at<signed char>(i-1, j) != -1 &&
+						map.at<signed char>(i, j+1) != -1 &&
+						map.at<signed char>(i, j-1) != -1)
 					{
-						erodedMap.at<char>(i, j) = 0; // empty
+						erodedMap.at<signed char>(i, j) = 0; // empty
 					}
 				}
 				map = erodedMap;
@@ -677,7 +677,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 					cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
 					if(end!=start)
 					{
-						map.at<char>(end.y, end.x) = 100; // obstacle
+						map.at<signed char>(end.y, end.x) = 100; // obstacle
 					}
 				}
 			}
@@ -703,7 +703,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 				cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
 				if(end!=start)
 				{
-					if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
+					if(localScans.size() > 1 || map.at<signed char>(end.y, end.x) != 0)
 					{
 						rayTrace(start, end, map, true); // trace free space
 					}
@@ -730,12 +730,12 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 				cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
 				if(end!=start)
 				{
-					if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
+					if(localScans.size() > 1 || map.at<signed char>(end.y, end.x) != 0)
 					{
 						rayTrace(start, end, map, true); // trace free space
-						if(map.at<char>(end.y, end.x) == -1)
+						if(map.at<signed char>(end.y, end.x) == -1)
 						{
-							map.at<char>(end.y, end.x) = 0; // empty
+							map.at<signed char>(end.y, end.x) = 0; // empty
 						}
 					}
 				}
@@ -879,14 +879,14 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid

 		for(int y = lowerbound; y<=(int)upperbound; ++y)
 		{
-			char * v;
+			signed char * v;
 			if(swapped)
 			{
-				v = &grid.at<char>(x, y);
+				v = &grid.at<signed char>(x, y);
 			}
 			else
 			{
-				v = &grid.at<char>(y, x);
+				v = &grid.at<signed char>(y, x);
 			}
 			if(*v == 100 && stopOnObstacle)
 			{
@@ -909,7 +909,7 @@ cv::Mat convertMap2Image8U(const cv::Mat & map8S, bool pgmFormat)
 	{
 		for (int j = 0; j < map8S.cols; ++j)
 		{
-			char v = pgmFormat?map8S.at<char>((map8S.rows-1)-i, j):map8S.at<char>(i, j);
+			signed char v = pgmFormat?map8S.at<signed char>((map8S.rows-1)-i, j):map8S.at<signed char>(i, j);
 			unsigned char gray;
 			if(v == 0)
 			{
@@ -950,7 +950,7 @@ cv::Mat convertImage8U2Map(const cv::Mat & map8U, bool pgmFormat)
 	{
 		for (int j = 0; j < map8U.cols; ++j)
 		{
-			unsigned char v = pgmFormat?map8U.at<char>((map8U.rows-1)-i, j):map8U.at<char>(i, j);
+			unsigned char v = pgmFormat?map8U.at<signed char>((map8U.rows-1)-i, j):map8U.at<signed char>(i, j);
 			char occupancy;
 			if(pgmFormat)
 			{
@@ -987,7 +987,7 @@ cv::Mat convertImage8U2Map(const cv::Mat & map8U, bool pgmFormat)
 				}
 			}

-			map8S.at<char>(i, j) = occupancy;
+			map8S.at<signed char>(i, j) = occupancy;
 		}
 	}
 	return map8S;
@@ -1001,20 +1001,20 @@ cv::Mat erodeMap(const cv::Mat & map)
 	{
 		for(int j=0; j<map.cols; ++j)
 		{
-			if(map.at<char>(i, j) == 100)
+			if(map.at<signed char>(i, j) == 100)
 			{
 				// remove obstacles which touch at least 3 empty cells but not unknown cells
-				int touchEmpty = (map.at<char>(i+1, j) == 0?1:0) +
-					(map.at<char>(i-1, j) == 0?1:0) +
-					(map.at<char>(i, j+1) == 0?1:0) +
-					(map.at<char>(i, j-1) == 0?1:0);
+				int touchEmpty = (map.at<signed char>(i+1, j) == 0?1:0) +
+					(map.at<signed char>(i-1, j) == 0?1:0) +
+					(map.at<signed char>(i, j+1) == 0?1:0) +
+					(map.at<signed char>(i, j-1) == 0?1:0);

-				if(touchEmpty>=3 && map.at<char>(i+1, j) != -1 &&
-					map.at<char>(i-1, j) != -1 &&
-					map.at<char>(i, j+1) != -1 &&
-					map.at<char>(i, j-1) != -1)
+				if(touchEmpty>=3 && map.at<signed char>(i+1, j) != -1 &&
+					map.at<signed char>(i-1, j) != -1 &&
+					map.at<signed char>(i, j+1) != -1 &&
+					map.at<signed char>(i, j-1) != -1)
 				{
-					erodedMap.at<char>(i, j) = 0; // empty
+					erodedMap.at<signed char>(i, j) = 0; // empty
 				}
 			}
 		}
@@ -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(
@@ -1,19 +1,19 @@

-FROM osrf/ros:humble-desktop
+# humble or jazzy
+ARG ROS_DISTRO=humble
+
+FROM osrf/ros:${ROS_DISTRO}-desktop

 # Install build dependencies
 RUN apt-get update && \
-    apt-get install -y git software-properties-common ros-humble-rtabmap-ros libqt6* qt6* && \
-    apt-get remove -y ros-humble-rtabmap* ros-humble-libg2o libpcl* libqt5* qt5* libvtk* libopencv* && \
+    apt-get install -y git software-properties-common ros-${ROS_DISTRO}-rtabmap-ros libqt6* qt6* qml6* && \
+    apt-get remove -y ros-${ROS_DISTRO}-rtabmap* ros-${ROS_DISTRO}-gtsam ros-${ROS_DISTRO}-libg2o libpcl* libqt5* qt5* libvtk* libopencv* && \
    apt-get clean && rm -rf /var/lib/apt/lists/

 WORKDIR /root/

 RUN rm /bin/sh && ln -s /bin/bash /bin/sh

-# ros2 seems not sourcing by default its multi-arch folders
-ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/humble/lib/x86_64-linux-gnu
-
 # Build latest VTK with Qt6
 RUN git clone https://github.com/Kitware/VTK.git && \
    cd VTK && \
@@ -26,11 +26,12 @@ RUN git clone https://github.com/Kitware/VTK.git && \
    rm -rf VTK
    
 # Build latest PCL with latest VTK
+# Make sure all libraries depending on Eigen are built with same CXX standard (17)
 RUN git clone https://github.com/PointCloudLibrary/pcl.git && \
    cd pcl && \
    mkdir build && \
    cd build && \
-    cmake -DBUILD_tools=OFF .. && \
+    cmake -DCMAKE_CXX_STANDARD=17 -DBUILD_tools=OFF -DPCL_ENABLE_AVX=OFF -DPCL_ENABLE_MARCHNATIVE=OFF -DPCL_ENABLE_SSE=OFF .. && \
    make -j$(nproc) && \
    make install && \
    cd ../.. && \
@@ -46,14 +47,14 @@ RUN git clone https://github.com/opencv/opencv.git && \
    make -j$(nproc) && \
    make install && \
    cd ../.. && \
-    rm -rf opencv
+    rm -rf opencv opencv_contrib

 # Build latest gtsam
 RUN git clone https://github.com/borglab/gtsam.git && \
 cd gtsam && \
 mkdir build && \
 cd build && \
- cmake -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_BUILD_UNSTABLE=OFF -DGTSAM_INSTALL_CPPUNILITE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON .. && \
+ cmake -DCMAKE_CXX_STANDARD=17 -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_BUILD_UNSTABLE=OFF -DGTSAM_INSTALL_CPPUNILITE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON .. && \
 cmake --build . --config Release --target install && \
 cd ../.. && \
 rm -rf gtsam
@@ -63,7 +64,7 @@ RUN git clone https://github.com/RainerKuemmerle/g2o.git  && \
 cd g2o && \
 mkdir build && \
 cd build && \
- cmake -DBUILD_WITH_MARCH_NATIVE=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DG2O_USE_OPENGL=OFF .. && \
+ cmake -DCMAKE_CXX_STANDARD=17 -DBUILD_WITH_MARCH_NATIVE=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DG2O_USE_OPENGL=OFF .. && \
 cmake --build . --config Release --target install && \
 cd ../.. && \
 rm -rf g2o
@@ -353,8 +353,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->horizontalSlider_B, SIGNAL(sliderMoved(int)), this, SLOT(sliderBMoved(int)));
 	ui_->spinBox_indexA->setEnabled(false);
 	ui_->spinBox_indexB->setEnabled(false);
-	connect(ui_->spinBox_indexA, SIGNAL(valueChanged(int)), this, SLOT(sliderAValueChanged(int)));
-	connect(ui_->spinBox_indexB, SIGNAL(valueChanged(int)), this, SLOT(sliderBValueChanged(int)));
+	connect(ui_->spinBox_indexA, SIGNAL(valueChanged(int)), ui_->horizontalSlider_A, SLOT(setValue(int)));
+	connect(ui_->spinBox_indexB, SIGNAL(valueChanged(int)), ui_->horizontalSlider_B, SLOT(setValue(int)));

 	connect(ui_->toolButton_edit_priorA, SIGNAL(clicked(bool)), this, SLOT(editConstraint()));
 	connect(ui_->toolButton_edit_priorB, SIGNAL(clicked(bool)), this, SLOT(editConstraint()));
@@ -443,6 +443,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified()));
 	connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
 	connect(ui_->actionVertical_Layout, SIGNAL(toggled(bool)), this, SLOT(configModified()));
+	connect(ui_->actionConcise_Layout, SIGNAL(toggled(bool)), this, SLOT(configModified()));
 	connect(ui_->checkBox_alignPosesWithGPS, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_alignPosesWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_alignScansCloudsWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
@@ -543,6 +544,27 @@ void DatabaseViewer::showCloseButton(bool visible)

 void DatabaseViewer::configModified()
 {
+	if(ui_->actionConcise_Layout->isChecked())
+	{
+		ui_->graphicsView_B->setVisible(false);
+		ui_->scrollArea->setVisible(false);
+		ui_->scrollArea_2->setVisible(false);
+		ui_->spinBox_indexB->setVisible(false);
+		ui_->widget_imageControls_B->setVisible(false);
+		ui_->widget_graphControl->setVisible(false);
+		ui_->graphicsView_A->clearLines();
+		ui_->graphicsView_B->clearLines();
+	}
+	else
+	{
+		ui_->graphicsView_B->setVisible(true);
+		ui_->scrollArea->setVisible(true);
+		ui_->scrollArea_2->setVisible(true);
+		ui_->spinBox_indexB->setVisible(true);	
+		ui_->widget_imageControls_B->setVisible(true);
+		ui_->widget_graphControl->setVisible(true);
+	}
+	
 	this->setWindowModified(true);
 }

@@ -582,6 +604,7 @@ void DatabaseViewer::readSettings()

 	ui_->comboBox_logger_level->setCurrentIndex(settings.value("loggerLevel", ui_->comboBox_logger_level->currentIndex()).toInt());
 	ui_->actionVertical_Layout->setChecked(settings.value("verticalLayout", ui_->actionVertical_Layout->isChecked()).toBool());
+	ui_->actionConcise_Layout->setChecked(settings.value("conciseLayout", ui_->actionConcise_Layout->isChecked()).toBool());
 	ui_->checkBox_ignoreIntermediateNodes->setChecked(settings.value("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked()).toBool());
 	ui_->checkBox_timeStats->setChecked(settings.value("timeStats", ui_->checkBox_timeStats->isChecked()).toBool());

@@ -672,6 +695,7 @@ void DatabaseViewer::writeSettings()

 	settings.setValue("loggerLevel", ui_->comboBox_logger_level->currentIndex());
 	settings.setValue("verticalLayout", ui_->actionVertical_Layout->isChecked());
+	settings.setValue("conciseLayout", ui_->actionConcise_Layout->isChecked());
 	settings.setValue("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked());
 	settings.setValue("timeStats", ui_->checkBox_timeStats->isChecked());

@@ -2399,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();
 			}
 		}
@@ -4440,15 +4465,18 @@ void DatabaseViewer::refineLinks()
 	std::multimap<int, Link> allLinks = updateLinksWithModifications(links_);
 	for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
 	{
-		int minId = iter->second.from()>iter->second.to()?iter->second.to():iter->second.from();
-		int maxId = iter->second.from()<iter->second.to()?iter->second.to():iter->second.from();
-		if(minNodeId == 0 || minNodeId > minId)
+		if(iter->second.type() < Link::kPosePrior)
 		{
-			minNodeId = minId;
-		}
-		if(maxNodeId == 0 || maxNodeId < maxId)
-		{
-			maxNodeId = maxId;
+			int minId = iter->second.from()>iter->second.to()?iter->second.to():iter->second.from();
+			int maxId = iter->second.from()<iter->second.to()?iter->second.to():iter->second.from();
+			if(minNodeId == 0 || minNodeId > minId)
+			{
+				minNodeId = minId;
+			}
+			if(maxNodeId == 0 || maxNodeId < maxId)
+			{
+				maxNodeId = maxId;
+			}
 		}
 	}
 	if(minNodeId > 0)
@@ -4472,7 +4500,8 @@ void DatabaseViewer::refineLinks()
 			linkRefiningDialog_->getIntraInterSessions(intra, inter);
 			for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
 			{
-				if(type==Link::kEnd || type == iter->second.type())
+				if(iter->second.type() < Link::kPosePrior && 
+				   (type==Link::kEnd || type == iter->second.type()))
 				{
 					int from = iter->second.from();
 					int to = iter->second.to();
@@ -4502,6 +4531,10 @@ void DatabaseViewer::refineLinks()
 			}
 		}
 	}
+	else
+	{
+		UWARN("No links can be refined!");
+	}
 }
 void DatabaseViewer::refineLinks(const QList<Link> & links)
 {
@@ -4565,11 +4598,11 @@ void DatabaseViewer::graphNodeSelected(int id)
 	if(id>0 && idToIndex_.contains(id))
 	{
 		static bool updateA = true;
-		if(updateA)
+		if(updateA || ui_->actionConcise_Layout->isChecked())
 			ui_->horizontalSlider_A->setValue(idToIndex_.value(id));
 		else
 			ui_->horizontalSlider_B->setValue(idToIndex_.value(id));
-		updateA = !updateA;
+		updateA = !updateA || ui_->actionConcise_Layout->isChecked();
 	}
 }

@@ -4838,9 +4871,7 @@ void DatabaseViewer::update(int value,
 				dbDriver_->loadLinks(id, gravityLink, Link::kGravity);
 				if(!gravityLink.empty())
 				{
-					float roll,pitch,yaw;
-					gravityLink.begin()->second.transform().getEulerAngles(roll, pitch, yaw);
-					Eigen::Vector3d v = Transform(0,0,0,roll,pitch,0).toEigen3d() * -Eigen::Vector3d::UnitZ();
+					Eigen::Vector3f v = gravityLink.begin()->second.transform().inverse().toEigen3f() * -Eigen::Vector3f::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));
 				}
@@ -5066,13 +5097,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);
-						if(pose.isIdentity())
-						{
-							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);
+						Eigen::Vector3f gravity =  gravityT.inverse().toEigen3f()*-Eigen::Vector3f::UnitZ();
+						cloudViewer_->addOrUpdateLine("gravity", pose, pose*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::yellow, true, false);
 					}

 					//add scan
@@ -5907,6 +5933,10 @@ void DatabaseViewer::updateStereo(const SensorData * data)

 void DatabaseViewer::updateWordsMatching(const std::vector<int> & inliers)
 {
+	if(ui_->actionConcise_Layout->isChecked()) {
+		return;
+	}
+
 	int from = ids_.at(ui_->horizontalSlider_A->value());
 	int to = ids_.at(ui_->horizontalSlider_B->value());
 	if(from && to)
@@ -7157,14 +7187,14 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 						for(int y=0; y<map.rows; ++y)
 						{
 							// check for first
-							if(!firstSet && map.at<char>(y, x) != -1)
+							if(!firstSet && map.at<signed char>(y, x) != -1)
 							{
 								xFirst = x;
 								firstSet = true;
 							}
 							// check for last
 							int opp = map.cols-(x+1);
-							if(!lastSet && map.at<char>(y, opp) != -1)
+							if(!lastSet && map.at<signed char>(y, opp) != -1)
 							{
 								xLast = opp;
 								lastSet = true;
@@ -7178,14 +7208,14 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 						for(int x=0; x<map.cols; ++x)
 						{
 							// check for first
-							if(!firstSet && map.at<char>(y, x) != -1)
+							if(!firstSet && map.at<signed char>(y, x) != -1)
 							{
 								yFirst = y;
 								firstSet = true;
 							}
 							// check for last
 							int opp = map.rows-(y+1);
-							if(!lastSet && map.at<char>(map.rows-(y+1), x) != -1)
+							if(!lastSet && map.at<signed char>(map.rows-(y+1), x) != -1)
 							{
 								yLast = opp;
 								lastSet = true;
@@ -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);

@@ -302,7 +302,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	_ui->posteriorPlot->showLegend(false);
 	_ui->posteriorPlot->setFixedYAxis(0,1);
 	UPlotCurveThreshold * tc;
-	tc = _ui->posteriorPlot->addThreshold("Loop closure thr", float(_preferencesDialog->getLoopThr()));
+	tc = _ui->posteriorPlot->addThreshold("1 - Loop Thr", 1.0 - _preferencesDialog->getLoopThr());
 	connect(this, SIGNAL(loopClosureThrChanged(qreal)), tc, SLOT(setThreshold(qreal)));

 	_likelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
@@ -2412,7 +2412,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)

 			ULOGGER_DEBUG("");
 			//Adjust thresholds
-			Q_EMIT(loopClosureThrChanged(_preferencesDialog->getLoopThr()));
+			Q_EMIT(loopClosureThrChanged(1.0 - _preferencesDialog->getLoopThr()));
 		}
 		if(!stat.likelihood().empty() && _ui->dockWidget_likelihood->isVisible())
 		{
@@ -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;
-					_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);
+					gravity = gravityT.inverse().toEigen3f()*gravity;
+					_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())
@@ -4976,7 +4976,7 @@ void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters, bo
 	_ui->doubleSpinBox_stats_timeLimit->setValue(_preferencesDialog->getTimeLimit());
 	_ui->actionSLAM_mode->setChecked(_preferencesDialog->isSLAMMode());

-	Q_EMIT(loopClosureThrChanged(_preferencesDialog->getLoopThr()));
+	Q_EMIT(loopClosureThrChanged(1.0 - _preferencesDialog->getLoopThr()));
 }

 void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords)
@@ -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);
 	}
@@ -724,6 +724,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	// usb group
 	connect(_ui->spinBox_usbcam_streamWidth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_usbcam_streamHeight, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_usbcam_fourcc, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	//video group
 	connect(_ui->source_video_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceVideoPath()));
 	connect(_ui->source_video_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -836,6 +837,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->spinBox_stereo_right_device, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_stereousbcam_streamWidth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_stereousbcam_streamHeight, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_stereousbcam_fourcc, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));

 	connect(_ui->comboBox_stereoZed_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_stereoZed_quality, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -993,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());
@@ -1055,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());
@@ -1287,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());
@@ -2732,6 +2737,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->spinBox_stereo_right_device->setValue(settings.value("device2", _ui->spinBox_stereo_right_device->value()).toInt());
 	_ui->spinBox_stereousbcam_streamWidth->setValue(settings.value("width", _ui->spinBox_stereousbcam_streamWidth->value()).toInt());
 	_ui->spinBox_stereousbcam_streamHeight->setValue(settings.value("height", _ui->spinBox_stereousbcam_streamHeight->value()).toInt());
+	_ui->lineEdit_stereousbcam_fourcc->setText(settings.value("fourcc", _ui->lineEdit_stereousbcam_fourcc->text()).toString());
 	settings.endGroup(); // StereoVideo

 	settings.beginGroup("StereoZed");
@@ -2809,6 +2815,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	settings.beginGroup("UsbCam");
 	_ui->spinBox_usbcam_streamWidth->setValue(settings.value("width", _ui->spinBox_usbcam_streamWidth->value()).toInt());
 	_ui->spinBox_usbcam_streamHeight->setValue(settings.value("height", _ui->spinBox_usbcam_streamHeight->value()).toInt());
+	_ui->lineEdit_usbcam_fourcc->setText(settings.value("fourcc", _ui->lineEdit_usbcam_fourcc->text()).toString());
 	settings.endGroup(); // UsbCam

 	settings.beginGroup("Video");
@@ -3332,6 +3339,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("device2",		_ui->spinBox_stereo_right_device->value());
 	settings.setValue("width",		    _ui->spinBox_stereousbcam_streamWidth->value());
 	settings.setValue("height",		    _ui->spinBox_stereousbcam_streamHeight->value());
+	settings.setValue("fourcc",         _ui->lineEdit_stereousbcam_fourcc->text());
 	settings.endGroup(); // StereoVideo

 	settings.beginGroup("StereoZed");
@@ -3407,6 +3415,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.beginGroup("UsbCam");
 	settings.setValue("width",          _ui->spinBox_usbcam_streamWidth->value());
 	settings.setValue("height",         _ui->spinBox_usbcam_streamHeight->value());
+	settings.setValue("fourcc",         _ui->lineEdit_usbcam_fourcc->text());
 	settings.endGroup(); // UsbCam

 	settings.beginGroup("Video");
@@ -4863,22 +4872,43 @@ 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 "
-							  "with g2o. Keeping default combo value: %s.",
-							  combo->objectName().toStdString().c_str(),
-							  combo->currentText().toStdString().c_str());
-						ok = false;
+						if(Optimizer::isAvailable(Optimizer::kTypeGTSAM)) {
+							UWARN("Trying to set \"%s\" to g2o but RTAB-Map isn't built "
+								"with g2o. Falling back to GTSAM.",
+								combo->objectName().toStdString().c_str());
+							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 "
-							  "with GTSAM. Keeping default combo value: %s.",
-							  combo->objectName().toStdString().c_str(),
-							  combo->currentText().toStdString().c_str());
-						ok = false;
+#ifndef RTABMAP_ORB_SLAM
+						if(Optimizer::isAvailable(Optimizer::kTypeG2O))
+#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)
@@ -6738,6 +6768,10 @@ Camera * PreferencesDialog::createCamera(
 				this->getSourceLocalTransform());
 		}
 		((CameraStereoVideo*)camera)->setResolution(_ui->spinBox_stereousbcam_streamWidth->value(), _ui->spinBox_stereousbcam_streamHeight->value());
+		if(!_ui->lineEdit_stereousbcam_fourcc->text().isEmpty())
+		{
+			((CameraStereoVideo*)camera)->setFOURCC(_ui->lineEdit_stereousbcam_fourcc->text().toStdString());
+		}
 	}
 	else if(driver == kSrcStereoVideo)
 	{
@@ -6866,6 +6900,10 @@ Camera * PreferencesDialog::createCamera(
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
 		((CameraVideo*)camera)->setResolution(_ui->spinBox_usbcam_streamWidth->value(), _ui->spinBox_usbcam_streamHeight->value());
+		if(!_ui->lineEdit_usbcam_fourcc->text().isEmpty())
+		{
+			((CameraVideo*)camera)->setFOURCC(_ui->lineEdit_usbcam_fourcc->text().toStdString());
+		}
 	}
 	else if(driver == kSrcVideo)
 	{
@@ -776,69 +776,71 @@
       </layout>
      </item>
      <item row="1" column="1">
-       <layout class="QHBoxLayout" name="horizontalLayout_2">
-        <property name="leftMargin">
-         <number>12</number>
-        </property>
-        <property name="topMargin">
-         <number>12</number>
-        </property>
-        <property name="rightMargin">
-         <number>12</number>
-        </property>
-        <property name="bottomMargin">
-         <number>12</number>
-        </property>
-        <item>
-         <layout class="QVBoxLayout" name="verticalLayout">
-          <item>
-           <widget class="QLabel" name="label_5">
-            <property name="text">
-             <string>Index :</string>
-            </property>
-           </widget>
-          </item>
-          <item>
-           <widget class="QLabel" name="label_4">
-            <property name="text">
-             <string>Id :</string>
-            </property>
-           </widget>
-          </item>
-         </layout>
-        </item>
-        <item>
-         <layout class="QVBoxLayout" name="verticalLayout_2">
-          <item>
-           <widget class="QSpinBox" name="spinBox_indexB">
-            <property name="frame">
-             <bool>false</bool>
-            </property>
-           </widget>
-          </item>
-          <item>
-           <widget class="QLabel" name="label_idB">
-            <property name="text">
-             <string>idB</string>
-            </property>
-           </widget>
-          </item>
-         </layout>
-        </item>
-        <item>
-         <widget class="QSlider" name="horizontalSlider_B">
-          <property name="focusPolicy">
-           <enum>Qt::ClickFocus</enum>
+       <widget class="QWidget" name="widget_imageControls_B">
+        <layout class="QHBoxLayout" name="horizontalLayout_2">
+          <property name="leftMargin">
+          <number>12</number>
          </property>
-          <property name="orientation">
-           <enum>Qt::Horizontal</enum>
+          <property name="topMargin">
+          <number>12</number>
          </property>
-          <property name="tickPosition">
-           <enum>QSlider::TicksAbove</enum>
+          <property name="rightMargin">
+          <number>12</number>
          </property>
-         </widget>
-        </item>
-       </layout>
+          <property name="bottomMargin">
+          <number>12</number>
+          </property>
+          <item>
+          <layout class="QVBoxLayout" name="verticalLayout">
+            <item>
+            <widget class="QLabel" name="label_5">
+              <property name="text">
+              <string>Index :</string>
+              </property>
+            </widget>
+            </item>
+            <item>
+            <widget class="QLabel" name="label_4">
+              <property name="text">
+              <string>Id :</string>
+              </property>
+            </widget>
+            </item>
+          </layout>
+          </item>
+          <item>
+          <layout class="QVBoxLayout" name="verticalLayout_2">
+            <item>
+            <widget class="QSpinBox" name="spinBox_indexB">
+              <property name="frame">
+              <bool>false</bool>
+              </property>
+            </widget>
+            </item>
+            <item>
+            <widget class="QLabel" name="label_idB">
+              <property name="text">
+              <string>idB</string>
+              </property>
+            </widget>
+            </item>
+          </layout>
+          </item>
+          <item>
+          <widget class="QSlider" name="horizontalSlider_B">
+            <property name="focusPolicy">
+            <enum>Qt::ClickFocus</enum>
+            </property>
+            <property name="orientation">
+            <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="tickPosition">
+            <enum>QSlider::TicksAbove</enum>
+            </property>
+          </widget>
+          </item>
+        </layout>
+       </widget>       
      </item>
     </layout>
    </item>
@@ -936,6 +938,7 @@
     <string>View</string>
    </property>
    <addaction name="actionVertical_Layout"/>
+    <addaction name="actionConcise_Layout"/>
   </widget>
   <addaction name="menuFile"/>
   <addaction name="menuEdit"/>
@@ -1243,322 +1246,328 @@
   <attribute name="dockWidgetArea">
    <number>1</number>
   </attribute>
-   <widget class="QWidget" name="dockWidgetContents_2">
+   <widget class="QWidget" name="dockWidget_graphContents">
    <layout class="QVBoxLayout" name="verticalLayout_9">
     <item>
      <widget class="rtabmap::GraphViewer" name="graphViewer"/>
     </item>
     <item>
-      <layout class="QHBoxLayout" name="horizontalLayout_15">
-       <item>
-        <widget class="QLabel" name="label_rotation">
-         <property name="text">
-          <string>0.0 deg</string>
-         </property>
-        </widget>
-       </item>
-       <item>
-        <widget class="QSlider" name="horizontalSlider_rotation">
-         <property name="focusPolicy">
-          <enum>Qt::ClickFocus</enum>
-         </property>
-         <property name="minimum">
-          <number>-1799</number>
-         </property>
-         <property name="maximum">
-          <number>1800</number>
-         </property>
-         <property name="sliderPosition">
-          <number>0</number>
-         </property>
-         <property name="orientation">
-          <enum>Qt::Horizontal</enum>
-         </property>
-         <property name="tickPosition">
-          <enum>QSlider::TicksAbove</enum>
-         </property>
-         <property name="tickInterval">
-          <number>100</number>
-         </property>
-        </widget>
-       </item>
-       <item>
-        <widget class="QPushButton" name="pushButton_applyRotation">
-         <property name="toolTip">
-          <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;The rotation will be applied temporary to optimized global graph. To save it to database, do File-&amp;gt;&amp;quot;Regenerate optimized 2D map...&amp;quot;.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-         </property>
-         <property name="text">
-          <string>Apply Rotation</string>
-         </property>
-        </widget>
-       </item>
-      </layout>
-     </item>
-     <item>
-      <layout class="QHBoxLayout" name="horizontalLayout_4">
-       <item>
-        <widget class="QLabel" name="label_iterations">
-         <property name="text">
-          <string>#</string>
-         </property>
-        </widget>
-       </item>
-       <item>
-        <widget class="QSlider" name="horizontalSlider_iterations">
-         <property name="focusPolicy">
-          <enum>Qt::ClickFocus</enum>
-         </property>
-         <property name="orientation">
-          <enum>Qt::Horizontal</enum>
-         </property>
-         <property name="tickPosition">
-          <enum>QSlider::TicksAbove</enum>
-         </property>
-        </widget>
-       </item>
-       <item>
-        <widget class="QComboBox" name="comboBox_optimizationFlavor">
-         <property name="sizeAdjustPolicy">
-          <enum>QComboBox::AdjustToContents</enum>
-         </property>
-         <item>
-          <property name="text">
-           <string>Global Iterative</string>
-          </property>
-         </item>
-         <item>
-          <property name="text">
-           <string>Global Full</string>
-          </property>
-         </item>
-         <item>
-          <property name="text">
-           <string>Local Optimized</string>
-          </property>
-         </item>
-        </widget>
-       </item>
-      </layout>
-     </item>
-     <item>
-      <layout class="QGridLayout" name="gridLayout_5" columnstretch="0,1">
-       <item row="2" column="1">
-        <widget class="QLabel" name="label_alignPosesWithGroundTruth">
-         <property name="text">
-          <string>Align poses with ground truth</string>
-         </property>
-         <property name="wordWrap">
-          <bool>true</bool>
-         </property>
-        </widget>
-       </item>
-       <item row="4" column="0">
-        <widget class="QCheckBox" name="checkBox_ignoreIntermediateNodes">
-         <property name="text">
-          <string/>
-         </property>
-         <property name="checked">
-          <bool>false</bool>
-         </property>
-        </widget>
-       </item>
-       <item row="0" column="1">
-        <layout class="QHBoxLayout" name="horizontalLayout_7" stretch="0,0,1">
-         <item>
-          <widget class="QLabel" name="label_optimizeFrom">
-           <property name="text">
-            <string>Root</string>
-           </property>
-          </widget>
-         </item>
-         <item>
-          <widget class="QCheckBox" name="checkBox_spanAllMaps">
-           <property name="text">
-            <string>Span to all maps</string>
-           </property>
-           <property name="checked">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item>
-          <widget class="QCheckBox" name="checkBox_wmState">
-           <property name="text">
-            <string>WM</string>
-           </property>
-          </widget>
-         </item>
-        </layout>
-       </item>
-       <item row="7" column="1">
-        <widget class="QLabel" name="label_45">
-         <property name="text">
-          <string>Time grid (s)</string>
-         </property>
-        </widget>
-       </item>
-       <item row="8" column="1">
-        <widget class="QLabel" name="label_rmse_title">
-         <property name="text">
-          <string>RMSE (m)</string>
-         </property>
-        </widget>
-       </item>
-       <item row="3" column="0">
-        <widget class="QCheckBox" name="checkBox_alignScansCloudsWithGroundTruth">
-         <property name="text">
-          <string/>
-         </property>
-         <property name="checked">
-          <bool>true</bool>
-         </property>
-        </widget>
-       </item>
-       <item row="9" column="0">
-        <widget class="QLabel" name="label_poses">
-         <property name="text">
-          <string/>
-         </property>
-         <property name="textInteractionFlags">
-          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-         </property>
-        </widget>
-       </item>
-       <item row="6" column="0">
-        <widget class="QLabel" name="label_timeOptimization">
-         <property name="text">
-          <string/>
-         </property>
-         <property name="textInteractionFlags">
-          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-         </property>
-        </widget>
-       </item>
-       <item row="3" column="1">
-        <widget class="QLabel" name="label_alignScansCloudsWithGroundTruth">
-         <property name="text">
-          <string>Align scans/clouds with ground truth</string>
-         </property>
-         <property name="wordWrap">
-          <bool>true</bool>
-         </property>
-        </widget>
-       </item>
-       <item row="8" column="0">
-        <widget class="QLabel" name="label_rmse">
-         <property name="text">
-          <string/>
-         </property>
-         <property name="textInteractionFlags">
-          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-         </property>
-        </widget>
-       </item>
-       <item row="7" column="0">
-        <widget class="QLabel" name="label_timeGrid">
-         <property name="text">
-          <string/>
-         </property>
-         <property name="textInteractionFlags">
-          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-         </property>
-        </widget>
-       </item>
-       <item row="10" column="1">
-        <widget class="QLabel" name="label_41">
-         <property name="toolTip">
-          <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;N: Neighbor&lt;/p&gt;&lt;p&gt;NM: Neighbor Merged&lt;/p&gt;&lt;p&gt;G: Global&lt;/p&gt;&lt;p&gt;LS: Local by Space (Proximity)&lt;/p&gt;&lt;p&gt;LT: Local by Time (Proximity)&lt;/p&gt;&lt;p&gt;U: User&lt;/p&gt;&lt;p&gt;P: Prior&lt;/p&gt;&lt;p&gt;LM: Landmark&lt;/p&gt;&lt;p&gt;GR: Gravity&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-         </property>
-         <property name="text">
-          <string>Links (N, NM, G, LS, LT, U, P, LM, GR)</string>
-         </property>
-        </widget>
-       </item>
-       <item row="5" column="1">
-        <widget class="QLabel" name="label_10">
-         <property name="text">
-          <string>Path length (m)</string>
-         </property>
-        </widget>
-       </item>
-       <item row="9" column="1">
-        <widget class="QLabel" name="label_52">
-         <property name="text">
-          <string>Poses</string>
-         </property>
-        </widget>
-       </item>
-       <item row="5" column="0">
-        <widget class="QLabel" name="label_pathLength">
-         <property name="text">
-          <string/>
-         </property>
-         <property name="textInteractionFlags">
-          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-         </property>
-        </widget>
-       </item>
-       <item row="0" column="0">
-        <widget class="QSpinBox" name="spinBox_optimizationsFrom"/>
-       </item>
-       <item row="10" column="0">
-        <widget class="QLabel" name="label_loopClosures">
-         <property name="text">
-          <string/>
-         </property>
-         <property name="textInteractionFlags">
-          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-         </property>
-        </widget>
-       </item>
-       <item row="4" column="1">
-        <widget class="QLabel" name="label_ignoreINtermediateNdoes">
-         <property name="text">
-          <string>Ignore intermediate nodes</string>
-         </property>
-         <property name="wordWrap">
-          <bool>true</bool>
-         </property>
-        </widget>
-       </item>
-       <item row="6" column="1">
-        <widget class="QLabel" name="label_39">
-         <property name="text">
-          <string>Time optimization (s)</string>
-         </property>
-        </widget>
-       </item>
-       <item row="2" column="0">
-        <widget class="QCheckBox" name="checkBox_alignPosesWithGroundTruth">
-         <property name="text">
-          <string/>
-         </property>
-         <property name="checked">
-          <bool>true</bool>
-         </property>
-        </widget>
-       </item>
-       <item row="1" column="1">
-        <widget class="QLabel" name="label_alignPosesWithGPS">
-         <property name="text">
-          <string>Align poses with GPS</string>
-         </property>
-         <property name="wordWrap">
-          <bool>true</bool>
-         </property>
-        </widget>
-       </item>
-       <item row="1" column="0">
-        <widget class="QCheckBox" name="checkBox_alignPosesWithGPS">
-         <property name="text">
-          <string/>
-         </property>
-         <property name="checked">
-          <bool>true</bool>
-         </property>
-        </widget>
-       </item>
-      </layout>
+      <widget class="QWidget" name="widget_graphControl">
+       <layout class="QVBoxLayout" name="verticalLayout_91">
+        <item>
+          <layout class="QHBoxLayout" name="horizontalLayout_15">
+          <item>
+            <widget class="QLabel" name="label_rotation">
+            <property name="text">
+              <string>0.0 deg</string>
+            </property>
+            </widget>
+          </item>
+          <item>
+            <widget class="QSlider" name="horizontalSlider_rotation">
+            <property name="focusPolicy">
+              <enum>Qt::ClickFocus</enum>
+            </property>
+            <property name="minimum">
+              <number>-1799</number>
+            </property>
+            <property name="maximum">
+              <number>1800</number>
+            </property>
+            <property name="sliderPosition">
+              <number>0</number>
+            </property>
+            <property name="orientation">
+              <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="tickPosition">
+              <enum>QSlider::TicksAbove</enum>
+            </property>
+            <property name="tickInterval">
+              <number>100</number>
+            </property>
+            </widget>
+          </item>
+          <item>
+            <widget class="QPushButton" name="pushButton_applyRotation">
+            <property name="toolTip">
+              <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;The rotation will be applied temporary to optimized global graph. To save it to database, do File-&amp;gt;&amp;quot;Regenerate optimized 2D map...&amp;quot;.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+            </property>
+            <property name="text">
+              <string>Apply Rotation</string>
+            </property>
+            </widget>
+          </item>
+          </layout>
+        </item>
+        <item>
+          <layout class="QHBoxLayout" name="horizontalLayout_4">
+          <item>
+            <widget class="QLabel" name="label_iterations">
+            <property name="text">
+              <string>#</string>
+            </property>
+            </widget>
+          </item>
+          <item>
+            <widget class="QSlider" name="horizontalSlider_iterations">
+            <property name="focusPolicy">
+              <enum>Qt::ClickFocus</enum>
+            </property>
+            <property name="orientation">
+              <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="tickPosition">
+              <enum>QSlider::TicksAbove</enum>
+            </property>
+            </widget>
+          </item>
+          <item>
+            <widget class="QComboBox" name="comboBox_optimizationFlavor">
+            <property name="sizeAdjustPolicy">
+              <enum>QComboBox::AdjustToContents</enum>
+            </property>
+            <item>
+              <property name="text">
+              <string>Global Iterative</string>
+              </property>
+            </item>
+            <item>
+              <property name="text">
+              <string>Global Full</string>
+              </property>
+            </item>
+            <item>
+              <property name="text">
+              <string>Local Optimized</string>
+              </property>
+            </item>
+            </widget>
+          </item>
+          </layout>
+        </item>
+        <item>
+          <layout class="QGridLayout" name="gridLayout_5" columnstretch="0,1">
+          <item row="2" column="1">
+            <widget class="QLabel" name="label_alignPosesWithGroundTruth">
+            <property name="text">
+              <string>Align poses with ground truth</string>
+            </property>
+            <property name="wordWrap">
+              <bool>true</bool>
+            </property>
+            </widget>
+          </item>
+          <item row="4" column="0">
+            <widget class="QCheckBox" name="checkBox_ignoreIntermediateNodes">
+            <property name="text">
+              <string/>
+            </property>
+            <property name="checked">
+              <bool>false</bool>
+            </property>
+            </widget>
+          </item>
+          <item row="0" column="1">
+            <layout class="QHBoxLayout" name="horizontalLayout_7" stretch="0,0,1">
+            <item>
+              <widget class="QLabel" name="label_optimizeFrom">
+              <property name="text">
+                <string>Root</string>
+              </property>
+              </widget>
+            </item>
+            <item>
+              <widget class="QCheckBox" name="checkBox_spanAllMaps">
+              <property name="text">
+                <string>Span to all maps</string>
+              </property>
+              <property name="checked">
+                <bool>true</bool>
+              </property>
+              </widget>
+            </item>
+            <item>
+              <widget class="QCheckBox" name="checkBox_wmState">
+              <property name="text">
+                <string>WM</string>
+              </property>
+              </widget>
+            </item>
+            </layout>
+          </item>
+          <item row="7" column="1">
+            <widget class="QLabel" name="label_45">
+            <property name="text">
+              <string>Time grid (s)</string>
+            </property>
+            </widget>
+          </item>
+          <item row="8" column="1">
+            <widget class="QLabel" name="label_rmse_title">
+            <property name="text">
+              <string>RMSE (m)</string>
+            </property>
+            </widget>
+          </item>
+          <item row="3" column="0">
+            <widget class="QCheckBox" name="checkBox_alignScansCloudsWithGroundTruth">
+            <property name="text">
+              <string/>
+            </property>
+            <property name="checked">
+              <bool>true</bool>
+            </property>
+            </widget>
+          </item>
+          <item row="9" column="0">
+            <widget class="QLabel" name="label_poses">
+            <property name="text">
+              <string/>
+            </property>
+            <property name="textInteractionFlags">
+              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+            </widget>
+          </item>
+          <item row="6" column="0">
+            <widget class="QLabel" name="label_timeOptimization">
+            <property name="text">
+              <string/>
+            </property>
+            <property name="textInteractionFlags">
+              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+            </widget>
+          </item>
+          <item row="3" column="1">
+            <widget class="QLabel" name="label_alignScansCloudsWithGroundTruth">
+            <property name="text">
+              <string>Align scans/clouds with ground truth</string>
+            </property>
+            <property name="wordWrap">
+              <bool>true</bool>
+            </property>
+            </widget>
+          </item>
+          <item row="8" column="0">
+            <widget class="QLabel" name="label_rmse">
+            <property name="text">
+              <string/>
+            </property>
+            <property name="textInteractionFlags">
+              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+            </widget>
+          </item>
+          <item row="7" column="0">
+            <widget class="QLabel" name="label_timeGrid">
+            <property name="text">
+              <string/>
+            </property>
+            <property name="textInteractionFlags">
+              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+            </widget>
+          </item>
+          <item row="10" column="1">
+            <widget class="QLabel" name="label_41">
+            <property name="toolTip">
+              <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;N: Neighbor&lt;/p&gt;&lt;p&gt;NM: Neighbor Merged&lt;/p&gt;&lt;p&gt;G: Global&lt;/p&gt;&lt;p&gt;LS: Local by Space (Proximity)&lt;/p&gt;&lt;p&gt;LT: Local by Time (Proximity)&lt;/p&gt;&lt;p&gt;U: User&lt;/p&gt;&lt;p&gt;P: Prior&lt;/p&gt;&lt;p&gt;LM: Landmark&lt;/p&gt;&lt;p&gt;GR: Gravity&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+            </property>
+            <property name="text">
+              <string>Links (N, NM, G, LS, LT, U, P, LM, GR)</string>
+            </property>
+            </widget>
+          </item>
+          <item row="5" column="1">
+            <widget class="QLabel" name="label_10">
+            <property name="text">
+              <string>Path length (m)</string>
+            </property>
+            </widget>
+          </item>
+          <item row="9" column="1">
+            <widget class="QLabel" name="label_52">
+            <property name="text">
+              <string>Poses</string>
+            </property>
+            </widget>
+          </item>
+          <item row="5" column="0">
+            <widget class="QLabel" name="label_pathLength">
+            <property name="text">
+              <string/>
+            </property>
+            <property name="textInteractionFlags">
+              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+            </widget>
+          </item>
+          <item row="0" column="0">
+            <widget class="QSpinBox" name="spinBox_optimizationsFrom"/>
+          </item>
+          <item row="10" column="0">
+            <widget class="QLabel" name="label_loopClosures">
+            <property name="text">
+              <string/>
+            </property>
+            <property name="textInteractionFlags">
+              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+            </widget>
+          </item>
+          <item row="4" column="1">
+            <widget class="QLabel" name="label_ignoreINtermediateNdoes">
+            <property name="text">
+              <string>Ignore intermediate nodes</string>
+            </property>
+            <property name="wordWrap">
+              <bool>true</bool>
+            </property>
+            </widget>
+          </item>
+          <item row="6" column="1">
+            <widget class="QLabel" name="label_39">
+            <property name="text">
+              <string>Time optimization (s)</string>
+            </property>
+            </widget>
+          </item>
+          <item row="2" column="0">
+            <widget class="QCheckBox" name="checkBox_alignPosesWithGroundTruth">
+            <property name="text">
+              <string/>
+            </property>
+            <property name="checked">
+              <bool>true</bool>
+            </property>
+            </widget>
+          </item>
+          <item row="1" column="1">
+            <widget class="QLabel" name="label_alignPosesWithGPS">
+            <property name="text">
+              <string>Align poses with GPS</string>
+            </property>
+            <property name="wordWrap">
+              <bool>true</bool>
+            </property>
+            </widget>
+          </item>
+          <item row="1" column="0">
+            <widget class="QCheckBox" name="checkBox_alignPosesWithGPS">
+            <property name="text">
+              <string/>
+            </property>
+            <property name="checked">
+              <bool>true</bool>
+            </property>
+            </widget>
+          </item>
+          </layout>
+        </item>
+       </layout>
+      </widget>       
     </item>
    </layout>
   </widget>
@@ -3272,6 +3281,17 @@
    <string>Vertical Layout</string>
   </property>
  </action>
+  <action name="actionConcise_Layout">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="checked">
+    <bool>false</bool>
+   </property>
+   <property name="text">
+    <string>Concise Layout</string>
+   </property>
+  </action>
  <action name="actionRestore_default_GUI_settings">
   <property name="text">
    <string>Restore default GUI settings</string>
@@ -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>
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
  <name>rtabmap</name>
-  <version>0.21.8</version>
+  <version>0.21.9</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>
@@ -674,27 +674,30 @@ int main(int argc, char * argv[])
 							previousMapId = m;

 							allWeights.insert(std::make_pair(*iter, w));
-							if((!ignoreInterNodes || w!=-1) && w!=-9)
+							if(!ignoreInterNodes || w!=-1)
 							{
-								odomPoses.insert(std::make_pair(*iter, p));
-								odomStamps.insert(std::make_pair(*iter, s));
-								if(!externalGtPoses.empty())
+								if(w!=-9)
 								{
-									std::map<double, rtabmap::Transform>::iterator nextIter = externalGtPoses.upper_bound(s);
-									if(nextIter!=externalGtPoses.end())
+									odomPoses.insert(std::make_pair(*iter, p));
+									odomStamps.insert(std::make_pair(*iter, s));
+									if(!externalGtPoses.empty())
 									{
-										std::map<double, rtabmap::Transform>::iterator previousIter = nextIter;
-										--previousIter;
-										if(s == previousIter->first || (nextIter->first-s <= gtMaxInterval && s-previousIter->first <= gtMaxInterval))
+										std::map<double, rtabmap::Transform>::iterator nextIter = externalGtPoses.upper_bound(s);
+										if(nextIter!=externalGtPoses.end())
 										{
-											UASSERT(s-previousIter->first >= 0);
-											gtPoses.insert(std::make_pair(*iter, previousIter->second.interpolate((s-previousIter->first)/(nextIter->first-previousIter->first),nextIter->second)));
+											std::map<double, rtabmap::Transform>::iterator previousIter = nextIter;
+											--previousIter;
+											if(s == previousIter->first || (nextIter->first-s <= gtMaxInterval && s-previousIter->first <= gtMaxInterval))
+											{
+												UASSERT(s-previousIter->first >= 0);
+												gtPoses.insert(std::make_pair(*iter, previousIter->second.interpolate((s-previousIter->first)/(nextIter->first-previousIter->first),nextIter->second)));
+											}
 										}
 									}
-								}
-								else if(!gt.isNull())
-								{
-									gtPoses.insert(std::make_pair(*iter, gt));
+									else if(!gt.isNull())
+									{
+										gtPoses.insert(std::make_pair(*iter, gt));
+									}
 								}

 								if(!localizationMultiStats.empty() && mappingSessionIds.find(m) != mappingSessionIds.end())
@@ -54,7 +54,7 @@ public:
 public:
 	UVariant();
 	UVariant(const bool & value);
-	UVariant(const char & value);
+	UVariant(const signed char & value);
 	UVariant(const unsigned char & value);
 	UVariant(const short & value);
 	UVariant(const unsigned short & value);
@@ -64,7 +64,7 @@ public:
 	UVariant(const double & value);
 	UVariant(const char * value);
 	UVariant(const std::string & value);
-	UVariant(const std::vector<char> & value);
+	UVariant(const std::vector<signed char> & value);
 	UVariant(const std::vector<unsigned char> & value);
 	UVariant(const std::vector<short> & value);
 	UVariant(const std::vector<unsigned short> & value);
@@ -96,7 +96,7 @@ public:
 	bool isDoubleArray() const {return type_ == kDoubleArray;}

 	bool toBool() const;
-	char toChar(bool * ok = 0) const;
+	signed char toChar(bool * ok = 0) const;
 	unsigned char toUChar(bool * ok = 0) const;
 	short toShort(bool * ok = 0) const;
 	unsigned short toUShort(bool * ok = 0) const;
@@ -105,7 +105,7 @@ public:
 	float toFloat(bool * ok = 0) const;
 	double toDouble(bool * ok = 0) const;
 	std::string toStr(bool * ok = 0) const;
-	std::vector<char> toCharArray(bool * ok = 0) const;
+	std::vector<signed char> toCharArray(bool * ok = 0) const;
 	std::vector<unsigned char> toUCharArray(bool * ok = 0) const;
 	std::vector<short> toShortArray(bool * ok = 0) const;
 	std::vector<unsigned short> toUShortArray(bool * ok = 0) const;
@@ -32,11 +32,11 @@ UVariant::UVariant(const bool & value) :
 {
 	data_[0] = value?1:0;
 }
-UVariant::UVariant(const char & value) :
+UVariant::UVariant(const signed char & value) :
 	type_(kChar),
-	data_(sizeof(char))
+	data_(sizeof(signed char))
 {
-	memcpy(data_.data(), &value, sizeof(char));
+	memcpy(data_.data(), &value, sizeof(signed char));
 }
 UVariant::UVariant(const unsigned char & value) :
 	type_(kUChar),
@@ -93,11 +93,11 @@ UVariant::UVariant(const std::string & value) :
 {
 	memcpy(data_.data(), value.data(), value.size()+1);
 }
-UVariant::UVariant(const std::vector<char> & value) :
+UVariant::UVariant(const std::vector<signed char> & value) :
 	type_(kCharArray),
-	data_(sizeof(char)*value.size())
+	data_(sizeof(signed char)*value.size())
 {
-	memcpy(data_.data(), value.data(), sizeof(char)*value.size());
+	memcpy(data_.data(), value.data(), sizeof(signed char)*value.size());
 }
 UVariant::UVariant(const std::vector<unsigned char> & value) :
 	type_(kUCharArray),
@@ -155,16 +155,16 @@ bool UVariant::toBool() const
 	return false;
 }

-char UVariant::toChar(bool * ok) const
+signed char UVariant::toChar(bool * ok) const
 {
 	if(ok)
 	{
 		*ok = false;
 	}
-	char v = 0;
+	signed char v = 0;
 	if(type_ == kChar)
 	{
-		memcpy(&v, data_.data(), sizeof(char));
+		memcpy(&v, data_.data(), sizeof(signed char));
 		if(ok)
 		{
 			*ok = true;
@@ -173,9 +173,9 @@ char UVariant::toChar(bool * ok) const
 	else if(type_ == kUChar)
 	{
 		unsigned char tmp = toUChar();
-		if(tmp <= std::numeric_limits<char>::max())
+		if(tmp <= std::numeric_limits<signed char>::max())
 		{
-			v = (char)tmp;
+			v = (signed char)tmp;
 			if(ok)
 			{
 				*ok = true;
@@ -185,9 +185,9 @@ char UVariant::toChar(bool * ok) const
 	else if(type_ == kShort)
 	{
 		short tmp = toShort();
-		if(tmp >= std::numeric_limits<char>::min() && tmp <= std::numeric_limits<char>::max())
+		if(tmp >= std::numeric_limits<signed char>::min() && tmp <= std::numeric_limits<signed char>::max())
 		{
-			v = (char)tmp;
+			v = (signed char)tmp;
 			if(ok)
 			{
 				*ok = true;
@@ -197,9 +197,9 @@ char UVariant::toChar(bool * ok) const
 	else if(type_ == kUShort)
 	{
 		unsigned short tmp = toUShort();
-		if(tmp <= std::numeric_limits<char>::max())
+		if(tmp <= std::numeric_limits<signed char>::max())
 		{
-			v = (char)tmp;
+			v = (signed char)tmp;
 			if(ok)
 			{
 				*ok = true;
@@ -209,9 +209,9 @@ char UVariant::toChar(bool * ok) const
 	else if(type_ == kInt)
 	{
 		int tmp = toInt();
-		if(tmp >= std::numeric_limits<char>::min() && tmp <= std::numeric_limits<char>::max())
+		if(tmp >= std::numeric_limits<signed char>::min() && tmp <= std::numeric_limits<signed char>::max())
 		{
-			v = (char)tmp;
+			v = (signed char)tmp;
 			if(ok)
 			{
 				*ok = true;
@@ -221,9 +221,9 @@ char UVariant::toChar(bool * ok) const
 	else if(type_ == kUInt)
 	{
 		unsigned int tmp = toUInt();
-		if(tmp <= (unsigned int)std::numeric_limits<char>::max())
+		if(tmp <= (unsigned int)std::numeric_limits<signed char>::max())
 		{
-			v = (char)tmp;
+			v = (signed char)tmp;
 			if(ok)
 			{
 				*ok = true;
@@ -395,7 +395,7 @@ unsigned short UVariant::toUShort(bool * ok) const
 	}
 	else if(type_ == kChar)
 	{
-		char tmp = toChar();
+		signed char tmp = toChar();
 		if(tmp >= 0)
 		{
 			v = (unsigned short)tmp;
@@ -529,7 +529,7 @@ unsigned int UVariant::toUInt(bool * ok) const
 	}
 	else if(type_ == kChar)
 	{
-		char tmp = toChar();
+		signed char tmp = toChar();
 		if(tmp >= 0)
 		{
 			v = (unsigned int)tmp;
@@ -690,14 +690,14 @@ std::string UVariant::toStr(bool * ok) const
 	return v;
 }

-std::vector<char> UVariant::toCharArray(bool * ok) const
+std::vector<signed char> UVariant::toCharArray(bool * ok) const
 {
 	if(ok)
 	{
 		*ok = false;
 	}

-	std::vector<char> v;
+	std::vector<signed char> v;
 	if(type_ == kCharArray)
 	{
 		if(ok)
@@ -706,7 +706,7 @@ std::vector<char> UVariant::toCharArray(bool * ok) const
 		}
 		if(data_.size())
 		{
-			v.resize(data_.size() / sizeof(char));
+			v.resize(data_.size() / sizeof(signed char));
 			memcpy(v.data(), data_.data(), data_.size());
 		}
 	}
Author	SHA1	Message	Date
matlabbe	4098f0683a	g2o vcpkg latest issue	2024-12-24 16:42:26 -08:00
matlabbe	2f431ce385	Export: added gps/gt export options	2024-12-22 11:33:41 -08:00
Borong Yuanandmatlabbe	89849ae245	add support for RVL depth image compression (#1409 ) * 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>	2024-12-15 18:35:05 -08:00
matlabbe	02e30ffc69	Fixed #1413	2024-12-15 17:51:15 -08:00
matlabbe	97c56361f8	Export CLI tool memory optimization (#1391 ) * Stashing changes so far * Tested and working * removed unused options * Added density_radius and density_angle options * improved description * backward compatibility fixes, matching texture exported file names with base name * Setting global full opt by default, updated preferences defaulting to g2o or gtsam if one or the other is unavailable	2024-12-15 13:08:15 -08:00
matlabbe	340248fed0	Refactored gravity links visualization (fixed rotation in some cases).	2024-12-14 16:33:29 -08:00
Johannes Schönberger	626bf64983	Support Ceres 2.1+ Manifolds in backwards-compatible manner (#1405 ) * Support Ceres 2.1+ Manifolds in backwards-compatible manner * d * d	2024-12-14 10:27:21 -08:00
matlabbe	38cacb7978	Added floor filtering options for features extraction (#1408 ) * Added floor filtering options for features extraction * cleanup	2024-12-10 12:36:21 -08:00
matlabbe	a613998652	Updated cmake target for latest grid_map_core version	2024-12-08 09:58:37 -08:00
matlabbe	d33e98a540	Preferences: making local grid ROI editable	2024-12-06 18:31:27 -08:00
matlabbe	5988f0bb3b	CI: only keeping oldest ROS1 and ROS2 not EOL versions (with all working deps)	2024-11-30 19:47:15 -08:00
matlabbe	8f46cc7370	Revert "CI: added builds when pushing to ***-devel branches" This reverts commit `10eea91044`.	2024-11-30 18:26:26 -08:00
matlabbe	10eea91044	CI: added builds when pushing to ***-devel branches	2024-11-30 18:06:41 -08:00
matlabbe	b55759174d	CI: upgrading ros-tooling to 0.7	2024-11-30 17:57:01 -08:00
matlabbe	13ed9137a5	Re-enabling jazzy CI build	2024-11-30 17:43:12 -08:00
matlabbe	ca877041e3	Revert "ULogger console: using std::cerr by default to avoid ROS2 default buffering on cout/printf." This reverts commit `3eda219f1d`.	2024-11-29 19:49:49 -08:00
matlabbe	3eda219f1d	ULogger console: using std::cerr by default to avoid ROS2 default buffering on cout/printf.	2024-11-29 18:59:12 -08:00
matlabbe	13bc3c5c2c	bump ros package version	2024-11-29 16:26:12 -08:00
matlabbe	8a26f42687	0.21.9: Grid's noise filtering only done on indices below Grid/RangeMax (when set). We can then keep very far points that would be filtered otherwise, which are useful for ray tracing.	2024-11-28 20:58:40 -08:00
matlabbe	968b9f8609	docker: updated latest deps Dockerfile example	2024-11-24 15:12:55 -08:00
matlabbe	afde4af3a7	Fixed OpenVINS build if openvins is built on ROS2 (#1379 )	2024-11-24 14:41:41 -08:00
matlabbe	cbd3995b60	Applying suggestion of #1388 (#1390 )	2024-11-23 20:17:39 -08:00
matlabbe	5fff349623	android: fixed app exiting when about to show toast	2024-11-23 09:30:53 -08:00
matlabbe	b2e26e81c1	Fixed #1380	2024-11-16 14:54:15 -08:00
Charles Ellison Chuck-Ellison matlabbe	f10dee4175	Concise layout (#1381 ) * created concise layout * Hiding matching lines and always update graphics view A when clicking on a node in the graph in concise view mode --------- Co-authored-by: Chuck-Ellison <charles.c.ellison@erdc.dren.mil> Co-authored-by: matlabbe <matlabbe@gmail.com>	2024-11-16 12:13:07 -08:00
matlabbe	a584245029	Fixed all -Wtype-limits warnings	2024-11-15 14:13:13 -08:00
matlabbe	966feefb35	DBViewer: fixed Refine links menu not showing up if there are landmark links	2024-11-14 20:15:04 -08:00
matlabbe	fd27380be7	CudaSift: switch to SIFT CPU if no cuda devices were found (instead of asserting).	2024-11-12 15:00:19 -08:00
Borong Yuan	75ac722996	draw "1 - Loop Thr" along with posterior (#1373 )	2024-11-02 12:51:05 -07:00
matlabbe	374df24ac9	report tool: fixed localization session not detected	2024-10-31 19:19:34 -07:00
matlabbe	6b313257f2	Reg/OpticalFlow: fixed missing pt reprojection for last camera	2024-10-30 14:14:29 -07:00
matlabbe	8b21c27f5f	Adding FOURCC usb camera support (#1366 ) * Adding FOURCC usb camera support * Improved warnings * more checks and warnings	2024-10-27 18:09:07 -07:00
Borong Yuan	2454b04f1f	Sort keypoints before SSC (#1364 ) * Sort keypoints before SSC * avoid using vector<bool>	2024-10-26 14:44:22 -07:00
matlabbe	e3fde4ecdd	DBViewer: update main slider when index spinbox value changes	2024-10-26 12:04:34 -07:00