merged master

CI: only keeping oldest ROS1 and ROS2 not EOL versions (with all working deps)
CI: only build humble in humble-devel branch
2026-09-16 16:27:45 +08:00 · 2024-11-30 19:49:10 -08:00 · 2024-11-30 19:47:15 -08:00 · 2024-11-30 18:32:29 -08:00 · 2024-11-30 18:26:26 -08:00 · 2024-11-30 18:06:41 -08:00
41 changed files with 1424 additions and 815 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_*
@@ -3,7 +3,7 @@ name: CMake-ROS
 on:
  push:
    branches: 
-      - master
+      - humble-devel
  pull_request:
    branches: 
      - '**'
@@ -23,20 +23,13 @@ jobs:
    strategy:
      fail-fast: false
      matrix:
-        ros_distribution: [ noetic, humble, iron]
+        ros_distribution: [ 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 7)
+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)
@@ -1054,9 +1066,6 @@ ELSE()
 ENDIF()
 IF(NOT grid_map_core_FOUND)
   SET(GRIDMAP "//")
-   SET(CONF_WITH_GRIDMAP 0)
-ELSE()
-   SET(CONF_WITH_GRIDMAP 1)
 ENDIF()
 IF(NOT CPUTSDF_FOUND)
   SET(CPUTSDF "//")
@@ -54,10 +54,6 @@ IF(@CONF_WITH_OCTOMAP@)
  find_dependency(octomap)
 ENDIF()

-IF(@CONF_WITH_GRIDMAP@)
-  find_dependency(grid_map_core)
-ENDIF()
-
 IF(@CONF_WITH_PYTHON@)
  find_dependency(Python3 COMPONENTS Interpreter Development NumPy)
 ENDIF()
@@ -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();
 				}
@@ -791,7 +791,7 @@
 			);
 			runOnlyForDeploymentPostprocessing = 0;
 			shellPath = /bin/sh;
-			shellScript = "# Type a script or drag a script file from your workspace to insert its path.\nversion=\"$MARKETING_VERSION\"\n/usr/libexec/PlistBuddy -c \"Set PreferenceSpecifiers:22:DefaultValue $version\" \"${SRCROOT}/Settings.bundle/Root.plist\"\n";
+			shellScript = "# Type a script or drag a script file from your workspace to insert its path.\nversion=\"$MARKETING_VERSION\"\n/usr/libexec/PlistBuddy -c \"Set PreferenceSpecifiers:21:DefaultValue $version\" \"${SRCROOT}/Settings.bundle/Root.plist\"\n";
 		};
 /* End PBXShellScriptBuildPhase section */

@@ -1013,7 +1013,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.21.7;
+				MARKETING_VERSION = 0.21.8;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1070,7 +1070,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.21.7;
+				MARKETING_VERSION = 0.21.8;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
@@ -54,6 +54,34 @@
 			<key>DefaultValue</key>
 			<true/>
 		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Choosing between high and low depth confidence depends on the application. If precise geometry is critical, prioritize high depth confidence even if it means fewer points. If you need a dense point cloud for analysis, even in areas with weak LiDAR signal, a lower depth confidence might be suitable at the cost of highly interpolated points (that could give a wavy look of some surfaces). Important: the points with too low confidence are not saved in the db file, so we cannot recover or filter these points in post-processing.</string>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<integer>2</integer>
+			<key>Key</key>
+			<string>DepthConfidence</string>
+			<key>Title</key>
+			<string>Depth Confidence</string>
+			<key>Titles</key>
+			<array>
+				<string>High</string>
+				<string>Medium</string>
+				<string>Low</string>
+			</array>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Values</key>
+			<array>
+				<integer>2</integer>
+				<integer>1</integer>
+				<integer>0</integer>
+			</array>
+		</dict>
 		<dict>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
@@ -74,7 +102,7 @@
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
 			<key>FooterText</key>
-			<string>Filter ARKit's re-localization events to avoid jumps in odometry when creating a map with RTAB-Map, which would cause large drift errors that are difficult to correct. Set an acceleration threshold to detect those events.</string>
+			<string>Filter ARKit&apos;s re-localization events to avoid jumps in odometry when creating a map with RTAB-Map, which would cause large drift errors that are difficult to correct. Set an acceleration threshold to detect those events.</string>
 		</dict>
 		<dict>
 			<key>Type</key>
@@ -642,13 +670,13 @@
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
 			<key>FooterText</key>
-			<string>ArUco markers can be detected for localization and graph optimization.</string>
+			<string>ArUco or AprilTag markers can be detected for localization and graph optimization. Marker number should be &gt;0.</string>
 		</dict>
 		<dict>
 			<key>Type</key>
 			<string>PSMultiValueSpecifier</string>
 			<key>Title</key>
-			<string>ArUco Marker Detection</string>
+			<string>ArUco/AprilTag Marker Detection</string>
 			<key>Key</key>
 			<string>ArUcoMarkerDetection</string>
 			<key>DefaultValue</key>
@@ -814,7 +842,7 @@
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
 			<key>FooterText</key>
-			<string>The database is kept in RAM for fast access. Set to false to reduce RAM used at the cost of slower access. This parameter is applied on reset or when a database is opened.</string>
+			<string>The database is kept in RAM for fast access. Set to false to reduce RAM used at the cost of slower access. This parameter is applied on reset or when a database is opened. Important: if enabled, the database cannot be recovered in case of the App crashes for some reason (e.g., low battery).</string>
 		</dict>
 		<dict>
 			<key>Type</key>
@@ -6,7 +6,7 @@
 	<array>
 		<dict>
 			<key>FooterText</key>
-			<string>Decrease density to reduce rendering time and memory. Tip: To apply a different density to current map: save the map, change density and re-open the same map to regenerate the point clouds at this density. Points over the maximum depth, under minimum depth or under the depth confidence are not rendered.</string>
+			<string>Decrease density to reduce rendering time and memory. Points over the maximum depth or under minimum depth are not rendered. In all cases, the raw data is still saved in the db file, meaning that the the point cloud can be regenerated differently by changing these settings and by re-opening the scan in the Library.</string>
 			<key>Title</key>
 			<string>Rendering</string>
 			<key>Type</key>
@@ -60,7 +60,7 @@
 			<string>PSMultiValueSpecifier</string>
 			<key>Values</key>
 			<array>
-				<real>0</real>
+				<real>0.0</real>
 				<real>5</real>
 				<real>4.5</real>
 				<real>4</real>
@@ -74,7 +74,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<real>0</real>
+			<real>0.0</real>
 			<key>Key</key>
 			<string>MinDepth</string>
 			<key>Title</key>
@@ -95,8 +95,8 @@
 			<string>PSMultiValueSpecifier</string>
 			<key>Values</key>
 			<array>
-				<real>0</real>
-				<real>0.3</real>
+				<real>0.0</real>
+				<real>0.29999999999999999</real>
 				<real>0.5</real>
 				<real>0.75</real>
 				<real>1</real>
@@ -106,31 +106,9 @@
 				<real>3</real>
 			</array>
 		</dict>
-		<dict>
-			<key>DefaultValue</key>
-			<integer>2</integer>
-			<key>Key</key>
-			<string>DepthConfidence</string>
-			<key>Title</key>
-			<string>Depth Confidence</string>
-			<key>Titles</key>
-			<array>
-				<string>High</string>
-				<string>Medium</string>
-				<string>Low</string>
-			</array>
-			<key>Type</key>
-			<string>PSMultiValueSpecifier</string>
-			<key>Values</key>
-			<array>
-				<integer>2</integer>
-				<integer>1</integer>
-				<integer>0</integer>
-			</array>
-		</dict>
 		<dict>
 			<key>FooterText</key>
-			<string>Increase minimum polygon angle to force scanning perpendicular to surfaces. Set a decimation factor to reduce the number of polygons. The higher the factor, the less polygons will remain. Note that the decimation factor is done after decreasing point cloud density. Decimation factor and texture resolution don&apos;t affect post-processing.</string>
+			<string>Increase minimum polygon angle to force scanning perpendicular to surfaces. Set a decimation factor to reduce the number of polygons. The higher the factor, the less polygons will remain. Note that the decimation factor is done after decreasing point cloud density. Decimation factor and texture resolution don't affect post-processing.</string>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
 		</dict>
@@ -196,7 +174,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<real>0</real>
+			<real>0.0</real>
 			<key>Key</key>
 			<string>MeshDecimationFactor</string>
 			<key>Title</key>
@@ -220,18 +198,18 @@
 			<string>PSMultiValueSpecifier</string>
 			<key>Values</key>
 			<array>
-				<real>0.99</real>
-				<real>0.95</real>
-				<real>0.9</real>
-				<real>0.8</real>
-				<real>0.7</real>
-				<real>0.6</real>
+				<real>0.98999999999999999</real>
+				<real>0.94999999999999996</real>
+				<real>0.90000000000000002</real>
+				<real>0.80000000000000004</real>
+				<real>0.69999999999999996</real>
+				<real>0.59999999999999998</real>
 				<real>0.5</real>
-				<real>0.4</real>
-				<real>0.3</real>
-				<real>0.2</real>
-				<real>0.1</real>
-				<real>0</real>
+				<real>0.40000000000000002</real>
+				<real>0.29999999999999999</real>
+				<real>0.20000000000000001</real>
+				<real>0.10000000000000001</real>
+				<real>0.0</real>
 			</array>
 		</dict>
 		<dict>
@@ -266,7 +244,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<real>0.2</real>
+			<real>0.20000000000000001</real>
 			<key>Key</key>
 			<string>BackgroundColor</string>
 			<key>Title</key>
@@ -288,16 +266,16 @@
 			<string>PSMultiValueSpecifier</string>
 			<key>Values</key>
 			<array>
-				<real>0.9</real>
-				<real>0.8</real>
-				<real>0.7</real>
-				<real>0.6</real>
+				<real>0.90000000000000002</real>
+				<real>0.80000000000000004</real>
+				<real>0.69999999999999996</real>
+				<real>0.59999999999999998</real>
 				<real>0.5</real>
-				<real>0.4</real>
-				<real>0.3</real>
-				<real>0.2</real>
-				<real>0.1</real>
-				<real>0</real>
+				<real>0.40000000000000002</real>
+				<real>0.29999999999999999</real>
+				<real>0.20000000000000001</real>
+				<real>0.10000000000000001</real>
+				<real>0.0</real>
 			</array>
 		</dict>
 		<dict>
@@ -402,19 +380,19 @@
 			<string>PSMultiValueSpecifier</string>
 			<key>Values</key>
 			<array>
-				<real>0.3</real>
-				<real>0.2</real>
-				<real>0.1</real>
-				<real>0.05</real>
-				<real>0.025</real>
+				<real>0.29999999999999999</real>
+				<real>0.20000000000000001</real>
+				<real>0.10000000000000001</real>
+				<real>0.050000000000000003</real>
+				<real>0.025000000000000001</real>
 				<real>0.02</real>
-				<real>0.015</real>
+				<real>0.014999999999999999</real>
 				<real>0.01</real>
 			</array>
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<real>0.05</real>
+			<real>0.050000000000000003</real>
 			<key>Key</key>
 			<string>NoiseFilteringRatio</string>
 			<key>Title</key>
@@ -439,16 +417,16 @@
 			<key>Values</key>
 			<array>
 				<real>1</real>
-				<real>0.9</real>
-				<real>0.8</real>
-				<real>0.7</real>
-				<real>0.6</real>
+				<real>0.90000000000000002</real>
+				<real>0.80000000000000004</real>
+				<real>0.69999999999999996</real>
+				<real>0.59999999999999998</real>
 				<real>0.5</real>
-				<real>0.4</real>
-				<real>0.3</real>
-				<real>0.2</real>
-				<real>0.1</real>
-				<real>0.05</real>
+				<real>0.40000000000000002</real>
+				<real>0.29999999999999999</real>
+				<real>0.20000000000000001</real>
+				<real>0.10000000000000001</real>
+				<real>0.050000000000000003</real>
 				<real>0.01</real>
 			</array>
 		</dict>
@@ -462,7 +440,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<string>0.21.7</string>
+			<string>0.21.8</string>
 			<key>Key</key>
 			<string>Version</string>
 			<key>Title</key>
@@ -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"

@@ -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;
 };
@@ -35,7 +35,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_types.h>
 #include <pcl/PolygonMesh.h>

-#include <grid_map_core/GridMap.hpp>
+namespace grid_map {
+	class GridMap;
+}

 namespace rtabmap {

@@ -44,9 +46,10 @@ class RTABMAP_CORE_EXPORT GridMap : public GlobalMap
 public:
 	GridMap(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap());

+	virtual ~GridMap();
 	virtual void clear();

-	const grid_map::GridMap & gridMap() const {return gridMap_;}
+	const grid_map::GridMap * gridMap() const {return gridMap_;}

 	cv::Mat createHeightMap(float & xMin, float & yMin, float & cellSize) const;
 	cv::Mat createColorMap(float & xMin, float & yMin, float & cellSize) const;
@@ -60,7 +63,7 @@ private:
 	cv::Mat toImage(const std::string & layer, float & xMin, float & yMin, float & cellSize) const;

 private:
-	grid_map::GridMap gridMap_;
+	grid_map::GridMap * gridMap_;
 	float minMapSize_;
 };

@@ -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(),
@@ -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.
@@ -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);
@@ -644,17 +644,17 @@ ENDIF(octomap_FOUND)

 IF(grid_map_core_FOUND)
    IF(TARGET grid_map_core)
-        SET(PUBLIC_LIBRARIES
-            ${PUBLIC_LIBRARIES}
+        SET(LIBRARIES
+            ${LIBRARIES}
            grid_map_core
        )
    ELSE()
-        SET(PUBLIC_INCLUDE_DIRS
-            ${PUBLIC_INCLUDE_DIRS}
+        SET(INCLUDE_DIRS
+            ${INCLUDE_DIRS}
            ${grid_map_core_INCLUDE_DIRS}
        )
-        SET(PUBLIC_LIBRARIES
-            ${PUBLIC_LIBRARIES}
+        SET(LIBRARIES
+            ${LIBRARIES}
            ${grid_map_core_LIBRARIES}
        )
    ENDIF()
@@ -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());
@@ -597,8 +597,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)
@@ -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());
 			}
 		}
 	}
@@ -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())
@@ -39,18 +39,27 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <grid_map_core/iterators/GridMapIterator.hpp>
 #include <pcl/io/pcd_io.h>

+#include <grid_map_core/GridMap.hpp>
+
 namespace rtabmap {

 GridMap::GridMap(const LocalGridCache * cache, const ParametersMap & parameters) :
 	GlobalMap(cache, parameters),
+	gridMap_(new grid_map::GridMap()),
 	minMapSize_(Parameters::defaultGridGlobalMinSize())
 {
 	Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
 }

+GridMap::~GridMap()
+{
+	delete gridMap_;
+}
+
 void GridMap::clear()
 {
-	gridMap_ = grid_map::GridMap();
+	delete gridMap_;
+	gridMap_ = new grid_map::GridMap();
 	GlobalMap::clear();
 }

@@ -66,15 +75,15 @@ cv::Mat GridMap::createColorMap(float & xMin, float & yMin, float & cellSize) co

 cv::Mat GridMap::toImage(const std::string & layer, float & xMin, float & yMin, float & cellSize) const
 {
-	if( gridMap_.hasBasicLayers())
+	if( gridMap_->hasBasicLayers())
 	{
-		const grid_map::Matrix& data = gridMap_[layer];
+		const grid_map::Matrix& data = (*gridMap_)[layer];

 		cv::Mat image;
 		if(layer.compare("elevation") == 0)
 		{
-			image = cv::Mat::zeros(gridMap_.getSize()(1), gridMap_.getSize()(0), CV_32FC1);
-			for(grid_map::GridMapIterator iterator(gridMap_); !iterator.isPastEnd(); ++iterator) {
+			image = cv::Mat::zeros(gridMap_->getSize()(1), gridMap_->getSize()(0), CV_32FC1);
+			for(grid_map::GridMapIterator iterator(*gridMap_); !iterator.isPastEnd(); ++iterator) {
 				const grid_map::Index index(*iterator);
 				const float& value = data(index(0), index(1));
 				const grid_map::Index imageIndex(iterator.getUnwrappedIndex());
@@ -86,8 +95,8 @@ cv::Mat GridMap::toImage(const std::string & layer, float & xMin, float & yMin,
 		}
 		else if(layer.compare("colors") == 0)
 		{
-			image = cv::Mat::zeros(gridMap_.getSize()(1), gridMap_.getSize()(0), CV_8UC3);
-			for(grid_map::GridMapIterator iterator(gridMap_); !iterator.isPastEnd(); ++iterator) {
+			image = cv::Mat::zeros(gridMap_->getSize()(1), gridMap_->getSize()(0), CV_8UC3);
+			for(grid_map::GridMapIterator iterator(*gridMap_); !iterator.isPastEnd(); ++iterator) {
 				const grid_map::Index index(*iterator);
 				const float& value = data(index(0), index(1));
 				const grid_map::Index imageIndex(iterator.getUnwrappedIndex());
@@ -106,9 +115,9 @@ cv::Mat GridMap::toImage(const std::string & layer, float & xMin, float & yMin,
 			UFATAL("Unknown layer \"%s\"", layer.c_str());
 		}

-		xMin = gridMap_.getPosition().x() - gridMap_.getLength().x()/2.0f;
-		yMin = gridMap_.getPosition().y() - gridMap_.getLength().y()/2.0f;
-		cellSize = gridMap_.getResolution();
+		xMin = gridMap_->getPosition().x() - gridMap_->getLength().x()/2.0f;
+		yMin = gridMap_->getPosition().y() - gridMap_->getLength().y()/2.0f;
+		cellSize = gridMap_->getResolution();

 		return image;

@@ -119,21 +128,21 @@ cv::Mat GridMap::toImage(const std::string & layer, float & xMin, float & yMin,
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr GridMap::createTerrainCloud() const
 {
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-	if( gridMap_.hasBasicLayers())
+	if( gridMap_->hasBasicLayers())
 	{
-		const grid_map::Matrix& dataElevation = gridMap_["elevation"];
-		const grid_map::Matrix& dataColors = gridMap_["colors"];
+		const grid_map::Matrix& dataElevation = (*gridMap_)["elevation"];
+		const grid_map::Matrix& dataColors = (*gridMap_)["colors"];

-		cloud->width = gridMap_.getSize()(0);
-		cloud->height = gridMap_.getSize()(1);
+		cloud->width = gridMap_->getSize()(0);
+		cloud->height = gridMap_->getSize()(1);
 		cloud->resize(cloud->width * cloud->height);
 		cloud->is_dense = false;

-		float xMin = gridMap_.getPosition().x() - gridMap_.getLength().x()/2.0f;
-		float yMin = gridMap_.getPosition().y() - gridMap_.getLength().y()/2.0f;
-		float cellSize = gridMap_.getResolution();
+		float xMin = gridMap_->getPosition().x() - gridMap_->getLength().x()/2.0f;
+		float yMin = gridMap_->getPosition().y() - gridMap_->getLength().y()/2.0f;
+		float cellSize = gridMap_->getResolution();

-		for(grid_map::GridMapIterator iterator(gridMap_); !iterator.isPastEnd(); ++iterator)
+		for(grid_map::GridMapIterator iterator(*gridMap_); !iterator.isPastEnd(); ++iterator)
 		{
 			const grid_map::Index index(*iterator);
 			const float& value = dataElevation(index(0), index(1));
@@ -189,13 +198,13 @@ void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 	bool undefinedSize = minMapSize_ == 0.0f;
 	std::map<int, cv::Mat> occupiedLocalMaps;

-	if(gridMap_.hasBasicLayers())
+	if(gridMap_->hasBasicLayers())
 	{
 		// update
 		minX=minValues_[0]+margin+cellSize_/2.0f;
 		minY=minValues_[1]+margin+cellSize_/2.0f;
-		maxX=minValues_[0]+float(gridMap_.getSize()[0])*cellSize_ - margin;
-		maxY=minValues_[1]+float(gridMap_.getSize()[1])*cellSize_ - margin;
+		maxX=minValues_[0]+float(gridMap_->getSize()[0])*cellSize_ - margin;
+		maxY=minValues_[1]+float(gridMap_->getSize()[1])*cellSize_ - margin;
 		undefinedSize = false;
 	}

@@ -353,26 +362,26 @@ void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 		{
 			UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, minValues_[0], minValues_[1], xMax, yMax);
 			cv::Size newMapSize((xMax - xMin) / cellSize_+0.5f, (yMax - yMin) / cellSize_+0.5f);
-			if(!gridMap_.hasBasicLayers())
+			if(!gridMap_->hasBasicLayers())
 			{
 				UDEBUG("Map empty!");
 				grid_map::Length length = grid_map::Length(xMax - xMin, yMax - yMin);
 				grid_map::Position position = grid_map::Position((xMax+xMin)/2.0f, (yMax+yMin)/2.0f);

 				UDEBUG("length: %f, %f position: %f, %f", length[0], length[1], position[0], position[1]);
-				gridMap_.setGeometry(length, cellSize_, position);
-				UDEBUG("size: %d, %d", gridMap_.getSize()[0], gridMap_.getSize()[1]);
+				gridMap_->setGeometry(length, cellSize_, position);
+				UDEBUG("size: %d, %d", gridMap_->getSize()[0], gridMap_->getSize()[1]);
 				// Add elevation layer
-				gridMap_.add("elevation");
-				gridMap_.add("node_ids");
-				gridMap_.add("colors");
-				gridMap_.setBasicLayers({"elevation"});
+				gridMap_->add("elevation");
+				gridMap_->add("node_ids");
+				gridMap_->add("colors");
+				gridMap_->setBasicLayers({"elevation"});
 			}
 			else
 			{
 				if(xMin == minValues_[0] && yMin == minValues_[1] &&
-						newMapSize.width == gridMap_.getSize()[0] &&
-						newMapSize.height == gridMap_.getSize()[1])
+						newMapSize.width == gridMap_->getSize()[0] &&
+						newMapSize.height == gridMap_->getSize()[1])
 				{
 					// same map size and origin, don't do anything
 					UDEBUG("Map same size!");
@@ -381,8 +390,8 @@ void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 				{
 					UASSERT_MSG(xMin <= minValues_[0]+cellSize_/2, uFormat("xMin=%f, xMin_=%f, cellSize_=%f", xMin, minValues_[0], cellSize_).c_str());
 					UASSERT_MSG(yMin <= minValues_[1]+cellSize_/2, uFormat("yMin=%f, yMin_=%f, cellSize_=%f", yMin, minValues_[1], cellSize_).c_str());
-					UASSERT_MSG(xMax >= minValues_[0]+float(gridMap_.getSize()[0])*cellSize_ - cellSize_/2, uFormat("xMin=%f, xMin_=%f, cols=%d cellSize_=%f", xMin, minValues_[0], gridMap_.getSize()[0], cellSize_).c_str());
-					UASSERT_MSG(yMax >= minValues_[1]+float(gridMap_.getSize()[1])*cellSize_ - cellSize_/2, uFormat("yMin=%f, yMin_=%f, cols=%d cellSize_=%f", yMin, minValues_[1], gridMap_.getSize()[1], cellSize_).c_str());
+					UASSERT_MSG(xMax >= minValues_[0]+float(gridMap_->getSize()[0])*cellSize_ - cellSize_/2, uFormat("xMin=%f, xMin_=%f, cols=%d cellSize_=%f", xMin, minValues_[0], gridMap_->getSize()[0], cellSize_).c_str());
+					UASSERT_MSG(yMax >= minValues_[1]+float(gridMap_->getSize()[1])*cellSize_ - cellSize_/2, uFormat("yMin=%f, yMin_=%f, cols=%d cellSize_=%f", yMin, minValues_[1], gridMap_->getSize()[1], cellSize_).c_str());

 					UDEBUG("Copy map");
 					// copy the old map in the new map
@@ -402,9 +411,9 @@ void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 					UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY);
 					newMapSize.width = (xMax - xMin) / cellSize_+0.5f;
 					newMapSize.height = (yMax - yMin) / cellSize_+0.5f;
-					UDEBUG("%d/%d -> %d/%d", gridMap_.getSize()[0], gridMap_.getSize()[1], newMapSize.width, newMapSize.height);
-					UASSERT(newMapSize.width >= gridMap_.getSize()[0] && newMapSize.height >= gridMap_.getSize()[1]);
-					UASSERT(newMapSize.width >= gridMap_.getSize()[0]+deltaX && newMapSize.height >= gridMap_.getSize()[1]+deltaY);
+					UDEBUG("%d/%d -> %d/%d", gridMap_->getSize()[0], gridMap_->getSize()[1], newMapSize.width, newMapSize.height);
+					UASSERT(newMapSize.width >= gridMap_->getSize()[0] && newMapSize.height >= gridMap_->getSize()[1]);
+					UASSERT(newMapSize.width >= gridMap_->getSize()[0]+deltaX && newMapSize.height >= gridMap_->getSize()[1]+deltaY);
 					UASSERT(deltaX>=0 && deltaY>=0);

 					grid_map::Length length = grid_map::Length(xMax - xMin, yMax - yMin);
@@ -413,28 +422,28 @@ void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 					grid_map::GridMap tmpExtendedMap;
 					tmpExtendedMap.setGeometry(length, cellSize_, position);

-					UDEBUG("%d/%d -> %d/%d", gridMap_.getSize()[0], gridMap_.getSize()[1], tmpExtendedMap.getSize()[0], tmpExtendedMap.getSize()[1]);
+					UDEBUG("%d/%d -> %d/%d", gridMap_->getSize()[0], gridMap_->getSize()[1], tmpExtendedMap.getSize()[0], tmpExtendedMap.getSize()[1]);

 					UDEBUG("extendToInclude (%f,%f,%f,%f) -> (%f,%f,%f,%f)",
-							gridMap_.getLength()[0], gridMap_.getLength()[1],
-							gridMap_.getPosition()[0], gridMap_.getPosition()[1],
+							gridMap_->getLength()[0], gridMap_->getLength()[1],
+							gridMap_->getPosition()[0], gridMap_->getPosition()[1],
 							tmpExtendedMap.getLength()[0], tmpExtendedMap.getLength()[1],
 							tmpExtendedMap.getPosition()[0], tmpExtendedMap.getPosition()[1]);
-					if(!gridMap_.extendToInclude(tmpExtendedMap))
+					if(!gridMap_->extendToInclude(tmpExtendedMap))
 					{
 						UERROR("Failed to update size of the grid map");
 					}
-					UDEBUG("Updated side: %d %d", gridMap_.getSize()[0], gridMap_.getSize()[1]);
+					UDEBUG("Updated side: %d %d", gridMap_->getSize()[0], gridMap_->getSize()[1]);
 				}
 			}
-			UDEBUG("map %d %d", gridMap_.getSize()[0], gridMap_.getSize()[1]);
+			UDEBUG("map %d %d", gridMap_->getSize()[0], gridMap_->getSize()[1]);
 			if(newPoses.size())
 			{
 				UDEBUG("first pose= %d last pose=%d", newPoses.begin()->first, newPoses.rbegin()->first);
 			}
-			grid_map::Matrix& gridMapData = gridMap_["elevation"];
-			grid_map::Matrix& gridMapNodeIds = gridMap_["node_ids"];
-			grid_map::Matrix& gridMapColors = gridMap_["colors"];
+			grid_map::Matrix& gridMapData = (*gridMap_)["elevation"];
+			grid_map::Matrix& gridMapNodeIds = (*gridMap_)["node_ids"];
+			grid_map::Matrix& gridMapColors = (*gridMap_)["colors"];
 			for(std::list<std::pair<int, Transform> >::const_iterator kter = newPoses.begin(); kter!=newPoses.end(); ++kter)
 			{
 				std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(kter->first);
@@ -447,10 +456,10 @@ void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 						float * ptf = iter->second.ptr<float>(0,i);
 						grid_map::Position position(ptf[0], ptf[1]);
 						grid_map::Index index;
-						if(gridMap_.getIndex(position, index))
+						if(gridMap_->getIndex(position, index))
 						{
 							// If no elevation has been set, use current elevation.
-							if (!gridMap_.isValid(index))
+							if (!gridMap_->isValid(index))
 							{
 								gridMapData(index(0), index(1)) = ptf[2];
 								gridMapNodeIds(index(0), index(1)) = kter->first;
@@ -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");
@@ -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
 			}
 		}

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

@@ -4440,15 +4464,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 +4499,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 +4530,10 @@ void DatabaseViewer::refineLinks()
 			}
 		}
 	}
+	else
+	{
+		UWARN("No links can be refined!");
+	}
 }
 void DatabaseViewer::refineLinks(const QList<Link> & links)
 {
@@ -4565,11 +4597,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();
 	}
 }

@@ -5907,6 +5939,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 +7193,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 +7214,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;
@@ -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())
 		{
@@ -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)
@@ -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()));
@@ -2732,6 +2734,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 +2812,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 +3336,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 +3412,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");
@@ -6738,6 +6744,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 +6876,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>
@@ -63,7 +63,7 @@
          <property name="geometry">
           <rect>
            <x>0</x>
-            <y>0</y>
+            <y>-269</y>
            <width>713</width>
            <height>4653</height>
           </rect>
@@ -95,7 +95,7 @@
                <enum>QFrame::Raised</enum>
               </property>
               <property name="currentIndex">
-                <number>24</number>
+                <number>5</number>
               </property>
               <widget class="QWidget" name="page_22">
                <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -3424,7 +3424,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                       <item>
                        <widget class="QStackedWidget" name="stackedWidget_src">
                         <property name="currentIndex">
-                          <number>0</number>
+                          <number>2</number>
                         </property>
                         <widget class="QWidget" name="page_41">
                          <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -5112,7 +5112,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              <item>
                               <widget class="QStackedWidget" name="stackedWidget_stereo">
                                <property name="currentIndex">
-                                 <number>4</number>
+                                 <number>5</number>
                                </property>
                                <widget class="QWidget" name="page_49">
                                 <layout class="QVBoxLayout" name="verticalLayout_91"/>
@@ -5595,29 +5595,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     <string>Usb Camera</string>
                                    </property>
                                    <layout class="QGridLayout" name="gridLayout_99" columnstretch="0,1">
-                                     <item row="0" column="1">
-                                      <widget class="QLabel" name="label_470">
-                                       <property name="text">
-                                        <string>Optional right device ID. It should be -1 if the stereo camera streams side-by-side images.</string>
-                                       </property>
-                                       <property name="wordWrap">
-                                        <bool>true</bool>
-                                       </property>
-                                       <property name="textInteractionFlags">
-                                        <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                                       </property>
-                                      </widget>
-                                     </item>
-                                     <item row="3" column="0">
-                                      <widget class="QSpinBox" name="spinBox_stereousbcam_streamHeight">
-                                       <property name="minimum">
-                                        <number>0</number>
-                                       </property>
-                                       <property name="maximum">
-                                        <number>10000</number>
-                                       </property>
-                                      </widget>
-                                     </item>
                                     <item row="1" column="1">
                                      <widget class="QLabel" name="label_620">
                                       <property name="text">
@@ -5631,6 +5608,29 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                       </property>
                                      </widget>
                                     </item>
+                                     <item row="5" column="0">
+                                      <spacer name="verticalSpacer_51">
+                                       <property name="orientation">
+                                        <enum>Qt::Vertical</enum>
+                                       </property>
+                                       <property name="sizeHint" stdset="0">
+                                        <size>
+                                         <width>20</width>
+                                         <height>0</height>
+                                        </size>
+                                       </property>
+                                      </spacer>
+                                     </item>
+                                     <item row="3" column="0">
+                                      <widget class="QSpinBox" name="spinBox_stereousbcam_streamHeight">
+                                       <property name="minimum">
+                                        <number>0</number>
+                                       </property>
+                                       <property name="maximum">
+                                        <number>10000</number>
+                                       </property>
+                                      </widget>
+                                     </item>
                                     <item row="3" column="1">
                                      <widget class="QLabel" name="label_619">
                                       <property name="text">
@@ -5644,13 +5644,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                       </property>
                                      </widget>
                                     </item>
-                                     <item row="0" column="0">
-                                      <widget class="QSpinBox" name="spinBox_stereo_right_device">
-                                       <property name="minimum">
-                                        <number>-1</number>
-                                       </property>
-                                      </widget>
-                                     </item>
                                     <item row="1" column="0">
                                      <widget class="QSpinBox" name="spinBox_stereousbcam_streamWidth">
                                       <property name="minimum">
@@ -5661,18 +5654,45 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                       </property>
                                      </widget>
                                     </item>
+                                     <item row="0" column="0">
+                                      <widget class="QSpinBox" name="spinBox_stereo_right_device">
+                                       <property name="minimum">
+                                        <number>-1</number>
+                                       </property>
+                                      </widget>
+                                     </item>
+                                     <item row="0" column="1">
+                                      <widget class="QLabel" name="label_470">
+                                       <property name="text">
+                                        <string>Optional right device ID. It should be -1 if the stereo camera streams side-by-side images.</string>
+                                       </property>
+                                       <property name="wordWrap">
+                                        <bool>true</bool>
+                                       </property>
+                                       <property name="textInteractionFlags">
+                                        <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                       </property>
+                                      </widget>
+                                     </item>
+                                     <item row="4" column="1">
+                                      <widget class="QLabel" name="label_755">
+                                       <property name="text">
+                                        <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;FOURCC (&lt;a href=&quot;https://fourcc.org/codecs.php&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;https://fourcc.org/codecs.php&lt;/span&gt;&lt;/a&gt;).&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                                       </property>
+                                       <property name="wordWrap">
+                                        <bool>true</bool>
+                                       </property>
+                                       <property name="textInteractionFlags">
+                                        <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                       </property>
+                                      </widget>
+                                     </item>
                                     <item row="4" column="0">
-                                      <spacer name="verticalSpacer_51">
-                                       <property name="orientation">
-                                        <enum>Qt::Vertical</enum>
+                                      <widget class="QLineEdit" name="lineEdit_stereousbcam_fourcc">
+                                       <property name="toolTip">
+                                        <string>Should be 4 characters</string>
                                       </property>
-                                       <property name="sizeHint" stdset="0">
-                                        <size>
-                                         <width>20</width>
-                                         <height>0</height>
-                                        </size>
-                                       </property>
-                                      </spacer>
+                                      </widget>
                                     </item>
                                    </layout>
                                   </widget>
@@ -6527,6 +6547,29 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     <string>USB Camera</string>
                                    </property>
                                    <layout class="QGridLayout" name="gridLayout_112" columnstretch="0,1">
+                                     <item row="0" column="0">
+                                      <widget class="QSpinBox" name="spinBox_usbcam_streamWidth">
+                                       <property name="minimum">
+                                        <number>0</number>
+                                       </property>
+                                       <property name="maximum">
+                                        <number>10000</number>
+                                       </property>
+                                      </widget>
+                                     </item>
+                                     <item row="0" column="1">
+                                      <widget class="QLabel" name="label_552">
+                                       <property name="text">
+                                        <string>Stream width (0 for default resolution).</string>
+                                       </property>
+                                       <property name="wordWrap">
+                                        <bool>true</bool>
+                                       </property>
+                                       <property name="textInteractionFlags">
+                                        <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                       </property>
+                                      </widget>
+                                     </item>
                                     <item row="1" column="0">
                                      <widget class="QSpinBox" name="spinBox_usbcam_streamHeight">
                                       <property name="minimum">
@@ -6550,10 +6593,17 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                       </property>
                                      </widget>
                                     </item>
-                                     <item row="0" column="1">
-                                      <widget class="QLabel" name="label_552">
+                                     <item row="2" column="0">
+                                      <widget class="QLineEdit" name="lineEdit_usbcam_fourcc">
+                                       <property name="toolTip">
+                                        <string>Should be 4 characters</string>
+                                       </property>
+                                      </widget>
+                                     </item>
+                                     <item row="2" column="1">
+                                      <widget class="QLabel" name="label_758">
                                       <property name="text">
-                                        <string>Stream width (0 for default resolution).</string>
+                                        <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;FOURCC (&lt;a href=&quot;https://fourcc.org/codecs.php&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;https://fourcc.org/codecs.php&lt;/span&gt;&lt;/a&gt;).&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                                       </property>
                                       <property name="wordWrap">
                                        <bool>true</bool>
@@ -6563,16 +6613,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                       </property>
                                      </widget>
                                     </item>
-                                     <item row="0" column="0">
-                                      <widget class="QSpinBox" name="spinBox_usbcam_streamWidth">
-                                       <property name="minimum">
-                                        <number>0</number>
-                                       </property>
-                                       <property name="maximum">
-                                        <number>10000</number>
-                                       </property>
-                                      </widget>
-                                     </item>
                                    </layout>
                                   </widget>
                                  </item>
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
  <name>rtabmap</name>
-  <version>0.21.7</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>
@@ -1953,6 +1953,53 @@ int main(int argc, char * argv[])
 						}
 						else
 						{
+							// Save multiband first
+							if(multiband)
+							{
+								timer.restart();
+								std::string outputPath=outputDirectory+"/"+baseName+"_mesh_multiband.obj";
+								printf("MultiBand texturing (size=%d, downscale=%d, unwrap method=%s, fill holes=%s, padding=%d, best score thr=%f, angle thr=%f, force visible=%s)... \"%s\"\n",
+										textureSize,
+										multibandDownScale,
+										multibandUnwrap==1?"ABF":multibandUnwrap==2?"LSCM":"Basic",
+										multibandFillHoles?"true":"false",
+										multibandPadding,
+										multibandBestScoreThr,
+										multibandAngleHardthr,
+										multibandForceVisible?"false":"true",
+										outputPath.c_str());
+								if(util3d::multiBandTexturing(outputPath,
+										textureMesh->cloud,
+										textureMesh->tex_polygons[0],
+										robotPosesFiltered,
+										vertexToPixels,
+										std::map<int, cv::Mat >(),
+										std::map<int, std::vector<CameraModel> >(),
+										rtabmap.getMemory(),
+										0,
+										textureSize,
+										multibandDownScale,
+										multibandNbContrib,
+										"jpg",
+										gains,
+										blendingGains,
+										contrastValues,
+										doGainCompensationRGB,
+										multibandUnwrap,
+										multibandFillHoles,
+										multibandPadding,
+										multibandBestScoreThr,
+										multibandAngleHardthr,
+										multibandForceVisible))
+								{
+									printf("MultiBand texturing...done (%fs).\n", timer.ticks());
+								}
+								else
+								{
+									printf("MultiBand texturing...failed! (%fs)\n", timer.ticks());
+								}
+							}
+
 							// TextureMesh OBJ
 							bool success = false;
 							UASSERT(!textures.empty());
@@ -1973,9 +2020,23 @@ int main(int argc, char * argv[])
 							}
 							if(success)
 							{
-
 								std::string outputPath=outputDirectory+"/"+baseName+"_mesh.obj";
 								printf("Saving obj (%d vertices) to %s.\n", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, outputPath.c_str());
+#if PCL_VERSION_COMPARE(>=, 1, 13, 0)
+								textureMesh->tex_coord_indices = std::vector<std::vector<pcl::Vertices>>();
+								auto nr_meshes = static_cast<unsigned>(textureMesh->tex_polygons.size());
+								unsigned f_idx = 0;
+								for (unsigned m = 0; m < nr_meshes; m++) {
+									std::vector<pcl::Vertices> ci = textureMesh->tex_polygons[m];
+									for(std::size_t i = 0; i < ci.size(); i++) {
+										for (std::size_t j = 0; j < ci[i].vertices.size(); j++) {
+											ci[i].vertices[j] = ci[i].vertices.size() * (i + f_idx) + j;
+										}
+									}
+									textureMesh->tex_coord_indices.push_back(ci);
+									f_idx += static_cast<unsigned>(textureMesh->tex_polygons[m].size());
+								}
+#endif								
 								success = pcl::io::saveOBJFile(outputPath, *textureMesh) == 0;

 								if(success)
@@ -1988,52 +2049,6 @@ int main(int argc, char * argv[])
 								}
 							}
 						}
-
-						if(multiband)
-						{
-							timer.restart();
-							std::string outputPath=outputDirectory+"/"+baseName+"_mesh_multiband.obj";
-							printf("MultiBand texturing (size=%d, downscale=%d, unwrap method=%s, fill holes=%s, padding=%d, best score thr=%f, angle thr=%f, force visible=%s)... \"%s\"\n",
-									textureSize,
-									multibandDownScale,
-									multibandUnwrap==1?"ABF":multibandUnwrap==2?"LSCM":"Basic",
-									multibandFillHoles?"true":"false",
-									multibandPadding,
-									multibandBestScoreThr,
-									multibandAngleHardthr,
-									multibandForceVisible?"false":"true",
-									outputPath.c_str());
-							if(util3d::multiBandTexturing(outputPath,
-									textureMesh->cloud,
-									textureMesh->tex_polygons[0],
-									robotPosesFiltered,
-									vertexToPixels,
-									std::map<int, cv::Mat >(),
-									std::map<int, std::vector<CameraModel> >(),
-									rtabmap.getMemory(),
-									0,
-									textureSize,
-									multibandDownScale,
-									multibandNbContrib,
-									"jpg",
-									gains,
-									blendingGains,
-									contrastValues,
-									doGainCompensationRGB,
-									multibandUnwrap,
-									multibandFillHoles,
-									multibandPadding,
-									multibandBestScoreThr,
-									multibandAngleHardthr,
-									multibandForceVisible))
-							{
-								printf("MultiBand texturing...done (%fs).\n", timer.ticks());
-							}
-							else
-							{
-								printf("MultiBand texturing...failed! (%fs)\n", timer.ticks());
-							}
-						}
 					}
 				}
 			}
@@ -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	bd99ac1007	merged master	2024-11-30 19:49:10 -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	0a9f0bfe4d	CI: only build humble in humble-devel branch	2024-11-30 18:32:29 -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
matlabbe	17c2142c98	iOS: updated settings information (#1355 )	2024-10-13 14:10:29 -07:00
matlabbe	465ea85d91	Removed grid_map as public dependency	2024-10-08 20:01:38 -07:00