Merge branch 'master' into offline_reduce_graph_tool

Author: matlabbe

.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.yml .github/workflows/cmake-windows.yml.github/workflows/cmake.yml renamed to .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.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><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>
         </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>

corelib/include/rtabmap/core/Features2d.h

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

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.");
@@ -740,8 +741,11 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Vis, CorFlowIterations,         int,   30,    uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowEps,                float, 0.01,  uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowMaxLevel,           int,   3,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
-    RTABMAP_PARAM(Vis, CorFlowGpu,                bool,  false, uFormat("[%s=1] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA).", kVisCorType().c_str()));
-#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
+    RTABMAP_PARAM(Vis, CorFlowUseMinEigenVals,    bool,  true,  uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach. Use minimum eigen values as an error measure, otherwise L1 distance between patches is used as an error measure.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowMinEigThreshold,    float, 1e-4,  uFormat("[%s=true] If the minimum eigenvalue of a feature's spatial gradient matrix is less than this threshold, then the feature is filtered out.", kVisCorFlowUseMinEigenVals().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowErrorThreshold,     float, 20,    uFormat("[%s=false] Filter out features with error greater than this threshold.", kVisCorFlowUseMinEigenVals().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowGpu,                bool,  false, uFormat("[%s=1] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA). Note that %s is not used in the GPU implementation.", kVisCorType().c_str(), kVisCorFlowUseMinEigenVals().c_str()));
+    #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
     RTABMAP_PARAM(Vis, BundleAdjustment,          int,   1,     "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
 #else
     RTABMAP_PARAM(Vis, BundleAdjustment,          int,   0,     "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
@@ -818,7 +822,10 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Stereo, OpticalFlow,           bool, true,    "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
     RTABMAP_PARAM(Stereo, SSD,                   bool, true,    uFormat("[%s=false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.", kStereoOpticalFlow().c_str()));
     RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  uFormat("[%s=true] Epsilon stop criterion.", kStereoOpticalFlow().c_str()));
-    RTABMAP_PARAM(Stereo, Gpu,                   bool, false,   uFormat("[%s=true] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA).", kStereoOpticalFlow().c_str()));
+    RTABMAP_PARAM(Stereo, UseMinEigenVals,       bool, true,    uFormat("[%s=true] Use minimum eigen values as an error measure, otherwise L1 distance between patches is used as an error measure.", kStereoOpticalFlow().c_str()));
+    RTABMAP_PARAM(Stereo, MinEigThreshold,       double, 1e-4,  uFormat("[%s=true] If the minimum eigenvalue of a feature's spatial gradient matrix is less than this threshold, then the feature is filtered out.", kStereoUseMinEigenVals().c_str()));
+    RTABMAP_PARAM(Stereo, ErrorThreshold,        double, 50,    uFormat("[%s=false] Filter out features with error greater than this threshold.", kStereoUseMinEigenVals().c_str()));
+    RTABMAP_PARAM(Stereo, Gpu,                   bool, false,   uFormat("[%s=true] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA). Note that %s is not used in the GPU implementation.", kStereoOpticalFlow().c_str(), kStereoUseMinEigenVals().c_str()));
 
     RTABMAP_PARAM(Stereo, DenseStrategy,         int, 0,  "0=cv::StereoBM, 1=cv::StereoSGBM");
 

corelib/include/rtabmap/core/RegistrationVis.h

@@ -91,6 +91,9 @@ class RTABMAP_CORE_EXPORT RegistrationVis : public Registration
 	float _flowEps;
 	int _flowMaxLevel;
 	bool _flowGpu;
+	bool _flowUseMinEigenVals;
+	float _flowMinEigThreshold;
+	float _flowErrorThreshold;
 	float _nndr;
 	int _nnType;
 	bool _gmsWithRotation;

corelib/include/rtabmap/core/Stereo.h

@@ -96,16 +96,23 @@ class RTABMAP_CORE_EXPORT StereoOpticalFlow : public Stereo {
 #endif
 
 	float epsilon() const {return epsilon_;}
+	bool usingMinEigenVals() const {return useMinEigenVals_;}
+	float minEigThreshold() const {return minEigThreshold_;}
+	float errorThreshold() const {return errorThreshold_;}
 	virtual bool isGpuEnabled() const;
 
 private:
 	void updateStatus(
 		const std::vector<cv::Point2f> & leftCorners,
 		const std::vector<cv::Point2f> & rightCorners,
-		std::vector<unsigned char> & status) const;
+		std::vector<unsigned char> & status,
+		std::vector<float> err = {}) const;
 
 private:
 	float epsilon_;
+	bool useMinEigenVals_;
+	float minEigThreshold_;
+	float errorThreshold_;
 	bool gpu_;
 };
 

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

@@ -102,10 +102,11 @@ class RTABMAP_CORE_EXPORT CameraImages :
 	}
 
 	// 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 @@ class RTABMAP_CORE_EXPORT CameraImages :
 	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
 			{