merged master->jazzy

* iOS CI

* g2o hard-coded path

* bump ios version

* Fixed opencv 4.10.0 build on docker resolute

.devcontainer/resolute/Dockerfile

@@ -0,0 +1,25 @@
+FROM introlab3it/rtabmap:resolute-deps
+
+# For devcontainer
+# remove ubuntu user
+RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu
+
+RUN apt-get update && apt-get install -y sudo && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG USERNAME=vscode
+ARG USER_UID=1000
+ARG USER_GID=1000
+
+RUN set -ex && \
+    groupadd --gid ${USER_GID} ${USERNAME} && \
+    useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
+    usermod -a -G sudo ${USERNAME} && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
+    chmod 0440 /etc/sudoers.d/${USERNAME}
+
+RUN mkdir -p /home/${USERNAME}/Documents/RTAB-Map && chown -R ${USERNAME} /home/${USERNAME}
+
+RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
+
+

.devcontainer/resolute/devcontainer.json

@@ -0,0 +1,30 @@
+{
+    "build": {
+      "dockerfile": "Dockerfile"
+    },
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "ms-vscode.cpptools-extension-pack"]
+        }
+      },
+    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
+    "workspaceFolder": "/home/vscode/rtabmap",
+    //"mounts": ["source=${localEnv:HOME}/Documents/RTAB-Map,target=/home/vscode/Documents/RTAB-Map,type=bind,consistency=cached"],
+    "settings": {
+      "terminal.integrated.defaultProfile.linux": "bash"
+    },
+    "remoteUser": "vscode",
+    "hostRequirements": {
+        "gpu": "optional"
+    },
+    "runArgs": ["--privileged",
+                "--network=host",
+                "--gpus=all",
+                //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer
+                "--env=DISPLAY",
+                "--env=QT_X11_NO_MITSHM=1",
+                "--volume=/tmp/.X11-unix:/tmp/.X11-unix"],
+    "containerEnv": {
+        "NVIDIA_VISIBLE_DEVICES": "all"
+    }
+}

.github/workflows/cmake-linux.yml

@@ -22,7 +22,7 @@ jobs:
     strategy:
       fail-fast: true
       matrix:
-        build_name: [ubuntu-22.04, ubuntu-24.04, ubuntu-24.04-with-opengv]
+        build_name: [ubuntu-22.04, ubuntu-24.04, ubuntu-24.04-with-opengv, ubuntu-26.04]
         include:
         - build_name: ubuntu-22.04
           os: ubuntu-22.04
@@ -36,6 +36,10 @@ jobs:
           os: ubuntu-24.04
           extra_deps: "libg2o-dev libceres-dev"
           extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON -DBUILD_OPENGV=ON"
+        - build_name: ubuntu-26.04
+          os: ubuntu-26.04
+          extra_deps: "libg2o-dev libceres-dev"
+          extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON -DBUILD_OPENGV=ON"
 
     steps:
     - uses: actions/checkout@v4

.github/workflows/cmake-ros.yml

@@ -3,7 +3,7 @@ name: CMake-ROS
 on:
   push:
     branches: 
-      - master
+      - jazzy-devel
   pull_request:
     branches: 
       - '**'
@@ -22,62 +22,25 @@ 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: ${{ matrix.ros_distribution }}-${{ matrix.os }}
+    name: ${{ matrix.ros_distribution }}
-    runs-on: ${{ matrix.os }}
+    runs-on: ubuntu-latest
     strategy:
       fail-fast: false
       matrix:
-        ros_distribution: [ humble, jazzy, kilted, rolling ]
+        ros_distribution: [ jazzy]
         include:
-        - ros_distribution: 'humble'
-          os: ubuntu-22.04
         - ros_distribution: 'jazzy'
-          os: ubuntu-24.04
+          skip_keys: ""
-        - ros_distribution: 'kilted'
+    container:
-          os: ubuntu-24.04
+      image: osrf/ros:${{ matrix.ros_distribution }}-desktop-full
-        - ros_distribution: 'rolling'
-          os: ubuntu-24.04
-
     steps:   
-    - name: Setup ROS2
+    - uses: actions/checkout@v4
-      # https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html
-      run: |
-        sudo apt install software-properties-common
-        sudo add-apt-repository universe
-        sudo apt update && sudo apt install curl -y
-        export ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}')
-        curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo ${UBUNTU_CODENAME:-${VERSION_CODENAME}})_all.deb"
-        sudo apt install /tmp/ros2-apt-source.deb
-        sudo apt update
-
     - uses: ros-tooling/setup-ros@v0.7
       with:
         required-ros-distributions: ${{ matrix.ros_distribution }}
-    
+    - uses: ros-tooling/action-ros-ci@v0.4
-    - uses: actions/checkout@v4
+      with:
-
+        package-name: rtabmap
-    - name: Point rosdep to latest Rolling on Noble until github runners support ubuntu 26.04
+        target-ros2-distro: ${{ matrix.ros_distribution }}
-      if: matrix.ros_distribution == 'rolling'
+        rosdep-skip-keys: "${{ matrix.skip_keys }}"
-      run: |
-        echo "ROSDISTRO_INDEX_URL=https://raw.githubusercontent.com/ros/rosdistro/6527fa694360d609c1491528ed50437bd853af5b/index-v4.yaml" >> $GITHUB_ENV
-
-    - name: Install dependencies
-      run: |
-        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
-        rosdep update
-        rosdep install --from-paths ${{github.workspace}} -y
-
-    - name: Configure CMake
-      run: |
-        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
-        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
-
-    - name: Build
-      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
-
-    - name: Info
-      working-directory: ${{github.workspace}}/build/bin
-      run: |
-        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
-        ./rtabmap-console --version
       

.github/workflows/docker.yml

@@ -4,6 +4,13 @@ on:
   push:
     branches:
       - 'master'
+  pull_request:
+    branches:
+      - '**'
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
+  cancel-in-progress: ${{ github.event_name == 'pull_request' }}
 
 jobs:
   docker_deps:
@@ -15,14 +22,15 @@ jobs:
     #    $ sudo apt-get upgrade qemu-user-static
     #    $ docker run --rm --privileged multiarch/qemu-user-static --reset -p yes -c yes
     # More info: https://github.com/introlab/rtabmap/issues/1454
-    # if: false
+    # Skipped on pull requests; built and pushed only on push to master.
+    if: github.event_name != 'pull_request'
 
     runs-on: ubuntu-latest
     
     strategy:
       fail-fast: false
       matrix:
-        docker_tag: [focal-deps, jammy-deps, noble-deps, noble-kilted-deps]
+        docker_tag: [focal-deps, jammy-deps, noble-deps, noble-kilted-deps, resolute-deps]
         include:
         - docker_tag: focal-deps
           docker_tags: |
@@ -52,6 +60,13 @@ jobs:
             linux/amd64
             linux/arm64
           docker_path: 'noble-kilted/deps'
+        - docker_tag: resolute-deps
+          docker_tags: |
+            introlab3it/rtabmap:resolute-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'resolute/deps'
     
     steps:
       -
@@ -85,12 +100,14 @@ jobs:
   
   docker:
     needs: docker_deps
+    # Run even when docker_deps is skipped (it is, on pull requests).
+    if: ${{ !cancelled() && !failure() }}
     runs-on: ubuntu-latest
     
     strategy:
       fail-fast: false
       matrix:
-        docker_tag: [bionic, focal, jammy, noble, noble-kilted, android23, android24, android26, android30]
+        docker_tag: [bionic, focal, jammy, noble, noble-kilted, resolute, android23, android24, android26, android30]
         include:
         - docker_tag: bionic
           docker_tags: |
@@ -142,6 +159,16 @@ jobs:
             linux/amd64
             linux/arm64
           docker_path: 'noble-kilted'
+        - docker_tag: resolute
+          docker_tags: |
+            introlab3it/rtabmap:resolute
+            introlab3it/rtabmap:26.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'resolute'
         - docker_tag: android23
           docker_tags: |
             introlab3it/rtabmap:android23
@@ -190,6 +217,9 @@ jobs:
         uses: docker/setup-buildx-action@v3
       -
         name: Login to DockerHub
+        # Only needed when pushing; skipped on pull requests (secrets are
+        # unavailable for fork PRs and we don't push there anyway).
+        if: github.event_name != 'pull_request'
         uses: docker/login-action@v3
         with:
           username: ${{ secrets.DOCKERHUB_USERNAME }}
@@ -199,8 +229,8 @@ jobs:
         uses: docker/build-push-action@v6
         with:
           context: .
-          push: true
+          push: ${{ github.event_name != 'pull_request' }}
-          platforms: ${{ matrix.docker_platforms }}
+          platforms: ${{ github.event_name == 'pull_request' && 'linux/amd64' || matrix.docker_platforms }}
           file: ./docker/${{ matrix.docker_path }}/Dockerfile
           build-args: |
             ${{ matrix.docker_args }}

.github/workflows/ios.yml

@@ -0,0 +1,114 @@
+name: iOS
+
+on:
+  push:
+    branches:
+      - master
+    paths: &ios_paths
+      - '.github/workflows/ios.yml'
+      - 'app/ios/**'
+      - 'app/android/jni/**'
+      - 'corelib/**'
+      - 'utilite/**'
+      - 'cmake_modules/**'
+      - 'CMakeLists.txt'
+  pull_request:
+    branches:
+      - '**'
+    paths: *ios_paths
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
+  cancel-in-progress: ${{ github.event_name == 'pull_request' }}
+
+env:
+  # Pre-built iOS dependencies (content of app/ios/RTABMapApp/Libraries, generated by install_deps.sh).
+  # Bump this when the dependency set changes (must match the Xcode toolchain below).
+  DEPS_URL: https://github.com/introlab/rtabmap/releases/download/0.23.1/libraries-ios-xcode26.5.zip
+  XCODE_VERSION: '26.5'
+
+jobs:
+  build:
+    name: build-ios
+    # macos-26 (Tahoe) ships Xcode 26.x, matching the toolchain used to build the prebuilt libraries.
+    runs-on: macos-26
+
+    steps:
+      - uses: actions/checkout@v4
+
+      - name: Select Xcode ${{ env.XCODE_VERSION }}
+        uses: maxim-lobanov/setup-xcode@v1
+        with:
+          xcode-version: ${{ env.XCODE_VERSION }}
+
+      - name: Versions
+        run: |
+          xcodebuild -version
+          cmake --version || brew install cmake
+
+      - name: Cache prebuilt dependencies archive
+        id: deps-cache
+        uses: actions/cache@v4
+        with:
+          path: deps.zip
+          # Keyed on the archive URL (release tag + filename), so the cache is
+          # reused until DEPS_URL is bumped, regardless of other workflow edits.
+          key: ${{ runner.os }}-ios-deps-${{ env.DEPS_URL }}
+
+      - name: Download prebuilt dependencies
+        if: steps.deps-cache.outputs.cache-hit != 'true'
+        run: curl -L "$DEPS_URL" -o deps.zip
+
+      - name: Extract dependencies into Libraries
+        run: |
+          set -eux
+          mkdir -p app/ios/RTABMapApp/Libraries
+          rm -rf deps_extract && mkdir -p deps_extract
+          unzip -q deps.zip -d deps_extract
+          # The archive holds the *content* of the Libraries folder (include/ lib/ share/),
+          # but tolerate an extra top-level Libraries/ wrapper just in case.
+          if [ -d deps_extract/Libraries ]; then
+            SRC=deps_extract/Libraries
+          else
+            SRC=deps_extract
+          fi
+          cp -R "$SRC"/. app/ios/RTABMapApp/Libraries/
+          test -d app/ios/RTABMapApp/Libraries/include
+          test -d app/ios/RTABMapApp/Libraries/lib
+
+      - name: Build rtabmap core (third-party deps are skipped, already provided by the archive)
+        working-directory: app/ios/RTABMapApp
+        run: ./install_deps.sh
+
+      - name: Build RTABMapApp
+        run: |
+          xcodebuild \
+            -project app/ios/RTABMapApp.xcodeproj \
+            -scheme RTABMapApp \
+            -configuration Release \
+            -sdk iphoneos \
+            -destination 'generic/platform=iOS' \
+            -derivedDataPath build \
+            CODE_SIGNING_ALLOWED=NO \
+            CODE_SIGNING_REQUIRED=NO \
+            CODE_SIGN_IDENTITY="" \
+            DEVELOPMENT_TEAM="" \
+            build
+
+      - name: Package app (unsigned .ipa)
+        run: |
+          set -eux
+          APP_DIR="build/Build/Products/Release-iphoneos"
+          rm -rf Payload && mkdir Payload
+          cp -R "$APP_DIR/RTABMapApp.app" Payload/
+          # Unsigned .ipa: not installable as-is, but ready for later (re)signing.
+          zip -q -r RTABMapApp-unsigned.ipa Payload
+
+      - name: Upload app artifact
+        uses: actions/upload-artifact@v4
+        with:
+          name: RTABMapApp-ios-unsigned
+          path: RTABMapApp-unsigned.ipa
+          compression-level: 0
+          if-no-files-found: error
+          retention-days: ${{ github.event_name == 'pull_request' && 1 || 90 }}

CMakeLists.txt

@@ -842,22 +842,7 @@ IF(WITH_VINS_FUSION)
 ENDIF(WITH_VINS_FUSION)
 
 IF(WITH_OPENVINS)
-    FIND_PACKAGE(ov_msckf)
+   FIND_PACKAGE(OpenVINS)
-    # On ROS2, the indirect includes and libraries
-    # are not forwarded by ov_msckf target, append them manually
-    FIND_PACKAGE(ov_core)
-    FIND_PACKAGE(ov_init)
-    IF(ov_msckf_FOUND AND ov_core_FOUND AND ov_init_FOUND)
-      SET(ov_msckf_INCLUDE_DIRS
-         ${ov_msckf_INCLUDE_DIRS}
-         ${ov_core_INCLUDE_DIRS}
-         ${ov_init_INCLUDE_DIRS})
-      SET(ov_msckf_LIBRARIES
-         ${ov_msckf_LIBRARIES}
-         ${ov_core_LIBRARIES}
-         ${ov_init_LIBRARIES})
-      MESSAGE(STATUS "Found OpenVINS: ${ov_msckf_INCLUDE_DIRS}")
-    ENDIF()
 ENDIF(WITH_OPENVINS)
 
 IF(WITH_FASTCV)
@@ -916,6 +901,9 @@ IF(WITH_OPENGV OR okvis_FOUND)
          set(BUILD_TESTS OFF)
          set(CMAKE_BUILD_TYPE Release)
          set(CMAKE_POLICY_DEFAULT_CMP0077 NEW)
+         # OpenGV's CMakeLists.txt declares cmake_minimum_required(VERSION 2.x),
+         # which CMake >= 4.0 (e.g. recent Ubuntu) rejects. Allow it to configure.
+         set(CMAKE_POLICY_VERSION_MINIMUM 3.5)
          # Eigen should have been already added by PCL, just populate the compatible variables
          IF(EIGEN_INCLUDE_DIRS)
             set(EIGEN_INCLUDE_DIRS "${EIGEN_INCLUDE_DIRS}" CACHE PATH "Eigen include dirs" FORCE)
@@ -1223,7 +1211,7 @@ ENDIF()
 IF(NOT vins_FOUND)
    SET(VINSFUSION "//")
 ENDIF()
-IF(NOT ov_msckf_FOUND)
+IF(NOT OpenVINS_FOUND)
    SET(OPENVINS "//")
 ENDIF()
 IF(NOT CUVSLAM_FOUND)
@@ -1981,7 +1969,7 @@ ELSE()
 MESSAGE(STATUS "  With VINS-Fusion          = NO (VINS-Fusion not found)")
 ENDIF()
 
-IF(ov_msckf_FOUND)
+IF(OpenVINS_FOUND)
 MESSAGE(STATUS "  With OpenVINS             = YES (License: GPLv3)")
 ELSEIF(NOT WITH_OPENVINS)
 MESSAGE(STATUS "  With OpenVINS             = NO (WITH_OPENVINS=OFF)")

app/ios/RTABMapApp.xcodeproj/project.pbxproj

@@ -1065,7 +1065,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib/opencv4/3rdparty",
 				);
-				MARKETING_VERSION = 0.22.0;
+				MARKETING_VERSION = 0.23.7;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1125,7 +1125,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib/opencv4/3rdparty",
 				);
-				MARKETING_VERSION = 0.22.0;
+				MARKETING_VERSION = 0.23.7;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;

app/ios/RTABMapApp/install_deps.sh

@@ -175,6 +175,18 @@ cd $pwd
 #rm -rf g2o
 fi
 
+# g2o's installed CMake config hard-codes the absolute build-time prefix of its
+# external dependencies (e.g. suitesparse) in INTERFACE_INCLUDE_DIRECTORIES. That
+# path doesn't exist when the prebuilt Libraries archive is unpacked on another
+# machine (CI), breaking find_package(g2o) with "includes non-existent path".
+# Rewrite those absolute paths to be relocatable (relative to the config file).
+# Run unconditionally (outside the build guard above) so it also fixes the prebuilt
+# archive in CI, where the g2o build step is skipped.
+find "$prefix/lib" -path '*/cmake/g2o/*.cmake' -print0 | while IFS= read -r -d '' f
+do
+  sed -i '' -E 's#[^";]*/Libraries#${CMAKE_CURRENT_LIST_DIR}/../../..#g' "$f"
+done
+
 # VTK
 if [ ! -e $prefix/lib/vtk.framework ]
 then

cmake_modules/FindOpenVINS.cmake

@@ -0,0 +1,44 @@
+# Find OpenVINS
+# 
+#   We search for a vins installation in ROS/ROS2 first, then fallback on 
+#   ros-free library in common install paths
+
+FIND_PACKAGE(ov_msckf QUIET)
+IF(ov_msckf_FOUND)
+   # On ROS2, the indirect includes and libraries
+   # are not forwarded by ov_msckf target, append them manually
+   FIND_PACKAGE(ov_core)
+   FIND_PACKAGE(ov_init)
+   IF(ov_msckf_FOUND AND ov_core_FOUND AND ov_init_FOUND)
+      SET(OpenVINS_FOUND TRUE)
+      SET(OpenVINS_INCLUDE_DIRS
+         ${ov_msckf_INCLUDE_DIRS}
+         ${ov_core_INCLUDE_DIRS}
+         ${ov_init_INCLUDE_DIRS})
+      SET(OpenVINS_LIBRARIES
+         ${ov_msckf_LIBRARIES}
+         ${ov_core_LIBRARIES}
+         ${ov_init_LIBRARIES})
+   ENDIF()
+ELSE()
+   find_path(OpenVINS_INCLUDE_DIR NAMES core/VioManager.h PATH_SUFFIXES open_vins)
+   find_library(OpenVINS_LIBRARY NAMES ov_msckf_lib)
+   IF (OpenVINS_INCLUDE_DIR AND OpenVINS_LIBRARY)
+      SET(OpenVINS_FOUND TRUE)
+      SET(OpenVINS_INCLUDE_DIRS ${OpenVINS_INCLUDE_DIR})
+      SET(OpenVINS_LIBRARIES ${OpenVINS_LIBRARY})
+   ENDIF()
+ENDIF()
+
+IF (OpenVINS_FOUND)
+   # show which OpenVINS was found only if not quiet
+   IF (NOT OpenVINS_FIND_QUIETLY)
+      MESSAGE(STATUS "Found OpenVINS: ${OpenVINS_LIBRARIES} ${OpenVINS_INCLUDE_DIRS}")
+   ENDIF (NOT OpenVINS_FIND_QUIETLY)
+ELSE (OpenVINS_FOUND)
+   # fatal error if OpenVINS is required but not found
+   IF (OpenVINS_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find OpenVINS")
+   ENDIF (OpenVINS_FIND_REQUIRED)
+ENDIF (OpenVINS_FOUND)
+

corelib/include/rtabmap/core/Parameters.h

@@ -620,66 +620,67 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM_STR(OdomVINSFusion, ConfigPath,     "",  "Path of VINS-Fusion config file.");
 
     // Odometry OpenVINS
-    RTABMAP_PARAM(OdomOpenVINS, UseStereo,                 bool,   true,   "If we have more than 1 camera, if we should try to track stereo constraints between pairs");
+    RTABMAP_PARAM_STR(OdomOpenVINS, ConfigPath,            "",   "Path of OpenVINS config file (*.yaml). Same format used than OpenVINS library. Note that any parameter from that config file will overwrite the same parameter in OdomOpenVINS group.");
-    RTABMAP_PARAM(OdomOpenVINS, UseKLT,                    bool,   true,   "If true we will use KLT, otherwise use a ORB descriptor + robust matching");
+    RTABMAP_PARAM(OdomOpenVINS, UseStereo,                 bool,   true,   "If we have more than 1 camera, if we should try to track stereo constraints between pairs.");
-    RTABMAP_PARAM(OdomOpenVINS, NumPts,                    int,    200,    "Number of points (per camera) we will extract and try to track");
+    RTABMAP_PARAM(OdomOpenVINS, UseKLT,                    bool,   true,   "If true we will use KLT, otherwise use a ORB descriptor + robust matching.");
-    RTABMAP_PARAM(OdomOpenVINS, MinPxDist,                 int,    15,     "Eistance between features (features near each other provide less information)");
+    RTABMAP_PARAM(OdomOpenVINS, NumPts,                    int,    200,    "Number of points (per camera) we will extract and try to track.");
-    RTABMAP_PARAM(OdomOpenVINS, FiTriangulate1d,           bool,   false,  "If we should perform 1d triangulation instead of 3d");
+    RTABMAP_PARAM(OdomOpenVINS, MinPxDist,                 int,    15,     "Eistance between features (features near each other provide less information).");
-    RTABMAP_PARAM(OdomOpenVINS, FiRefineFeatures,          bool,   true,   "If we should perform Levenberg-Marquardt refinement");
+    RTABMAP_PARAM(OdomOpenVINS, FiTriangulate1d,           bool,   false,  "If we should perform 1d triangulation instead of 3d.");
-    RTABMAP_PARAM(OdomOpenVINS, FiMaxRuns,                 int,    5,      "Max runs for Levenberg-Marquardt");
+    RTABMAP_PARAM(OdomOpenVINS, FiRefineFeatures,          bool,   true,   "If we should perform Levenberg-Marquardt refinement.");
-    RTABMAP_PARAM(OdomOpenVINS, FiMaxBaseline,             double, 40,     "Max baseline ratio to accept triangulated features");
+    RTABMAP_PARAM(OdomOpenVINS, FiMaxRuns,                 int,    5,      "Max runs for Levenberg-Marquardt.");
-    RTABMAP_PARAM(OdomOpenVINS, FiMaxCondNumber,           double, 10000,  "Max condition number of linear triangulation matrix accept triangulated features");
+    RTABMAP_PARAM(OdomOpenVINS, FiMaxBaseline,             double, 40,     "Max baseline ratio to accept triangulated features.");
+    RTABMAP_PARAM(OdomOpenVINS, FiMaxCondNumber,           double, 10000,  "Max condition number of linear triangulation matrix accept triangulated features.");
 
-    RTABMAP_PARAM(OdomOpenVINS, UseFEJ,                    bool,   true,   "If first-estimate Jacobians should be used (enable for good consistency)");
+    RTABMAP_PARAM(OdomOpenVINS, UseFEJ,                    bool,   true,   "If first-estimate Jacobians should be used (enable for good consistency).");
-    RTABMAP_PARAM(OdomOpenVINS, Integration,               int,    1,      "0=discrete, 1=rk4, 2=analytical (if rk4 or analytical used then analytical covariance propagation is used)");
+    RTABMAP_PARAM(OdomOpenVINS, Integration,               int,    1,      "0=discrete, 1=rk4, 2=analytical (if rk4 or analytical used then analytical covariance propagation is used).");
-    RTABMAP_PARAM(OdomOpenVINS, CalibCamExtrinsics,        bool,   false,  "Bool to determine whether or not to calibrate imu-to-camera pose");
+    RTABMAP_PARAM(OdomOpenVINS, CalibCamExtrinsics,        bool,   false,  "Bool to determine whether or not to calibrate imu-to-camera pose.");
-    RTABMAP_PARAM(OdomOpenVINS, CalibCamIntrinsics,        bool,   false,  "Bool to determine whether or not to calibrate camera intrinsics");
+    RTABMAP_PARAM(OdomOpenVINS, CalibCamIntrinsics,        bool,   false,  "Bool to determine whether or not to calibrate camera intrinsics.");
-    RTABMAP_PARAM(OdomOpenVINS, CalibCamTimeoffset,        bool,   false,  "Bool to determine whether or not to calibrate camera to IMU time offset");
+    RTABMAP_PARAM(OdomOpenVINS, CalibCamTimeoffset,        bool,   false,  "Bool to determine whether or not to calibrate camera to IMU time offset.");
-    RTABMAP_PARAM(OdomOpenVINS, CalibIMUIntrinsics,        bool,   false,  "Bool to determine whether or not to calibrate the IMU intrinsics");
+    RTABMAP_PARAM(OdomOpenVINS, CalibIMUIntrinsics,        bool,   false,  "Bool to determine whether or not to calibrate the IMU intrinsics.");
-    RTABMAP_PARAM(OdomOpenVINS, CalibIMUGSensitivity,      bool,   false,  "Bool to determine whether or not to calibrate the Gravity sensitivity");
+    RTABMAP_PARAM(OdomOpenVINS, CalibIMUGSensitivity,      bool,   false,  "Bool to determine whether or not to calibrate the Gravity sensitivity.");
-    RTABMAP_PARAM(OdomOpenVINS, MaxClones,                 int,    11,     "Max clone size of sliding window");
+    RTABMAP_PARAM(OdomOpenVINS, MaxClones,                 int,    11,     "Max clone size of sliding window.");
-    RTABMAP_PARAM(OdomOpenVINS, MaxSLAM,                   int,    50,     "Max number of estimated SLAM features");
+    RTABMAP_PARAM(OdomOpenVINS, MaxSLAM,                   int,    50,     "Max number of estimated SLAM features.");
-    RTABMAP_PARAM(OdomOpenVINS, MaxSLAMInUpdate,           int,    25,     "Max number of SLAM features we allow to be included in a single EKF update.");
+    RTABMAP_PARAM(OdomOpenVINS, MaxSLAMInUpdate,           int,    25,     "Max number of SLAM features we allow to be included in a single EKF update..");
-    RTABMAP_PARAM(OdomOpenVINS, MaxMSCKFInUpdate,          int,    50,     "Max number of MSCKF features we will use at a given image timestep.");
+    RTABMAP_PARAM(OdomOpenVINS, MaxMSCKFInUpdate,          int,    50,     "Max number of MSCKF features we will use at a given image timestep..");
-    RTABMAP_PARAM(OdomOpenVINS, FeatRepMSCKF,              int,    0,      "What representation our features are in (msckf features)");
+    RTABMAP_PARAM(OdomOpenVINS, FeatRepMSCKF,              int,    0,      "What representation our features are in (msckf features).");
-    RTABMAP_PARAM(OdomOpenVINS, FeatRepSLAM,               int,    4,      "What representation our features are in (slam features)");
+    RTABMAP_PARAM(OdomOpenVINS, FeatRepSLAM,               int,    4,      "What representation our features are in (slam features).");
-    RTABMAP_PARAM(OdomOpenVINS, DtSLAMDelay,               double, 0.0,    "Delay, in seconds, that we should wait from init before we start estimating SLAM features");
+    RTABMAP_PARAM(OdomOpenVINS, DtSLAMDelay,               double, 0.0,    "Delay, in seconds, that we should wait from init before we start estimating SLAM features.");
-    RTABMAP_PARAM(OdomOpenVINS, GravityMag,                double, 9.81,   "Gravity magnitude in the global frame (i.e. should be 9.81 typically)");
+    RTABMAP_PARAM(OdomOpenVINS, GravityMag,                double, 9.81,   "Gravity magnitude in the global frame (i.e. should be 9.81 typically).");
-    RTABMAP_PARAM_STR(OdomOpenVINS, LeftMaskPath,                  "",     "Mask for left image");
+    RTABMAP_PARAM_STR(OdomOpenVINS, LeftMaskPath,                  "",     "Mask for left image.");
-    RTABMAP_PARAM_STR(OdomOpenVINS, RightMaskPath,                 "",     "Mask for right image");
+    RTABMAP_PARAM_STR(OdomOpenVINS, RightMaskPath,                 "",     "Mask for right image.");
 
-    RTABMAP_PARAM(OdomOpenVINS, InitWindowTime,            double, 2.0,    "Amount of time we will initialize over (seconds)");
+    RTABMAP_PARAM(OdomOpenVINS, InitWindowTime,            double, 2.0,    "Amount of time we will initialize over (seconds).");
-    RTABMAP_PARAM(OdomOpenVINS, InitIMUThresh,             double, 1.0,    "Variance threshold on our acceleration to be classified as moving");
+    RTABMAP_PARAM(OdomOpenVINS, InitIMUThresh,             double, 1.0,    "Variance threshold on our acceleration to be classified as moving.");
-    RTABMAP_PARAM(OdomOpenVINS, InitMaxDisparity,          double, 10.0,   "Max disparity to consider the platform stationary (dependent on resolution)");
+    RTABMAP_PARAM(OdomOpenVINS, InitMaxDisparity,          double, 10.0,   "Max disparity to consider the platform stationary (dependent on resolution).");
-    RTABMAP_PARAM(OdomOpenVINS, InitMaxFeatures,           int,    50,     "How many features to track during initialization (saves on computation)");
+    RTABMAP_PARAM(OdomOpenVINS, InitMaxFeatures,           int,    50,     "How many features to track during initialization (saves on computation).");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynUse,                bool,   false,  "If dynamic initialization should be used");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynUse,                bool,   false,  "If dynamic initialization should be used.");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEOptCalib,        bool,   false,  "If we should optimize calibration during intialization (not recommended)");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEOptCalib,        bool,   false,  "If we should optimize calibration during intialization (not recommended).");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxIter,         int,    50,     "How many iterations the MLE refinement should use (zero to skip the MLE)");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxIter,         int,    50,     "How many iterations the MLE refinement should use (zero to skip the MLE).");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxTime,         double, 0.05,   "How many seconds the MLE should be completed in");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxTime,         double, 0.05,   "How many seconds the MLE should be completed in.");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxThreads,      int,    6,      "How many threads the MLE should use");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxThreads,      int,    6,      "How many threads the MLE should use.");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynNumPose,            int,    6,      "Number of poses to use within our window time (evenly spaced)");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynNumPose,            int,    6,      "Number of poses to use within our window time (evenly spaced).");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMinDeg,             double, 10.0,   "Orientation change needed to try to init");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMinDeg,             double, 10.0,   "Orientation change needed to try to init.");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationOri,       double, 10.0,   "What to inflate the recovered q_GtoI covariance by");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationOri,       double, 10.0,   "What to inflate the recovered q_GtoI covariance by.");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationVel,       double, 100.0,  "What to inflate the recovered v_IinG covariance by");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationVel,       double, 100.0,  "What to inflate the recovered v_IinG covariance by.");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBg,        double, 10.0,   "What to inflate the recovered bias_g covariance by");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBg,        double, 10.0,   "What to inflate the recovered bias_g covariance by.");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBa,        double, 100.0,  "What to inflate the recovered bias_a covariance by");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBa,        double, 100.0,  "What to inflate the recovered bias_a covariance by.");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMinRecCond,         double, 1e-15,  "Reciprocal condition number thresh for info inversion");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMinRecCond,         double, 1e-15,  "Reciprocal condition number thresh for info inversion.");
 
-    RTABMAP_PARAM(OdomOpenVINS, TryZUPT,                   bool,   true,   "If we should try to use zero velocity update");
+    RTABMAP_PARAM(OdomOpenVINS, TryZUPT,                   bool,   true,   "If we should try to use zero velocity update.");
-    RTABMAP_PARAM(OdomOpenVINS, ZUPTChi2Multiplier,        double, 0.0,    "Chi2 multiplier for zero velocity");
+    RTABMAP_PARAM(OdomOpenVINS, ZUPTChi2Multiplier,        double, 0.0,    "Chi2 multiplier for zero velocity.");
-    RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxVelodicy,           double, 0.1,    "Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt)");
+    RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxVelodicy,           double, 0.1,    "Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt).");
-    RTABMAP_PARAM(OdomOpenVINS, ZUPTNoiseMultiplier,       double, 10.0,   "Multiplier of our zupt measurement IMU noise matrix (default should be 1.0)");
+    RTABMAP_PARAM(OdomOpenVINS, ZUPTNoiseMultiplier,       double, 10.0,   "Multiplier of our zupt measurement IMU noise matrix (default should be 1.0).");
-    RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxDisparity,          double, 0.5,    "Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt)");
+    RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxDisparity,          double, 0.5,    "Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt).");
-    RTABMAP_PARAM(OdomOpenVINS, ZUPTOnlyAtBeginning,       bool,   false,  "If we should only use the zupt at the very beginning static initialization phase");
+    RTABMAP_PARAM(OdomOpenVINS, ZUPTOnlyAtBeginning,       bool,   false,  "If we should only use the zupt at the very beginning static initialization phase.");
 
-    RTABMAP_PARAM(OdomOpenVINS, AccelerometerNoiseDensity, double, 0.01,   "[m/s^2/sqrt(Hz)] (accel \"white noise\")");
+    RTABMAP_PARAM(OdomOpenVINS, AccelerometerNoiseDensity, double, 0.01,   "[m/s^2/sqrt(Hz)] (accel \"white noise\").");
-    RTABMAP_PARAM(OdomOpenVINS, AccelerometerRandomWalk,   double, 0.001,  "[m/s^3/sqrt(Hz)] (accel bias diffusion)");
+    RTABMAP_PARAM(OdomOpenVINS, AccelerometerRandomWalk,   double, 0.001,  "[m/s^3/sqrt(Hz)] (accel bias diffusion).");
-    RTABMAP_PARAM(OdomOpenVINS, GyroscopeNoiseDensity,     double, 0.001,  "[rad/s/sqrt(Hz)] (gyro \"white noise\")");
+    RTABMAP_PARAM(OdomOpenVINS, GyroscopeNoiseDensity,     double, 0.001,  "[rad/s/sqrt(Hz)] (gyro \"white noise\").");
-    RTABMAP_PARAM(OdomOpenVINS, GyroscopeRandomWalk,       double, 0.0001, "[rad/s^2/sqrt(Hz)] (gyro bias diffusion)");
+    RTABMAP_PARAM(OdomOpenVINS, GyroscopeRandomWalk,       double, 0.0001, "[rad/s^2/sqrt(Hz)] (gyro bias diffusion).");
-    RTABMAP_PARAM(OdomOpenVINS, UpMSCKFSigmaPx,            double, 1.0,    "Pixel noise for MSCKF features");
+    RTABMAP_PARAM(OdomOpenVINS, UpMSCKFSigmaPx,            double, 1.0,    "Pixel noise for MSCKF features.");
-    RTABMAP_PARAM(OdomOpenVINS, UpMSCKFChi2Multiplier,     double, 1.0,    "Chi2 multiplier for MSCKF features");
+    RTABMAP_PARAM(OdomOpenVINS, UpMSCKFChi2Multiplier,     double, 1.0,    "Chi2 multiplier for MSCKF features.");
-    RTABMAP_PARAM(OdomOpenVINS, UpSLAMSigmaPx,             double, 1.0,    "Pixel noise for SLAM features");
+    RTABMAP_PARAM(OdomOpenVINS, UpSLAMSigmaPx,             double, 1.0,    "Pixel noise for SLAM features.");
-    RTABMAP_PARAM(OdomOpenVINS, UpSLAMChi2Multiplier,      double, 1.0,    "Chi2 multiplier for SLAM features");
+    RTABMAP_PARAM(OdomOpenVINS, UpSLAMChi2Multiplier,      double, 1.0,    "Chi2 multiplier for SLAM features.");
 
     // Odometry Open3D
     RTABMAP_PARAM(OdomOpen3D, MaxDepth,         float, 3.0,  "Maximum depth.");

corelib/src/CMakeLists.txt

@@ -781,16 +781,16 @@ IF(vins_FOUND)
 	)
 ENDIF(vins_FOUND)
 
-IF(ov_msckf_FOUND)
+IF(OpenVINS_FOUND)
     SET(INCLUDE_DIRS
-		${ov_msckf_INCLUDE_DIRS}
+		${OpenVINS_INCLUDE_DIRS}
 		${INCLUDE_DIRS}
 	)
 	SET(LIBRARIES
-		${ov_msckf_LIBRARIES}
+		${OpenVINS_LIBRARIES}
 		${LIBRARIES}
 	)
-ENDIF(ov_msckf_FOUND)
+ENDIF(OpenVINS_FOUND)
 
 IF(ORB_SLAM_FOUND)
     SET(INCLUDE_DIRS

corelib/src/Features2d.cpp

@@ -887,8 +887,17 @@ cv::Mat Feature2D::generateDescriptors(
 		UASSERT(!image.empty());
 		UASSERT(image.type() == CV_8UC1);
 		descriptors = generateDescriptorsImpl(image, keypoints);
-		UASSERT_MSG(descriptors.rows == (int)keypoints.size(), uFormat("descriptors=%d, keypoints=%d", descriptors.rows, (int)keypoints.size()).c_str());
+		if(descriptors.rows != (int)keypoints.size())
-		UDEBUG("Descriptors extracted = %d, remaining kpts=%d", descriptors.rows, (int)keypoints.size());
+		{
+			UWARN("Descriptor extraction returned %d rows for %d keypoints — "
+					"clearing keypoints to keep them in sync.",
+					descriptors.rows, (int)keypoints.size());
+			keypoints.clear();
+			descriptors = cv::Mat();
+		}
+		else {
+			UDEBUG("Descriptors extracted = %d, remaining kpts=%d", descriptors.rows, (int)keypoints.size());
+		}
 	}
 	return descriptors;
 }
@@ -2630,7 +2639,31 @@ cv::Mat SuperPointTorch::generateDescriptorsImpl(const cv::Mat & image, std::vec
 {
 #ifdef RTABMAP_TORCH
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
-	return superPoint_->compute(keypoints);
+	cv::Mat descriptors;
+	if(!keypoints.empty())
+	{
+		descriptors = superPoint_->compute(keypoints);
+		if(descriptors.empty())
+		{
+			// superpoint may have been reset between keypoint detection and now,
+			// re-detect features to re-inialize the descriptors matrix, then
+			// re-extract descriptors with original keypoints.
+			UWARN("Re-initializing superpoint on that image to extract descriptors");
+			if(!superPoint_->detect(image).empty())
+			{
+				descriptors = superPoint_->compute(keypoints);
+				if(descriptors.rows == (int)keypoints.size())
+				{
+					UWARN("Sucessfully re-initialized superpoint, returning %d descriptors.", descriptors.rows);
+				}
+			}
+			else
+			{
+				UWARN("Failed to re-initialize superpoint on that image, returning empty descriptors.");
+			}
+		}
+	}
+	return descriptors;
 #else
 	UWARN("RTAB-Map is not built with Torch support so SuperPoint Torch feature cannot be used!");
 	return cv::Mat();
@@ -2732,7 +2765,31 @@ cv::Mat SuperPointRpautrat::generateDescriptorsImpl(const cv::Mat & image, std::
 {
 #if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
-	return superPoint_->compute(keypoints);
+	cv::Mat descriptors;
+	if(!keypoints.empty())
+	{
+		descriptors = superPoint_->compute(keypoints);
+		if(descriptors.empty())
+		{
+			// superpoint may have been reset between keypoint detection and now,
+			// re-detect features to re-inialize the descriptors matrix, then
+			// re-extract descriptors with original keypoints.
+			UWARN("Re-initializing superpoint on that image to extract descriptors");
+			if(!superPoint_->detect(image).empty())
+			{
+				descriptors = superPoint_->compute(keypoints);
+				if(descriptors.rows == (int)keypoints.size())
+				{
+					UWARN("Sucessfully re-initialized superpoint, returning %d descriptors.", descriptors.rows);
+				}
+			}
+			else
+			{
+				UWARN("Failed to re-initialize superpoint on that image, returning empty descriptors.");
+			}
+		}
+	}
+	return descriptors;
 #else
 	UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!");
 	return cv::Mat();

corelib/src/MarkerDetector.cpp

@@ -551,7 +551,7 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 			}
 			if(idsAdded.find(det->id)!=idsAdded.end())
 			{
-				UWARN("Marker %d already added by another camera, ignoring detection from camera %d", det->id, cameraIndex);
+				UDEBUG("Marker %d already added by another camera, ignoring detection from camera %d", det->id, cameraIndex);
 				continue;
 			}
 
@@ -668,7 +668,7 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 			}
 			if(idsAdded.find(id) != idsAdded.end())
 			{
-				UWARN("Marker %d already added by another camera, ignoring detection from camera %d", id, cameraIndex);
+				UDEBUG("Marker %d already added by another camera, ignoring detection from camera %d", id, cameraIndex);
 				continue;
 			}
 

corelib/src/Memory.cpp

@@ -5205,6 +5205,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		// is using less features than feature2D->getMaxFeatures()
 		meanWordsPerLocation = 0;
 	}
+	UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
 
 	if(_parallelized && !isIntermediateNode)
 	{
@@ -5474,41 +5475,90 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
 			UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
 
-			UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
+			if(!imagesRectified && decimatedData.cameraModels().size())
-			if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
 			{
-				descriptors = cv::Mat();
+				UASSERT_MSG((int)keypoints.size() == descriptors.rows, uFormat("%d vs %d", (int)keypoints.size(), descriptors.rows).c_str());
-			}
+				std::vector<cv::KeyPoint> keypointsValid;
-			else
+				keypointsValid.reserve(keypoints.size());
-			{
+				cv::Mat descriptorsValid;
-				if(!imagesRectified && decimatedData.cameraModels().size())
+				descriptorsValid.reserve(descriptors.rows);
-				{
-					UASSERT_MSG((int)keypoints.size() == descriptors.rows, uFormat("%d vs %d", (int)keypoints.size(), descriptors.rows).c_str());
-					std::vector<cv::KeyPoint> keypointsValid;
-					keypointsValid.reserve(keypoints.size());
-					cv::Mat descriptorsValid;
-					descriptorsValid.reserve(descriptors.rows);
 
-					//undistort keypoints before projection (RGB-D)
+				//undistort keypoints before projection (RGB-D)
-					if(decimatedData.cameraModels().size() == 1)
+				if(decimatedData.cameraModels().size() == 1)
+				{
+					std::vector<cv::Point2f> pointsIn, pointsOut;
+					cv::KeyPoint::convert(keypoints,pointsIn);
+					if(decimatedData.cameraModels()[0].D_raw().cols == 6)
 					{
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+						// Equidistant / FishEye
+						// get only k parameters (k1,k2,p1,p2,k3,k4)
+						cv::Mat D(1, 4, CV_64FC1);
+						D.at<double>(0,0) = decimatedData.cameraModels()[0].D_raw().at<double>(0,0);
+						D.at<double>(0,1) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1);
+						D.at<double>(0,2) = decimatedData.cameraModels()[0].D_raw().at<double>(0,4);
+						D.at<double>(0,3) = decimatedData.cameraModels()[0].D_raw().at<double>(0,5);
+						cv::fisheye::undistortPoints(pointsIn, pointsOut,
+								decimatedData.cameraModels()[0].K_raw(),
+								D,
+								decimatedData.cameraModels()[0].R(),
+								decimatedData.cameraModels()[0].P());
+					}
+					else
+#else
+						UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
+								CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
+					}
+#endif
+					{
+						//RadialTangential
+						cv::undistortPoints(pointsIn, pointsOut,
+								decimatedData.cameraModels()[0].K_raw(),
+								decimatedData.cameraModels()[0].D_raw(),
+								decimatedData.cameraModels()[0].R(),
+								decimatedData.cameraModels()[0].P());
+					}
+					UASSERT(pointsOut.size() == keypoints.size());
+					for(unsigned int i=0; i<pointsOut.size(); ++i)
+					{
+						if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<decimatedData.cameraModels()[0].imageWidth() &&
+							pointsOut.at(i).y>=0 && pointsOut.at(i).y<decimatedData.cameraModels()[0].imageHeight())
+						{
+							keypointsValid.push_back(keypoints.at(i));
+							keypointsValid.back().pt.x = pointsOut.at(i).x;
+							keypointsValid.back().pt.y = pointsOut.at(i).y;
+							descriptorsValid.push_back(descriptors.row(i));
+						}
+					}
+				}
+				else
+				{
+					UASSERT(int((decimatedData.imageRaw().cols/decimatedData.cameraModels().size())*decimatedData.cameraModels().size()) == decimatedData.imageRaw().cols);
+					float subImageWidth = decimatedData.imageRaw().cols/decimatedData.cameraModels().size();
+					for(unsigned int i=0; i<keypoints.size(); ++i)
+					{
+						int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
+						UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)decimatedData.cameraModels().size(),
+								uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
+										cameraIndex, (int)decimatedData.cameraModels().size(), keypoints[i].pt.x, subImageWidth, decimatedData.cameraModels()[0].imageWidth()).c_str());
+
 						std::vector<cv::Point2f> pointsIn, pointsOut;
-						cv::KeyPoint::convert(keypoints,pointsIn);
+						pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
-						if(decimatedData.cameraModels()[0].D_raw().cols == 6)
+						if(decimatedData.cameraModels()[cameraIndex].D_raw().cols == 6)
 						{
 #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
 							// Equidistant / FishEye
 							// get only k parameters (k1,k2,p1,p2,k3,k4)
 							cv::Mat D(1, 4, CV_64FC1);
-							D.at<double>(0,0) = decimatedData.cameraModels()[0].D_raw().at<double>(0,0);
+							D.at<double>(0,0) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,0);
-							D.at<double>(0,1) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1);
+							D.at<double>(0,1) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
-							D.at<double>(0,2) = decimatedData.cameraModels()[0].D_raw().at<double>(0,4);
+							D.at<double>(0,2) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
-							D.at<double>(0,3) = decimatedData.cameraModels()[0].D_raw().at<double>(0,5);
+							D.at<double>(0,3) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
 							cv::fisheye::undistortPoints(pointsIn, pointsOut,
-									decimatedData.cameraModels()[0].K_raw(),
+									decimatedData.cameraModels()[cameraIndex].K_raw(),
 									D,
-									decimatedData.cameraModels()[0].R(),
+									decimatedData.cameraModels()[cameraIndex].R(),
-									decimatedData.cameraModels()[0].P());
+									decimatedData.cameraModels()[cameraIndex].P());
 						}
 						else
 #else
@@ -5519,115 +5569,58 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						{
 							//RadialTangential
 							cv::undistortPoints(pointsIn, pointsOut,
-									decimatedData.cameraModels()[0].K_raw(),
+									decimatedData.cameraModels()[cameraIndex].K_raw(),
-									decimatedData.cameraModels()[0].D_raw(),
+									decimatedData.cameraModels()[cameraIndex].D_raw(),
-									decimatedData.cameraModels()[0].R(),
+									decimatedData.cameraModels()[cameraIndex].R(),
-									decimatedData.cameraModels()[0].P());
+									decimatedData.cameraModels()[cameraIndex].P());
 						}
-						UASSERT(pointsOut.size() == keypoints.size());
+
-						for(unsigned int i=0; i<pointsOut.size(); ++i)
+						if(pointsOut[0].x>=0 && pointsOut[0].x<decimatedData.cameraModels()[cameraIndex].imageWidth() &&
+							pointsOut[0].y>=0 && pointsOut[0].y<decimatedData.cameraModels()[cameraIndex].imageHeight())
 						{
-							if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<decimatedData.cameraModels()[0].imageWidth() &&
+							keypointsValid.push_back(keypoints.at(i));
-							   pointsOut.at(i).y>=0 && pointsOut.at(i).y<decimatedData.cameraModels()[0].imageHeight())
+							keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
-							{
+							keypointsValid.back().pt.y = pointsOut[0].y;
-								keypointsValid.push_back(keypoints.at(i));
+							descriptorsValid.push_back(descriptors.row(i));
-								keypointsValid.back().pt.x = pointsOut.at(i).x;
-								keypointsValid.back().pt.y = pointsOut.at(i).y;
-								descriptorsValid.push_back(descriptors.row(i));
-							}
 						}
 					}
-					else
-					{
-						UASSERT(int((decimatedData.imageRaw().cols/decimatedData.cameraModels().size())*decimatedData.cameraModels().size()) == decimatedData.imageRaw().cols);
-						float subImageWidth = decimatedData.imageRaw().cols/decimatedData.cameraModels().size();
-						for(unsigned int i=0; i<keypoints.size(); ++i)
-						{
-							int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
-							UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)decimatedData.cameraModels().size(),
-									uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
-											cameraIndex, (int)decimatedData.cameraModels().size(), keypoints[i].pt.x, subImageWidth, decimatedData.cameraModels()[0].imageWidth()).c_str());
-
-							std::vector<cv::Point2f> pointsIn, pointsOut;
-							pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
-							if(decimatedData.cameraModels()[cameraIndex].D_raw().cols == 6)
-							{
-#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
-								// Equidistant / FishEye
-								// get only k parameters (k1,k2,p1,p2,k3,k4)
-								cv::Mat D(1, 4, CV_64FC1);
-								D.at<double>(0,0) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,0);
-								D.at<double>(0,1) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
-								D.at<double>(0,2) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
-								D.at<double>(0,3) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
-								cv::fisheye::undistortPoints(pointsIn, pointsOut,
-										decimatedData.cameraModels()[cameraIndex].K_raw(),
-										D,
-										decimatedData.cameraModels()[cameraIndex].R(),
-										decimatedData.cameraModels()[cameraIndex].P());
-							}
-							else
-#else
-								UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
-										CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
-							}
-#endif
-							{
-								//RadialTangential
-								cv::undistortPoints(pointsIn, pointsOut,
-										decimatedData.cameraModels()[cameraIndex].K_raw(),
-										decimatedData.cameraModels()[cameraIndex].D_raw(),
-										decimatedData.cameraModels()[cameraIndex].R(),
-										decimatedData.cameraModels()[cameraIndex].P());
-							}
-
-							if(pointsOut[0].x>=0 && pointsOut[0].x<decimatedData.cameraModels()[cameraIndex].imageWidth() &&
-							   pointsOut[0].y>=0 && pointsOut[0].y<decimatedData.cameraModels()[cameraIndex].imageHeight())
-							{
-								keypointsValid.push_back(keypoints.at(i));
-								keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
-								keypointsValid.back().pt.y = pointsOut[0].y;
-								descriptorsValid.push_back(descriptors.row(i));
-							}
-						}
-					}
-
-					keypoints = keypointsValid;
-					descriptors = descriptorsValid;
-
-					t = timer.ticks();
-					if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
-					UDEBUG("time rectification = %fs", t);
 				}
 
-				if(useProvided3dPoints && keypoints.size() != data.keypoints3D().size())
+				keypoints = keypointsValid;
+				descriptors = descriptorsValid;
+
+				t = timer.ticks();
+				if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
+				UDEBUG("time rectification = %fs", t);
+			}
+
+			if(useProvided3dPoints && keypoints.size() != data.keypoints3D().size())
+			{
+				UDEBUG("Using provided 3d points (%d->%d)", (int)data.keypoints3D().size(), (int)keypoints.size());
+				keypoints3D.resize(keypoints.size());
+				for(size_t i=0; i<keypoints.size(); ++i)
 				{
-					UDEBUG("Using provided 3d points (%d->%d)", (int)data.keypoints3D().size(), (int)keypoints.size());
+					UASSERT(keypoints[i].class_id < (int)data.keypoints3D().size());
-					keypoints3D.resize(keypoints.size());
+					keypoints3D[i] = data.keypoints3D()[keypoints[i].class_id];
-					for(size_t i=0; i<keypoints.size(); ++i)
-					{
-						UASSERT(keypoints[i].class_id < (int)data.keypoints3D().size());
-						keypoints3D[i] = data.keypoints3D()[keypoints[i].class_id];
-					}
-				}
-				else if(useProvided3dPoints && keypoints.size() == data.keypoints3D().size())
-				{
-					UDEBUG("Using provided 3d points (%d)", (int)data.keypoints3D().size());
-					keypoints3D = data.keypoints3D();
-				}
-				else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
-				   (!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModels().size() && decimatedData.stereoCameraModels()[0].isValidForProjection()))
-				{
-					keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
-					t = timer.ticks();
-					if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
-					UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
-				}
-				if(depthMask.empty() && (_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f))
-				{
-					_feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth());
 				}
 			}
+			else if(useProvided3dPoints && keypoints.size() == data.keypoints3D().size())
+			{
+				UDEBUG("Using provided 3d points (%d)", (int)data.keypoints3D().size());
+				keypoints3D = data.keypoints3D();
+			}
+			else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
+				(!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModels().size() && decimatedData.stereoCameraModels()[0].isValidForProjection()))
+			{
+				keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
+				t = timer.ticks();
+				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+				UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
+			}
+			if(depthMask.empty() && (_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f))
+			{
+				_feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth());
+			}
 		}
 		else if(data.imageRaw().empty())
 		{
@@ -5853,12 +5846,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			t = timer.ticks();
 			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
 			UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
-
-			UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
-			if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
-			{
-				descriptors = cv::Mat();
-			}
 		}
 	}
 
@@ -5884,7 +5871,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	bool addedToDictionary = false;
 	if(!keypoints.empty())
 	{
-		if(descriptors.rows && !isIntermediateNode)
+		if(descriptors.rows && 
+			!isIntermediateNode && // don't add intermediate nodes to dictionary
+			descriptors.rows >= int(_badSignRatio * float(meanWordsPerLocation))) // don't add bad signatures to dictionary
 		{
 			// In case the number of features we want to do quantization is lower
 			// than extracted ones (that would be used for transform estimation)
@@ -5991,7 +5980,11 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		else
 		{
 			// Set all words as not used in dictionary
-			wordIds.resize(keypoints.size(),-1);
+			int negIndex = -1;
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				wordIds.push_back(negIndex--);
+			}
 		}
 
 		t = timer.ticks();

corelib/src/odometry/OdometryOpenVINS.cpp

@@ -152,6 +152,85 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) :
 	params_->init_options.sigma_wb = params_->imu_noises.sigma_wb;
 	params_->init_options.sigma_pix = params_->slam_options.sigma_pix;
 	params_->init_options.gravity_mag = params_->gravity_mag;
+
+	if(parameters.find(Parameters::kOdomOpenVINSConfigPath()) != parameters.end())
+	{
+		// Load the config: will override all parameters above!
+		std::string configPath = parameters.at(Parameters::kOdomOpenVINSConfigPath());
+		if(!configPath.empty())
+		{
+			if(UFile::exists(configPath))
+			{
+				UWARN("OpenVINS config file is provided (%s=\"%s\"), reading it. The parameters from the config file will overwrite OdomOpenVINS/*** parameters.",
+					Parameters::kOdomOpenVINSConfigPath().c_str(), configPath.c_str());
+				auto parser = std::make_shared<ov_core::YamlParser>(configPath);
+
+				// The sequence of loading is based on VioManagerOptions::print_and_load()
+				// We removed all parts about intrinsics/extrinsics, which will be loaded later
+				// when we receive the data (which should already include intrinsics and extrinsics).
+
+				params_->state_options.print(parser);
+				
+				params_->init_options.print_and_load_initializer(parser);
+    			params_->init_options.print_and_load_noise(parser);
+				parser->parse_config("gravity_mag", params_->init_options.gravity_mag);
+				parser->parse_config("max_cameras", params_->init_options.num_cameras);
+				parser->parse_config("use_stereo", params_->init_options.use_stereo);
+				parser->parse_config("downsample_cameras", params_->init_options.downsample_cameras);
+
+				parser->parse_config("dt_slam_delay", params_->dt_slam_delay);
+				parser->parse_config("try_zupt", params_->try_zupt);
+				parser->parse_config("zupt_max_velocity",params_-> zupt_max_velocity);
+				parser->parse_config("zupt_noise_multiplier", params_->zupt_noise_multiplier);
+				parser->parse_config("zupt_max_disparity", params_->zupt_max_disparity);
+				parser->parse_config("zupt_only_at_beginning", params_->zupt_only_at_beginning);
+				parser->parse_config("record_timing_information", params_->record_timing_information);
+				parser->parse_config("record_timing_filepath", params_->record_timing_filepath);
+
+				params_->print_and_load_trackers(parser);
+				params_->print_and_load_noise(parser);
+
+				if(params_->state_options.num_cameras > 2)
+				{
+					UFATAL("OpenVINS integration in RTAB-Map doesn't support more than 2 cameras (num_cameras=%d).", params_->state_options.num_cameras);
+				}
+
+				parser->parse_config("gravity_mag", params_->gravity_mag);
+				parser->parse_config("use_mask", params_->use_mask);
+				params_->masks.clear();
+				if (params_->use_mask) {
+					for (int i = 0; i < params_->state_options.num_cameras; i++) {
+						std::string mask_path;
+						std::string mask_node = "mask" + std::to_string(i);
+						parser->parse_config(mask_node, mask_path);
+						std::string total_mask_path = parser->get_config_folder() + mask_path;
+						if (!boost::filesystem::exists(total_mask_path)) {
+							PRINT_ERROR(RED "VioManager(): invalid mask path:\n" RESET);
+							PRINT_ERROR(RED "\t- mask%d - %s\n" RESET, i, total_mask_path.c_str());
+							std::exit(EXIT_FAILURE);
+						}
+						params_->masks.emplace(i, cv::imread(total_mask_path, cv::IMREAD_GRAYSCALE));
+					}
+				}
+
+				if (!parser->successful()) {
+					UWARN("Not all expected OpenVINS parameters were read successfully "
+						"from \"%s\". Values from RTAB-Map's OpenOpenVINS/* parameters "
+						"will be used instead for the missing ones.",
+						configPath.c_str());
+				}
+				else {
+					UINFO("OpenVINS config file(%s=\"%s\") read.",
+					Parameters::kOdomOpenVINSConfigPath().c_str(), configPath.c_str());
+				}
+			}
+			else
+			{
+				UERROR("OpenVINS config file is provided (%s=\"%s\") but it doesn't exist!",
+					Parameters::kOdomOpenVINSConfigPath().c_str(), configPath.c_str());
+			}
+		}
+	}
 #endif
 }
 

corelib/src/optimizer/OptimizerTORO.cpp

@@ -380,7 +380,7 @@ bool OptimizerTORO::saveGraph(
 	if(file)
 	{
 		
-		for (std::map<int, Transform>::const_iterator iter = poses.begin(); iter != poses.end(); ++iter)
+		for (std::map<int, Transform>::const_iterator iter = poses.lower_bound(0); iter != poses.end(); ++iter)
 		{
 			if (isSlam2d())
 			{
@@ -409,7 +409,7 @@ bool OptimizerTORO::saveGraph(
 
 		for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 		{
-			if (iter->second.type() != Link::kPosePrior && iter->second.type() != Link::kGravity)
+			if (iter->second.from() != iter->second.to() && iter->second.type() != Link::kLandmark)
 			{
 				if (isSlam2d())
 				{
@@ -494,9 +494,31 @@ bool OptimizerTORO::loadGraph(
 		while ( fgets (line , 400 , file) != NULL )
 		{
 			std::vector<std::string> strList = uListToVector(uSplit(uReplaceChar(line, '\n', ' '), ' '));
-			if(strList.size() == 8)
+			if(strList.empty())
 			{
-				//VERTEX3
+				continue;
+			}
+			const std::string & tag = strList[0];
+			if(tag.compare("VERTEX2") == 0 && strList.size() == 5)
+			{
+				//VERTEX2 id x y theta
+				int id = atoi(strList[1].c_str());
+				float x = uStr2Float(strList[2]);
+				float y = uStr2Float(strList[3]);
+				float theta = uStr2Float(strList[4]);
+				Transform pose(x, y, theta);
+				if(poses.find(id) == poses.end())
+				{
+					poses.insert(std::make_pair(id, pose));
+				}
+				else
+				{
+					UFATAL("Pose %d already added", id);
+				}
+			}
+			else if(tag.compare("VERTEX3") == 0 && strList.size() == 8)
+			{
+				//VERTEX3 id x y z roll pitch yaw
 				int id = atoi(strList[1].c_str());
 				float x = uStr2Float(strList[2]);
 				float y = uStr2Float(strList[3]);
@@ -514,9 +536,44 @@ bool OptimizerTORO::loadGraph(
 					UFATAL("Pose %d already added", id);
 				}
 			}
-			else if(strList.size() == 30)
+			else if(tag.compare("EDGE2") == 0 && strList.size() == 12)
 			{
-				//EDGE3
+				//EDGE2 observed_vertex_id observing_vertex_id x y theta inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
+				int idFrom = atoi(strList[1].c_str());
+				int idTo = atoi(strList[2].c_str());
+				float x = uStr2Float(strList[3]);
+				float y = uStr2Float(strList[4]);
+				float theta = uStr2Float(strList[5]);
+				cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
+				informationMatrix.at<double>(0,0) = uStr2Float(strList[6]);  // x-x
+				informationMatrix.at<double>(0,1) = uStr2Float(strList[7]);  // x-y
+				informationMatrix.at<double>(0,5) = uStr2Float(strList[8]);  // x-theta
+				informationMatrix.at<double>(1,1) = uStr2Float(strList[9]);  // y-y
+				informationMatrix.at<double>(1,5) = uStr2Float(strList[10]); // y-theta
+				informationMatrix.at<double>(5,5) = uStr2Float(strList[11]); // theta-theta
+				// symmetric counterparts
+				informationMatrix.at<double>(1,0) = informationMatrix.at<double>(0,1);
+				informationMatrix.at<double>(5,0) = informationMatrix.at<double>(0,5);
+				informationMatrix.at<double>(5,1) = informationMatrix.at<double>(1,5);
+				informationMatrix.at<double>(2,2) = 0.00010001; // 9999 cov
+				informationMatrix.at<double>(3,3) = 0.00010001; // 9999 cov
+				informationMatrix.at<double>(4,4) = 0.00010001; // 9999 cov
+				UASSERT_MSG(informationMatrix.at<double>(0,0) > 0.0 && informationMatrix.at<double>(1,1) > 0.0 && informationMatrix.at<double>(5,5) > 0.0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
+				Transform transform(x, y, theta);
+				if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end())
+				{
+					//Link type is unknown
+					Link link(idFrom, idTo, Link::kUndef, transform, informationMatrix);
+					edgeConstraints.insert(std::pair<int, Link>(idFrom, link));
+				}
+				else
+				{
+					UERROR("Referred poses from the link (%d->%d) don't exist! Link ignored!", idFrom, idTo);
+				}
+			}
+			else if(tag.compare("EDGE3") == 0 && strList.size() == 30)
+			{
+				//EDGE3 observed_vertex_id observing_vertex_id x y z roll pitch yaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
 				int idFrom = atoi(strList[1].c_str());
 				int idTo = atoi(strList[2].c_str());
 				float x = uStr2Float(strList[3]);
@@ -525,15 +582,20 @@ bool OptimizerTORO::loadGraph(
 				float roll = uStr2Float(strList[6]);
 				float pitch = uStr2Float(strList[7]);
 				float yaw = uStr2Float(strList[8]);
+				// upper triangle is stored row by row (same order as saveGraph)
 				cv::Mat informationMatrix(6,6,CV_64FC1);
-				informationMatrix.at<double>(3,3) = uStr2Float(strList[9]);
+				int index = 9;
-				informationMatrix.at<double>(4,4) = uStr2Float(strList[15]);
+				for(int i=0; i<6; ++i)
-				informationMatrix.at<double>(5,5) = uStr2Float(strList[20]);
+				{
-				UASSERT_MSG(informationMatrix.at<double>(3,3) > 0.0 && informationMatrix.at<double>(4,4) > 0.0 && informationMatrix.at<double>(5,5) > 0.0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
+					for(int j=i; j<6; ++j)
-				informationMatrix.at<double>(0,0) = uStr2Float(strList[24]);
+					{
-				informationMatrix.at<double>(1,1) = uStr2Float(strList[27]);
+						double value = uStr2Float(strList[index++]);
-				informationMatrix.at<double>(2,2) = uStr2Float(strList[29]);
+						informationMatrix.at<double>(i,j) = value;
-				UASSERT_MSG(informationMatrix.at<double>(0,0) > 0.0 && informationMatrix.at<double>(1,1) > 0.0 && informationMatrix.at<double>(2,2) > 0.0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
+						informationMatrix.at<double>(j,i) = value;
+					}
+				}
+				UASSERT_MSG(informationMatrix.at<double>(0,0) > 0.0 && informationMatrix.at<double>(1,1) > 0.0 && informationMatrix.at<double>(2,2) > 0.0 &&
+						informationMatrix.at<double>(3,3) > 0.0 && informationMatrix.at<double>(4,4) > 0.0 && informationMatrix.at<double>(5,5) > 0.0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
 				Transform transform(x, y, z, roll, pitch, yaw);
 				if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end())
 				{
@@ -546,7 +608,7 @@ bool OptimizerTORO::loadGraph(
 					UERROR("Referred poses from the link (%d->%d) don't exist! Link ignored!", idFrom, idTo);
 				}
 			}
-			else if(strList.size())
+			else
 			{
 				UFATAL("Error parsing graph file %s on line \"%s\" (strList.size()=%d)", fileName.c_str(), line, (int)strList.size());
 			}

corelib/src/python/PyDetector.cpp

@@ -202,18 +202,30 @@ std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & imag
 
 					arrayPtr = reinterpret_cast<PyArrayObject*>(descPtr);
 					int nDesc = PyArray_SHAPE(arrayPtr)[0];
-					UASSERT(nDesc = nKpts);
 					int dim = PyArray_SHAPE(arrayPtr)[1];
 					type = PyArray_TYPE(arrayPtr);
 					UDEBUG("Desc array %dx%d (type=%d)", nDesc, dim, type);
-					UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned matches should type FLOAT=11, received type=%d", type).c_str());
 
-					c_out = reinterpret_cast<float*>(PyArray_DATA(arrayPtr));
+					if(nDesc != nKpts || dim <= 0)
-					for (int i = 0, kpt_idx = 0; i < nDesc*dim; i+=dim, kpt_idx++)
 					{
-						if(keep_kpt[kpt_idx]) {
+						UWARN("Python detector returned mismatched arrays: "
-							cv::Mat descriptor = cv::Mat(1, dim, CV_32FC1, &c_out[i]).clone();
+								"%d keypoints vs %d descriptors (dim=%d). "
-							descriptors_.push_back(descriptor);
+								"Returning empty features.",
+								nKpts, nDesc, dim);
+						keypoints.clear();
+						descriptors_ = cv::Mat();
+					}
+					else
+					{
+						UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned matches should type FLOAT=11, received type=%d", type).c_str());
+
+						c_out = reinterpret_cast<float*>(PyArray_DATA(arrayPtr));
+						for (int i = 0, kpt_idx = 0; i < nDesc*dim; i+=dim, kpt_idx++)
+						{
+							if(keep_kpt[kpt_idx]) {
+								cv::Mat descriptor = cv::Mat(1, dim, CV_32FC1, &c_out[i]).clone();
+								descriptors_.push_back(descriptor);
+							}
 						}
 					}
 				}
@@ -235,7 +247,15 @@ std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & imag
 
 cv::Mat PyDetector::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
-	UASSERT((int)keypoints.size() == descriptors_.rows);
+	if(!keypoints.empty() && (int)keypoints.size() != descriptors_.rows)
+	{
+		UERROR("The number of keypoints (%ld) doesn't match the number of buffered "
+			"descriptors (%d). PyDetector's descriptors extraction should "
+			"be called right after keypoints detection, with same keypoints "
+			"returned by the detection. Returning empty descriptors.", 
+			keypoints.size(), descriptors_.rows);
+		return cv::Mat();
+	}
 	return descriptors_;
 }
 

corelib/src/superpoint_rpautrat/SuperpointRpautrat.cpp

@@ -130,7 +130,7 @@ cv::Mat SPDetectorRpautrat::compute(const std::vector<cv::KeyPoint> &keypoints)
 {
 	if(!detected_)
 	{
-		UERROR("SPDetector has been reset before extracting the descriptors! detect() should be called before compute().");
+		UERROR("SPDetectorRpautrat has been reset before extracting the descriptors! detect() should be called before compute().");
 		return cv::Mat();
 	}
     if(keypoints.empty())

corelib/src/superpoint_torch/SuperPoint.cc

@@ -144,7 +144,12 @@ std::vector<cv::KeyPoint> SPDetector::detect(const cv::Mat &img, const cv::Mat &
 	UASSERT(img.type() == CV_8UC1);
 	UASSERT(mask.empty() || (mask.type() == CV_8UC1 && img.cols == mask.cols && img.rows == mask.rows));
 	detected_ = false;
-	if(model_)
+	if(!model_)
+	{
+		UERROR("No model is loaded!");
+		return std::vector<cv::KeyPoint>();
+	}
+	try
 	{
 		torch::NoGradGuard no_grad_guard;
 		auto x = torch::from_blob(img.data, {1, 1, img.rows, img.cols}, torch::kByte);
@@ -199,9 +204,9 @@ std::vector<cv::KeyPoint> SPDetector::detect(const cv::Mat &img, const cv::Mat &
 		detected_ = true;
 		return keypoints;
 	}
-	else
+	catch(const std::exception & e)
 	{
-		UERROR("No model is loaded!");
+		UERROR("SPDetector::detect() threw: %s", e.what());
 		return std::vector<cv::KeyPoint>();
 	}
 }

docker/resolute/Dockerfile

@@ -0,0 +1,20 @@
+# Image: introlab3it/rtabmap:resolute
+
+FROM introlab3it/rtabmap:resolute-deps
+
+# Will be used to read/store databases on host
+RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map
+
+# Copy current source code
+COPY . /root/rtabmap
+
+# Build RTAB-Map project
+RUN source /ros_entrypoint.sh && \
+    cd rtabmap/build && \
+    cmake -DWITH_OPENGV=ON .. && \
+    make -j4 && \
+    make install && \
+    cd ../.. && \
+    rm -rf rtabmap && \
+    ldconfig
+

docker/resolute/deps/Dockerfile

@@ -0,0 +1,121 @@
+
+# Image: introlab3it/rtabmap:resolute-deps
+
+FROM ubuntu:26.04
+
+ARG TARGETPLATFORM
+ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64}
+RUN echo "I am building for $TARGETPLATFORM"
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+# Install ROS2
+RUN apt update && \
+    apt install software-properties-common -y && \
+    add-apt-repository universe && \
+    apt update && \
+    apt install curl -y && \
+    curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \
+    echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+# Install build dependencies
+RUN apt-get update && \
+    apt upgrade -y && \
+    apt-get install -y \
+      git \
+      wget \
+      libtbb-dev \
+      libproj-dev \
+      libpcl-dev \
+      liboctomap-dev \
+      libfreenect-dev \
+      libceres-dev \
+      ros-lyrical-ros-base \
+      ros-dev-tools \
+      ros-lyrical-cv-bridge \
+      ros-lyrical-image-geometry \
+      ros-lyrical-laser-geometry \
+      ros-lyrical-pcl-conversions \
+      ros-lyrical-rviz-common \
+      ros-lyrical-rviz-rendering \
+      ros-lyrical-rviz-default-plugins \
+      ros-lyrical-pcl-ros \
+      ros-lyrical-imu-filter-madgwick \
+      ros-lyrical-image-transport \
+      ros-lyrical-octomap-msgs \
+      ros-lyrical-libg2o \
+      ros-lyrical-gtsam \
+      ros-lyrical-qt-gui-cpp \
+      ros-lyrical-diagnostic-updater && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+WORKDIR /root/
+
+# libfreenect2 
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2..." && \
+    apt-get update && apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \
+    apt-get clean && rm -rf /var/lib/apt/lists/ && \
+    git clone https://github.com/OpenKinect/libfreenect2 && \
+    cd libfreenect2 && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_POLICY_VERSION_MINIMUM=3.5 .. && \
+    make -j4 && \
+    make install && \
+    cd && \
+    rm -r libfreenect2; fi
+
+# zed open capture
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \
+    apt-get update && apt install -y libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \
+    apt-get clean && rm -rf /var/lib/apt/lists/ && \
+    git clone https://github.com/stereolabs/zed-open-capture.git && \
+    cd zed-open-capture && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_POLICY_VERSION_MINIMUM=3.5 .. && \
+    make -j4 && \
+    make install && \
+    cd && \
+    rm -r zed-open-capture; fi
+
+# OpenCV with all modules (same version than distro version to avoid conflicts with cv_bridge ros package)
+RUN git clone --branch 4.10.0 https://github.com/opencv/opencv.git && \
+    git clone --branch 4.10.0 https://github.com/opencv/opencv_contrib.git && \
+    cd opencv && \
+    # FFmpeg 7/8 compatibility (Ubuntu 26.04): avcodec_close / av_stream_get_side_data removed
+    git -c user.email=docker@build -c user.name=docker cherry-pick -x 90c444abd387ffa70b2e72a34922903a2f0f4f5a 443d0ae63fad6dfd8c485d609203db16c8bd0ec3 && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release -DWITH_TBB=ON -DWITH_ADE=OFF -DWITH_OPENMP=ON -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \
+    make -j4 && \
+    make install && \
+    cd ../.. && \
+    rm -rf opencv opencv_contrib
+
+RUN git clone https://github.com/laurentkneip/opengv.git && \
+    cd opengv && \
+    git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 && \
+    wget https://gist.githubusercontent.com/matlabbe/a412cf7c4627253874f81a00745a7fbb/raw/accc3acf465d1ffd0304a46b17741f62d4d354ef/opengv_disable_march_native.patch && \
+    git apply opengv_disable_march_native.patch && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF -DCMAKE_POLICY_VERSION_MINIMUM=3.5 .. && \
+    make -j4 && \
+    make install && \
+    cd && \
+    rm -r opengv
+
+RUN rm /bin/sh && ln -s /bin/bash /bin/sh
+
+RUN echo -e '#!/bin/bash\nset -e\n\n# setup ros2 environment\nsource "/opt/ros/lyrical/setup.bash" --\nexec "$@"' > /ros_entrypoint.sh
+RUN chmod +x /ros_entrypoint.sh
+ENTRYPOINT [ "/ros_entrypoint.sh" ]
+
+# ros2 seems not sourcing by default its multi-arch folders
+ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/lyrical/lib/x86_64-linux-gnu:/opt/ros/lyrical/lib/aarch64-linux-gnu
+
+# for jetson (https://github.com/introlab/rtabmap/issues/776)
+ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra
+

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -372,6 +372,7 @@ private Q_SLOTS:
 	void changeOdometryORBSLAMVocabulary();
 	void changeOdometryOKVISConfigPath();
 	void changeOdometryVINSFusionConfigPath();
+	void changeOdometryOpenVINSConfigPath();
 	void changeOdometryLIOSAMConfigPath();
 	void changeOdometryOpenVINSLeftMask();
 	void changeOdometryOpenVINSRightMask();

guilib/src/PreferencesDialog.cpp

@@ -1617,6 +1617,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->toolButton_OdomVinsFusionPath, SIGNAL(clicked()), this, SLOT(changeOdometryVINSFusionConfigPath()));
 
 	// Odometry OpenVINS
+	_ui->lineEdit_openvinsConfigPath->setObjectName(Parameters::kOdomOpenVINSConfigPath().c_str());
+	connect(_ui->toolButton_openvinsConfigPath, SIGNAL(clicked()), this, SLOT(changeOdometryOpenVINSConfigPath()));
 	_ui->checkBox_OdomOpenVINSUseStereo->setObjectName(Parameters::kOdomOpenVINSUseStereo().c_str());
 	_ui->checkBox_OdomOpenVINSUseKLT->setObjectName(Parameters::kOdomOpenVINSUseKLT().c_str());
 	_ui->spinBox_OdomOpenVINSNumPts->setObjectName(Parameters::kOdomOpenVINSNumPts().c_str());
@@ -5792,6 +5794,23 @@ void PreferencesDialog::changeOdometryVINSFusionConfigPath()
 	}
 }
 
+void PreferencesDialog::changeOdometryOpenVINSConfigPath()
+{
+	QString path;
+	if(_ui->lineEdit_openvinsConfigPath->text().isEmpty())
+	{
+		path = QFileDialog::getOpenFileName(this, tr("OpenVINS Config"), this->getWorkingDirectory(), tr("OpenVINS config (*.yaml)"));
+	}
+	else
+	{
+		path = QFileDialog::getOpenFileName(this, tr("OpenVINS Config"), _ui->lineEdit_openvinsConfigPath->text(), tr("OpenVINS config (*.yaml)"));
+	}
+	if(!path.isEmpty())
+	{
+		_ui->lineEdit_openvinsConfigPath->setText(path);
+	}
+}
+
 void PreferencesDialog::changeOdometryLIOSAMConfigPath()
 {
 	QString path;

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-648</y>
             <width>684</width>
             <height>5218</height>
            </rect>
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>18</number>
+                <number>19</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -16763,7 +16763,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_odometryType">
                       <property name="currentIndex">
-                       <number>1</number>
+                       <number>10</number>
                       </property>
                       <widget class="QWidget" name="page_52">
                        <layout class="QVBoxLayout" name="verticalLayout_77">
@@ -20388,7 +20388,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                           <property name="title">
                            <string>OpenVINS</string>
                           </property>
-                          <layout class="QVBoxLayout" name="verticalLayout_161" stretch="0,0,0,0,0,0">
+                          <layout class="QVBoxLayout" name="verticalLayout_161">
                            <item>
                             <widget class="QLabel" name="label_632">
                              <property name="text">
@@ -20405,6 +20405,33 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                              </property>
                             </widget>
                            </item>
+                           <item>
+                            <layout class="QHBoxLayout" name="horizontalLayout_19" stretch="0,0,1">
+                             <item>
+                              <widget class="QToolButton" name="toolButton_openvinsConfigPath">
+                               <property name="text">
+                                <string>...</string>
+                               </property>
+                              </widget>
+                             </item>
+                             <item>
+                              <widget class="QLineEdit" name="lineEdit_openvinsConfigPath"/>
+                             </item>
+                             <item>
+                              <widget class="QLabel" name="label_796">
+                               <property name="text">
+                                <string>Configuration file (*.yaml). Same format used than OpenVINS library. Note that any parameter from that config file will overwrite the same parameter below.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                            </layout>
+                           </item>
                            <item>
                             <widget class="QGroupBox" name="groupBox_36">
                              <property name="title">
@@ -20421,7 +20448,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="0" column="1">
                                <widget class="QLabel" name="label_682">
                                 <property name="text">
-                                 <string>If we should process two cameras are being stereo or binocular. If binocular, we do monocular feature tracking on each image.</string>
+                                 <string>Stereo mode. If we should process two cameras are being stereo or binocular. If binocular, we do monocular feature tracking on each image. Ignored if provided input data is not stereo.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20441,7 +20468,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="1" column="1">
                                <widget class="QLabel" name="label_683">
                                 <property name="text">
-                                 <string>If we should use KLT tracking, or descriptor matcher</string>
+                                 <string>KLT tracking. Uncheck to use descriptor matcher.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20467,7 +20494,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="2" column="1">
                                <widget class="QLabel" name="label_684">
                                 <property name="text">
-                                 <string>Number of points (per camera) we will extract and try to track</string>
+                                 <string>Number of points (per camera) we will extract and try to track.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20490,7 +20517,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="3" column="1">
                                <widget class="QLabel" name="label_688">
                                 <property name="text">
-                                 <string>Will check after doing KLT track and remove any features closer than this</string>
+                                 <string>Minimum pixel distance. Will check after doing KLT track and remove any features closer than this.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20510,7 +20537,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="4" column="1">
                                <widget class="QLabel" name="label_689">
                                 <property name="text">
-                                 <string>If we should perform 1d triangulation instead of 3d</string>
+                                 <string>If we should perform 1d triangulation instead of 3d.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20530,7 +20557,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="5" column="1">
                                <widget class="QLabel" name="label_732">
                                 <property name="text">
-                                 <string>If we should perform Levenberg-Marquardt refinement</string>
+                                 <string>If we should perform Levenberg-Marquardt refinement.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20553,7 +20580,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="6" column="1">
                                <widget class="QLabel" name="label_733">
                                 <property name="text">
-                                 <string>Max runs for Levenberg-Marquardt</string>
+                                 <string>Max runs for Levenberg-Marquardt.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20579,7 +20606,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="7" column="1">
                                <widget class="QLabel" name="label_734">
                                 <property name="text">
-                                 <string>Max baseline ratio to accept triangulated features</string>
+                                 <string>Max baseline ratio to accept triangulated features.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20605,7 +20632,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="8" column="1">
                                <widget class="QLabel" name="label_735">
                                 <property name="text">
-                                 <string>Max condition number of linear triangulation matrix accept triangulated features</string>
+                                 <string>Max condition number of linear triangulation matrix accept triangulated features.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20634,7 +20661,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="0" column="1">
                                <widget class="QLabel" name="label_690">
                                 <property name="text">
-                                 <string>If first-estimate Jacobians should be used (enable for good consistency)</string>
+                                 <string>If first-estimate Jacobians should be used (enable for good consistency).</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20672,7 +20699,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="1" column="1">
                                <widget class="QLabel" name="label_691">
                                 <property name="text">
-                                 <string>Numerical integration methods</string>
+                                 <string>Numerical integration methods.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20692,7 +20719,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="2" column="1">
                                <widget class="QLabel" name="label_685">
                                 <property name="text">
-                                 <string>If the transform between camera and IMU should be optimized (R_ItoC, p_CinI)</string>
+                                 <string>If the transform between camera and IMU should be optimized (R_ItoC, p_CinI).</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20712,7 +20739,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="3" column="1">
                                <widget class="QLabel" name="label_686">
                                 <property name="text">
-                                 <string>If camera intrinsics should be optimized (focal, center, distortion)</string>
+                                 <string>If camera intrinsics should be optimized (focal, center, distortion).</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20732,7 +20759,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="4" column="1">
                                <widget class="QLabel" name="label_687">
                                 <property name="text">
-                                 <string>If timeoffset between camera and IMU should be optimized</string>
+                                 <string>If timeoffset between camera and IMU should be optimized.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20752,7 +20779,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="5" column="1">
                                <widget class="QLabel" name="label_718">
                                 <property name="text">
-                                 <string>If imu intrinsics should be calibrated (rotation and skew-scale matrix)</string>
+                                 <string>If imu intrinsics should be calibrated (rotation and skew-scale matrix).</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20772,7 +20799,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="6" column="1">
                                <widget class="QLabel" name="label_719">
                                 <property name="text">
-                                 <string>If gyroscope gravity sensitivity (Tg) should be calibrated</string>
+                                 <string>If gyroscope gravity sensitivity (Tg) should be calibrated.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20795,7 +20822,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="7" column="1">
                                <widget class="QLabel" name="label_692">
                                 <property name="text">
-                                 <string>Max clone size of sliding window</string>
+                                 <string>Max clone size of sliding window.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20821,7 +20848,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="8" column="1">
                                <widget class="QLabel" name="label_693">
                                 <property name="text">
-                                 <string>Max number of estimated SLAM features</string>
+                                 <string>Max number of estimated SLAM features.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20926,7 +20953,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="11" column="1">
                                <widget class="QLabel" name="label_696">
                                 <property name="text">
-                                 <string>What representation our features are in (msckf features)</string>
+                                 <string>What representation our features are in (msckf features).</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20942,7 +20969,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                  <number>4</number>
                                 </property>
                                 <property name="sizeAdjustPolicy">
-                                 <enum>QComboBox::AdjustToContents</enum>
+                                 <enum>QComboBox::AdjustToContentsOnFirstShow</enum>
                                 </property>
                                 <item>
                                  <property name="text">
@@ -20979,7 +21006,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="12" column="1">
                                <widget class="QLabel" name="label_697">
                                 <property name="text">
-                                 <string>What representation our features are in (slam features)</string>
+                                 <string>What representation our features are in (slam features).</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21008,7 +21035,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="13" column="1">
                                <widget class="QLabel" name="label_698">
                                 <property name="text">
-                                 <string>Delay before initializing (helps with stability from bad initialization...)</string>
+                                 <string>Delay before initializing (helps with stability from bad initialization...).</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21034,7 +21061,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="14" column="1">
                                <widget class="QLabel" name="label_699">
                                 <property name="text">
-                                 <string>Magnitude of gravity in this location</string>
+                                 <string>Magnitude of gravity in this location.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21061,7 +21088,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="15" column="1">
                                <widget class="QLabel" name="label_736">
                                 <property name="text">
-                                 <string>Mask for left image</string>
+                                 <string>Mask for left image (stereo mode) or mono image (RGB-D mode). For RGB-D mode, to use depth as mask, enable that option under Visual Registration panel.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21088,7 +21115,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="16" column="1">
                                <widget class="QLabel" name="label_737">
                                 <property name="text">
-                                 <string>Mask for right image</string>
+                                 <string>Mask for right image.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21126,7 +21153,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="0" column="1">
                                <widget class="QLabel" name="label_700">
                                 <property name="text">
-                                 <string>Amount of time we will initialize over</string>
+                                 <string>Amount of time we will initialize over.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21155,7 +21182,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="1" column="1">
                                <widget class="QLabel" name="label_701">
                                 <property name="text">
-                                 <string>Variance threshold on our acceleration to be classified as moving</string>
+                                 <string>Variance threshold on our acceleration to be classified as moving.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21181,7 +21208,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="2" column="1">
                                <widget class="QLabel" name="label_702">
                                 <property name="text">
-                                 <string>Max disparity to consider the platform stationary (dependent on resolution)</string>
+                                 <string>Max disparity to consider the platform stationary (dependent on resolution).</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21207,7 +21234,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="3" column="1">
                                <widget class="QLabel" name="label_703">
                                 <property name="text">
-                                 <string>How many features to track during initialization (saves on computation)</string>
+                                 <string>How many features to track during initialization (saves on computation).</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21227,7 +21254,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="4" column="1">
                                <widget class="QLabel" name="label_720">
                                 <property name="text">
-                                 <string>If we should perform dynamic initialization</string>
+                                 <string>If we should perform dynamic initialization.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21247,7 +21274,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="5" column="1">
                                <widget class="QLabel" name="label_721">
                                 <property name="text">
-                                 <string>If we should optimize and recover the calibration in our MLE</string>
+                                 <string>If we should optimize and recover the calibration in our MLE.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21270,7 +21297,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="6" column="1">
                                <widget class="QLabel" name="label_722">
                                 <property name="text">
-                                 <string>Max number of MLE iterations for dynamic initialization</string>
+                                 <string>Max number of MLE iterations for dynamic initialization.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21299,7 +21326,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="7" column="1">
                                <widget class="QLabel" name="label_723">
                                 <property name="text">
-                                 <string>Max time for MLE optimization</string>
+                                 <string>Max time for MLE optimization.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21322,7 +21349,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="8" column="1">
                                <widget class="QLabel" name="label_724">
                                 <property name="text">
-                                 <string>Max number of MLE threads for dynamic initialization</string>
+                                 <string>Max number of MLE threads for dynamic initialization.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21345,7 +21372,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="9" column="1">
                                <widget class="QLabel" name="label_725">
                                 <property name="text">
-                                 <string>Number of poses to use during initialization (max should be cam freq * window)</string>
+                                 <string>Number of poses to use during initialization (max should be cam freq * window).</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21374,7 +21401,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="10" column="1">
                                <widget class="QLabel" name="label_726">
                                 <property name="text">
-                                 <string>Minimum degrees we need to rotate before we try to init (sum of norm)</string>
+                                 <string>Minimum degrees we need to rotate before we try to init (sum of norm).</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21400,7 +21427,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="11" column="1">
                                <widget class="QLabel" name="label_727">
                                 <property name="text">
-                                 <string>Magnitude we will inflate initial covariance of orientation</string>
+                                 <string>Magnitude we will inflate initial covariance of orientation.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21426,7 +21453,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="12" column="1">
                                <widget class="QLabel" name="label_728">
                                 <property name="text">
-                                 <string>Magnitude we will inflate initial covariance of velocity</string>
+                                 <string>Magnitude we will inflate initial covariance of velocity.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21452,7 +21479,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="13" column="1">
                                <widget class="QLabel" name="label_729">
                                 <property name="text">
-                                 <string>Magnitude we will inflate initial covariance of gyroscope bias</string>
+                                 <string>Magnitude we will inflate initial covariance of gyroscope bias.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21478,7 +21505,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="14" column="1">
                                <widget class="QLabel" name="label_730">
                                 <property name="text">
-                                 <string>Magnitude we will inflate initial covariance of accelerometer bias</string>
+                                 <string>Magnitude we will inflate initial covariance of accelerometer bias.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21507,7 +21534,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="15" column="1">
                                <widget class="QLabel" name="label_731">
                                 <property name="text">
-                                 <string>Minimum reciprocal condition number acceptable for our covariance recovery</string>
+                                 <string>Minimum reciprocal condition number acceptable for our covariance recovery.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21536,7 +21563,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="0" column="1">
                                <widget class="QLabel" name="label_704">
                                 <property name="text">
-                                 <string>If we should try to use zero velocity update</string>
+                                 <string>If we should try to use zero velocity update.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21565,7 +21592,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="1" column="1">
                                <widget class="QLabel" name="label_705">
                                 <property name="text">
-                                 <string>Chi2 multiplier for zero velocity</string>
+                                 <string>Chi2 multiplier for zero velocity.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21594,7 +21621,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="2" column="1">
                                <widget class="QLabel" name="label_706">
                                 <property name="text">
-                                 <string>Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt)</string>
+                                 <string>Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt).</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21623,7 +21650,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="3" column="1">
                                <widget class="QLabel" name="label_707">
                                 <property name="text">
-                                 <string>Multiplier of our zupt measurement IMU noise matrix (default should be 1.0)</string>
+                                 <string>Multiplier of our zupt measurement IMU noise matrix (default should be 1.0).</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21652,7 +21679,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="4" column="1">
                                <widget class="QLabel" name="label_708">
                                 <property name="text">
-                                 <string>Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt)</string>
+                                 <string>Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt).</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21672,7 +21699,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="5" column="1">
                                <widget class="QLabel" name="label_709">
                                 <property name="text">
-                                 <string>If we should only use the zupt at the very beginning static initialization phase</string>
+                                 <string>If we should only use the zupt at the very beginning static initialization phase.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21713,7 +21740,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="0" column="1">
                                <widget class="QLabel" name="label_710">
                                 <property name="text">
-                                 <string>Accel &quot;white noise&quot;</string>
+                                 <string>Accel &quot;white noise&quot;.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21745,7 +21772,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="1" column="1">
                                <widget class="QLabel" name="label_711">
                                 <property name="text">
-                                 <string>Accel bias diffusion</string>
+                                 <string>Accel bias diffusion.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21777,7 +21804,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="2" column="1">
                                <widget class="QLabel" name="label_712">
                                 <property name="text">
-                                 <string>Gyro &quot;white noise&quot;</string>
+                                 <string>Gyro &quot;white noise&quot;.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21809,7 +21836,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="3" column="1">
                                <widget class="QLabel" name="label_713">
                                 <property name="text">
-                                 <string>Gyro bias diffusion</string>
+                                 <string>Gyro bias diffusion.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21838,7 +21865,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="4" column="1">
                                <widget class="QLabel" name="label_714">
                                 <property name="text">
-                                 <string>Pixel noise for MSCKF features</string>
+                                 <string>Pixel noise for MSCKF features.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21867,7 +21894,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="5" column="1">
                                <widget class="QLabel" name="label_715">
                                 <property name="text">
-                                 <string>Chi2 multiplier for MSCKF features</string>
+                                 <string>Chi2 multiplier for MSCKF features.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21896,7 +21923,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="6" column="1">
                                <widget class="QLabel" name="label_716">
                                 <property name="text">
-                                 <string>Pixel noise for SLAM features</string>
+                                 <string>Pixel noise for SLAM features.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21925,7 +21952,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                               <item row="7" column="1">
                                <widget class="QLabel" name="label_717">
                                 <property name="text">
-                                 <string>Chi2 multiplier for SLAM features</string>
+                                 <string>Chi2 multiplier for SLAM features.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>rtabmap</name>
-  <version>0.23.5</version>
+  <version>0.23.7</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -16,6 +16,7 @@
   <depend>cv_bridge</depend>
   <depend>libg2o</depend>
   <depend>gtsam</depend>
+  <depend>tbb</depend> <!-- till tbb is added to gtsam dependencies https://github.com/borglab/gtsam/issues/1759 -->
   <!-- <depend>libopenni2-dev</depend> --> <!-- not available on Jessie -->
   <depend>libpcl-all-dev</depend> <!--  include libvtk-qt -->
   <depend>libpointmatcher</depend> <!-- optional but recommended if lidar is used, also not available on 32 bits system, but rtabmap can be built without it -->

tools/Reprocess/main.cpp

@@ -82,6 +82,7 @@ void showUsage()
 			"     -stop #     Last node to process.\n"
 			"     -start_s #  Start from this map session ID.\n"
 			"     -stop_s #   Last map session to process.\n"
+			"     -stop_loop  Stop after the first loop closure is detected.\n"
 			"     -a          Append mode: if Mem/IncrementalMemory is true, RTAB-Map is initialized with the first input database,\n"
 			"                 then next databases are reprocessed on top of the first one.\n"
 			"     -cam #      Camera index to stream. Ignored if a database doesn't contain multi-camera data. Can also be multiple \n"
@@ -271,6 +272,7 @@ int main(int argc, char * argv[])
 	int stopId = 0;
 	int startMapId = 0;
 	int stopMapId = -1;
+	bool stopOnLoopClosure = false;
 	bool appendMode = false;
 	std::vector<unsigned int> cameraIndices;
 	std::vector<Transform> cameraLocalTransformOverrides;
@@ -419,6 +421,10 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(strcmp(argv[i], "-stop_loop") == 0 || strcmp(argv[i], "--stop_loop") == 0)
+		{
+			stopOnLoopClosure = true;
+		}
 		else if (strcmp(argv[i], "-a") == 0 || strcmp(argv[i], "--a") == 0)
 		{
 			appendMode = true;
@@ -1288,6 +1294,11 @@ int main(int argc, char * argv[])
 				++loopIntra;
 			}
 			printf("[%f] Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms %s on %d [%d]\n", data.stamp(), ++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);
+			if(stopOnLoopClosure)
+			{
+				printf("First loop closure has been detected and --stop_loop option is enabled, stop processing...\n");
+				break;
+			}
 		}
 		else if(landmarkId != 0)
 		{