refactor, debugging code

.devcontainer/resolute/Dockerfile

@@ -1,25 +0,0 @@
-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

@@ -1,30 +0,0 @@
-{
-    "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, ubuntu-26.04]
+        build_name: [ubuntu-22.04, ubuntu-24.04, ubuntu-24.04-with-opengv]
         include:
         - build_name: ubuntu-22.04
           os: ubuntu-22.04
@@ -36,10 +36,6 @@ 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: 
-      - humble-devel
+      - master
   pull_request:
     branches: 
       - '**'
@@ -22,25 +22,62 @@ 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 }}
+    name: ${{ matrix.ros_distribution }}-${{ matrix.os }}
-    runs-on: ubuntu-latest
+    runs-on: ${{ matrix.os }}
     strategy:
       fail-fast: false
       matrix:
-        ros_distribution: [ humble ]
+        ros_distribution: [ humble, jazzy, kilted, rolling ]
         include:
         - ros_distribution: 'humble'
-          skip_keys: ""
+          os: ubuntu-22.04
-    container:
+        - ros_distribution: 'jazzy'
-      image: osrf/ros:${{ matrix.ros_distribution }}-desktop-full
+          os: ubuntu-24.04
+        - ros_distribution: 'kilted'
+          os: ubuntu-24.04
+        - ros_distribution: 'rolling'
+          os: ubuntu-24.04
+
     steps:   
-    - uses: actions/checkout@v4
+    - name: Setup ROS2
+      # 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
+    
-      with:
+    - uses: actions/checkout@v4
-        package-name: rtabmap
+
-        target-ros2-distro: ${{ matrix.ros_distribution }}
+    - name: Point rosdep to latest Rolling on Noble until github runners support ubuntu 26.04
-        rosdep-skip-keys: "${{ matrix.skip_keys }}"
+      if: matrix.ros_distribution == 'rolling'
+      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,13 +4,6 @@ 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:
@@ -22,15 +15,14 @@ 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
-    # Skipped on pull requests; built and pushed only on push to master.
+    # if: false
-    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, resolute-deps]
+        docker_tag: [focal-deps, jammy-deps, noble-deps, noble-kilted-deps]
         include:
         - docker_tag: focal-deps
           docker_tags: |
@@ -60,13 +52,6 @@ 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:
       -
@@ -100,14 +85,12 @@ 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, resolute, android23, android24, android26, android30]
+        docker_tag: [bionic, focal, jammy, noble, noble-kilted, android23, android24, android26, android30]
         include:
         - docker_tag: bionic
           docker_tags: |
@@ -159,16 +142,6 @@ 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
@@ -217,9 +190,6 @@ 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 }}
@@ -229,8 +199,8 @@ jobs:
         uses: docker/build-push-action@v6
         with:
           context: .
-          push: ${{ github.event_name != 'pull_request' }}
+          push: true
-          platforms: ${{ github.event_name == 'pull_request' && 'linux/amd64' || matrix.docker_platforms }}
+          platforms: ${{ matrix.docker_platforms }}
           file: ./docker/${{ matrix.docker_path }}/Dockerfile
           build-args: |
             ${{ matrix.docker_args }}

.github/workflows/ios.yml

@@ -1,114 +0,0 @@
-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,7 +842,22 @@ IF(WITH_VINS_FUSION)
 ENDIF(WITH_VINS_FUSION)
 
 IF(WITH_OPENVINS)
-   FIND_PACKAGE(OpenVINS)
+    FIND_PACKAGE(ov_msckf)
+    # 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)
@@ -901,9 +916,6 @@ 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)
@@ -1211,7 +1223,7 @@ ENDIF()
 IF(NOT vins_FOUND)
    SET(VINSFUSION "//")
 ENDIF()
-IF(NOT OpenVINS_FOUND)
+IF(NOT ov_msckf_FOUND)
    SET(OPENVINS "//")
 ENDIF()
 IF(NOT CUVSLAM_FOUND)
@@ -1969,7 +1981,7 @@ ELSE()
 MESSAGE(STATUS "  With VINS-Fusion          = NO (VINS-Fusion not found)")
 ENDIF()
 
-IF(OpenVINS_FOUND)
+IF(ov_msckf_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.23.7;
+				MARKETING_VERSION = 0.22.0;
 				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.23.7;
+				MARKETING_VERSION = 0.22.0;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;

app/ios/RTABMapApp/install_deps.sh

@@ -175,18 +175,6 @@ 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

@@ -1,44 +0,0 @@
-# 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,67 +620,66 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM_STR(OdomVINSFusion, ConfigPath,     "",  "Path of VINS-Fusion config file.");
 
     // Odometry OpenVINS
-    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, UseStereo,                 bool,   true,   "If we have more than 1 camera, if we should try to track stereo constraints between pairs");
-    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, UseKLT,                    bool,   true,   "If true we will use KLT, otherwise use a ORB descriptor + robust matching");
-    RTABMAP_PARAM(OdomOpenVINS, UseKLT,                    bool,   true,   "If true we will use KLT, otherwise use a ORB descriptor + robust matching.");
+    RTABMAP_PARAM(OdomOpenVINS, NumPts,                    int,    200,    "Number of points (per camera) we will extract and try to track");
-    RTABMAP_PARAM(OdomOpenVINS, NumPts,                    int,    200,    "Number of points (per camera) we will extract and try to track.");
+    RTABMAP_PARAM(OdomOpenVINS, MinPxDist,                 int,    15,     "Eistance between features (features near each other provide less information)");
-    RTABMAP_PARAM(OdomOpenVINS, MinPxDist,                 int,    15,     "Eistance between features (features near each other provide less information).");
+    RTABMAP_PARAM(OdomOpenVINS, FiTriangulate1d,           bool,   false,  "If we should perform 1d triangulation instead of 3d");
-    RTABMAP_PARAM(OdomOpenVINS, FiTriangulate1d,           bool,   false,  "If we should perform 1d triangulation instead of 3d.");
+    RTABMAP_PARAM(OdomOpenVINS, FiRefineFeatures,          bool,   true,   "If we should perform Levenberg-Marquardt refinement");
-    RTABMAP_PARAM(OdomOpenVINS, FiRefineFeatures,          bool,   true,   "If we should perform Levenberg-Marquardt refinement.");
+    RTABMAP_PARAM(OdomOpenVINS, FiMaxRuns,                 int,    5,      "Max runs for Levenberg-Marquardt");
-    RTABMAP_PARAM(OdomOpenVINS, FiMaxRuns,                 int,    5,      "Max runs for Levenberg-Marquardt.");
+    RTABMAP_PARAM(OdomOpenVINS, FiMaxBaseline,             double, 40,     "Max baseline ratio to 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, 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/include/rtabmap/core/Statistics.h

@@ -83,6 +83,7 @@ class RTABMAP_CORE_EXPORT Statistics
 	RTABMAP_STATS(Loop, Optimization_max_error_removed_from_id, );
 	RTABMAP_STATS(Loop, Optimization_max_error_removed_to_id, );
 	RTABMAP_STATS(Loop, Optimization_max_error_removed_count, );
+	RTABMAP_STATS(Loop, Optimization_factors, );
 	RTABMAP_STATS(Loop, Linear_variance,);
 	RTABMAP_STATS(Loop, Angular_variance,);
 	RTABMAP_STATS(Loop, Landmark_detected,);

corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define OPTIMIZERGTSAM_H_
 
 #include <rtabmap/core/Optimizer.h>
+#include <tuple>
 
 namespace gtsam {
 	class ISAM2;
@@ -58,26 +59,25 @@ public:
 			double * finalError = 0,
 			int * iterationsDone = 0);
 
+	// True when iSAM2 (incremental) backend is active.
+	bool isIncremental() const {return isam2_ != 0;}
+
+	// Number of factors we believe are live in iSAM2.
+	std::size_t getTrackedFactorsCount() const {
+		return trackedFactors_.size() + (lastRootFactorIndex_.first != 0 ? 1 : 0);
+	}
+
+	// Live factor count reported by iSAM2 itself (should match getTrackedFactorsCount()).
+	std::size_t getISAM2LiveFactorsCount() const;
+
 private:
 	int internalOptimizerType_;
 
 	gtsam::ISAM2 * isam2_;
-	struct ConstraintToFactor {
-		ConstraintToFactor(int _from, int _to, std::uint64_t _factorIndice)
-		{
-			from = _from;
-			to = _to;
-			factorIndice = _factorIndice;
-		}
-		int from;
-		int to;
-		std::uint64_t factorIndice;
-	};
-
-	std::vector<ConstraintToFactor> lastAddedConstraints_;
 	int lastSwitchId_;
 	std::set<int> addedPoses_;
 	std::map<int, bool> isLandmarkWithRotation_; // persists across iSAM2 incremental calls
+	std::map<std::tuple<int, int, int>, std::uint64_t> trackedFactors_; // iSAM2 tracked constraints
 	std::pair<int, std::uint64_t> lastRootFactorIndex_;
 };
 

corelib/src/CMakeLists.txt

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

corelib/src/Features2d.cpp

@@ -887,17 +887,8 @@ cv::Mat Feature2D::generateDescriptors(
 		UASSERT(!image.empty());
 		UASSERT(image.type() == CV_8UC1);
 		descriptors = generateDescriptorsImpl(image, keypoints);
-		if(descriptors.rows != (int)keypoints.size())
+		UASSERT_MSG(descriptors.rows == (int)keypoints.size(), uFormat("descriptors=%d, keypoints=%d", descriptors.rows, (int)keypoints.size()).c_str());
-		{
+		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;
 }
@@ -2639,31 +2630,7 @@ cv::Mat SuperPointTorch::generateDescriptorsImpl(const cv::Mat & image, std::vec
 {
 #ifdef RTABMAP_TORCH
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
-	cv::Mat descriptors;
+	return superPoint_->compute(keypoints);
-	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();
@@ -2765,31 +2732,7 @@ 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);
-	cv::Mat descriptors;
+	return superPoint_->compute(keypoints);
-	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())
 			{
-				UDEBUG("Marker %d already added by another camera, ignoring detection from camera %d", det->id, cameraIndex);
+				UWARN("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())
 			{
-				UDEBUG("Marker %d already added by another camera, ignoring detection from camera %d", id, cameraIndex);
+				UWARN("Marker %d already added by another camera, ignoring detection from camera %d", id, cameraIndex);
 				continue;
 			}
 

corelib/src/Memory.cpp

@@ -5205,7 +5205,6 @@ 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)
 	{
@@ -5475,90 +5474,41 @@ 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);
 
-			if(!imagesRectified && decimatedData.cameraModels().size())
+			UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
+			if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
 			{
-				UASSERT_MSG((int)keypoints.size() == descriptors.rows, uFormat("%d vs %d", (int)keypoints.size(), descriptors.rows).c_str());
+				descriptors = cv::Mat();
-				std::vector<cv::KeyPoint> keypointsValid;
+			}
-				keypointsValid.reserve(keypoints.size());
+			else
-				cv::Mat descriptorsValid;
+			{
-				descriptorsValid.reserve(descriptors.rows);
+				if(!imagesRectified && decimatedData.cameraModels().size())
-
-				//undistort keypoints before projection (RGB-D)
-				if(decimatedData.cameraModels().size() == 1)
 				{
-					std::vector<cv::Point2f> pointsIn, pointsOut;
+					UASSERT_MSG((int)keypoints.size() == descriptors.rows, uFormat("%d vs %d", (int)keypoints.size(), descriptors.rows).c_str());
-					cv::KeyPoint::convert(keypoints,pointsIn);
+					std::vector<cv::KeyPoint> keypointsValid;
-					if(decimatedData.cameraModels()[0].D_raw().cols == 6)
+					keypointsValid.reserve(keypoints.size());
-					{
+					cv::Mat descriptorsValid;
-#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)))
+					descriptorsValid.reserve(descriptors.rows);
-						// 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());
 
+					//undistort keypoints before projection (RGB-D)
+					if(decimatedData.cameraModels().size() == 1)
+					{
 						std::vector<cv::Point2f> pointsIn, pointsOut;
-						pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
+						cv::KeyPoint::convert(keypoints,pointsIn);
-						if(decimatedData.cameraModels()[cameraIndex].D_raw().cols == 6)
+						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()[cameraIndex].D_raw().at<double>(0,0);
+							D.at<double>(0,0) = decimatedData.cameraModels()[0].D_raw().at<double>(0,0);
-							D.at<double>(0,1) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
+							D.at<double>(0,1) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1);
-							D.at<double>(0,2) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
+							D.at<double>(0,2) = decimatedData.cameraModels()[0].D_raw().at<double>(0,4);
-							D.at<double>(0,3) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
+							D.at<double>(0,3) = decimatedData.cameraModels()[0].D_raw().at<double>(0,5);
 							cv::fisheye::undistortPoints(pointsIn, pointsOut,
-									decimatedData.cameraModels()[cameraIndex].K_raw(),
+									decimatedData.cameraModels()[0].K_raw(),
 									D,
-									decimatedData.cameraModels()[cameraIndex].R(),
+									decimatedData.cameraModels()[0].R(),
-									decimatedData.cameraModels()[cameraIndex].P());
+									decimatedData.cameraModels()[0].P());
 						}
 						else
 #else
@@ -5569,57 +5519,114 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						{
 							//RadialTangential
 							cv::undistortPoints(pointsIn, pointsOut,
-									decimatedData.cameraModels()[cameraIndex].K_raw(),
+									decimatedData.cameraModels()[0].K_raw(),
-									decimatedData.cameraModels()[cameraIndex].D_raw(),
+									decimatedData.cameraModels()[0].D_raw(),
-									decimatedData.cameraModels()[cameraIndex].R(),
+									decimatedData.cameraModels()[0].R(),
-									decimatedData.cameraModels()[cameraIndex].P());
+									decimatedData.cameraModels()[0].P());
 						}
-
+						UASSERT(pointsOut.size() == keypoints.size());
-						if(pointsOut[0].x>=0 && pointsOut[0].x<decimatedData.cameraModels()[cameraIndex].imageWidth() &&
+						for(unsigned int i=0; i<pointsOut.size(); ++i)
-							pointsOut[0].y>=0 && pointsOut[0].y<decimatedData.cameraModels()[cameraIndex].imageHeight())
 						{
-							keypointsValid.push_back(keypoints.at(i));
+							if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<decimatedData.cameraModels()[0].imageWidth() &&
-							keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
+							   pointsOut.at(i).y>=0 && pointsOut.at(i).y<decimatedData.cameraModels()[0].imageHeight())
-							keypointsValid.back().pt.y = pointsOut[0].y;
+							{
-							descriptorsValid.push_back(descriptors.row(i));
+								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;
+							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);
 				}
 
-				keypoints = keypointsValid;
+				if(useProvided3dPoints && keypoints.size() != data.keypoints3D().size())
-				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)
 				{
-					UASSERT(keypoints[i].class_id < (int)data.keypoints3D().size());
+					UDEBUG("Using provided 3d points (%d->%d)", (int)data.keypoints3D().size(), (int)keypoints.size());
-					keypoints3D[i] = data.keypoints3D()[keypoints[i].class_id];
+					keypoints3D.resize(keypoints.size());
+					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())
@@ -5846,6 +5853,12 @@ 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();
+			}
 		}
 	}
 
@@ -5871,9 +5884,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	bool addedToDictionary = false;
 	if(!keypoints.empty())
 	{
-		if(descriptors.rows && 
+		if(descriptors.rows && !isIntermediateNode)
-			!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)
@@ -5980,11 +5991,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		else
 		{
 			// Set all words as not used in dictionary
-			int negIndex = -1;
+			wordIds.resize(keypoints.size(),-1);
-			for(size_t i=0; i<keypoints.size(); ++i)
-			{
-				wordIds.push_back(negIndex--);
-			}
 		}
 
 		t = timer.ticks();

corelib/src/Rtabmap.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Version.h"
 #include "rtabmap/core/Features2d.h"
 #include "rtabmap/core/Optimizer.h"
+#include "rtabmap/core/optimizer/OptimizerGTSAM.h"
 #include "rtabmap/core/Graph.h"
 #include "rtabmap/core/Signature.h"
 
@@ -4231,6 +4232,12 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kLoopLandmark_detected_node_ref(), landmarksDetected.empty() || landmarksDetected.begin()->second.empty()?0:*landmarksDetected.begin()->second.begin());
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers_mean_dist(), loopClosureVisualInliersMeanDist);
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers_distribution(), loopClosureVisualInliersDistribution);
+			OptimizerGTSAM * gtsamOpt = dynamic_cast<OptimizerGTSAM*>(_graphOptimizer);
+			if(gtsamOpt != 0 && gtsamOpt->isIncremental())
+			{
+				statistics_.addStatistic(Statistics::kLoopOptimization_factors(), (float)gtsamOpt->getTrackedFactorsCount());
+				statistics_.addStatistic("Loop/Optimization_internal_factors", (float)gtsamOpt->getISAM2LiveFactorsCount());
+			}
 
 			statistics_.addStatistic(Statistics::kProximityTime_detections(), proximityDetectionsInTimeFound);
 			statistics_.addStatistic(Statistics::kProximitySpace_detections_added_visually(), proximityDetectionsAddedVisually);

corelib/src/odometry/OdometryOpenVINS.cpp

@@ -152,85 +152,6 @@ 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/OptimizerGTSAM.cpp

@@ -90,6 +90,27 @@ bool OptimizerGTSAM::available()
 #endif
 }
 
+std::size_t OptimizerGTSAM::getISAM2LiveFactorsCount() const
+{
+#ifdef RTABMAP_GTSAM
+	if(isam2_ == 0)
+	{
+		return 0;
+	}
+	// iSAM2 keeps removed factors as null entries to preserve factor indices,
+	// so we have to skip nulls to get the actual live count.
+	const gtsam::NonlinearFactorGraph & factors = isam2_->getFactorsUnsafe();
+	std::size_t live = 0;
+	for(const gtsam::NonlinearFactorGraph::sharedFactor & f : factors)
+	{
+		if(f) ++live;
+	}
+	return live;
+#else
+	return 0;
+#endif
+}
+
 void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
 {
 	Optimizer::parseParameters(parameters);
@@ -124,7 +145,7 @@ void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
 		isam2_ = new gtsam::ISAM2(params);
 
 		addedPoses_.clear();
-		lastAddedConstraints_.clear();
+		trackedFactors_.clear();
 		lastRootFactorIndex_.first = 0;
 		lastSwitchId_ = 1000000000;
 	}
@@ -197,7 +218,21 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			}
 		}
 
-		std::vector<ConstraintToFactor> addedPrior;
+		// Ordered list of (from, to, type) entries added to the gtsam graph
+		// during this optimize() call. The order matches the order of factors
+		// pushed into iSAM2, so result.newFactorsIndices[j] gives the factor
+		// index for addedConstraints[j]. Used post-update to populate
+		// trackedFactors_. type holds Link::Type as int; type == -1 marks
+		// the synthetic root prior, which is managed separately via
+		// lastRootFactorIndex_ and stays out of trackedFactors_.
+		struct ConstraintToFactor {
+			ConstraintToFactor(int _from, int _to, int _type = -1) :
+				from(_from), to(_to), type(_type) {}
+			int from;
+			int to;
+			int type;
+		};
+		std::vector<ConstraintToFactor> addedConstraints;
 		gtsam::FactorIndices removeFactorIndices;
 
 		//prior first pose
@@ -211,7 +246,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			{
 				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:1e-9));
 				graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
-				addedPrior.push_back(ConstraintToFactor(rootId, rootId, -1));
+				addedConstraints.push_back(ConstraintToFactor(rootId, rootId));
 			}
 			else
 			{
@@ -221,7 +256,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 								(hasGPSPrior?2:1e-2), hasGPSPrior?2:1e-2, hasGPSPrior?2:1e-2 // xyz
 								).finished());
 				graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
-				addedPrior.push_back(ConstraintToFactor(rootId, rootId, -1));
+				addedConstraints.push_back(ConstraintToFactor(rootId, rootId));
 			}
 			if(isam2_ && lastRootFactorIndex_.first!=0)
 			{
@@ -238,7 +273,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					delete isam2_;
 					isam2_ = new gtsam::ISAM2(params);
 					addedPoses_.clear();
-					lastAddedConstraints_.clear();
+					trackedFactors_.clear();
 					isLandmarkWithRotation_.clear();
 					lastRootFactorIndex_.first = 0;
 					lastSwitchId_ = 1000000000;
@@ -250,6 +285,16 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		std::map<int, Transform> newPoses;
 		std::multimap<int, Link> newEdgeConstraints;
 
+		// trackedFactors_ keys are (min(from,to), max(from,to), type) so that:
+		//   - (A,B) and (B,A) with the same type map to the same entry
+		//     (matches graph::findLink(checkBothWays=true) and the
+		//     "Input links should be unique!" invariant in Graph.cpp);
+		//   - the same pair with a *different* Link::Type counts as a
+		//     distinct constraint and gets its own factor index.
+		auto linkKey = [](int from, int to, int type) {
+			return std::make_tuple(std::min(from, to), std::max(from, to), type);
+		};
+
 		if(isam2_)
 		{
 			UDEBUG("Add new poses...");
@@ -264,31 +309,78 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			}
 			UDEBUG("Add new links...");
 			// new links?
+			//   - self-referring (priors, gravity): add once per new pose, by
+			//     checking addedPoses_ (these are never removed/re-added).
+			//   - regular edges: add if (from,to,type) is not already a live
+			//     factor in iSAM2 (trackedFactors_) AND not already queued
+			//     earlier in this same call (queuedLinks). The latter dedupes
+			//     bidirectional duplicates of the same logical edge supplied
+			//     within a single optimize() call — only one factor goes into
+			//     iSAM2.
+			// Self-ref constraints (priors, gravity) are tracked uniformly
+			// with edges: when a node is transferred to LTM its prior is
+			// dropped from the input, and we need to remove its factor from
+			// iSAM2. Likewise a prior re-supplied after being dropped must
+			// be re-added.
+			std::set<std::tuple<int, int, int> > queuedLinks;
 			for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 			{
-				if(addedPoses_.find(iter->second.from()) == addedPoses_.end() ||
+				const int from = iter->second.from();
-				   addedPoses_.find(iter->second.to()) == addedPoses_.end())
+				const int to = iter->second.to();
+				const std::tuple<int, int, int> key = linkKey(from, to, (int)iter->second.type());
+				bool isNew;
+				if(trackedFactors_.find(key) != trackedFactors_.end())
+				{
+					isNew = false;
+				}
+				else if(!queuedLinks.insert(key).second)
+				{
+					// Already queued earlier in this call (bidirectional
+					// duplicate supplied as both A->B and B->A with the
+					// same type). Skip the second copy.
+					UDEBUG("Ignoring duplicate constraint %d (%d->%d type=%d)", iter->first, from, to, (int)iter->second.type());
+					isNew = false;
+				}
+				else
+				{
+					isNew = true;
+				}
+				if(isNew)
 				{
 					newEdgeConstraints.insert(*iter);
-					UDEBUG("Adding constraint %d (%d->%d) to factor graph", iter->first, iter->second.from(), iter->second.to());
+					UDEBUG("Adding constraint %d (%d->%d type=%d) to factor graph", iter->first, from, to, (int)iter->second.type());
 				}
 			}
 
 			if(!this->isRobust())
 			{
 				UDEBUG("Remove links...");
-				// Remove constraints not there anymore in case the last loop closures were rejected.
+				// Remove every tracked non-self-ref factor whose (from,to,type)
-				// As we don't track "switch" constraints, we don't support this if vertigo is used.
+				// is no longer present in the input. Covers loop closures
-				for(size_t i=0; i<lastAddedConstraints_.size(); ++i)
+				// rejected since the last call, links deleted by graph repair,
+				// or any external deleteLink() applied to old edges. We don't
+				// track "switch" constraints, so this is skipped when vertigo
+				// is used.
+				std::set<std::tuple<int, int, int> > inputLinks;
+				for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 				{
-					if(lastAddedConstraints_[i].from != lastAddedConstraints_[i].to &&
+					inputLinks.insert(linkKey(iter->second.from(), iter->second.to(), (int)iter->second.type()));
-					   graph::findLink(edgeConstraints, lastAddedConstraints_[i].from, lastAddedConstraints_[i].to) == edgeConstraints.end())
+				}
+				for(std::map<std::tuple<int, int, int>, std::uint64_t>::iterator iter=trackedFactors_.begin(); iter!=trackedFactors_.end(); )
+				{
+					if(inputLinks.find(iter->first) == inputLinks.end())
 					{
-						removeFactorIndices.push_back(lastAddedConstraints_[i].factorIndice);
+						removeFactorIndices.push_back(iter->second);
-						UDEBUG("Removing constraint %d->%d (factor indice=%ld)",
+						UDEBUG("Removing constraint %d->%d type=%d (factor indice=%ld)",
-								lastAddedConstraints_[i].from,
+								std::get<0>(iter->first),
-								lastAddedConstraints_[i].to,
+								std::get<1>(iter->first),
-								lastAddedConstraints_[i].factorIndice);
+								std::get<2>(iter->first),
+								iter->second);
+						iter = trackedFactors_.erase(iter);
+					}
+					else
+					{
+						++iter;
 					}
 				}
 			}
@@ -298,7 +390,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				return optimizedPoses;
 			}
 
-			lastAddedConstraints_ = addedPrior;
+			
 		}
 		else
 		{
@@ -403,7 +495,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 									1/iter->second.infMatrix().at<double>(0,0),
 									1/iter->second.infMatrix().at<double>(1,1)));
 							graph.add(XYFactor<gtsam::Point2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
-							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
+							addedConstraints.push_back(ConstraintToFactor(id1, id1, (int)iter->second.type()));
 						}
 						else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
 						{
@@ -411,7 +503,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 									1/iter->second.infMatrix().at<double>(0,0),
 									1/iter->second.infMatrix().at<double>(1,1)));
 							graph.add(XYFactor<gtsam::Pose2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
-							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
+							addedConstraints.push_back(ConstraintToFactor(id1, id1, (int)iter->second.type()));
 						}
 						else
 						{
@@ -431,7 +523,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 
 							gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
 							graph.add(gtsam::PriorFactor<gtsam::Pose2>(id1, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
-							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
+							addedConstraints.push_back(ConstraintToFactor(id1, id1, (int)iter->second.type()));
 						}
 					}
 					else
@@ -443,7 +535,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 										iter->second.infMatrix().at<double>(1,1),
 										iter->second.infMatrix().at<double>(2,2)));
 							graph.add(XYZFactor<gtsam::Point3>(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model));
-							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
+							addedConstraints.push_back(ConstraintToFactor(id1, id1, (int)iter->second.type()));
 						}
 						else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
 							1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
@@ -454,7 +546,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 										iter->second.infMatrix().at<double>(1,1),
 										iter->second.infMatrix().at<double>(2,2)));
 							graph.add(XYZFactor<gtsam::Pose3>(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model));
-							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
+							addedConstraints.push_back(ConstraintToFactor(id1, id1, (int)iter->second.type()));
 						}
 						else
 						{
@@ -472,7 +564,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 							gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
 
 							graph.add(gtsam::PriorFactor<gtsam::Pose3>(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
-							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
+							addedConstraints.push_back(ConstraintToFactor(id1, id1, (int)iter->second.type()));
 						}
 					}
 				}
@@ -489,7 +581,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 #else
 					graph.add(gtsam::AttitudeFactor<gtsam::Pose3>(iter->first, nZ, model, bGMeas));
 #endif
-					lastAddedConstraints_.push_back(ConstraintToFactor(iter->first, iter->first, -1));
+					addedConstraints.push_back(ConstraintToFactor(iter->first, iter->first, (int)iter->second.type()));
 				}
 			}
 			else if(id1<0 || id2 < 0)
@@ -563,7 +655,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 #endif
 							{
 								graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(t.x(), t.y(), t.theta()), model));
-								lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
+								addedConstraints.push_back(ConstraintToFactor(id1, id2, (int)iter->second.type()));
 							}
 						}
 						else if(1 / static_cast<double>(iter->second.infMatrix().at<double>(1,1)) < 9999)
@@ -579,7 +671,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 							gtsam::Point2 landmark(t.x(), t.y());
 							gtsam::Pose2 p;
 							graph.add(gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2>(id1, id2, p.bearing(landmark), p.range(landmark), model));
-							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
+							addedConstraints.push_back(ConstraintToFactor(id1, id2, (int)iter->second.type()));
 						}
 						else
 						{
@@ -594,7 +686,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 							gtsam::Point2 landmark(t.x(), t.y());
 							gtsam::Pose2 p;
 							graph.add(gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2>(id1, id2, p.bearing(landmark), model));
-							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
+							addedConstraints.push_back(ConstraintToFactor(id1, id2, (int)iter->second.type()));
 						}
 					}
 					else
@@ -626,7 +718,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 #endif
 							{
 								graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(t.toEigen4d()), model));
-								lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
+								addedConstraints.push_back(ConstraintToFactor(id1, id2, (int)iter->second.type()));
 							}
 						}
 						else if(1 / static_cast<double>(iter->second.infMatrix().at<double>(2,2)) < 9999)
@@ -643,7 +735,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 							gtsam::Point3 landmark(t.x(), t.y(), t.z());
 							gtsam::Pose3 p;
 							graph.add(gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Point3>(id1, id2, p.bearing(landmark), p.range(landmark), model));
-							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
+							addedConstraints.push_back(ConstraintToFactor(id1, id2, (int)iter->second.type()));
 						}
 						else
 						{
@@ -659,7 +751,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 							gtsam::Point3 landmark(t.x(), t.y(), t.z());
 							gtsam::Pose3 p;
 							graph.add(gtsam::BearingFactor<gtsam::Pose3, gtsam::Point3>(id1, id2, p.bearing(landmark), model));
-							lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
+							addedConstraints.push_back(ConstraintToFactor(id1, id2, (int)iter->second.type()));
 						}
 					}
 				}
@@ -717,7 +809,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 #endif
 					{
 						graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
-						lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
+						addedConstraints.push_back(ConstraintToFactor(id1, id2, (int)iter->second.type()));
 					}
 				}
 				else
@@ -747,7 +839,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 #endif
 					{
 						graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
-						lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
+						addedConstraints.push_back(ConstraintToFactor(id1, id2, (int)iter->second.type()));
 					}
 				}
 			}
@@ -870,6 +962,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				{
 					UDEBUG("Update iSAM with the new factors");
 					result = isam2_->update(graph, initialEstimate, removeFactorIndices);
+
 #if BOOST_VERSION >= 106800
 					UASSERT(result.errorBefore.has_value());
 					UASSERT(result.errorAfter.has_value());
@@ -882,12 +975,26 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					error = result.errorAfter.value();
 					if(!this->isRobust())
 					{
-						UASSERT_MSG(lastAddedConstraints_.size() == result.newFactorsIndices.size(),
+						UASSERT_MSG(addedConstraints.size() == result.newFactorsIndices.size(),
-								uFormat("%ld versus %ld", lastAddedConstraints_.size(), result.newFactorsIndices.size()).c_str());
+								uFormat("%ld versus %ld", addedConstraints.size(), result.newFactorsIndices.size()).c_str());
 						for(size_t j=0; j<result.newFactorsIndices.size(); ++j)
 						{
 							UDEBUG("New factor indice: %ld", result.newFactorsIndices[j]);
-							lastAddedConstraints_[j].factorIndice = result.newFactorsIndices[j];
+							// Persist all input-derived factors (including
+							// self-ref priors / gravity) so later calls can
+							// remove them when dropped from input or skip
+							// re-adding when still present. type == -1 marks
+							// the synthetic root prior, which is managed
+							// separately via lastRootFactorIndex_ and must
+							// stay out of trackedFactors_.
+							if(addedConstraints[j].type != -1)
+							{
+								trackedFactors_[linkKey(
+										addedConstraints[j].from,
+										addedConstraints[j].to,
+										addedConstraints[j].type)] =
+									result.newFactorsIndices[j];
+							}
 						}
 					}
 					if(rootId != 0 && lastRootFactorIndex_.first == 0)
@@ -928,7 +1035,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					delete isam2_;
 					isam2_ = new gtsam::ISAM2(params);
 					addedPoses_.clear();
-					lastAddedConstraints_.clear();
+					trackedFactors_.clear();
 					lastRootFactorIndex_.first = 0;
 					lastSwitchId_ = 1000000000;
 				}

corelib/src/optimizer/OptimizerTORO.cpp

@@ -380,7 +380,7 @@ bool OptimizerTORO::saveGraph(
 	if(file)
 	{
 		
-		for (std::map<int, Transform>::const_iterator iter = poses.lower_bound(0); iter != poses.end(); ++iter)
+		for (std::map<int, Transform>::const_iterator iter = poses.begin(); 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.from() != iter->second.to() && iter->second.type() != Link::kLandmark)
+			if (iter->second.type() != Link::kPosePrior && iter->second.type() != Link::kGravity)
 			{
 				if (isSlam2d())
 				{
@@ -494,31 +494,9 @@ bool OptimizerTORO::loadGraph(
 		while ( fgets (line , 400 , file) != NULL )
 		{
 			std::vector<std::string> strList = uListToVector(uSplit(uReplaceChar(line, '\n', ' '), ' '));
-			if(strList.empty())
+			if(strList.size() == 8)
 			{
-				continue;
+				//VERTEX3
-			}
-			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]);
@@ -536,44 +514,9 @@ bool OptimizerTORO::loadGraph(
 					UFATAL("Pose %d already added", id);
 				}
 			}
-			else if(tag.compare("EDGE2") == 0 && strList.size() == 12)
+			else if(strList.size() == 30)
 			{
-				//EDGE2 observed_vertex_id observing_vertex_id x y theta inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
+				//EDGE3
-				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]);
@@ -582,20 +525,15 @@ 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);
-				int index = 9;
+				informationMatrix.at<double>(3,3) = uStr2Float(strList[9]);
-				for(int i=0; i<6; ++i)
+				informationMatrix.at<double>(4,4) = uStr2Float(strList[15]);
-				{
+				informationMatrix.at<double>(5,5) = uStr2Float(strList[20]);
-					for(int j=i; j<6; ++j)
+				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());
-					{
+				informationMatrix.at<double>(0,0) = uStr2Float(strList[24]);
-						double value = uStr2Float(strList[index++]);
+				informationMatrix.at<double>(1,1) = uStr2Float(strList[27]);
-						informationMatrix.at<double>(i,j) = value;
+				informationMatrix.at<double>(2,2) = uStr2Float(strList[29]);
-						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, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
-					}
-				}
-				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())
 				{
@@ -608,7 +546,7 @@ bool OptimizerTORO::loadGraph(
 					UERROR("Referred poses from the link (%d->%d) don't exist! Link ignored!", idFrom, idTo);
 				}
 			}
-			else
+			else if(strList.size())
 			{
 				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,30 +202,18 @@ 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());
 
-					if(nDesc != nKpts || dim <= 0)
+					c_out = reinterpret_cast<float*>(PyArray_DATA(arrayPtr));
+					for (int i = 0, kpt_idx = 0; i < nDesc*dim; i+=dim, kpt_idx++)
 					{
-						UWARN("Python detector returned mismatched arrays: "
+						if(keep_kpt[kpt_idx]) {
-								"%d keypoints vs %d descriptors (dim=%d). "
+							cv::Mat descriptor = cv::Mat(1, dim, CV_32FC1, &c_out[i]).clone();
-								"Returning empty features.",
+							descriptors_.push_back(descriptor);
-								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);
-							}
 						}
 					}
 				}
@@ -247,15 +235,7 @@ std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & imag
 
 cv::Mat PyDetector::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
-	if(!keypoints.empty() && (int)keypoints.size() != descriptors_.rows)
+	UASSERT((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("SPDetectorRpautrat has been reset before extracting the descriptors! detect() should be called before compute().");
+		UERROR("SPDetector 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,12 +144,7 @@ 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);
@@ -204,9 +199,9 @@ std::vector<cv::KeyPoint> SPDetector::detect(const cv::Mat &img, const cv::Mat &
 		detected_ = true;
 		return keypoints;
 	}
-	catch(const std::exception & e)
+	else
 	{
-		UERROR("SPDetector::detect() threw: %s", e.what());
+		UERROR("No model is loaded!");
 		return std::vector<cv::KeyPoint>();
 	}
 }

docker/resolute/Dockerfile

@@ -1,20 +0,0 @@
-# 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

@@ -1,121 +0,0 @@
-
-# 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,7 +372,6 @@ private Q_SLOTS:
 	void changeOdometryORBSLAMVocabulary();
 	void changeOdometryOKVISConfigPath();
 	void changeOdometryVINSFusionConfigPath();
-	void changeOdometryOpenVINSConfigPath();
 	void changeOdometryLIOSAMConfigPath();
 	void changeOdometryOpenVINSLeftMask();
 	void changeOdometryOpenVINSRightMask();

guilib/src/PreferencesDialog.cpp

@@ -1617,8 +1617,6 @@ 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());
@@ -5794,23 +5792,6 @@ 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>-648</y>
+            <y>0</y>
             <width>684</width>
             <height>5218</height>
            </rect>
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>19</number>
+                <number>18</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>10</number>
+                       <number>1</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">
+                          <layout class="QVBoxLayout" name="verticalLayout_161" stretch="0,0,0,0,0,0">
                            <item>
                             <widget class="QLabel" name="label_632">
                              <property name="text">
@@ -20405,33 +20405,6 @@ 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">
@@ -20448,7 +20421,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>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>
+                                 <string>If we should process two cameras are being stereo or binocular. If binocular, we do monocular feature tracking on each image.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20468,7 +20441,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>KLT tracking. Uncheck to use descriptor matcher.</string>
+                                 <string>If we should use KLT tracking, or descriptor matcher</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20494,7 +20467,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>
@@ -20517,7 +20490,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>Minimum pixel distance. Will check after doing KLT track and remove any features closer than this.</string>
+                                 <string>Will check after doing KLT track and remove any features closer than this</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -20537,7 +20510,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>
@@ -20557,7 +20530,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>
@@ -20580,7 +20553,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>
@@ -20606,7 +20579,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>
@@ -20632,7 +20605,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>
@@ -20661,7 +20634,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>
@@ -20699,7 +20672,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>
@@ -20719,7 +20692,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>
@@ -20739,7 +20712,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>
@@ -20759,7 +20732,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>
@@ -20779,7 +20752,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>
@@ -20799,7 +20772,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>
@@ -20822,7 +20795,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>
@@ -20848,7 +20821,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>
@@ -20953,7 +20926,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>
@@ -20969,7 +20942,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::AdjustToContentsOnFirstShow</enum>
+                                 <enum>QComboBox::AdjustToContents</enum>
                                 </property>
                                 <item>
                                  <property name="text">
@@ -21006,7 +20979,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>
@@ -21035,7 +21008,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>
@@ -21061,7 +21034,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>
@@ -21088,7 +21061,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 (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>
+                                 <string>Mask for left image</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -21115,7 +21088,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>
@@ -21153,7 +21126,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>
@@ -21182,7 +21155,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>
@@ -21208,7 +21181,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>
@@ -21234,7 +21207,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>
@@ -21254,7 +21227,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>
@@ -21274,7 +21247,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>
@@ -21297,7 +21270,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>
@@ -21326,7 +21299,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>
@@ -21349,7 +21322,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>
@@ -21372,7 +21345,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>
@@ -21401,7 +21374,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>
@@ -21427,7 +21400,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>
@@ -21453,7 +21426,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>
@@ -21479,7 +21452,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>
@@ -21505,7 +21478,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>
@@ -21534,7 +21507,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>
@@ -21563,7 +21536,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>
@@ -21592,7 +21565,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>
@@ -21621,7 +21594,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>
@@ -21650,7 +21623,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>
@@ -21679,7 +21652,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>
@@ -21699,7 +21672,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>
@@ -21740,7 +21713,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>
@@ -21772,7 +21745,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>
@@ -21804,7 +21777,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>
@@ -21836,7 +21809,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>
@@ -21865,7 +21838,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>
@@ -21894,7 +21867,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>
@@ -21923,7 +21896,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>
@@ -21952,7 +21925,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.7</version>
+  <version>0.23.5</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>

tools/Reprocess/main.cpp

@@ -82,7 +82,6 @@ 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"
@@ -272,7 +271,6 @@ 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;
@@ -421,10 +419,6 @@ 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;
@@ -1294,11 +1288,6 @@ 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)
 		{