Merge tag '0.23.7' of github.com:introlab/rtabmap into rolling-devel

* iOS CI

* g2o hard-coded path

* bump ios version

* Fixed opencv 4.10.0 build on docker resolute

.devcontainer/noble/devcontainer.json

@@ -4,7 +4,7 @@
     },
     "customizations": {
         "vscode": {
-          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "ms-vscode.cpptools-extension-pack"]
         }
       },
     "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
@@ -14,5 +14,17 @@
       "terminal.integrated.defaultProfile.linux": "bash"
     },
     "remoteUser": "vscode",
-    "runArgs": ["--privileged", "--network=host"]
+    "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"
+    }
 }

.devcontainer/resolute/Dockerfile

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

.devcontainer/resolute/devcontainer.json

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

.devcontainer/rolling/Dockerfile

@@ -6,11 +6,12 @@ ENV DEBIAN_FRONTEND=noninteractive
 # Install ROS2
 RUN apt update && \
     apt install software-properties-common -y && \
-    add-apt-repository universe && \
+    add-apt-repository universe -y && \
     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 && \
+    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" && \
+    apt install /tmp/ros2-apt-source.deb && \
     apt-get clean && rm -rf /var/lib/apt/lists/
 
 # Install build dependencies

.devcontainer/rolling/devcontainer.json

@@ -14,5 +14,17 @@
       "terminal.integrated.defaultProfile.linux": "bash"
     },
     "remoteUser": "vscode",
-    "runArgs": ["--privileged", "--network=host"]
+    "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,20 +22,24 @@ jobs:
     strategy:
       fail-fast: true
       matrix:
-        build_name: [ubuntu-22.04, ubuntu-24.04, ubuntu-24.04-with-opengv]
+        build_name: [ubuntu-22.04, ubuntu-24.04, ubuntu-24.04-with-opengv, ubuntu-26.04]
         include:
         - build_name: ubuntu-22.04
           os: ubuntu-22.04
           extra_deps: "libunwind-dev libceres-dev"
-          extra_cmake_def: "-DWITH_CERES=ON"
+          extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON"
         - build_name: ubuntu-24.04
           os: ubuntu-24.04
           extra_deps: "libg2o-dev libceres-dev"
-          extra_cmake_def: "-DWITH_CERES=ON"
+          extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON"
         - build_name: ubuntu-24.04-with-opengv
           os: ubuntu-24.04
           extra_deps: "libg2o-dev libceres-dev"
-          extra_cmake_def: "-DWITH_CERES=ON -DBUILD_OPENGV=ON"
+          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
@@ -46,9 +50,19 @@ jobs:
         sudo apt-get update
         sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev ${{ matrix.extra_deps }}
 
+    - name: Set up Python
+      uses: actions/setup-python@v5
+      with:
+        python-version: '3.x'
+
+    - name: Install Python Dependencies
+      run: |
+        python -m pip install --upgrade pip
+        pip install numpy pybind11
+
     - name: Configure CMake
       run: |
-        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{ matrix.extra_cmake_def }}
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DPython3_EXECUTABLE=$(which python3) -Dpybind11_DIR=$(python3 -m pybind11 --cmakedir) ${{ matrix.extra_cmake_def }}
 
     - name: Build
       run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}

.github/workflows/cmake-macos.yml

@@ -0,0 +1,83 @@
+name: CMake-MacOS
+
+on:
+  push:
+    branches: 
+      - master
+  pull_request:
+    branches: 
+      - '**'
+
+env:
+  BUILD_TYPE: Release
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
+  cancel-in-progress: ${{ github.event_name == 'pull_request' }}
+
+jobs:
+  build:
+    name: ${{ matrix.build_name }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      fail-fast: true
+      matrix:
+        build_name: [macos-sequoia-intel, macos-sequoia-apple-silicon, macos-tahoe-intel, macos-tahoe-apple-silicon]
+        include:
+        - build_name: macos-sequoia-intel
+          os: macos-15-intel
+          extra_deps: ""
+          extra_cmake_def: '-DBUILD_AS_BUNDLE=ON'
+        - build_name: macos-sequoia-apple-silicon
+          os: macos-15
+          extra_deps: ""
+          extra_cmake_def: '-DBUILD_AS_BUNDLE=ON'
+        - build_name: macos-tahoe-intel
+          os: macos-26-intel
+          extra_deps: ""
+          extra_cmake_def: '-DBUILD_AS_BUNDLE=ON'
+        - build_name: macos-tahoe-apple-silicon
+          os: macos-26
+          extra_deps: ""
+          extra_cmake_def: '-DBUILD_AS_BUNDLE=ON'
+
+    steps:
+    - uses: actions/checkout@v4
+
+    - name: Install Brew Dependencies
+      run: |
+        # Update brew and install from Brewfile if present, or specific packages
+        brew install pcl opencv octomap g2o pdal
+
+    - name: Configure CMake
+      run: |
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{ matrix.extra_cmake_def }}
+
+    - name: Build
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+    - name: Info
+      working-directory: ${{github.workspace}}/build/bin
+      run: |
+        ./rtabmap-console --version
+
+#    - name: Build MacOS Package
+#      run: |
+#        cmake --build ${{ github.workspace }}/build --config ${{ env.BUILD_TYPE }} --target package
+
+#    - name: Upload RTABMap Artifacts (DMG)
+#      uses: actions/upload-artifact@v4
+#      with:
+#        name: RTABMap-Binaries-${{ matrix.build_name }}-zip
+#        path: |
+#          build/RTABMap-*.dmg
+#        compression-level: 0
+#        if-no-files-found: warn
+#        retention-days: ${{ github.event_name == 'pull_request' && 1 || 90 }}
+
+#    - name: Test
+#      working-directory: ${{github.workspace}}/build
+#      # Execute tests defined by the CMake configuration.  
+#      # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail
+#      run: ctest -C ${{env.BUILD_TYPE}}
+      

.github/workflows/cmake-ros.yml

@@ -3,7 +3,7 @@ name: CMake-ROS
 on:
   push:
     branches: 
-      - master
+      - rolling-devel
   pull_request:
     branches: 
       - '**'
@@ -22,57 +22,25 @@ jobs:
     # well on Windows or Mac.  You can convert this to a matrix build if you need
     # cross-platform coverage.
     # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
-    name: ${{ matrix.ros_distribution }}-${{ matrix.os }}
-    runs-on: ${{ matrix.os }}
+    name: ${{ matrix.ros_distribution }}
+    runs-on: ubuntu-latest
     strategy:
       fail-fast: false
       matrix:
-        ros_distribution: [ humble, jazzy, kilted, rolling ]
+        ros_distribution: [ rolling ]
         include:
-        - ros_distribution: 'humble'
-          os: ubuntu-22.04
-        - ros_distribution: 'jazzy'
-          os: ubuntu-24.04
-        - ros_distribution: 'kilted'
-          os: ubuntu-24.04
         - ros_distribution: 'rolling'
-          os: ubuntu-24.04
-
+          skip_keys: ""
+    container:
+      image: osrf/ros:${{ matrix.ros_distribution }}-desktop-full
     steps:   
-    - 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 $VERSION_CODENAME)_all.deb"
-        sudo apt install /tmp/ros2-apt-source.deb
-        sudo apt update
-
+    - uses: actions/checkout@v4
     - uses: ros-tooling/setup-ros@v0.7
       with:
         required-ros-distributions: ${{ matrix.ros_distribution }}
-    
-    - uses: actions/checkout@v4
-
-    - 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
+    - uses: ros-tooling/action-ros-ci@v0.4
+      with:
+        package-name: rtabmap
+        target-ros2-distro: ${{ matrix.ros_distribution }}
+        rosdep-skip-keys: "${{ matrix.skip_keys }}"
       

.github/workflows/docker.yml

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

.github/workflows/ios.yml

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

CMakeLists.txt

@@ -22,9 +22,17 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 23)
-SET(RTABMAP_PATCH_VERSION 4)
+SET(RTABMAP_PATCH_VERSION 7)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
+
+# Make sure we have valid version so that RTABMAP_VERSION_COMPARE logic in Version.h works
+IF(RTABMAP_MINOR_VERSION GREATER 99)
+MESSAGE(FATAL_ERROR "RTABMAP_MINOR_VERSION must be < 100, bump major version and restart minor to 0!")
+ENDIF()
+IF(RTABMAP_PATCH_VERSION GREATER 99)
+MESSAGE(FATAL_ERROR "RTABMAP_PATCH_VERSION must be < 100, bump minor version and restart patch to 0!")
+ENDIF()
   
 SET(PROJECT_VERSION "${RTABMAP_VERSION}")
 
@@ -192,6 +200,7 @@ option(WITH_CCCORELIB     "Include CCCoreLib support"            OFF)
 option(WITH_OPEN3D        "Include Open3D support"               OFF)
 option(WITH_LOAM          "Include LOAM support"                 OFF)
 option(WITH_FLOAM         "Include FLOAM support"                OFF)
+option(WITH_LIOSAM        "Include LIO-SAM support"              OFF)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 option(WITH_ZED           "Include ZED sdk support"              ON)
 option(WITH_ZEDOC         "Include ZED Open Capture support"     ON)
@@ -221,6 +230,7 @@ option(WITH_FASTCV        "Include FastCV support"               ON)
 option(WITH_OPENMP        "Include OpenMP support"               ON)
 option(WITH_OPENGV        "Include OpenGV support"               ON)
 option(BUILD_OPENGV       "Build OpenGV internally instead of using the system one" OFF)
+option(WITH_APRILTAG      "Include AprilTag support"             OFF)
 IF(MOBILE_BUILD)
 option(PCL_OMP            "With PCL OMP implementations"         OFF)
 ELSE()
@@ -630,6 +640,12 @@ IF(WITH_FLOAM)
        FIND_PACKAGE(Ceres REQUIRED)
     ENDIF(floam_FOUND)
 ENDIF(WITH_FLOAM)
+IF(WITH_LIOSAM)
+    find_package(lio_sam QUIET)
+    IF(lio_sam_FOUND)
+       MESSAGE(STATUS "Found lio_sam: ${lio_sam_INCLUDE_DIRS}")
+    ENDIF(lio_sam_FOUND)
+ENDIF(WITH_LIOSAM)
 
 SET(ZED_FOUND FALSE)
 IF(WITH_ZED)
@@ -826,22 +842,7 @@ IF(WITH_VINS_FUSION)
 ENDIF(WITH_VINS_FUSION)
 
 IF(WITH_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()
+   FIND_PACKAGE(OpenVINS)
 ENDIF(WITH_OPENVINS)
 
 IF(WITH_FASTCV)
@@ -851,6 +852,20 @@ IF(WITH_FASTCV)
     ENDIF(FastCV_FOUND)
 ENDIF(WITH_FASTCV)
 
+IF(WITH_APRILTAG)
+    FIND_PACKAGE(apriltag QUIET)
+    IF(apriltag_FOUND)
+      get_target_property(APRILTAG_LOCATION apriltag::apriltag LOCATION)
+      get_target_property(APRILTAG_INCLUDES apriltag::apriltag INTERFACE_INCLUDE_DIRECTORIES)
+      FIND_FILE(apriltag_aruco_4x4_50_header NAMES tagAruco4x4_50.h PATH_SUFFIXES aruco PATHS ${APRILTAG_INCLUDES} NO_DEFAULT_PATH)
+      SET(WITH_APRILTAG_ARUCO NO)
+      IF(apriltag_aruco_4x4_50_header)
+         SET(WITH_APRILTAG_ARUCO YES)
+      ENDIF()
+      MESSAGE(STATUS "Found apriltag (with aruco=${WITH_APRILTAG_ARUCO}): ${APRILTAG_LOCATION} ${APRILTAG_INCLUDES}")
+    ENDIF(apriltag_FOUND)
+ENDIF(WITH_APRILTAG)
+
 IF(WITH_OPENGV OR okvis_FOUND)
    if(NOT BUILD_OPENGV)
       FIND_PACKAGE(opengv QUIET)
@@ -886,6 +901,9 @@ IF(WITH_OPENGV OR okvis_FOUND)
          set(BUILD_TESTS OFF)
          set(CMAKE_BUILD_TYPE Release)
          set(CMAKE_POLICY_DEFAULT_CMP0077 NEW)
+         # OpenGV's CMakeLists.txt declares cmake_minimum_required(VERSION 2.x),
+         # which CMake >= 4.0 (e.g. recent Ubuntu) rejects. Allow it to configure.
+         set(CMAKE_POLICY_VERSION_MINIMUM 3.5)
          # Eigen should have been already added by PCL, just populate the compatible variables
          IF(EIGEN_INCLUDE_DIRS)
             set(EIGEN_INCLUDE_DIRS "${EIGEN_INCLUDE_DIRS}" CACHE PATH "Eigen include dirs" FORCE)
@@ -924,7 +942,7 @@ IF(NOT (APPLE OR WIN32) AND BUILD_WITH_RPATH_NOT_RUNPATH)
 ENDIF()
 
 IF(NOT MSVC)
-    IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1) OR TORCH_FOUND OR MRPT_FOUND)
+    IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1) OR (GTSAM_FOUND AND GTSAM_VERSION VERSION_GREATER_EQUAL "4.3.0") OR TORCH_FOUND OR MRPT_FOUND)
       # Qt6 requires c++17
       include(CheckCXXCompilerFlag)
       CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17)
@@ -1051,6 +1069,12 @@ ENDIF(NOT Open3D_FOUND)
 IF(NOT FastCV_FOUND)
    SET(FASTCV "//")
 ENDIF(NOT FastCV_FOUND)
+IF(NOT apriltag_FOUND)
+   SET(APRILTAG "//")
+   SET(APRILTAG_ARUCO "//")
+ELSEIF(NOT WITH_APRILTAG_ARUCO)
+   SET(APRILTAG_ARUCO "//")
+ENDIF()
 IF(NOT opengv_FOUND OR NOT WITH_OPENGV)
    SET(OPENGV "//")
 ENDIF(NOT opengv_FOUND OR NOT WITH_OPENGV)
@@ -1069,6 +1093,9 @@ ENDIF(NOT loam_velodyne_FOUND)
 IF(NOT floam_FOUND)
    SET(FLOAM "//")
 ENDIF(NOT floam_FOUND)
+IF(NOT lio_sam_FOUND)
+   SET(LIOSAM "//")
+ENDIF(NOT lio_sam_FOUND)
 IF(NOT Freenect_FOUND)
    SET(FREENECT "//")
 ENDIF()
@@ -1184,7 +1211,7 @@ ENDIF()
 IF(NOT vins_FOUND)
    SET(VINSFUSION "//")
 ENDIF()
-IF(NOT ov_msckf_FOUND)
+IF(NOT OpenVINS_FOUND)
    SET(OPENVINS "//")
 ENDIF()
 IF(NOT CUVSLAM_FOUND)
@@ -1457,28 +1484,33 @@ ENDIF(PCL_COMPILE_OPTIONS)
 MESSAGE(STATUS "")
 MESSAGE(STATUS "Optional dependencies ('*' affects some default parameters) :")
 IF(OpenCV_FOUND)
+  IF(OPENCV_ARUCO_FOUND)
+    set(ARUCO_STR "YES")
+  ELSE()
+    set(ARUCO_STR "NO")
+  ENDIF()
   IF(OpenCV_VERSION_MAJOR EQUAL 2)
     IF(OPENCV_NONFREE_FOUND)
-      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = YES (License: Non commercial)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = YES, aruco = ${ARUCO_STR} (License: Non commercial)")
     ELSE()
-      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found, License: BSD)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = NO, aruco = ${ARUCO_STR} (not found, License: BSD)")
     ENDIF()
   ELSE()
     IF(OPENCV_XFEATURES2D_FOUND)
       IF(NONFREE STREQUAL "//")
         IF((OpenCV_VERSION_MAJOR LESS 4) OR ((OpenCV_VERSION_MAJOR EQUAL 4) AND (OpenCV_VERSION_MINOR LESS 5)))
-          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO (License: BSD)")
+          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO, aruco = ${ARUCO_STR} (License: BSD)")
         ELSE()
-          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO (License: Apache 2)")
+          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO, aruco = ${ARUCO_STR} (License: Apache 2)")
         ENDIF()
       ELSE()
-        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = YES (License: Non commercial)")
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = YES, aruco = ${ARUCO_STR} (License: Non commercial)")
       ENDIF()
     ELSE()
       IF((OpenCV_VERSION_MAJOR LESS 4) OR ((OpenCV_VERSION_MAJOR EQUAL 4) AND (OpenCV_VERSION_MINOR LESS 5)))
-        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO (License: BSD)")
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO, aruco = ${ARUCO_STR} (License: BSD)")
       ELSE()
-        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO (License: Apache 2)")
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO, aruco = ${ARUCO_STR} (License: Apache 2)")
       ENDIF()
     ENDIF()
   ENDIF()
@@ -1553,6 +1585,14 @@ ELSE()
 MESSAGE(STATUS "  With FastCV               = NO (FastCV not found)")
 ENDIF()
 
+IF(apriltag_FOUND)
+MESSAGE(STATUS "  With AprilTag ${apriltag_VERSION}       = YES (aruco=${WITH_APRILTAG_ARUCO}) (License: BSD 2-Clause License)")
+ELSEIF(NOT WITH_APRILTAG)
+MESSAGE(STATUS "  With AprilTag             = NO (WITH_APRILTAG=OFF)")
+ELSE()
+MESSAGE(STATUS "  With AprilTag             = NO (apriltag not found)")
+ENDIF()
+
 IF(PDAL_FOUND)
 MESSAGE(STATUS "  With PDAL ${PDAL_VERSION}           = YES (License: BSD)")
 ELSEIF(NOT WITH_PDAL)
@@ -1873,6 +1913,13 @@ MESSAGE(STATUS "  With floam                = NO (WITH_FLOAM=OFF)")
 ELSE()
 MESSAGE(STATUS "  With floam                = NO (floam not found)")
 ENDIF()
+IF(lio_sam_FOUND)
+MESSAGE(STATUS "  With lio_sam              = YES (License: BSD)")
+ELSEIF(NOT WITH_LIOSAM)
+MESSAGE(STATUS "  With lio_sam              = NO (WITH_LIOSAM=OFF)")
+ELSE()
+MESSAGE(STATUS "  With lio_sam              = NO (lio_sam not found)")
+ENDIF()
 
 IF(libfovis_FOUND)
 MESSAGE(STATUS "  With libfovis             = YES (License: GPLv2)")
@@ -1922,7 +1969,7 @@ ELSE()
 MESSAGE(STATUS "  With VINS-Fusion          = NO (VINS-Fusion not found)")
 ENDIF()
 
-IF(ov_msckf_FOUND)
+IF(OpenVINS_FOUND)
 MESSAGE(STATUS "  With OpenVINS             = YES (License: GPLv3)")
 ELSEIF(NOT WITH_OPENVINS)
 MESSAGE(STATUS "  With OpenVINS             = NO (WITH_OPENVINS=OFF)")

README.md

@@ -35,7 +35,12 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
   <table>
     <tbody>
         <tr>
-           <td><a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-linux.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-linux.yml/badge.svg" alt="CMake Linux Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-windows.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-windows.yml/badge.svg" alt="CMake Windows Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg" alt="CMake ROS Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/docker.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg" alt="Docker Build Status"/>
+           <td>
+           <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-linux.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-linux.yml/badge.svg" alt="CMake Linux Build Status"/> <br> 
+           <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-windows.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-windows.yml/badge.svg" alt="CMake Windows Build Status"/> <br> 
+           <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-macos.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-macos.yml/badge.svg" alt="CMake MaCOS Build Status"/> <br> 
+           <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg" alt="CMake ROS Build Status"/> <br> 
+           <a href="https://github.com/introlab/rtabmap/actions/workflows/docker.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg" alt="Docker Build Status"/>
            </td>
         </tr>
      </tbody>

Version.h.in

@@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define RTABMAP_VERSION_MINOR @PROJECT_VERSION_MINOR@
 #define RTABMAP_VERSION_PATCH @PROJECT_VERSION_PATCH@
 
-#define RTABMAP_VERSION_COMPARE(major, minor, patch)  (major>=@PROJECT_VERSION_MAJOR@ || (major==@PROJECT_VERSION_MAJOR@ && minor>=@PROJECT_VERSION_MINOR@) || (major==@PROJECT_VERSION_MAJOR@ && minor==@PROJECT_VERSION_MINOR@ && patch >=@PROJECT_VERSION_PATCH@))
+#define RTABMAP_VERSION_COMPARE(OP, MAJOR, MINOR, PATCH) (RTABMAP_VERSION_MAJOR*10000+RTABMAP_VERSION_MINOR*100+RTABMAP_VERSION_PATCH OP MAJOR*10000+MINOR*100+PATCH)
 
 @NONFREE@#define RTABMAP_NONFREE
 @TORO@#define RTABMAP_TORO
@@ -63,6 +63,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @CUDASIFT@#define RTABMAP_CUDASIFT
 @LOAM@#define RTABMAP_LOAM
 @FLOAM@#define RTABMAP_FLOAM
+@LIOSAM@#define RTABMAP_LIOSAM
 @DC1394@#define RTABMAP_DC1394
 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
 @ZED@#define RTABMAP_ZED
@@ -92,6 +93,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @TORCH@#define RTABMAP_TORCH
 @PYTHON@#define RTABMAP_PYTHON
 @MADGWICK@#define RTABMAP_MADGWICK
+@APRILTAG@#define RTABMAP_APRILTAG
+@APRILTAG_ARUCO@#define RTABMAP_APRILTAG_WITH_ARUCO
 
 #include <pcl/pcl_config.h>
 

app/android/jni/RTABMapApp.cpp

@@ -3000,7 +3000,7 @@ void RTABMapApp::setTrajectoryMode(bool enabled)
 void RTABMapApp::setGraphOptimization(bool enabled)
 {
 	graphOptimization_ = enabled;
-	if((sensorCaptureThread_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0)
+	if((sensorCaptureThread_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature(false)!=0)
 	{
 		std::map<int, rtabmap::Transform> poses;
 		std::multimap<int, rtabmap::Link> links;

app/ios/RTABMapApp.xcodeproj/project.pbxproj

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

app/ios/RTABMapApp/install_deps.sh

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

cmake_modules/FindG2O.cmake

@@ -101,16 +101,16 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FIL
 	${G2O_TYPES_SBA}
         ${G2O_STUFF_LIBRARY})
   
-  IF(CSPARSE_FOUND)
+  IF(CSPARSE_FOUND AND G2O_SOLVER_CSPARSE AND G2O_SOLVER_CSPARSE_EXTENSION)
      SET(G2O_INCLUDE_DIRS 
         ${G2O_INCLUDE_DIRS} 
         ${CSPARSE_INCLUDE_DIR})
      SET(G2O_LIBRARIES
         ${G2O_LIBRARIES} 
-	${G2O_SOLVER_CSPARSE} 
-	${G2O_SOLVER_CSPARSE_EXTENSION}
-	${CSPARSE_LIBRARY})
-  ENDIF(CSPARSE_FOUND)
+        ${G2O_SOLVER_CSPARSE} 
+        ${G2O_SOLVER_CSPARSE_EXTENSION}
+        ${CSPARSE_LIBRARY})
+  ENDIF(CSPARSE_FOUND AND G2O_SOLVER_CSPARSE AND G2O_SOLVER_CSPARSE_EXTENSION)
 
   IF(G2O_SOLVER_CHOLMOD)
     SET(G2O_INCLUDE_DIRS 

cmake_modules/FindOpenVINS.cmake

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

corelib/include/rtabmap/core/DBReader.h

@@ -60,6 +60,7 @@ public:
 			 int stopMapId = -1,
 			 bool priorsIgnored = false,
 			 bool imuIgnored = false,
+			 bool intermediateNodesAreNormalNodes = false,
 			 const std::vector<Transform> & cameraLocalTransformOverrides = std::vector<Transform>());
 	DBReader(const std::list<std::string> & databasePaths,
 			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
@@ -76,6 +77,7 @@ public:
 			 int stopMapId = -1,
 			 bool priorsIgnored = false,
 			 bool imuIgnored = false,
+			 bool intermediateNodesAreNormalNodes = false,
 			 const std::vector<Transform> & cameraLocalTransformOverrides = std::vector<Transform>());
 	virtual ~DBReader();
 
@@ -106,6 +108,7 @@ private:
 	int _stopId;
 	std::vector<unsigned int> _cameraIndices;
 	bool _intermediateNodesIgnored;
+	bool _intermediateNodesAreNormalNodes;
 	bool _landmarksIgnored;
 	bool _featuresIgnored;
 	bool _priorsIgnored;

corelib/include/rtabmap/core/Graph.h

@@ -118,15 +118,18 @@ Transform RTABMAP_CORE_EXPORT calcRMSE(
 		float & rotational_max,
 		bool align2D = false);
 
-void RTABMAP_CORE_EXPORT computeMaxGraphErrors(
+struct MaxGraphErrors
+{
+	float linear=-1.0f;         // absolute error (m) of the link with maximum linear error
+	float angular=-1.0f;        // absolute error (rad)  of the link with maximum angular error
+	float linearRatio=-1.0f;    // Ratio = absolute error (m) / linear std (m), of the link with maximum linear error
+	float angularRatio=-1.0f;   // Ratio = absolute error (rad) / angular std (rad), of the link with maximum angular error
+	Link linearLink;  // link with maximum linear error
+	Link angularLink; // link with maximum angular error
+};
+MaxGraphErrors RTABMAP_CORE_EXPORT computeMaxGraphErrors(
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
-		float & maxLinearErrorRatio,
-		float & maxAngularErrorRatio,
-		float & maxLinearError,
-		float & maxAngularError,
-		const Link ** maxLinearErrorLink = 0,
-		const Link ** maxAngularErrorLink = 0,
 		bool for3DoF = false);
 
 std::vector<double> RTABMAP_CORE_EXPORT getMaxOdomInf(const std::multimap<int, Link> & links);

corelib/include/rtabmap/core/LaserScan.h

@@ -48,7 +48,8 @@ public:
 		kXYZNormal=8,
 		kXYZINormal=9,
 		kXYZRGBNormal=10,
-		kXYZIT=11};
+		kXYZIT=11,
+		kXYZIRT=12};
 
 	static std::string formatName(const Format & format);
 	static int channels(const Format & format);
@@ -57,6 +58,7 @@ public:
 	static bool isScanHasRGB(const Format & format);
 	static bool isScanHasIntensity(const Format & format);
 	static bool isScanHasTime(const Format & format);
+	static bool isScanHasRing(const Format & format);
 	static LaserScan backwardCompatibility(
 			const cv::Mat & oldScanFormat,
 			int maxPoints = 0,
@@ -135,6 +137,7 @@ public:
 	bool hasRGB() const {return isScanHasRGB(format_);}
 	bool hasIntensity() const {return isScanHasIntensity(format_);}
 	bool hasTime() const {return isScanHasTime(format_);}
+	bool hasRing() const {return isScanHasRing(format_);}
 	bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
 	bool isOrganized() const {return data_.rows > 1;}
 	LaserScan clone() const;
@@ -143,7 +146,8 @@ public:
 	int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
 	int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
 	int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
-	int getTimeOffset() const {return hasTime()?4:-1;}
+	int getRingOffset() const {return format_==kXYZIRT?4:-1;}
+	int getTimeOffset() const {return format_==kXYZIT?4:(format_==kXYZIRT?5:-1);}
 
 	float & field(unsigned int pointIndex, unsigned int channelOffset);
 

corelib/include/rtabmap/core/MarkerDetector.h

@@ -57,6 +57,12 @@ private:
 };
 
 class RTABMAP_CORE_EXPORT MarkerDetector {
+
+public:
+  enum Strategy {
+    kStrategyOpencv,
+    kStrategyApriltag
+  };
     
 public:
 	MarkerDetector(const ParametersMap & parameters = ParametersMap());
@@ -84,15 +90,19 @@ public:
 		    cv::Mat * imageWithDetections = 0);
 
 private:
-#ifdef HAVE_OPENCV_ARUCO
-	cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
-	float markerLength_;
+  Strategy strategy_;
+  float markerLength_;
+  std::map<int, float> markerLengths_;
 	float maxDepthError_;
 	float maxRange_;
 	float minRange_;
 	int dictionaryId_;
+#ifdef HAVE_OPENCV_ARUCO
+	cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
 	cv::Ptr<cv::aruco::Dictionary> dictionary_;
 #endif
+  void * apriltagLibDetector_;
+  void * apriltagLibFamily_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Memory.h

@@ -140,10 +140,11 @@ public:
 			float radius,
 			const std::map<int, Transform> & optimizedPoses,
 			int maxGraphDepth) const;
+	void convertToIntermediate(int locationId);
 	void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
 	void saveLocationData(int locationId);
 	void removeLink(int idA, int idB);
-	void removeRawData(int id, bool image = true, bool scan = true, bool userData = true);
+	void removeRawData(int id, bool image = true, bool scan = true, bool userData = true, bool occupancyGrid = true);
 	int reduceNode(int id, float maxDistance = 0.0f, bool keepLinkedInDb = false, int direction = 0);
 
 	//getters
@@ -162,7 +163,7 @@ public:
 	float getSimilarityThreshold() const {return _similarityThreshold;}
 	std::map<int, int> getWeights() const;
 	int getLastSignatureId() const;
-	const Signature * getLastWorkingSignature() const;
+	const Signature * getLastWorkingSignature(bool ignoreIntermediateNodes) const;
 	std::map<int, Link> getNodesObservingLandmark(int landmarkId, bool lookInDatabase) const;
 	int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const;
 	bool labelSignature(int id, const std::string & label);

corelib/include/rtabmap/core/Odometry.h

@@ -57,7 +57,8 @@ public:
 		kTypeOpenVINS = 10,
 		kTypeFLOAM = 11,
 		kTypeOpen3D = 12,
-		kTypeCuVSLAM = 13
+		kTypeCuVSLAM = 13,
+		kTypeLIOSAM = 14
 	};
 
 public:

corelib/include/rtabmap/core/Parameters.h

@@ -218,7 +218,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Mem, ReduceGraph,                 bool, false,    uFormat("Reduce graph. Merge nodes when loop closures are added (ignoring those with user data). Note that this approach assumes that 100%% of the loop closures accepted are good, so it is highly recommended to enable \"%s\" at the same time.", kRGBDOptimizeMaxError().c_str()));
     RTABMAP_PARAM(Mem, RecentWmRatio,               float, 0.2,     "Ratio of locations after the last loop closure in WM that cannot be transferred.");
     RTABMAP_PARAM(Mem, TransferSortingByWeightId,   bool, false,    "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");
-    RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne,  bool, false,    "On merge, update to new id. When false, no copy.");
+    RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne,  bool, false,    uFormat("On merge, update to new id. When false, no copy. Keep this disable if %s=true.", kRtabmapCreateIntermediateNodes().c_str()));
     RTABMAP_PARAM(Mem, RehearsalWeightIgnoredWhileMoving, bool, false, "When the robot is moving, weights are not updated on rehearsal.");
     RTABMAP_PARAM(Mem, GenerateIds,                 bool, true,     "True=Generate location IDs, False=use input image IDs.");
     RTABMAP_PARAM(Mem, BadSignaturesIgnored,        bool, false,    "Bad signatures are ignored.");
@@ -249,7 +249,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Kp, MinDepth,                 float, 0,     "Filter extracted keypoints by depth.");
     RTABMAP_PARAM(Kp, MaxFeatures,              int, 500,     "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
     RTABMAP_PARAM(Kp, SSC,                      bool, false,  "If true, SSC (Suppression via Square Covering) is applied to limit keypoints.");
-    RTABMAP_PARAM(Kp, BadSignRatio,             float, 0.5,   "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
+    RTABMAP_PARAM(Kp, BadSignRatio,             float, 0.5,   uFormat("Bad signature ratio. If %s=0, the ratio is computed from the average number of words per signature (less than Ratio x AverageWordsPerImage = bad).", kKpMaxFeatures().c_str()));
     RTABMAP_PARAM(Kp, NndrRatio,                float, 0.8,   "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
 #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
     // OpenCV>2 without xFeatures2D module doesn't have BRIEF
@@ -379,6 +379,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
     RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
     RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 3.0,   uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. If used with \"%s\", the disabled loop closure links will be removed.", kOptimizerRobust().c_str()));
+    RTABMAP_PARAM(RGBD, OptimizeMaxErrorRepairRadius, float, 0.0,   uFormat("If two consecutive loop closures are rejected by %s on the same old loop closure link, we will remove that old link, and other old links under that radius if necessary, until optimization is accepted. When optimization is accepted, the old loop closure links are removed from the graph. This feature is useful to reject bad loop closures that were accepted previously. Set to 0 to disable this feature.", kRGBDOptimizeMaxError().c_str()));
     RTABMAP_PARAM(RGBD, MaxLoopClosureDistance,   float, 0.0,   "Reject loop closures/localizations if the distance from the map is over this distance (0=disabled).");
     RTABMAP_PARAM(RGBD, ForceOdom3DoF,            bool, true,  uFormat("Force odometry pose to be 3DoF if %s=true.", kRegForce3DoF().c_str()));
     RTABMAP_PARAM(RGBD, StartAtOrigin,            bool, false, uFormat("If true, rtabmap will assume the robot is starting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode (%s=false).", kMemIncrementalMemory().c_str()));
@@ -465,7 +466,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(GTSAM, IncRelinearizeSkip,       int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.");
 
     // Odometry
-    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D 13=cuVSLAM");
+    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D 13=cuVSLAM 14=LIO-SAM");
     RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,       "Automatically reset odometry after X consecutive images where odometry cannot be computed (a value of 0 disables auto-reset). When a reset occurs, odometry resumes from the last successfully computed pose with large covariance to trigger a new map. If external odometry is used, it will also be reset based on the motion estimated relative to the last computed pose but no large covariance will be received, so that a new map won't be triggered.");
     RTABMAP_PARAM(Odom, Holonomic,              bool, true,   "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
     RTABMAP_PARAM(Odom, FillInfoData,           bool, true,   "Fill info with data (inliers/outliers features).");
@@ -619,66 +620,67 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM_STR(OdomVINSFusion, ConfigPath,     "",  "Path of VINS-Fusion config file.");
 
     // Odometry OpenVINS
-    RTABMAP_PARAM(OdomOpenVINS, UseStereo,                 bool,   true,   "If we have more than 1 camera, if we should try to track stereo constraints between pairs");
-    RTABMAP_PARAM(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, 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, FiRefineFeatures,          bool,   true,   "If we should perform Levenberg-Marquardt refinement");
-    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, FiMaxCondNumber,           double, 10000,  "Max condition number of linear triangulation matrix accept triangulated features");
+    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, 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, 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, FiRefineFeatures,          bool,   true,   "If we should perform Levenberg-Marquardt refinement.");
+    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, 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, 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, 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, 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, MaxClones,                 int,    11,     "Max clone size of sliding window");
-    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, 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, 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, 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, RightMaskPath,                 "",     "Mask for right image");
+    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, 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, 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, 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, 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, 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, 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, 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, RightMaskPath,                 "",     "Mask for right image.");
 
-    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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 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, UpMSCKFSigmaPx,            double, 1.0,    "Pixel noise 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, UpSLAMChi2Multiplier,      double, 1.0,    "Chi2 multiplier for SLAM features");
+    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, 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, UpMSCKFSigmaPx,            double, 1.0,    "Pixel noise 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, UpSLAMChi2Multiplier,      double, 1.0,    "Chi2 multiplier for SLAM features.");
 
     // Odometry Open3D
     RTABMAP_PARAM(OdomOpen3D, MaxDepth,         float, 3.0,  "Maximum depth.");
@@ -687,6 +689,21 @@ class RTABMAP_CORE_EXPORT Parameters
     // Odometry cuVSLAM
     RTABMAP_PARAM(OdomCuVSLAM, MulticamMode,        int, 0,  "cuVSLAM multicam_mode setting: 0=moderate, 1=performance, 2=precision.");
 
+    // Odometry LIO-SAM
+    RTABMAP_PARAM_STR(OdomLIOSAM, ConfigPath,  "", "Path to LIO-SAM params.yaml config file. When set, sensor/IMU/feature parameters are loaded from the file and the individual parameters below are ignored.");
+    RTABMAP_PARAM(OdomLIOSAM, Sensor,       int,   0,     "LiDAR sensor: 0=Velodyne, 1=Ouster, 2=Livox");
+    RTABMAP_PARAM(OdomLIOSAM, NScan,        int,   16,    "Number of LiDAR channels (16, 32, 64, 128).");
+    RTABMAP_PARAM(OdomLIOSAM, HorizonScan,  int,   1800,  "Horizontal resolution (Velodyne:1800, Ouster:512/1024/2048).");
+    RTABMAP_PARAM(OdomLIOSAM, ImuAccNoise,  float, 0.01,  "IMU accelerometer white noise.");
+    RTABMAP_PARAM(OdomLIOSAM, ImuGyrNoise,  float, 0.001, "IMU gyroscope white noise.");
+    RTABMAP_PARAM(OdomLIOSAM, ImuAccBiasN,  float, 0.0002,"IMU accelerometer bias noise.");
+    RTABMAP_PARAM(OdomLIOSAM, ImuGyrBiasN,  float, 0.00003,"IMU gyroscope bias noise.");
+    RTABMAP_PARAM(OdomLIOSAM, ImuGravity,   float, 9.80511,"Gravity magnitude.");
+    RTABMAP_PARAM(OdomLIOSAM, EdgeThreshold,float, 1.0,   "Edge feature curvature threshold.");
+    RTABMAP_PARAM(OdomLIOSAM, SurfThreshold,float, 0.1,   "Surface feature curvature threshold.");
+    RTABMAP_PARAM(OdomLIOSAM, LinVar,       float, 0.01,  "Linear output variance.");
+    RTABMAP_PARAM(OdomLIOSAM, AngVar,       float, 0.01,  "Angular output variance.");
+
     // Common registration parameters
     RTABMAP_PARAM(Reg, RepeatOnce,               bool, true,    "Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration (like on loop closure). It can be useful if the registration approach used can use a guess to get better matches.");
     RTABMAP_PARAM(Reg, Strategy,                 int, 0,        "0=Vis, 1=Icp, 2=VisIcp");
@@ -901,19 +918,29 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(GridGlobal, ProbClampingMax,      float,  0.971,   "Probability clamping maximum (value between 0 and 1).");
     RTABMAP_PARAM(GridGlobal, FloodFillDepth,       unsigned int, 0, "Flood fill filter (0=disabled), used to remove empty cells outside the map. The flood fill is done at the specified depth (between 1 and 16) of the OctoMap.");
 
-    RTABMAP_PARAM(Marker, Dictionary,             int,   0,     "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20");
-    RTABMAP_PARAM(Marker, Length,                 float, 0,     "The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization).");
+    RTABMAP_PARAM(Marker, Strategy,               int,   0,     "Marker detection implementation: 0=OpenCV, 1=AprilTag");
+    RTABMAP_PARAM(Marker, Dictionary,             int,   0,     "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20, DICT_ARUCO_MIP_36H12=21");
+    RTABMAP_PARAM(Marker, Length,                 float, 0,     "The length (m) of the markers' side. Value <=0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization). If 0, the length is estimated only on the first marker detected, then re-used for all next detections (i.e., this assumes that markers have all the same length). With <0, the length is estimated once for each unique marker, then re-used for next detections with the same marker ID.");
+    RTABMAP_PARAM_STR(Marker, Lengths,            "",           uFormat("List of markers to detect. Format is the marker's ID followed by its length (in meters), multiple markers are separated by a vertical line (\"id1 length|id2 length\"). We can also define a range of markers with \"id1:id2 length\" (id2 included). If empty, all markers of the chosen dictionary can be detected and their length is set/estimated based on %s. For example, to detect markers 12 and 14 with lengths of 8 and 15 cm respectively, and all markers between 30 and 40 with a length of 10 cm, set \"12 0.08|14 0.15|30:40 0.1\".", kMarkerLength().c_str()).c_str());
     RTABMAP_PARAM(Marker, MaxDepthError,          float, 0.01,  uFormat("Maximum depth error between all corners of a marker when estimating the marker length (when %s is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length.", kMarkerLength().c_str()));
     RTABMAP_PARAM(Marker, VarianceLinear,         float, 0.001, uFormat("Linear variance to set on marker detections. If %s is enabled and %s=2 (GTSAM): it is the variance of the range factor, with 9999 to disable range factor and to do only bearing.", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str()));
     RTABMAP_PARAM(Marker, VarianceAngular,        float, 0.01,  uFormat("Angular variance to set on marker detections. If %s is enabled, it is ignored with %s=1 (g2o) and it corresponds to bearing variance with %s=2 (GTSAM).", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str(), kOptimizerStrategy().c_str()));
     RTABMAP_PARAM(Marker, VarianceOrientationIgnored, bool, false, uFormat("When this setting is false, the landmark's orientation is optimized during graph optimization. When this setting is true, only the position of the landmark is optimized. This can be useful when the landmark's orientation estimation is not reliable. Note that for %s=1 (g2o), only %s needs be set if we ignore orientation. For %s=2 (GTSAM), instead of optimizing the landmark's position directly, a bearing/range factor is used, with %s as the variance of the range factor (with 9999 to optimize the position with only a bearing factor) and %s as the variance of the bearing factor (pitch/yaw).", kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kMarkerVarianceAngular().c_str()));
-    RTABMAP_PARAM(Marker, CornerRefinementMethod, int,   0,     "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
     RTABMAP_PARAM(Marker, MaxRange,               float, 0.0,   "Maximum range in which markers will be detected. <=0 for unlimited range.");
     RTABMAP_PARAM(Marker, MinRange,               float, 0.0,   "Miniminum range in which markers will be detected. <=0 for unlimited range.");
-    RTABMAP_PARAM_STR(Marker, Priors,             "",           "World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), markers are separated by vertical line (\"id1 x y z roll pitch yaw|id2 x y z roll pitch yaw\"). Example:  \"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57\" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).");
+    RTABMAP_PARAM_STR(Marker, Priors,             "",           "World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), multiple markers are separated by vertical line (\"id1 x y z roll pitch yaw|id2 x y z roll pitch yaw\"). Example:  \"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57\" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).");
     RTABMAP_PARAM(Marker, PriorsVarianceLinear,   float, 0.001, "Linear variance to set on marker priors.");
     RTABMAP_PARAM(Marker, PriorsVarianceAngular,  float, 0.001, "Angular variance to set on marker priors.");
 
+    RTABMAP_PARAM(MarkerAprilTag, NThreads,         int,     1,     "How many threads should be used?");
+    RTABMAP_PARAM(MarkerAprilTag, QuadDecimate,     float,   1.0,   "Detection of quads can be done on a lower-resolution image, improving speed at a cost of pose accuracy and a slight decrease in detection rate. Decoding the binary payload is still done at full resolution.");
+    RTABMAP_PARAM(MarkerAprilTag, QuadSigma,        float,   0.0,   "What Gaussian blur should be applied to the segmented image (used for quad detection?)  Parameter is the standard deviation in pixels.  Very noisy images benefit from non-zero values (e.g. 0.8).");
+    RTABMAP_PARAM(MarkerAprilTag, RefineEdges,      bool,    true,   uFormat("When true, the edges of the each quad are adjusted to \"snap to\" strong gradients nearby. This is useful when decimation is employed, as it can increase the quality of the initial quad estimate substantially. Generally recommended to be on (true). Very computationally inexpensive. Option is ignored if %s = 1.", kMarkerAprilTagQuadDecimate().c_str()));
+    RTABMAP_PARAM(MarkerAprilTag, DecodeSharpening, double,  0.25,  "How much sharpening should be done to decoded images? This can help decode small tags but may or may not help in odd lighting conditions or low light conditions.");
+    RTABMAP_PARAM(MarkerAprilTag, Debug,            bool,    false, uFormat("When true, write a variety of debugging images to the working directory where the app started (not %s) at various stages through the detection process. (Somewhat slow).", kRtabmapWorkingDirectory().c_str()));
+
+    RTABMAP_PARAM(MarkerOpenCV, CornerRefinementMethod, int,   0,     "Corner refinement method for OpenCV strategy (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
+
     RTABMAP_PARAM(ImuFilter, MadgwickGain,                  double, 0.1,  "Gain of the filter. Higher values lead to faster convergence but more noise. Lower values lead to slower convergence but smoother signal, belongs in [0, 1].");
     RTABMAP_PARAM(ImuFilter, MadgwickZeta,                  double, 0.0,  "Gyro drift gain (approx. rad/s), belongs in [-1, 1].");
 

corelib/include/rtabmap/core/Rtabmap.h

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Statistics.h"
 #include "rtabmap/core/Link.h"
 #include "rtabmap/core/ProgressState.h"
+#include "rtabmap/core/Graph.h"
 
 #include <opencv2/core/core.hpp>
 #include <list>
@@ -264,6 +265,13 @@ private:
 			std::multimap<int, Link> * constraints = 0,
 			double * error = 0,
 			int * iterationsDone = 0) const;
+	std::list<std::pair<int, int> > repairGraph(
+		graph::MaxGraphErrors & maxGraphErrors,
+		std::map<int, Transform> & poses,
+		std::multimap<int, Link> & constraints,
+		double & optimizationError,
+		int & optimizationIterations,
+		cv::Mat & optimizationCovariance);
 	void updateGoalIndex();
 	bool computePath(int targetNode, std::map<int, Transform> nodes, const std::multimap<int, rtabmap::Link> & constraints);
 
@@ -320,6 +328,7 @@ private:
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
 	float _optimizationMaxError;
+	float _optimizationMaxErrorRepairRadius;
 	bool _startNewMapOnLoopClosure;
 	bool _startNewMapOnGoodSignature;
 	float _goalReachedRadius; // meters
@@ -379,6 +388,7 @@ private:
 	std::map<int, Transform> _odomCachePoses;       // used in localization mode to reject loop closures
 	std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
 	std::map<int, Transform> _markerPriors;
+	std::pair<int, int> _lastRejectedLoopClosureIds;
 
 	std::set<int> _nodesToRepublish;
 

corelib/include/rtabmap/core/RtabmapThread.h

@@ -106,7 +106,6 @@ private:
 	unsigned int _dataBufferMaxSize;
 	float _rate;
 	bool _createIntermediateNodes;
-	UTimer * _frameRateTimer;
 	double _previousStamp;
 
 	Rtabmap * _rtabmap;

corelib/include/rtabmap/core/SensorData.h

@@ -212,6 +212,12 @@ public:
 			_userDataCompressed.empty() &&
 			_keypoints.size() == 0 &&
 			_descriptors.empty() &&
+			_groundCellsRaw.empty() &&
+			_groundCellsCompressed.empty() &&
+			_obstacleCellsRaw.empty() &&
+			_obstacleCellsCompressed.empty() &&
+			_emptyCellsRaw.empty() &&
+			_emptyCellsCompressed.empty() &&
 			imu_.empty());
 	}
 
@@ -309,8 +315,6 @@ public:
 			const cv::Mat & empty,
 			float cellSize,
 			const cv::Point3f & viewPoint);
-	// remove raw occupancy grids
-	void clearOccupancyGridRaw() {_groundCellsRaw = cv::Mat(); _obstacleCellsRaw = cv::Mat();}
 	const cv::Mat & gridGroundCellsRaw() const {return _groundCellsRaw;}
 	const cv::Mat & gridGroundCellsCompressed() const {return _groundCellsCompressed;}
 	const cv::Mat & gridObstacleCellsRaw() const {return _obstacleCellsRaw;}
@@ -355,12 +359,12 @@ public:
 	 * Clear compressed rgb/depth (left/right) images, compressed laser scan and compressed user data.
 	 * Raw data are kept is set.
 	 */
-	void clearCompressedData(bool images = true, bool scan = true, bool userData = true);
+	void clearCompressedData(bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true);
 	/**
 	 * Clear raw rgb/depth (left/right) images, raw laser scan and raw user data.
 	 * Compressed data are kept is set.
 	 */
-	void clearRawData(bool images = true, bool scan = true, bool userData = true);
+	void clearRawData(bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true);
 
 	bool isPointVisibleFromCameras(const cv::Point3f & pt) const; // assuming point is in robot frame
 

corelib/include/rtabmap/core/Statistics.h

@@ -76,6 +76,13 @@ class RTABMAP_CORE_EXPORT Statistics
 	RTABMAP_STATS(Loop, Optimization_max_ang_error_ratio, );
 	RTABMAP_STATS(Loop, Optimization_error, );
 	RTABMAP_STATS(Loop, Optimization_iterations, );
+	RTABMAP_STATS(Loop, Optimization_max_error_from_id, );
+	RTABMAP_STATS(Loop, Optimization_max_error_to_id, );
+	RTABMAP_STATS(Loop, Optimization_max_ang_error_from_id, );
+	RTABMAP_STATS(Loop, Optimization_max_ang_error_to_id, );
+	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, Linear_variance,);
 	RTABMAP_STATS(Loop, Angular_variance,);
 	RTABMAP_STATS(Loop, Landmark_detected,);

corelib/include/rtabmap/core/impl/util3d.hpp

@@ -41,11 +41,11 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
 		return LaserScan();
 	}
 	//determine the output type
-	int fieldStates[8] = {0}; // x,y,z,normal_x,normal_y,normal_z,rgb,intensity
+	int fieldStates[10] = {0}; // x,y,z,normal_x,normal_y,normal_z,rgb,intensity,time,ring
 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
-	std::uint32_t fieldOffsets[8] = {0};
+	std::uint32_t fieldOffsets[10] = {0};
 #else
-	pcl::uint32_t fieldOffsets[8] = {0};
+	pcl::uint32_t fieldOffsets[10] = {0};
 #endif
 	for(unsigned int i=0; i<cloud.fields.size(); ++i)
 	{
@@ -102,6 +102,42 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
 			fieldStates[7] = 1;
 			fieldOffsets[7] = cloud.fields[i].offset;
 		}
+		else if(cloud.fields[i].name.compare("time") == 0)
+		{
+			if(cloud.fields[i].datatype != pcl::PCLPointField::FLOAT32)
+			{
+				static bool warningShown = false;
+				if(!warningShown)
+				{
+					UWARN("The input scan cloud has an \"time\" field "
+							"but the datatype (%d) is not supported. Time will be ignored. "
+							"This message is only shown once.", cloud.fields[i].datatype);
+					warningShown = true;
+				}
+				continue;
+			}
+
+			fieldStates[8] = 1;
+			fieldOffsets[8] = cloud.fields[i].offset;
+		}
+		else if(cloud.fields[i].name.compare("ring") == 0)
+		{
+			if(cloud.fields[i].datatype != pcl::PCLPointField::UINT16)
+			{
+				static bool warningShown = false;
+				if(!warningShown)
+				{
+					UWARN("The input scan cloud has an \"ring\" field "
+							"but the datatype (%d) is not supported. Ring will be ignored. "
+							"This message is only shown once.", cloud.fields[i].datatype);
+					warningShown = true;
+				}
+				continue;
+			}
+
+			fieldStates[9] = 1;
+			fieldOffsets[9] = cloud.fields[i].offset;
+		}
 		else
 		{
 			UDEBUG("Ignoring \"%s\" field", cloud.fields[i].name.c_str());
@@ -117,6 +153,8 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
 	bool hasNormals = fieldStates[3] || fieldStates[4] || fieldStates[5];
 	bool hasIntensity = fieldStates[7];
 	bool hasRGB = !hasIntensity&&fieldStates[6];
+	bool hasTime = hasIntensity&&fieldStates[8];
+	bool hasRing = hasIntensity&&fieldStates[9];
 	bool is3D = fieldStates[0] && fieldStates[1] && fieldStates[2];
 
 	LaserScan::Format format;
@@ -140,7 +178,18 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
 		}
 		else if(!hasNormals && hasIntensity)
 		{
-			format = LaserScan::kXYZI;
+			if(hasTime && hasRing)
+			{
+				format = LaserScan::kXYZIRT;
+			}
+			else if(hasTime)
+			{
+				format = LaserScan::kXYZIT;
+			}
+			else
+			{
+				format = LaserScan::kXYZI;
+			}
 		}
 		else if(!hasNormals && hasRGB)
 		{
@@ -183,6 +232,7 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
 		transformRot = transform.rotation();
 	}
 	int oi=0;
+	UASSERT(cloud.height == 1 || cloud.row_step != 0);
 	for (uint32_t row = 0; row < (uint32_t)cloud.height; ++row)
 	{
 		const uint8_t* row_data = &cloud.data[row * cloud.row_step];
@@ -242,26 +292,45 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
 			{
 				ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
 				ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
-				ptr[2] = *(float*)(msg_data + fieldOffsets[3]);
-				ptr[3] = *(float*)(msg_data + fieldOffsets[4]);
-				ptr[4] = *(float*)(msg_data + fieldOffsets[5]);
+				if(format == LaserScan::kXYZIT)
+				{
+					ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
+					ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
+					ptr[4] = *(float*)(msg_data + fieldOffsets[8]);
+				}
+				else // kXYNormal
+				{
+					ptr[2] = *(float*)(msg_data + fieldOffsets[3]);
+					ptr[3] = *(float*)(msg_data + fieldOffsets[4]);
+					ptr[4] = *(float*)(msg_data + fieldOffsets[5]);
+				}
 				valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]);
 			}
 			else if(laserScan.channels() == 6)
 			{
 				ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
 				ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
-				if(format == LaserScan::kXYINormal)
-				{
-					ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
-				}
-				else // XYZNormal
+				if(format == LaserScan::kXYZIRT)
 				{
 					ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
+					ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
+					ptr[4] = float(*(unsigned short*)(msg_data + fieldOffsets[9])); // Convert 16U to float
+					ptr[5] = *(float*)(msg_data + fieldOffsets[8]);
+				}
+				else // with normal
+				{
+					if(format == LaserScan::kXYINormal)
+					{
+						ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
+					}
+					else // XYZNormal
+					{
+						ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
+					}
+					ptr[3] = *(float*)(msg_data + fieldOffsets[3]);
+					ptr[4] = *(float*)(msg_data + fieldOffsets[4]);
+					ptr[5] = *(float*)(msg_data + fieldOffsets[5]);
 				}
-				ptr[3] = *(float*)(msg_data + fieldOffsets[3]);
-				ptr[4] = *(float*)(msg_data + fieldOffsets[4]);
-				ptr[5] = *(float*)(msg_data + fieldOffsets[5]);
 				valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]) &&  uIsFinite(ptr[5]);
 			}
 			else if(laserScan.channels() == 7)

corelib/include/rtabmap/core/odometry/OdometryLIOSAM.h

@@ -0,0 +1,82 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef ODOMETRYLIOSAM_H_
+#define ODOMETRYLIOSAM_H_
+
+#include <rtabmap/core/Odometry.h>
+
+#ifdef RTABMAP_LIOSAM
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <vector>
+namespace lio_sam { class LioSamCore; }
+#endif
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT OdometryLIOSAM : public Odometry
+{
+public:
+	OdometryLIOSAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryLIOSAM();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeLIOSAM;}
+	virtual bool canProcessAsyncIMU() const {return true;}
+
+private:
+	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+#ifdef RTABMAP_LIOSAM
+	bool init(const Transform & imuLocalTransform, const Transform & lidarLocalTransform);
+#endif
+
+private:
+#ifdef RTABMAP_LIOSAM
+	lio_sam::LioSamCore * lioSam_;
+	Transform lastPose_;
+	bool lost_;
+	float linVar_;
+	float angVar_;
+	ParametersMap parameters_;
+	Transform imuLocalTransform_; // base_link -> imu_link (cached for deferred init)
+
+	// Buffered IMU samples received before initialization
+	struct ImuSample {
+		double stamp;
+		Eigen::Vector3d acc;
+		Eigen::Vector3d gyro;
+		Eigen::Quaterniond orientation;
+	};
+	std::vector<ImuSample> imuBuffer_;
+#endif
+};
+
+}
+
+#endif /* ODOMETRYLIOSAM_H_ */

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

@@ -41,6 +41,12 @@ public:
 	static bool isCSparseAvailable();
 	static bool isCholmodAvailable();
 
+public:
+	static bool loadGraph(
+			const std::string & fileName,
+			std::map<int, Transform> & poses,
+			std::multimap<int, Link> & edgeConstraints);
+
 public:
 	OptimizerG2O(const ParametersMap & parameters = ParametersMap());
 	virtual ~OptimizerG2O() {}

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

@@ -77,6 +77,7 @@ private:
 	std::vector<ConstraintToFactor> lastAddedConstraints_;
 	int lastSwitchId_;
 	std::set<int> addedPoses_;
+	std::map<int, bool> isLandmarkWithRotation_; // persists across iSAM2 incremental calls
 	std::pair<int, std::uint64_t> lastRootFactorIndex_;
 };
 

corelib/include/rtabmap/core/util3d.h

@@ -39,12 +39,26 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Parameters.h>
 #include <opencv2/core/core.hpp>
 #include <rtabmap/core/ProgressState.h>
+#include <cstdint>
 #include <map>
 #include <list>
 
 namespace rtabmap
 {
 
+// Point type carrying xyz + intensity + ring (laser line index) + time
+// (per-point acquisition offset, seconds from the scan start). Matches the
+// layout expected by LIO-SAM's Velodyne feature extractor so it can be fed
+// directly via util3d::laserScanFromPointCloud().
+struct EIGEN_ALIGN16 PointXYZIRT
+{
+	PCL_ADD_POINT4D;
+	float intensity;
+	std::uint16_t ring;
+	float time;
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
 namespace util3d
 {
 
@@ -297,6 +311,9 @@ LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl:
 // return CV_32FC4 (x,y,z,I)
 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
+// return CV_32FC6 (x,y,z,I,ring,time)
+LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<rtabmap::PointXYZIRT> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<rtabmap::PointXYZIRT> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
 // return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)
 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
@@ -512,6 +529,15 @@ LaserScan RTABMAP_CORE_EXPORT deskew(
 } // namespace util3d
 } // namespace rtabmap
 
+POINT_CLOUD_REGISTER_POINT_STRUCT(rtabmap::PointXYZIRT,
+	(float, x, x)
+	(float, y, y)
+	(float, z, z)
+	(float, intensity, intensity)
+	(std::uint16_t, ring, ring)
+	(float, time, time)
+)
+
 #include "rtabmap/core/impl/util3d.hpp"
 
 #endif /* UTIL3D_H_ */

corelib/src/CMakeLists.txt

@@ -98,6 +98,7 @@ SET(SRC_FILES
     odometry/OdometryORBSLAM3.cpp
     odometry/OdometryLOAM.cpp
     odometry/OdometryFLOAM.cpp
+    odometry/OdometryLIOSAM.cpp
     odometry/OdometryMSCKF.cpp
     odometry/OdometryVINSFusion.cpp
     odometry/OdometryOpenVINS.cpp
@@ -532,6 +533,13 @@ IF(FastCV_FOUND)
 	)
 ENDIF(FastCV_FOUND)
 
+IF(apriltag_FOUND)
+	SET(LIBRARIES
+		apriltag::apriltag
+		${LIBRARIES}
+	)
+ENDIF(apriltag_FOUND)
+
 IF(opengv_FOUND)
     SET(LIBRARIES
         ${LIBRARIES}
@@ -604,6 +612,18 @@ IF(floam_FOUND)
     )
 ENDIF(floam_FOUND)
 
+IF(lio_sam_FOUND)
+    SET(INCLUDE_DIRS
+        ${INCLUDE_DIRS}
+        ${lio_sam_INCLUDE_DIRS}
+    )
+    link_directories(${lio_sam_LIBRARY_DIRS})
+    SET(LIBRARIES
+        ${LIBRARIES}
+        lio_sam_core
+    )
+ENDIF(lio_sam_FOUND)
+
 IF(ZED_FOUND)
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}
@@ -761,16 +781,16 @@ IF(vins_FOUND)
 	)
 ENDIF(vins_FOUND)
 
-IF(ov_msckf_FOUND)
+IF(OpenVINS_FOUND)
     SET(INCLUDE_DIRS
-		${ov_msckf_INCLUDE_DIRS}
+		${OpenVINS_INCLUDE_DIRS}
 		${INCLUDE_DIRS}
 	)
 	SET(LIBRARIES
-		${ov_msckf_LIBRARIES}
+		${OpenVINS_LIBRARIES}
 		${LIBRARIES}
 	)
-ENDIF(ov_msckf_FOUND)
+ENDIF(OpenVINS_FOUND)
 
 IF(ORB_SLAM_FOUND)
     SET(INCLUDE_DIRS
@@ -791,10 +811,6 @@ IF(CUVSLAM_FOUND)
 ENDIF(CUVSLAM_FOUND)
 
 IF(GTSAM_FOUND)
-	SET(SRC_FILES
-    	${SRC_FILES}
-		optimizer/gtsam/GravityFactor.cpp
-	)
 	SET(LIBRARIES
 		${LIBRARIES}
 		gtsam

corelib/src/DBReader.cpp

@@ -57,6 +57,7 @@ DBReader::DBReader(const std::string & databasePath,
 				   int stopMapId,
 				   bool priorsIgnored,
 				   bool imuIgnored,
+				   bool intermediateNodesAreNormalNodes,
 				   const std::vector<Transform> & cameraLocalTransformOverrides) :
 	Camera(frameRate),
 	_paths(uSplit(databasePath, ';')),
@@ -67,6 +68,7 @@ DBReader::DBReader(const std::string & databasePath,
 	_stopId(stopId),
 	_cameraIndices(cameraIndices),
 	_intermediateNodesIgnored(intermediateNodesIgnored),
+	_intermediateNodesAreNormalNodes(intermediateNodesAreNormalNodes),
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
 	_priorsIgnored(priorsIgnored),
@@ -99,6 +101,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   int stopMapId,
 				   bool priorsIgnored,
 				   bool imuIgnored,
+				   bool intermediateNodesAreNormalNodes,
 				   const std::vector<Transform> & cameraLocalTransformOverrides) :
 	Camera(frameRate),
    _paths(databasePaths),
@@ -109,6 +112,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_stopId(stopId),
 	_cameraIndices(cameraIndices),
 	_intermediateNodesIgnored(intermediateNodesIgnored),
+	_intermediateNodesAreNormalNodes(intermediateNodesAreNormalNodes),
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
 	_priorsIgnored(priorsIgnored),
@@ -749,7 +753,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 					data.setStereoCameraModels(combinedStereoModels);
 				}
 			}
-			data.setId(seq);
+			data.setId(!_intermediateNodesAreNormalNodes && s->getWeight()==-1 ? -1 : seq);
 			data.setStamp(s->getStamp());
 			data.setGroundTruth(s->getGroundTruthPose());
 			if(!globalPose.isNull())

corelib/src/Features2d.cpp

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

corelib/src/Graph.cpp

@@ -218,7 +218,14 @@ bool importPoses(
 	else if(format == 4) // g2o
 	{
 		std::multimap<int, Link> constraintsTmp;
-		UERROR("Cannot import from g2o format because it is not yet supported!");
+		if(OptimizerG2O::loadGraph(filePath, poses, constraintsTmp))
+		{
+			if(constraints)
+			{
+				*constraints = constraintsTmp;
+			}
+			return true;
+		}
 		return false;
 	}
 	else
@@ -927,21 +934,12 @@ Transform calcRMSE (
 	return t;
 }
 
-void computeMaxGraphErrors(
+MaxGraphErrors computeMaxGraphErrors(
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
-		float & maxLinearErrorRatio,
-		float & maxAngularErrorRatio,
-		float & maxLinearError,
-		float & maxAngularError,
-		const Link ** maxLinearErrorLink,
-		const Link ** maxAngularErrorLink,
 		bool force3DoF)
 {
-	maxLinearErrorRatio = -1;
-	maxAngularErrorRatio = -1;
-	maxLinearError = -1;
-	maxAngularError = -1;
+	MaxGraphErrors maxError;
 
 	UDEBUG("poses=%d links=%d", (int)poses.size(), (int)links.size());
 	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
@@ -963,19 +961,7 @@ void computeMaxGraphErrors(
 					iter->second.to(),
 					t2.prettyPrint().c_str());
 
-				if(maxLinearErrorLink)
-				{
-					*maxLinearErrorLink = 0;
-				}
-				if(maxAngularErrorLink)
-				{
-					*maxAngularErrorLink = 0;
-				}
-				maxLinearErrorRatio = -1;
-				maxAngularErrorRatio = -1;
-				maxLinearError = -1;
-				maxAngularError = -1;
-				return;
+				return MaxGraphErrors();
 			}
 
 			Transform t;
@@ -999,14 +985,11 @@ void computeMaxGraphErrors(
 			UASSERT(iter->second.transVariance(false)>0.0);
 			float stddevLinear = sqrt(iter->second.transVariance(false));
 			float linearErrorRatio = linearError/stddevLinear;
-			if(linearErrorRatio > maxLinearErrorRatio)
+			if(linearErrorRatio > maxError.linearRatio)
 			{
-				maxLinearError = linearError;
-				maxLinearErrorRatio = linearErrorRatio;
-				if(maxLinearErrorLink)
-				{
-					*maxLinearErrorLink = &iter->second;
-				}
+				maxError.linear = linearError;
+				maxError.linearRatio = linearErrorRatio;
+				maxError.linearLink = iter->second;
 			}
 
 			// For landmark links, don't compute angular error if it doesn't estimate orientation
@@ -1031,18 +1014,16 @@ void computeMaxGraphErrors(
 				UASSERT(iter->second.rotVariance(false)>0.0);
 				float stddevAngular = sqrt(iter->second.rotVariance(false));
 				float angularErrorRatio = angularError/stddevAngular;
-				if(angularErrorRatio > maxAngularErrorRatio)
+				if(angularErrorRatio > maxError.angularRatio)
 				{
-					maxAngularError = angularError;
-					maxAngularErrorRatio = angularErrorRatio;
-					if(maxAngularErrorLink)
-					{
-						*maxAngularErrorLink = &iter->second;
-					}
+					maxError.angular = angularError;
+					maxError.angularRatio = angularErrorRatio;
+					maxError.angularLink = iter->second;
 				}
 			}
 		}
 	}
+	return maxError;
 }
 
 std::vector<double> getMaxOdomInf(const std::multimap<int, Link> & links)

corelib/src/LaserScan.cpp

@@ -68,6 +68,9 @@ std::string LaserScan::formatName(const Format & format)
 		case kXYZIT:
 			name = "XYZIT";
 			break;
+		case kXYZIRT:
+			name = "XYZIRT";
+			break;
 		default:
 			name = "Unknown";
 			break;
@@ -96,6 +99,7 @@ int LaserScan::channels(const Format & format)
 			break;
 		case kXYZNormal:
 		case kXYINormal:
+		case kXYZIRT:
 			channels = 6;
 			break;
 		case kXYZINormal:
@@ -123,11 +127,15 @@ bool LaserScan::isScanHasRGB(const Format & format)
 }
 bool LaserScan::isScanHasIntensity(const Format & format)
 {
-	return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal || format==kXYZIT;
+	return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal || format==kXYZIT || format==kXYZIRT;
 }
 bool LaserScan::isScanHasTime(const Format & format)
 {
-	return format==kXYZIT;
+	return format==kXYZIT || format==kXYZIRT;
+}
+bool LaserScan::isScanHasRing(const Format & format)
+{
+	return format==kXYZIRT;
 }
 
 LaserScan LaserScan::backwardCompatibility(
@@ -404,7 +412,7 @@ void LaserScan::init(
 			UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 			UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 			UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal || format == kXYZIT)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
-			UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
+			UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal || format == kXYZIRT)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 			UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 		}
 	}

corelib/src/Odometry.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/odometry/OdometryORBSLAM3.h"
 #include "rtabmap/core/odometry/OdometryLOAM.h"
 #include "rtabmap/core/odometry/OdometryFLOAM.h"
+#include "rtabmap/core/odometry/OdometryLIOSAM.h"
 #include "rtabmap/core/odometry/OdometryMSCKF.h"
 #include "rtabmap/core/odometry/OdometryVINSFusion.h"
 #include "rtabmap/core/odometry/OdometryOpenVINS.h"
@@ -101,6 +102,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeFLOAM:
 		odometry = new OdometryFLOAM(parameters);
 		break;
+	case Odometry::kTypeLIOSAM:
+		odometry = new OdometryLIOSAM(parameters);
+		break;
 	case Odometry::kTypeMSCKF:
 		odometry = new OdometryMSCKF(parameters);
 		break;
@@ -653,7 +657,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			UWARN("Could not find imu transform at %f", data.stamp());
 		}
 	}
-	else if(!guess.isNull()) {
+	else if(!guess.isNull() && (!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())) {
 		UDEBUG("Using guess from motion %s", guess.prettyPrint().c_str());
 	}
 

corelib/src/Optimizer.cpp

@@ -240,7 +240,8 @@ void Optimizer::getConnectedGraph(
 {
 	UDEBUG("IN: fromId=%d poses=%d links=%d priorsIgnored=%d landmarksIgnored=%d", fromId, (int)posesIn.size(), (int)linksIn.size(), priorsIgnored()?1:0, landmarksIgnored()?1:0);
 	UASSERT(fromId>0);
-	UASSERT(uContains(posesIn, fromId));
+	UASSERT_MSG(uContains(posesIn, fromId), uFormat("poses=%ld (first=%d last=%d) fromId=%d",
+		posesIn.size(), posesIn.empty()?0:posesIn.begin()->first, posesIn.empty()?0:posesIn.rbegin()->first, fromId).c_str());
 
 	posesOut.clear();
 	linksOut.clear();

corelib/src/Parameters.cpp

@@ -113,11 +113,14 @@ ParametersMap Parameters::deserialize(const std::string & parameters)
 	std::list<std::string> tuplets = uSplit(parameters, ';');
 	for(std::list<std::string>::iterator iter=tuplets.begin(); iter!=tuplets.end(); ++iter)
 	{
-		std::list<std::string> p = uSplit(*iter, ':');
-		if(p.size() == 2)
+		// Split on the FIRST ':' only. Using uSplit() here would discard
+		// empty tokens, so a tuplet like "Marker/Lengths:" (legitimate empty
+		// string value) would lose the value side and be dropped entirely.
+		size_t colonPos = iter->find(':');
+		if(colonPos != std::string::npos && colonPos > 0)
 		{
-			std::string key = p.front();
-			std::string value = p.back();
+			std::string key = iter->substr(0, colonPos);
+			std::string value = iter->substr(colonPos + 1);
 
 			// look for old parameter name
 			bool addParameter = true;
@@ -238,6 +241,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.23.7
+		removedParameters_.insert(std::make_pair("Marker/CornerRefinementMethod", std::make_pair(true, Parameters::kMarkerOpenCVCornerRefinementMethod())));
+
 		// 0.23.1
 		removedParameters_.insert(std::make_pair("OdomVINS/ConfigPath",    std::make_pair(true, Parameters::kOdomVINSFusionConfigPath())));
 
@@ -290,7 +296,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 		removedParameters_.insert(std::make_pair("Aruco/MaxDepthError",          std::make_pair(true,  Parameters::kMarkerMaxDepthError())));
 		removedParameters_.insert(std::make_pair("Aruco/VarianceLinear",         std::make_pair(true,  Parameters::kMarkerVarianceLinear())));
 		removedParameters_.insert(std::make_pair("Aruco/VarianceAngular",        std::make_pair(true,  Parameters::kMarkerVarianceAngular())));
-		removedParameters_.insert(std::make_pair("Aruco/CornerRefinementMethod", std::make_pair(true,  Parameters::kMarkerCornerRefinementMethod())));
+		removedParameters_.insert(std::make_pair("Aruco/CornerRefinementMethod", std::make_pair(true,  Parameters::kMarkerOpenCVCornerRefinementMethod())));
 
 		// 0.17.5
 		removedParameters_.insert(std::make_pair("Grid/OctoMapOccupancyThr",     std::make_pair(true,  Parameters::kGridGlobalOccupancyThr())));
@@ -681,6 +687,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With AprilTag:";
+#ifdef RTABMAP_APRILTAG
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With OpenGV:";
 #ifdef RTABMAP_OPENGV
@@ -903,6 +915,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With LIO-SAM:";
+#ifdef RTABMAP_LIOSAM
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With FOVIS:";
 #ifdef RTABMAP_FOVIS
@@ -1227,13 +1245,13 @@ void readINIImpl(const CSimpleIniA & ini, const std::string & configFilePath, Pa
 				std::vector<std::string> version = uListToVector(uSplit((*iter).second, '.'));
 				if(version.size() == 3)
 				{
-					if(!RTABMAP_VERSION_COMPARE(std::atoi(version[0].c_str()), std::atoi(version[1].c_str()), std::atoi(version[2].c_str())))
+					if(RTABMAP_VERSION_COMPARE(<, std::atoi(version[0].c_str()), std::atoi(version[1].c_str()), std::atoi(version[2].c_str())))
 					{
 						if(configFilePath.find(".rtabmap") != std::string::npos)
 						{
 							UWARN("Version in the config file \"%s\" is more recent (\"%s\") than "
-								   "current RTAB-Map version used (\"%s\"). The config file will be upgraded "
-								   "to new version.",
+								   "current RTAB-Map version used (\"%s\"). The config file will be downgraded "
+								   "to current RTAB-Map version if saved.",
 								   configFilePath.c_str(),
 								   (*iter).second,
 								   RTABMAP_VERSION);

corelib/src/RtabmapThread.cpp

@@ -47,8 +47,7 @@ RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
 		_dataBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
 		_rate(Parameters::defaultRtabmapDetectionRate()),
 		_createIntermediateNodes(Parameters::defaultRtabmapCreateIntermediateNodes()),
-		_frameRateTimer(new UTimer()),
-		_previousStamp(0.0),
+		_previousStamp(-1.0),
 		_rtabmap(rtabmap),
 		_paused(false),
 		lastPose_(Transform::getIdentity())
@@ -62,8 +61,6 @@ RtabmapThread::~RtabmapThread()
 	UEventsManager::removeHandler(this);
 
 	close(true);
-
-	delete _frameRateTimer;
 }
 
 void RtabmapThread::pushNewState(State newState, const RtabmapEventCmd & cmdEvent)
@@ -88,7 +85,7 @@ void RtabmapThread::clearBufferedData()
 		_newMapEvents.clear();
 		lastPose_.setIdentity();
 		covariance_ = cv::Mat();
-		_previousStamp = 0;
+		_previousStamp = -1;
 	}
 	_dataMutex.unlock();
 
@@ -500,9 +497,10 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		bool ignoreFrame = false;
 		if(_rate>0.0f)
 		{
-			if((_previousStamp>=0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate) ||
-				((_previousStamp<=0.0 || odomEvent.data().stamp()<=_previousStamp) && _frameRateTimer->getElapsedTime() < 1.0f/_rate))
+			if((_previousStamp>=0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate))
 			{
+				UDEBUG("Ignoring frame %f (previous stamp=%f, period=%f)",
+					odomEvent.data().stamp(), _previousStamp, 1.0/_rate);
 				ignoreFrame = true;
 			}
 		}
@@ -540,7 +538,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		}
 		else if(!ignoreFrame)
 		{
-			_frameRateTimer->start();
 			_previousStamp = odomEvent.data().stamp();
 		}
 
@@ -559,7 +556,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			// set negative id so rtabmap will detect it as an intermediate node
 			SensorData tmp = odomEvent.data();
 			tmp.setId(-1);
-			tmp.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
 			_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), odomInfo));
 		}
 		else

corelib/src/SensorData.cpp

@@ -976,7 +976,7 @@ unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 			(_descriptors.empty()?0:_descriptors.total()*_descriptors.elemSize());
 }
 
-void SensorData::clearCompressedData(bool images, bool scan, bool userData)
+void SensorData::clearCompressedData(bool images, bool scan, bool userData, bool occupancyGrid)
 {
 	if(images)
 	{
@@ -992,14 +992,32 @@ void SensorData::clearCompressedData(bool images, bool scan, bool userData)
 	{
 		_userDataCompressed=cv::Mat();
 	}
+	if(occupancyGrid)
+	{
+		_groundCellsCompressed=cv::Mat();
+		_emptyCellsCompressed=cv::Mat();
+		_obstacleCellsCompressed=cv::Mat();
+		
+		if( _groundCellsCompressed.empty()  && _groundCellsRaw.empty() &&
+			_obstacleCellsCompressed.empty()  && _obstacleCellsRaw.empty() &&
+			_emptyCellsCompressed.empty()  && _emptyCellsRaw.empty())
+		{
+			_cellSize = 0.0f;
+			_viewPoint = cv::Point3f();
+		}
+	}
 }
-void SensorData::clearRawData(bool images, bool scan, bool userData)
+void SensorData::clearRawData(bool images, bool scan, bool userData, bool occupancyGrid)
 {
 	if(images)
 	{
 		_imageRaw=cv::Mat();
 		_depthOrRightRaw=cv::Mat();
 		_depthConfidenceRaw=cv::Mat();
+#ifdef HAVE_OPENCV_CUDEV
+		_imageRawGpu = cv::cuda::GpuMat();
+		_depthOrRightRawGpu = cv::cuda::GpuMat();
+#endif
 	}
 	if(scan)
 	{
@@ -1009,6 +1027,20 @@ void SensorData::clearRawData(bool images, bool scan, bool userData)
 	{
 		_userDataRaw=cv::Mat();
 	}
+	if(occupancyGrid)
+	{
+		_groundCellsRaw=cv::Mat();
+		_emptyCellsRaw=cv::Mat();
+		_obstacleCellsRaw=cv::Mat();
+
+		if( _groundCellsCompressed.empty()  && _groundCellsRaw.empty() &&
+			_obstacleCellsCompressed.empty()  && _obstacleCellsRaw.empty() &&
+			_emptyCellsCompressed.empty()  && _emptyCellsRaw.empty())
+		{
+			_cellSize = 0.0f;
+			_viewPoint = cv::Point3f();
+		}
+	}
 }
 
 

corelib/src/global_map/OccupancyGrid.cpp

@@ -222,14 +222,14 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
 
 	if(!cache().empty())
 	{
-		UDEBUG("Updating from cache");
+		UDEBUG("Updating %ld poses from cache", newPoses.size());
 		for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
 		{
 			if(uContains(cache(), iter->first))
 			{
 				const LocalGrid & localGrid = cache().at(iter->first);
 
-				UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
+				//UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
 
 				//ground
 				cv::Mat ground;

corelib/src/odometry/OdometryLIOSAM.cpp

@@ -0,0 +1,478 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/odometry/OdometryLIOSAM.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
+
+#ifdef RTABMAP_LIOSAM
+#include <LioSamCore.h>
+#include <pcl/common/transforms.h>
+#endif
+
+namespace rtabmap {
+
+static ParametersMap disableDeskewing(ParametersMap params) {
+	// LIO-SAM performs its own internal deskewing via imageProjection.
+	// The base-class deskew must be disabled so that the original per-point
+	// timestamps reach LIO-SAM intact.
+	params[Parameters::kOdomDeskewing()] = "false";
+	return params;
+}
+
+OdometryLIOSAM::OdometryLIOSAM(const ParametersMap & parameters) :
+	Odometry(disableDeskewing(parameters))
+#ifdef RTABMAP_LIOSAM
+	,lioSam_(0)
+	,lastPose_(Transform::getIdentity())
+	,lost_(false)
+	,linVar_(Parameters::defaultOdomLIOSAMLinVar())
+	,angVar_(Parameters::defaultOdomLIOSAMAngVar())
+	,parameters_(parameters)
+#endif
+{
+#ifdef RTABMAP_LIOSAM
+	Parameters::parse(parameters, Parameters::kOdomLIOSAMLinVar(), linVar_);
+	UASSERT(linVar_ > 0.0f);
+	Parameters::parse(parameters, Parameters::kOdomLIOSAMAngVar(), angVar_);
+	UASSERT(angVar_ > 0.0f);
+#endif
+}
+
+OdometryLIOSAM::~OdometryLIOSAM()
+{
+#ifdef RTABMAP_LIOSAM
+	delete lioSam_;
+#endif
+}
+
+void OdometryLIOSAM::reset(const Transform & initialPose)
+{
+	Odometry::reset(initialPose);
+#ifdef RTABMAP_LIOSAM
+	if(lioSam_)
+	{
+		lioSam_->reset();
+	}
+	lastPose_ = Transform::getIdentity();
+	lost_ = false;
+	imuLocalTransform_ = Transform();
+	imuBuffer_.clear();
+#endif
+}
+
+#ifdef RTABMAP_LIOSAM
+bool OdometryLIOSAM::init(const Transform & imuLocalTransform, const Transform & lidarLocalTransform)
+{
+	ParamServer config;
+
+	// Check if a config file path was provided
+	std::string configPath;
+	Parameters::parse(parameters_, Parameters::kOdomLIOSAMConfigPath(), configPath);
+	if(!configPath.empty())
+	{
+		configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
+		if(!UFile::exists(configPath))
+		{
+			UERROR("LIO-SAM config file not found: %s", configPath.c_str());
+			return false;
+		}
+		UINFO("Loading LIO-SAM parameters from config file: %s", configPath.c_str());
+		config = loadParamsFromYaml(configPath);
+	}
+	else
+	{
+		UINFO("No LIO-SAM config file provided, using rtabmap parameters");
+
+		// Build ParamServer from individual rtabmap parameters
+		int sensorType = Parameters::defaultOdomLIOSAMSensor();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMSensor(), sensorType);
+		if(sensorType == 1)
+			config.sensor = SensorType::OUSTER;
+		else if(sensorType == 2)
+			config.sensor = SensorType::LIVOX;
+		else
+			config.sensor = SensorType::VELODYNE;
+
+		config.N_SCAN = Parameters::defaultOdomLIOSAMNScan();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMNScan(), config.N_SCAN);
+
+		config.Horizon_SCAN = Parameters::defaultOdomLIOSAMHorizonScan();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMHorizonScan(), config.Horizon_SCAN);
+
+		config.imuAccNoise = Parameters::defaultOdomLIOSAMImuAccNoise();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuAccNoise(), config.imuAccNoise);
+
+		config.imuGyrNoise = Parameters::defaultOdomLIOSAMImuGyrNoise();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuGyrNoise(), config.imuGyrNoise);
+
+		config.imuAccBiasN = Parameters::defaultOdomLIOSAMImuAccBiasN();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuAccBiasN(), config.imuAccBiasN);
+
+		config.imuGyrBiasN = Parameters::defaultOdomLIOSAMImuGyrBiasN();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuGyrBiasN(), config.imuGyrBiasN);
+
+		config.imuGravity = Parameters::defaultOdomLIOSAMImuGravity();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuGravity(), config.imuGravity);
+
+		config.edgeThreshold = Parameters::defaultOdomLIOSAMEdgeThreshold();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMEdgeThreshold(), config.edgeThreshold);
+
+		config.surfThreshold = Parameters::defaultOdomLIOSAMSurfThreshold();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMSurfThreshold(), config.surfThreshold);
+
+		// Set reasonable defaults for params not exposed via rtabmap
+		config.downsampleRate = 1;
+		config.lidarMinRange = 1.0f;
+		config.lidarMaxRange = 1000.0f;
+		config.imuRPYWeight = 0.01f;
+		config.odometrySurfLeafSize = 0.2f;
+		config.mappingCornerLeafSize = 0.2f;
+		config.mappingSurfLeafSize = 0.4f;
+		config.z_tollerance = FLT_MAX;
+		config.rotation_tollerance = FLT_MAX;
+		config.numberOfCores = 4;
+		config.mappingProcessInterval = 0.01;
+		config.surroundingkeyframeAddingDistThreshold = 1.0f;
+		config.surroundingkeyframeAddingAngleThreshold = 0.2f;
+		config.surroundingKeyframeDensity = 1.0f;
+		config.surroundingKeyframeSearchRadius = 50.0f;
+		config.loopClosureEnableFlag = false;  // rtabmap handles loop closures
+		config.loopClosureFrequency = 1.0f;
+		config.surroundingKeyframeSize = 50;
+		config.historyKeyframeSearchRadius = 10.0f;
+		config.historyKeyframeSearchTimeDiff = 30.0f;
+		config.historyKeyframeSearchNum = 25;
+		config.historyKeyframeFitnessScore = 0.3f;
+		config.globalMapVisualizationSearchRadius = 1e3f;
+		config.globalMapVisualizationPoseDensity = 10.0f;
+		config.globalMapVisualizationLeafSize = 1.0f;
+		config.edgeFeatureMinValidNum = 10;
+		config.surfFeatureMinValidNum = 100;
+		config.savePCD = false;
+		config.useImuHeadingInitialization = false;
+		config.useGpsElevation = false;
+		config.gpsCovThreshold = 2.0f;
+		config.poseCovThreshold = 25.0f;
+	}
+
+	// Always override extrinsics from sensor local transforms when available.
+	// This ensures the IMU-to-lidar transform matches the actual sensor setup
+	// regardless of what the config file says.
+	//   imuLocalTransform   = T_base_imu   (base_link -> imu_link)
+	//   lidarLocalTransform = T_base_lidar  (base_link -> lidar_link)
+	// LIO-SAM's imuConverter() expects T_lidar_imu:
+	//   T_lidar_imu = T_base_lidar^{-1} * T_base_imu
+	if(!imuLocalTransform.isNull() && !lidarLocalTransform.isNull())
+	{
+		Transform T_lidar_imu = lidarLocalTransform.inverse() * imuLocalTransform;
+		Eigen::Matrix4d T = T_lidar_imu.toEigen4d();
+		Eigen::Matrix3d rot = T.block<3,3>(0,0);
+		Eigen::Vector3d trans = T.block<3,1>(0,3);
+		config.extRotV = {rot(0,0), rot(0,1), rot(0,2),
+		                  rot(1,0), rot(1,1), rot(1,2),
+		                  rot(2,0), rot(2,1), rot(2,2)};
+		config.extRPYV = config.extRotV;
+		config.extTransV = {trans(0), trans(1), trans(2)};
+		UINFO("LIO-SAM extrinsics (T_lidar_imu) computed from sensor local transforms: %s", T_lidar_imu.prettyPrint().c_str());
+	}
+	else if(config.extRotV.size() != 9 || config.extTransV.size() != 3)
+	{
+		// No valid extrinsics from sensor data or config file
+		UERROR("Cannot compute IMU-to-lidar extrinsics: IMU local transform %s, lidar local transform %s. "
+		       "Both must be valid, or the config file must contain valid extrinsics.",
+		       imuLocalTransform.isNull() ? "is null" : "is valid",
+		       lidarLocalTransform.isNull() ? "is null" : "is valid");
+		return false;
+	}
+	else
+	{
+		UINFO("Using extrinsics from config file (sensor local transforms not available)");
+	}
+
+	// Set the global extrinsics used by imuConverter
+	extRot = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> >(config.extRotV.data());
+	extRPY = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> >(config.extRPYV.data());
+	extTrans = Eigen::Map<const Eigen::Matrix<double, 3, 1> >(config.extTransV.data());
+	extQRPY = Eigen::Quaterniond(extRPY).inverse();
+
+	lioSam_ = new lio_sam::LioSamCore(config);
+
+	// Replay buffered IMU samples
+	UINFO("Replaying %d buffered IMU samples into LIO-SAM", (int)imuBuffer_.size());
+	for(const ImuSample & s : imuBuffer_)
+	{
+		lioSam_->addImu(s.stamp, s.acc, s.gyro, s.orientation);
+	}
+	imuBuffer_.clear();
+
+	return true;
+}
+#endif
+
+Transform OdometryLIOSAM::computeTransform(
+		SensorData & data,
+		const Transform & guess,
+		OdometryInfo * info)
+{
+	Transform t;
+#ifdef RTABMAP_LIOSAM
+	UTimer timer;
+	UTimer timerTotal;
+
+	// Handle async IMU data (canProcessAsyncIMU() == true means
+	// the base class sends IMU-only data directly to computeTransform)
+	if(!data.imu().empty())
+	{
+		Eigen::Quaterniond qd(
+			data.imu().orientation()[3],  // w
+			data.imu().orientation()[0],  // x
+			data.imu().orientation()[1],  // y
+			data.imu().orientation()[2]); // z
+		Eigen::Vector3d acc(
+			data.imu().linearAcceleration()[0],
+			data.imu().linearAcceleration()[1],
+			data.imu().linearAcceleration()[2]);
+		Eigen::Vector3d gyro(
+			data.imu().angularVelocity()[0],
+			data.imu().angularVelocity()[1],
+			data.imu().angularVelocity()[2]);
+
+		// Deferred initialization: need both IMU and lidar local transforms
+		// to compute T_lidar_imu extrinsics for LIO-SAM.
+		if(!lioSam_)
+		{
+			// Cache IMU local transform when first available
+			if(imuLocalTransform_.isNull() && !data.imu().localTransform().isNull())
+			{
+				imuLocalTransform_ = data.imu().localTransform();
+			}
+
+			// Try to initialize if we have both transforms
+			if(!imuLocalTransform_.isNull() && !data.laserScanRaw().isEmpty() &&
+			   !data.laserScanRaw().localTransform().isNull())
+			{
+				if(!init(imuLocalTransform_, data.laserScanRaw().localTransform()))
+				{
+					UERROR("Failed to initialize LIO-SAM");
+					return t;
+				}
+			}
+			else
+			{
+				// Buffer IMU until we can initialize
+				ImuSample s;
+				s.stamp = data.stamp();
+				s.acc = acc;
+				s.gyro = gyro;
+				s.orientation = qd;
+				imuBuffer_.push_back(s);
+
+				if(data.laserScanRaw().isEmpty())
+				{
+					return t;
+				}
+			}
+		}
+
+		if(lioSam_)
+		{
+			lioSam_->addImu(data.stamp(), acc, gyro, qd);
+		}
+
+		// IMU-only: no pose to return
+		if(data.laserScanRaw().isEmpty())
+		{
+			return t;
+		}
+	}
+
+	if(!lioSam_)
+	{
+		// A scan arrived without IMU in the same message.
+		// Try to init if the IMU local transform was already cached.
+		if(!imuLocalTransform_.isNull() && !data.laserScanRaw().isEmpty() &&
+		   !data.laserScanRaw().localTransform().isNull())
+		{
+			if(!init(imuLocalTransform_, data.laserScanRaw().localTransform()))
+			{
+				UERROR("Failed to initialize LIO-SAM");
+				return t;
+			}
+		}
+		else
+		{
+			UDEBUG("LIO-SAM not yet initialized, waiting for IMU (have=%s) and lidar (need scan) local transforms...",
+				imuLocalTransform_.isNull() ? "no" : "yes");
+			return t;
+		}
+	}
+
+	if(data.laserScanRaw().isEmpty())
+	{
+		UERROR("LIO-SAM requires laser scans and the current input is empty. Aborting odometry update...");
+		return t;
+	}
+	else if(data.laserScanRaw().is2d())
+	{
+		UERROR("LIO-SAM requires 3D laser scans. Aborting odometry update...");
+		return t;
+	}
+
+	cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999;
+	if(!lost_)
+	{
+		const LaserScan & scan = data.laserScanRaw();
+		if(scan.format() != LaserScan::kXYZIRT)
+		{
+			UERROR("LIO-SAM requires a scan in format %s (got %s). "
+			       "Populate the scan via util3d::laserScanFromPointCloud<PointXYZIRT>() "
+			       "so that per-point ring and time fields are available.",
+			       LaserScan::formatName(LaserScan::kXYZIRT).c_str(),
+			       scan.formatName().c_str());
+			return t;
+		}
+
+		// Split the kXYZIRT scan into the three parallel buffers LIO-SAM expects.
+		const int numPoints = scan.size();
+		const int ringOffset = scan.getRingOffset();
+		const int timeOffset = scan.getTimeOffset();
+		pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudIn(new pcl::PointCloud<pcl::PointXYZI>);
+		laserCloudIn->reserve(numPoints);
+		std::vector<int> rings;
+		std::vector<float> times;
+		rings.reserve(numPoints);
+		times.reserve(numPoints);
+		for(int i=0; i<numPoints; ++i)
+		{
+			const int row = i / scan.data().cols;
+			const int col = i - row * scan.data().cols;
+			const float * ptr = scan.data().ptr<float>(row, col);
+			pcl::PointXYZI pt;
+			pt.x = ptr[0];
+			pt.y = ptr[1];
+			pt.z = ptr[2];
+			pt.intensity = ptr[3];
+			laserCloudIn->push_back(pt);
+			rings.push_back(static_cast<int>(ptr[ringOffset]));
+			times.push_back(ptr[timeOffset]);
+		}
+		UDEBUG("Scan split: %fs, points=%d", timer.ticks(), (int)laserCloudIn->size());
+
+		// Process scan. Retrieve the deskewed (motion-compensated) cloud
+		// produced by LIO-SAM's image projection stage so we can propagate
+		// it back into SensorData: otherwise downstream consumers such as
+		// loop closure registration would still see the raw pre-deskew scan.
+		Eigen::Affine3f poseOut;
+		Eigen::MatrixXd covOut;
+		pcl::PointCloud<pcl::PointXYZI>::Ptr deskewedCloud(new pcl::PointCloud<pcl::PointXYZI>);
+		bool ok = lioSam_->processScan(data.stamp(), laserCloudIn, rings, times, poseOut, covOut, deskewedCloud);
+		UDEBUG("LIO-SAM process: %fs", timer.ticks());
+
+		if(ok)
+		{
+			// Replace the raw scan on SensorData with LIO-SAM's deskewed
+			// cloud so downstream stages (loop closure registration in
+			// particular) use the motion-compensated points instead of
+			// the raw pre-deskew scan. The deskewed cloud is still in the
+			// lidar frame, so the existing localTransform/rangeMax apply.
+			if(deskewedCloud && !deskewedCloud->empty())
+			{
+				const LaserScan & rawScan = data.laserScanRaw();
+				LaserScan deskewedScan(
+					util3d::laserScanFromPointCloud(*deskewedCloud),
+					rawScan.maxPoints(),
+					rawScan.rangeMax(),
+					rawScan.localTransform());
+				data.setLaserScan(deskewedScan);
+				UDEBUG("Replaced raw scan with deskewed cloud (%d -> %d points)",
+					(int)laserCloudIn->size(), (int)deskewedCloud->size());
+			}
+
+			Transform pose = Transform::fromEigen3f(poseOut);
+
+			if(!pose.isNull())
+			{
+				covariance = cv::Mat::eye(6, 6, CV_64FC1);
+				covariance(cv::Range(0, 3), cv::Range(0, 3)) *= linVar_;
+				covariance(cv::Range(3, 6), cv::Range(3, 6)) *= angVar_;
+
+				t = lastPose_.inverse() * pose;  // incremental
+				lastPose_ = pose;
+
+				const Transform & localTransform = data.laserScanRaw().localTransform();
+				if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
+				{
+					// from laser frame to base frame
+					t = localTransform * t * localTransform.inverse();
+				}
+
+				if(info)
+				{
+					info->type = (int)kTypeLIOSAM;
+					if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1)
+					{
+						info->reg.covariance = covariance;
+					}
+
+					if(this->isInfoDataFilled())
+					{
+						pcl::PointCloud<pcl::PointXYZI>::Ptr localMap = lioSam_->getLocalMap();
+						if(localMap && !localMap->empty())
+						{
+							info->localScanMapSize = localMap->size();
+							info->localScanMap = LaserScan(util3d::laserScanFromPointCloud(*localMap), 0, data.laserScanRaw().rangeMax(), data.laserScanRaw().localTransform());
+						}
+						UDEBUG("Fill info data: %fs", timer.ticks());
+					}
+				}
+			}
+			else
+			{
+				lost_ = true;
+				UWARN("LIO-SAM failed to register the latest scan, odometry should be reset.");
+			}
+		}
+		else
+		{
+			UDEBUG("LIO-SAM processScan returned false (may be initializing)");
+		}
+	}
+	UINFO("LIO-SAM odom update time = %fs, lost=%s", timerTotal.elapsed(), lost_ ? "true" : "false");
+
+#else
+	UERROR("RTAB-Map is not built with LIO-SAM support! Select another odometry approach.");
+#endif
+	return t;
+}
+
+} // namespace rtabmap

corelib/src/odometry/OdometryMono.cpp

@@ -239,7 +239,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 		{
 			UDEBUG("");
 			bool newPtsAdded = false;
-			const Signature * newS = memory_->getLastWorkingSignature();
+			const Signature * newS = memory_->getLastWorkingSignature(false);
 			UDEBUG("newWords=%d", (int)newS->getWords().size());
 			nFeatures = (int)newS->getWords().size();
 			if((int)newS->getWords().size() > minInliers_)
@@ -646,7 +646,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 			info->type = 1;
 		}
 
-		const Signature * refS = memory_->getLastWorkingSignature();
+		const Signature * refS = memory_->getLastWorkingSignature(false);
 
 		std::vector<cv::Point2f> refCorners(firstFrameGuessCorners_.size());
 		std::vector<cv::Point2f> refCornersGuess(firstFrameGuessCorners_.size());
@@ -804,10 +804,10 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 				if(!refWords3.empty())
 				{
 					UDEBUG("Added %d/%d valid 3D features", (int)refWords3.size(), (int)localMap_.size());
-					keyFrameWords3D_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), refWords3));
+					keyFrameWords3D_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), refWords3));
 				}
-				keyFramePoses_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), this->getPose()));
-				keyFrameModels_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), newModel));
+				keyFramePoses_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), this->getPose()));
+				keyFrameModels_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), newModel));
 			}
 		}
 		else
@@ -829,7 +829,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 			// generate kpts
 		if(memory_->update(SensorData(data)))
 		{
-			const Signature * s = memory_->getLastWorkingSignature();
+			const Signature * s = memory_->getLastWorkingSignature(false);
 			const std::multimap<int, int> & words = s->getWords();
 			if((int)words.size() > minInliers_ && !s->getWordsKpts().empty())
 			{

corelib/src/odometry/OdometryOpenVINS.cpp

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

corelib/src/optimizer/OptimizerG2O.cpp

@@ -2135,6 +2135,390 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 	return optimizedPoses;
 }
 
+bool OptimizerG2O::loadGraph(
+		const std::string & fileName,
+		std::map<int, Transform> & poses,
+		std::multimap<int, Link> & edgeConstraints)
+{
+	FILE * file = 0;
+#ifdef _MSC_VER
+	fopen_s(&file, fileName.c_str(), "r");
+#else
+	file = fopen(fileName.c_str(), "r");
+#endif
+
+	if(!file)
+	{
+		UERROR("Cannot open file %s", fileName.c_str());
+		return false;
+	}
+
+	// saveGraph() writes landmarks (originally negative ids, remapped to
+	// landmarkOffset - id) first in DESCENDING file-id order, then regular
+	// poses in ASCENDING file-id order. We recover landmarkOffset from this
+	// order to restore the original negative landmark ids.
+	struct VertexEntry {
+		int fileId;
+		Transform transform;
+		bool definitelyLandmark; // VERTEX_XY / VERTEX_TRACKXYZ
+	};
+	struct EdgeEntry {
+		int from;
+		int to;
+		Link::Type type;
+		Transform transform;
+		cv::Mat info;
+		bool isPrior;             // from==to, prior on a single vertex
+		bool hasLandmarkEndpoint; // tag implies a landmark on one side
+	};
+	std::vector<VertexEntry> verticesList;
+	std::vector<EdgeEntry> edgesList;
+
+	char line[2048];
+	while(fgets(line, 2048, file) != NULL)
+	{
+		std::list<std::string> tokenList = uSplit(uReplaceChar(uReplaceChar(line, '\n', ' '), '\r', ' '), ' ');
+		std::vector<std::string> v;
+		v.reserve(tokenList.size());
+		for(std::list<std::string>::const_iterator iter = tokenList.begin(); iter != tokenList.end(); ++iter)
+		{
+			if(!iter->empty())
+			{
+				v.push_back(*iter);
+			}
+		}
+		if(v.empty())
+		{
+			continue;
+		}
+		const std::string & tag = v[0];
+
+		// Skip parameters, switch helpers and unrelated entries
+		if(tag == "PARAMS_SE2OFFSET" || tag == "PARAMS_SE3OFFSET" ||
+		   tag == "VERTEX_SWITCH" || tag == "EDGE_SWITCH_PRIOR")
+		{
+			continue;
+		}
+
+		if(tag == "VERTEX_SE2" && v.size() == 5)
+		{
+			VertexEntry e;
+			e.fileId = atoi(v[1].c_str());
+			e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]));
+			e.definitelyLandmark = false;
+			verticesList.push_back(e);
+		}
+		else if(tag == "VERTEX_XY" && v.size() == 4)
+		{
+			VertexEntry e;
+			e.fileId = atoi(v[1].c_str());
+			e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), 0);
+			e.definitelyLandmark = true;
+			verticesList.push_back(e);
+		}
+		else if(tag == "VERTEX_SE3:QUAT" && v.size() == 9)
+		{
+			VertexEntry e;
+			e.fileId = atoi(v[1].c_str());
+			e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]),
+									uStr2Float(v[5]), uStr2Float(v[6]), uStr2Float(v[7]), uStr2Float(v[8]));
+			e.definitelyLandmark = false;
+			verticesList.push_back(e);
+		}
+		else if(tag == "VERTEX_TRACKXYZ" && v.size() == 5)
+		{
+			VertexEntry e;
+			e.fileId = atoi(v[1].c_str());
+			e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]), 0, 0, 0);
+			e.definitelyLandmark = true;
+			verticesList.push_back(e);
+		}
+		else if(tag == "EDGE_SE2" && v.size() == 12)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to   = atoi(v[2].c_str());
+			e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), uStr2Float(v[5]));
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			e.info.at<double>(0, 0) = uStr2Double(v[6]);
+			e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[7]);
+			e.info.at<double>(0, 5) = e.info.at<double>(5, 0) = uStr2Double(v[8]);
+			e.info.at<double>(1, 1) = uStr2Double(v[9]);
+			e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[10]);
+			e.info.at<double>(5, 5) = uStr2Double(v[11]);
+			e.type = Link::kUndef; // disambiguated after we know landmarkOffset
+			e.isPrior = false;
+			e.hasLandmarkEndpoint = false;
+			edgesList.push_back(e);
+		}
+		else if(tag == "EDGE_SE2_XY" && v.size() == 8)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to   = atoi(v[2].c_str());
+			e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), 0);
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			e.info.at<double>(0, 0) = uStr2Double(v[5]);
+			e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[6]);
+			e.info.at<double>(1, 1) = uStr2Double(v[7]);
+			e.type = Link::kLandmark;
+			e.isPrior = false;
+			e.hasLandmarkEndpoint = true;
+			edgesList.push_back(e);
+		}
+		else if((tag == "EDGE_SE3:QUAT" || tag == "EDGE_SE3") && v.size() == 31)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to   = atoi(v[2].c_str());
+			e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), uStr2Float(v[5]),
+									uStr2Float(v[6]), uStr2Float(v[7]), uStr2Float(v[8]), uStr2Float(v[9]));
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			int idx = 10;
+			for(int r = 0; r < 6; ++r)
+			{
+				for(int c = r; c < 6; ++c)
+				{
+					e.info.at<double>(r, c) = uStr2Double(v[idx++]);
+					if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
+				}
+			}
+			// EDGE_SE3 (no :QUAT) is the landmark variant emitted by saveGraph
+			bool landmarkTag = (tag == "EDGE_SE3");
+			e.type = landmarkTag ? Link::kLandmark : Link::kUndef;
+			e.isPrior = false;
+			e.hasLandmarkEndpoint = landmarkTag;
+			edgesList.push_back(e);
+		}
+		else if(tag == "EDGE_SE3_TRACKXYZ" && v.size() == 13)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to   = atoi(v[2].c_str());
+			// v[3] = param_offset id, ignored
+			e.transform = Transform(uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]), 0, 0, 0);
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			e.info.at<double>(0, 0) = uStr2Double(v[7]);
+			e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[8]);
+			e.info.at<double>(0, 2) = e.info.at<double>(2, 0) = uStr2Double(v[9]);
+			e.info.at<double>(1, 1) = uStr2Double(v[10]);
+			e.info.at<double>(1, 2) = e.info.at<double>(2, 1) = uStr2Double(v[11]);
+			e.info.at<double>(2, 2) = uStr2Double(v[12]);
+			e.type = Link::kLandmark;
+			e.isPrior = false;
+			e.hasLandmarkEndpoint = true;
+			edgesList.push_back(e);
+		}
+		else if(tag == "EDGE_PRIOR_SE2" && v.size() == 11)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to = e.from;
+			e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]));
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			e.info.at<double>(0, 0) = uStr2Double(v[5]);
+			e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[6]);
+			e.info.at<double>(0, 5) = e.info.at<double>(5, 0) = uStr2Double(v[7]);
+			e.info.at<double>(1, 1) = uStr2Double(v[8]);
+			e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[9]);
+			e.info.at<double>(5, 5) = uStr2Double(v[10]);
+			e.type = Link::kPosePrior;
+			e.isPrior = true;
+			e.hasLandmarkEndpoint = false;
+			edgesList.push_back(e);
+		}
+		else if(tag == "EDGE_PRIOR_SE2_XY" && v.size() == 7)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to = e.from;
+			e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), 0);
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			e.info.at<double>(0, 0) = uStr2Double(v[4]);
+			e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[5]);
+			e.info.at<double>(1, 1) = uStr2Double(v[6]);
+			// no orientation info on this prior
+			e.info.at<double>(3, 3) = e.info.at<double>(4, 4) = e.info.at<double>(5, 5) = 1.0 / 9999.0;
+			e.type = Link::kPosePrior;
+			e.isPrior = true;
+			e.hasLandmarkEndpoint = false;
+			edgesList.push_back(e);
+		}
+		else if(tag == "EDGE_SE3_PRIOR" && v.size() == 31)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to = e.from;
+			// v[2] = param_offset id, ignored
+			e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), uStr2Float(v[5]),
+									uStr2Float(v[6]), uStr2Float(v[7]), uStr2Float(v[8]), uStr2Float(v[9]));
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			int idx = 10;
+			for(int r = 0; r < 6; ++r)
+			{
+				for(int c = r; c < 6; ++c)
+				{
+					e.info.at<double>(r, c) = uStr2Double(v[idx++]);
+					if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
+				}
+			}
+			e.type = Link::kPosePrior;
+			e.isPrior = true;
+			e.hasLandmarkEndpoint = false;
+			edgesList.push_back(e);
+		}
+		else if(tag == "EDGE_POINTXYZ_PRIOR" && v.size() == 11)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to = e.from;
+			e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]), 0, 0, 0);
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			e.info.at<double>(0, 0) = uStr2Double(v[5]);
+			e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[6]);
+			e.info.at<double>(0, 2) = e.info.at<double>(2, 0) = uStr2Double(v[7]);
+			e.info.at<double>(1, 1) = uStr2Double(v[8]);
+			e.info.at<double>(1, 2) = e.info.at<double>(2, 1) = uStr2Double(v[9]);
+			e.info.at<double>(2, 2) = uStr2Double(v[10]);
+			// no orientation info on this prior
+			e.info.at<double>(3, 3) = e.info.at<double>(4, 4) = e.info.at<double>(5, 5) = 1.0 / 9999.0;
+			e.type = Link::kPosePrior;
+			e.isPrior = true;
+			e.hasLandmarkEndpoint = false;
+			edgesList.push_back(e);
+		}
+		else if(tag == "EDGE_SE2_SWITCHABLE" && v.size() == 13)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to   = atoi(v[2].c_str());
+			// v[3] = switch vertex id, ignored
+			e.transform = Transform(uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]));
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			e.info.at<double>(0, 0) = uStr2Double(v[7]);
+			e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[8]);
+			e.info.at<double>(0, 5) = e.info.at<double>(5, 0) = uStr2Double(v[9]);
+			e.info.at<double>(1, 1) = uStr2Double(v[10]);
+			e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[11]);
+			e.info.at<double>(5, 5) = uStr2Double(v[12]);
+			e.type = Link::kUndef;
+			e.isPrior = false;
+			e.hasLandmarkEndpoint = false;
+			edgesList.push_back(e);
+		}
+		else if(tag == "EDGE_SE3_SWITCHABLE" && v.size() == 32)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to   = atoi(v[2].c_str());
+			// v[3] = switch vertex id, ignored
+			e.transform = Transform(uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]),
+									uStr2Float(v[7]), uStr2Float(v[8]), uStr2Float(v[9]), uStr2Float(v[10]));
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			int idx = 11;
+			for(int r = 0; r < 6; ++r)
+			{
+				for(int c = r; c < 6; ++c)
+				{
+					e.info.at<double>(r, c) = uStr2Double(v[idx++]);
+					if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
+				}
+			}
+			e.type = Link::kUndef;
+			e.isPrior = false;
+			e.hasLandmarkEndpoint = false;
+			edgesList.push_back(e);
+		}
+		else
+		{
+			UWARN("Unsupported or malformed g2o line: \"%s\" (tag=%s, tokens=%d)", line, tag.c_str(), (int)v.size());
+		}
+	}
+	fclose(file);
+
+	// Recover landmarkOffset from vertex order:
+	//   file order = [landmarks with DESCENDING file_ids] + [regular poses with ASCENDING file_ids]
+	// Walk backwards from the end and take the longest ascending suffix as the regular poses.
+	// landmarkOffset = max regular pose id (= last fileId of that suffix).
+	int landmarkOffset = 0;
+	int firstRegularIdx = (int)verticesList.size();
+	if(!verticesList.empty())
+	{
+		firstRegularIdx = (int)verticesList.size() - 1;
+		while(firstRegularIdx > 0 &&
+			  verticesList[firstRegularIdx - 1].fileId < verticesList[firstRegularIdx].fileId)
+		{
+			--firstRegularIdx;
+		}
+		landmarkOffset = verticesList.back().fileId;
+
+		// If the alleged regular suffix actually starts on a definite landmark
+		// (VERTEX_XY / VERTEX_TRACKXYZ), then there are no regular poses and
+		// saveGraph used landmarkOffset = 0; restore that case.
+		if(verticesList[firstRegularIdx].definitelyLandmark)
+		{
+			landmarkOffset = 0;
+			firstRegularIdx = (int)verticesList.size();
+		}
+	}
+
+	// Insert vertices into poses, remapping landmark file ids back to negative.
+	for(int i = 0; i < (int)verticesList.size(); ++i)
+	{
+		int originalId;
+		if(i < firstRegularIdx)
+		{
+			originalId = landmarkOffset - verticesList[i].fileId; // negative
+		}
+		else
+		{
+			originalId = verticesList[i].fileId;
+		}
+		if(poses.find(originalId) == poses.end())
+		{
+			poses.insert(std::make_pair(originalId, verticesList[i].transform));
+		}
+		else
+		{
+			UWARN("Vertex %d (file id %d) already exists, ignoring duplicate", originalId, verticesList[i].fileId);
+		}
+	}
+
+	// Remap edge endpoints. Any file id > landmarkOffset (or, if landmarkOffset == 0
+	// and there are any landmarks at all, any id present in the landmark prefix)
+	// is a landmark and gets the negative id back.
+	bool allLandmarks = (landmarkOffset == 0 && firstRegularIdx == (int)verticesList.size() && !verticesList.empty());
+	auto remap = [&](int fileId) -> int {
+		if(landmarkOffset > 0 && fileId > landmarkOffset)
+		{
+			return landmarkOffset - fileId; // negative
+		}
+		if(allLandmarks)
+		{
+			return -fileId;
+		}
+		return fileId;
+	};
+
+	for(const EdgeEntry & e : edgesList)
+	{
+		int from = remap(e.from);
+		int to   = e.isPrior ? from : remap(e.to);
+		Link::Type type = e.type;
+		// Promote ambiguous edges (EDGE_SE2) to kLandmark when an endpoint
+		// turns out to be a landmark after remapping.
+		if(type == Link::kUndef && (from < 0 || to < 0))
+		{
+			type = Link::kLandmark;
+		}
+		edgeConstraints.insert(std::make_pair(from, Link(from, to, type, e.transform, e.info)));
+	}
+
+	UINFO("Graph loaded from %s (%d poses, %d edges, landmarkOffset=%d)",
+		  fileName.c_str(), (int)poses.size(), (int)edgeConstraints.size(), landmarkOffset);
+	return true;
+}
+
 bool OptimizerG2O::saveGraph(
 		const std::string & fileName,
 		const std::map<int, Transform> & poses,

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -51,7 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/nonlinear/Values.h>
-#include "gtsam/GravityFactor.h"
+#include <gtsam/navigation/AttitudeFactor.h>
 #include <optimizer/gtsam/XYFactor.h>
 #include <optimizer/gtsam/XYZFactor.h>
 #include <gtsam/nonlinear/ISAM2.h>
@@ -121,7 +121,7 @@ void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
 		params.relinearizeThreshold = threshold;
 		params.relinearizeSkip = skip;
 		params.evaluateNonlinearError = true;
-		isam2_ = new ISAM2(params);
+		isam2_ = new gtsam::ISAM2(params);
 
 		addedPoses_.clear();
 		lastAddedConstraints_.clear();
@@ -239,6 +239,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					isam2_ = new gtsam::ISAM2(params);
 					addedPoses_.clear();
 					lastAddedConstraints_.clear();
+					isLandmarkWithRotation_.clear();
 					lastRootFactorIndex_.first = 0;
 					lastSwitchId_ = 1000000000;
 				}
@@ -308,7 +309,13 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		UDEBUG("fill poses to gtsam... rootId=%d (priorsIgnored=%d landmarksIgnored=%d)",
 				rootId, priorsIgnored()?1:0, landmarksIgnored()?1:0);
 		gtsam::Values initialEstimate;
-		std::map<int, bool> isLandmarkWithRotation;
+		// In batch (non-iSAM2) mode each optimize() call is independent.
+		// In iSAM2 mode the map persists so we can resolve landmarks added
+		// in a previous incremental call but referenced by a new edge.
+		if(!isam2_)
+		{
+			isLandmarkWithRotation_.clear();
+		}
 		for(std::map<int, Transform>::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
 		{
 			UASSERT(!iter->second.isNull());
@@ -328,12 +335,12 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
 					{
 						initialEstimate.insert(iter->first, gtsam::Point2(iter->second.x(), iter->second.y()));
-						isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
+						isLandmarkWithRotation_.insert(std::make_pair(iter->first, false));
 					}
 					else
 					{
 						initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
-						isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
+						isLandmarkWithRotation_.insert(std::make_pair(iter->first, true));
 					}
 					addedPoses_.insert(iter->first);
 				}
@@ -357,12 +364,12 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 						1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
 					{
 						initialEstimate.insert(iter->first, gtsam::Point3(iter->second.x(), iter->second.y(), iter->second.z()));
-						isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
+						isLandmarkWithRotation_.insert(std::make_pair(iter->first, false));
 					}
 					else
 					{
 						initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
-						isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
+						isLandmarkWithRotation_.insert(std::make_pair(iter->first, true));
 					}
 					addedPoses_.insert(iter->first);
 				}
@@ -379,8 +386,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			int id1 = iter->second.from();
 			int id2 = iter->second.to();
 
-            UASSERT_MSG(poses.find(id1)!=poses.end(), uFormat("id1=%d", id1).c_str());
-            UASSERT_MSG(poses.find(id2)!=poses.end(), uFormat("id2=%d", id2).c_str());
+            UASSERT_MSG(poses.find(id1)!=poses.end(), uFormat("id1=%d for constraint %d->%d (type=%d)", id1, id1, id2, iter->second.type()).c_str());
+            UASSERT_MSG(poses.find(id2)!=poses.end(), uFormat("id2=%d for constraint %d->%d (type=%d)", id2, id1, id2, iter->second.type()).c_str());
 
 			UASSERT(!iter->second.transform().isNull());
 			if(id1 == id2)
@@ -390,9 +397,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				{
 					if(isSlam2d())
 					{
-						if(id1 < 0 && !isLandmarkWithRotation.at(id1))
+						if(id1 < 0 && !isLandmarkWithRotation_.at(id1))
 						{
-							noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
+							gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector2(
 									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));
@@ -400,7 +407,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 						}
 						else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
 						{
-							noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
+							gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector2(
 									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));
@@ -429,9 +436,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					}
 					else
 					{
-						if(id1 < 0 && !isLandmarkWithRotation.at(id1))
+						if(id1 < 0 && !isLandmarkWithRotation_.at(id1))
 						{
-							noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
+							gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Precisions(gtsam::Vector3(
 										iter->second.infMatrix().at<double>(0,0),
 										iter->second.infMatrix().at<double>(1,1),
 										iter->second.infMatrix().at<double>(2,2)));
@@ -442,7 +449,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 							1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
 							1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
 						{
-							noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
+							gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Precisions(gtsam::Vector3(
 										iter->second.infMatrix().at<double>(0,0),
 										iter->second.infMatrix().at<double>(1,1),
 										iter->second.infMatrix().at<double>(2,2)));
@@ -471,10 +478,17 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				}
 				else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && newPoses.find(iter->first) != newPoses.end())
 				{
-					Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz();
-					gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1));
-					gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), gravitySigma()));
-					graph.add(Pose3GravityFactor(iter->first, nG, model, Unit3(0,0,1)));
+					gtsam::Rot3 nRbMeas = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation();
+					gtsam::Unit3 nZ(0,0,1);
+					gtsam::Unit3 bGMeas = nRbMeas.unrotate(nZ);
+					gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigma(2, gravitySigma());
+#if GTSAM_VERSION_NUMERIC <= 40300
+					// Note: till 40301 is officially released, version 40300 with "4.3a1" would fail here.
+					//       Just replace "<=" above by "<" to use AttitudeFactor<Pose3> below.
+					graph.add(gtsam::Pose3AttitudeFactor(iter->first, nZ, model, bGMeas));
+#else
+					graph.add(gtsam::AttitudeFactor<gtsam::Pose3>(iter->first, nZ, model, bGMeas));
+#endif
 					lastAddedConstraints_.push_back(ConstraintToFactor(iter->first, iter->first, -1));
 				}
 			}
@@ -494,9 +508,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 						t = iter->second.transform().inverse();
 						std::swap(id1, id2); // should be node -> landmark
 					}
-
+					UASSERT(isLandmarkWithRotation_.find(id2) != isLandmarkWithRotation_.end());
 #ifdef RTABMAP_VERTIGO
-					if(this->isRobust() && isLandmarkWithRotation.at(id2))
+					if(this->isRobust() && isLandmarkWithRotation_.at(id2))
 					{
 						// create new switch variable
 						// Sunderhauf IROS 2012:
@@ -514,7 +528,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 						gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
 						graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior), switchPriorModel));
 					}
-					else if(this->isRobust() && !isLandmarkWithRotation.at(id2))
+					else if(this->isRobust() && !isLandmarkWithRotation_.at(id2))
 					{
 						UWARN("%s cannot be used for landmark constraints without orientation.", Parameters::kOptimizerRobust().c_str());
 					}
@@ -522,7 +536,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 
 					if(isSlam2d())
 					{
-						if(isLandmarkWithRotation.at(id2))
+						if(isLandmarkWithRotation_.at(id2))
 						{
 							Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
 							if(!isCovarianceIgnored())
@@ -585,7 +599,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					}
 					else
 					{
-						if(isLandmarkWithRotation.at(id2))
+						if(isLandmarkWithRotation_.at(id2))
 						{
 							Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
 							if(!isCovarianceIgnored())
@@ -783,7 +797,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			{
 				float x,y,z,roll,pitch,yaw;
 				std::map<int, Transform> tmpPoses;
-				const Values values = isam2_?isam2_->calculateEstimate():optimizer->values();
+				const gtsam::Values values = isam2_?isam2_->calculateEstimate():optimizer->values();
 #if GTSAM_VERSION_NUMERIC >= 40200
 				for(gtsam::Values::deref_iterator iter=values.begin(); iter!=values.end(); ++iter)
 #else
@@ -800,9 +814,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 								gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
 								tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
 							}
-							else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
+							else if(!landmarksIgnored() && isLandmarkWithRotation_.find(key)!=isLandmarkWithRotation_.end())
 							{
-								if(isLandmarkWithRotation.at(key))
+								if(isLandmarkWithRotation_.at(key))
 								{
 									newPoses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 									gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
@@ -823,9 +837,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 								gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
 								tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
 							}
-							else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
+							else if(!landmarksIgnored() && isLandmarkWithRotation_.find(key)!=isLandmarkWithRotation_.end())
 							{
-								if(isLandmarkWithRotation.at(key))
+								if(isLandmarkWithRotation_.at(key))
 								{
 									gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
 									tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
@@ -993,9 +1007,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 						gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
 						optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
 					}
-					else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
+					else if(!landmarksIgnored() && isLandmarkWithRotation_.find(key)!=isLandmarkWithRotation_.end())
 					{
-						if(isLandmarkWithRotation.at(key))
+						if(isLandmarkWithRotation_.at(key))
 						{
 							poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 							gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
@@ -1016,9 +1030,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 						gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
 						optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
 					}
-					else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
+					else if(!landmarksIgnored() && isLandmarkWithRotation_.find(key)!=isLandmarkWithRotation_.end())
 					{
-						if(isLandmarkWithRotation.at(key))
+						if(isLandmarkWithRotation_.at(key))
 						{
 							gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
 							optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));

corelib/src/optimizer/OptimizerTORO.cpp

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

corelib/src/optimizer/gtsam/GravityFactor.cpp

@@ -1,91 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * Author: Mathieu Labbe
- * This file is a copy of AttitudeFactor.cpp of gtsam library but
- * with attitudeError() function overridden to ignore yaw errors.
- * For the noise model, use Sigmas(Vector2(0.1, 10)) (with second sigma high!)
- */
-
-/**
- *  @file   GravityFactor.cpp
- *  @author Frank Dellaert
- *  @brief  Implementation file for Attitude factor
- *  @date   January 28, 2014
- **/
-
-#include "GravityFactor.h"
-
-using namespace std;
-
-namespace rtabmap {
-
-//***************************************************************************
-Vector GravityFactor::attitudeError(const Rot3& nRb,
-    OptionalJacobian<2, 3> H) const {
-  if (H) {
-    Matrix23 D_nRef_R;
-    Matrix22 D_e_nRef;
-    Vector3 r = nRb.xyz();
-    Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0).rotate(bRef_, D_nRef_R);
-    Vector e = nZ_.error(nRef, D_e_nRef);
-    (*H) = D_e_nRef * D_nRef_R;
-    //printf("ref=%f %f %f grav=%f %f %f e= %f %f H=%f %f %f, %f %f %f\n",
-    //		nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1),
-	//		(*H)(0,0), (*H)(0,1), (*H)(0,2), (*H)(1,0), (*H)(1,1), (*H)(1,2));
-    return e;
-  } else {
-    Vector3 r = nRb.xyz();
-    Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0) * bRef_;
-    Vector e = nZ_.error(nRef);
-    //printf("ref=%f %f %f grav=%f %f %f e= %f %f\n", nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1));
-    return e;
-  }
-}
-
-//***************************************************************************
-void Rot3GravityFactor::print(const string& s,
-    const KeyFormatter& keyFormatter) const {
-  cout << s << "Rot3GravityFactor on " << keyFormatter(this->key()) << "\n";
-  nZ_.print("  measured direction in nav frame: ");
-  bRef_.print("  reference direction in body frame: ");
-  this->noiseModel_->print("  noise model: ");
-}
-
-//***************************************************************************
-bool Rot3GravityFactor::equals(const NonlinearFactor& expected,
-    double tol) const {
-  const This* e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
-      && this->bRef_.equals(e->bRef_, tol);
-}
-
-//***************************************************************************
-void Pose3GravityFactor::print(const string& s,
-    const KeyFormatter& keyFormatter) const {
-  cout << s << "Pose3GravityFactor on " << keyFormatter(this->key()) << "\n";
-  nZ_.print("  measured direction in nav frame: ");
-  bRef_.print("  reference direction in body frame: ");
-  this->noiseModel_->print("  noise model: ");
-}
-
-//***************************************************************************
-bool Pose3GravityFactor::equals(const NonlinearFactor& expected,
-    double tol) const {
-  const This* e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
-      && this->bRef_.equals(e->bRef_, tol);
-}
-
-//***************************************************************************
-
-}/// namespace gtsam

corelib/src/optimizer/gtsam/GravityFactor.h

@@ -1,271 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * Author: Mathieu Labbe
- * This file is a copy of AttitudeFactor.h of gtsam library but
- * with attitudeError() function overridden to ignore yaw errors.
- * For the noise model, use Sigmas(Vector2(0.1, 10)) (with second sigma high!)
- */
-
-/**
- *  @file   Pose3GravityFactor.h
- *  @author Frank Dellaert
- *  @brief  Header file for Attitude factor
- *  @date   January 28, 2014
- **/
-#pragma once
-
-#include <gtsam/nonlinear/NonlinearFactor.h>
-#if GTSAM_VERSION_NUMERIC >= 40300 && defined(GTSAM_WITH_NOISE_MODEL_FACTOR_N)
-#include <gtsam/nonlinear/NoiseModelFactorN.h>
-#endif
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/geometry/Unit3.h>
-
-using namespace gtsam;
-
-namespace rtabmap {
-
-/**
- * Base class for prior on gravity
- * Example:
- * - measurement is direction of gravity in navigation frame nG
- * - reference is direction of z axis in body frame bF
- * This factor will give zero error if nG is opposite direction of bF
- * @addtogroup Navigation
- */
-class GravityFactor {
-
-protected:
-
-  const Unit3 nZ_, bRef_; ///< Position measurement in
-
-public:
-
-  /** default constructor - only use for serialization */
-  GravityFactor() {
-  }
-
-  /**
-   * @brief Constructor
-   * @param nZ measured direction in navigation frame
-   * @param bRef reference direction in body frame (default Z-axis in NED frame, i.e., [0; 0; 1])
-   */
-  GravityFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) :
-      nZ_(nZ), bRef_(bRef) {
-  }
-
-  /** vector of errors */
-  Vector attitudeError(const Rot3& p,
-#if GTSAM_VERSION_NUMERIC >= 40300
-      OptionalJacobian<2,3> H = {}) const;
-#else
-  	  OptionalJacobian<2,3> H = boost::none) const;
-#endif
-
-  /** Serialization function */
-#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
-  friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    /*ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
-    ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));*/
-  }
-#endif
-};
-
-/**
- * Version of GravityFactor for Rot3
- * @addtogroup Navigation
- */
-class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
-
-  typedef NoiseModelFactor1<Rot3> Base;
-
-public:
-
-  /// shorthand for a smart pointer to a factor
-#if GTSAM_VERSION_NUMERIC >= 40300
-  typedef std::shared_ptr<Rot3GravityFactor> shared_ptr;
-  #else
-  typedef boost::shared_ptr<Rot3GravityFactor> shared_ptr;
-#endif
-
-  /// Typedef to this class
-  typedef Rot3GravityFactor This;
-
-  /** default constructor - only use for serialization */
-  Rot3GravityFactor() {
-  }
-
-  virtual ~Rot3GravityFactor() {
-  }
-
-  /**
-   * @brief Constructor
-   * @param key of the Rot3 variable that will be constrained
-   * @param nZ measured direction in navigation frame (remove yaw before rotating the gravity vector)
-   * @param model Gaussian noise model
-   * @param bRef reference direction in body frame (default Z-axis)
-   */
-  Rot3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
-      const Unit3& bRef = Unit3(0, 0, 1)) :
-      Base(model, key), GravityFactor(nZ, bRef) {
-  }
-
-  /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-#if GTSAM_VERSION_NUMERIC >= 40300
-    return std::static_pointer_cast<gtsam::NonlinearFactor>(
-#else
-    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-#endif
-        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
-  }
-
-  /** print */
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
-
-  /** equals */
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
-
-  /** vector of errors */
-  virtual Vector evaluateError(const Rot3& nRb, //
-#if GTSAM_VERSION_NUMERIC >= 40300
-	OptionalMatrixType H = OptionalNone) const {
-#else
-	boost::optional<Matrix&> H = boost::none) const {
-#endif
-    return attitudeError(nRb, H);
-  }
-  Unit3 nZ() const {
-    return nZ_;
-  }
-  Unit3 bRef() const {
-    return bRef_;
-  }
-
-private:
-#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
-  /** Serialization function */
-  friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    /*ar & boost::serialization::make_nvp("NoiseModelFactor1",
-        boost::serialization::base_object<Base>(*this));
-    ar & boost::serialization::make_nvp("GravityFactor",
-        boost::serialization::base_object<GravityFactor>(*this));*/
-  }
-#endif
-
-public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-
-/**
- * Version of GravityFactor for Pose3
- * @addtogroup Navigation
- */
-class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
-    public GravityFactor {
-
-  typedef NoiseModelFactor1<Pose3> Base;
-
-public:
-
-  /// shorthand for a smart pointer to a factor
-#if GTSAM_VERSION_NUMERIC >= 40300
-  typedef std::shared_ptr<Pose3GravityFactor> shared_ptr;
-#else
-  typedef boost::shared_ptr<Pose3GravityFactor> shared_ptr;
-#endif
-  /// Typedef to this class
-  typedef Pose3GravityFactor This;
-
-  /** default constructor - only use for serialization */
-  Pose3GravityFactor() {
-  }
-
-  virtual ~Pose3GravityFactor() {
-  }
-
-  /**
-   * @brief Constructor
-   * @param key of the Pose3 variable that will be constrained
-   * @param nZ measured direction in navigation frame (remove yaw before rotating the gravity vector)
-   * @param model Gaussian noise model
-   * @param bRef reference direction in body frame (default Z-axis)
-   */
-  Pose3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
-      const Unit3& bRef = Unit3(0, 0, 1)) :
-      Base(model, key), GravityFactor(nZ, bRef) {
-  }
-
-  /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-#if GTSAM_VERSION_NUMERIC >= 40300
-    return std::static_pointer_cast<gtsam::NonlinearFactor>(
-#else
-    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-#endif
-        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
-  }
-
-  /** print */
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
-
-  /** equals */
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
-
-  /** vector of errors */
-  virtual Vector evaluateError(const Pose3& nTb, //
-#if GTSAM_VERSION_NUMERIC >= 40300
-      OptionalMatrixType H = OptionalNone) const {
-#else
-      boost::optional<Matrix&> H = boost::none) const {
-#endif
-    Vector e = attitudeError(nTb.rotation(), H);
-    if (H) {
-      Matrix H23 = *H;
-      *H = Matrix::Zero(2,6);
-      H->block<2,3>(0,0) = H23;
-    }
-    return e;
-  }
-  Unit3 nZ() const {
-    return nZ_;
-  }
-  Unit3 bRef() const {
-    return bRef_;
-  }
-
-private:
-#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
-  /** Serialization function */
-  friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    /*ar & boost::serialization::make_nvp("NoiseModelFactor1",
-        boost::serialization::base_object<Base>(*this));
-    ar & boost::serialization::make_nvp("GravityFactor",
-        boost::serialization::base_object<GravityFactor>(*this));*/
-  }
-#endif
-public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-} /// namespace gtsam
-

corelib/src/optimizer/gtsam/XYFactor.h

@@ -43,7 +43,7 @@ public:
   // @param H    the optional Jacobian matrix, which use boost optional and has default null pointer
   gtsam::Vector evaluateError(const VALUE& p,
 #if GTSAM_VERSION_NUMERIC >= 40300
-		  OptionalMatrixType H = OptionalNone) const {
+		  gtsam::OptionalMatrixType H = OptionalNone) const {
 #else
 		  boost::optional<gtsam::Matrix&> H = boost::none) const {
 #endif
@@ -59,5 +59,5 @@ public:
 
 };
 
-} // namespace gtsamexamples
+} // namespace rtabmap
 

corelib/src/optimizer/gtsam/XYZFactor.h

@@ -43,7 +43,7 @@ public:
   // @param H    the optional Jacobian matrix, which use boost optional and has default null pointer
   gtsam::Vector evaluateError(const gtsam::Pose3& p,
 #if GTSAM_VERSION_NUMERIC >= 40300
-		  OptionalMatrixType H = OptionalNone) const {
+		  gtsam::OptionalMatrixType H = OptionalNone) const {
 #else
 		  boost::optional<gtsam::Matrix&> H = boost::none) const {
 #endif
@@ -55,7 +55,7 @@ public:
   }
   gtsam::Vector evaluateError(const gtsam::Point3& p,
 #if GTSAM_VERSION_NUMERIC >= 40300
-		  OptionalMatrixType H = OptionalNone) const {
+		  gtsam::OptionalMatrixType H = OptionalNone) const {
 #else
 		  boost::optional<gtsam::Matrix&> H = boost::none) const {
 #endif
@@ -63,5 +63,5 @@ public:
   }
 };
 
-} // namespace gtsamexamples
+} // namespace rtabmap
 

corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h

@@ -31,9 +31,9 @@ namespace vertigo {
 
       gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s,
 #if GTSAM_VERSION_NUMERIC >= 40300
-		  OptionalMatrixType H1 = OptionalNone,
-		  OptionalMatrixType H2 = OptionalNone,
-		  OptionalMatrixType H3 = OptionalNone) const
+		  gtsam::OptionalMatrixType H1 = OptionalNone,
+		  gtsam::OptionalMatrixType H2 = OptionalNone,
+		  gtsam::OptionalMatrixType H3 = OptionalNone) const
 #else
           boost::optional<gtsam::Matrix&> H1 = boost::none,
           boost::optional<gtsam::Matrix&> H2 = boost::none,
@@ -71,9 +71,9 @@ namespace vertigo {
 
       gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s,
 #if GTSAM_VERSION_NUMERIC >= 40300
-		  OptionalMatrixType H1 = OptionalNone,
-		  OptionalMatrixType H2 = OptionalNone,
-		  OptionalMatrixType H3 = OptionalNone) const
+		  gtsam::OptionalMatrixType H1 = OptionalNone,
+		  gtsam::OptionalMatrixType H2 = OptionalNone,
+		  gtsam::OptionalMatrixType H3 = OptionalNone) const
 #else
           boost::optional<gtsam::Matrix&> H1 = boost::none,
           boost::optional<gtsam::Matrix&> H2 = boost::none,

corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h

@@ -13,6 +13,7 @@
 // DerivedValue2.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac
 #include "DerivedValue.h"
 #include <gtsam/base/Lie.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace vertigo {
 
@@ -77,8 +78,8 @@ namespace vertigo {
     /** between operation */
     inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
 #if GTSAM_VERSION_NUMERIC >= 40300
-		OptionalMatrixType H1=OptionalNone,
-		OptionalMatrixType H2=OptionalNone) const {
+		gtsam::OptionalMatrixType H1=OptionalNone,
+		gtsam::OptionalMatrixType H2=OptionalNone) const {
 #else
         boost::optional<gtsam::Matrix&> H1=boost::none,
         boost::optional<gtsam::Matrix&> H2=boost::none) const {

corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h

@@ -13,6 +13,7 @@
 // DerivedValue.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac
 #include "DerivedValue.h"
 #include <gtsam/base/Lie.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace vertigo {
 
@@ -77,8 +78,8 @@ namespace vertigo {
     /** between operation */
     inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2,
 #if GTSAM_VERSION_NUMERIC >= 40300
-        OptionalMatrixType H1=OptionalNone,
-		OptionalMatrixType H2=OptionalNone) const {
+        gtsam::OptionalMatrixType H1=OptionalNone,
+		    gtsam::OptionalMatrixType H2=OptionalNone) const {
 #else
         boost::optional<gtsam::Matrix&> H1=boost::none,
         boost::optional<gtsam::Matrix&> H2=boost::none) const {

corelib/src/python/PyDetector.cpp

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

corelib/src/python/PythonInterface.cpp

@@ -19,9 +19,11 @@ PythonInterface::PythonInterface()
 	guard_ = new pybind11::scoped_interpreter();
 
 	// Tell Python to look in this directory for DLLs
+#ifdef _WIN32
 	std::string exe_dir = std::filesystem::current_path().string();
     pybind11::module_ os = pybind11::module_::import("os");
     os.attr("add_dll_directory")(exe_dir);
+#endif
 
 	pybind11::module::import("threading");
 	release_ = new pybind11::gil_scoped_release();

corelib/src/superpoint_rpautrat/SuperpointRpautrat.cpp

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

corelib/src/superpoint_torch/SuperPoint.cc

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

corelib/src/util3d.cpp

@@ -1762,6 +1762,55 @@ LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud,
 	return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
 }
 
+LaserScan laserScanFromPointCloud(const pcl::PointCloud<rtabmap::PointXYZIRT> & cloud, const Transform & transform, bool filterNaNs)
+{
+	return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
+}
+
+LaserScan laserScanFromPointCloud(const pcl::PointCloud<rtabmap::PointXYZIRT> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
+{
+	// Layout: [x, y, z, intensity, ring, time] (ring cast to float, values up to
+	// ~16M are exactly representable so all realistic laser line counts fit).
+	cv::Mat laserScan;
+	bool nullTransform = transform.isNull() || transform.isIdentity();
+	Eigen::Affine3f transform3f = transform.toEigen3f();
+	int oi = 0;
+	const int total = indices.get() ? (int)indices->size() : (int)cloud.size();
+	laserScan = cv::Mat(1, total, CV_32FC(6));
+	for(int i=0; i<total; ++i)
+	{
+		int index = indices.get() ? indices->at(i) : i;
+		const rtabmap::PointXYZIRT & src = cloud.at(index);
+		if(filterNaNs && !pcl::isFinite(src))
+		{
+			continue;
+		}
+		float * ptr = laserScan.ptr<float>(0, oi++);
+		if(!nullTransform)
+		{
+			pcl::PointXYZ pt(src.x, src.y, src.z);
+			pt = pcl::transformPoint(pt, transform3f);
+			ptr[0] = pt.x;
+			ptr[1] = pt.y;
+			ptr[2] = pt.z;
+		}
+		else
+		{
+			ptr[0] = src.x;
+			ptr[1] = src.y;
+			ptr[2] = src.z;
+		}
+		ptr[3] = src.intensity;
+		ptr[4] = static_cast<float>(src.ring);
+		ptr[5] = src.time;
+	}
+	if(oi == 0)
+	{
+		return LaserScan();
+	}
+	return LaserScan(laserScan(cv::Range::all(), cv::Range(0, oi)), 0, 0.0f, LaserScan::kXYZIRT);
+}
+
 LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
 {
 	cv::Mat laserScan;
@@ -2343,7 +2392,7 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con
 	{
 		pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
 	}
-	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT)
+	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT || laserScan.format() == LaserScan::kXYZIRT)
 	{
 		pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
 	}
@@ -3807,9 +3856,11 @@ LaserScan deskew(
 		return LaserScan();
 	}
 
-	if(input.format() != LaserScan::kXYZIT)
+	if(!input.hasTime())
 	{
-		UERROR("input scan doesn't have \"time\" channel! Only format \"%s\" supported yet.", LaserScan::formatName(LaserScan::kXYZIT).c_str());
+		UERROR("input scan doesn't have a \"time\" channel! Supported formats: \"%s\", \"%s\".",
+			LaserScan::formatName(LaserScan::kXYZIT).c_str(),
+			LaserScan::formatName(LaserScan::kXYZIRT).c_str());
 		return LaserScan();
 	}
 
@@ -3865,7 +3916,14 @@ LaserScan deskew(
 	double stamp;
 	UTimer processingTime;
 	double scanTime = lastStamp - firstStamp;
-	cv::Mat output(1, input.size(), CV_32FC4); // XYZI - Dense
+	// Preserve ring when input carries it (kXYZIRT): the geometric channel is
+	// still meaningful after deskewing. Per-point time is zeroed because all
+	// points share the same pose after correction.
+	const bool preserveRing = input.hasRing();
+	const int offsetRing = input.getRingOffset();
+	const LaserScan::Format outputFormat = preserveRing ? LaserScan::kXYZIRT : LaserScan::kXYZI;
+	const int outputChannels = preserveRing ? 6 : 4;
+	cv::Mat output(1, input.size(), CV_32FC(outputChannels));
 	int offsetIntensity = input.getIntensityOffset();
 	bool isLocalTransformIdentity = input.localTransform().isIdentity();
 	Transform localTransformInv = input.localTransform().inverse();
@@ -3904,7 +3962,12 @@ LaserScan deskew(
 					dataPtr[0] = pt.x;
 					dataPtr[1] = pt.y;
 					dataPtr[2] = pt.z;
-					dataPtr[3] = input.data().ptr<float>(v, u)[offsetIntensity];
+					dataPtr[3] = inputPtr[offsetIntensity];
+					if(preserveRing)
+					{
+						dataPtr[4] = inputPtr[offsetRing];
+						dataPtr[5] = 0.0f;
+					}
 				}
 			}
 		}
@@ -3941,14 +4004,19 @@ LaserScan deskew(
 					dataPtr[0] = pt.x;
 					dataPtr[1] = pt.y;
 					dataPtr[2] = pt.z;
-					dataPtr[3] = input.data().ptr<float>(v, u)[offsetIntensity];
+					dataPtr[3] = inputPtr[offsetIntensity];
+					if(preserveRing)
+					{
+						dataPtr[4] = inputPtr[offsetRing];
+						dataPtr[5] = 0.0f;
+					}
 				}
 			}
 		}
 	}
 	output = cv::Mat(output, cv::Range::all(), cv::Range(0, oi));
 	UDEBUG("Lidar deskewing time=%fs", processingTime.elapsed());
-	return LaserScan(output, input.maxPoints(), input.rangeMax(), LaserScan::kXYZI, input.localTransform());
+	return LaserScan(output, input.maxPoints(), input.rangeMax(), outputFormat, input.localTransform());
 }
 
 

docker/resolute/Dockerfile

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

docker/resolute/deps/Dockerfile

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

examples/WifiMapping/WifiThread.h

@@ -177,7 +177,8 @@ private:
 			struct iwreq req;
 			struct iw_statistics stats;
 
-			strncpy(req.ifr_name, interfaceName_.c_str(), IFNAMSIZ);
+			strncpy(req.ifr_name, interfaceName_.c_str(), IFNAMSIZ - 1);
+			req.ifr_name[IFNAMSIZ - 1] = '\0';
 
 			//make room for the iw_statistics object
 			req.u.data.pointer = (caddr_t) &stats;

guilib/include/rtabmap/gui/CameraViewer.h

@@ -77,6 +77,7 @@ private:
 	QElapsedTimer fpsTimer_;
 	double lastCapturePeriod_;
 	double previousCaptureStamp_;
+	std::map<int, float> _landmarksSize;
 };
 
 } /* namespace rtabmap */

guilib/include/rtabmap/gui/CloudViewer.h

@@ -231,7 +231,8 @@ public:
 				const Transform & to,
 				const QColor & color,
 				bool arrow = false,
-				bool foreground = false);
+				bool foreground = false,
+				double width = 1.0);
 	void removeLine(const std::string & id);
 	void removeAllLines();
 	const std::set<std::string> & getAddedLines() const {return _lines;}

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -364,6 +364,7 @@ private Q_SLOTS:
 	void updateStereoDisparityVisibility();
 	void updateFeatureMatchingVisibility();
 	void updateGlobalDescriptorVisibility();
+	void updateAvailableMarkerDictionaries();
 	void updateOdometryStackedIndex(int index);
 	void useOdomFeatures();
 	void changeWorkingDirectory();
@@ -371,6 +372,8 @@ private Q_SLOTS:
 	void changeOdometryORBSLAMVocabulary();
 	void changeOdometryOKVISConfigPath();
 	void changeOdometryVINSFusionConfigPath();
+	void changeOdometryOpenVINSConfigPath();
+	void changeOdometryLIOSAMConfigPath();
 	void changeOdometryOpenVINSLeftMask();
 	void changeOdometryOpenVINSRightMask();
 	void changeIcpPMConfigPath();

guilib/src/AboutDialog.cpp

@@ -107,6 +107,13 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_fastcv->setText("No");
 	_ui->label_fastcv_license->setEnabled(false);
 #endif
+#ifdef RTABMAP_APRILTAG
+	_ui->label_apriltag->setText("Yes");
+	_ui->label_apriltag_license->setEnabled(true);
+#else
+	_ui->label_apriltag->setText("No");
+	_ui->label_apriltag_license->setEnabled(false);
+#endif
 #ifdef RTABMAP_PDAL
 	_ui->label_pdal->setText("Yes");
 	_ui->label_pdal_license->setEnabled(true);

guilib/src/CameraViewer.cpp

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/gui/CloudViewer.h>
 #include <rtabmap/utilite/UCv2Qt.h>
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <QtCore/QMetaType>
 #include <QHBoxLayout>
 #include <QVBoxLayout>
@@ -84,7 +85,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
 	showScanCheckbox_->setChecked(true);
 
 	markerCheckbox_ = new QCheckBox("Detect markers", this);
-#ifdef HAVE_OPENCV_ARUCO
+#if defined(HAVE_OPENCV_ARUCO) || defined(RTABMAP_APRILTAG)
 	markerCheckbox_->setEnabled(true);
 	markerDetector_ = new MarkerDetector(parameters);
 #else
@@ -151,6 +152,7 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 
 	imageView_->setVisible(!left.empty() || !left.empty());
 	std::map<int, MarkerInfo> detections;
+	UTimer markerDetectionTime;
 	if(!left.empty())
 	{
 		std::vector<CameraModel> models;
@@ -165,12 +167,22 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 				}
 			}
 		}
+		else
+		{
+			_landmarksSize.clear();
+		}
 			
 		if(!models.empty() && models[0].isValidForProjection())
 		{
 			cv::Mat imageWithDetections;
-			detections = markerDetector_->detect(left, models, depthOrRight, std::map<int, float>(), &imageWithDetections);
+			detections = markerDetector_->detect(left, models, depthOrRight, _landmarksSize, &imageWithDetections);
 			imageView_->setImage(uCvMat2QImage(imageWithDetections));
+			for(std::map<int, MarkerInfo>::iterator iter=detections.begin(); iter!=detections.end(); ++iter)
+			{
+				if(iter->second.length() > 0.0f) {
+					_landmarksSize.insert(std::make_pair(iter->first, iter->second.length()));
+				}
+			}
 		}
 		else
 		{
@@ -184,6 +196,10 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 		sizes.append(QString(" Depth=%1x%2").arg(depthOrRight.cols).arg(depthOrRight.rows));
 	}
 	sizes.append(QString(" FPS capture=%1 render=%2").arg(lastCapturePeriod_>0.0?(int)round(1.0/lastCapturePeriod_):0).arg((int)round(1.0/fpsTimer_.restart()*1000)));
+	if(markerCheckbox_->isEnabled() && markerCheckbox_->isChecked())
+	{
+		sizes.append(QString(" Marker=%1ms").arg(int(markerDetectionTime.ticks()*1000)));
+	}
 	imageSizeLabel_->setText(sizes);
 
 	if(!depthOrRight.empty() &&
@@ -236,7 +252,7 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
 					cloudView_->addOrUpdateCoordinate(uFormat("landmark_%d", iter->first), iter->second.pose(), iter->second.length(), false);
 #endif
-					std::string num = uNumber2Str(iter->first);
+					std::string num = uFormat("%d (%.1f cm)", iter->first, iter->second.length()*100.0f);
 					cloudView_->addOrUpdateText(
 							std::string("landmark_str_") + num,
 							num,

guilib/src/CloudViewer.cpp

@@ -1732,7 +1732,8 @@ void CloudViewer::addOrUpdateLine(
 			const Transform & to,
 			const QColor & color,
 			bool arrow,
-			bool foreground)
+			bool foreground,
+			double width)
 {
 	if(id.empty())
 	{
@@ -1764,6 +1765,7 @@ void CloudViewer::addOrUpdateLine(
 			_visualizer->addLine(pt2, pt1, c.redF(), c.greenF(), c.blueF(), id, foreground?3:2);
 		}
 		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
+		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, width, id);
 	}
 }
 
@@ -2969,18 +2971,19 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose)
 				Eigen::Vector3f zAxis(cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
 				Eigen::Vector3f yAxis = zAxis.cross(vPosToFocal);
 				Eigen::Vector3f xAxis = yAxis.cross(zAxis);
-				Transform PR(xAxis[0], xAxis[1], xAxis[2],0,
-							yAxis[0], yAxis[1], yAxis[2],0,
-							zAxis[0], zAxis[1], zAxis[2],0);
 
-				PR.normalizeRotation();
+				Eigen::Matrix3f m;
+				m << xAxis[0], xAxis[1], xAxis[2],
+					yAxis[0], yAxis[1], yAxis[2],
+					zAxis[0], zAxis[1], zAxis[2];
 
-				Transform P(PR[0], PR[1], PR[2], cameras.front().pos[0],
-							PR[4], PR[5], PR[6], cameras.front().pos[1],
-							PR[8], PR[9], PR[10], cameras.front().pos[2]);
-				Transform F(PR[0], PR[1], PR[2], cameras.front().focal[0],
-							PR[4], PR[5], PR[6], cameras.front().focal[1],
-							PR[8], PR[9], PR[10], cameras.front().focal[2]);
+				// Make sure it is normalized
+				Eigen::Quaternionf q = Eigen::Quaternionf(m).normalized();
+
+				Transform P(cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
+							q.x(), q.y(), q.z(), q.w());
+				Transform F(cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
+							q.x(), q.y(), q.z(), q.w());
 				Transform N = pose;
 				Transform O = _lastPose;
 				Transform O2N = O.inverse()*N;

guilib/src/DataRecorder.cpp

@@ -140,7 +140,7 @@ void DataRecorder::addData(const rtabmap::SensorData & data, const Transform & p
 		//save to database
 		UTimer time;
 		memory_->update(data, pose, covariance);
-		const Signature * s = memory_->getLastWorkingSignature();
+		const Signature * s = memory_->getLastWorkingSignature(false);
 		totalSizeKB_ += (int)s->sensorData().imageCompressed().total()/1000;
 		totalSizeKB_ += (int)s->sensorData().depthOrRightCompressed().total()/1000;
 		totalSizeKB_ += (int)s->sensorData().laserScanCompressed().data().total()/1000;

guilib/src/DatabaseViewer.cpp

@@ -7930,8 +7930,7 @@ void DatabaseViewer::updateGraphView()
 
 		// remove intermediate nodes?
 		if(ui_->checkBox_ignoreIntermediateNodes->isVisible() &&
-		   ui_->checkBox_ignoreIntermediateNodes->isEnabled() &&
-		   ui_->checkBox_ignoreIntermediateNodes->isChecked())
+		   (ui_->checkBox_ignoreIntermediateNodes->isChecked() || ui_->comboBox_optimizationFlavor->currentIndex() == 2))
 		{
 			for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
 			{
@@ -9129,10 +9128,6 @@ bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool si
 		int fromId = newLink.from();
 		std::multimap<int, Link> linksIn = updateLinksWithModifications(links_);
 		linksIn.insert(std::make_pair(newLink.from(), newLink));
-		const Link * maxLinearLink = 0;
-		const Link * maxAngularLink = 0;
-		float maxLinearErrorRatio = 0.0f;
-		float maxAngularErrorRatio = 0.0f;
 		Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> links;
@@ -9165,51 +9160,43 @@ bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool si
 		std::string msg;
 		if(poses.size())
 		{
-			float maxLinearError = 0.0f;
-			float maxAngularError = 0.0f;
-			graph::computeMaxGraphErrors(
+			graph::MaxGraphErrors maxGraphErrors = graph::computeMaxGraphErrors(
 					poses,
-					links,
-					maxLinearErrorRatio,
-					maxAngularErrorRatio,
-					maxLinearError,
-					maxAngularError,
-					&maxLinearLink,
-					&maxAngularLink);
-			if(maxLinearLink)
+					links);
+			if(maxGraphErrors.linearLink.isValid())
 			{
-				UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
-				if(maxLinearErrorRatio > maxOptimizationError)
+				UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.transVariance(), maxGraphErrors.linear/sqrt(maxGraphErrors.linearLink.transVariance()));
+				if(maxGraphErrors.linearRatio > maxOptimizationError)
 				{
 					msg = uFormat("Rejecting edge %d->%d because "
 						  "graph error is too large (abs=%f m) after optimization (ratio %f for edge %d->%d, stddev=%f m). "
 						  "\"%s\" is %f.",
 						  newLink.from(),
 						  newLink.to(),
-						  maxLinearError,
-						  maxLinearErrorRatio,
-						  maxLinearLink->from(),
-						  maxLinearLink->to(),
-						  sqrt(maxLinearLink->transVariance()),
+						  maxGraphErrors.linear,
+						  maxGraphErrors.linearRatio,
+						  maxGraphErrors.linearLink.from(),
+						  maxGraphErrors.linearLink.to(),
+						  sqrt(maxGraphErrors.linearLink.transVariance()),
 						  Parameters::kRGBDOptimizeMaxError().c_str(),
 						  maxOptimizationError);
 				}
 			}
-			if(maxAngularLink)
+			if(maxGraphErrors.angularLink.isValid())
 			{
-				UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
-				if(maxAngularErrorRatio > maxOptimizationError)
+				UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxGraphErrors.angular*180.0f/CV_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.rotVariance(), maxGraphErrors.angular/sqrt(maxGraphErrors.angularLink.rotVariance()));
+				if(maxGraphErrors.angularRatio > maxOptimizationError)
 				{
 					msg = uFormat("Rejecting edge %d->%d because "
 						  "graph error is too large (abs=%f deg) after optimization (ratio %f for edge %d->%d, stddev=%f deg). "
 						  "\"%s\" is %f.",
 						  newLink.from(),
 						  newLink.to(),
-						  maxAngularError*180.0f/CV_PI,
-						  maxAngularErrorRatio,
-						  maxAngularLink->from(),
-						  maxAngularLink->to(),
-						  sqrt(maxAngularLink->rotVariance()),
+						  maxGraphErrors.angular*180.0f/CV_PI,
+						  maxGraphErrors.angularRatio,
+						  maxGraphErrors.angularLink.from(),
+						  maxGraphErrors.angularLink.to(),
+						  sqrt(maxGraphErrors.angularLink.rotVariance()),
 						  Parameters::kRGBDOptimizeMaxError().c_str(),
 						  maxOptimizationError);
 				}
@@ -9451,7 +9438,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
 		findIter = rtabmap::graph::findLink(linksRemoved_, iter->second.from(), iter->second.to());
 		if(findIter != linksRemoved_.end())
 		{
-			UDEBUG("Removed link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
+			//UDEBUG("Removed link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
 			continue; // don't add this link
 		}
 
@@ -9468,7 +9455,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
 			{
 				links.insert(*findIter);
 			}
-			UDEBUG("Updated link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
+			//UDEBUG("Updated link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
 			continue;
 		}
 
@@ -9487,11 +9474,11 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
 			if(findIter->second.from() != findIter->second.to()) {
 				links.insert(std::make_pair(findIter->second.to(), findIter->second.inverse())); // return both ways 
 			}
-			UDEBUG("Added refined link (%d->%d, %d)", findIter->second.from(), findIter->second.to(), findIter->second.type());
+			//UDEBUG("Added refined link (%d->%d, %d)", findIter->second.from(), findIter->second.to(), findIter->second.type());
 			continue;
 		}
 
-		UDEBUG("Added link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
+		//UDEBUG("Added link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
 		links.insert(*iter);
 		if(iter->second.from() != iter->second.to()) {
 			links.insert(std::make_pair(iter->second.to(), iter->second.inverse())); // return both ways 

guilib/src/GraphViewer.cpp

@@ -2270,11 +2270,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	aMouseTracking->setChecked(_mouseTracking);
 	aMouseTracking->setEnabled(_viewPlane == XY);
 	aShowHideGraph->setEnabled(_viewPlane == XY);
-	aShowHideGraphNodes->setEnabled(_graphRoot->isVisible());
 	aShowHideGlobalPath->setEnabled(_globalPathLinkItems.size());
 	aShowHideLocalPath->setEnabled(_localPathLinkItems.size());
-	aShowHideGtGraph->setEnabled(_gtGraphRoot->isVisible());
-	aShowHideGPSGraph->setEnabled(_gpsGraphRoot->isVisible());
 	aShowHideOdomCacheOverlay->setEnabled(_odomCacheOverlay->rect().width()>0);
 
 	QMenu * viewPlaneMenu = menu.addMenu("View Plane...");

guilib/src/MainWindow.cpp

@@ -585,6 +585,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	// Apply state
 	this->changeState(kIdle);
 	this->applyPrefSettings(PreferencesDialog::kPanelAll);
+	applyPrefSettings(parameters, false);
 
 	_ui->statsToolBox->setNewFigureMaxItems(50);
 	_ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
@@ -709,10 +710,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	this->loadFigures();
 	connect(_ui->statsToolBox, SIGNAL(figuresSetupChanged()), this, SLOT(configGUIModified()));
 
-	// update loop closure viewer parameters
-	_loopClosureViewer->setDecimation(_preferencesDialog->getCloudDecimation(0));
-	_loopClosureViewer->setMaxDepth(_preferencesDialog->getCloudMaxDepth(0));
-
 	if (splash)
 	{
 		splash->close();
@@ -2110,6 +2107,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		}
 
 		// For intermediate empty nodes, keep latest image shown
+		bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
 		if(signature.getWeight() >= 0)
 		{
 			_ui->imageView_source->clear();
@@ -2134,7 +2132,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 			_ui->label_matchId->clear();
 
-			bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
 			int proximityTimeDetections = (int)uValue(stat.data(), Statistics::kProximityTime_detections(), 0.0f);
 			bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kNeighborLinkRefiningAccepted(), 0.0f);
 			_ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
@@ -2458,6 +2455,18 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				UDEBUG("time= %d ms (update loop closure viewer)", time.restart());
 			}
 		}
+		else if(rehearsedSimilarity)
+		{
+			_ui->imageView_source->setBackgroundColor(Qt::darkBlue);
+		}
+		else if(smallMovement)
+		{
+			_ui->imageView_source->setBackgroundColor(Qt::gray);
+		}
+		else if(fastMovement)
+		{
+			_ui->imageView_source->setBackgroundColor(Qt::magenta);
+		}
 
 		// PDF AND LIKELIHOOD
 		if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible())
@@ -2715,7 +2724,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			Signature & s = *_cachedSignatures.find(stat.refImageId());
 			_cachedMemoryUsage -= s.sensorData().getMemoryUsed();
 			s.sensorData().clearRawData();
-			s.sensorData().clearOccupancyGridRaw();
 			_cachedMemoryUsage += s.sensorData().getMemoryUsed();
 		}
 
@@ -3254,14 +3262,17 @@ void MainWindow::updateMapCloud(
 	}
 
 	std::map<int, Transform> posesWithOdomCache;
-
+	std::set<int> odomCachePosesIds;
 	if(_ui->graphicsView_graphView->isVisible() ||
 	   ((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown(0)) && _currentPosesMap.size()))
 	{
 		posesWithOdomCache = posesIn;
 		for(std::map<int, Transform>::const_iterator iter=odomCachePoses.begin(); iter!=odomCachePoses.end(); ++iter)
 		{
-			posesWithOdomCache.insert(std::make_pair(iter->first, _odometryCorrection*iter->second));
+			if(posesWithOdomCache.insert(std::make_pair(iter->first, _odometryCorrection*iter->second)).second)
+			{
+				odomCachePosesIds.insert(iter->first);
+			}
 		}
 	}
 
@@ -3520,7 +3531,7 @@ void MainWindow::updateMapCloud(
 		std::multimap<int, Link> constraintsWithOdomCache;
 		constraintsWithOdomCache = constraints;
 		constraintsWithOdomCache.insert(odomCacheConstraints.begin(), odomCacheConstraints.end());
-		_ui->graphicsView_graphView->updateGraph(posesWithOdomCache, constraintsWithOdomCache, mapIdsIn, std::map<int, int>(), uKeysSet(odomCachePoses));
+		_ui->graphicsView_graphView->updateGraph(posesWithOdomCache, constraintsWithOdomCache, mapIdsIn, std::map<int, int>(), odomCachePosesIds);
 		if(_preferencesDialog->isGroundTruthAligned() && !mapToGt.isIdentity())
 		{
 			std::map<int, Transform> gtPoses = _currentGTPosesMap;
@@ -5228,6 +5239,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 
 void MainWindow::drawLandmarks(cv::Mat & image, const Signature & signature)
 {
+	UDEBUG("%ld landmarks", signature.getLandmarks().size());
 	for(std::map<int, Link>::const_iterator iter=signature.getLandmarks().begin(); iter!=signature.getLandmarks().end(); ++iter)
 	{
 		// Project in all cameras in which the landmark is visible
@@ -5284,6 +5296,9 @@ void MainWindow::drawLandmarks(cv::Mat & image, const Signature & signature)
 							{
 								imagePoints[j].x += i*model.imageWidth();
 							}
+							// Make sure the frame origin is visible
+							valid = imagePoints[0].x >= i*model.imageWidth() && imagePoints[0].x < (i+1)*model.imageWidth() &&
+									imagePoints[0].y >= 0 && imagePoints[0].y < image.rows;
 						}
 					}
 					if(valid)
@@ -6862,10 +6877,6 @@ void MainWindow::postProcessing(
 												}
 												std::multimap<int, Link> linksIn = _currentLinksMap;
 												linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, information)));
-												const Link * maxLinearLink = 0;
-												const Link * maxAngularLink = 0;
-												float maxLinearError = 0.0f;
-												float maxAngularError = 0.0f;
 												std::map<int, Transform> poses;
 												std::multimap<int, Link> links;
 												UASSERT(_currentPosesMap.find(fromId) != _currentPosesMap.end());
@@ -6880,51 +6891,43 @@ void MainWindow::postProcessing(
 												std::string msg;
 												if(poses.size())
 												{
-													float maxLinearErrorRatio = 0.0f;
-													float maxAngularErrorRatio = 0.0f;
-													graph::computeMaxGraphErrors(
+													graph::MaxGraphErrors maxGraphErrors = graph::computeMaxGraphErrors(
 															poses,
-															links,
-															maxLinearErrorRatio,
-															maxAngularErrorRatio,
-															maxLinearError,
-															maxAngularError,
-															&maxLinearLink,
-															&maxAngularLink);
-													if(maxLinearLink)
+															links);
+													if(maxGraphErrors.linearLink.isValid())
 													{
-														UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
-														if(maxLinearErrorRatio > optimizeMaxError)
+														UINFO("Max optimization linear error = %f m (link %d->%d)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to());
+														if(maxGraphErrors.linearRatio > optimizeMaxError)
 														{
 															msg = uFormat("Rejecting edge %d->%d because "
 																	  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
 																	  "\"%s\" is %f.",
 																	  from,
 																	  to,
-																	  maxLinearError,
-																	  maxLinearLink->from(),
-																	  maxLinearLink->to(),
-																	  maxLinearErrorRatio,
-																	  sqrt(maxLinearLink->transVariance()),
+																	  maxGraphErrors.linear,
+																	  maxGraphErrors.linearLink.from(),
+																	  maxGraphErrors.linearLink.to(),
+																	  maxGraphErrors.linearRatio,
+																	  sqrt(maxGraphErrors.linearLink.transVariance()),
 																	  Parameters::kRGBDOptimizeMaxError().c_str(),
 																	  optimizeMaxError);
 														}
 													}
-													else if(maxAngularLink)
+													else if(maxGraphErrors.angularLink.isValid())
 													{
-														UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
-														if(maxAngularErrorRatio > optimizeMaxError)
+														UINFO("Max optimization angular error = %f deg (link %d->%d)", maxGraphErrors.angular*180.0f/M_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to());
+														if(maxGraphErrors.angularRatio > optimizeMaxError)
 														{
 															msg = uFormat("Rejecting edge %d->%d because "
 																	  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
 																	  "\"%s\" is %f m.",
 																	  from,
 																	  to,
-																	  maxAngularError*180.0f/M_PI,
-																	  maxAngularLink->from(),
-																	  maxAngularLink->to(),
-																	  maxAngularErrorRatio,
-																	  sqrt(maxAngularLink->rotVariance()),
+																	  maxGraphErrors.angular*180.0f/M_PI,
+																	  maxGraphErrors.angularLink.from(),
+																	  maxGraphErrors.angularLink.to(),
+																	  maxGraphErrors.angularRatio,
+																	  sqrt(maxGraphErrors.angularLink.rotVariance()),
 																	  Parameters::kRGBDOptimizeMaxError().c_str(),
 																	  optimizeMaxError);
 														}

guilib/src/PreferencesDialog.cpp

@@ -243,6 +243,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 #ifndef RTABMAP_CUVSLAM
 	_ui->odom_strategy->setItemData(13, 0, Qt::UserRole - 1);
 #endif
+#ifndef RTABMAP_LIOSAM
+	_ui->odom_strategy->setItemData(14, 0, Qt::UserRole - 1);
+#endif
 
 #if CV_MAJOR_VERSION < 3
 	_ui->stereosgbm_mode->setItemData(2, 0, Qt::UserRole - 1);
@@ -472,15 +475,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->checkBox_showOdomFrustums->setChecked(false);
 #endif
 
-	//if OpenCV < 3.4.2
-#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
-	_ui->ArucoDictionary->setItemData(17, 0, Qt::UserRole - 1);
-	_ui->ArucoDictionary->setItemData(18, 0, Qt::UserRole - 1);
-	_ui->ArucoDictionary->setItemData(19, 0, Qt::UserRole - 1);
-	_ui->ArucoDictionary->setItemData(20, 0, Qt::UserRole - 1);
+#if !defined(HAVE_OPENCV_ARUCO) && !defined(RTABMAP_APRILTAG)
+	_ui->label_markerDetection->setText(_ui->label_markerDetection->text()+" This option works only if OpenCV has been built with \"aruco\" module and/or RTAB-Map has been built with AprilTag library support.");
 #endif
 #ifndef HAVE_OPENCV_ARUCO
-	_ui->label_markerDetection->setText(_ui->label_markerDetection->text()+" This option works only if OpenCV has been built with \"aruco\" module.");
+	_ui->MarkerStrategy->setItemData(0, 0, Qt::UserRole - 1);
+#endif
+#ifndef RTABMAP_APRILTAG
+	_ui->MarkerStrategy->setItemData(1, 0, Qt::UserRole - 1);
 #endif
 
 #ifndef RTABMAP_MADGWICK
@@ -758,6 +760,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->source_checkBox_ignoreFeatures, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_ignorePriors, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_ignoreIMU, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->source_checkBox_intermediateNodesAreNormalNodes, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_databaseStartId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_databaseStopId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1269,6 +1272,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kOptimizerVarianceIgnored().c_str());
 	_ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
 	_ui->graphOptimization_maxError->setObjectName(Parameters::kRGBDOptimizeMaxError().c_str());
+	_ui->graphOptimization_maxErrorRepairRadius->setObjectName(Parameters::kRGBDOptimizeMaxErrorRepairRadius().c_str());
 	_ui->graphOptimization_gravitySigma->setObjectName(Parameters::kOptimizerGravitySigma().c_str());
 	_ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kOptimizerEpsilon().c_str());
 	_ui->graphOptimization_robust->setObjectName(Parameters::kOptimizerRobust().c_str());
@@ -1613,6 +1617,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->toolButton_OdomVinsFusionPath, SIGNAL(clicked()), this, SLOT(changeOdometryVINSFusionConfigPath()));
 
 	// Odometry OpenVINS
+	_ui->lineEdit_openvinsConfigPath->setObjectName(Parameters::kOdomOpenVINSConfigPath().c_str());
+	connect(_ui->toolButton_openvinsConfigPath, SIGNAL(clicked()), this, SLOT(changeOdometryOpenVINSConfigPath()));
 	_ui->checkBox_OdomOpenVINSUseStereo->setObjectName(Parameters::kOdomOpenVINSUseStereo().c_str());
 	_ui->checkBox_OdomOpenVINSUseKLT->setObjectName(Parameters::kOdomOpenVINSUseKLT().c_str());
 	_ui->spinBox_OdomOpenVINSNumPts->setObjectName(Parameters::kOdomOpenVINSNumPts().c_str());
@@ -1698,6 +1704,22 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	// Odometry CuVSLAM
 	_ui->odom_cuvslam_multicam_mode->setObjectName(Parameters::kOdomCuVSLAMMulticamMode().c_str());
 
+	// Odometry LIO-SAM
+	_ui->lineEdit_OdomLIOSAMPath->setObjectName(Parameters::kOdomLIOSAMConfigPath().c_str());
+	connect(_ui->toolButton_OdomLIOSAMPath, SIGNAL(clicked()), this, SLOT(changeOdometryLIOSAMConfigPath()));
+	_ui->odom_liosam_sensor->setObjectName(Parameters::kOdomLIOSAMSensor().c_str());
+	_ui->odom_liosam_nscan->setObjectName(Parameters::kOdomLIOSAMNScan().c_str());
+	_ui->odom_liosam_horizon_scan->setObjectName(Parameters::kOdomLIOSAMHorizonScan().c_str());
+	_ui->odom_liosam_imu_acc_noise->setObjectName(Parameters::kOdomLIOSAMImuAccNoise().c_str());
+	_ui->odom_liosam_imu_gyr_noise->setObjectName(Parameters::kOdomLIOSAMImuGyrNoise().c_str());
+	_ui->odom_liosam_imu_acc_bias_n->setObjectName(Parameters::kOdomLIOSAMImuAccBiasN().c_str());
+	_ui->odom_liosam_imu_gyr_bias_n->setObjectName(Parameters::kOdomLIOSAMImuGyrBiasN().c_str());
+	_ui->odom_liosam_imu_gravity->setObjectName(Parameters::kOdomLIOSAMImuGravity().c_str());
+	_ui->odom_liosam_edge_threshold->setObjectName(Parameters::kOdomLIOSAMEdgeThreshold().c_str());
+	_ui->odom_liosam_surf_threshold->setObjectName(Parameters::kOdomLIOSAMSurfThreshold().c_str());
+	_ui->odom_liosam_linvar->setObjectName(Parameters::kOdomLIOSAMLinVar().c_str());
+	_ui->odom_liosam_angvar->setObjectName(Parameters::kOdomLIOSAMAngVar().c_str());
+
 	//StereoDense
 	_ui->comboBox_stereoDense_strategy->setObjectName(Parameters::kStereoDenseStrategy().c_str());
 	connect(_ui->comboBox_stereoDense_strategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_stereoDense, SLOT(setCurrentIndex(int)));
@@ -1730,18 +1752,30 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->stereosgbm_mode->setObjectName(Parameters::kStereoSGBMMode().c_str());
 
 	// Aruco marker
-	_ui->ArucoDictionary->setObjectName(Parameters::kMarkerDictionary().c_str());
-	_ui->ArucoMarkerLength->setObjectName(Parameters::kMarkerLength().c_str());
-	_ui->ArucoMaxDepthError->setObjectName(Parameters::kMarkerMaxDepthError().c_str());
-	_ui->ArucoVarianceLinear->setObjectName(Parameters::kMarkerVarianceLinear().c_str());
-	_ui->ArucoVarianceAngular->setObjectName(Parameters::kMarkerVarianceAngular().c_str());
-	_ui->ArucoVarianceOrientationIgnored->setObjectName(Parameters::kMarkerVarianceOrientationIgnored().c_str());
-	_ui->ArucoMarkerRangeMin->setObjectName(Parameters::kMarkerMinRange().c_str());
-	_ui->ArucoMarkerRangeMax->setObjectName(Parameters::kMarkerMaxRange().c_str());
-	_ui->ArucoMarkerPriors->setObjectName(Parameters::kMarkerPriors().c_str());
-	_ui->ArucoPriorsVarianceLinear->setObjectName(Parameters::kMarkerPriorsVarianceLinear().c_str());
-	_ui->ArucoPriorsVarianceAngular->setObjectName(Parameters::kMarkerPriorsVarianceAngular().c_str());
-	_ui->ArucoCornerRefinementMethod->setObjectName(Parameters::kMarkerCornerRefinementMethod().c_str());
+	_ui->MarkerStrategy->setObjectName(Parameters::kMarkerStrategy().c_str());
+	connect(_ui->MarkerStrategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_markerStrategy, SLOT(setCurrentIndex(int)));
+	connect(_ui->MarkerStrategy, SIGNAL(currentIndexChanged(int)), this, SLOT(updateAvailableMarkerDictionaries()));
+	_ui->MarkerStrategy->setCurrentIndex(Parameters::defaultMarkerStrategy());
+	updateAvailableMarkerDictionaries();
+	_ui->MarkerDictionary->setObjectName(Parameters::kMarkerDictionary().c_str());
+	_ui->MarkerLength->setObjectName(Parameters::kMarkerLength().c_str());
+	_ui->MarkerLengths->setObjectName(Parameters::kMarkerLengths().c_str());
+	_ui->MarkerMaxDepthError->setObjectName(Parameters::kMarkerMaxDepthError().c_str());
+	_ui->MarkerVarianceLinear->setObjectName(Parameters::kMarkerVarianceLinear().c_str());
+	_ui->MarkerVarianceAngular->setObjectName(Parameters::kMarkerVarianceAngular().c_str());
+	_ui->MarkerVarianceOrientationIgnored->setObjectName(Parameters::kMarkerVarianceOrientationIgnored().c_str());
+	_ui->MarkerRangeMin->setObjectName(Parameters::kMarkerMinRange().c_str());
+	_ui->MarkerRangeMax->setObjectName(Parameters::kMarkerMaxRange().c_str());
+	_ui->MarkerPriors->setObjectName(Parameters::kMarkerPriors().c_str());
+	_ui->MarkerPriorsVarianceLinear->setObjectName(Parameters::kMarkerPriorsVarianceLinear().c_str());
+	_ui->MarkerPriorsVarianceAngular->setObjectName(Parameters::kMarkerPriorsVarianceAngular().c_str());
+	_ui->OpenCVCornerRefinementMethod->setObjectName(Parameters::kMarkerOpenCVCornerRefinementMethod().c_str());
+	_ui->apriltag_nthreads->setObjectName(Parameters::kMarkerAprilTagNThreads().c_str());
+	_ui->apriltag_quad_decimate->setObjectName(Parameters::kMarkerAprilTagQuadDecimate().c_str());
+	_ui->apriltag_quad_sigma->setObjectName(Parameters::kMarkerAprilTagQuadSigma().c_str());
+	_ui->apriltag_refine_edges->setObjectName(Parameters::kMarkerAprilTagRefineEdges().c_str());
+	_ui->apriltag_decode_sharpening->setObjectName(Parameters::kMarkerAprilTagDecodeSharpening().c_str());
+	_ui->apriltag_debug->setObjectName(Parameters::kMarkerAprilTagDebug().c_str());
 
 	// IMU filter
 	_ui->doubleSpinBox_imuFilterMadgwickGain->setObjectName(Parameters::kImuFilterMadgwickGain().c_str());
@@ -2209,6 +2243,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->source_checkBox_ignoreFeatures->setChecked(true);
 		_ui->source_checkBox_ignorePriors->setChecked(false);
 		_ui->source_checkBox_ignoreIMU->setChecked(false);
+		_ui->source_checkBox_intermediateNodesAreNormalNodes->setChecked(false);
 		_ui->source_spinBox_databaseStartId->setValue(0);
 		_ui->source_spinBox_databaseStopId->setValue(0);
 		_ui->source_lineEdit_databaseCameraIndex->setText("");
@@ -2971,6 +3006,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->source_checkBox_ignoreFeatures->setChecked(settings.value("ignoreFeatures", _ui->source_checkBox_ignoreFeatures->isChecked()).toBool());
 	_ui->source_checkBox_ignorePriors->setChecked(settings.value("ignorePriors", _ui->source_checkBox_ignorePriors->isChecked()).toBool());
 	_ui->source_checkBox_ignoreIMU->setChecked(settings.value("ignoreImu", _ui->source_checkBox_ignoreIMU->isChecked()).toBool());
+	_ui->source_checkBox_intermediateNodesAreNormalNodes->setChecked(settings.value("intermediateNodesAreNormalNodes", _ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked()).toBool());
 	
 	_ui->source_spinBox_databaseStartId->setValue(settings.value("startId", _ui->source_spinBox_databaseStartId->value()).toInt());
 	_ui->source_spinBox_databaseStopId->setValue(settings.value("stopId", _ui->source_spinBox_databaseStopId->value()).toInt());
@@ -3591,6 +3627,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("ignoreFeatures",  _ui->source_checkBox_ignoreFeatures->isChecked());
 	settings.setValue("ignorePriors",  _ui->source_checkBox_ignorePriors->isChecked());
 	settings.setValue("ignoreImu",  _ui->source_checkBox_ignoreIMU->isChecked());
+	settings.setValue("intermediateNodesAreNormalNodes", _ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked());
 	settings.setValue("startId",          _ui->source_spinBox_databaseStartId->value());
 	settings.setValue("stopId",          _ui->source_spinBox_databaseStopId->value());
 	settings.setValue("cameraIndices",       _ui->source_lineEdit_databaseCameraIndex->text());
@@ -3914,15 +3951,36 @@ bool PreferencesDialog::validateForm()
 		_ui->checkbox_odomDisabled->setChecked(false);
 	}
 
-#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
-	if(_ui->ArucoDictionary->currentIndex()>=17)
-	{
-		QMessageBox::warning(this, tr("Parameter warning"),
-				tr("ArUco dictionary: cannot select AprilTag dictionary, OpenCV version should be at least 3.4.2. Setting back to 0."));
-		_ui->ArucoDictionary->setCurrentIndex(0);
-	}
-#endif
 
+	if(_ui->MarkerStrategy->currentIndex() == 0)
+	{
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
+		if(_ui->MarkerDictionary->currentIndex()>=17)
+		{
+			QMessageBox::warning(this, tr("Parameter warning"),
+					tr("opencv-aruco: cannot use the selected dictionary (%1), OpenCV version should be at least 3.4.2. Setting back to 0.").arg(_ui->MarkerDictionary->currentIndex()));
+			_ui->MarkerDictionary->setCurrentIndex(0);
+		}
+#elif CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <8)
+		if(_ui->MarkerDictionary->currentIndex()>=21)
+		{
+			QMessageBox::warning(this, tr("Parameter warning"),
+					tr("Opencv Strategy: cannot use selected dictionary (%1), OpenCV version should be at least 4.8.0. Setting back to 0.").arg(_ui->MarkerDictionary->currentIndex()));
+			_ui->MarkerDictionary->setCurrentIndex(0);
+		}
+#endif
+	}
+	else if(_ui->MarkerStrategy->currentIndex() == 1)
+	{
+#ifndef RTABMAP_APRILTAG_WITH_ARUCO
+		if(_ui->MarkerDictionary->currentIndex() < 17 || _ui->MarkerDictionary->currentIndex() == 21)
+		{
+			QMessageBox::warning(this, tr("Parameter warning"),
+					tr("AprilTag Strategy: cannot use selected dictionary (%1), AprilTag should be built with aruco support. Setting back to 17.").arg(_ui->MarkerDictionary->currentIndex()));
+			_ui->MarkerDictionary->setCurrentIndex(17);
+		}
+#endif
+	}
 	return true;
 }
 
@@ -5549,6 +5607,64 @@ void PreferencesDialog::updateGlobalDescriptorVisibility()
 	_ui->groupBox_pydescriptor->setVisible(_ui->comboBox_globalDescriptorExtractor->currentIndex() == 1);
 }
 
+void PreferencesDialog::updateAvailableMarkerDictionaries()
+{
+	Qt::ItemFlags enableFlags = Qt::ItemFlags(Qt::ItemIsEnabled) | Qt::ItemIsSelectable;
+	for(int i=0;i<_ui->MarkerDictionary->count();++i) {
+		_ui->MarkerDictionary->setItemData(i, QVariant(static_cast<int>(enableFlags)), Qt::UserRole - 1); 
+	}
+
+	if(_ui->MarkerStrategy->currentIndex() == 1) // AprilTag Strategy is selected
+	{
+		// ARUCO_ORIGINAL not available with AprilTag lib
+		_ui->MarkerDictionary->setItemData(16, 0, Qt::UserRole - 1);
+#ifndef RTABMAP_APRILTAG_WITH_ARUCO
+		// disable all aruco dictionaries
+		for(int i=0;i<17;++i) {
+			_ui->MarkerDictionary->setItemData(i, 0, Qt::UserRole - 1); 
+		}
+		_ui->MarkerDictionary->setItemData(21, 0, Qt::UserRole - 1);
+		if(_ui->MarkerDictionary->currentIndex() < 17 || _ui->MarkerDictionary->currentIndex() > 20)
+		{
+			_ui->MarkerDictionary->setCurrentIndex(20); // 36h11 by default
+		}
+#endif
+	}
+	else //if(_ui->MarkerStrategy->currentIndex() == 0) // OpenCV Strategy is selected
+	{
+		//if OpenCV < 3.4.2
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
+		// disable all apriltag dictionaries
+		for(int i=17;i<21;++i) {
+			_ui->MarkerDictionary->setItemData(i, 0, Qt::UserRole - 1); 
+		}
+		if(_ui->MarkerDictionary->currentIndex() >=17 && _ui->MarkerDictionary->currentIndex() <= 20)
+		{
+			_ui->MarkerDictionary->setCurrentIndex(Parameters::defaultMarkerDictionary());
+		}
+#else
+		if(_ui->MarkerDictionary->currentIndex() >=17 && _ui->MarkerDictionary->currentIndex() <= 20)
+		{
+			// If apriltag is selected, select apriltag refinement by default
+			_ui->OpenCVCornerRefinementMethod->setCurrentIndex(3);
+		}
+		else if(_ui->OpenCVCornerRefinementMethod->currentIndex() == 3)
+		{
+			// If not apriltag dictionary selected, reset refinement to default.
+			_ui->OpenCVCornerRefinementMethod->setCurrentIndex(Parameters::defaultMarkerOpenCVCornerRefinementMethod());
+		}
+#endif
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <8)
+		// disable aruco MPI dictionary	
+		_ui->MarkerDictionary->setItemData(21, 0, Qt::UserRole - 1);
+		if(_ui->MarkerDictionary->currentIndex() == 21)
+		{
+			_ui->MarkerDictionary->setCurrentIndex(Parameters::defaultMarkerDictionary());
+		}
+#endif
+	}
+}
+
 void PreferencesDialog::updateOdometryStackedIndex(int index)
 {
 	if(index == 11) // FLOAM -> LOAM
@@ -5572,6 +5688,7 @@ void PreferencesDialog::updateOdometryStackedIndex(int index)
 	_ui->groupBox_odomOpenVINS->setVisible(index==10);
 	_ui->groupBox_odomOpen3D->setVisible(index==12);
 	_ui->groupBox_odomCuvslam->setVisible(index==13);
+	_ui->groupBox_odomLIOSAM->setVisible(index==14);
 }
 
 void PreferencesDialog::useOdomFeatures()
@@ -5677,6 +5794,40 @@ 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;
+	if(_ui->lineEdit_OdomLIOSAMPath->text().isEmpty())
+	{
+		path = QFileDialog::getOpenFileName(this, tr("LIO-SAM Config"), this->getWorkingDirectory(), tr("LIO-SAM config (*.yaml)"));
+	}
+	else
+	{
+		path = QFileDialog::getOpenFileName(this, tr("LIO-SAM Config"), _ui->lineEdit_OdomLIOSAMPath->text(), tr("LIO-SAM config (*.yaml)"));
+	}
+	if(!path.isEmpty())
+	{
+		_ui->lineEdit_OdomLIOSAMPath->setText(path);
+	}
+}
+
 void PreferencesDialog::changeOdometryOpenVINSLeftMask()
 {
 	QString path;
@@ -6195,7 +6346,7 @@ bool PreferencesDialog::isMarkerDetection() const
 }
 double PreferencesDialog::getMarkerLength() const
 {
-	return _ui->ArucoMarkerLength->value();
+	return _ui->MarkerLength->value();
 }
 bool PreferencesDialog::isCloudMeshing() const
 {
@@ -7265,13 +7416,14 @@ Camera * PreferencesDialog::createCamera(
 				_ui->source_spinBox_databaseStartId->value(),
 				cameraIndices,
 				_ui->source_spinBox_databaseStopId->value(),
-				!_ui->general_checkBox_createIntermediateNodes->isChecked(),
+				!_ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked() && !_ui->general_checkBox_createIntermediateNodes->isChecked(),
 				_ui->source_checkBox_ignoreLandmarks->isChecked(),
 				_ui->source_checkBox_ignoreFeatures->isChecked(),
 				0,
 				-1,
 				_ui->source_checkBox_ignorePriors->isChecked(),
 				_ui->source_checkBox_ignoreIMU->isChecked(),
+				_ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked(),
 				localTransformOverrides);
 	}
 	else