Merge branch 'master' of github.com:introlab/rtabmap into compress_features_in_db

.github/workflows/cmake-linux.yml

@@ -0,0 +1,66 @@
+name: CMake-Linux
+
+on:
+  push:
+    branches: 
+      - master
+  pull_request:
+    branches: 
+      - '**'
+
+env:
+  BUILD_TYPE: Release
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
+  cancel-in-progress: ${{ github.event_name == 'pull_request' }}
+
+jobs:
+  build:
+    name: ${{ matrix.build_name }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      fail-fast: true
+      matrix:
+        build_name: [ubuntu-22.04, ubuntu-24.04, ubuntu-24.04-with-opengv]
+        include:
+        - build_name: ubuntu-22.04
+          os: ubuntu-22.04
+          extra_deps: "libunwind-dev libceres-dev"
+          extra_cmake_def: "-DWITH_CERES=ON"
+        - build_name: ubuntu-24.04
+          os: ubuntu-24.04
+          extra_deps: "libg2o-dev libceres-dev"
+          extra_cmake_def: "-DWITH_CERES=ON"
+        - build_name: ubuntu-24.04-with-opengv
+          os: ubuntu-24.04
+          extra_deps: "libg2o-dev libceres-dev"
+          extra_cmake_def: "-DWITH_CERES=ON -DBUILD_OPENGV=ON"
+
+    steps:
+    - uses: actions/checkout@v4
+
+    - name: Install Linux Dependencies
+      run: |
+        DEBIAN_FRONTEND=noninteractive
+        sudo apt-get update
+        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev ${{ matrix.extra_deps }}
+
+    - name: Configure CMake
+      run: |
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{ matrix.extra_cmake_def }}
+
+    - name: Build
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+    - name: Info
+      working-directory: ${{github.workspace}}/build/bin
+      run: |
+        ./rtabmap-console --version
+
+#    - name: Test
+#      working-directory: ${{github.workspace}}/build
+#      # Execute tests defined by the CMake configuration.  
+#      # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail
+#      run: ctest -C ${{env.BUILD_TYPE}}
+      

.github/workflows/cmake-ros.yml

@@ -22,7 +22,7 @@ jobs:
     # well on Windows or Mac.  You can convert this to a matrix build if you need
     # cross-platform coverage.
     # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
-    name: Build on ros ${{ matrix.ros_distribution }} and ${{ matrix.os }}
+    name: ${{ matrix.ros_distribution }}-${{ matrix.os }}
     runs-on: ${{ matrix.os }}
     strategy:
       fail-fast: false

.github/workflows/cmake-windows.yml

@@ -1,4 +1,4 @@
-name: CMake
+name: CMake-Windows
 
 on:
   push:
@@ -22,20 +22,8 @@ jobs:
     strategy:
       fail-fast: true
       matrix:
-        build_name: [ubuntu-22.04, ubuntu-24.04, ubuntu-24.04-with-opengv, windows-2022, windows-2022-cuda]
+        build_name: [windows-2022, windows-2022-cuda]
         include:
-        - build_name: ubuntu-22.04
-          os: ubuntu-22.04
-          extra_deps: "libunwind-dev libceres-dev"
-          extra_cmake_def: "-DWITH_CERES=ON"
-        - build_name: ubuntu-24.04
-          os: ubuntu-24.04
-          extra_deps: "libg2o-dev libceres-dev"
-          extra_cmake_def: "-DWITH_CERES=ON"
-        - build_name: ubuntu-24.04-with-opengv
-          os: ubuntu-24.04
-          extra_deps: "libg2o-dev libceres-dev"
-          extra_cmake_def: "-DWITH_CERES=ON -DBUILD_OPENGV=ON"
         - build_name: windows-2022
           os: windows-2022
           extra_deps: ""
@@ -56,15 +44,7 @@ jobs:
       if: matrix.build_name == 'windows-2022-cuda'
       uses: ./.github/actions/install-windows-cuda-deps
 
-    - name: Install Linux Dependencies
-      if: matrix.os != 'windows-2022'
-      run: |
-        DEBIAN_FRONTEND=noninteractive
-        sudo apt-get update
-        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev ${{ matrix.extra_deps }}
-
     - name: Configure CMake
-      if: matrix.os == 'windows-2022'
       run: |
         cmake `
           -B ${{github.workspace}}/build `
@@ -76,16 +56,10 @@ jobs:
           -DCMAKE_TOOLCHAIN_FILE=${{env.VCPKG_EXPORT_PATH}}/scripts/buildsystems/vcpkg.cmake `
           -DTorch_DIR=${{env.VCPKG_EXPORT_PATH}}/installed/x64-windows-release/tools/python3/Lib/site-packages/torch/share/cmake/Torch
 
-    - name: Configure CMake
-      if: matrix.os != 'windows-2022'
-      run: |
-        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{ matrix.extra_cmake_def }}
-
     - name: Build
       run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
 
     - name: Build Windows Package
-      if: matrix.os == 'windows-2022'
       shell: pwsh
       run: |
         if ("${{ github.event_name }}" -eq "pull_request") {
@@ -105,7 +79,6 @@ jobs:
         ./rtabmap-console --version
 
     - name: Upload RTABMap Artifacts (ZIP)
-      if: matrix.os == 'windows-2022'
       uses: actions/upload-artifact@v4
       with:
         name: RTABMap-Binaries-${{ matrix.build_name }}-zip
@@ -116,7 +89,7 @@ jobs:
         retention-days: ${{ github.event_name == 'pull_request' && 1 || 90 }}
 
     - name: Upload RTABMap Artifacts (Installer)
-      if: matrix.os == 'windows-2022' && github.event_name != 'pull_request'
+      if: github.event_name != 'pull_request'
       uses: actions/upload-artifact@v4
       with:
         name: RTABMap-Binaries-${{ matrix.build_name }}-exe

README.md

@@ -7,7 +7,7 @@ rtabmap
 [![Downloads][downloads-image]][downloads]
 [![License][license-image]][license]
 
-[release-image]: https://img.shields.io/badge/release-0.21.4-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-0.23.1-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases
 
 [downloads-image]: https://img.shields.io/github/downloads/introlab/rtabmap/total?label=downloads
@@ -35,13 +35,7 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
   <table>
     <tbody>
         <tr>
-           <td>Linux</td>
+           <td><a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-linux.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-linux.yml/badge.svg" alt="CMake Linux Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-windows.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-windows.yml/badge.svg" alt="CMake Windows Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg" alt="CMake ROS Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/docker.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg" alt="Docker Build Status"/>
-           <td><a href="https://github.com/introlab/rtabmap/actions/workflows/cmake.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/docker.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg" alt="Build Status"/>
-           </td>
-        </tr>
-        <tr>
-           <td>Windows</td>
-           <td><a href="https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master"><img src="https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true" alt="Build Status"/>
            </td>
         </tr>
      </tbody>
@@ -59,7 +53,7 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
             <td><a href="http://build.ros.org/job/Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary" alt="Build Status"/></td>
         </tr>
         <tr>
-            <td rowspan="3">ROS 2</td>
+            <td rowspan="4">ROS 2</td>
             <td>Humble</td>
             <td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
         </tr>
@@ -67,6 +61,10 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
             <td>Jazzy</td>
             <td><a href="http://build.ros2.org/job/Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
         </tr>
+        <tr>
+            <td>Kilted</td>
+            <td><a href="http://build.ros2.org/job/Kbin_uN64__rtabmap__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Kbin_uN64__rtabmap__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
+        </tr>
         <tr>
             <td>Rolling</td>
             <td><a href="http://build.ros2.org/job/Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>

corelib/include/rtabmap/core/Features2d.h

@@ -309,7 +309,8 @@ private:
 	bool preciseUpscale_;
 	bool rootSIFT_;
 	bool gpu_;
-	float guaussianThreshold_;
+	float gaussianThreshold_;
+	float maxGaussianThreshold_;
 	bool upscale_;
 
 	cv::Ptr<CV_SIFT> sift_;

corelib/include/rtabmap/core/Graph.h

@@ -277,7 +277,8 @@ std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
 		bool lookInDatabase = true,
 		bool updateNewCosts = false,
 		float linearVelocity = 0.0f,   // m/sec
-		float angularVelocity = 0.0f); // rad/sec
+		float angularVelocity = 0.0f,  // rad/sec
+		bool ignoreDirectLinks = false); 
 
 /**
  * Find the nearest node of the target pose
@@ -336,9 +337,7 @@ RTABMAP_DEPRECATED std::map<int, Transform> RTABMAP_CORE_EXPORT getPosesInRadius
 RTABMAP_DEPRECATED std::map<int, Transform> RTABMAP_CORE_EXPORT getPosesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f);
 
 float RTABMAP_CORE_EXPORT computePathLength(
-		const std::vector<std::pair<int, Transform> > & path,
+		const std::vector<std::pair<int, Transform> > & path);
-		unsigned int fromIndex = 0,
-		unsigned int toIndex = 0);
 
 // assuming they are all linked in map order
 float RTABMAP_CORE_EXPORT computePathLength(

corelib/include/rtabmap/core/Memory.h

@@ -144,6 +144,7 @@ public:
 	void saveLocationData(int locationId);
 	void removeLink(int idA, int idB);
 	void removeRawData(int id, bool image = true, bool scan = true, bool userData = true);
+	int reduceNode(int id, float maxDistance = 0.0f, bool keepLinkedInDb = false, int direction = 0);
 
 	//getters
 	const std::map<int, double> & getWorkingMem() const {return _workingMem;}
@@ -277,6 +278,7 @@ private:
 	void initCountId();
 	void rehearsal(Signature * signature, Statistics * stats = 0);
 	bool rehearsalMerge(int oldId, int newId);
+	bool canBeReduced(const Link & link, float maxDistance, int direction);
 
 	const std::map<int, Signature*> & getSignatures() const {return _signatures;}
 

corelib/include/rtabmap/core/Parameters.h

@@ -224,7 +224,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Mem, BadSignaturesIgnored,        bool, false,    "Bad signatures are ignored.");
     RTABMAP_PARAM(Mem, InitWMWithAllNodes,          bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
     RTABMAP_PARAM(Mem, DepthAsMask,                 bool, true,     "Use depth image as mask when extracting features for vocabulary.");
-    RTABMAP_PARAM(Mem, DepthMaskFloorThr,           float, 0.0,     uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kMemDepthAsMask().c_str()));
+    RTABMAP_PARAM(Mem, DepthMaskFloorThr,           float, 0.0,     uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled. Ignored if %s is false.", kMemDepthAsMask().c_str()));
     RTABMAP_PARAM(Mem, StereoFromMotion,            bool, false,    uFormat("Triangulate features without depth using stereo from motion (odometry). It would be ignored if %s is true and the feature detector used supports masking.", kMemDepthAsMask().c_str()));
     RTABMAP_PARAM(Mem, ImagePreDecimation,          unsigned int, 1, uFormat("Decimation of the RGB image before visual feature detection. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.",kMemDepthAsMask().c_str()));
     RTABMAP_PARAM(Mem, ImagePostDecimation,         unsigned int, 1, uFormat("Decimation of the RGB image before saving it to database. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. Decimation is done from the original image. If set to same value than %s, data already decimated is saved (no need to re-decimate the image).", kMemImagePreDecimation().c_str()));
@@ -294,7 +294,8 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(SIFT, PreciseUpscale,    bool, false,  "Whether to enable precise upscaling in the scale pyramid (OpenCV >= 4.8).");
     RTABMAP_PARAM(SIFT, RootSIFT,          bool, false,  "Apply RootSIFT normalization of the descriptors.");
     RTABMAP_PARAM(SIFT, Gpu,               bool, false,  "CudaSift: Use GPU version of SIFT. This option is enabled only if RTAB-Map is built with CudaSift dependency and GPUs are detected.");
-    RTABMAP_PARAM(SIFT, GaussianThreshold, float, 2.0,   "CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features are produced by the detector.");
+    RTABMAP_PARAM(SIFT, GaussianThreshold, float, 2.0,   "CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features with low response/hessian are produced by the detector.");
+    RTABMAP_PARAM(SIFT, MaxGaussianThreshold, float, 0.0,   uFormat("CudaSift: Maximum threshold on difference of Gaussians for feature pruning (ignored if smaller or equal than %s). The lower the threshold, the less features with high response/hessian are produced by the detector.", kSIFTGaussianThreshold().c_str()));
     RTABMAP_PARAM(SIFT, Upscale,           bool, false,  "CudaSift: Whether to enable upscaling.");
 
     RTABMAP_PARAM(BRIEF, Bytes,            int, 32,      "Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");
@@ -724,7 +725,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,     "Max depth of the features (0 means no limit).");
     RTABMAP_PARAM(Vis, MinDepth,                  float, 0,     "Min depth of the features (0 means no limit).");
     RTABMAP_PARAM(Vis, DepthAsMask,               bool,  true,  "Use depth image as mask when extracting features.");
-    RTABMAP_PARAM(Vis, DepthMaskFloorThr,         float, 0.0,    uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kVisDepthAsMask().c_str()));
+    RTABMAP_PARAM(Vis, DepthMaskFloorThr,         float, 0.0,    uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled. Ignored if %s is false.", kVisDepthAsMask().c_str()));
     RTABMAP_PARAM_STR(Vis, RoiRatios,        "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
     RTABMAP_PARAM(Vis, SubPixWinSize,             int,   3,     "See cv::cornerSubPix().");
     RTABMAP_PARAM(Vis, SubPixIterations,          int,   0,     "See cv::cornerSubPix(). 0 disables sub pixel refining.");

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

@@ -102,10 +102,11 @@ public:
 	}
 
 	// 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV, 12=rgbd_bonn
-	void setGroundTruthPath(const std::string & filePath, int format = 0)
+	void setGroundTruthPath(const std::string & filePath, int format = 0, const Transform & localTransform = Transform::getIdentity())
 	{
 		_groundTruthPath = filePath;
 		_groundTruthFormat = format;
+		_groundTruthLocalTransform = localTransform;
 	}
 
 	void setMaxPoseTimeDiff(double diff) {_maxPoseTimeDiff = diff;}
@@ -164,6 +165,7 @@ private:
 	int _odometryFormat;
 	std::string _groundTruthPath;
 	int _groundTruthFormat;
+	Transform _groundTruthLocalTransform;
 	double _maxPoseTimeDiff;
 
 	std::list<double> _stamps;

corelib/src/CameraModel.cpp

@@ -353,6 +353,22 @@ bool CameraModel::load(const std::string & filePath)
 						data[0], data[1], data[2], data[3],
 						data[4], data[5], data[6], data[7],
 						data[8], data[9], data[10], data[11]);
+				Transform detCheck = localTransform_.clone();
+				localTransform_.normalizeRotation(); /// Normalize by default
+				float det = detCheck.toEigen3f().linear().determinant();
+				if(fabs(det - 1.0f) > 0.0001)
+				{
+					std::stringstream streamBefore, streamAfter;
+					streamBefore << detCheck << std::endl;
+					streamAfter << localTransform_ << std::endl;
+					UWARN("The camera model's local_transform from \"%s\" doesn't "
+						"have a normalized rotation matrix (dertminant=%f). We will normalize "
+						"it for convenience.\nWas:\n%sNow\n%s",
+						filePath.c_str(),
+						det,
+						streamBefore.str().c_str(),
+						streamAfter.str().c_str());
+				}
 			}
 			else
 			{

corelib/src/DBDriverSqlite3.cpp

@@ -4461,14 +4461,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
 		ULOGGER_DEBUG("Update Node table, Time=%fs", timer.ticks());
 
 		// Update links part1
-		if(uStrNumCmp(_version, "0.18.3") >= 0)
+		query = uFormat("DELETE FROM Link WHERE from_id=?;");
-		{
-			query = uFormat("DELETE FROM Link WHERE from_id=? and type!=%d;", (int)Link::kLandmark);
-		}
-		else
-		{
-			query = uFormat("DELETE FROM Link WHERE from_id=?;");
-		}
 		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 		for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
@@ -4503,6 +4496,12 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
 				{
 					stepLink(ppStmt, i->second);
 				}
+				// Save landmarks
+				const std::map<int, Link> & landmarks = (*j)->getLandmarks();
+				for(std::map<int, Link>::const_iterator i=landmarks.begin(); i!=landmarks.end(); ++i)
+				{
+					stepLink(ppStmt, i->second);
+				}
 			}
 		}
 		// Finalize (delete) the statement

corelib/src/Features2d.cpp

@@ -466,7 +466,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
 				minimumHessian = iter->first;
 			}
 		}
-		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, maxKeypoints, minimumHessian);
+		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size()-removed, minimumHessian);
 		ULOGGER_DEBUG("filter keypoints time = %f s", timer.ticks());
 	}
 	else
@@ -1251,7 +1251,8 @@ SIFT::SIFT(const ParametersMap & parameters) :
 	preciseUpscale_(Parameters::defaultSIFTPreciseUpscale()),
 	rootSIFT_(Parameters::defaultSIFTRootSIFT()),
 	gpu_(Parameters::defaultSIFTGpu()),
-	guaussianThreshold_(Parameters::defaultSIFTGaussianThreshold()),
+	gaussianThreshold_(Parameters::defaultSIFTGaussianThreshold()),
+	maxGaussianThreshold_(Parameters::defaultSIFTMaxGaussianThreshold()),
 	upscale_(Parameters::defaultSIFTUpscale()),
 	cudaSiftData_(0),
 	cudaSiftMemory_(0),
@@ -1284,23 +1285,25 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kSIFTPreciseUpscale(), preciseUpscale_);
 	Parameters::parse(parameters, Parameters::kSIFTRootSIFT(), rootSIFT_);
 	Parameters::parse(parameters, Parameters::kSIFTGpu(), gpu_);
-	Parameters::parse(parameters, Parameters::kSIFTGaussianThreshold(), guaussianThreshold_);
+	Parameters::parse(parameters, Parameters::kSIFTGaussianThreshold(), gaussianThreshold_);
+	Parameters::parse(parameters, Parameters::kSIFTMaxGaussianThreshold(), maxGaussianThreshold_);
 	Parameters::parse(parameters, Parameters::kSIFTUpscale(), upscale_);
 	
 	if(gpu_)
 	{
 #ifdef RTABMAP_CUDASIFT
 		// Check if there is a cuda device
-		if(InitCuda(0, ULogger::level() == ULogger::kDebug)) {
+		if(cudaSiftData_==0)
-			UDEBUG("Init SiftData");
+		{
-			if(cudaSiftData_ == 0) {
+			if(InitCuda(0, ULogger::level() == ULogger::kDebug)) {
+				UDEBUG("Init SiftData");
 				cudaSiftData_ = new SiftData();
 				InitSiftData(*cudaSiftData_, 8192, true, true);
 			}
-		}
+			else{
-		else{
+				UWARN("No cuda device(s) detected, CudaSift is not available! Using SIFT CPU version instead.");
-			UWARN("No cuda device(s) detected, CudaSift is not available! Using SIFT CPU version instead.");
+				gpu_ = false;
-			gpu_ = false;
+			}
 		}
 #else
 		UWARN("RTAB-Map is not built with CudaSift so %s cannot be used!", Parameters::kSIFTGpu().c_str());
@@ -1363,7 +1366,7 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 			numOctaves = 7; // hard-coded limit in CudaSift
 		}
 		float initBlur = sigma_; /* Amount of initial Gaussian blurring in standard deviations */
-		float thresh = guaussianThreshold_;   /* Threshold on difference of Gaussians for feature pruning */
+		float thresh = gaussianThreshold_;   /* Threshold on difference of Gaussians for feature pruning */
 		float edgeLimit = edgeThreshold_;   
 		float minScale = 0.0f; /* Minimum acceptable scale to remove fine-scale features */
 		UDEBUG("numOctaves=%d initBlur=%f thresh=%f edgeLimit=%f minScale=%f upScale=%s w=%d h=%d", numOctaves, initBlur, thresh, edgeLimit, minScale, upscale_?"true":"false", w, h);
@@ -1388,15 +1391,9 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 		cudaSiftDescriptors_ = cv::Mat();
 		if(cudaSiftData_->numPts)
 		{
-			int maxKeypoints = this->getMaxFeatures();
+			keypoints.resize(cudaSiftData_->numPts);
-			if(maxKeypoints == 0 || maxKeypoints > cudaSiftData_->numPts)
+			cudaSiftDescriptors_ = cv::Mat(cudaSiftData_->numPts, 128, CV_32FC1);
-			{
+			size_t k=0;
-				maxKeypoints = cudaSiftData_->numPts;
-			}
-
-			// Re-using same implementation of limitKeypoints() directly here to avoid doubling memory copies
-			// Sort words by hessian
-			std::multimap<float, int> hessianMap; // <hessian,id>
 			for(int i=0; i<cudaSiftData_->numPts; ++i)
 			{
 				// Ignore keypoints with invalid descriptors
@@ -1413,29 +1410,40 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 					continue;
 				}
 
-				//Keep track of the data, to be easier to manage the data in the next step
+				if(i>0 && 
-				hessianMap.insert(std::pair<float, int>(abs(cudaSiftData_->h_data[i].sharpness), i));
+					cudaSiftData_->h_data[i].subsampling == cudaSiftData_->h_data[i-1].subsampling &&
-			}
+					fabs(cudaSiftData_->h_data[i].xpos-cudaSiftData_->h_data[i-1].xpos) +
+					fabs(cudaSiftData_->h_data[i].xpos-cudaSiftData_->h_data[i-1].ypos) < 0.1f)
+				{
+					// Same feature, skip doubles
+					continue;
+				}
 
-			if((int)hessianMap.size() < maxKeypoints)
+				float response = abs(cudaSiftData_->h_data[i].sharpness);
-			{
+				if(maxGaussianThreshold_>gaussianThreshold_ && response > maxGaussianThreshold_)
-				maxKeypoints = hessianMap.size();
+				{
-			}
+					continue;
+				}
 
-			std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
-			keypoints.resize(maxKeypoints);
-			cudaSiftDescriptors_ = cv::Mat(maxKeypoints, 128, CV_32FC1);
-			for(unsigned int k=0; k<keypoints.size() && iter!=hessianMap.rend(); ++k, ++iter)
-			{
-				int i = iter->second;
-				float *desc = cudaSiftData_->h_data[i].data;
 				cv::Mat(1, 128, CV_32FC1, desc).copyTo(cudaSiftDescriptors_.row(k));
 				keypoints[k].pt.x = cudaSiftData_->h_data[i].xpos;
 				keypoints[k].pt.y = cudaSiftData_->h_data[i].ypos;
 				keypoints[k].size = 2.0f*cudaSiftData_->h_data[i].scale; // x2 because the scale is more like a radius than a diameter, see CudaSift's ExtractSiftDescriptors function to see how they convert scale to patch size
 				keypoints[k].angle = cudaSiftData_->h_data[i].orientation;
-				keypoints[k].response = abs(cudaSiftData_->h_data[i].sharpness); 
+				keypoints[k].response = response; 
 				keypoints[k].octave = log2(cudaSiftData_->h_data[i].subsampling)-(upscale_?1:0);
+				++k;
+			}
+			if(k < keypoints.size())
+			{
+				UDEBUG("keypoints extracted = %d, valid=%d", keypoints.size(), k);
+				keypoints.resize(k);
+				cudaSiftDescriptors_.resize(k);
+			}
+			if(this->getMaxFeatures() != 0 && this->getMaxFeatures() < (int)keypoints.size())
+			{
+				// Call limitKeypoints() now to filter the descriptors.
+				this->limitKeypoints(keypoints, cudaSiftDescriptors_, this->getMaxFeatures(), cv::Size(w,h), this->getSSC());
 			}
 		}
 	}
@@ -1457,12 +1465,13 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 
 cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
+	cv::Mat descriptors;
 #ifdef RTABMAP_CUDASIFT
 	if(gpu_)
 	{
 		if((int)keypoints.size() == cudaSiftDescriptors_.rows)
 		{
-			return cudaSiftDescriptors_.clone();
+			descriptors = cudaSiftDescriptors_.clone();
 		}
 		else
 		{
@@ -1470,19 +1479,25 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 			return cv::Mat();
 		}
 	}
+	else
+	{
 #endif
 
-	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+		UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
-	cv::Mat descriptors;
 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 #ifdef RTABMAP_NONFREE
-	sift_->compute(image, keypoints, descriptors);
+		sift_->compute(image, keypoints, descriptors);
 #else
-	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
+		UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
 #else // >=4.4, >=3.4.11
-	sift_->compute(image, keypoints, descriptors);
+		sift_->compute(image, keypoints, descriptors);
 #endif
+
+#ifdef RTABMAP_CUDASIFT
+	}
+#endif
+
 	if( rootSIFT_ && !descriptors.empty())
 	{
 		UDEBUG("Performing RootSIFT...");

corelib/src/FlannIndex.cpp

@@ -96,7 +96,7 @@ std::vector<unsigned char> FlannIndex::serializeIndex(bool computeChecksum) cons
 #else
 		UTimer timer;
 		const int headerSizeBytes = sizeof(int)*FLANN_INDEX_HEADER_SIZE;
-		std::vector<unsigned char> indexData(1024*1024*100 + headerSizeBytes); // Max 100 MB
+		std::vector<unsigned char> indexData(1024*1024*1024 + headerSizeBytes); // Max 1 GB
         FILE* indexDataPtr = fmemopen(indexData.data()+headerSizeBytes, indexData.size() - headerSizeBytes, "wb");
 		long bytes_written = 0;
         if (indexDataPtr) {

corelib/src/Graph.cpp

@@ -2020,19 +2020,21 @@ std::list<std::pair<int, Transform> > computePath(
 		bool lookInDatabase,
 		bool updateNewCosts,
 		float linearVelocity,  // m/sec
-		float angularVelocity) // rad/sec
+		float angularVelocity, // rad/sec
+		bool ignoreDirectLinks) 
 {
 	UASSERT(memory!=0);
 	UASSERT(fromId>=0);
 	UASSERT(toId!=0);
 	std::list<std::pair<int, Transform> > path;
-	UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f",
+	UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f ignoreDirectLinks=%d",
 			fromId,
 			toId,
 			lookInDatabase?1:0,
 			updateNewCosts?1:0,
 			linearVelocity,
-			angularVelocity);
+			angularVelocity,
+			ignoreDirectLinks?1:0);
 
 	std::multimap<int, Link> allLinks;
 	if(lookInDatabase)
@@ -2110,7 +2112,9 @@ std::list<std::pair<int, Transform> > computePath(
 		}
 		for(std::multimap<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
 		{
-			if(iter->second.from() != iter->second.to())
+			if(iter->second.from() != iter->second.to() &&
+			  (!ignoreDirectLinks || 
+				(!(iter->second.from()==fromId && iter->second.to()==toId) && !(iter->second.to()==fromId && iter->second.from()==toId))))
 			{
 				Transform nextPose = currentNode->pose()*iter->second.transform();
 				float cost = 0.0f;
@@ -2396,26 +2400,15 @@ std::map<int, Transform> getPosesInRadius(const Transform & targetPose, const st
 
 
 float computePathLength(
-		const std::vector<std::pair<int, Transform> > & path,
+		const std::vector<std::pair<int, Transform> > & path)
-		unsigned int fromIndex,
-		unsigned int toIndex)
 {
 	float length = 0.0f;
 	if(path.size() > 1)
 	{
-		UASSERT(fromIndex  < path.size() && toIndex < path.size() && fromIndex <= toIndex);
+		for(unsigned int i=0; i<path.size()-1; ++i)
-		if(fromIndex >= toIndex)
 		{
-			toIndex = (unsigned int)path.size()-1;
+			length+=path[i].second.getDistance(path[i+1].second);
 		}
-		float x=0, y=0, z=0;
-		for(unsigned int i=fromIndex; i<toIndex-1; ++i)
-		{
-			x += fabs(path[i].second.x() - path[i+1].second.x());
-			y += fabs(path[i].second.y() - path[i+1].second.y());
-			z += fabs(path[i].second.z() - path[i+1].second.z());
-		}
-		length = sqrt(x*x + y*y + z*z);
 	}
 	return length;
 }
@@ -2426,19 +2419,15 @@ float computePathLength(
 	float length = 0.0f;
 	if(path.size() > 1)
 	{
-		float x=0, y=0, z=0;
 		std::map<int, Transform>::const_iterator iter=path.begin();
 		Transform previousPose = iter->second;
 		++iter;
 		for(; iter!=path.end(); ++iter)
 		{
 			const Transform & currentPose = iter->second;
-			x += fabs(previousPose.x() - currentPose.x());
+			length+=previousPose.getDistance(currentPose);
-			y += fabs(previousPose.y() - currentPose.y());
-			z += fabs(previousPose.z() - currentPose.z());
 			previousPose = currentPose;
 		}
-		length = sqrt(x*x + y*y + z*z);
 	}
 	return length;
 }

corelib/src/IMUThread.cpp

@@ -135,8 +135,20 @@ void IMUThread::mainLoop()
 		std::stringstream stream(line);
 		std::string s;
 		std::getline(stream, s, ',');
-		std::string nanoseconds = s.substr(s.size() - 9, 9);
+
-		std::string seconds = s.substr(0, s.size() - 9);
+		double stamp = 0.0;
+		if(s.find('.') != std::string::npos)
+		{
+			// Normal [epoch] timestamp
+			stamp = uStr2Double(s);
+		}
+		else
+		{
+			// Assume EuRoC format
+			std::string nanoseconds = s.substr(s.size() - 9, 9);
+			std::string seconds = s.substr(0, s.size() - 9);
+			stamp = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9;
+		}
 
 		cv::Vec3d gyr;
 		for (int j = 0; j < 3; ++j) {
@@ -150,7 +162,6 @@ void IMUThread::mainLoop()
 			acc[j] = uStr2Double(s);
 		}
 
-		double stamp = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9;
 		if(previousStamp_>0 && stamp > previousStamp_)
 		{
 			captureDelay_ = stamp - previousStamp_;

corelib/src/Memory.cpp

@@ -1288,114 +1288,180 @@ void Memory::addSignatureToWmFromLTM(Signature * signature)
 	}
 }
 
-void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
+bool Memory::canBeReduced(const Link & link, float maxDistance, int direction)
 {
-	UDEBUG("Inserting node %d from STM in WM...", id);
+	return  link.to() != link.from() &&
-	UASSERT(_stMem.find(id) != _stMem.end());
+			link.type() != Link::kNeighbor &&
+			link.type() != Link::kNeighborMerged &&
+			link.userDataCompressed().empty() &&
+			link.type() != Link::kUndef &&
+			link.type() != Link::kVirtualClosure &&
+			(maxDistance == 0.0f || link.transform().getNorm() < maxDistance) &&
+			(direction == 0 || (direction==-1 && link.to() < link.from()) || (direction==1 && link.to() > link.from()));
+}
+
+int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb, int direction)
+{
+	UDEBUG("Reducing %d (max distance=%f, keep linked in db=%s, direction=%d)",
+		id, maxDistance, keepLinkedInDb?"true":"false", direction);
 	Signature * s = this->_getSignature(id);
-	UASSERT(s!=0);
+	if(s==0)
-
-	if(_reduceGraph)
 	{
-		bool merge = false;
+		UWARN("Node %d is not in WM/STM, cannot reduce it.", id);
-		const std::multimap<int, Link> & links = s->getLinks();
+		return 0;
-		std::map<int, Link> neighbors;
+	}
-		for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
-		{
-			if(!merge)
-			{
-				merge = iter->second.to() < s->id() && // should be a parent->child link
-						iter->second.to() != iter->second.from() &&
-						iter->second.type() != Link::kNeighbor &&
-						iter->second.type() != Link::kNeighborMerged &&
-						iter->second.userDataCompressed().empty() &&
-						iter->second.type() != Link::kUndef &&
-						iter->second.type() != Link::kVirtualClosure;
-				if(merge)
-				{
-					UDEBUG("Reduce %d to %d", s->id(), iter->second.to());
-					if(reducedTo)
-					{
-						*reducedTo = iter->second.to();
-					}
-				}
 
-			}
+	if(!s->getLabel().empty())
-			if(iter->second.type() == Link::kNeighbor)
+	{
+		// We currently not remove nodes with labels
+		return 0;
+	}
+
+	std::multimap<int, Link> links = s->getLinks();
+	std::map<int, Link> neighbors;
+	int reducedTo = 0;
+	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		if(canBeReduced(iter->second, maxDistance, direction))
+		{
+			float distance = iter->second.transform().getNorm();
+			reducedTo = iter->second.to();
+			UDEBUG("Reduce %d to %d (distance=%f)",
+				s->id(), iter->second.to(), distance);
+		}
+		
+		if(iter->second.type() == Link::kNeighbor)
+		{
+			neighbors.insert(*iter);
+		}
+	}
+	if(reducedTo>0)
+	{
+		if(maxDistance > 0.0f)
+		{
+			// Only reduce if all neighbor merged links are also below maxDistance
+			for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 			{
-				neighbors.insert(*iter);
+				if( iter->second.type() == Link::kNeighborMerged &&
+					iter->second.transform().getNorm() > maxDistance)
+				{
+					return 0;
+				}
 			}
 		}
-		if(merge)
+
+		for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 		{
-			if(s->getLabel().empty())
+			Signature * sTo = this->_getSignature(iter->first);
+			if(sTo->id()!=s->id()) // Not Prior/Gravity links...
 			{
-				for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+				UASSERT_MSG(sTo!=0, uFormat("id=%d", iter->first).c_str());
+				sTo->removeLink(s->id());
+				if(iter->second.type() != Link::kNeighbor &&
+					iter->second.type() != Link::kUndef)
 				{
-					Signature * sTo = this->_getSignature(iter->first);
+					if(iter->second.type() == Link::kNeighborMerged)
-					if(sTo->id()!=s->id()) // Not Prior/Gravity links...
 					{
-						UASSERT_MSG(sTo!=0, uFormat("id=%d", iter->first).c_str());
+						s->removeLink(sTo->id());
-						sTo->removeLink(s->id());
+						if(maxDistance == 0.0f)
-						if(iter->second.type() != Link::kNeighbor &&
-						   iter->second.type() != Link::kNeighborMerged &&
-						   iter->second.type() != Link::kUndef)
 						{
-							// link to all neighbors
+							// online graph reduction, always skip these links
-							for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
+							continue;
+						}
+					}
+					// link to all neighbors
+					for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
+					{
+						if(!sTo->hasLink(jter->second.to()))
+						{
+							Link l = iter->second.inverse().merge(
+									jter->second,
+									iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type());
+							UDEBUG("Merging link %d->%d (type=%d) to with %d->%d (type %d). Adding %d->%d (type %d) to %d and %d",
+								iter->second.to(), iter->second.from(), iter->second.type(),
+								jter->second.from(), jter->second.to(), jter->second.type(),
+								l.from(), l.to(), l.type(), sTo->id(), l.to());
+							sTo->addLink(l);
+							Signature * sB = this->_getSignature(l.to());
+							UASSERT(sB!=0);
+							UASSERT_MSG(!sB->hasLink(l.from()), uFormat("%d->%d type=%d", sB->id(), l.to(), l.type()).c_str());
+							sB->addLink(l.inverse());
+						}
+					}
+					// link to all landmarks
+					for(std::map<int, Link>::const_iterator jter=s->getLandmarks().begin(); jter!=s->getLandmarks().end(); ++jter)
+					{
+						if(!uContains(sTo->getLandmarks(), jter->first))
+						{
+							UDEBUG("Move landmark observation %d from %d to %d",
+									jter->first, s->id(), sTo->id());
+							Link l = iter->second.inverse().merge(
+									jter->second,
+									jter->second.type());
+							sTo->addLandmark(l);
+							// Update landmark index
+							std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(jter->first);
+							if(nter!=_landmarksIndex.end())
 							{
-								if(!sTo->hasLink(jter->second.to()))
+								nter->second.insert(sTo->id());
-								{
+							}
-									UDEBUG("Merging link %d->%d (type=%d) to link %d->%d (type %d)",
+							else
-											iter->second.from(), iter->second.to(), iter->second.type(),
+							{
-											jter->second.from(), jter->second.to(), jter->second.type());
+								std::set<int> tmp;
-									Link l = iter->second.inverse().merge(
+								tmp.insert(sTo->id());
-											jter->second,
+								_landmarksIndex.insert(std::make_pair(jter->first, tmp));
-											iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type());
-									sTo->addLink(l);
-									Signature * sB = this->_getSignature(l.to());
-									UASSERT(sB!=0);
-									UASSERT_MSG(!sB->hasLink(l.from()), uFormat("%d->%d", sB->id(), l.to()).c_str());
-									sB->addLink(l.inverse());
-								}
 							}
 						}
 					}
 				}
+			}
+		}
 
-				//remove neighbor links
+		this->moveToTrash(s, keepLinkedInDb);
-				std::multimap<int, Link> linksCopy = links;
+		s = 0;
-				for(std::multimap<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
+		_linksChanged = true;
+		_memoryChanged = true;
+	}
+	return reducedTo;
+}
+
+void Memory::moveSignatureToWMFromSTM(int id, int * reducedToOut)
+{
+	UDEBUG("Inserting node %d from STM in WM...", id);
+	UASSERT(_stMem.find(id) != _stMem.end());
+	int reducedId = 0;
+	if(_reduceGraph)
+	{
+		Signature * s = this->_getSignature(id);
+		UASSERT(s!=0);
+		std::multimap<int, Link> links = s->getLinks();
+		// Setting true to make sure we save all visual
+		// words that could be referenced in a previously
+		// transferred node in LTM (#979)
+		reducedId = reduceNode(s->id(), 0, true);
+		if(reducedToOut) {
+			*reducedToOut = reducedId;
+		}
+		if(reducedId>0)
+		{
+			for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
+			{
+				if(iter->second.type() == Link::kNeighbor)
 				{
-					if(iter->second.type() == Link::kNeighborMerged)
+					if(_lastGlobalLoopClosureId == s->id())
 					{
-						// Removing only merged neighbor links, we keep original neighbor 
+						_lastGlobalLoopClosureId = iter->first;
-						// links to be able to reprocess databases with correct odometry covariance.
-						s->removeLink(iter->first);
-					}
-					if(iter->second.type() == Link::kNeighbor)
-					{
-						if(_lastGlobalLoopClosureId == s->id())
-						{
-							_lastGlobalLoopClosureId = iter->first;
-						}
 					}
 				}
-
-				// Setting true to make sure we save all visual
-				// words that could be referenced in a previously
-				// transferred node in LTM (#979)
-				this->moveToTrash(s, true);
-				s = 0;
 			}
 		}
 	}
-	if(s != 0)
+	if(reducedId == 0)
 	{
 		_workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now()));
 		_stMem.erase(*_stMem.begin());
 	}
-	// else already removed from STM/WM in moveToTrash()
+	// else already removed from STM/WM in reduceNode()
 }
 
 const Signature * Memory::getSignature(int id) const
@@ -2610,9 +2676,10 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 		// If not saved to database
 		if(!keepLinkedToGraph)
 		{
-			UASSERT_MSG(this->isInSTM(s->id()),
+			UASSERT_MSG(this->isInSTM(s->id()) || this->isInWM(s->id()),
 						uFormat("Deleting location (%d) outside the "
-								"STM is not implemented!", s->id()).c_str());
+								"WM/STM is not implemented! STM size=%ld WM size=%ld",
+								s->id(), this->getStMem().size(), this->getWorkingMem().size()).c_str());
 			const std::multimap<int, Link> & links = s->getLinks();
 			for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 			{
@@ -2623,7 +2690,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 					UASSERT_MSG(sTo!=0,
 								uFormat("A neighbor (%d) of the deleted location %d is "
 										"not found in WM/STM! Are you deleting a location "
-										"outside the STM?", iter->first, s->id()).c_str());
+										"outside the WM/STM?", iter->first, s->id()).c_str());
 
 					if(iter->first > s->id() && links.size()>1 && sTo->hasLink(s->id()))
 					{
@@ -2633,7 +2700,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 					}
 
 					// child
-					if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id() && s->getWeight()>0)
+					if(iter->second.type() == Link::kGlobalClosure && s->getWeight()>0)
 					{
 						sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight
 					}
@@ -5168,16 +5235,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					{
 						UASSERT(!decimatedData.cameraModels().empty());
 						UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
-						if(_maskFloorThreshold<0.0f)
+						depthMask = util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold);
-						{
-							cv::Mat depthBelow;
-							util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
-							depthMask = depthBelow;
-						}
-						else
-						{
-							depthMask = util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold);
-						}
 						UDEBUG("Masking floor done.");
 					}
 
@@ -5227,6 +5285,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			else
 			{
 				int oldMaxFeatures = _feature2D->getMaxFeatures();
+				bool oldSSC = _feature2D->getSSC();
 				UDEBUG("rawDescriptorsKept=%d, pose=%d, maxFeatures=%d, visMaxFeatures=%d", _rawDescriptorsKept?1:0, pose.isNull()?0:1, _feature2D->getMaxFeatures(), _visMaxFeatures);
 				ParametersMap tmpMaxFeatureParameter;
 				if(_rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures)
@@ -5234,6 +5293,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					// The total extracted features should match the number of features used for transformation estimation
 					UDEBUG("Changing temporary max features from %d to %d", _feature2D->getMaxFeatures(), _visMaxFeatures);
 					tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(_visMaxFeatures)));
+					tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpSSC(), uNumber2Str(_visSSC)));
 					_feature2D->parseParameters(tmpMaxFeatureParameter);
 				}
 
@@ -5244,6 +5304,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				if(tmpMaxFeatureParameter.size())
 				{
 					tmpMaxFeatureParameter.at(Parameters::kKpMaxFeatures()) = uNumber2Str(oldMaxFeatures);
+					tmpMaxFeatureParameter.at(Parameters::kKpSSC()) = uBool2Str(oldSSC);
 					_feature2D->parseParameters(tmpMaxFeatureParameter); // reset back
 				}
 				t = timer.ticks();
@@ -5444,8 +5505,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		bool ssc = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visSSC:_feature2D->getSSC();
 		if((int)keypoints.size() > maxFeatures)
 		{
-			if(data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)
+			if(data.cameraModels().size()>=1 || data.stereoCameraModels().size()>=1)
-				_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures, data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(), ssc);
+				_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures, data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(), data.cameraModels()[0].imageHeight()):cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(), data.stereoCameraModels()[0].left().imageHeight()), ssc);
 			else
 				_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
 		}
@@ -5678,13 +5739,17 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					UWARN("Ignored %s and %s parameters as they cannot be used for multi-cameras setup or uncalibrated camera.",
 							Parameters::kKpGridCols().c_str(), Parameters::kKpGridRows().c_str());
 				}
-				if(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
+				if(decimatedData.cameraModels().size()>=1 || decimatedData.stereoCameraModels().size()>=1 ||
-					data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)
+					data.cameraModels().size()>=1 || data.stereoCameraModels().size()>=1)
 				{
-					Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(),
+					Feature2D::limitKeypoints(
-						decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
+						keypoints,
-						decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
+						inliers,
-						data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
+						_feature2D->getMaxFeatures(),
+						decimatedData.cameraModels().size()?cv::Size(decimatedData.cameraModels()[0].imageWidth()*decimatedData.cameraModels().size(), decimatedData.cameraModels()[0].imageHeight()):
+						decimatedData.stereoCameraModels().size()?cv::Size(decimatedData.stereoCameraModels()[0].left().imageWidth()*decimatedData.stereoCameraModels().size(), decimatedData.stereoCameraModels()[0].left().imageWidth()):
+						data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(), data.cameraModels()[0].imageHeight()):
+						cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(), data.stereoCameraModels()[0].left().imageHeight()),
 						_feature2D->getSSC());
 				}
 				else

corelib/src/OdometryThread.cpp

@@ -188,7 +188,7 @@ void OdometryThread::addData(const SensorEvent & event)
 					"(%f), skipping that frame (imu buffer size=%ld). "
 					"When using async IMU, make sure IMU is published faster "
 					"than camera/lidar (assuming IMU latency is very small compared to camera/lidar)."
-					"Current camera/lidar delay is %fs.",
+					"Current camera/lidar delay with system time is %fs.",
 					event.data().stamp(), _oldestAsyncImuStamp, _imuBuffer.size(), UTimer::now() - event.data().stamp());
 				notify = false;
 			}
@@ -197,7 +197,7 @@ void OdometryThread::addData(const SensorEvent & event)
 					"(%f), skipping that frame (imu buffer size=%ld). "
 					"When using async IMU, make sure IMU is published faster "
 					"than camera/lidar (assuming IMU latency is very small compared to camera/lidar). "
-					"Current camera/lidar delay is %fs.",
+					"Current camera/lidar delay with system time is %fs.",
 					event.data().stamp(), _newestAsyncImuStamp, _imuBuffer.size(), UTimer::now() - event.data().stamp());
 				notify = false;
 			}

corelib/src/Optimizer.cpp

@@ -217,6 +217,10 @@ LinkIdKey(int id, Link::Type type) :
 		{
 			return true;
 		}
+		else if(k.type_ == Link::kNeighborMerged && type_ != Link::kNeighbor && type_ != Link::kNeighborMerged)
+		{
+			return false;
+		}
 		else
 		{
 			// normal link, sort by smallest to largest id
@@ -256,7 +260,7 @@ void Optimizer::getConnectedGraph(
 		}
 	}
 
-	while(nextPoses.size())
+	while(!nextPoses.empty())
 	{
 		// Fill up all nodes before landmarks
 		// For nodes, fill up all neightbor nodes before loop closure ones

corelib/src/RegistrationVis.cpp

@@ -447,16 +447,7 @@ Transform RegistrationVis::computeTransformationImpl(
 							{
 								UASSERT(!fromSignature.sensorData().cameraModels().empty());
 								UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
-								if(_maskFloorThreshold<0.0f)
+								depthMask = util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold);
-								{
-									cv::Mat depthBelow;
-									util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
-									depthMask = depthBelow;
-								}
-								else
-								{
-									depthMask = util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold);
-								}
 								UDEBUG("Masking floor done.");
 							}
 
@@ -817,16 +808,7 @@ Transform RegistrationVis::computeTransformationImpl(
 							{
 								UASSERT(!toSignature.sensorData().cameraModels().empty());
 								UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
-								if(_maskFloorThreshold<0.0f)
+								depthMask = util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold);
-								{
-									cv::Mat depthBelow;
-									util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
-									depthMask = depthBelow;
-								}
-								else
-								{
-									depthMask = util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold);
-								}
 								UDEBUG("Masking floor done.");
 							}
 

corelib/src/Rtabmap.cpp

@@ -5731,6 +5731,7 @@ int Rtabmap::detectMoreLoopClosures(
 
 		if(toFromMapId >=0)
 		{
+			size_t clustersBefore = clusters.size();
 			for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end();)
 			{
 				int mapId = uValue(mapIds, iter->first, 0);
@@ -5742,7 +5743,7 @@ int Rtabmap::detectMoreLoopClosures(
 					++iter;
 				}
 			}
-			UINFO("Looking for more loop closures: filtered %ld clusters for map session %d.", clusters.size(), toFromMapId);
+			UINFO("Looking for more loop closures: filtered %ld/%ld clusters for map session %d.", clustersBefore-clusters.size(), clustersBefore, toFromMapId);
 			if(clusters.empty())
 			{
 				UERROR("No clusters belong to mapId %d, aborting.", toFromMapId);
@@ -5750,6 +5751,33 @@ int Rtabmap::detectMoreLoopClosures(
 			}
 		}
 
+		if(_memory->getMaxStMemSize() > 1)
+		{
+			size_t clustersBefore = clusters.size();
+			for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end();)
+			{
+				if(abs(iter->first - iter->second) < _memory->getMaxStMemSize())
+				{
+					iter = clusters.erase(iter);
+				}
+				else
+				{
+					// compute path to know how far we are in terms of graph length
+					std::map<int, int> ids = _memory->getNeighborsId(iter->first, _memory->getMaxStMemSize(), -1, true, true, true);
+					if(ids.find(iter->second) != ids.end())
+					{
+						iter = clusters.erase(iter);
+					}
+					else
+					{
+						++iter;
+					}
+				}
+			}
+			UINFO("Looking for more loop closures: filtered %ld/%ld clusters for too close nodes (below %s=%d).",
+				clustersBefore-clusters.size(), clustersBefore, Parameters::kMemSTMSize().c_str(), _memory->getMaxStMemSize());
+		}
+
 		int i=0;
 		std::set<int> addedLinks;
 		for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter, ++i)

corelib/src/Transform.cpp

@@ -52,6 +52,26 @@ Transform::Transform(
 			r11, r12, r13, o14,
 			r21, r22, r23, o24,
 			r31, r32, r33, o34);
+
+	if( r11>0.0f || r12>0.0f || r13>0.0f ||
+		r21>0.0f || r22>0.0f || r23>0.0f ||
+		r31>0.0f || r32>0.0f || r33>0.0f)
+	{
+		Eigen::Matrix3f m;
+		m << r11, r12, r13,
+			r21, r22, r23,
+			r31, r32, r33;
+		float d = m.determinant();
+		if(fabs(d-1.0f) > 0.0001)
+		{
+			UWARN("Created transform doesn't have normalized rotation. Any transformation with this transform can cause unexpected results!"
+				" Determinant([%f %f %f;%f %f %f;%f %f %f])=%f",
+				r11, r12, r13,
+				r21, r22, r23,
+				r31, r32, r33,
+				d);
+		}
+	}
 }
 
 Transform::Transform(const cv::Mat & transformationMatrix)
@@ -509,6 +529,11 @@ Transform Transform::fromString(const std::string & string)
 					  numbers[4], numbers[5], numbers[6], numbers[7],
 					  numbers[8], numbers[9], numbers[10], numbers[11]);
 	}
+	// Always normalize
+	if(!t.isNull())
+	{
+		t.normalizeRotation();
+	}
 	return t;
 }
 

corelib/src/VWDictionary.cpp

@@ -573,6 +573,15 @@ void VWDictionary::update()
 				_removedIndexedWords.size() == 0 &&
 				_visualWords.size())
 		{
+			const int IMGIDX_SHIFT = 18;
+    		const int IMGIDX_ONE = (1 << IMGIDX_SHIFT); // a limit defined in https://github.com/opencv/opencv/blob/4.x/modules/features2d/src/matchers.cpp
+			if(_dataTree.rows >= IMGIDX_ONE)
+			{
+				UWARN("%s=%d is not a FLANN strategy and the number of words in the vocabulary (%d) is over %d (IMGIDX_ONE), so opencv may "
+					"assert on an IMGIDX_ONE check when adding new words. Use a FLANN strategy instead (%s<%d).",
+					Parameters::kKpNNStrategy().c_str(), _strategy, _dataTree.rows, IMGIDX_ONE, Parameters::kKpNNStrategy().c_str(), kNNBruteForce);
+			}
+
 			//just add not indexed words
 			int i = _dataTree.rows;
 			if(!_dataTree.empty()) {
@@ -1006,7 +1015,7 @@ std::list<int> VWDictionary::addNewWords(
 	if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k))
 	{
 		//Find nearest neighbors
-		UDEBUG("newPts.total()=%d ", descriptors.rows);
+		UDEBUG("newPts.total()=%d _strategy=%d", descriptors.rows, _strategy);
 
 		if(_strategy == kNNFlannNaive || _strategy == kNNFlannKdTree || _strategy == kNNFlannLSH)
 		{

corelib/src/camera/CameraImages.cpp

@@ -63,6 +63,7 @@ CameraImages::CameraImages() :
 		_syncImageRateWithStamps(true),
 		_odometryFormat(0),
 		_groundTruthFormat(0),
+		_groundTruthLocalTransform(Transform::getIdentity()),
 		_maxPoseTimeDiff(0.02),
 		_captureDelay(0.0)
 	{}
@@ -93,6 +94,7 @@ CameraImages::CameraImages(const std::string & path,
 	_syncImageRateWithStamps(true),
 	_odometryFormat(0),
 	_groundTruthFormat(0),
+	_groundTruthLocalTransform(Transform::getIdentity()),
 	_maxPoseTimeDiff(0.02),
 	_captureDelay(0.0)
 {
@@ -478,27 +480,43 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 		if(success && _odometryPath.size() && odometry_.empty())
 		{
 			success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat, _maxPoseTimeDiff);
+			if(!success)
+			{
+				UERROR("Failed to read odometry poses.");
+			}
+			
+			if(success)
+			{
+				for(size_t i=0; i<odometry_.size(); ++i)
+				{
+					// linear cov = 0.0001
+					cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1) * (i==0?9999.0:0.0001);
+					if(i!=0)
+					{
+						// angular cov = 0.000001
+						covariance.at<double>(3,3) *= 0.01;
+						covariance.at<double>(4,4) *= 0.01;
+						covariance.at<double>(5,5) *= 0.01;
+					}
+					covariances_.push_back(covariance);
+				}
+			}
 		}
 
 		if(success && _groundTruthPath.size())
 		{
 			success = readPoses(groundTruth_, _stamps, _groundTruthPath, _groundTruthFormat, _maxPoseTimeDiff);
-		}
+			if(!success)
-
-		if(!odometry_.empty())
-		{
-			for(size_t i=0; i<odometry_.size(); ++i)
 			{
-				// linear cov = 0.0001
+				UERROR("Failed to read ground truth poses.");
-				cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1) * (i==0?9999.0:0.0001);
+			}
-				if(i!=0)
+			else if(!_groundTruthLocalTransform.isIdentity())
+			{
+				Transform gtInv = _groundTruthLocalTransform.inverse();
+				for(auto pose: groundTruth_)
 				{
-					// angular cov = 0.000001
+					pose = pose*gtInv; // pose of base_link, assuming ground truth frame and base frame are rigidly fixed
-					covariance.at<double>(3,3) *= 0.01;
-					covariance.at<double>(4,4) *= 0.01;
-					covariance.at<double>(5,5) *= 0.01;
 				}
-				covariances_.push_back(covariance);
 			}
 		}
 	}
@@ -607,7 +625,7 @@ bool CameraImages::readPoses(
 		}
 		if(validPoses != (int)inOutStamps.size())
 		{
-			UWARN("%d valid poses of %d stamps", validPoses, (int)inOutStamps.size());
+			UWARN("%d/%ld valid poses of %ld stamps", validPoses, outputPoses.size(), inOutStamps.size());
 		}
 	}
 	else
@@ -756,32 +774,19 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 
 			if(_stamps.size())
 			{
-				stamp = _stamps.front();
+				UERROR("stamps cannot be used when startAt < 0");
-				_stamps.pop_front();
-				if(_stamps.size())
-				{
-					_captureDelay = _stamps.front() - stamp;
-				}
 			}
 			if(odometry_.size())
 			{
-				odometryPose = odometry_.front();
+				UERROR("odometry cannot be used when startAt < 0");
-				odometry_.pop_front();
-				if(covariances_.size())
-				{
-					covariance = covariances_.front();
-					covariances_.pop_front();
-				}
 			}
 			if(groundTruth_.size())
 			{
-				groundTruthPose = groundTruth_.front();
+				UERROR("groundTruth cannot be used when startAt < 0");
-				groundTruth_.pop_front();
 			}
 			if(_models.size() && !model.isValidForProjection())
 			{
-				model = _models.front();
+				UERROR("models cannot be used when startAt < 0");
-				_models.pop_front();
 			}
 		}
 		else
@@ -792,6 +797,7 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 			{
 				imageFilePath = _path + imageFileName;
 				scanFilePath = _scanPath + scanFileName;
+				size_t stampsSize = _stamps.size();
 				if(_stamps.size())
 				{
 					stamp = _stamps.front();
@@ -803,6 +809,8 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 				}
 				if(odometry_.size())
 				{
+					UASSERT_MSG(stampsSize==0 || stampsSize == odometry_.size(), 
+						uFormat("Stamps=%ld odometry=%ld", _stamps.size(), odometry_.size()).c_str());
 					odometryPose = odometry_.front();
 					odometry_.pop_front();
 					if(covariances_.size())
@@ -813,11 +821,15 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 				}
 				if(groundTruth_.size())
 				{
+					UASSERT_MSG(stampsSize==0 || stampsSize == groundTruth_.size(), 
+						uFormat("Stamps=%ld groundTruth=%ld", _stamps.size(), groundTruth_.size()).c_str());
 					groundTruthPose = groundTruth_.front();
 					groundTruth_.pop_front();
 				}
 				if(_models.size() && !model.isValidForProjection())
 				{
+					UASSERT_MSG(stampsSize==0 || stampsSize == _models.size(), 
+						uFormat("Stamps=%ld models=%ld", _stamps.size(), _models.size()).c_str());
 					model = _models.front();
 					_models.pop_front();
 				}
@@ -834,6 +846,7 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 
 					imageFilePath = _path + imageFileName;
 					scanFilePath = _scanPath + scanFileName;
+					size_t stampsSize = _stamps.size();
 					if(_stamps.size())
 					{
 						stamp = _stamps.front();
@@ -845,6 +858,8 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 					}
 					if(odometry_.size())
 					{
+						UASSERT_MSG(stampsSize==0 || stampsSize == odometry_.size(), 
+							uFormat("Stamps=%ld odometry=%ld", stampsSize, odometry_.size()).c_str());
 						odometryPose = odometry_.front();
 						odometry_.pop_front();
 						if(covariances_.size())
@@ -855,11 +870,15 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 					}
 					if(groundTruth_.size())
 					{
+						UASSERT_MSG(stampsSize==0 || stampsSize == groundTruth_.size(), 
+							uFormat("Stamps=%ld groundTruth=%ld", _stamps.size(), groundTruth_.size()).c_str());
 						groundTruthPose = groundTruth_.front();
 						groundTruth_.pop_front();
 					}
 					if(_models.size() && !model.isValidForProjection())
 					{
+						UASSERT_MSG(stampsSize==0 || stampsSize == _models.size(), 
+							uFormat("Stamps=%ld models=%ld", _stamps.size(), _models.size()).c_str());
 						model = _models.front();
 						_models.pop_front();
 					}

corelib/src/odometry/OdometryORBSLAM3.cpp

@@ -430,7 +430,14 @@ Transform OdometryORBSLAM3::computeTransform(
 		(data.stereoCameraModels().size() == 1 &&
 			data.stereoCameraModels()[0].isValidForProjection())))
 	{
-		UERROR("Invalid camera model!");
+		if(data.cameraModels().size() > 1 || data.stereoCameraModels().size() > 1)
+		{
+			UERROR("Multi-camera not supported with ORB_SLAM integration!");
+		}
+		else
+		{
+			UERROR("Invalid camera model!");
+		}
 		return t;
 	}
 

corelib/src/optimizer/OptimizerG2O.cpp

@@ -61,7 +61,6 @@ typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> Matr
 #include "g2o/types/slam3d/types_slam3d.h"
 #include "g2o/edge_se3_xyzprior.h" // Include after types_slam3d.h to be ignored on newest g2o versions
 #include "g2o/edge_se3_gravity.h"
-#include "g2o/edge_sbacam_gravity.h"
 #include "g2o/edge_xy_prior.h"  // Include after types_slam2d.h to be ignored on newest g2o versions
 #include "g2o/edge_xyz_prior.h" // Include after types_slam3d.h to be ignored on newest g2o versions
 #ifdef G2O_HAVE_CSPARSE
@@ -77,6 +76,19 @@ typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> Matr
 #include "g2o/types/types_sba.h"
 #include "g2o/types/types_six_dof_expmap.h"
 #include "g2o/solvers/linear_solver_eigen.h"
+#include "g2o/edge_se3_expmap.h"
+#endif
+
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
+namespace rtabmap {
+#ifdef RTABMAP_ORB_SLAM
+typedef g2o::VertexSE3Expmap VertexCam;
+#else
+typedef g2o::VertexCam VertexCam;
+#endif
+}
+#include "g2o/edge_sbacam_gravity.h"
+#include "g2o/edge_sbacam_prior.h"
 #endif
 
 typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
@@ -1414,81 +1426,6 @@ std::map<int, Transform> OptimizerG2O::optimize(
 	return optimizedPoses;
 }
 
-#ifdef RTABMAP_ORB_SLAM
-/**
- * \brief 3D edge between two SBAcam
- */
- class EdgeSE3Expmap : public g2o::BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>
-{
-  public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
-    EdgeSE3Expmap():  BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>(){}
-    bool read(std::istream& is)
-      {
-        return false;
-      }
-
-      bool write(std::ostream& os) const
-      {
-        return false;
-      }
-
-    void computeError()
-    {
-      const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
-      const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
-      g2o::SE3Quat delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
-      _error[0]=delta.translation().x();
-      _error[1]=delta.translation().y();
-      _error[2]=delta.translation().z();
-      _error[3]=delta.rotation().x();
-      _error[4]=delta.rotation().y();
-      _error[5]=delta.rotation().z();
-    }
-
-    virtual void setMeasurement(const g2o::SE3Quat& meas){
-      _measurement=meas;
-      _inverseMeasurement=meas.inverse();
-    }
-
-    virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& , g2o::OptimizableGraph::Vertex* ) { return 1.;}
-    virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* ){
-    	g2o::VertexSE3Expmap* from = static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
-    	g2o::VertexSE3Expmap* to = static_cast<g2o::VertexSE3Expmap*>(_vertices[1]);
-		if (from_.count(from) > 0)
-		  to->setEstimate((g2o::SE3Quat) from->estimate() * _measurement);
-		else
-		  from->setEstimate((g2o::SE3Quat) to->estimate() * _inverseMeasurement);
-    }
-
-    virtual bool setMeasurementData(const double* d){
-      Eigen::Map<const g2o::Vector7d> v(d);
-      _measurement.fromVector(v);
-      _inverseMeasurement = _measurement.inverse();
-      return true;
-    }
-
-    virtual bool getMeasurementData(double* d) const{
-      Eigen::Map<g2o::Vector7d> v(d);
-      v = _measurement.toVector();
-      return true;
-    }
-
-    virtual int measurementDimension() const {return 7;}
-
-    virtual bool setMeasurementFromState() {
-    	const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
-		const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
-		_measurement = (v1->estimate().inverse()*v2->estimate());
-		_inverseMeasurement = _measurement.inverse();
-		return true;
-    }
-
-  protected:
-    g2o::SE3Quat _inverseMeasurement;
-};
-#endif
-
 std::map<int, Transform> OptimizerG2O::optimizeBA(
 		int rootId,
 		const std::map<int, Transform> & poses,
@@ -1559,7 +1496,13 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 #endif // RTABMAP_ORB_SLAM
 
 #ifndef RTABMAP_ORB_SLAM
-		if(optimizer_ == 1)
+		// ISSUE: It seems the fatal error
+		//        "[SetJac] infinite jac" happens relatively
+		//        easily with GaussNewton on SBA problem,
+		//        ignore optimizer_ and always use Levenberg for SBA.
+		// TODO: Note that g2o/RobustKernelDelta parameter could be
+		//       potentially tuned to avoid that error with GaussNewton.
+		if(0)//optimizer_ == 1)
 		{
 #ifdef RTABMAP_G2O_CPP11
 			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(
@@ -1579,8 +1522,23 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 #endif
 		}
 
+		// detect if there are gravity constraints
+		bool hasGravityConstraints = false;
+		if(!isSlam2d() && gravitySigma() > 0)
+		{
+			for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+			{
+				if( iter->second.from() == iter->second.to() &&
+					iter->second.type() == Link::kGravity)
+				{
+					hasGravityConstraints = true;
+					break;
+				}
+			}
+		}
 
-		UDEBUG("fill poses to g2o...");
+
+		UDEBUG("fill %ld poses to g2o... (rootId=%d hasGravityConstraints=%d isSlam2d=%d)", poses.size(), rootId, hasGravityConstraints?1:0, isSlam2d()?1:0);
 		for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 		{
 			if(iter->first > 0)
@@ -1596,11 +1554,8 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 					// Add node's pose
 					UASSERT(!camPose.isNull());
-#ifdef RTABMAP_ORB_SLAM
+
-					g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
+					rtabmap::VertexCam * vCam = new rtabmap::VertexCam();
-#else
-					g2o::VertexCam * vCam = new g2o::VertexCam();
-#endif
 
 					Eigen::Affine3d a = camPose.toEigen3d();
 #ifdef RTABMAP_ORB_SLAM
@@ -1619,7 +1574,65 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 					vCam->setId(iter->first*MULTICAM_OFFSET + i);
 
 					// negative root means that all other poses should be fixed instead of the root
-					vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
+					bool fixNode = (rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId);
+
+					UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert cam vertex %d (pose=%d)!?", vCam->id(), iter->first).c_str());
+				
+					if(this->isSlam2d())
+					{
+						if(fixNode)
+						{
+							UDEBUG("Set node %d fixed", iter->first);
+							vCam->setFixed(true);
+						}
+						else if(i==0) // Only set prior on the first camera
+						{
+							// add a singleton constraint that locks the position of the robot on the plane
+							EdgeSBACamPrior* planeConstraint = new EdgeSBACamPrior();
+							Eigen::Matrix<double, 6, 6> pinfo = Eigen::Matrix<double, 6, 6>::Zero();
+							pinfo(2, 2) = 1e9;
+							planeConstraint->setInformation(pinfo);
+							g2o::SE3Quat fixedZ = g2o::SE3Quat();
+							fixedZ.setTranslation(Eigen::Vector3d(0,0,iter->second.z()));
+							planeConstraint->setMeasurement(fixedZ);
+							Eigen::Affine3d a = iterModel->second[i].localTransform().inverse().toEigen3d();
+							planeConstraint->setCameraInvLocalTransform(g2o::SE3Quat(a.linear(), a.translation()));
+							planeConstraint->vertices()[0] = vCam;
+							optimizer.addEdge(planeConstraint);
+						}
+					}
+					else if(fixNode)
+					{
+						if(rootId < 0 || !hasGravityConstraints)
+						{
+							UDEBUG("Set node %d fixed", iter->first);
+							vCam->setFixed(true);
+						}
+						else if(hasGravityConstraints && i==0) // Only set prior on the first camera in case of multi-cam
+						{
+							// Setup root prior (fixed x,y,z,yaw)
+							EdgeSBACamPrior * e = new EdgeSBACamPrior();
+							e->vertices()[0] = vCam;
+							Eigen::Affine3d a = iter->second.toEigen3d();
+							e->setMeasurement(g2o::SE3Quat(a.linear(), a.translation()));
+							a = iterModel->second[i].localTransform().inverse().toEigen3d();
+							e->setCameraInvLocalTransform(g2o::SE3Quat(a.linear(), a.translation()));
+							Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity()*10e6;
+							// pitch and roll not fixed
+							information(3,3) = information(4,4) = 1;
+							e->setInformation(information);
+							if (!optimizer.addEdge(e))
+							{
+								delete e;
+								UERROR("Map: Failed adding fixed constraint of node %d, set as fixed instead", iter->first);
+								vCam->setFixed(true);
+							}
+							else
+							{
+								UDEBUG("Set node %d fixed with prior (have gravity constraints)", iter->first);
+							}
+						}
+					}
 
 					/*UDEBUG("camPose %d (camid=%d) (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
 							iter->first,
@@ -1632,8 +1645,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 							iterModel->second[i].Tx(),
 							iterModel->second[i].Tx()<0.0?-iterModel->second[i].Tx()/iterModel->second[i].fx():baseline_,
 							camPose.prettyPrint().c_str());*/
-
-					UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert cam vertex %d (pose=%d)!?", vCam->id(), iter->first).c_str());
 				}
 			}
 		}
@@ -1652,7 +1663,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 				if(id1 == id2)
 				{
-#ifndef RTABMAP_ORB_SLAM
 					g2o::HyperGraph::Edge * edge = 0;
 					if(gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
 					{
@@ -1666,7 +1676,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 						Eigen::MatrixXd information = Eigen::MatrixXd::Identity(3, 3) * 1.0/(gravitySigma()*gravitySigma());
 
-						g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1*MULTICAM_OFFSET);
+						rtabmap::VertexCam* v1 = (rtabmap::VertexCam*)optimizer.vertex(id1*MULTICAM_OFFSET);
 						EdgeSBACamGravity* priorEdge(new EdgeSBACamGravity());
 						std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
 						// Gravity constraint added only to first camera of a pose
@@ -1683,7 +1693,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
 						return optimizedPoses;
 					}
-#endif
 				}
 				else if(id1>0 && id2>0) // not supporting landmarks
 				{
@@ -1839,14 +1848,14 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 						g2o::OptimizableGraph::Edge * e;
 						double baseline = 0.0;
+						rtabmap::VertexCam* vcam = dynamic_cast<rtabmap::VertexCam*>(optimizer.vertex(camId));
 #ifdef RTABMAP_ORB_SLAM
-						g2o::VertexSE3Expmap* vcam = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(camId));
+						
 						std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(poseId);
 
-						UASSERT(iterModel != models.end() && camIndex<iterModel->second.size() && iterModel->second[camIndex].isValidForProjection());
+						UASSERT(iterModel != models.end() && camIndex<(int)iterModel->second.size() && iterModel->second[camIndex].isValidForProjection());
 						baseline = iterModel->second[camIndex].Tx()<0.0?-iterModel->second[camIndex].Tx()/iterModel->second[camIndex].fx():baseline_;
 #else
-						g2o::VertexCam* vcam = dynamic_cast<g2o::VertexCam*>(optimizer.vertex(camId));
 						baseline = vcam->estimate().baseline;
 #endif
 						double variance = pixelVariance_;
@@ -1944,7 +1953,8 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 			if(uIsNan(chi2))
 			{
-				UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current=%d).", optimizer_);
+				UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current %s=%d) or solver (current %s=%d).",
+						Parameters::kg2oOptimizer().c_str(), optimizer_, Parameters::kg2oSolver().c_str(), solver_);
 				return optimizedPoses;
 			}
 			UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
@@ -1978,15 +1988,18 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						//UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
 #endif
 
-						cv::Point3f pt3d;
+						int id=-1;
 						if((*iter)->vertex(0)->id() > negVertexOffset)
 						{
-							pt3d = points3DMap.at(negVertexOffset - (*iter)->vertex(0)->id());
+							id = negVertexOffset - (*iter)->vertex(0)->id();
 						}
 						else
 						{
-							pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
+							id = (*iter)->vertex(0)->id() - stepVertexId;
 						}
+						UASSERT_MSG(points3DMap.find(id) != points3DMap.end(), uFormat("word id=%d points3DMap=%ld vertex id=%d (negVertexOffset=%d stepVertexId=%d)",
+							id, points3DMap.size(), (*iter)->vertex(0)->id(), negVertexOffset, stepVertexId).c_str());
+						cv::Point3f pt3d = points3DMap.at(id);
 						((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
 
 						if(outliers)
@@ -2043,12 +2056,26 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						return optimizedPoses;
 					}
 
-					// FIXME: is there a way that we can add the 2D constraint directly in SBA?
 					if(this->isSlam2d())
 					{
-						// get transform between old and new pose
+						// The optimized poses should be already fixed to original height,
-						t = iter->second.inverse() * t;
+						// but it may have varied a little (not exaclty the same number).
-						optimizedPoses.insert(std::pair<int, Transform>(iter->first, iter->second * t.to3DoF()));
+						// Here we just put back the original z value.
+						if(fabs(t.z() - iter->second.z()) < 0.001)
+						{
+							t.z() = iter->second.z();
+							optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
+						}
+						else 
+						{
+							UWARN("Planar constraints didn't work!? original pose (%d), pose %s -> %s. Falling back to old approach.",
+								iter->first,
+								iter->second.prettyPrint().c_str(),
+								t.prettyPrint().c_str());
+							// get transform between old and new pose
+							t = iter->second.inverse() * t;
+							optimizedPoses.insert(std::pair<int, Transform>(iter->first, iter->second * t.to3DoF()));
+						}
 					}
 					else
 					{

corelib/src/optimizer/g2o/edge_sbacam_gravity.h

@@ -32,18 +32,23 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef RTAB_G2O_EDGE_SBACAM_GRAVITY_H_
 #define RTAB_G2O_EDGE_SBACAM_GRAVITY_H_
 
+#ifdef RTABMAP_ORB_SLAM
+#include "g2o/types/types_six_dof_expmap.h"
+#else
 #include "g2o/types/sba/types_sba.h"
+#endif
 #include "g2o/core/base_unary_edge.h"
 namespace rtabmap {
   /**
    * \brief EdgeSBACamGravity
     * \brief g2o edge with gravity constraint
    */
-class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6, 1>, g2o::VertexCam> {
+class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6, 1>, VertexCam> {
   public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 	EdgeSBACamGravity(){
     	information().setIdentity();
+      cameraInvLocalTransform_.setIdentity();
     }
     virtual bool read(std::istream& is) {return false;} // not implemented
     virtual bool write(std::ostream& os) const {return false;} // not implemented
@@ -55,30 +60,36 @@ class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6,
 
     // return the error estimate as a 3-vector
     void computeError(){
-    	const g2o::VertexCam* v1 = static_cast<const g2o::VertexCam*>(_vertices[0]);
+    	const VertexCam* v = static_cast<const VertexCam*>(_vertices[0]);
 
-		Eigen::Vector3d direction = _measurement.head<3>();
+      Eigen::Vector3d direction = _measurement.head<3>();
-		Eigen::Vector3d measurement = _measurement.tail<3>();
+      Eigen::Vector3d measurement = _measurement.tail<3>();
 
-		Eigen::Vector3d ea;
+      g2o::SE3Quat estimate;
+#ifdef RTABMAP_ORB_SLAM
+      estimate = v->estimate().inverse();
+#else
+      estimate = v->estimate();
+#endif
 
-		// Transform pose from camera frame to world frame
+      // Transform pose from camera frame to world frame
-		Eigen::Matrix3d t = v1->estimate().rotation().toRotationMatrix() * cameraInvLocalTransform_;
+      Eigen::Matrix3d t = estimate.rotation().toRotationMatrix() * cameraInvLocalTransform_;
-		ea[0] = atan2(t (2, 1), t (2, 2));
+      Eigen::Vector3d ea;
-		ea[1] = asin(-t (2, 0));
+      ea[0] = atan2(t (2, 1), t (2, 2));
-		ea[2] = atan2(t (1, 0), t (0, 0));
+      ea[1] = asin(-t (2, 0));
+      ea[2] = atan2(t (1, 0), t (0, 0));
 
-		Eigen::Matrix3d rot =
+      Eigen::Matrix3d rot =
-		   (Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) *
+        (Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) *
-			Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitX())).toRotationMatrix();
+        Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitX())).toRotationMatrix();
 
-		Eigen::Vector3d estimate = rot * -direction;
+      Eigen::Vector3d newEstimate = rot * -direction;
-		_error = estimate - measurement;
+      _error = newEstimate - measurement;
 
-		/*printf("%d : measured=%f %f %f est=%f %f %f error=%f %f %f\n", v1->id(),
+      /*printf("%d : measured=%f %f %f est=%f %f %f error=%f %f %f\n", v1->id(),
-		  measurement[0], measurement[1], measurement[2],
+        measurement[0], measurement[1], measurement[2],
-		  estimate[0], estimate[1], estimate[2],
+        estimate[0], estimate[1], estimate[2],
-		  _error[0], _error[1], _error[2]);*/
+        _error[0], _error[1], _error[2]);*/
     }
 
     // 6 values:

corelib/src/optimizer/g2o/edge_sbacam_prior.h

@@ -0,0 +1,135 @@
+/*
+Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+/**
+ * Adapted from EdgeSE3Prior
+ */
+
+#ifndef RTAB_G2O_EDGE_SBACAM_PRIOR_H_
+#define RTAB_G2O_EDGE_SBACAM_PRIOR_H_
+
+#ifdef RTABMAP_ORB_SLAM
+#include "g2o/types/types_six_dof_expmap.h"
+#else
+#include "g2o/types/sba/types_sba.h"
+#endif
+#include "g2o/core/base_unary_edge.h"
+namespace rtabmap {
+  /**
+   * \brief EdgeSBACamPrior
+    * \brief g2o edge with gravity constraint
+   */
+class EdgeSBACamPrior : public g2o::BaseUnaryEdge<6, g2o::SE3Quat, VertexCam> {
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    EdgeSBACamPrior() {
+      setMeasurement(g2o::SE3Quat());
+      information().setIdentity();
+    }
+
+    void setCameraInvLocalTransform(const g2o::SE3Quat & t)
+    {
+    	_cameraInvLocalTransform = t;
+    }
+
+    // return the error estimate as a 3-vector
+    void computeError() {
+      const VertexCam* v = static_cast<const VertexCam*>(_vertices[0]);
+      g2o::SE3Quat estimate;
+#ifdef RTABMAP_ORB_SLAM
+      estimate = v->estimate().inverse();
+#else
+      estimate = v->estimate();
+#endif
+      g2o::SE3Quat delta = _inverseMeasurement * estimate * _cameraInvLocalTransform;
+      _error[0]=delta.translation().x();
+      _error[1]=delta.translation().y();
+      _error[2]=delta.translation().z();
+      _error[3]=delta.rotation().x();
+      _error[4]=delta.rotation().y();
+      _error[5]=delta.rotation().z();
+    }
+    
+    // jacobian
+    virtual void linearizeOplus() {
+      _jacobianOplusXi = Eigen::Matrix<double, 6, 6>::Identity();
+    }
+
+    virtual void setMeasurement(const g2o::SE3Quat& m){
+      _measurement = m;
+      _inverseMeasurement = m.inverse();
+    }
+
+    virtual bool setMeasurementData(const double* d) override {
+      Eigen::Map<const Eigen::Matrix<double, 7, 1, Eigen::ColMajor> > v(d);
+      // SE3Quat expects [x, y, z, qx, qy, qz, qw]
+      _measurement.fromVector(v);
+      _inverseMeasurement = _measurement.inverse();
+      return true;
+    }
+    
+    virtual bool getMeasurementData(double* d) const override {
+      Eigen::Map<Eigen::Matrix<double, 7, 1, Eigen::ColMajor> > v(d);
+      // Returns [x, y, z, qx, qy, qz, qw]
+      v = _measurement.toVector();
+      return true;
+    }
+  
+    virtual int measurementDimension() const {return 7;}
+
+    virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& /*from*/, 
+             g2o::OptimizableGraph::Vertex* /*to*/) { 
+      return 1.;
+    }
+
+    virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from, g2o::OptimizableGraph::Vertex* to) {
+      VertexCam *v = static_cast<VertexCam*>(_vertices[0]);
+      assert(v && "Vertex for the Prior edge is not set");
+
+#ifdef RTABMAP_ORB_SLAM
+      g2o::SE3Quat newEstimate = _cameraInvLocalTransform * _inverseMeasurement;
+#else
+      g2o::SE3Quat newEstimate = measurement()*_cameraInvLocalTransform.inverse();
+#endif
+      if (_information.block<3,3>(0,0).array().abs().sum() == 0){ // do not set translation, as that part of the information is all zero
+        newEstimate.setTranslation(v->estimate().translation());
+      }
+      if (_information.block<3,3>(3,3).array().abs().sum() == 0){ // do not set rotation, as that part of the information is all zero
+        newEstimate.setRotation(v->estimate().rotation());
+      }
+      v->setEstimate(newEstimate);
+    }
+
+    virtual bool read(std::istream& is) override { return true; } 
+    virtual bool write(std::ostream& os) const override { return true; }
+  protected:
+    g2o::SE3Quat _inverseMeasurement;
+    g2o::SE3Quat _cameraInvLocalTransform;
+};
+
+}
+#endif

corelib/src/optimizer/g2o/edge_se3_expmap.h

@@ -0,0 +1,74 @@
+#include "g2o/types/types_six_dof_expmap.h"
+
+/**
+ * \brief 3D edge between two SBAcam
+ */
+ class EdgeSE3Expmap : public g2o::BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>
+{
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+    EdgeSE3Expmap():  BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>(){}
+    bool read(std::istream& is)
+      {
+        return false;
+      }
+
+      bool write(std::ostream& os) const
+      {
+        return false;
+      }
+
+    void computeError()
+    {
+      const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
+      const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
+      g2o::SE3Quat delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
+      _error[0]=delta.translation().x();
+      _error[1]=delta.translation().y();
+      _error[2]=delta.translation().z();
+      _error[3]=delta.rotation().x();
+      _error[4]=delta.rotation().y();
+      _error[5]=delta.rotation().z();
+    }
+
+    virtual void setMeasurement(const g2o::SE3Quat& meas){
+      _measurement=meas;
+      _inverseMeasurement=meas.inverse();
+    }
+
+    virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& , g2o::OptimizableGraph::Vertex* ) { return 1.;}
+    virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* ){
+    	g2o::VertexSE3Expmap* from = static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
+    	g2o::VertexSE3Expmap* to = static_cast<g2o::VertexSE3Expmap*>(_vertices[1]);
+		if (from_.count(from) > 0)
+		  to->setEstimate((g2o::SE3Quat) from->estimate() * _measurement);
+		else
+		  from->setEstimate((g2o::SE3Quat) to->estimate() * _inverseMeasurement);
+    }
+
+    virtual bool setMeasurementData(const double* d){
+      Eigen::Map<const g2o::Vector7d> v(d);
+      _measurement.fromVector(v);
+      _inverseMeasurement = _measurement.inverse();
+      return true;
+    }
+
+    virtual bool getMeasurementData(double* d) const{
+      Eigen::Map<g2o::Vector7d> v(d);
+      v = _measurement.toVector();
+      return true;
+    }
+
+    virtual int measurementDimension() const {return 7;}
+
+    virtual bool setMeasurementFromState() {
+    	const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
+		const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
+		_measurement = (v1->estimate().inverse()*v2->estimate());
+		_inverseMeasurement = _measurement.inverse();
+		return true;
+    }
+
+  protected:
+    g2o::SE3Quat _inverseMeasurement;
+};

corelib/src/util2d.cpp

@@ -2296,6 +2296,7 @@ std::vector<int> SSC(
 	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();
+	maxKeypoints = maxKeypoints - round(maxKeypoints * tolerance); // Just the make sure the solution will always be <= input maxKeypoints
 
 	// several temp expression variables to simplify solution equation
 	int exp1 = rows + cols + 2*maxKeypoints;

guilib/include/rtabmap/gui/MainWindow.h

@@ -330,6 +330,7 @@ protected:
 			int iterations,
 			bool interSession,
 			bool intraSession,
+			int minGraphDistance,
 			// SBA params:
 			bool sba,
 			int sbaIterations,

guilib/include/rtabmap/gui/PostProcessingDialog.h

@@ -59,6 +59,7 @@ public:
 	int iterations() const;
 	bool intraSession() const;
 	bool interSession() const;
+	int minGraphDistance() const;
 	bool isRefineNeighborLinks() const;
 	bool isRefineLoopClosureLinks() const;
 	bool isSBA() const;
@@ -74,6 +75,7 @@ public:
 	void setIterations(int iterations);
 	void setIntraSession(bool enabled);
 	void setInterSession(bool enabled);
+	void setMinGraphDistance(int value);
 	void setRefineNeighborLinks(bool on);
 	void setRefineLoopClosureLinks(bool on);
 	void setSBA(bool on);

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -293,6 +293,7 @@ public:
 	double getSourceScanForceGroundNormalsUp() const;
 	Transform getSourceLocalTransform() const;    //Openni group
 	Transform getLaserLocalTransform() const; // directory images
+	Transform getGroundTruthLocalTransform() const; // directory images
 	Transform getIMULocalTransform() const; // directory images
 	QString getIMUPath() const;
 	int getIMURate() const;

guilib/include/rtabmap/gui/StatsToolBox.h

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <QWidget>
 #include <QtCore/QMap>
+#include <QTimer>
 
 class QToolButton;
 class QLabel;
@@ -59,6 +60,7 @@ public:
 
 public Q_SLOTS:
 	void updateMenu(const QMenu * menu);
+	void updateLabel();
 
 Q_SIGNALS:
 	void valueAdded(qreal);
@@ -117,6 +119,8 @@ Q_SIGNALS:
 private Q_SLOTS:
 	void plot(const StatItem * stat, const QString & plotName = QString());
 	void figureDeleted(QObject * obj);
+	void requestLabelsUpdate();
+	void updateLabels();
 
 protected:
 	virtual void contextMenuEvent(QContextMenuEvent * event);
@@ -127,6 +131,7 @@ private:
 	QString _workingDirectory;
 	int _newFigureMaxItems;
 	QMap<QString, QWidget*> _figures;
+	QTimer _updateLabelsTimer;
 };
 
 }

guilib/src/DatabaseViewer.cpp

@@ -239,6 +239,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopClosureReextractFeatures()));
 	parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopCovLimited()));
 	parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDProximityPathFilteringRadius()));
+	parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kMemSTMSize()));
 	ui_->parameters_toolbox->setupUi(parameters);
 	exportDialog_->setObjectName("ExportCloudsDialog");
 	restoreDefaultSettings();
@@ -481,6 +482,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_detectMore_intraSession, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_detectMore_interSession, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
+	connect(ui_->spinBox_minGraphDistance, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_opt_graph_as_guess, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 
 	connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
@@ -678,6 +680,7 @@ void DatabaseViewer::readSettings()
 	ui_->checkBox_detectMore_intraSession->setChecked(settings.value("intra_session", ui_->checkBox_detectMore_intraSession->isChecked()).toBool());
 	ui_->checkBox_detectMore_interSession->setChecked(settings.value("inter_session", ui_->checkBox_detectMore_interSession->isChecked()).toBool());
 	ui_->checkBox_opt_graph_as_guess->setChecked(settings.value("opt_graph_as_guess", ui_->checkBox_opt_graph_as_guess->isChecked()).toBool());
+	ui_->spinBox_minGraphDistance->setValue(settings.value("min_graph_distance", ui_->spinBox_minGraphDistance->value()).toInt());
 	settings.endGroup();
 	settings.endGroup();
 
@@ -775,6 +778,7 @@ void DatabaseViewer::writeSettings()
 	settings.setValue("intra_session", ui_->checkBox_detectMore_intraSession->isChecked());
 	settings.setValue("inter_session", ui_->checkBox_detectMore_interSession->isChecked());
 	settings.setValue("opt_graph_as_guess", ui_->checkBox_opt_graph_as_guess->isChecked());
+	settings.setValue("min_graph_distance", ui_->spinBox_minGraphDistance->value());
 	settings.endGroup();
 	settings.endGroup();
 
@@ -856,6 +860,7 @@ void DatabaseViewer::restoreDefaultSettings()
 	ui_->checkBox_detectMore_interSession->setChecked(true);
 	ui_->checkBox_opt_graph_as_guess->setChecked(true);
 	ui_->spinBox_fromToMapId->setValue(-1);
+	ui_->spinBox_minGraphDistance->setValue(10);
 }
 
 void DatabaseViewer::openDatabase()
@@ -4285,10 +4290,12 @@ void DatabaseViewer::detectMoreLoopClosures()
 	const ParametersMap & parameters = ui_->parameters_toolbox->getParameters();
 	bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
 	Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
+	std::multimap<int, Link> links = updateLinksWithModifications(links_);
 	if(loopCovLimited)
 	{
-		odomMaxInf_ = graph::getMaxOdomInf(updateLinksWithModifications(links_));
+		odomMaxInf_ = graph::getMaxOdomInf(links);
 	}
+	links = graph::filterLinks(links, Link::kNeighbor, true); // keep only neighbor links
 
 	int iterations = ui_->spinBox_detectMore_iterations->value();
 	UASSERT(iterations > 0);
@@ -4299,6 +4306,7 @@ void DatabaseViewer::detectMoreLoopClosures()
 	bool interSession = ui_->checkBox_detectMore_interSession->isChecked();
 	bool useOptimizedGraphAsGuess = ui_->checkBox_opt_graph_as_guess->isChecked();
 	int fromToMapId = ui_->spinBox_fromToMapId->value();
+	int minimumGraphDistance = ui_->spinBox_minGraphDistance->value();
 	if(!interSession && !intraSession)
 	{
 		QMessageBox::warning(this, tr("Cannot detect more loop closures"), tr("Intra and inter session parameters are disabled! Enable one or both."));
@@ -4324,6 +4332,7 @@ void DatabaseViewer::detectMoreLoopClosures()
 		progressDialog->appendText(tr("Looking for more loop closures: %1 clusters found.").arg(clusters.size()));
 		if(fromToMapId >=0)
 		{
+			int clusterBefore = clusters.size();
 			for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end();)
 			{
 				int mapId = uValue(mapIds_, iter->first, 0);
@@ -4335,7 +4344,8 @@ void DatabaseViewer::detectMoreLoopClosures()
 					++iter;
 				}
 			}
-			progressDialog->appendText(tr("Looking for more loop closures: filtered %1 clusters for map session %2.").arg(clusters.size()).arg(fromToMapId));
+			progressDialog->appendText(tr("Looking for more loop closures: filtered %1/%2 clusters for map session %3.")
+				.arg(clusterBefore-clusters.size()).arg(clusterBefore).arg(fromToMapId));
 			if(clusters.empty())
 			{
 				progressDialog->appendText(tr("No clusters belong to mapId %1, aborting!").arg(fromToMapId));
@@ -4344,6 +4354,34 @@ void DatabaseViewer::detectMoreLoopClosures()
 			}
 		}
 
+		if(minimumGraphDistance > 1)
+		{
+			int clusterBefore = clusters.size();
+			for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end();)
+			{
+				if(abs(iter->first - iter->second) < minimumGraphDistance)
+				{
+					iter = clusters.erase(iter);
+				}
+				else
+				{
+					// compute path to know how far we are in terms of graph length
+					std::list<int> path = graph::computePath(links, iter->first, iter->second);
+					if(!path.empty() && (int)path.size() <= minimumGraphDistance)
+					{
+						iter = clusters.erase(iter);
+					}
+					else
+					{
+						++iter;
+					}
+				}
+			}
+			progressDialog->appendText(tr("Filtered %1/%2 clusters for too close nodes (below minimum graph distance=%3).")
+				.arg(clusterBefore-clusters.size()).arg(clusterBefore).arg(minimumGraphDistance));
+			QApplication::processEvents();
+		}
+
 		progressDialog->setMaximumSteps(progressDialog->maximumSteps()+(int)clusters.size());
 		QApplication::processEvents();
 
@@ -4700,10 +4738,17 @@ void DatabaseViewer::graphNodeSelected(int id)
 
 void DatabaseViewer::graphLinkSelected(int from, int to)
 {
-	if(from>0 && idToIndex_.contains(from))
+	if(from < 0 || to < 0)
-		ui_->horizontalSlider_A->setValue(idToIndex_.value(from));
+	{
-	if(to>0 && idToIndex_.contains(to))
+		updateLoopClosuresSlider(from, to);
-		ui_->horizontalSlider_B->setValue(idToIndex_.value(to));
+	}
+	else
+	{
+		if(idToIndex_.contains(from))
+			ui_->horizontalSlider_A->setValue(idToIndex_.value(from));
+		if(idToIndex_.contains(to))
+			ui_->horizontalSlider_B->setValue(idToIndex_.value(to));
+	}
 }
 
 void DatabaseViewer::sliderAValueChanged(int value)
@@ -6163,6 +6208,14 @@ void DatabaseViewer::updateWordsMatching(const std::vector<int> & inliers)
 							kptB->keypoint().pt.y,
 							cB);
 				}
+				else if(ids[i]<0)
+				{
+					ui_->graphicsView_A->setFeatureColor(ids[i], Qt::gray);
+				}
+			}
+			for(auto iter = wordsB.begin(); iter.key()<0 && iter!=wordsB.end(); ++iter)
+			{
+				ui_->graphicsView_B->setFeatureColor(iter.key(), Qt::gray);
 			}
 			ui_->graphicsView_A->update();
 			ui_->graphicsView_B->update();
@@ -8043,6 +8096,17 @@ void DatabaseViewer::updateGraphView()
 				ui_->label_timeOptimization->setNum(0);
 				ui_->label_poses->setNum((int)optPoses.size());
 				graphes_.push_back(optPoses);
+				// Just get the links:
+				std::map<int, rtabmap::Transform> posesOut;
+				UINFO("Get connected graph from %d (%d poses, %d links)", fromId, (int)poses.size(), (int)links.size());
+				std::shared_ptr<Optimizer> optimizer(Optimizer::create(parameters));
+				optimizer->getConnectedGraph(
+						fromId,
+						optPoses,
+						links,
+						posesOut,
+						graphLinks_);
+				UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)graphLinks_.size());
 			}
 			ui_->horizontalSlider_rotation->setEnabled(false);
 			ui_->pushButton_applyRotation->setEnabled(false);

guilib/src/ExportCloudsDialog.cpp

@@ -3703,7 +3703,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 							0,
 							0,
 							0,
-							_ui->checkBox_fromDepth->isChecked()?&confidence:0);
+							_ui->checkBox_fromDepth->isChecked()&&_ui->spinBox_depthConfidence->value()>0?&confidence:0);
 				}
 				else if(_dbDriver)
 				{
@@ -3717,7 +3717,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 							0,
 							0,
 							0,
-							_ui->checkBox_fromDepth->isChecked()?&confidence:0);
+							_ui->checkBox_fromDepth->isChecked()&&_ui->spinBox_depthConfidence->value()>0?&confidence:0);
 				}
 
 				if(_ui->checkBox_fromDepth->isChecked() && !data.imageRaw().empty() && !data.depthOrRightRaw().empty())

guilib/src/ImageView.cpp

@@ -1255,11 +1255,11 @@ void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, c
 	{
 		if (xRatio > 0 && yRatio > 0)
 		{
-			addFeature(iter->first, iter->second, util2d::getDepth(depth, iter->second.pt.x*xRatio, iter->second.pt.y*yRatio, false), color);
+			addFeature(iter->first, iter->second, util2d::getDepth(depth, iter->second.pt.x*xRatio, iter->second.pt.y*yRatio, false), iter->first<0?Qt::gray:color);
 		}
 		else
 		{
-			addFeature(iter->first, iter->second, 0, color);
+			addFeature(iter->first, iter->second, 0, iter->first<0?Qt::gray:color);
 		}
 	}
 

guilib/src/MainWindow.cpp

@@ -3313,7 +3313,7 @@ void MainWindow::updateMapCloud(
 							{
 								std::string gtFrustumId = uFormat("f_gt_%d", iter->first);
 								color = Qt::gray;
-								_cloudViewer->addOrUpdateFrustum(gtFrustumId, _currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
+								_cloudViewer->addOrUpdateFrustum(gtFrustumId, mapToGt*_currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
 							}
 						}
 					}
@@ -6563,6 +6563,7 @@ void MainWindow::showPostProcessingDialog()
 			_postProcessingDialog->iterations(),
 			_postProcessingDialog->interSession(),
 			_postProcessingDialog->intraSession(),
+			_postProcessingDialog->minGraphDistance(),
 			_postProcessingDialog->isSBA(),
 			_postProcessingDialog->sbaIterations(),
 			_postProcessingDialog->sbaVariance(),
@@ -6579,6 +6580,7 @@ void MainWindow::postProcessing(
 		int iterations,
 		bool interSession,
 		bool intraSession,
+		int minGraphDistance,
 		bool sba,
 		int sbaIterations,
 		double sbaVariance,
@@ -6687,6 +6689,7 @@ void MainWindow::postProcessing(
 		{
 			odomMaxInf = graph::getMaxOdomInf(_currentLinksMap);
 		}
+		std::multimap<int, Link> neigborLinks = graph::filterLinks(_currentLinksMap, Link::kNeighbor, true);
 
 		std::shared_ptr<Registration> registration(Registration::create(parameters));
 
@@ -6705,6 +6708,34 @@ void MainWindow::postProcessing(
 			_progressDialog->appendText(tr("Looking for more loop closures, clustering poses... found %1 clusters.").arg(clusters.size()));
 			QApplication::processEvents();
 
+			if(minGraphDistance > 1)
+			{
+				int clustersBefore = clusters.size();
+				for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end();)
+				{
+					if(abs(iter->first - iter->second) < minGraphDistance)
+					{
+						iter = clusters.erase(iter);
+					}
+					else
+					{
+						// compute path to know how far we are in terms of graph length
+						std::list<int> path = graph::computePath(neigborLinks, iter->first, iter->second);
+						if(!path.empty() && (int)path.size() <= minGraphDistance)
+						{
+							iter = clusters.erase(iter);
+						}
+						else
+						{
+							++iter;
+						}
+					}
+				}
+				_progressDialog->appendText(tr("Filtered %1/%2 clusters for too close nodes (below minimum graph distance=%3).")
+					.arg(clustersBefore-clusters.size()).arg(clustersBefore).arg(minGraphDistance));
+				QApplication::processEvents();
+			}
+
 			int i=0;
 			std::set<int> addedLinks;
 			for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end() && !_progressCanceled; ++iter, ++i)

guilib/src/PostProcessingDialog.cpp

@@ -82,6 +82,7 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
 	connect(_ui->iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->intraSession, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->interSession, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->minGraphDistance, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 
@@ -150,6 +151,7 @@ void PostProcessingDialog::saveSettings(QSettings & settings, const QString & gr
 	settings.setValue("iterations", this->iterations());
 	settings.setValue("intra_session", this->intraSession());
 	settings.setValue("inter_session", this->interSession());
+	settings.setValue("min_graph_distance", this->minGraphDistance());
 	settings.setValue("refine_neigbors", this->isRefineNeighborLinks());
 	settings.setValue("refine_lc", this->isRefineLoopClosureLinks());
 	settings.setValue("sba", this->isSBA());
@@ -175,6 +177,7 @@ void PostProcessingDialog::loadSettings(QSettings & settings, const QString & gr
 	this->setIterations(settings.value("iterations", this->iterations()).toInt());
 	this->setIntraSession(settings.value("intra_session", this->intraSession()).toBool());
 	this->setInterSession(settings.value("inter_session", this->interSession()).toBool());
+	this->setMinGraphDistance(settings.value("min_graph_distance", this->minGraphDistance()).toInt());
 	this->setRefineNeighborLinks(settings.value("refine_neigbors", this->isRefineNeighborLinks()).toBool());
 	this->setRefineLoopClosureLinks(settings.value("refine_lc", this->isRefineLoopClosureLinks()).toBool());
 	this->setSBA(settings.value("sba", this->isSBA()).toBool());
@@ -197,6 +200,7 @@ void PostProcessingDialog::restoreDefaults()
 	setIterations(5);
 	setIntraSession(true);
 	setInterSession(true);
+	setMinGraphDistance(10);
 	setRefineNeighborLinks(false);
 	setRefineLoopClosureLinks(false);
 	setSBA(false);
@@ -254,6 +258,11 @@ bool PostProcessingDialog::interSession() const
 	return _ui->interSession->isChecked();
 }
 
+int PostProcessingDialog::minGraphDistance() const
+{
+	return _ui->minGraphDistance->value();
+}
+
 bool PostProcessingDialog::isRefineNeighborLinks() const
 {
 	return _ui->refineNeighborLinks->isChecked();
@@ -311,6 +320,10 @@ void PostProcessingDialog::setInterSession(bool enabled)
 {
 	_ui->interSession->setChecked(enabled);
 }
+void PostProcessingDialog::setMinGraphDistance(int value)
+{
+	_ui->minGraphDistance->setValue(value);
+}
 void PostProcessingDialog::setRefineNeighborLinks(bool on)
 {
 	_ui->refineNeighborLinks->setChecked(on);

guilib/src/PreferencesDialog.cpp

@@ -171,6 +171,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->sift_label_gpu->setEnabled(false);
 	_ui->sift_doubleSpinBox_gaussianDiffThreshold->setEnabled(false);
 	_ui->sift_label_gaussianThreshold->setEnabled(false);
+	_ui->sift_doubleSpinBox_maxGaussianDiffThreshold->setEnabled(false);
+	_ui->sift_label_maxGaussianThreshold->setEnabled(false);
 	_ui->sift_checkBox_upscale->setEnabled(false);
 	_ui->sift_label_upscale->setEnabled(false);
 #endif
@@ -850,6 +852,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->comboBox_cameraImages_odomFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraImages_gt, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_cameraImages_gtFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_cameraImages_gt_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_maxPoseTimeDiff, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraImages_path_imu, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraImages_imu_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1144,6 +1147,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->sift_checkBox_rootsift->setObjectName(Parameters::kSIFTRootSIFT().c_str());
 	_ui->sift_checkBox_gpu->setObjectName(Parameters::kSIFTGpu().c_str());
 	_ui->sift_doubleSpinBox_gaussianDiffThreshold->setObjectName(Parameters::kSIFTGaussianThreshold().c_str());
+	_ui->sift_doubleSpinBox_maxGaussianDiffThreshold->setObjectName(Parameters::kSIFTMaxGaussianThreshold().c_str());
 	_ui->sift_checkBox_upscale->setObjectName(Parameters::kSIFTUpscale().c_str());
 
 	//BRIEF descriptor
@@ -2360,6 +2364,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->comboBox_cameraImages_odomFormat->setCurrentIndex(0);
 		_ui->lineEdit_cameraImages_gt->setText("");
 		_ui->comboBox_cameraImages_gtFormat->setCurrentIndex(0);
+		_ui->lineEdit_cameraImages_gt_transform->setText("0 0 0 0 0 0");
 		_ui->doubleSpinBox_maxPoseTimeDiff->setValue(0.02);
 		_ui->lineEdit_cameraImages_path_imu->setText("");
 		_ui->lineEdit_cameraImages_imu_transform->setText("0 0 1 0 -1 0 1 0 0");
@@ -2894,6 +2899,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->comboBox_cameraImages_odomFormat->setCurrentIndex(settings.value("odom_format", _ui->comboBox_cameraImages_odomFormat->currentIndex()).toInt());
 	_ui->lineEdit_cameraImages_gt->setText(settings.value("gt_path", _ui->lineEdit_cameraImages_gt->text()).toString());
 	_ui->comboBox_cameraImages_gtFormat->setCurrentIndex(settings.value("gt_format", _ui->comboBox_cameraImages_gtFormat->currentIndex()).toInt());
+	_ui->lineEdit_cameraImages_gt_transform->setText(settings.value("gt_transform", _ui->lineEdit_cameraImages_gt_transform->text()).toString());
 	_ui->doubleSpinBox_maxPoseTimeDiff->setValue(settings.value("max_pose_time_diff", _ui->doubleSpinBox_maxPoseTimeDiff->value()).toDouble());
 
 	_ui->lineEdit_cameraImages_path_imu->setText(settings.value("imu_path", _ui->lineEdit_cameraImages_path_imu->text()).toString());
@@ -3514,6 +3520,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("odom_format",         _ui->comboBox_cameraImages_odomFormat->currentIndex());
 	settings.setValue("gt_path",             _ui->lineEdit_cameraImages_gt->text());
 	settings.setValue("gt_format",           _ui->comboBox_cameraImages_gtFormat->currentIndex());
+	settings.setValue("gt_transform",        _ui->lineEdit_cameraImages_gt_transform->text());
 	settings.setValue("max_pose_time_diff",  _ui->doubleSpinBox_maxPoseTimeDiff->value());
 	settings.setValue("imu_path",            _ui->lineEdit_cameraImages_path_imu->text());
 	settings.setValue("imu_local_transform", _ui->lineEdit_cameraImages_imu_transform->text());
@@ -6525,6 +6532,15 @@ Transform PreferencesDialog::getLaserLocalTransform() const
 	}
 	return t;
 }
+Transform PreferencesDialog::getGroundTruthLocalTransform() const
+{
+	Transform t = Transform::fromString(_ui->lineEdit_cameraImages_gt_transform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString());
+	if(t.isNull())
+	{
+		return Transform::getIdentity();
+	}
+	return t;
+}
 
 QString PreferencesDialog::getIMUPath() const
 {
@@ -6891,7 +6907,7 @@ Camera * PreferencesDialog::createCamera(
 		((CameraRGBDImages*)camera)->setMaxFrames(_ui->spinBox_cameraRGBDImages_maxFrames->value());
 		((CameraRGBDImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
 		((CameraRGBDImages*)camera)->setOdometryPath(_ui->lineEdit_cameraImages_odom->text().toStdString(), _ui->comboBox_cameraImages_odomFormat->currentIndex());
-		((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
+		((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex(), this->getGroundTruthLocalTransform());
 		((CameraRGBDImages*)camera)->setMaxPoseTimeDiff(_ui->doubleSpinBox_maxPoseTimeDiff->value());
 		((CameraRGBDImages*)camera)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
@@ -6937,7 +6953,7 @@ Camera * PreferencesDialog::createCamera(
 		((CameraStereoImages*)camera)->setMaxFrames(_ui->spinBox_cameraStereoImages_maxFrames->value());
 		((CameraStereoImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
 		((CameraStereoImages*)camera)->setOdometryPath(_ui->lineEdit_cameraImages_odom->text().toStdString(), _ui->comboBox_cameraImages_odomFormat->currentIndex());
-		((CameraStereoImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
+		((CameraStereoImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex(), this->getGroundTruthLocalTransform());
 		((CameraStereoImages*)camera)->setMaxPoseTimeDiff(_ui->doubleSpinBox_maxPoseTimeDiff->value());
 		((CameraStereoImages*)camera)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
@@ -7137,7 +7153,8 @@ Camera * PreferencesDialog::createCamera(
 				_ui->comboBox_cameraImages_odomFormat->currentIndex());
 		((CameraImages*)camera)->setGroundTruthPath(
 				_ui->lineEdit_cameraImages_gt->text().toStdString(),
-				_ui->comboBox_cameraImages_gtFormat->currentIndex());
+				_ui->comboBox_cameraImages_gtFormat->currentIndex(),
+				this->getGroundTruthLocalTransform());
 		((CameraImages*)camera)->setMaxPoseTimeDiff(_ui->doubleSpinBox_maxPoseTimeDiff->value());
 		((CameraImages*)camera)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),

guilib/src/StatsToolBox.cpp

@@ -69,6 +69,7 @@ StatItem::StatItem(const QString & name, bool cacheOn, const std::vector<qreal>
 		_y = y;
 	}
 	_unit->setText(unit);
+	_value->setTextFormat(Qt::PlainText);
 	this->updateMenu(menu);
 }
 
@@ -90,7 +91,6 @@ void StatItem::addValue(qreal y)
 	{
 		_y.push_back(y);
 	}
-	_value->setText(QString::number(y, 'g', 3));
 	Q_EMIT valueAdded(y);
 }
 
@@ -107,7 +107,6 @@ void StatItem::addValue(qreal x, qreal y)
 		_x.push_back(x);
 	}
 
-	_value->setText(QString::number(y, 'g', 3));
 	Q_EMIT valueAdded(x,y);
 }
 
@@ -117,18 +116,23 @@ void StatItem::setValues(const std::vector<qreal> & x, const std::vector<qreal>
 	{
 		_x = x;
 		_y = y;
-		if(y.size())
-		{
-			_value->setNum(y[y.size()-1]);
-		}
-	}
-	else
-	{
-		_value->setText("*");
 	}
 	Q_EMIT valuesChanged(x,y);
 }
 
+void StatItem::updateLabel()
+{
+	QString newText;
+	if(_y.size())
+	{
+		newText = QString::number(_y.back(), 'g', 3);
+	}
+	if(newText != _value->text())
+	{
+		_value->setText(newText);
+	}
+}
+
 QString StatItem::value() const
 {
 	return _value->text();
@@ -227,6 +231,8 @@ StatsToolBox::StatsToolBox(QWidget * parent) :
 	_plotMenu->addAction(tr("<New figure>"));
 	_workingDirectory = QDir::homePath();
 	_newFigureMaxItems = 0;
+	_updateLabelsTimer.setSingleShot(true);
+	connect(&_updateLabelsTimer, &QTimer::timeout, this, &StatsToolBox::updateLabels);
 }
 
 StatsToolBox::~StatsToolBox()
@@ -263,6 +269,7 @@ void StatsToolBox::updateStat(const QString & statFullName, qreal y, bool cacheO
 	std::vector<qreal> vx,vy(1);
 	vy[0] = y;
 	updateStat(statFullName, vx, vy, cacheOn);
+	requestLabelsUpdate();
 }
 
 void StatsToolBox::updateStat(const QString & statFullName, qreal x, qreal y, bool cacheOn)
@@ -271,6 +278,7 @@ void StatsToolBox::updateStat(const QString & statFullName, qreal x, qreal y, bo
 	vx[0] = x;
 	vy[0] = y;
 	updateStat(statFullName, vx, vy, cacheOn);
+	requestLabelsUpdate();
 }
 
 void StatsToolBox::updateStat(const QString & statFullName, const std::vector<qreal> & x, const std::vector<qreal> & y, bool cacheOn)
@@ -295,6 +303,7 @@ void StatsToolBox::updateStat(const QString & statFullName, const std::vector<qr
 		{
 			item->setValues(x, y);
 		}
+		requestLabelsUpdate();
 	}
 	else
 	{
@@ -378,6 +387,22 @@ void StatsToolBox::updateStat(const QString & statFullName, const std::vector<qr
 	}
 }
 
+void StatsToolBox::requestLabelsUpdate()
+{
+	if(!_updateLabelsTimer.isActive())
+	{
+		_updateLabelsTimer.start(100); // Max 10 Hz
+	}
+}
+void StatsToolBox::updateLabels()
+{
+	QList<StatItem *> items = _statBox->findChildren<StatItem *>();
+	for(int i=0; i<items.size(); ++i)
+	{
+		items[i]->updateLabel();
+	}
+}
+
 void StatsToolBox::plot(const StatItem * stat, const QString & plotName)
 {
 	QWidget * fig = _figures.value(plotName, (QWidget*)0);

guilib/src/ui/DatabaseViewer.ui

@@ -1729,7 +1729,7 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>314</width>
+          <width>403</width>
           <height>188</height>
          </rect>
         </property>
@@ -1885,8 +1885,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>276</width>
+          <width>518</width>
-          <height>1598</height>
+          <height>1071</height>
          </rect>
         </property>
         <attribute name="label">
@@ -2604,15 +2604,29 @@
         <property name="geometry">
          <rect>
           <x>0</x>
-          <y>-152</y>
+          <y>-175</y>
           <width>403</width>
-          <height>287</height>
+          <height>319</height>
          </rect>
         </property>
         <attribute name="label">
          <string>Detect more loop closures</string>
         </attribute>
         <layout class="QGridLayout" name="gridLayout_10" columnstretch="0,1">
+         <item row="2" column="1">
+          <widget class="QLabel" name="label_30">
+           <property name="text">
+            <string>Angle</string>
+           </property>
+          </widget>
+         </item>
+         <item row="5" column="1">
+          <widget class="QLabel" name="label_34">
+           <property name="text">
+            <string>Inter-session</string>
+           </property>
+          </widget>
+         </item>
          <item row="5" column="0">
           <widget class="QCheckBox" name="checkBox_detectMore_interSession">
            <property name="text">
@@ -2620,6 +2634,103 @@
            </property>
           </widget>
          </item>
+         <item row="0" column="1">
+          <widget class="QLabel" name="label_29">
+           <property name="text">
+            <string>Radius Max</string>
+           </property>
+          </widget>
+         </item>
+         <item row="4" column="1">
+          <widget class="QLabel" name="label_32">
+           <property name="text">
+            <string>Intra-session</string>
+           </property>
+          </widget>
+         </item>
+         <item row="9" column="0">
+          <spacer name="verticalSpacer_5">
+           <property name="orientation">
+            <enum>Qt::Vertical</enum>
+           </property>
+           <property name="sizeHint" stdset="0">
+            <size>
+             <width>20</width>
+             <height>40</height>
+            </size>
+           </property>
+          </spacer>
+         </item>
+         <item row="2" column="0">
+          <widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_angle">
+           <property name="suffix">
+            <string> degrees</string>
+           </property>
+           <property name="decimals">
+            <number>0</number>
+           </property>
+           <property name="maximum">
+            <double>180.000000000000000</double>
+           </property>
+           <property name="singleStep">
+            <double>1.000000000000000</double>
+           </property>
+           <property name="value">
+            <double>30.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="1" column="1">
+          <widget class="QLabel" name="label_36">
+           <property name="text">
+            <string>Radius Min</string>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="0">
+          <widget class="QSpinBox" name="spinBox_detectMore_iterations">
+           <property name="minimum">
+            <number>1</number>
+           </property>
+           <property name="maximum">
+            <number>100</number>
+           </property>
+           <property name="value">
+            <number>5</number>
+           </property>
+          </widget>
+         </item>
+         <item row="7" column="1">
+          <widget class="QLabel" name="label_38">
+           <property name="text">
+            <string>From/to map ID only (-1 is all)</string>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="1">
+          <widget class="QLabel" name="label_31">
+           <property name="text">
+            <string>Iterations</string>
+           </property>
+          </widget>
+         </item>
+         <item row="6" column="0">
+          <widget class="QCheckBox" name="checkBox_opt_graph_as_guess">
+           <property name="text">
+            <string/>
+           </property>
+           <property name="checked">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="4" column="0">
+          <widget class="QCheckBox" name="checkBox_detectMore_intraSession">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
          <item row="1" column="0">
           <widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_radiusMin">
            <property name="suffix">
@@ -2639,43 +2750,6 @@
            </property>
           </widget>
          </item>
-         <item row="4" column="1">
-          <widget class="QLabel" name="label_32">
-           <property name="text">
-            <string>Intra-session</string>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="1">
-          <widget class="QLabel" name="label_31">
-           <property name="text">
-            <string>Iterations</string>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="0">
-          <widget class="QSpinBox" name="spinBox_detectMore_iterations">
-           <property name="minimum">
-            <number>1</number>
-           </property>
-           <property name="maximum">
-            <number>100</number>
-           </property>
-           <property name="value">
-            <number>5</number>
-           </property>
-          </widget>
-         </item>
-         <item row="6" column="0">
-          <widget class="QCheckBox" name="checkBox_opt_graph_as_guess">
-           <property name="text">
-            <string/>
-           </property>
-           <property name="checked">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
          <item row="6" column="1">
           <widget class="QLabel" name="label_37">
            <property name="text">
@@ -2686,13 +2760,6 @@
            </property>
           </widget>
          </item>
-         <item row="2" column="1">
-          <widget class="QLabel" name="label_30">
-           <property name="text">
-            <string>Angle</string>
-           </property>
-          </widget>
-         </item>
          <item row="0" column="0">
           <widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_radius">
            <property name="suffix">
@@ -2712,66 +2779,6 @@
            </property>
           </widget>
          </item>
-         <item row="8" column="0">
-          <spacer name="verticalSpacer_5">
-           <property name="orientation">
-            <enum>Qt::Vertical</enum>
-           </property>
-           <property name="sizeHint" stdset="0">
-            <size>
-             <width>20</width>
-             <height>40</height>
-            </size>
-           </property>
-          </spacer>
-         </item>
-         <item row="4" column="0">
-          <widget class="QCheckBox" name="checkBox_detectMore_intraSession">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
-         <item row="0" column="1">
-          <widget class="QLabel" name="label_29">
-           <property name="text">
-            <string>Radius Max</string>
-           </property>
-          </widget>
-         </item>
-         <item row="1" column="1">
-          <widget class="QLabel" name="label_36">
-           <property name="text">
-            <string>Radius Min</string>
-           </property>
-          </widget>
-         </item>
-         <item row="5" column="1">
-          <widget class="QLabel" name="label_34">
-           <property name="text">
-            <string>Inter-session</string>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="0">
-          <widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_angle">
-           <property name="suffix">
-            <string> degrees</string>
-           </property>
-           <property name="decimals">
-            <number>0</number>
-           </property>
-           <property name="maximum">
-            <double>180.000000000000000</double>
-           </property>
-           <property name="singleStep">
-            <double>1.000000000000000</double>
-           </property>
-           <property name="value">
-            <double>30.000000000000000</double>
-           </property>
-          </widget>
-         </item>
          <item row="7" column="0">
           <widget class="QSpinBox" name="spinBox_fromToMapId">
            <property name="minimum">
@@ -2785,10 +2792,23 @@
            </property>
           </widget>
          </item>
-         <item row="7" column="1">
+         <item row="8" column="1">
-          <widget class="QLabel" name="label_38">
+          <widget class="QLabel" name="label_40">
            <property name="text">
-            <string>From/to map ID only (-1 is all)</string>
+            <string>Minimum graph distance</string>
+           </property>
+          </widget>
+         </item>
+         <item row="8" column="0">
+          <widget class="QSpinBox" name="spinBox_minGraphDistance">
+           <property name="minimum">
+            <number>1</number>
+           </property>
+           <property name="maximum">
+            <number>9999</number>
+           </property>
+           <property name="value">
+            <number>10</number>
            </property>
           </widget>
          </item>
@@ -2799,8 +2819,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>181</width>
+          <width>428</width>
-          <height>485</height>
+          <height>196</height>
          </rect>
         </property>
         <attribute name="label">

guilib/src/ui/postProcessingDialog.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>552</width>
+    <width>553</width>
-    <height>633</height>
+    <height>662</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -67,10 +67,10 @@
           </property>
          </widget>
         </item>
-        <item row="0" column="1">
+        <item row="4" column="1">
-         <widget class="QLabel" name="label_3">
+         <widget class="QLabel" name="label_9">
           <property name="text">
-           <string>Cluster radius</string>
+           <string>Inter-session</string>
           </property>
           <property name="wordWrap">
            <bool>true</bool>
@@ -87,10 +87,10 @@
           </property>
          </widget>
         </item>
-        <item row="2" column="1">
+        <item row="0" column="1">
-         <widget class="QLabel" name="label_6">
+         <widget class="QLabel" name="label_3">
           <property name="text">
-           <string>Iterations</string>
+           <string>Cluster radius</string>
           </property>
           <property name="wordWrap">
            <bool>true</bool>
@@ -120,16 +120,6 @@
           </property>
          </widget>
         </item>
-        <item row="4" column="1">
-         <widget class="QLabel" name="label_9">
-          <property name="text">
-           <string>Inter-session</string>
-          </property>
-          <property name="wordWrap">
-           <bool>true</bool>
-          </property>
-         </widget>
-        </item>
         <item row="3" column="0">
          <widget class="QCheckBox" name="intraSession">
           <property name="text">
@@ -137,6 +127,16 @@
           </property>
          </widget>
         </item>
+        <item row="2" column="1">
+         <widget class="QLabel" name="label_6">
+          <property name="text">
+           <string>Iterations</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
         <item row="4" column="0">
          <widget class="QCheckBox" name="interSession">
           <property name="text">
@@ -144,6 +144,32 @@
           </property>
          </widget>
         </item>
+        <item row="5" column="1">
+         <widget class="QLabel" name="label_11">
+          <property name="text">
+           <string>Minimum graph distance</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
+        <item row="5" column="0">
+         <widget class="QSpinBox" name="minGraphDistance">
+          <property name="suffix">
+           <string> nodes</string>
+          </property>
+          <property name="minimum">
+           <number>1</number>
+          </property>
+          <property name="maximum">
+           <number>9999</number>
+          </property>
+          <property name="value">
+           <number>10</number>
+          </property>
+         </widget>
+        </item>
        </layout>
       </item>
       <item>

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-96</y>
             <width>684</width>
-            <height>5110</height>
+            <height>5218</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>24</number>
+                <number>9</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -7536,129 +7536,50 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                           </property>
                           <item>
                            <layout class="QGridLayout" name="gridLayout_68" columnstretch="0,0,1">
-                            <item row="6" column="0">
+                            <item row="3" column="1">
-                             <widget class="QToolButton" name="toolButton_cameraImages_odom">
+                             <widget class="QCheckBox" name="checkBox_cameraImages_syncTimeStamps">
+                              <property name="text">
+                               <string/>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="11" column="1">
+                             <widget class="QDoubleSpinBox" name="doubleSpinBox_maxPoseTimeDiff">
+                              <property name="suffix">
+                               <string> s</string>
+                              </property>
+                              <property name="decimals">
+                               <number>4</number>
+                              </property>
+                              <property name="maximum">
+                               <double>9.990000000000000</double>
+                              </property>
+                              <property name="singleStep">
+                               <double>0.010000000000000</double>
+                              </property>
+                              <property name="value">
+                               <double>0.020000000000000</double>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="1" column="1">
+                             <widget class="QCheckBox" name="checkBox_cameraImages_configForEachFrame">
+                              <property name="text">
+                               <string/>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="15" column="0">
+                             <widget class="QToolButton" name="toolButton_cameraImages_path_imu">
                               <property name="text">
                                <string>...</string>
                               </property>
                              </widget>
                             </item>
-                            <item row="0" column="2">
-                             <widget class="QLabel" name="label_265">
-                              <property name="text">
-                               <string>Bayer mode. For convenience, if the images are bayered.</string>
-                              </property>
-                              <property name="wordWrap">
-                               <bool>true</bool>
-                              </property>
-                              <property name="textInteractionFlags">
-                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="16" column="2">
-                             <widget class="QLabel" name="label_465">
-                              <property name="text">
-                               <string>IMU Rate. To synchronize capture rate with IMU timestamps, set to 0. This can be set a little over the actual IMU rate to keep up with camera capture rate if images are dropped by odometry.</string>
-                              </property>
-                              <property name="wordWrap">
-                               <bool>true</bool>
-                              </property>
-                              <property name="textInteractionFlags">
-                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="14" column="1">
-                             <widget class="QLineEdit" name="lineEdit_cameraImages_path_imu">
-                              <property name="text">
-                               <string/>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="15" column="1">
-                             <widget class="QLineEdit" name="lineEdit_cameraImages_imu_transform">
-                              <property name="toolTip">
-                               <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;EuRoC: /base_link to /imu = 0 0 1 0 -1 0 1 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-                              </property>
-                              <property name="text">
-                               <string>0 0 0 0 0 0</string>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="8" column="2">
-                             <widget class="QLabel" name="label_288">
-                              <property name="text">
-                               <string>Ground truth file. Select the correct format below.</string>
-                              </property>
-                              <property name="wordWrap">
-                               <bool>true</bool>
-                              </property>
-                              <property name="textInteractionFlags">
-                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="12" column="1">
-                             <widget class="QLineEdit" name="lineEdit_cameraImages_laser_transform">
-                              <property name="toolTip">
-                               <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0&lt;br/&gt;KITTI: /base_footprint to /scan = -0.27 0 1.75 0 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-                              </property>
-                              <property name="text">
-                               <string>0 0 0 0 0 0</string>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="13" column="2">
-                             <widget class="QLabel" name="label_292">
-                              <property name="text">
-                               <string>Maximum laser scan points.</string>
-                              </property>
-                              <property name="wordWrap">
-                               <bool>true</bool>
-                              </property>
-                              <property name="textInteractionFlags">
-                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="6" column="2">
-                             <widget class="QLabel" name="label_348">
-                              <property name="text">
-                               <string>Odometry file. Select the correct format below.</string>
-                              </property>
-                              <property name="wordWrap">
-                               <bool>true</bool>
-                              </property>
-                              <property name="textInteractionFlags">
-                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                              </property>
-                             </widget>
-                            </item>
                             <item row="15" column="2">
-                             <widget class="QLabel" name="label_464">
+                             <widget class="QLabel" name="label_463">
                               <property name="text">
-                               <string>Local transform from /base_link to /imu_link. Mouse over the box to show formats.</string>
+                               <string>Path to file containing optional IMU data (*.csv [EuRoC format]). Mouse over the box to show formats.</string>
-                              </property>
-                              <property name="wordWrap">
-                               <bool>true</bool>
-                              </property>
-                              <property name="textInteractionFlags">
-                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="6" column="1">
-                             <widget class="QLineEdit" name="lineEdit_cameraImages_odom">
-                              <property name="text">
-                               <string/>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="3" column="2">
-                             <widget class="QLabel" name="label_256">
-                              <property name="text">
-                               <string>Synchronize capture rate with timestamps.</string>
                               </property>
                               <property name="wordWrap">
                                <bool>true</bool>
@@ -7743,10 +7664,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                               </item>
                              </widget>
                             </item>
-                            <item row="4" column="2">
+                            <item row="17" column="2">
-                             <widget class="QLabel" name="label_251">
+                             <widget class="QLabel" name="label_465">
                               <property name="text">
-                               <string>Timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if &quot;Use file names as timestamps&quot; above is checked. </string>
+                               <string>IMU Rate. To synchronize capture rate with IMU timestamps, set to 0. This can be set a little over the actual IMU rate to keep up with camera capture rate if images are dropped by odometry.</string>
                               </property>
                               <property name="wordWrap">
                                <bool>true</bool>
@@ -7756,10 +7677,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                               </property>
                              </widget>
                             </item>
-                            <item row="7" column="2">
+                            <item row="3" column="2">
-                             <widget class="QLabel" name="label_349">
+                             <widget class="QLabel" name="label_256">
                               <property name="text">
-                               <string>Odometry format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images.</string>
+                               <string>Synchronize capture rate with timestamps.</string>
                               </property>
                               <property name="wordWrap">
                                <bool>true</bool>
@@ -7769,51 +7690,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                               </property>
                              </widget>
                             </item>
-                            <item row="10" column="2">
+                            <item row="13" column="2">
-                             <widget class="QLabel" name="label_443">
-                              <property name="text">
-                               <string>Max time difference between data and corresponding pose for format with stamps. If delay is over this threshold, the pose won't be set on data loaded. This is used when odometry and/or ground truth files are set.</string>
-                              </property>
-                              <property name="wordWrap">
-                               <bool>true</bool>
-                              </property>
-                              <property name="textInteractionFlags">
-                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="11" column="1">
-                             <widget class="QLineEdit" name="lineEdit_cameraImages_path_scans">
-                              <property name="text">
-                               <string/>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="3" column="1">
-                             <widget class="QCheckBox" name="checkBox_cameraImages_syncTimeStamps">
-                              <property name="text">
-                               <string/>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="11" column="0">
-                             <widget class="QToolButton" name="toolButton_cameraImages_path_scans">
-                              <property name="text">
-                               <string>...</string>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="16" column="1">
-                             <widget class="QSpinBox" name="spinBox_cameraImages_max_imu_rate">
-                              <property name="toolTip">
-                               <string>EuRoC: 200 Hz -&gt; 250 Hz</string>
-                              </property>
-                              <property name="maximum">
-                               <number>99999999</number>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="12" column="2">
                              <widget class="QLabel" name="label_294">
                               <property name="text">
                                <string>Local transform from /base_link to /laser_link. Mouse over the box to show formats.</string>
@@ -7826,7 +7703,60 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                               </property>
                              </widget>
                             </item>
-                            <item row="11" column="2">
+                            <item row="6" column="2">
+                             <widget class="QLabel" name="label_348">
+                              <property name="text">
+                               <string>Odometry file. Select the correct format below.</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="15" column="1">
+                             <widget class="QLineEdit" name="lineEdit_cameraImages_path_imu">
+                              <property name="toolTip">
+                               <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;cvs format (comma split): &amp;quot;stamp_sec,gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z&amp;quot;&lt;/p&gt;&lt;p&gt;EuRoC format: &amp;quot;stamp_nanosec,gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z&amp;quot;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                              </property>
+                              <property name="text">
+                               <string/>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="16" column="2">
+                             <widget class="QLabel" name="label_464">
+                              <property name="text">
+                               <string>Local transform from /base_link to /imu_link. Mouse over the box to show formats.</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="12" column="1">
+                             <widget class="QLineEdit" name="lineEdit_cameraImages_path_scans">
+                              <property name="text">
+                               <string/>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="13" column="1">
+                             <widget class="QLineEdit" name="lineEdit_cameraImages_laser_transform">
+                              <property name="toolTip">
+                               <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0&lt;br/&gt;KITTI: /base_footprint to /scan = -0.27 0 1.75 0 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                              </property>
+                              <property name="text">
+                               <string>0 0 0 0 0 0</string>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="12" column="2">
                              <widget class="QLabel" name="label_293">
                               <property name="text">
                                <string>Path to directory containing optional laser scans (*.pcd, *.ply, *.bin [KITTI format]). The directory should have the same size has the images directory. </string>
@@ -7839,6 +7769,141 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                               </property>
                              </widget>
                             </item>
+                            <item row="2" column="1">
+                             <widget class="QCheckBox" name="checkBox_cameraImages_timestamps">
+                              <property name="text">
+                               <string/>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="14" column="2">
+                             <widget class="QLabel" name="label_292">
+                              <property name="text">
+                               <string>Maximum laser scan points.</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="14" column="1">
+                             <widget class="QSpinBox" name="spinBox_cameraImages_max_scan_pts">
+                              <property name="toolTip">
+                               <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                              </property>
+                              <property name="maximum">
+                               <number>99999999</number>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="17" column="1">
+                             <widget class="QSpinBox" name="spinBox_cameraImages_max_imu_rate">
+                              <property name="toolTip">
+                               <string>EuRoC: 200 Hz -&gt; 250 Hz</string>
+                              </property>
+                              <property name="maximum">
+                               <number>99999999</number>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="0" column="1">
+                             <widget class="QComboBox" name="comboBox_cameraImages_bayerMode">
+                              <property name="sizeAdjustPolicy">
+                               <enum>QComboBox::AdjustToContents</enum>
+                              </property>
+                              <item>
+                               <property name="text">
+                                <string>Disabled</string>
+                               </property>
+                              </item>
+                              <item>
+                               <property name="text">
+                                <string>BG</string>
+                               </property>
+                              </item>
+                              <item>
+                               <property name="text">
+                                <string>GB</string>
+                               </property>
+                              </item>
+                              <item>
+                               <property name="text">
+                                <string>RG</string>
+                               </property>
+                              </item>
+                              <item>
+                               <property name="text">
+                                <string>GR</string>
+                               </property>
+                              </item>
+                             </widget>
+                            </item>
+                            <item row="11" column="2">
+                             <widget class="QLabel" name="label_443">
+                              <property name="text">
+                               <string>Max time difference between data and corresponding pose for format with stamps. If delay is over this threshold, the pose won't be set on data loaded. This is used when odometry and/or ground truth files are set.</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="12" column="0">
+                             <widget class="QToolButton" name="toolButton_cameraImages_path_scans">
+                              <property name="text">
+                               <string>...</string>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="4" column="1">
+                             <widget class="QLineEdit" name="lineEdit_cameraImages_timestamps">
+                              <property name="text">
+                               <string/>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="2" column="2">
+                             <widget class="QLabel" name="label_255">
+                              <property name="text">
+                               <string>Use file names as timestamps. Format is epoch time. Example: &quot;1305031102.175304.png&quot;.</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="16" column="1">
+                             <widget class="QLineEdit" name="lineEdit_cameraImages_imu_transform">
+                              <property name="toolTip">
+                               <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;EuRoC: /base_link to /imu = 0 0 1 0 -1 0 1 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                              </property>
+                              <property name="text">
+                               <string>0 0 0 0 0 0</string>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="8" column="2">
+                             <widget class="QLabel" name="label_288">
+                              <property name="text">
+                               <string>Ground truth file. Select the correct format below.</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
                             <item row="8" column="0">
                              <widget class="QToolButton" name="toolButton_cameraImages_gt">
                               <property name="text">
@@ -7846,16 +7911,17 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                               </property>
                              </widget>
                             </item>
-                            <item row="2" column="2">
+                            <item row="8" column="1">
-                             <widget class="QLabel" name="label_255">
+                             <widget class="QLineEdit" name="lineEdit_cameraImages_gt">
                               <property name="text">
-                               <string>Use file names as timestamps. Format is epoch time. Example: &quot;1305031102.175304.png&quot;.</string>
+                               <string/>
                               </property>
-                              <property name="wordWrap">
+                             </widget>
-                               <bool>true</bool>
+                            </item>
-                              </property>
+                            <item row="4" column="0">
-                              <property name="textInteractionFlags">
+                             <widget class="QToolButton" name="toolButton_cameraImages_timestamps">
-                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              <property name="text">
+                               <string>...</string>
                               </property>
                              </widget>
                             </item>
@@ -7934,25 +8000,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                               </item>
                              </widget>
                             </item>
-                            <item row="10" column="1">
-                             <widget class="QDoubleSpinBox" name="doubleSpinBox_maxPoseTimeDiff">
-                              <property name="suffix">
-                               <string> s</string>
-                              </property>
-                              <property name="decimals">
-                               <number>4</number>
-                              </property>
-                              <property name="maximum">
-                               <double>9.990000000000000</double>
-                              </property>
-                              <property name="singleStep">
-                               <double>0.010000000000000</double>
-                              </property>
-                              <property name="value">
-                               <double>0.020000000000000</double>
-                              </property>
-                             </widget>
-                            </item>
                             <item row="1" column="2">
                              <widget class="QLabel" name="label_605">
                               <property name="text">
@@ -7966,59 +8013,17 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                               </property>
                              </widget>
                             </item>
-                            <item row="13" column="1">
+                            <item row="6" column="0">
-                             <widget class="QSpinBox" name="spinBox_cameraImages_max_scan_pts">
+                             <widget class="QToolButton" name="toolButton_cameraImages_odom">
-                              <property name="toolTip">
-                               <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-                              </property>
-                              <property name="maximum">
-                               <number>99999999</number>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="1" column="1">
-                             <widget class="QCheckBox" name="checkBox_cameraImages_configForEachFrame">
                               <property name="text">
-                               <string/>
+                               <string>...</string>
                               </property>
                              </widget>
                             </item>
-                            <item row="0" column="1">
+                            <item row="7" column="2">
-                             <widget class="QComboBox" name="comboBox_cameraImages_bayerMode">
+                             <widget class="QLabel" name="label_349">
-                              <property name="sizeAdjustPolicy">
-                               <enum>QComboBox::AdjustToContents</enum>
-                              </property>
-                              <item>
-                               <property name="text">
-                                <string>Disabled</string>
-                               </property>
-                              </item>
-                              <item>
-                               <property name="text">
-                                <string>BG</string>
-                               </property>
-                              </item>
-                              <item>
-                               <property name="text">
-                                <string>GB</string>
-                               </property>
-                              </item>
-                              <item>
-                               <property name="text">
-                                <string>RG</string>
-                               </property>
-                              </item>
-                              <item>
-                               <property name="text">
-                                <string>GR</string>
-                               </property>
-                              </item>
-                             </widget>
-                            </item>
-                            <item row="14" column="2">
-                             <widget class="QLabel" name="label_463">
                               <property name="text">
-                               <string>Path to file containing optional IMU data (*.csv [EuRoC format]).</string>
+                               <string>Odometry format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images.</string>
                               </property>
                               <property name="wordWrap">
                                <bool>true</bool>
@@ -8028,8 +8033,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                               </property>
                              </widget>
                             </item>
-                            <item row="4" column="1">
+                            <item row="6" column="1">
-                             <widget class="QLineEdit" name="lineEdit_cameraImages_timestamps">
+                             <widget class="QLineEdit" name="lineEdit_cameraImages_odom">
                               <property name="text">
                                <string/>
                               </property>
@@ -8048,31 +8053,52 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                               </property>
                              </widget>
                             </item>
-                            <item row="2" column="1">
+                            <item row="4" column="2">
-                             <widget class="QCheckBox" name="checkBox_cameraImages_timestamps">
+                             <widget class="QLabel" name="label_251">
                               <property name="text">
-                               <string/>
+                               <string>Timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if &quot;Use file names as timestamps&quot; above is checked. </string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                               </property>
                              </widget>
                             </item>
-                            <item row="14" column="0">
+                            <item row="0" column="2">
-                             <widget class="QToolButton" name="toolButton_cameraImages_path_imu">
+                             <widget class="QLabel" name="label_265">
                               <property name="text">
-                               <string>...</string>
+                               <string>Bayer mode. For convenience, if the images are bayered.</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                               </property>
                              </widget>
                             </item>
-                            <item row="8" column="1">
+                            <item row="10" column="2">
-                             <widget class="QLineEdit" name="lineEdit_cameraImages_gt">
+                             <widget class="QLabel" name="label_794">
                               <property name="text">
-                               <string/>
+                               <string>Local transform from /base_link to /gt_link. Mouse over the box to show formats. By default, we assume the ground truth matches the base frame, if the ground truth refers to another frame, set this to convert the poses in base frame.</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">
+                            <item row="10" column="1">
-                             <widget class="QToolButton" name="toolButton_cameraImages_timestamps">
+                             <widget class="QLineEdit" name="lineEdit_cameraImages_gt_transform">
+                              <property name="toolTip">
+                               <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                              </property>
                               <property name="text">
-                               <string>...</string>
+                               <string>0 0 0 0 0 0</string>
                               </property>
                              </widget>
                             </item>
@@ -11633,7 +11659,7 @@ generate the number of words requested.</string>
                          <item row="4" column="1">
                           <widget class="QLabel" name="label_591">
                            <property name="text">
-                            <string>Filter floor from depth mask. 0 means disabled, negative means keeping pixels below the floor theshold instead.</string>
+                            <string>Filter floor from depth mask. 0 means disabled.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -23834,7 +23860,7 @@ Lower the ratio -&gt; higher the precision.</string>
                          <item row="7" column="1">
                           <widget class="QLabel" name="label_759">
                            <property name="text">
-                            <string>Filter floor from depth mask. 0 means disabled, negative means keeping pixels below the floor theshold instead.</string>
+                            <string>Filter floor from depth mask. 0 means disabled.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -26707,39 +26733,6 @@ Lower the ratio -&gt; higher the precision.</string>
                     <string>SIFT</string>
                    </property>
                    <layout class="QGridLayout" name="gridLayout_19" columnstretch="0,1">
-                    <item row="4" column="0">
-                     <widget class="QCheckBox" name="sift_checkBox_preciseUpscale">
-                      <property name="text">
-                       <string/>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="2" column="1">
-                     <widget class="QLabel" name="label_30">
-                      <property name="text">
-                       <string>Edge threshold. The threshold used to filter out edge-like features. Note that the its meaning is different from the contrastThreshold, i.e. the larger the edgeThreshold, the less features are filtered out (more features are retained).</string>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
-                      </property>
-                      <property name="textInteractionFlags">
-                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="0" column="0">
-                     <widget class="QSpinBox" name="sift_spinBox_nOctaveLayers"/>
-                    </item>
-                    <item row="4" column="1">
-                     <widget class="QLabel" name="sift_label_preciseUpscale">
-                      <property name="text">
-                       <string>Whether to enable precise upscaling in the scale pyramid.</string>
-                      </property>
-                      <property name="textInteractionFlags">
-                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                      </property>
-                     </widget>
-                    </item>
                     <item row="0" column="1">
                      <widget class="QLabel" name="label_130">
                       <property name="text">
@@ -26753,13 +26746,53 @@ Lower the ratio -&gt; higher the precision.</string>
                       </property>
                      </widget>
                     </item>
-                    <item row="2" column="0">
+                    <item row="6" column="0">
-                     <widget class="QDoubleSpinBox" name="sift_doubleSpinBox_edgeThr">
+                     <widget class="QCheckBox" name="sift_checkBox_gpu">
-                      <property name="singleStep">
+                      <property name="text">
-                       <double>0.100000000000000</double>
+                       <string/>
                       </property>
-                      <property name="value">
+                     </widget>
-                       <double>10.000000000000000</double>
+                    </item>
+                    <item row="4" column="1">
+                     <widget class="QLabel" name="sift_label_preciseUpscale">
+                      <property name="text">
+                       <string>Whether to enable precise upscaling in the scale pyramid.</string>
+                      </property>
+                      <property name="textInteractionFlags">
+                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="7" column="1">
+                     <widget class="QLabel" name="sift_label_gaussianThreshold">
+                      <property name="text">
+                       <string>CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features with low response/hessian are produced by the detector.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                      <property name="textInteractionFlags">
+                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="9" column="0">
+                     <widget class="QCheckBox" name="sift_checkBox_upscale">
+                      <property name="text">
+                       <string/>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="1" column="1">
+                     <widget class="QLabel" name="label_5">
+                      <property name="text">
+                       <string>Contrast threshold. The contrast threshold used to filter out weak features in semi-uniform (low-contrast) regions. The larger the threshold, the less features are produced by the detector. Not used by CudaSift.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                      <property name="textInteractionFlags">
+                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                       </property>
                      </widget>
                     </item>
@@ -26770,6 +26803,42 @@ Lower the ratio -&gt; higher the precision.</string>
                       </property>
                      </widget>
                     </item>
+                    <item row="4" column="0">
+                     <widget class="QCheckBox" name="sift_checkBox_preciseUpscale">
+                      <property name="text">
+                       <string/>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="6" column="1">
+                     <widget class="QLabel" name="sift_label_gpu">
+                      <property name="text">
+                       <string>CudaSift: Use GPU version of SIFT. This option is enabled only RTAB-Map is built with CudaSift dependency and GPUs are detected.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                      <property name="textInteractionFlags">
+                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="0" column="0">
+                     <widget class="QSpinBox" name="sift_spinBox_nOctaveLayers"/>
+                    </item>
+                    <item row="3" column="1">
+                     <widget class="QLabel" name="label_129">
+                      <property name="text">
+                       <string>Sigma. The sigma of the Gaussian applied to the input image at the octave #0. If your image is captured with a weak camera with soft lenses, you might want to reduce the number.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                      <property name="textInteractionFlags">
+                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                      </property>
+                     </widget>
+                    </item>
                     <item row="1" column="0">
                      <widget class="QDoubleSpinBox" name="sift_doubleSpinBox_contrastThr">
                       <property name="decimals">
@@ -26799,23 +26868,20 @@ Lower the ratio -&gt; higher the precision.</string>
                       </property>
                      </widget>
                     </item>
-                    <item row="6" column="1">
+                    <item row="9" column="1">
-                     <widget class="QLabel" name="sift_label_gpu">
+                     <widget class="QLabel" name="sift_label_upscale">
                       <property name="text">
-                       <string>CudaSift: Use GPU version of SIFT. This option is enabled only RTAB-Map is built with CudaSift dependency and GPUs are detected.</string>
+                       <string>CudaSift: Whether to enable upscaling.</string>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
                       </property>
                       <property name="textInteractionFlags">
                        <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                       </property>
                      </widget>
                     </item>
-                    <item row="1" column="1">
+                    <item row="2" column="1">
-                     <widget class="QLabel" name="label_5">
+                     <widget class="QLabel" name="label_30">
                       <property name="text">
-                       <string>Contrast threshold. The contrast threshold used to filter out weak features in semi-uniform (low-contrast) regions. The larger the threshold, the less features are produced by the detector. Not used by CudaSift.</string>
+                       <string>Edge threshold. The threshold used to filter out edge-like features. Note that the its meaning is different from the contrastThreshold, i.e. the larger the edgeThreshold, the less features are filtered out (more features are retained).</string>
                       </property>
                       <property name="wordWrap">
                        <bool>true</bool>
@@ -26844,6 +26910,16 @@ Lower the ratio -&gt; higher the precision.</string>
                       </property>
                      </widget>
                     </item>
+                    <item row="2" column="0">
+                     <widget class="QDoubleSpinBox" name="sift_doubleSpinBox_edgeThr">
+                      <property name="singleStep">
+                       <double>0.100000000000000</double>
+                      </property>
+                      <property name="value">
+                       <double>10.000000000000000</double>
+                      </property>
+                     </widget>
+                    </item>
                     <item row="5" column="1">
                      <widget class="QLabel" name="sift_label_rootsift">
                       <property name="text">
@@ -26854,43 +26930,13 @@ Lower the ratio -&gt; higher the precision.</string>
                       </property>
                      </widget>
                     </item>
-                    <item row="7" column="1">
-                     <widget class="QLabel" name="sift_label_gaussianThreshold">
-                      <property name="text">
-                       <string>CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features are produced by the detector.</string>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
-                      </property>
-                      <property name="textInteractionFlags">
-                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="6" column="0">
-                     <widget class="QCheckBox" name="sift_checkBox_gpu">
-                      <property name="text">
-                       <string/>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="3" column="1">
-                     <widget class="QLabel" name="label_129">
-                      <property name="text">
-                       <string>Sigma. The sigma of the Gaussian applied to the input image at the octave #0. If your image is captured with a weak camera with soft lenses, you might want to reduce the number.</string>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
-                      </property>
-                      <property name="textInteractionFlags">
-                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                      </property>
-                     </widget>
-                    </item>
                     <item row="8" column="1">
-                     <widget class="QLabel" name="sift_label_upscale">
+                     <widget class="QLabel" name="sift_label_maxGaussianThreshold">
                       <property name="text">
-                       <string>CudaSift: Whether to enable upscaling.</string>
+                       <string>CudaSift: Maximum threshold on difference of Gaussians for feature pruning (ignored if smaller or equal than gaussian threshold above). The lower the threshold, the less features with high response/hessian are produced by the detector.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
                       </property>
                       <property name="textInteractionFlags">
                        <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
@@ -26898,9 +26944,21 @@ Lower the ratio -&gt; higher the precision.</string>
                      </widget>
                     </item>
                     <item row="8" column="0">
-                     <widget class="QCheckBox" name="sift_checkBox_upscale">
+                     <widget class="QDoubleSpinBox" name="sift_doubleSpinBox_maxGaussianDiffThreshold">
-                      <property name="text">
+                      <property name="decimals">
-                       <string/>
+                       <number>2</number>
+                      </property>
+                      <property name="minimum">
+                       <double>0.010000000000000</double>
+                      </property>
+                      <property name="maximum">
+                       <double>100.000000000000000</double>
+                      </property>
+                      <property name="singleStep">
+                       <double>0.100000000000000</double>
+                      </property>
+                      <property name="value">
+                       <double>4.500000000000000</double>
                       </property>
                      </widget>
                     </item>

tools/CMakeLists.txt

@@ -7,6 +7,7 @@ ADD_SUBDIRECTORY( StereoEval )
 ADD_SUBDIRECTORY( KittiDataset )
 ADD_SUBDIRECTORY( RgbdDataset )
 ADD_SUBDIRECTORY( EurocDataset )
+ADD_SUBDIRECTORY( CidSimsDataset )
 ADD_SUBDIRECTORY( Recovery )
 ADD_SUBDIRECTORY( Reprocess )
 ADD_SUBDIRECTORY( DetectMoreLoopClosures )
@@ -15,6 +16,7 @@ ADD_SUBDIRECTORY( Report )
 ADD_SUBDIRECTORY( Info )
 ADD_SUBDIRECTORY( CleanupLocalGrids )
 ADD_SUBDIRECTORY( GlobalBundleAdjustment )
+ADD_SUBDIRECTORY( ReduceGraph )
 
 IF(OPENCV_NONFREE_FOUND)
 ADD_SUBDIRECTORY( VocabularyComparison )

tools/CidSimsDataset/CMakeLists.txt

@@ -0,0 +1,11 @@
+
+ADD_EXECUTABLE(cidsims_dataset main.cpp)
+  
+TARGET_LINK_LIBRARIES(cidsims_dataset rtabmap_core)
+
+SET_TARGET_PROPERTIES( cidsims_dataset 
+    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-cidsims_dataset)
+    
+INSTALL(TARGETS cidsims_dataset
+	RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+	BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

tools/CidSimsDataset/main.cpp

@@ -0,0 +1,688 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/Odometry.h>
+#include "rtabmap/core/Rtabmap.h"
+#include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/Graph.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/Memory.h"
+#include "rtabmap/core/util3d_registration.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UMath.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UProcessInfo.h"
+#include <pcl/common/common.h>
+#include <rtabmap/core/SensorCaptureThread.h>
+#include <stdio.h>
+#include <signal.h>
+#include <fstream>
+
+using namespace rtabmap;
+
+void showUsage(const char * appName)
+{
+	printf("\nUsage:\n"
+			"%s [options] path\n"
+			"  path               Folder of the sequence (e.g., \"~/apartment1_1\")\n"
+			"                        containing color, depth, groundtruth.txt, imu.txt and odom.txt.\n"
+			"  --output           Output directory. By default, results are saved in \"path\".\n"
+			"  --output_name      Output database name (default \"rtabmap\").\n"
+			"  --max_time_diff #.#  Maximum time difference with frame to attribute a valid odometry and/or ground truth pose (default 0.1 s).\n"
+			"  --quiet            Don't show log messages and iteration updates.\n"
+			"  --use_imu          Use IMU.\n"
+			"  --gt               Record ground truth.\n"
+			"  --odom             Use wheel odometry as input guess to visual odometry.\n"
+			"  --imu #            Use IMU and set filter: 0=madgwick, 1=complementary.\n"
+			"  --quiet            Don't show log messages and iteration updates.\n"
+			"%s\n"
+			"Example:\n\n"
+			"   $ %s \\\n"
+			"       --Rtabmap/DetectionRate 2\\\n"
+			"       --RGBD/OptimizeMaxError 5\\\n"
+			"       --Mem/STMSize 30\\\n"
+			"       --gt\\\n"
+			"       --odom\\\n"
+			"       --imu 1\\\n"
+			"       ~/apartment1_1\n\n", appName, rtabmap::Parameters::showUsage(), appName);
+	exit(1);
+}
+
+// catch ctrl-c
+bool g_forever = true;
+void sighandler(int sig)
+{
+	printf("\nSignal %d caught...\n", sig);
+	g_forever = false;
+}
+
+int main(int argc, char * argv[])
+{
+	signal(SIGABRT, &sighandler);
+	signal(SIGTERM, &sighandler);
+	signal(SIGINT, &sighandler);
+
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	ParametersMap parameters;
+	std::string path;
+	std::string output;
+	std::string outputName = "rtabmap";
+	int skipFrames = 0;
+	float maxTimeDiff = 0.1f;
+	bool quiet = false;
+	int imuFilter = 1;
+	bool useImu = false;
+	bool useOdom = false;
+	bool recordGt = false;
+	if(argc < 2)
+	{
+		showUsage(argv[0]);
+	}
+	else
+	{
+		for(int i=1; i<argc; ++i)
+		{
+			if(std::strcmp(argv[i], "--output") == 0)
+			{
+				output = argv[++i];
+			}
+			else if(std::strcmp(argv[i], "--output_name") == 0)
+			{
+				outputName = argv[++i];
+			}
+			else if(std::strcmp(argv[i], "--max_time_diff") == 0)
+			{
+				maxTimeDiff = atof(argv[++i]);
+				UASSERT(maxTimeDiff > 0.0f);
+			}
+			else if(std::strcmp(argv[i], "--odom") == 0)
+			{
+				useOdom = true;
+			}
+			else if(std::strcmp(argv[i], "--imu") == 0)
+			{
+				useImu = true;
+				imuFilter = atoi(argv[++i]);
+			}
+			else if(std::strcmp(argv[i], "--gt") == 0)
+			{
+				recordGt = true;
+			}
+			else if(std::strcmp(argv[i], "--quiet") == 0)
+			{
+				quiet = true;
+			}
+		}
+		parameters = Parameters::parseArguments(argc, argv);
+		path = argv[argc-1];
+		path = uReplaceChar(path, '~', UDirectory::homeDir());
+		path = uReplaceChar(path, '\\', '/');
+		if(output.empty())
+		{
+			output = path;
+		}
+		else
+		{
+			output = uReplaceChar(output, '~', UDirectory::homeDir());
+			UDirectory::makeDir(output);
+		}
+		parameters.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), output));
+		parameters.insert(ParametersPair(Parameters::kRtabmapPublishRAMUsage(), "true"));
+	}
+
+	std::string seq = uSplit(path, '/').back();
+	std::string pathRgbImages  = path+"/color";
+	std::string pathDepthImages = path+"/depth";
+	std::string pathGt = recordGt?path+"/groundtruth.txt":"";
+	if(recordGt && !UFile::exists(pathGt))
+	{
+		UWARN("Ground truth file path doesn't exist: \"%s\", benchmark values won't be computed.", pathGt.c_str());
+		pathGt.clear();
+	}
+	std::string pathOdom = useOdom?path+"/rtabmap_odom.txt":"";
+	if(useOdom && !UFile::exists(pathOdom))
+	{
+		std::string orgOdom = path+"/odom.txt";
+		if(UFile::exists(orgOdom))
+		{
+			printf("Converting odom.txt to rtabmap_odom.txt...");
+			std::ifstream inputFile(orgOdom);
+			std::ofstream outputFile(pathOdom);
+			std::string line;
+
+			if (!inputFile.is_open()) {
+				UERROR("Error: Could not open input file: %s", orgOdom.c_str());
+				return 1;
+			}
+
+			double previousStamp = 0.0;
+			float odomX = 0.0f;
+			float odomY = 0.0f;
+			float odomTheta = 0.0f;
+			while (std::getline(inputFile, line)) {
+				auto strList = uListToVector(uSplit(line, ' '));
+				if(line.empty())
+				{
+					break;
+				}
+				if(strList.size() != 14)
+				{
+					UERROR("Odometry shoud, have 14 entries per line, got %ld: \"%s\"", strList.size(), line.c_str());
+					return 1;
+				}
+				// Recompute wheel odometry based on velocity
+				double stamp = uStr2Double(strList[0]);
+				if(previousStamp==0)
+				{
+					previousStamp = uStr2Double(strList[0]);
+				}
+				float dt = stamp - previousStamp;
+				float vx = uStr2Float(strList[8]);
+				float vtheta = uStr2Float(strList[13]);
+				
+				odomX += vx * cos(odomTheta) * dt;
+				odomY += vx * sin(odomTheta) * dt;
+				odomTheta = odomTheta + vtheta * dt;
+				
+				Transform t(odomX, odomY, odomTheta);
+				Eigen::Quaternionf q = t.getQuaternionf();
+				outputFile << strList[0] << ' ' << t.x() << ' ' << t.y() << ' ' << t.z() << ' ' << q.x() << ' ' << q.y() << ' ' << q.z() << ' ' << q.w() << std::endl;
+				previousStamp = stamp;
+			}
+
+			inputFile.close();
+			outputFile.close();
+			printf("Converting odom.txt to rtabmap_odom.txt...done!");
+		}
+		else 
+		{
+			pathOdom.clear();
+		}
+	}
+	std::string pathImu = useImu?path+"/imu.txt":"";
+	if(useImu && !UFile::exists(pathImu))
+	{
+		pathImu.clear();
+	}
+
+	if(quiet)
+	{
+		ULogger::setLevel(ULogger::kError);
+	}
+
+	printf("Paths:\n"
+			"   Dataset name:    %s\n"
+			"   Dataset path:    %s\n"
+			"   Color path:      %s\n"
+			"   Depth path:      %s\n"
+			"   Output:          %s\n"
+			"   Output name:     %s\n"
+			"   Max time diff:   %f\n",
+			seq.c_str(),
+			path.c_str(),
+			pathRgbImages.c_str(),
+			pathDepthImages.c_str(),
+			output.c_str(),
+			outputName.c_str(),
+			maxTimeDiff);
+	printf("   Ground Truth: %s\n", !pathGt.empty()?pathGt.c_str():"Set --gt to record ground truth");
+	printf("   Odometry:     %s\n", !pathOdom.empty()?pathOdom.c_str():"Set --odom use wheel odometry");
+	printf("   IMU:          %s\n", !pathImu.empty()?pathImu.c_str():"Set --imu 1 to use IMU");
+	printf("   IMU Filter:   %d\n", imuFilter);
+	if(!parameters.empty())
+	{
+		printf("Parameters:\n");
+		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+		{
+			printf("   %s=%s\n", iter->first.c_str(), iter->second.c_str());
+		}
+	}
+	printf("RTAB-Map version: %s\n", RTABMAP_VERSION);
+
+	// setup calibration file, based on https://cid-sims.github.io/calibration/calibration.yaml
+	CameraModel model(outputName+"_calib", 
+			386.52199190267083, 387.32300428823663, 
+			326.5103569741365, 237.40293732598795, 
+			Transform::getIdentity(), 0, cv::Size(640,480));
+	std::string sequenceName = UFile(path).getName();
+	// The ground truth corresponds to camera frame, thus make the camera the base frame
+	Transform cameraHigh(
+		0.013246,  0.0521412, 0.9982324, pathGt.empty()?0.28:0,
+		-0.9962321, 0.0610127, 0.0024175, 0,
+		-0.05647427, -0.9950674, 0.0591213, pathGt.empty()?0.20:0);
+	Transform cameraLow(
+		-0.00477153, 0.0888742, 0.995711, pathGt.empty()?0.369117:0, 
+		-0.99571, 0.0665738, -0.018348, pathGt.empty()?0.0130432:0, 
+		-0.0637357, -0.99188, 0.094639, pathGt.empty()?0.016175:0);
+	Transform camImu(
+		0.9999691, 0.00720362, -0.00314765, -0.02707507,
+		-0.0071841, 0.99995517, 0.0061682, -0.004337,
+		0.00319195, -0.0061454, 0.99997602, -0.01595186);
+	Transform imuHigh = cameraHigh * camImu; // base->IMU
+	Transform imuLow = cameraLow * camImu; // base->IMU
+	Transform baseToImu;
+
+	// Based on https://cid-sims.github.io/overview/index.html
+	if( sequenceName.find("apartment1_1") != std::string::npos ||
+		sequenceName.find("apartment2_1") != std::string::npos ||
+		sequenceName.find("apartment2_3") != std::string::npos ||
+		sequenceName.find("apartment3_1") != std::string::npos ||
+		sequenceName.find("apartment3_1") != std::string::npos)
+	{
+		// using camera low
+		model.setLocalTransform(cameraLow);
+		baseToImu = imuLow;
+	}
+	else
+	{
+		// using camera high
+		model.setLocalTransform(cameraHigh);
+		baseToImu = imuHigh;
+	}
+	model.save(path);
+
+	SensorCaptureThread cameraThread(new
+		CameraRGBDImages(
+				pathRgbImages,
+				pathDepthImages), parameters);
+	((CameraRGBDImages*)cameraThread.camera())->setTimestamps(true, "", false);
+	if(!pathGt.empty())
+	{
+		((CameraRGBDImages*)cameraThread.camera())->setGroundTruthPath(pathGt, 1);
+	}
+	if(!pathOdom.empty())
+	{
+		((CameraRGBDImages*)cameraThread.camera())->setOdometryPath(pathOdom, 10);
+	}
+	((CameraRGBDImages*)cameraThread.camera())->setMaxPoseTimeDiff(maxTimeDiff);
+	if(!pathImu.empty())
+	{
+		cameraThread.enableIMUFiltering(imuFilter, parameters);
+	}
+
+	bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
+	float detectionRate = Parameters::defaultRtabmapDetectionRate();
+	int odomStrategy = Parameters::defaultOdomStrategy();
+	Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
+	Parameters::parse(parameters, Parameters::kOdomStrategy(), odomStrategy);
+	Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
+	std::string databasePath = output+"/"+outputName+".db";
+	UFile::erase(databasePath);
+	if(cameraThread.camera()->init(path, outputName+"_calib"))
+	{
+		int totalImages = (int)((CameraRGBDImages*)cameraThread.camera())->filenames().size();
+
+		if(skipFrames>0)
+		{
+			totalImages /= skipFrames+1;
+		}
+
+		printf("Processing %d images...\n", totalImages);
+
+		ParametersMap odomParameters = parameters;
+		odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
+		Odometry * odom = Odometry::create(odomParameters);
+		Rtabmap rtabmap;
+		rtabmap.init(parameters, databasePath);
+
+		std::ifstream imu_file;
+
+		if(!pathImu.empty())
+		{
+			// open the IMU file
+			std::string line;
+			imu_file.open(pathImu.c_str());
+			if (!imu_file.good()) {
+				UERROR("no imu file found at %s",pathImu.c_str());
+				return -1;
+			}
+			int number_of_lines = 0;
+			while (std::getline(imu_file, line))
+				++number_of_lines;
+			printf("No. IMU measurements: %d\n", number_of_lines-1);
+			if (number_of_lines - 1 <= 0) {
+				UERROR("no imu messages present in %s", pathImu.c_str());
+				return -1;
+			}
+			// set reading position to second line
+			imu_file.clear();
+			imu_file.seekg(0, std::ios::beg);
+			std::getline(imu_file, line);
+		}
+
+		UTimer totalTime;
+		UTimer timer;
+		SensorCaptureInfo cameraInfo;
+		SensorData data = cameraThread.camera()->takeData(&cameraInfo);
+		int iteration = 0;
+		double start = data.stamp();
+
+		/////////////////////////////
+		// Processing dataset begin
+		/////////////////////////////
+		int odomKeyFrames = 0;
+		double previousStamp = 0.0;
+		Transform previousOdomPose;
+		
+		while(data.isValid() && g_forever)
+		{
+			// get all IMU measurements till then
+			double t_imu = start;
+			do {
+				std::string line;
+				if (!std::getline(imu_file, line)) {
+					std::cout << std::endl << "Finished parsing IMU." << std::endl << std::flush;
+					break;
+				}
+
+				std::stringstream stream(line);
+				std::string s;
+				std::getline(stream, s, ' ');
+				t_imu = uStr2Double(s);
+
+				cv::Vec3d gyr;
+				for (int j = 0; j < 3; ++j) {
+					std::getline(stream, s, ' ');
+					gyr[j] = uStr2Double(s);
+				}
+
+				cv::Vec3d acc;
+				for (int j = 0; j < 3; ++j) {
+					std::getline(stream, s, ' ');
+					acc[j] = uStr2Double(s);
+				}
+
+				if (t_imu - start + 1 > 0) {
+
+					SensorData dataImu(IMU(gyr, cv::Mat(3,3,CV_64FC1), acc, cv::Mat(3,3,CV_64FC1), baseToImu), 0, t_imu);
+					cameraThread.postUpdate(&dataImu);
+					odom->process(dataImu);
+				}
+
+			} while (t_imu <= data.stamp());
+
+			cameraThread.postUpdate(&data, &cameraInfo);
+			cameraInfo.timeTotal = timer.ticks();
+
+			Transform guess = (!pathOdom.empty() && !cameraInfo.odomPose.isNull() && !previousOdomPose.isNull())?previousOdomPose.inverse() * cameraInfo.odomPose:Transform();	
+
+			OdometryInfo odomInfo;
+			Transform previous = odom->getPose();
+			Transform pose = odom->process(data,
+				guess,
+				&odomInfo);
+
+			if(!pose.isNull() && odomInfo.reg.covariance.total() == 36)
+			{
+				previousOdomPose = cameraInfo.odomPose;
+				if(uIsFinite(odomInfo.reg.covariance.at<double>(0,0)) &&
+					odomInfo.reg.covariance.at<double>(0,0)>0.0)
+				{
+					if( !pathOdom.empty() && 
+						odomInfo.reg.covariance.at<double>(0,0) >= 9999 && 
+						!previousOdomPose.isNull() &&
+						(pose.x() != 0.0f || pose.y() != 0.0f || pose.z() != 0.0f)) // not the first frame
+					{
+						// In case of external guess and auto reset, keep reporting lost till we
+						// process the second frame with valid covariance. This way it
+						// won't trigger a new map.
+						pose = Transform();
+					}
+				}
+			}
+
+			if(odomStrategy == 2)
+			{
+				//special case for FOVIS, set covariance 1 if 9999 is detected
+				if(!odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at<double>(0,0) >= 9999)
+				{
+					odomInfo.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
+				}
+			}
+			
+			if(iteration!=0 && !pose.isNull() && !odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at<double>(0,0)>=9999)
+            {
+				UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomInfo.reg.covariance.at<double>(0,0));
+				rtabmap.triggerNewMap();
+            }
+
+			if(odomInfo.keyFrameAdded)
+			{
+				++odomKeyFrames;
+			}
+
+			bool processData = true;
+			if(detectionRate>0.0f &&
+				previousStamp>0.0 &&
+				data.stamp()>previousStamp && data.stamp() - previousStamp < 1.0/detectionRate)
+			{
+				processData = false;
+			}
+
+			if(processData)
+			{
+				previousStamp = data.stamp();
+			}
+
+			if(!processData)
+			{
+				// set negative id so rtabmap will detect it as an intermediate node
+				data.setId(-1);
+				data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
+				processData = intermediateNodes;
+			}
+
+			timer.restart();
+			if(processData)
+			{
+				std::map<std::string, float> externalStats;
+				// save camera statistics to database
+				externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
+				externalStats.insert(std::make_pair("Camera/HistogramEqualization/ms", cameraInfo.timeHistogramEqualization*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
+				externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
+				externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
+				// save odometry statistics to database
+				externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
+				externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
+				externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
+				externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
+				externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
+				externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
+				externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
+				externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
+				externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
+
+				OdometryEvent e(SensorData(), Transform(), odomInfo);
+				rtabmap.process(data, pose, odomInfo.reg.covariance, e.velocity(), externalStats);
+			}
+
+			++iteration;
+			if(!quiet || iteration == totalImages)
+			{
+				double slamTime = timer.ticks();
+
+				float rmse = -1;
+				if(rtabmap.getStatistics().data().find(Statistics::kGtTranslational_rmse()) != rtabmap.getStatistics().data().end())
+				{
+					rmse = rtabmap.getStatistics().data().at(Statistics::kGtTranslational_rmse());
+				}
+
+				if(rmse >= 0.0f)
+				{
+					printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
+							iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
+				}
+				else
+				{
+					printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms",
+							iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
+				}
+				if(processData && rtabmap.getLoopClosureId()>0)
+				{
+					printf(" *");
+				}
+				printf("\n");
+			}
+			else if(iteration % (totalImages/10) == 0)
+			{
+				printf(".");
+				fflush(stdout);
+			}
+
+			cameraInfo = SensorCaptureInfo();
+			timer.restart();
+			data = cameraThread.camera()->takeData(&cameraInfo);
+		}
+		delete odom;
+		printf("Total time=%fs\n", totalTime.ticks());
+		/////////////////////////////
+		// Processing dataset end
+		/////////////////////////////
+
+		// Save trajectory
+		printf("Saving trajectory...\n");
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> links;
+		std::map<int, Signature> signatures;
+		std::map<int, double> stamps;
+		rtabmap.getGraph(poses, links, true, true, &signatures);
+		for(std::map<int, Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
+		{
+			stamps.insert(std::make_pair(iter->first, iter->second.getStamp()));
+		}
+		std::string pathTrajectory = output+"/"+outputName+"_poses.txt";
+		if(poses.size() && graph::exportPoses(pathTrajectory, 1, poses, links, stamps))
+		{
+			printf("Saving %s... done!\n", pathTrajectory.c_str());
+		}
+		else
+		{
+			printf("Saving %s... failed!\n", pathTrajectory.c_str());
+		}
+
+		if(!pathGt.empty())
+		{
+			// Log ground truth statistics
+			std::map<int, Transform> groundTruth;
+
+			for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				Transform o, gtPose;
+				int m,w;
+				std::string l;
+				double s;
+				std::vector<float> v;
+				GPS gps;
+				EnvSensors sensors;
+				rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, sensors, true);
+				if(!gtPose.isNull())
+				{
+					groundTruth.insert(std::make_pair(iter->first, gtPose));
+				}
+			}
+
+
+			// compute RMSE statistics
+			float translational_rmse = 0.0f;
+			float translational_mean = 0.0f;
+			float translational_median = 0.0f;
+			float translational_std = 0.0f;
+			float translational_min = 0.0f;
+			float translational_max = 0.0f;
+			float rotational_rmse = 0.0f;
+			float rotational_mean = 0.0f;
+			float rotational_median = 0.0f;
+			float rotational_std = 0.0f;
+			float rotational_min = 0.0f;
+			float rotational_max = 0.0f;
+			graph::calcRMSE(
+					groundTruth,
+					poses,
+					translational_rmse,
+					translational_mean,
+					translational_median,
+					translational_std,
+					translational_min,
+					translational_max,
+					rotational_rmse,
+					rotational_mean,
+					rotational_median,
+					rotational_std,
+					rotational_min,
+					rotational_max);
+
+			printf("   translational_rmse=   %f m\n", translational_rmse);
+			printf("   rotational_rmse=      %f deg\n", rotational_rmse);
+
+			FILE * pFile = 0;
+			std::string pathErrors = output+"/"+outputName+"_rmse.txt";
+			pFile = fopen(pathErrors.c_str(),"w");
+			if(!pFile)
+			{
+				UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
+			}
+			fprintf(pFile, "Ground truth comparison:\n");
+			fprintf(pFile, "  translational_rmse=   %f\n", translational_rmse);
+			fprintf(pFile, "  translational_mean=   %f\n", translational_mean);
+			fprintf(pFile, "  translational_median= %f\n", translational_median);
+			fprintf(pFile, "  translational_std=    %f\n", translational_std);
+			fprintf(pFile, "  translational_min=    %f\n", translational_min);
+			fprintf(pFile, "  translational_max=    %f\n", translational_max);
+			fprintf(pFile, "  rotational_rmse=      %f\n", rotational_rmse);
+			fprintf(pFile, "  rotational_mean=      %f\n", rotational_mean);
+			fprintf(pFile, "  rotational_median=    %f\n", rotational_median);
+			fprintf(pFile, "  rotational_std=       %f\n", rotational_std);
+			fprintf(pFile, "  rotational_min=       %f\n", rotational_min);
+			fprintf(pFile, "  rotational_max=       %f\n", rotational_max);
+			fclose(pFile);
+		}
+	}
+	else
+	{
+		UERROR("Camera init failed!");
+	}
+
+	printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+outputName+".db").c_str());
+	printf("Do:\n"
+			" $ rtabmap-databaseViewer %s\n\n", (output+"/"+outputName+".db").c_str());
+
+	return 0;
+}

tools/DetectMoreLoopClosures/main.cpp

@@ -63,14 +63,7 @@ void showUsage()
 	exit(1);
 }
 
-// catch ctrl-c
 bool g_loopForever = true;
-void sighandler(int sig)
-{
-	printf("\nSignal %d caught...\n", sig);
-	g_loopForever = false;
-}
-
 class PrintProgressState : public ProgressState
 {
 public:
@@ -86,6 +79,15 @@ public:
 private:
 	double stamp_;
 };
+PrintProgressState progress;
+
+// catch ctrl-c
+void sighandler(int sig)
+{
+	printf("\nSignal %d caught...\n", sig);
+	g_loopForever = false;
+	progress.setCanceled(true);
+}
 
 int main(int argc, char * argv[])
 {
@@ -94,7 +96,7 @@ int main(int argc, char * argv[])
 	signal(SIGINT, &sighandler);
 
 	ULogger::setType(ULogger::kTypeConsole);
-	ULogger::setLevel(ULogger::kError);
+	ULogger::setLevel(ULogger::kWarning);
 
 	if(argc < 2)
 	{
@@ -195,7 +197,7 @@ int main(int argc, char * argv[])
 
 	// Add some optimizations (soft set, can be overriden by arguments)
 	inputParams.insert(ParametersPair(Parameters::kMemLoadVisualLocalFeaturesOnInit(), "false")); // don't need features already loaded in RAM
-	inputParams.insert(ParametersPair(Parameters::kKpNNStrategy(), "3")); // don't need flann index
+	inputParams.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // should be incremental to update links
 
 	std::string dbPath = argv[argc-1];
 	if(!UFile::exists(dbPath))
@@ -245,6 +247,7 @@ int main(int argc, char * argv[])
 	Rtabmap rtabmap;
 	printf("Initialization...\n");
 	UTimer timer;
+	ParametersMap originalParameters = parameters;
 	uInsert(parameters, inputParams);
 	rtabmap.init(parameters, dbPath);
 	printf("Initialization... done! (%f sec)\n", timer.ticks());
@@ -267,7 +270,6 @@ int main(int argc, char * argv[])
 		printf("From/To Session ID = %d%s\n", fromToMapId, last?" (last session)":"");
 	}
 
-	PrintProgressState progress;
 	printf("Detecting...\n");
 	int detected = rtabmap.detectMoreLoopClosures(clusterRadiusMax, clusterAngle, iterations, intraSession, interSession, &progress, clusterRadiusMin, fromToMapId);
 	if(detected < 0)
@@ -306,6 +308,9 @@ int main(int argc, char * argv[])
 		}
 	}
 
+	// Restore original parameters before saving back the database
+	rtabmap.parseParameters(originalParameters);
+
 	rtabmap.close();
 
 	return 0;

tools/Matcher/main.cpp

@@ -87,6 +87,11 @@ void showUsage()
 			"   --to_depth \"to_depth.png\"        Depth or right image file of the second image.\n"
 			"                                        For 3D->3D estimation, from_depth and to_depth\n"
 			"                                        should be both set.\n"
+			"   --raw                              Provided images are raw and should be rectified.\n"
+			"                                        Doesn't need to be explicitly set if calibration\n"
+			"                                        is not provided. For RGB-D data, only the RGB image\n"
+			"                                        is rectified, the depth is assumed already matching\n"
+			"                                        the rectified one.\n"
 			"\n\n"
 			"%s\n",
 			Parameters::showUsage());
@@ -107,6 +112,7 @@ int main(int argc, char * argv[])
 	std::string toDepthPath;
 	std::string calibrationPath;
 	std::string calibrationToPath;
+	bool imagesRectified = true;
 	for(int i=1; i<argc-2; ++i)
 	{
 		if(strcmp(argv[i], "--from_depth") == 0)
@@ -157,6 +163,10 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(strcmp(argv[i], "--raw") == 0)
+		{
+			imagesRectified = false;
+		}
 		else if(strcmp(argv[i], "--help") == 0)
 		{
 			showUsage();
@@ -171,6 +181,10 @@ int main(int argc, char * argv[])
 	}
 	printf("  --from_depth  = \"%s\"\n", fromDepthPath.c_str());
 	printf("  --to_depth    = \"%s\"\n", toDepthPath.c_str());
+	if(!imagesRectified)
+	{
+		printf("  --raw (images will be rectified)\n");
+	}
 
 #ifdef RTABMAP_PYTHON
 	rtabmap::PythonInterface pythonInterface;
@@ -311,12 +325,57 @@ int main(int argc, char * argv[])
 		if(model.isValidForProjection())
 		{
 			printf("Mono calibration model detected.\n");
+
+			if(!imagesRectified)
+			{
+				if(!model.isValidForRectification())
+				{
+					printf("ERROR: calibration model \"%s\" is not valid for rectification and --raw option was set. Aborting.\n", calibrationPath.c_str());
+					exit(-1);
+				}
+				if(!model.isRectificationMapInitialized()) {
+					model.initRectificationMap();
+				}
+				if(!modelTo.isValidForRectification())
+				{
+					printf("ERROR: calibration model \"%s\" is not valid for rectification and --raw option was set. Aborting.\n", calibrationToPath.c_str());
+					exit(-1);
+				}
+				if(!modelTo.isRectificationMapInitialized()) {
+					modelTo.initRectificationMap();
+				}
+				imageFrom = model.rectifyImage(imageFrom);
+				imageTo = modelTo.rectifyImage(imageTo);
+			}
 			dataFrom = SensorData(imageFrom, fromDepth, model, 1);
 			dataTo = SensorData(imageTo, toDepth, modelTo, 2);
 		}
 		else //stereo
 		{
 			printf("Stereo calibration model detected.\n");
+			if(!imagesRectified)
+			{
+				if(!stereoModel.isValidForRectification())
+				{
+					printf("ERROR: stereo calibration model \"%s\" is not valid for rectification and --raw option was set. Aborting.\n", calibrationPath.c_str());
+					exit(-1);
+				}
+				if(!stereoModel.isRectificationMapInitialized()) {
+					stereoModel.initRectificationMap();
+				}
+				if(!stereoModelTo.isValidForRectification())
+				{
+					printf("ERROR: stereo calibration model \"%s\" is not valid for rectification and --raw option was set. Aborting.\n", calibrationToPath.c_str());
+					exit(-1);
+				}
+				if(!stereoModelTo.isRectificationMapInitialized()) {
+					stereoModelTo.initRectificationMap();
+				}
+				imageFrom = stereoModel.left().rectifyImage(imageFrom);
+				fromDepth = stereoModel.right().rectifyImage(fromDepth);
+				imageTo = stereoModelTo.left().rectifyImage(imageTo);
+				toDepth = stereoModelTo.right().rectifyImage(toDepth);
+			}
 			dataFrom = SensorData(imageFrom, fromDepth, stereoModel, 1);
 			dataTo = SensorData(imageTo, toDepth, stereoModelTo, 2);
 		}
@@ -329,7 +388,7 @@ int main(int argc, char * argv[])
 		{
 			parameters.insert(ParametersPair(Parameters::kVisEstimationType(), "2")); // Set 2D->2D estimation for mono images
 			parameters.insert(ParametersPair(Parameters::kVisEpipolarGeometryVar(), "1")); //Unknown scale
-			printf("Calibration not set, setting %s=1 and %s=2 by default (2D->2D estimation)\n", Parameters::kVisEpipolarGeometryVar().c_str(), Parameters::kVisEstimationType().c_str());
+			printf("Depth/Stereo not set, setting %s=1 and %s=2 by default (2D->2D estimation)\n", Parameters::kVisEpipolarGeometryVar().c_str(), Parameters::kVisEstimationType().c_str());
 		}
 		RegistrationVis reg(parameters);
 		RegistrationInfo info;

tools/ReduceGraph/CMakeLists.txt

@@ -0,0 +1,13 @@
+
+ADD_EXECUTABLE(reduceGraph main.cpp)
+  
+TARGET_LINK_LIBRARIES(reduceGraph rtabmap_core)
+
+SET_TARGET_PROPERTIES( reduceGraph 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-reduceGraph)
+
+INSTALL(TARGETS reduceGraph
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
+
+

tools/ReduceGraph/main.cpp

@@ -0,0 +1,281 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/DBDriver.h>
+#include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/Memory.h>
+#include <rtabmap/core/global_map/OccupancyGrid.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_surface.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+#include <pcl/filters/filter.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/obj_io.h>
+#include <pcl/common/common.h>
+#include <pcl/surface/poisson.h>
+#include <stdio.h>
+#include <signal.h>
+
+using namespace rtabmap;
+
+void showUsage(const char * exec)
+{
+	printf("\nUsage:\n"
+			"%s [Options] database.db\n"
+			"Options:\n"
+			"    --keep_latest  Merge old nodes to newer nodes, thus keeping only latest nodes.\n"
+			"    --keep_linked  Keep reduced nodes linked to graph.\n"
+			"    --pre_cleanup  Remove all user loop closures linking nodes closer than %s in the graph before reducing the graph.\n"
+			"    --radius #.#   Maximum loop closure distance that can be merged. Default is 1 m. Should be > 0.\n"
+			"    --udebug/--uinfo/--warn can also be used to change verbosity.\n"
+			"\n", exec, Parameters::kMemSTMSize().c_str());
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	if(argc < 2)
+	{
+		showUsage(argv[0]);
+	}
+
+	bool keepLatest = false;
+	bool keepLinked = false;
+	float radius = 1.0f;
+	bool preCleanup = false;
+	for(int i=1; i<argc; ++i)
+	{
+		if(std::strcmp(argv[i], "--help") == 0)
+		{
+			showUsage(argv[0]);
+		}
+		else if(std::strcmp(argv[i], "--keep_latest") == 0)
+		{
+			keepLatest = true;
+		}
+		else if(std::strcmp(argv[i], "--keep_linked") == 0)
+		{
+			keepLinked = true;
+		}
+		else if(std::strcmp(argv[i], "--pre_cleanup") == 0)
+		{
+			preCleanup = true;
+		}
+		else if(std::strcmp(argv[i], "--radius") == 0)
+		{
+			++i;
+			if(i < argc-1)
+			{
+				radius = uStr2Float(argv[i]);
+				if(radius <= 0.0f)
+				{
+					printf("--radius should be > 0, parsed %f\n", radius);
+					showUsage(argv[0]);
+				}
+			}
+			else {
+				showUsage(argv[0]);
+			}
+		}
+	}
+	printf("Parameters:\n");
+	printf("  radius = %f m\n", radius);
+	printf("  keep_latest = %s\n", keepLatest?"true":"false");
+	printf("  keep_linked = %s\n", keepLinked?"true":"false");
+	printf("  pre_cleanup = %s\n", preCleanup?"true":"false");
+	
+	// Just parse logging options
+	Parameters::parseArguments(argc, argv);
+
+	// Add some optimizations
+	ParametersMap inputParams;
+	inputParams.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true")); // load the whole map in RAM
+	inputParams.insert(ParametersPair(Parameters::kMemLoadVisualLocalFeaturesOnInit(), "false")); // don't need features already loaded in RAM
+
+	std::string dbPath = argv[argc-1];
+	if(!UFile::exists(dbPath))
+	{
+		printf("Database %s doesn't exist!\n", dbPath.c_str());
+		return 1;
+	}
+
+	printf("Database: %s\n", dbPath.c_str());
+
+	// Get parameters
+	ParametersMap parameters;
+	DBDriver * driver = DBDriver::create();
+	if(driver->openConnection(dbPath))
+	{
+		parameters = driver->getLastParameters();
+		driver->closeConnection(false);
+	}
+	else
+	{
+		UERROR("Cannot open database %s!", dbPath.c_str());
+	}
+	delete driver;
+
+	Memory memory;
+	printf("Initialization...\n");
+	UTimer timer;
+	ParametersMap originalParameters = parameters;
+	uInsert(parameters, inputParams);
+	if(!memory.init(dbPath, false, parameters))
+	{
+		printf("Initialization... failed! Aborting!\n");
+		return 1;
+	}
+	std::set<int> ids = memory.getAllSignatureIds();
+	printf("Initialization... done! %ld nodes loaded. (%f sec)\n", ids.size(), timer.ticks());
+
+	if(ids.empty())
+	{
+		printf("IDs are empty?! Aborting.\n");
+		return 1;
+	}
+
+	Transform lastLocalizationPose;
+	std::map<int, Transform> optimizedPoses = memory.loadOptimizedPoses(&lastLocalizationPose);
+	float xMin, yMin, cellSize;
+	bool hasOptimizedMap = !memory.load2DMap(xMin, yMin, cellSize).empty();
+
+	int totalNodesReduced = 0;
+	std::vector<int> vids;
+	vids.reserve(ids.size());
+	if(keepLatest)
+	{
+		// we process older to newer nodes, merging to new nodes
+		vids.insert(vids.end(), ids.begin(), ids.end());
+	}
+	else
+	{
+		// we process newer to older nodes, merging to old nodes
+		vids.insert(vids.end(), ids.rbegin(), ids.rend());
+	}
+
+	if(preCleanup)
+	{
+		if(memory.getMaxStMemSize() <= 1)
+		{
+			printf("--pre_cleanup is used but %s <= 1, skipping pre cleanup...\n", Parameters::kMemSTMSize().c_str());
+		}
+		else
+		{
+			int totalRemoved = 0;
+			for(auto id: vids)
+			{
+				auto nids = memory.getNeighborsId(id, memory.getMaxStMemSize(), -1, true, true, true);
+				auto links = memory.getLinks(id, true, false);
+				for(auto link:links)
+				{
+					if( link.second.type() == Link::kUserClosure &&
+						nids.find(link.first)!=nids.end())
+					{
+						memory.removeLink(id, link.first);
+						++totalRemoved;
+					}
+				}
+			}
+			printf("Removed %d user links that were linking nodes that were close in the graph (below %s=%d)\n",
+				totalRemoved, Parameters::kMemSTMSize().c_str(), memory.getMaxStMemSize());
+		}
+	}
+
+	for(auto id: vids)
+	{
+		// Nodes can be already reduced by other nodes, check if they are still there
+		if(memory.getSignature(id) != 0)
+		{
+			int reducedId = memory.reduceNode(id, radius, keepLinked, keepLatest?1:-1);
+			if(reducedId > 0)
+			{
+				printf("Reduced node %d to node %d!\n", id, reducedId);
+				++totalNodesReduced;
+			}
+		}
+	}
+	printf("Reduced a total of %d nodes out of %ld nodes\n", totalNodesReduced, ids.size());
+
+	if(!optimizedPoses.empty())
+	{
+		size_t removed = 0;
+		// cleanup reduced nodes from the optimized poses
+		for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(0); iter!=optimizedPoses.end();)
+		{
+			if(memory.getSignature(iter->first) == 0)
+			{
+				iter = optimizedPoses.erase(iter);
+				++removed;
+			}
+			else
+			{
+				++iter;
+			}
+		}
+		printf("Updated optimized graph from %ld poses to %ld poses\n", optimizedPoses.size()+removed, optimizedPoses.size());
+		memory.saveOptimizedPoses(optimizedPoses, lastLocalizationPose);
+	}
+	if(hasOptimizedMap)
+	{
+		printf("The database has a global occupancy grid, regenerating one with the remaining nodes of the optimized graph!\n");
+		LocalGridCache cache;
+		OccupancyGrid grid(&cache, parameters);
+		for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(0); iter!=optimizedPoses.end(); ++iter)
+		{
+			SensorData data = memory.getNodeData(iter->first, false, false, false, true);
+			data.uncompressData();
+			cache.add(iter->first, data.gridGroundCellsRaw(), data.gridObstacleCellsRaw(), data.gridEmptyCellsRaw(), data.gridCellSize(), data.gridViewPoint());
+		}
+		grid.update(optimizedPoses);
+		cv::Mat map = grid.getMap(xMin, yMin);
+		if(map.empty())
+		{
+			printf("Could not regenerate the global occupancy grid! The grid is not updated.\n");
+		}
+		else
+		{
+			memory.save2DMap(map, xMin, yMin, grid.getCellSize());
+			printf("Saved the new global occupancy grid!\n");
+		}
+	}
+
+	// Restore original parameters before saving back the database
+	memory.parseParameters(originalParameters);
+	
+	printf("Saving all changes to database...\n");
+	memory.close(true);
+
+	return 0;
+}

tools/Reprocess/main.cpp

@@ -57,7 +57,7 @@ void showUsage()
 			"   rtabmap-reprocess [options] \"input.db\" \"output.db\"\n"
 			"   rtabmap-reprocess [options] \"input1.db;input2.db;input3.db\" \"output.db\"\n"
 			"\n"
-			"   For the second example, only parameters from the first database are used.\n"
+			"   For the second example, only parameters from the first database are used (unless -params_last or -default are used).\n"
 			"   If Mem/IncrementalMemory is false, RTAB-Map is initialized with the first input database,\n"
 			"   then localization-only is done with next databases against the first one.\n"
 			"   To see warnings when loop closures are rejected, add \"--uwarn\" argument.\n"
@@ -71,6 +71,7 @@ void showUsage()
 			"                       from the database. If custom parameters are also set as \n"
 			"                       arguments, they overwrite those in config file and the database.\n"
 			"     -default    Input database's parameters are ignored, using default ones instead.\n"
+			"     -params_last  Parameters of the last database is used instead of the first one (ignored if -default is also used).\n"
 			"     -odom       Recompute odometry. See \"Odom/\" parameters with --params. If -skip option\n"
 			"                 is used, it will be applied to odometry frames, not rtabmap frames. Multi-session\n"
 			"                 may not be detected correctly if the input covariance between sessions doesn't have 9999.\n"
@@ -128,6 +129,8 @@ void sighandler(int sig)
 int loopCount = 0;
 int proxCount = 0;
 int loopCountMotion = 0;
+int loopInter = 0;
+int loopIntra = 0;
 int totalFrames = 0;
 int totalFramesMotion = 0;
 std::vector<float> previousLocalizationDistances;
@@ -258,6 +261,7 @@ int main(int argc, char * argv[])
 	bool assemble3dOctoMap = false;
 	bool useDatabaseRate = false;
 	bool useDefaultParameters = false;
+	bool useLastDatabaseParameters = false;
 	bool recomputeOdometry = false;
 	bool useInputOdometryAsGuess = false;
 	double odomLinVarOverride = 0.0;
@@ -316,6 +320,10 @@ int main(int argc, char * argv[])
 			useDefaultParameters = true;
 			printf("Using default parameters.\n");
 		}
+		else if(strcmp(argv[i], "-params_last") == 0 || strcmp(argv[i], "--params_last") == 0)
+		{
+			useLastDatabaseParameters = true;
+		}
 		else if(strcmp(argv[i], "-odom") == 0 || strcmp(argv[i], "--odom") == 0)
 		{
 			recomputeOdometry = true;
@@ -681,11 +689,10 @@ int main(int argc, char * argv[])
 	}
 
 	// Get parameters of the first database
-	DBDriver * dbDriver = DBDriver::create();
+	std::shared_ptr<DBDriver> dbDriver(DBDriver::create());
 	if(!dbDriver->openConnection(databases.front(), false))
 	{
-		printf("Failed opening input database!\n");
+		printf("Failed opening the input database!\n");
-		delete dbDriver;
 		return 1;
 	}
 
@@ -693,13 +700,28 @@ int main(int argc, char * argv[])
 	std::string targetVersion;
 	if(!useDefaultParameters)
 	{
-		parameters = dbDriver->getLastParameters();
+		if(databases.size() > 1 && useLastDatabaseParameters)
-		targetVersion = dbDriver->getDatabaseVersion();
+		{
-		parameters.insert(ParametersPair(Parameters::kDbTargetVersion(), targetVersion));
+			printf("Using last database's parameters.\n");
+			std::shared_ptr<DBDriver> lastDbDriver(DBDriver::create());
+			if(!lastDbDriver->openConnection(databases.back(), true))
+			{
+				printf("Failed opening the last input database!\n");
+				return 1;
+			}
+			parameters = lastDbDriver->getLastParameters();
+			targetVersion = lastDbDriver->getDatabaseVersion();
+		}
+		else
+		{
+			parameters = dbDriver->getLastParameters();
+			targetVersion = dbDriver->getDatabaseVersion();
+		}
 		if(parameters.empty())
 		{
-			printf("WARNING: Failed getting parameters from database, reprocessing will be done with default parameters! Database version may be too old (%s).\n", dbDriver->getDatabaseVersion().c_str());
+			printf("WARNING: Failed getting parameters from database, reprocessing will be done with default parameters! Database version may be too old (%s).\n", targetVersion.c_str());
 		}
+		parameters.insert(ParametersPair(Parameters::kDbTargetVersion(), targetVersion));
 	}
 
 	if(customParameters.size())
@@ -798,7 +820,6 @@ int main(int argc, char * argv[])
 	{
 		printf("Input database doesn't have any nodes saved in it.\n");
 		dbDriver->closeConnection(false);
-		delete dbDriver;
 		return 1;
 	}
 	if(!((!incrementalMemory || appendMode) && databases.size() > 1))
@@ -820,7 +841,6 @@ int main(int argc, char * argv[])
 		if (!dbDriver->openConnection(*iter, false))
 		{
 			printf("Failed opening input database!\n");
-			delete dbDriver;
 			return 1;
 		}
 		ids.clear();
@@ -828,8 +848,7 @@ int main(int argc, char * argv[])
 		totalIds += ids.size();
 		dbDriver->closeConnection(false);
 	}
-	delete dbDriver;
+	dbDriver.reset();
-	dbDriver = 0;
 
 	std::string workingDirectory = UDirectory::getDir(outputDatabasePath);
 	printf("Set working directory to \"%s\".\n", workingDirectory.c_str());
@@ -1239,15 +1258,23 @@ int main(int argc, char * argv[])
 				++loopCountMotion;
 			}
 			int loopMapId = stats.loopClosureId() > 0? stats.loopClosureMapId(): stats.proximityDetectionMapId();
-			printf("Processed %d/%d nodes [id=%d map=%d opt_graph=%d]... %dms %s on %d [%d]\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(iterationTime.ticks() * 1000), stats.loopClosureId() > 0?"Loop":"Prox", loopId, loopMapId);
+			if(loopMapId != stats.refImageMapId())
+			{
+				++loopInter;
+			}
+			else
+			{
+				++loopIntra;
+			}
+			printf("Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms %s on %d [%d]\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000), stats.loopClosureId() > 0?"Loop":"Prox", loopId, loopMapId);
 		}
 		else if(landmarkId != 0)
 		{
-			printf("Processed %d/%d nodes [id=%d map=%d opt_graph=%d]... %dms Loop on landmark %d\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()),  int(iterationTime.ticks() * 1000), landmarkId);
+			printf("Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms Loop on landmark %d\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f),  int(iterationTime.ticks() * 1000), landmarkId);
 		}
 		else
 		{
-			printf("Processed %d/%d nodes [id=%d map=%d opt_graph=%d]... %dms\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(iterationTime.ticks() * 1000));
+			printf("Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000));
 		}
 
 		// Here we accumulate statistics about distance from last localization
@@ -1340,7 +1367,8 @@ int main(int argc, char * argv[])
 	}
 	else
 	{
-		printf("Total loop closures = %d (Loop=%d, Prox=%d, In Motion=%d/%d)\n", loopCount+proxCount, loopCount, proxCount, loopCountMotion, totalFramesMotion);
+		printf("Total loop closures = %d (Loop=%d, Prox=%d, In Motion=%d/%d, Intra=%d, Inter=%d)\n",
+			loopCount+proxCount, loopCount, proxCount, loopCountMotion, totalFramesMotion, loopIntra, loopInter);
 
 		if(databases.size()>1)
 		{
@@ -1375,13 +1403,12 @@ int main(int argc, char * argv[])
 		{
 			if(save2DMap)
 			{
-				DBDriver * driver = DBDriver::create();
+				std::shared_ptr<DBDriver> driver(DBDriver::create());
 				if(driver->openConnection(outputDatabasePath))
 				{
 					driver->save2DMap(map, xMin, yMin, grid.getCellSize());
 					printf("Saving occupancy grid to database... done!\n");
 				}
-				delete driver;
 			}
 			else
 			{
@@ -1470,13 +1497,12 @@ int main(int argc, char * argv[])
 		{
 			if(save2DMap)
 			{
-				DBDriver * driver = DBDriver::create();
+				std::shared_ptr<DBDriver> driver(DBDriver::create());
 				if(driver->openConnection(outputDatabasePath))
 				{
 					driver->save2DMap(map, xMin, yMin, cellSize);
 					printf("Saving occupancy grid to database... done!\n");
 				}
-				delete driver;
 			}
 			else
 			{