missing ressources

* Fixing prior and gravity constraints support for SBA with orbslam dependency

* Updated an error log msg

.appveyor.yml

@@ -1,152 +0,0 @@
-
-branches:
-  only:
-    - master
-    - devel
-
-os: Visual Studio 2015
-
-clone_folder: c:\projects\rtabmap
-
-platform: x64
-configuration: Release
-
-init:
-  - cmake --version
-  - call "C:\Program Files\Microsoft SDKs\Windows\v7.1\Bin\SetEnv.cmd" /x64
-  - call "C:\Program Files (x86)\Microsoft Visual Studio 14.0\VC\vcvarsall.bat" x86_amd64
-
-install:
-  # To download from google drive
-  - set PATH=C:\Python38-x64;C:\Python38-x64\Scripts;%PATH%
-  - ps: py -m pip --disable-pip-version-check install gdown>=5.1.0 
-  # Qt
-  - set QTDIR=C:\Qt\5.10.1\msvc2015_64
-  # make sure Qt bin path is before cmake bin path to avoid copying qt5 dlls from cmake before qt installation
-  - set PATH=%QTDIR%\bin;%PATH%
-  # Boost
-  - set PATH=%PATH%;C:\Libraries\boost_1_62_0\lib64-msvc-14.0
-  # Openni2
-  - ps: wget 'https://dl.dropboxusercontent.com/s/d98jv79l6oy9fxz/OpenNI2.exe?dl=0' -outfile OpenNI2.exe
-  - cmd: OpenNI2.exe -o"C:\Program Files" -y
-  - ECHO "Installed OpenNI2:"
-  - ps: "ls \"C:/Program Files/OpenNI2\""
-  - set PATH=%PATH%;C:\Program Files\OpenNI2\Redist
-  - set OPENNI2_INCLUDE64=C:\Program Files\OpenNI2\Include
-  - set OPENNI2_LIB64=C:\Program Files\OpenNI2\Lib
-  - set OPENNI2_REDIST64=C:\Program Files\OpenNI2\Redist
-  # OpenCV
-  #- appveyor-retry appveyor DownloadFile http://downloads.sourceforge.net/project/opencvlibrary/4.5.2/opencv-4.5.2-vc14_vc15.exe
-  #- cmd: opencv-4.5.2-vc14_vc15.exe -o"C:\Program Files" -y
-  #- ECHO "Installed OpenCV:"
-  #- ps: "ls \"C:/Program Files/opencv/build\""
-  #- set PATH=%PATH%;C:\Program Files\opencv\build\x64\vc14\bin
-  - ps: wget 'https://dl.dropboxusercontent.com/s/o6ofn491bc0jso1/opencv450_vc14.exe?dl=0' -outfile opencv.exe
-  - cmd: opencv.exe -o"C:\Program Files" -y
-  - ECHO "Installed OpenCV:"
-  - ps: "ls \"C:/Program Files/opencv\""
-  - set PATH=%PATH%;C:\Program Files\opencv\x64\vc14\bin
-  # VTK (including QVTK)
-  - ps: wget 'https://dl.dropboxusercontent.com/s/1l33b5l3f3y52gf/VTK-6_3-msvc140.exe?dl=0' -outfile VTK-6_3.exe
-  - cmd: VTK-6_3.exe -o"C:\Program Files" -y
-  - ECHO "Installed VTK:"
-  - ps: "ls \"C:/Program Files/VTK\""
-  - set PATH=%PATH%;C:\Program Files\VTK\bin
-  # QHull 
-  - ps: wget 'https://dl.dropboxusercontent.com/s/9widnk9msdsh2b8/Qhull-msvc140.exe?dl=0' -outfile Qhull.exe
-  - cmd: Qhull.exe -o"C:\Program Files" -y
-  - ECHO "Installed QHull:"
-  - ps: "ls \"C:/Program Files/Qhull\""
-  - set PATH=%PATH%;C:\Program Files\Qhull\bin
-  # FLANN 
-  - ps: wget 'https://dl.dropboxusercontent.com/s/7k58jbmqa51sxmh/FLANN-msvc140.exe?dl=0' -outfile FLANN.exe
-  - cmd: FLANN.exe -o"C:\Program Files" -y
-  - ECHO "Installed FLANN:"
-  - ps: "ls \"C:/Program Files/FLANN\""
-  - set PATH=%PATH%;C:\Program Files\FLANN\bin
-  # Eigen 
-  - ps: wget 'https://dl.dropboxusercontent.com/s/3v6i9i8dxj4o8ji/Eigen.exe?dl=0' -outfile Eigen.exe
-  - cmd: Eigen.exe -o"C:\Program Files" -y
-  - ECHO "Installed Eigen:"
-  - ps: "ls \"C:/Program Files/Eigen\""
-  # PCL
-  - ps: wget 'https://dl.dropboxusercontent.com/s/2iayr4lyqa50i9j/PCL_181_August2018_x64_vc14.exe?dl=0' -outfile PCL_1.8.1.exe
-  - cmd: PCL_1.8.1.exe -o"C:\Program Files" -y
-  - ECHO "Installed PCL:"
-  - ps: "ls \"C:/Program Files/PCL\""
-  - set PATH=%PATH%;C:\Program Files\PCL\bin
-  # zlib
-  - ps: gdown -q 0B46akLGdg-uaYm9MTTI4MUtUcmc
-  - ps: Expand-Archive zlib-1.2.8-vc2010-x64.zip -DestinationPath 'C:\Program Files'
-  - ECHO "Installed zlib:"
-  - ps: "ls \"C:/Program Files/zlib\""
-  - set PATH=%PATH%;C:\Program Files\zlib\bin
-  # g2o
-  - ps: wget 'https://dl.dropboxusercontent.com/s/ht74s5pa21wokzw/g2o.exe?dl=0' -outfile g2o.exe
-  - cmd: g2o.exe -o"C:\Program Files" -y
-  - ECHO "Installed g2o:"
-  - ps: "ls \"C:/Program Files/g2o\""
-  - set PATH=%PATH%;C:\Program Files\g2o\bin
-  # GTSAM
-  - ps: wget 'https://dl.dropboxusercontent.com/s/0fpr6r4cgsqmvhf/GTSAM-4_0_0_alpha2-msvc140.exe?dl=0' -outfile GTSAM.exe
-  - cmd: GTSAM.exe -o"C:\Program Files" -y
-  - ECHO "Installed GTSAM:"
-  - ps: "ls \"C:/Program Files/GTSAM\""
-  - set PATH=%PATH%;C:\Program Files\GTSAM\bin
-  # OctoMap
-  - ps: wget 'https://dl.dropboxusercontent.com/s/6jpxu0nm8ne6e54/octomap_x64_vc14.exe?dl=0' -outfile octomap.exe
-  - cmd: octomap.exe -o"C:\Program Files" -y
-  - ECHO "Installed OctoMap:"
-  - ps: "ls \"C:/Program Files/octomap-distribution\""
-  - set PATH=%PATH%;C:\Program Files\octomap-distribution\bin
-  # CPU-TSDF
-  - ps: wget 'https://dl.dropboxusercontent.com/s/mgges9va1uzxr0q/cpu_tsdf_sept2015_x64_vc14.exe?dl=0' -outfile cpu_tsdf.exe
-  - cmd: cpu_tsdf.exe -o"C:\Program Files" -y
-  - ECHO "Installed CPU-TSDF:"
-  - ps: "ls \"C:/Program Files/cpu_tsdf\""
-  - set PATH=%PATH%;C:\Program Files\cpu_tsdf\bin
-  # Open Chisel
-  - ps: wget 'https://dl.dropboxusercontent.com/s/0aaphcde4acrinm/open_chisel_x64_vc14.exe?dl=0' -outfile open_chisel.exe
-  - cmd: open_chisel.exe -o"C:\Program Files" -y
-  - ECHO "Installed Open Chisel:"
-  - ps: "ls \"C:/Program Files/open_chisel\""
-  - set PATH=%PATH%;C:\Program Files\open_chisel\bin
-  # yaml-cpp
-  - ps: wget 'https://dl.dropboxusercontent.com/s/22qfvftwj6zq8tj/yaml-cpp_x64_vc14.exe?dl=0' -outfile yaml-cpp.exe
-  - cmd: yaml-cpp.exe -o"C:\Program Files" -y
-  - ECHO "Installed yaml-cpp:"
-  - ps: "ls \"C:/Program Files/yaml-cpp\""
-  # RealSense2
-  - ps: wget 'https://github.com/IntelRealSense/librealsense/releases/download/v2.40.0/Intel.RealSense.SDK-WIN10-2.40.0.2482.exe' -outfile realsense2.exe
-  - cmd: realsense2.exe /VERYSILENT
-  - ECHO "Installed RealSense2:"
-  - ps: "ls \"C:/Program Files (x86)/Intel RealSense SDK 2.0\""
-  - set PATH=%PATH%;C:\Program Files (x86)\Intel RealSense SDK 2.0\bin\x64
-  - set RealSense2_ROOT_DIR=C:\Program Files (x86)\Intel RealSense SDK 2.0
-  # Kinect 4 Azure
-  - ps: wget 'https://download.microsoft.com/download/3/d/6/3d6d9e99-a251-4cf3-8c6a-8e108e960b4b/Azure%20Kinect%20SDK%201.4.1.exe' -outfile azure.exe
-  - cmd: azure.exe /quiet
-  - ECHO "Installed Kinect For Azure:"
-  - ps: "ls \"C:/Program Files/Azure Kinect SDK v1.4.1\""
-  - set PATH=%PATH%;C:\Program Files\Azure Kinect SDK v1.4.1\tools
-  - set K4A_ROOT_DIR=C:\Program Files\Azure Kinect SDK v1.4.1
-  
-before_build:
-  - cd c:\projects\rtabmap\build
-  - ECHO %PROGRAMFILES%
-  - ECHO %PATH%
-  - cmake -G "Visual Studio 14 2015 Win64" -DOpenCV_DIR="C:\Program Files\opencv\build" -DPCL_DIR="C:\Program Files\PCL\cmake" -DCPUTSDF_DIR="C:\Program Files\cpu_tsdf\share\cpu_tsdf" -Dyaml-cpp_DIR="C:\Program Files\yaml-cpp\CMake" -DBUILD_AS_BUNDLE=ON ..
-  
-after_build :
-  - cmake --build . --config Release --target package
-
-artifacts:
-  - path: build\RTABMap-*
-  
-notifications:
-  - provider: Email
-    to:
-      - matlabbe@gmail.com
-    on_build_success: false
-    on_build_failure: false
-    on_build_status_changed: true

.github/actions/install-windows-cuda-deps/action.yml

@@ -0,0 +1,53 @@
+name: 'Install Windows Dependencies with CUDA'
+description: 'Installs PCL, Qt, VTK, g2o and others'
+runs:
+  using: "composite"
+  steps:
+    - name: Set up MSVC Developer Command Prompt
+      uses: ilammy/msvc-dev-cmd@v1
+      with:
+        arch: x64 
+  
+    - name: Install CUDA
+      uses: Jimver/cuda-toolkit@v0.2.30
+      id: cuda-toolkit
+      with:
+        cuda: '13.0.0'
+        use-github-cache: True
+
+    - name: Verify CUDA
+      shell: bash
+      run: |
+        nvcc --version
+        echo "CUDA Path: $CUDA_PATH"
+  
+    - name: Cache vcpkg
+      id: cache-vcpkg
+      uses: actions/cache@v4
+      with:
+        path: ${{ runner.workspace }}/vcpkg_installed
+        key: ${{ runner.os }}-vcpkg-export-66c0373d-x64-vs2022-cuda130_v1
+
+    - name: Download and Install vcpkg
+      if: steps.cache-vcpkg.outputs.cache-hit != 'true'
+      shell: pwsh
+      run: |
+        $install_dir = "${{ runner.workspace }}\vcpkg_installed"
+        $archivePath = "${{ runner.workspace }}\vcpkg-export.7z"
+
+        # The file has been built locally with bundle-windows-deps.bat
+        $url = "https://github.com/introlab/rtabmap/releases/download/0.23.1/vcpkg-export-66c0373d-x64-vs2022-cuda130.7z"
+
+        Invoke-WebRequest -Uri $url -OutFile $archivePath
+        & 7z x $archivePath "-o$install_dir" -y
+
+    - name: Add vcpkg to PATH and env variable
+      shell: pwsh
+      run: |
+        $vcpkg_path = "${{ runner.workspace }}\vcpkg_installed"
+        echo "VCPKG_EXPORT_PATH=$vcpkg_path" | Out-File -FilePath $env:GITHUB_ENV -Encoding utf8 -Append
+        echo "$vcpkg_path\installed\x64-windows-release\bin" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append     
+        echo "${{env.CUDA_PATH}}\bin\x64" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append
+        echo "${{env.CUDA_PATH}}\extras\CUPTI\lib64" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append
+        echo "$vcpkg_path\installed\x64-windows-release\tools\python3\Lib\site-packages\torch\lib" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append
+        echo "$vcpkg_path\installed\x64-windows-release\tools\python3\Lib\site-packages\numpy.libs" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append

.github/actions/install-windows-deps/action.yml

@@ -0,0 +1,37 @@
+name: 'Install Windows Dependencies'
+description: 'Installs PCL, Qt, VTK, g2o and others'
+runs:
+  using: "composite"
+  steps:
+    - name: Set up MSVC Developer Command Prompt
+      uses: ilammy/msvc-dev-cmd@v1
+      with:
+        arch: x64 
+  
+    - name: Cache vcpkg
+      id: cache-vcpkg
+      uses: actions/cache@v4
+      with:
+        path: ${{ runner.workspace }}/vcpkg_installed
+        key: ${{ runner.os }}-vcpkg-export-66c0373d-x64-vs2022-v4
+
+    - name: Download and Install vcpkg
+      if: steps.cache-vcpkg.outputs.cache-hit != 'true'
+      shell: pwsh
+      run: |
+        $install_dir = "${{ runner.workspace }}\vcpkg_installed"
+        $archivePath = "${{ runner.workspace }}\vcpkg-export.7z"
+
+        # The file has been built locally with bundle-windows-deps.bat
+        $url = "https://github.com/introlab/rtabmap/releases/download/0.23.1/vcpkg-export-66c0373d-x64-vs2022.7z"
+
+        Invoke-WebRequest -Uri $url -OutFile $archivePath
+        & 7z x $archivePath "-o$install_dir" -y
+
+    - name: Add vcpkg to PATH and env variable
+      shell: pwsh
+      run: |
+        $vcpkg_path = "${{ runner.workspace }}\vcpkg_installed"
+        echo "VCPKG_EXPORT_PATH=$vcpkg_path" | Out-File -FilePath $env:GITHUB_ENV -Encoding utf8 -Append
+        echo "$vcpkg_path\installed\x64-windows-release\bin" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append
+        echo "$vcpkg_path\installed\x64-windows-release\tools\python3\Lib\site-packages\numpy.libs" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append

.github/workflows/cleanup-pr-artifacts.yml

@@ -0,0 +1,15 @@
+name: Cleanup PR Artifacts
+on:
+  pull_request:
+    types: [closed]
+
+jobs:
+  delete-artifacts:
+    runs-on: ubuntu-latest
+    permissions:
+      actions: write
+    steps:
+      - name: Delete PR Artifacts
+        uses: geekyeggo/delete-artifact@v5
+        with:
+          name: build-output-*

.github/workflows/cmake-linux.yml

@@ -1,4 +1,4 @@
-name: CMake
+name: CMake-Linux
 
 on:
   push:
@@ -11,31 +11,41 @@ on:
 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.os }}
+    name: ${{ matrix.build_name }}
     runs-on: ${{ matrix.os }}
     strategy:
-      fail-fast: false
+      fail-fast: true
       matrix:
-        os: [ubuntu-24.04, ubuntu-22.04]
+        build_name: [ubuntu-22.04, ubuntu-24.04, ubuntu-24.04-with-opengv]
         include:
-        - os: ubuntu-22.04
+        - build_name: ubuntu-22.04
+          os: ubuntu-22.04
           extra_deps: "libunwind-dev libceres-dev"
-          extra_cmake_def: ""
-        - os: ubuntu-24.04
+          extra_cmake_def: "-DWITH_CERES=ON"
+        - build_name: ubuntu-24.04
+          os: ubuntu-24.04
           extra_deps: "libg2o-dev libceres-dev"
           extra_cmake_def: "-DWITH_CERES=ON"
+        - build_name: ubuntu-24.04-with-opengv
+          os: ubuntu-24.04
+          extra_deps: "libg2o-dev libceres-dev"
+          extra_cmake_def: "-DWITH_CERES=ON -DBUILD_OPENGV=ON"
 
-    steps:    
-    - name: Install dependencies
+    steps:
+    - uses: actions/checkout@v4
+
+    - name: Install Linux Dependencies
       run: |
         DEBIAN_FRONTEND=noninteractive
         sudo apt-get update
         sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev ${{ matrix.extra_deps }}
 
-    - uses: actions/checkout@v4
-
     - name: Configure CMake
       run: |
         cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{ matrix.extra_cmake_def }}

.github/workflows/cmake-ros.yml

@@ -12,13 +12,17 @@ env:
   # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
   BUILD_TYPE: Release
 
+concurrency:
+  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
+  cancel-in-progress: ${{ github.event_name == 'pull_request' }}
+
 jobs:
   build:
     # The CMake configure and build commands are platform agnostic and should work equally
     # well on Windows or Mac.  You can convert this to a matrix build if you need
     # cross-platform coverage.
     # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
-    name: Build on ros ${{ matrix.ros_distribution }} and ${{ matrix.os }}
+    name: ${{ matrix.ros_distribution }}-${{ matrix.os }}
     runs-on: ${{ matrix.os }}
     strategy:
       fail-fast: false

.github/workflows/cmake-windows.yml

@@ -0,0 +1,107 @@
+name: CMake-Windows
+
+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: [windows-2022, windows-2022-cuda]
+        include:
+        - build_name: windows-2022
+          os: windows-2022
+          extra_deps: ""
+          extra_cmake_def: '-DBUILD_AS_BUNDLE=ON -DWITH_PYTHON=ON -DWITH_TORCH=OFF'
+        - build_name: windows-2022-cuda
+          os: windows-2022
+          extra_deps: ""
+          extra_cmake_def: '-DBUILD_AS_BUNDLE=ON -DWITH_PYTHON=ON -DWITH_TORCH=ON'
+
+    steps:
+    - uses: actions/checkout@v4
+
+    - name: Install Windows Dependencies
+      if: matrix.build_name == 'windows-2022'
+      uses: ./.github/actions/install-windows-deps
+ 
+    - name: Install Windows Dependencies with CUDA
+      if: matrix.build_name == 'windows-2022-cuda'
+      uses: ./.github/actions/install-windows-cuda-deps
+
+    - name: Configure CMake
+      run: |
+        cmake `
+          -B ${{github.workspace}}/build `
+          -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} `
+          ${{ matrix.extra_cmake_def }} `
+          -DVCPKG_MANIFEST_INSTALL=OFF `
+          -DVCPKG_TARGET_TRIPLET=x64-windows-release `
+          -DVCPKG_INSTALLED_DIR="${{env.VCPKG_EXPORT_PATH}}/installed" `
+          -DCMAKE_TOOLCHAIN_FILE=${{env.VCPKG_EXPORT_PATH}}/scripts/buildsystems/vcpkg.cmake `
+          -DTorch_DIR=${{env.VCPKG_EXPORT_PATH}}/installed/x64-windows-release/tools/python3/Lib/site-packages/torch/share/cmake/Torch
+
+    - name: Build
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+    - name: Build Windows Package
+      shell: pwsh
+      run: |
+        if ("${{ github.event_name }}" -eq "pull_request") {
+          cpack --config build/CPackConfig.cmake -G ZIP -B build
+        } else {
+          cmake --build ${{ github.workspace }}/build --config ${{ env.BUILD_TYPE }} --target package
+        }
+
+    - name: Rename CUDA artifacts
+      if: matrix.build_name == 'windows-2022-cuda'
+      shell: pwsh
+      run: Get-ChildItem -Path "build" -Filter "RTABMap-*" | Rename-Item -NewName { $_.BaseName + "_cuda" + $_.Extension }
+
+    - name: Info
+      working-directory: ${{github.workspace}}/build/bin
+      run: |
+        ./rtabmap-console --version
+
+    - name: Upload RTABMap Artifacts (ZIP)
+      uses: actions/upload-artifact@v4
+      with:
+        name: RTABMap-Binaries-${{ matrix.build_name }}-zip
+        path: |
+          build/RTABMap-*.zip
+        compression-level: 0
+        if-no-files-found: warn
+        retention-days: ${{ github.event_name == 'pull_request' && 1 || 90 }}
+
+    - name: Upload RTABMap Artifacts (Installer)
+      if: github.event_name != 'pull_request'
+      uses: actions/upload-artifact@v4
+      with:
+        name: RTABMap-Binaries-${{ matrix.build_name }}-exe
+        path: |
+          build/RTABMap-*.exe
+        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}}
+      

CMakeLists.txt

@@ -1,5 +1,6 @@
 # Top-Level CmakeLists.txt
 cmake_minimum_required(VERSION 3.14)
+
 PROJECT( RTABMap )
 SET(PROJECT_PREFIX rtabmap)
 
@@ -11,6 +12,7 @@ IF(NOT MULTI_ARCH AND NOT DEFINED CMAKE_INSTALL_LIBDIR)
 ENDIF(NOT MULTI_ARCH AND NOT DEFINED CMAKE_INSTALL_LIBDIR)
 
 INCLUDE(GNUInstallDirs)
+INCLUDE(FetchContent)
 
 ####### local cmake modules #######
 SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
@@ -19,8 +21,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 23)
-SET(RTABMAP_PATCH_VERSION 2)
+SET(RTABMAP_MINOR_VERSION 24)
+SET(RTABMAP_PATCH_VERSION 0)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -83,14 +85,6 @@ IF(MINGW)
     SET(CMAKE_SHARED_LINKER_FLAGS "-Wl,--enable-auto-import")
 ENDIF(MINGW)
 
-# GCC 4 required
-IF(UNIX OR MINGW)
-    EXEC_PROGRAM( gcc ARGS "-dumpversion" OUTPUT_VARIABLE GCC_VERSION )
-    IF(GCC_VERSION VERSION_LESS "4.0.0")
-        MESSAGE(FATAL_ERROR "GCC ${GCC_VERSION} found, but version 4.x.x minimum is required")
-    ENDIF(GCC_VERSION VERSION_LESS "4.0.0")
-ENDIF(UNIX OR MINGW)
-
 #The CDT Error Parser cannot handle error messages that span 
 #more than one line, which is the default gcc behavior. 
 #In order to force gcc to generate single line error messages with no line wrapping
@@ -226,6 +220,7 @@ option(WITH_MADGWICK      "Include Madgwick IMU filtering support" ON)
 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)
 IF(MOBILE_BUILD)
 option(PCL_OMP            "With PCL OMP implementations"         OFF)
 ELSE()
@@ -533,6 +528,16 @@ ENDIF(WITH_G2O)
 IF(WITH_GTSAM)
     # Force config mode to ignore PCL's findGTSAM.cmake file
     FIND_PACKAGE(GTSAM CONFIG QUIET)
+    IF(GTSAM_FOUND)
+      # For issue https://github.com/introlab/rtabmap/pull/1626
+      FIND_FILE(GTSAM_NOISE_MODEL_FACTOR_N_FILE gtsam/nonlinear/NoiseModelFactorN.h
+         PATHS ${GTSAM_INCLUDE_DIR}
+         NO_DEFAULT_PATH)
+      IF(GTSAM_NOISE_MODEL_FACTOR_N_FILE)
+         MESSAGE(STATUS "GTSAM with NoiseModelFactorN.h")
+         ADD_DEFINITIONS("-DGTSAM_WITH_NOISE_MODEL_FACTOR_N")
+      ENDIF(GTSAM_NOISE_MODEL_FACTOR_N_FILE)
+    ENDIF(GTSAM_FOUND)
 ENDIF(WITH_GTSAM)
 
 IF(WITH_MRPT)
@@ -637,7 +642,8 @@ IF(WITH_ZED)
        IF(CUDA_FOUND)
         MESSAGE(STATUS "Found CUDA: ${CUDA_INCLUDE_DIRS}")
        ELSE()
-        MESSAGE(FATAL_ERROR "CUDA is required to build with Zed sdk! Set -DWITH_ZED=OFF if you don't have CUDA.")
+        MESSAGE(WARNING "CUDA is required to build with Zed sdk! Set -DWITH_ZED=OFF if you don't have CUDA.")
+		SET(ZED_FOUND FALSE)
        ENDIF()
     ENDIF(ZED_FOUND)
 ENDIF(WITH_ZED)
@@ -782,8 +788,6 @@ IF(WITH_OKVIS)
        MESSAGE(STATUS "Found okvis: ${OKVIS_INCLUDE_DIRS}")
        find_package(brisk 2 REQUIRED)
        MESSAGE(STATUS "Found brisk: ${BRISK_INCLUDE_DIRS}")
-       find_package(opengv REQUIRED)
-       MESSAGE(STATUS "Found opengv: ${OPENGV_INCLUDE_DIRS}")
        find_package(Ceres 1.9.0 REQUIRED EXACT) # OKVIS requires this specific version
        MESSAGE(STATUS "Found ceres ${Ceres_VERSION}: ${CERES_INCLUDE_DIRS}")
     ENDIF(okvis_FOUND)
@@ -847,12 +851,58 @@ IF(WITH_FASTCV)
     ENDIF(FastCV_FOUND)
 ENDIF(WITH_FASTCV)
 
-IF(WITH_OPENGV)
-    FIND_PACKAGE(opengv QUIET)
-    IF(opengv_FOUND)
-       MESSAGE(STATUS "Found OpenGV: ${opengv_INCLUDE_DIRS}")
-    ENDIF(opengv_FOUND)
-ENDIF(WITH_OPENGV)
+IF(WITH_OPENGV OR okvis_FOUND)
+   if(NOT BUILD_OPENGV)
+      FIND_PACKAGE(opengv QUIET)
+   endif()
+
+   if(opengv_FOUND)
+      MESSAGE(STATUS "Found system-installed OpenGV: ${opengv_INCLUDE_DIRS}")
+   elseif(BUILD_OPENGV)
+      SET(PCL_USING_MARCHNATIVE OFF)
+      if(PCL_COMPILE_OPTIONS)
+         if("${PCL_COMPILE_OPTIONS}" MATCHES "-march=native")
+            set(PCL_USING_MARCHNATIVE ON)
+         endif()
+      elseif("${PCL_DEFINITIONS}" MATCHES "-march=native")
+         set(PCL_USING_MARCHNATIVE ON)
+      endif()
+      SET(MSG_EXTRA "without -march-native (not used by PCL)")
+      if(PCL_USING_MARCHNATIVE)
+         set(MSG_EXTRA "with -march-native (used by PCL)")
+      endif()
+   
+      message(STATUS "Download/Build OpenGV internally (BUILD_OPENGV=ON) ${MSG_EXTRA}.")     
+      function(add_submodule_opengv)
+         FetchContent_Declare(
+            opengv
+            GIT_REPOSITORY https://github.com/laurentkneip/opengv.git
+            GIT_TAG        91f4b19c73450833a40e463ad3648aae80b3a7f3
+            PATCH_COMMAND ${CMAKE_COMMAND} 
+                  -DPATCH_FILE=${CMAKE_CURRENT_LIST_DIR}/patches/opengv_91f4b19c.patch
+                  -P ${CMAKE_CURRENT_LIST_DIR}/patches/apply_patch.cmake
+         )
+         set(BUILD_SHARED_LIBS OFF)
+         set(BUILD_TESTS OFF)
+         set(CMAKE_BUILD_TYPE Release)
+         set(CMAKE_POLICY_DEFAULT_CMP0077 NEW)
+         # 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)
+            set(EIGEN_INCLUDE_DIR "${EIGEN_INCLUDE_DIRS}" CACHE PATH "Eigen include dir" FORCE)
+         ELSEIF(Eigen3_INCLUDE_DIRS)
+            set(EIGEN_INCLUDE_DIRS "${Eigen3_INCLUDE_DIRS}" CACHE PATH "Eigen include dirs" FORCE)
+            set(EIGEN_INCLUDE_DIR "${Eigen3_INCLUDE_DIRS}" CACHE PATH "Eigen include dir" FORCE)
+         ENDIF()
+         set(BUILD_WITH_MARCHNATIVE ${PCL_USING_MARCHNATIVE})
+         FetchContent_MakeAvailable(opengv)
+      endfunction()
+
+      add_submodule_opengv()
+      set(opengv_FOUND TRUE)
+      set(opengv_VERSION "internal")
+   endif()
+ENDIF()
 
 IF(WITH_ORB_SLAM AND NOT G2O_FOUND)
     FIND_PACKAGE(ORB_SLAM)
@@ -862,7 +912,7 @@ IF(WITH_ORB_SLAM AND NOT G2O_FOUND)
 ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND)
 
 IF(WITH_CUVSLAM)
-    FIND_PACKAGE(CuVSLAM)
+    FIND_PACKAGE(CuVSLAM 14.0.0)
     IF(CUVSLAM_FOUND)
         MESSAGE(STATUS "Found cuVSLAM: ${CUVSLAM_INCLUDE_DIRS}")
     ENDIF()
@@ -1283,7 +1333,17 @@ install(FILES package.xml DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_PR
 #######################
 IF(BUILD_AS_BUNDLE)
   SET(CMAKE_INSTALL_SYSTEM_RUNTIME_COMPONENT runtime)
+  IF(WIN32)
+    set(CMAKE_INSTALL_SYSTEM_RUNTIME_LIBS_SKIP TRUE)
+	set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS "
+      ExecWait '\\\"$INSTDIR\\\\vc_redist.x64.exe\\\" /quiet /norestart'
+    ")
+  ENDIF()
   INCLUDE(InstallRequiredSystemLibraries)
+  set(CPACK_NSIS_COMPONENT_INSTALL OFF)
+  set(CPACK_ARCHIVE_COMPONENT_INSTALL ON)
+  set(CPACK_COMPONENTS_GROUPING ALL_COMPONENTS_IN_ONE)
+  set(CPACK_COMPONENTS_ALL runtime)
 ENDIF(BUILD_AS_BUNDLE)
 
 SET(CPACK_PACKAGE_NAME "${PROJECT_NAME}")
@@ -1453,15 +1513,26 @@ MESSAGE(STATUS "  With ORB OcTree           = NO (WITH_ORB_OCTREE=OFF)")
 ENDIF()
 
 IF(TORCH_FOUND)
-MESSAGE(STATUS "  With SuperPoint          = YES (License: GPLv3) libtorch=${Torch_VERSION}")
+MESSAGE(STATUS "  With SuperPoint           = YES (License: GPLv3) libtorch=${Torch_VERSION}")
 ELSEIF(NOT WITH_TORCH)
-MESSAGE(STATUS "  With SuperPoint          = NO (WITH_TORCH=OFF)")
+MESSAGE(STATUS "  With SuperPoint           = NO (WITH_TORCH=OFF)")
 ELSE()
-MESSAGE(STATUS "  With SuperPoint          = NO (libtorch not found)")
+MESSAGE(STATUS "  With SuperPoint           = NO (libtorch not found)")
 ENDIF()
 
+IF(TORCH_FOUND AND WITH_PYTHON AND Python3_FOUND)
+MESSAGE(STATUS "  With Superpoint Rpautrat  = YES (Liscense: MIT) libtorch=${Torch_VERSION}")
+ELSEIF(NOT WITH_TORCH)
+MESSAGE(STATUS "  With Superpoint Rpautrat  = NO (WITH_TORCH=OFF)")
+ELSEIF(NOT WITH_PYTHON)
+MESSAGE(STATUS "  With Superpoint Rpautrat  = NO (WITH_PYTHON=OFF)")
+ELSE()
+MESSAGE(STATUS "  Wtih Superpoint Rpautrat  = NO (libtorch and/or python3 not found)")
+ENDIF()
+
+
 IF(WITH_PYTHON AND Python3_FOUND)
-MESSAGE(STATUS "  With Python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}            = YES (License: PSF)")
+MESSAGE(STATUS "  With Python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}           = YES (License: PSF)")
 ELSEIF(NOT WITH_PYTHON)
 MESSAGE(STATUS "  With Python3              = NO (WITH_PYTHON=OFF)")
 ELSE()
@@ -1591,7 +1662,11 @@ MESSAGE(STATUS "  With Open3D               = NO (Open3D not found)")
 ENDIF()
 
 IF(opengv_FOUND AND WITH_OPENGV)
+IF(opengv_VERSION STREQUAL "internal")
+MESSAGE(STATUS "  With OpenGV (internal)    = YES (License: BSD)")
+ELSE()
 MESSAGE(STATUS "  With OpenGV ${opengv_VERSION}           = YES (License: BSD)")
+ENDIF()
 ELSEIF(NOT WITH_OPENGV)
 MESSAGE(STATUS "  With OpenGV               = NO (WITH_OPENGV=OFF)")
 ELSE()
@@ -1706,7 +1781,7 @@ ELSE()
 MESSAGE(STATUS "  With FlyCapture2/Triclops = NO (Point Grey SDK not found)")
 ENDIF()
 
-IF(ZED_FOUND AND CUDA_FOUND)
+IF(ZED_FOUND)
 MESSAGE(STATUS "  With ZED                  = YES")
 ELSEIF(NOT WITH_ZED)
 MESSAGE(STATUS "  With ZED                  = NO (WITH_ZED=OFF)")

README.md

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

app/ios/RTABMapApp.xcodeproj/project.pbxproj

@@ -1078,7 +1078,7 @@
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.22\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.23\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",
@@ -1139,7 +1139,7 @@
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.22\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.23\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",

app/ios/RTABMapApp/install_deps.sh

@@ -288,6 +288,6 @@ cmake -DANDROID_PREBUILD=ON ../../../../..
 cmake --build . --config Release
 mkdir -p ios
 cd ios
-cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=ON  -DBUILD_EXAMPLES=OFF -DWITH_LIBLAS=ON ../../../../../..
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=ON  -DBUILD_EXAMPLES=OFF -DWITH_LIBLAS=ON -DWITH_OPENGV=OFF ../../../../../..
 cmake --build . --config Release
 cmake --build . --config Release --target install

app/src/CMakeLists.txt

@@ -95,9 +95,11 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
           DESTINATION ${thirdparty_dest_dir} 
           COMPONENT runtime 
           REGEX ".*pdb" EXCLUDE)
-    INSTALL(FILES "${OpenNI2_BIN_DIR}/OpenNI.ini" 
+	IF(NOT WIN32)
+		INSTALL(FILES "${OpenNI2_BIN_DIR}/OpenNI.ini" 
           DESTINATION ${thirdparty_dest_dir} 
           COMPONENT runtime)
+	ENDIF()
   ENDIF(OpenNI2_FOUND)
   
   IF(k4a_FOUND)
@@ -138,23 +140,107 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   ENDIF(OrbbecSDK_FOUND)
   
   IF(Torch_FOUND)
-	# Install needed cudnn_ops_infer64_8.dll and cudnn_cnn_infer64_8.dll
-	# TODO: should be a more general way to include them if version is different
+	# Install needed cudnn dlls
     IF(WIN32 AND CUDA_FOUND)
-		find_file(CUDNN_OPS_DLL NAMES cudnn_ops_infer64_8.dll)
-		find_file(CUDNN_CNN_DLL NAMES cudnn_cnn_infer64_8.dll)
-		IF(CUDNN_OPS_DLL AND CUDNN_CNN_DLL)
-			MESSAGE(STATUS "Found ${CUDNN_OPS_DLL}")
-			MESSAGE(STATUS "Found ${CUDNN_CNN_DLL}")
-			INSTALL(FILES ${CUDNN_OPS_DLL} ${CUDNN_CNN_DLL}
-			  DESTINATION ${thirdparty_dest_dir} 
-			  COMPONENT runtime)
-		ELSE()
-			MESSAGE(AUTHOR_WARNING "Using Torch with CUDA, but cudnn_ops_infer64_8.dll and cudnn_cnn_infer64_8.dll are not found on the PATH, so it won't be added to package.")
+	    find_path(cuDNN_BIN_DIR NAMES cudnn.dll cudnn64.dll cudnn64_9.dll)
+        IF(NOT cuDNN_BIN_DIR)
+            MESSAGE(FATAL_ERROR "cudnn dlls not found! Make sure the dlls are in a directory on your PATH.")
+        ENDIF(NOT cuDNN_BIN_DIR)
+		MESSAGE(STATUS "cuDNN_BIN_DIR = ${cuDNN_BIN_DIR}")
+		file(GLOB CUDNN_DLLS "${cuDNN_BIN_DIR}/cudnn*.dll")
+		IF(CUDNN_DLLS)
+			MESSAGE(STATUS "Found cuDNN DLLs: ${CUDNN_DLLS}")
+			INSTALL(FILES ${CUDNN_DLLS}
+					DESTINATION ${thirdparty_dest_dir} 
+					COMPONENT runtime)
 		ENDIF()
 	ENDIF(WIN32 AND CUDA_FOUND)
   ENDIF(Torch_FOUND)
+  
+  set(python_pyd_dir "")
+  IF(Python3_FOUND)
+	# bundle python3
+    IF(WIN32)
+		set(python_pyd_dir "bin/Lib/site-packages")
+		set(PYTHON_ZIP_NAME "python${Python3_VERSION_MAJOR}${Python3_VERSION_MINOR}.zip")
+		get_filename_component(PYTHON_ROOT "${Python3_EXECUTABLE}" DIRECTORY)
+		file(TO_CMAKE_PATH "${Python3_STDLIB}" SANITIZED_STDLIB)
 
+		MESSAGE(STATUS "Python3_EXECUTABLE=${Python3_EXECUTABLE}")
+		MESSAGE(STATUS "Python3_STDLIB=${SANITIZED_STDLIB}")
+		
+		install(FILES "${Python3_EXECUTABLE}" DESTINATION bin COMPONENT runtime)
+		
+		# when using python-opencv, it expects python3.dll, not python312.dll
+		get_filename_component(VCPKG_TRIPLET_ROOT "${PYTHON_TOOLS_DIR}/../../" ABSOLUTE)
+		set(VCPKG_BIN_DIR "${VCPKG_TRIPLET_ROOT}/bin")
+		find_file(PYTHON3_STABLE_DLL 
+			NAMES python3.dll 
+			PATHS "${VCPKG_BIN_DIR}"
+			NO_DEFAULT_PATH
+		)
+		if(PYTHON3_STABLE_DLL)
+			message(STATUS "Found python3.dll at: ${PYTHON3_STABLE_DLL}")
+			install(FILES "${PYTHON3_STABLE_DLL}" DESTINATION bin COMPONENT runtime)
+		endif()
+
+		# install python Lib in python312.zip (without site-packages, which is installed separatly afterwards)
+		file(TO_CMAKE_PATH "${Python3_STDLIB}" SANITIZED_STDLIB)
+		file(GLOB LIB_CONTENTS RELATIVE "${SANITIZED_STDLIB}" "${SANITIZED_STDLIB}/*")
+		list(REMOVE_ITEM LIB_CONTENTS "site-packages")
+		install(CODE "
+			execute_process(
+				COMMAND \"${CMAKE_COMMAND}\" -E tar cf \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin/${PYTHON_ZIP_NAME}\" --format=zip -- ${LIB_CONTENTS}
+				WORKING_DIRECTORY \"${SANITIZED_STDLIB}\"
+			)
+		" COMPONENT runtime)
+		set(PYTHON_SITE_PACKAGES "${SANITIZED_STDLIB}/site-packages")
+		
+		install(DIRECTORY "${PYTHON_SITE_PACKAGES}/"
+				DESTINATION "${thirdparty_dest_dir}/Lib/site-packages"
+				COMPONENT runtime
+				PATTERN "*.exe" EXCLUDE
+				PATTERN "*.lib" EXCLUDE
+				PATTERN "*.hpp" EXCLUDE
+				PATTERN "*.h" EXCLUDE
+				PATTERN "*/torch/*" EXCLUDE
+		)
+		if(EXISTS "${PYTHON_SITE_PACKAGES}/torch")
+			install(DIRECTORY "${PYTHON_SITE_PACKAGES}/torch"
+					DESTINATION "${thirdparty_dest_dir}/Lib/site-packages/"
+					COMPONENT runtime
+					PATTERN "*.exe" EXCLUDE
+					PATTERN "*.lib" EXCLUDE
+					PATTERN "*.hpp" EXCLUDE
+					PATTERN "*.h" EXCLUDE
+					PATTERN "*.dll" EXCLUDE
+			)
+			file(GLOB_RECURSE PY_DLL_FILES "${PYTHON_SITE_PACKAGES}/torch/*.dll")
+			if(PY_DLL_FILES)
+				install(FILES ${PY_DLL_FILES} DESTINATION bin COMPONENT runtime)
+			endif()
+		endif()
+
+		# install python DDLs
+		file(GLOB_RECURSE PY_DLL_FILES \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${python_pyd_dir}/*.dll\")
+		install(DIRECTORY "${PYTHON_ROOT}/DLLs"
+				DESTINATION "${thirdparty_dest_dir}/"
+				COMPONENT runtime
+				FILES_MATCHING 
+				PATTERN "*.dll"
+				PATTERN "*.pyd"
+		)
+
+		# install our python scripts in share for convenience
+		install(DIRECTORY "${PROJECT_SOURCE_DIR}/corelib/src/python/"
+				DESTINATION share
+				COMPONENT runtime
+				FILES_MATCHING
+				PATTERN "*.py"
+		)
+	ENDIF(WIN32)
+  ENDIF(Python3_FOUND)
+  
   IF(Qt6_FOUND)
 	# Reference: https://doc-snapshots.qt.io/qt6-6.4/qt-deploy-runtime-dependencies.html
 	# The following script must only be executed at install time
@@ -248,8 +334,8 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   SET(DIRS "${QT_LIBRARY_DIRS}" "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/lib")
   IF(APPLE)
     SET(DIRS ${DIRS} /usr/local /usr/local/lib /opt/homebrew /opt/homebrew/lib /opt/homebrew/lib/gcc/current)
-  ENDIF(APPLE)
- 
+  ENDIF(APPLE)
+
   # Now the work of copying dependencies into the bundle/package
   # The quotes are escaped and variables to use at install time have their $ escaped
   # An alternative is the do a configure_file() on a script and use install(SCRIPT  ...).
@@ -257,11 +343,26 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   # over.
   # To find dependencies, cmake use "otool" on Apple and "dumpbin" on Windows (make sure you have one of them).
   install(CODE "
-   file(GLOB_RECURSE QTPLUGINS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${plugin_dest_dir}/*${CMAKE_SHARED_LIBRARY_SUFFIX}\")
+    # Glob Qt Plugins
+    file(GLOB_RECURSE ALL_LIBS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${plugin_dest_dir}/*${CMAKE_SHARED_LIBRARY_SUFFIX}\")
+    
+    if(NOT \"${python_pyd_dir}\" STREQUAL \"\")
+        file(GLOB_RECURSE PYD_FILES \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${python_pyd_dir}/*.pyd\")
+        if(PYD_FILES)
+            list(APPEND ALL_LIBS \${PYD_FILES})
+        endif()
+		if(WIN32)
+			file(GLOB_RECURSE DLL_FILES \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${python_pyd_dir}/torch/*.dll\")
+			if(DLL_FILES)
+				list(APPEND ALL_LIBS \${DLL_FILES})
+			endif()
+		endif()
+    endif()
+	
     set(BU_CHMOD_BUNDLE_ITEMS ON)
     include(\"BundleUtilities\")
-    fixup_bundle(\"${APPS}\" \"\${QTPLUGINS}\" \"${DIRS}\")
-  " COMPONENT runtime)
-
+    fixup_bundle(\"${APPS}\" \"\${ALL_LIBS}\" \"${DIRS}\")
+  " COMPONENT runtime)
+  
 ENDIF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   

bundle_windows_deps.bat

@@ -0,0 +1,227 @@
+@echo off
+setlocal enabledelayedexpansion
+
+:: --- CONFIGURATION ---
+set "VCPKG_ROOT=%~dp0vcpkg"
+set "EXPORT_DIR=%~dp0vcpkg_binaries"
+set "TRIPLET=x64-windows-release"
+set "SEVENZIP_EXE=C:\Program Files\7-Zip\7z.exe"
+
+set "VCPKG_JSON=%~dp0vcpkg.json"
+for /f "usebackq tokens=*" %%a in (`powershell -NoProfile -Command "(Get-Content '%VCPKG_JSON%' -Raw | ConvertFrom-Json).'builtin-baseline'"` ) do set "VCPKG_COMMIT=%%a"
+echo [+] Detected VCPKG baseline commit: %VCPKG_COMMIT%
+
+if "%VCPKG_COMMIT%"=="" (
+    echo [X] Error: Could not find 'builtin-baseline' in %VCPKG_JSON%
+    pause
+    exit /b 1
+)
+
+set VCPKG_COMMIT_SHORT=%VCPKG_COMMIT:~0,8%
+
+:: 1. Setup Local vcpkg
+if not exist "%VCPKG_ROOT%" (
+    echo [+] Local vcpkg not found. Cloning...
+    git clone https://github.com/microsoft/vcpkg.git "%VCPKG_ROOT%"
+)
+pushd "%VCPKG_ROOT%"
+git checkout %VCPKG_COMMIT%
+call .\bootstrap-vcpkg.bat
+popd
+
+:: 2. Install vcpkg dependencies
+echo [+] Installing dependencies via vcpkg manifest...
+"%VCPKG_ROOT%\vcpkg.exe" install ^
+    --triplet=%TRIPLET% ^
+    --host-triplet=%TRIPLET% ^
+	--clean-after-build ^
+    --x-feature=tools ^
+    --x-feature=k4w2 ^
+    --x-feature=octomap ^
+    --x-feature=openmp ^
+    --x-feature=realsense2 ^
+    --x-feature=openni2 ^
+    --x-feature=gtsam-deps ^
+    --x-feature=python ^
+    --x-feature=libpointmatcher-deps || exit /b !errorlevel!
+
+:: 3. Export
+echo [+] Exporting built binaries to raw folder...
+set "VS_LOCATOR=%ProgramFiles(x86)%\Microsoft Visual Studio\Installer\vswhere.exe"
+for /f "usebackq tokens=*" %%i in (`"%VS_LOCATOR%" -latest -property catalog_productLineVersion`) do set VS_YEAR=vs%%i
+set TARGET_NAME=vcpkg-export-%VCPKG_COMMIT_SHORT%-x64-%VS_YEAR%
+set TARGET_FULL_PATH=%EXPORT_DIR%\%TARGET_NAME%
+if exist "%TARGET_FULL_PATH%" rd /s /q "%TARGET_FULL_PATH%"
+"%VCPKG_ROOT%\vcpkg.exe" export --raw --output-dir="%EXPORT_DIR%" --triplet=%TRIPLET%  || exit /b !errorlevel!
+
+:: Find the actual exported folder name (it usually contains a date/hash)
+for /d %%i in ("%EXPORT_DIR%\vcpkg-export-20??????-??????") do set "FINAL_EXPORT_PATH=%%i"
+
+echo [+] Rename folder %FINAL_EXPORT_PATH% to %TARGET_NAME%
+ren "%FINAL_EXPORT_PATH%" "%TARGET_NAME%" || exit /b !errorlevel!
+set "FINAL_EXPORT_PATH=%TARGET_FULL_PATH%"
+
+echo [+] Add numpy...
+:: We install numpy<2 to be compatible with SuperPoint and SuperGlue scripts
+%FINAL_EXPORT_PATH%/installed/%TRIPLET%/tools/python3/python.exe -m ensurepip --upgrade || exit /b %errorlevel%
+%FINAL_EXPORT_PATH%/installed/%TRIPLET%/tools/python3/python.exe -m pip install --upgrade pip || exit /b !errorlevel!
+%FINAL_EXPORT_PATH%/installed/%TRIPLET%/tools/python3/python.exe -m pip install "numpy<2" || exit /b !errorlevel!
+
+:: 4. Other dependencies not in vcpkg
+:: libnabo
+echo [+] Building libnabo...
+if not exist libnabo (
+    echo [+] Downloading...
+    git clone https://github.com/ethz-asl/libnabo.git
+    cd libnabo
+    :: Jan 27, 2022
+    git checkout c925c47
+    git apply ../patches/libnabo_c925c47.patch
+    cd ..
+)
+cd libnabo
+cmake -S . -B build -GNinja ^
+  -DVCPKG_MANIFEST_INSTALL=OFF  ^
+  -DVCPKG_TARGET_TRIPLET=x64-windows-release  ^
+  -DVCPKG_INSTALLED_DIR="%FINAL_EXPORT_PATH%\installed"  ^
+  -DCMAKE_TOOLCHAIN_FILE="%FINAL_EXPORT_PATH%\scripts\buildsystems\vcpkg.cmake" ^
+  -DCMAKE_INSTALL_PREFIX="%FINAL_EXPORT_PATH%\installed\%TRIPLET%" ^
+  -DCMAKE_BUILD_TYPE=Release ^
+  -DSHARED_LIBS=FALSE ^
+  -DLIBNABO_BUILD_DOXYGEN=OFF ^
+  -DLIBNABO_BUILD_EXAMPLES=OFF ^
+  -DLIBNABO_BUILD_PYTHON=OFF ^
+  -DLIBNABO_BUILD_TESTS=OFF || exit /b !errorlevel!
+cmake --build build --config Release --target install || exit /b !errorlevel!
+cd ..
+
+:: libpointmatcher
+echo [+] Building libpointmatcher...
+if not exist libpointmatcher (
+    echo [+] Downloading and applying patch...
+    git clone https://github.com/ethz-asl/libpointmatcher.git
+    cd libpointmatcher
+    :: Mar 17, 2023
+    git checkout 7dc58e5
+    git apply ../patches/pointmatcher_7dc58e5.patch
+    cd ..
+)
+cd libpointmatcher
+cmake -S . -B build -GNinja ^
+  -DVCPKG_MANIFEST_INSTALL=OFF  ^
+  -DVCPKG_TARGET_TRIPLET=x64-windows-release  ^
+  -DVCPKG_INSTALLED_DIR="%FINAL_EXPORT_PATH%\installed"  ^
+  -DCMAKE_TOOLCHAIN_FILE="%FINAL_EXPORT_PATH%\scripts\buildsystems\vcpkg.cmake"  ^
+  -DCMAKE_INSTALL_PREFIX="%FINAL_EXPORT_PATH%\installed\%TRIPLET%" ^
+  -DCMAKE_BUILD_TYPE=Release ^
+  -DBUILD_TESTS=OFF  ^
+  -DBUILD_SHARED_LIBS=ON  ^
+  -DPOINTMATCHER_BUILD_EVALUATIONS=OFF ^
+  -DPOINTMATCHER_BUILD_EXAMPLES=OFF ^
+  -DCMAKE_CXX_FLAGS="-DBOOST_TIMER_ENABLE_DEPRECATED /EHsc -DBOOST_EXCEPTION_DISABLE" || exit /b !errorlevel!
+cmake --build build --config Release --target install || exit /b !errorlevel!
+cd ..
+
+:: We remove the files in the top-level CMake directory to force use of share/libpointmatcher/cmake
+if exist "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\CMake\" (
+    rd /s /q "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\CMake"
+)
+
+:: gtsam
+echo [+] Building gtsam...
+if not exist gtsam (
+    echo [+] Downloading and applying patch...
+    git clone https://github.com/borglab/gtsam.git
+    cd gtsam
+    :: June 18, 2025
+    git checkout 4.3a0-ros
+    git cherry-pick 18af4e6
+    git apply ../patches/gtsam_4_3a0-ros.patch
+    cd ..
+)
+cd gtsam
+cmake -S . -B build -GNinja ^
+  -DVCPKG_MANIFEST_INSTALL=OFF  ^
+  -DVCPKG_TARGET_TRIPLET=x64-windows-release  ^
+  -DVCPKG_INSTALLED_DIR="%FINAL_EXPORT_PATH%\installed"  ^
+  -DCMAKE_TOOLCHAIN_FILE="%FINAL_EXPORT_PATH%\scripts\buildsystems\vcpkg.cmake"  ^
+  -DCMAKE_INSTALL_PREFIX="%FINAL_EXPORT_PATH%\installed\%TRIPLET%" ^
+  -DCMAKE_BUILD_TYPE=Release ^
+  -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF  ^
+  -DGTSAM_BUILD_TESTS=OFF  ^
+  -DGTSAM_BUILD_UNSTABLE=OFF  ^
+  -DGTSAM_USE_SYSTEM_EIGEN=ON ^
+  -DGTSAM_BUILD_WITH_PRECOMPILED_HEADERS=OFF ^
+  -DGTSAM_UNSTABLE_BUILD_PYTHON=OFF ^
+  -DGTSAM_WITH_EIGEN_MKL=OFF ^
+  -DGTSAM_WITH_EIGEN_MKL_OPENMP=OFF ^
+  -DCMAKE_CXX_FLAGS="-DBOOST_TIMER_ENABLE_DEPRECATED -DBOOST_BIND_GLOBAL_PLACEHOLDERS" || exit /b !errorlevel!
+cmake --build build --config Release --target install || exit /b !errorlevel!
+cd ..
+
+:: opengv
+echo [+] Building opengv...
+if not exist opengv (
+    echo [+] Downloading and applying patch...
+    git clone https://github.com/laurentkneip/opengv.git
+    cd opengv
+    :: Aug 6, 2020
+    git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3
+    git apply ../patches/opengv_91f4b19c.patch
+    cd ..
+)
+cd opengv
+cmake -S . -B build -GNinja ^
+  -DVCPKG_MANIFEST_INSTALL=OFF  ^
+  -DVCPKG_TARGET_TRIPLET=x64-windows-release  ^
+  -DVCPKG_INSTALLED_DIR="%FINAL_EXPORT_PATH%\installed"  ^
+  -DCMAKE_TOOLCHAIN_FILE="%FINAL_EXPORT_PATH%\scripts\buildsystems\vcpkg.cmake"  ^
+  -DCMAKE_INSTALL_PREFIX="%FINAL_EXPORT_PATH%\installed\%TRIPLET%" ^
+  -DBUILD_TESTS=OFF  ^
+  -DBUILD_SHARED_LIBS=ON || exit /b !errorlevel!
+cmake --build build --config Release --target install || exit /b !errorlevel!
+cd ..
+
+:: 5. ZIP the folder
+echo [+] Creating final package with 7-Zip...
+:: Rip off pdb files
+cd /d "%FINAL_EXPORT_PATH%"
+del /s /q /f *.pdb >nul 2>&1
+
+cd ..
+
+set "FINAL_ZIP=%TARGET_NAME%.7z"
+
+:: compress contents without the root folder
+"%SEVENZIP_EXE%" u -t7z -mx9 "%FINAL_ZIP%" "%FINAL_EXPORT_PATH%\*" -up0q0
+
+if !errorlevel! EQU 0 (
+    echo [!] Success! Package created at %FINAL_ZIP%
+) else (
+    echo [X] 7-Zip failed with error code !errorlevel!
+)
+
+
+:: Example building rtabmap afterwards
+goto :EndComment
+
+set VCPKG_UNZIPPED_EXPORT_PATH=%USERPROFILE%\Downloads\vcpkg-export-########-x64-vs2022
+set PATH=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\x64-windows-release\bin;%PATH%
+
+cmake -B build -GNinja ^
+  -DCMAKE_BUILD_TYPE=Release ^
+  -DBUILD_AS_BUNDLE=ON -DWITH_PYTHON=ON ^
+  -DWITH_ZED=OFF ^
+  -DVCPKG_MANIFEST_INSTALL=OFF ^
+  -DVCPKG_TARGET_TRIPLET=x64-windows-release ^
+  -DVCPKG_INSTALLED_DIR="%VCPKG_UNZIPPED_EXPORT_PATH%/installed" ^
+  -DCMAKE_TOOLCHAIN_FILE=%VCPKG_UNZIPPED_EXPORT_PATH%/scripts/buildsystems/vcpkg.cmake ^
+  -DGTSAM_DIR=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\x64-windows-release\CMake 
+
+cmake --build build --config Release --target package
+
+:: To install CPU pytorch inside rtabmap package afterwards.
+:: Note that python.exe is the one in the bin directory of the package, not the system one.
+python.exe -m pip install torch torchvision opencv-python-headless "numpy<2"
+
+:EndComment

bundle_windows_deps_cuda.bat

@@ -0,0 +1,239 @@
+@echo off
+setlocal enabledelayedexpansion
+
+IF NOT DEFINED CUDA_PATH (
+    echo [ERROR] CUDA_PATH is not set.
+    exit /b 1
+)
+
+set PATH=%CUDA_PATH%\bin;%PATH%
+set PATH=%CUDA_PATH%\bin\x64;%PATH%
+set PATH=%CUDA_PATH%\extras\CUPTI\lib64;%PATH%
+
+:: CUDA Toolkit should be manually installed on the computer before running this script
+:: We assume also that cuDNN is merged into CUDA installed directory.
+where nvcc >nul 2>&1
+if !errorlevel! neq 0 (
+    echo [ERROR] nvcc was not found in your PATH.
+    pause
+    exit /b
+)
+for /f "tokens=5" %%a in ('nvcc --version ^| findstr "release"') do (
+    set "RAW_VER=%%a"
+    :: This removes the trailing comma
+    set "CUDA_VER=!RAW_VER:,=!"
+	set "CUDA_VER_SHORT=!CUDA_VER:.=!"
+)
+if "!CUDA_VER!"=="" (
+    echo [ERROR] Could not parse CUDA version.
+    pause
+    exit /b
+)
+echo Installed CUDA Toolkit: %CUDA_VER%
+
+:: --- CONFIGURATION ---
+set "VCPKG_ROOT=%~dp0vcpkg"
+set "EXPORT_DIR=%~dp0vcpkg_binaries"
+set "TRIPLET=x64-windows-release"
+set "SEVENZIP_EXE=C:\Program Files\7-Zip\7z.exe"
+
+set "VCPKG_JSON=%~dp0vcpkg.json"
+for /f "usebackq tokens=*" %%a in (`powershell -NoProfile -Command "(Get-Content '%VCPKG_JSON%' -Raw | ConvertFrom-Json).'builtin-baseline'"` ) do set "VCPKG_COMMIT=%%a"
+
+if "%VCPKG_COMMIT%"=="" (
+    echo [X] Error: Could not find 'builtin-baseline' in %VCPKG_JSON%
+    pause
+    exit /b 1
+)
+
+set VCPKG_COMMIT_SHORT=%VCPKG_COMMIT:~0,8%
+set "VS_LOCATOR=%ProgramFiles(x86)%\Microsoft Visual Studio\Installer\vswhere.exe"
+for /f "usebackq tokens=*" %%i in (`"%VS_LOCATOR%" -latest -property catalog_productLineVersion`) do set VS_YEAR=vs%%i
+set ORG_TARGET_NAME=vcpkg-export-%VCPKG_COMMIT_SHORT%-x64-%VS_YEAR%
+set TARGET_NAME=%ORG_TARGET_NAME%-cuda%CUDA_VER_SHORT%
+set VCPKG_EXPORT_PATH=%EXPORT_DIR%\%ORG_TARGET_NAME%
+set FINAL_EXPORT_PATH=%EXPORT_DIR%\%TARGET_NAME%
+
+if not exist "%FINAL_EXPORT_PATH%" (
+    if not exist "%VCPKG_EXPORT_PATH%" (
+        call bundle_windows_deps.bat || exit /b !errorlevel!
+	)
+	echo [+] Copying %VCPKG_EXPORT_PATH% to %FINAL_EXPORT_PATH%
+	xcopy "%VCPKG_EXPORT_PATH%" "%FINAL_EXPORT_PATH%\" /E /I /H /Y /Q || exit /b !errorlevel!
+	echo [+] Remove opencv built by vcpkg
+	rmdir /s /q "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\include\opencv4" || exit /b !errorlevel!
+	rmdir /s /q "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\share\opencv4" || exit /b !errorlevel!
+	rmdir /s /q "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\share\opencv" || exit /b !errorlevel!
+	del "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\bin\opencv*" || exit /b !errorlevel!
+	del "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\lib\opencv*" || exit /b !errorlevel!
+	:: bundle cudnn runtime libraries
+	xcopy "%CUDA_PATH%\bin\x64\cudnn*.dll" "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\bin\" /Y
+)
+
+:: pytorch deps
+%FINAL_EXPORT_PATH%/installed/%TRIPLET%/tools/python3/python.exe -m pip install numpy packaging "setuptools<82" pyyaml typing_extensions
+
+git config --global core.longpaths true
+
+:: pytorch, build with local cuda libraries to avoid duplicating them when we install rtabmap
+echo [+] Building pytorch with cuda support...
+if not exist pytorch (
+    echo [+] Downloading pytorch...
+    git clone https://github.com/pytorch/pytorch || exit /b !errorlevel!
+    cd pytorch
+	:: Jan 21, 2026
+    git checkout v2.10.0 
+	git submodule update --init --recursive || exit /b !errorlevel!
+    cd ..
+)
+
+set "PYTHONHOME=%FINAL_EXPORT_PATH%\installed\%TRIPLET%\tools\python3"
+set "Python_ROOT_DIR=%FINAL_EXPORT_PATH%\installed\%TRIPLET%"
+set CMAKE_GENERATOR=Ninja
+set BUILD_TEST=0
+set ATEN_NO_TEST=1
+set INSTALL_TEST=OFF
+set "LIB=%FINAL_EXPORT_PATH%\installed\%TRIPLET%\lib;%LIB%"
+set "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\include;INCLUDE=%INCLUDE%"
+
+:: check if torch is installed
+%PYTHONHOME%/python.exe -m pip show torch >nul 2>&1
+if !errorlevel! neq 0 (
+    cd pytorch
+    %PYTHONHOME%/python.exe setup.py install || exit /b !errorlevel!
+    cd ..
+)
+if exist "%PYTHONHOME%\Lib\site-packages\torch\test" rd /s /q %PYTHONHOME%\Lib\site-packages\torch\test"
+del "%PYTHONHOME%\Lib\site-packages\torch\bin\test_*" || exit /b !errorlevel!
+
+echo [+] Building torchvision...
+if not exist torchvision (
+    echo [+] Downloading torchvision...
+    git clone https://github.com/pytorch/vision.git torchvision || exit /b !errorlevel!
+    cd torchvision
+	:: Jan 6, 2026
+    git checkout v0.25.0
+	git submodule update --init --recursive || exit /b !errorlevel!
+    cd ..
+)
+cd torchvision
+set PATH=%FINAL_EXPORT_PATH%\installed\%TRIPLET%\bin;%PATH%
+set DISTUTILS_USE_SDK=1
+set TORCHVISION_INCLUDE=%FINAL_EXPORT_PATH%\installed\%TRIPLET%\include
+set TORCHVISION_LIBRARY=%FINAL_EXPORT_PATH%\installed\%TRIPLET%\lib
+%FINAL_EXPORT_PATH%/installed/%TRIPLET%/tools/python3/python.exe -m pip install . -v --no-build-isolation || exit /b !errorlevel!
+cd ..
+
+:: opencv_cuda
+echo [+] Building opencv with cuda support...
+if not exist opencv (
+    echo [+] Downloading opencv...
+    git clone https://github.com/opencv/opencv.git || exit /b !errorlevel!
+    cd opencv
+    :: 4.13.0 minimum required to be compatible with cuda 13
+	:: Dec 31, 2025
+    git checkout 4.13.0
+    cd ..
+)
+if not exist opencv_contrib (
+    echo [+] Downloading opencv_contrib...
+	git clone https://github.com/opencv/opencv_contrib.git || exit /b !errorlevel!
+    cd opencv
+    :: 4.13.0 minimum required to be compatible with cuda 13
+	:: Dec 31, 2025
+    git checkout 4.13.0
+    cd ..
+)
+cd opencv
+cmake -S . -B build -GNinja ^
+  -DVCPKG_MANIFEST_INSTALL=OFF  ^
+  -DVCPKG_TARGET_TRIPLET=%TRIPLET%  ^
+  -DVCPKG_INSTALLED_DIR="%FINAL_EXPORT_PATH%\installed"  ^
+  -DCMAKE_TOOLCHAIN_FILE="%FINAL_EXPORT_PATH%\scripts\buildsystems\vcpkg.cmake" ^
+  -DCMAKE_INSTALL_PREFIX="%FINAL_EXPORT_PATH%\installed\%TRIPLET%" ^
+  -DCMAKE_BUILD_TYPE=Release ^
+  -DOPENCV_BIN_INSTALL_PATH="bin" ^
+  -DOPENCV_LIB_INSTALL_PATH="lib" ^
+  -DOPENCV_CONFIG_INSTALL_PATH="share/opencv" ^
+  -DOPENCV_EXTRA_MODULES_PATH=../opencv_contrib/modules ^
+  -DBUILD_SHARED_LIBS=ON ^
+  -DBUILD_TESTS=OFF ^
+  -DBUILD_PERF_TESTS=OFF ^
+  -DOPENCV_ENABLE_NONFREE=ON ^
+  -DBUILD_opencv_apps=OFF ^
+  -DBUILD_opencv_python3=ON ^
+  -DPYTHON3_EXECUTABLE=%FINAL_EXPORT_PATH%/installed/%TRIPLET%/tools/python3/python.exe ^
+  -DPYTHON3_PACKAGES_PATH=bin/Lib/site-packages ^
+  -DBUILD_opencv_java_bindings_generator=OFF ^
+  -DWITH_CUDA=ON ^
+  -DWITH_VTK=OFF ^
+  -DWITH_TBB=ON || exit /b !errorlevel!
+cmake --build build --config Release --target install || exit /b !errorlevel!
+:: move cv2 package under tools/python3
+robocopy "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\bin\Lib" "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\tools\python3\Lib" /E /MOVE /NFL /NDL /NJH /NC /NS /NP
+cd ..
+
+
+:: 5. ZIP the folder
+echo [+] Creating final package with 7-Zip...
+:: Rip off pdb files
+cd /d "%FINAL_EXPORT_PATH%"
+del /s /q /f *.pdb >nul 2>&1
+cd ..
+
+set "FINAL_ZIP=%TARGET_NAME%.7z"
+
+:: compress contents without the root folder
+"%SEVENZIP_EXE%" u -t7z -mx9 "%FINAL_ZIP%" "%FINAL_EXPORT_PATH%\*" -up0q0 || exit /b !errorlevel!
+
+if !errorlevel! EQU 0 (
+    echo [!] Success! Package created at %FINAL_ZIP%
+) else (
+    echo [X] 7-Zip failed with error code !errorlevel!
+)
+
+:: Example building rtabmap with opencv cuda and libtorch afterwards
+goto :EndComment
+
+:: Set path of unzipped deps
+set VCPKG_UNZIPPED_EXPORT_PATH=
+
+set TRIPLET=x64-windows-release
+set PATH=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\%TRIPLET%\bin;%PATH%
+set PATH=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\%TRIPLET%\tools\python3;%PATH%
+set PATH=%CUDA_PATH%\bin;%PATH%
+set PATH=%CUDA_PATH%\bin\x64;%PATH%
+set PATH=%CUDA_PATH%\extras\CUPTI\lib64;%PATH%
+set PATH=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\%TRIPLET%\tools\python3\Lib\site-packages\torch\lib;%PATH%
+set PATH=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\%TRIPLET%\tools\python3\Lib\site-packages\numpy.libs;%PATH%
+
+:: Other dependencies
+
+:: For ZED, modify zed-config.cmake and remove all dependencies
+set PATH=%PATH%;%ZED_SDK_ROOT_DIR%\bin
+
+:: For kinect 4 windows SDK v2, move kinect20.dll from system32 to KINECTSDK20_DIR\bin 
+:: For kinect 4 windows SDK v1, move kinect10.dll and KinectAudio10.dll to KINECTSDK20_DIR\bin
+set PATH=%PATH%;%KINECTSDK20_DIR%\bin
+
+cmake -B build_cuda -GNinja ^
+  -DCMAKE_BUILD_TYPE=Release ^
+  -DBUILD_AS_BUNDLE=ON ^
+  -DWITH_PYTHON=ON ^
+  -DWITH_TORCH=ON ^
+  -DWITH_ZED=ON ^
+  -DVCPKG_MANIFEST_INSTALL=OFF ^
+  -DVCPKG_TARGET_TRIPLET=%TRIPLET% ^
+  -DVCPKG_INSTALLED_DIR="%VCPKG_UNZIPPED_EXPORT_PATH%/installed" ^
+  -DCMAKE_TOOLCHAIN_FILE=%VCPKG_UNZIPPED_EXPORT_PATH%/scripts/buildsystems/vcpkg.cmake ^
+  -DGTSAM_DIR=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\%TRIPLET%\CMake ^
+  -DTorch_DIR=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\%TRIPLET%\tools\python3\Lib\site-packages\torch\share\cmake\Torch
+
+cmake --build build_cuda --config Release --target package
+
+:: Generate superpoint weights (from share directory of the installed package)
+curl -L -O "https://raw.githubusercontent.com/magicleap/SuperPointPretrainedNetwork/master/demo_superpoint.py"
+curl -L -O "https://github.com/magicleap/SuperPointPretrainedNetwork/raw/refs/heads/master/superpoint_v1.pth"
+..\bin\python.exe rtabmap_trace_superpoint.py
+
+:EndComment

cmake_modules/FindCuVSLAM.cmake

@@ -4,6 +4,7 @@
 #
 # It sets the following variables:
 #  CUVSLAM_FOUND         - Set to false, or undefined, if cuVSLAM isn't found.
+#  CUVSLAM_VERSION       - The version of cuVSLAM found (e.g., "14.0.0").
 #  CUVSLAM_INCLUDE_DIRS  - The cuVSLAM include directory.
 #  CUVSLAM_LIBRARIES     - The cuVSLAM library to link against.
 
@@ -33,7 +34,18 @@ find_library(CUVSLAM_LIBRARY
 )
 
 if(CUVSLAM_INCLUDE_DIRS AND CUVSLAM_LIBRARY)
-    set(CUVSLAM_FOUND TRUE)
+    # Extract version from cuvslam.h header
+    file(STRINGS "${CUVSLAM_INCLUDE_DIRS}/cuvslam.h" CUVSLAM_VERSION_MAJOR_LINE 
+         REGEX "^#define CUVSLAM_API_VERSION_MAJOR")
+    file(STRINGS "${CUVSLAM_INCLUDE_DIRS}/cuvslam.h" CUVSLAM_VERSION_MINOR_LINE 
+         REGEX "^#define CUVSLAM_API_VERSION_MINOR")
+    
+    if(CUVSLAM_VERSION_MAJOR_LINE AND CUVSLAM_VERSION_MINOR_LINE)
+        string(REGEX MATCH "[0-9]+" CUVSLAM_VERSION_MAJOR "${CUVSLAM_VERSION_MAJOR_LINE}")
+        string(REGEX MATCH "[0-9]+" CUVSLAM_VERSION_MINOR "${CUVSLAM_VERSION_MINOR_LINE}")
+        set(CUVSLAM_VERSION "${CUVSLAM_VERSION_MAJOR}.${CUVSLAM_VERSION_MINOR}.0")
+    endif()
+    
     set(CUVSLAM_LIBRARIES 
         ${CUVSLAM_LIBRARY} 
         ${CUDA_LIBRARIES}
@@ -46,11 +58,35 @@ if(CUVSLAM_INCLUDE_DIRS AND CUVSLAM_LIBRARY)
     )
 endif()
 
+# Version compatibility check - cuVSLAM only guarantees API compatibility within the same major version
+set(CUVSLAM_VERSION_MISMATCH_REASON "")
+if(CuVSLAM_FIND_VERSION AND CUVSLAM_VERSION)
+    string(REGEX MATCH "^[0-9]+" REQUESTED_MAJOR_VERSION "${CuVSLAM_FIND_VERSION}")
+    if(NOT CUVSLAM_VERSION_MAJOR EQUAL REQUESTED_MAJOR_VERSION)
+        set(CUVSLAM_VERSION_MISMATCH_REASON "Major version mismatch: found ${CUVSLAM_VERSION_MAJOR}.x but requested ${REQUESTED_MAJOR_VERSION}.x.\ncuVSLAM only guarantees API compatibility within the same major version.\nPlease install cuVSLAM ${REQUESTED_MAJOR_VERSION}.x or update CMakeLists.txt to request version ${CUVSLAM_VERSION_MAJOR}.0.0")
+        
+        if(CuVSLAM_FIND_REQUIRED)
+            message(FATAL_ERROR 
+                "cuVSLAM major version mismatch: found version ${CUVSLAM_VERSION} but version ${CuVSLAM_FIND_VERSION} is required.\n"
+                "cuVSLAM only guarantees API compatibility within the same major version.\n"
+                "Found major version ${CUVSLAM_VERSION_MAJOR} is not compatible with requested major version ${REQUESTED_MAJOR_VERSION}.\n"
+                "Please install cuVSLAM ${REQUESTED_MAJOR_VERSION}.x or update CMakeLists.txt to request version ${CUVSLAM_VERSION_MAJOR}.x."
+            )
+        else()
+            # Clear the found variables to indicate incompatibility
+            unset(CUVSLAM_LIBRARIES)
+            unset(CUVSLAM_INCLUDE_DIRS)
+        endif()
+    endif()
+endif()
+
 # Handle the QUIET and REQUIRED arguments
 include(FindPackageHandleStandardArgs)
 find_package_handle_standard_args(CuVSLAM
     FOUND_VAR CUVSLAM_FOUND
     REQUIRED_VARS CUVSLAM_LIBRARIES CUVSLAM_INCLUDE_DIRS
+    VERSION_VAR CUVSLAM_VERSION
+    REASON_FAILURE_MESSAGE "${CUVSLAM_VERSION_MISMATCH_REASON}"
     HANDLE_COMPONENTS
 )
 
@@ -64,16 +100,6 @@ if(CUVSLAM_FOUND)
             INTERFACE_LINK_LIBRARIES "${CUVSLAM_LIBRARIES};Eigen3::Eigen"
         )
     endif()
-    
-    # Show which cuVSLAM was found only if not quiet
-    if(NOT CUVSLAM_FIND_QUIETLY)
-        message(STATUS "Found cuVSLAM: ${CUVSLAM_LIBRARIES}")
-    endif()
-else()
-    # Fatal error if cuVSLAM is required but not found
-    if(CUVSLAM_FIND_REQUIRED)
-        message(FATAL_ERROR "Could not find cuVSLAM library")
-    endif()
 endif()
 
 mark_as_advanced(CUVSLAM_INCLUDE_DIRS CUVSLAM_LIBRARY)

cmake_modules/FindORB_SLAM.cmake

@@ -11,6 +11,7 @@
 find_path(ORB_SLAM_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/include)
 find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM_ROOT_DIR}/lib)
 find_library(ORB_SLAM3_LIBRARY NAMES ORB_SLAM3 PATHS $ENV{ORB_SLAM_ROOT_DIR}/lib)
+find_path(DBoW2_INCLUDE_DIR NAMES DBoW2/BowVector.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/DBoW2 NO_DEFAULT_PATH)
 find_path(g2o_INCLUDE_DIR NAMES g2o/core/sparse_optimizer.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/g2o NO_DEFAULT_PATH)
 find_path(sophus_INCLUDE_DIR NAMES sophus/se3.hpp PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/Sophus NO_DEFAULT_PATH)
 find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/g2o/lib NO_DEFAULT_PATH)
@@ -22,9 +23,9 @@ IF(ORB_SLAM2_LIBRARY)
 ELSEIF(ORB_SLAM3_LIBRARY)
    SET(ORB_SLAM_VERSION 3)
    SET(ORB_SLAM_LIBRARY ${ORB_SLAM3_LIBRARY})
-   IF(g2o_INCLUDE_DIR AND sophus_INCLUDE_DIR) # ORB_SLAM3 v1
-      SET(g2o_INCLUDE_DIR ${g2o_INCLUDE_DIR} ${sophus_INCLUDE_DIR})
-   ENDIF(g2o_INCLUDE_DIR AND sophus_INCLUDE_DIR)
+   IF(g2o_INCLUDE_DIR AND sophus_INCLUDE_DIR AND DBoW2_INCLUDE_DIR) # ORB_SLAM3 v1
+      SET(g2o_INCLUDE_DIR ${g2o_INCLUDE_DIR} ${sophus_INCLUDE_DIR} ${DBoW2_INCLUDE_DIR} $ENV{ORB_SLAM_ROOT_DIR})
+   ENDIF(g2o_INCLUDE_DIR AND sophus_INCLUDE_DIR AND DBoW2_INCLUDE_DIR)
 ENDIF()
 
 IF (ORB_SLAM_INCLUDE_DIR AND ORB_SLAM_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)

corelib/include/rtabmap/core/Camera.h

@@ -38,7 +38,7 @@ class IMUFilter;
 
 /**
  * Class Camera
- *
+ *
  */
 class RTABMAP_CORE_EXPORT Camera : public SensorCapture
 {
@@ -48,7 +48,7 @@ public:
 	SensorData takeImage(SensorCaptureInfo * info = 0) {return takeData(info);}
 	float getImageRate() const {return getFrameRate();}
 	void setImageRate(float imageRate) {setFrameRate(imageRate);}
-	void setInterIMUPublishing(bool enabled, IMUFilter * filter = 0); // Take ownership of filter
+	void setInterIMUPublishing(bool enabled, IMUFilter * filter = 0, bool baseFrameConversion = false); // Take ownership of filter
 	bool isInterIMUPublishing() const {return publishInterIMU_;}
 
 	bool initFromFile(const std::string & calibrationPath);
@@ -73,6 +73,7 @@ private:
 private:
 	IMUFilter * imuFilter_;
 	bool publishInterIMU_;
+	bool imuBaseFrameConversion_;
 };
 
 

corelib/include/rtabmap/core/DBDriver.h

@@ -134,7 +134,7 @@ public:
 public:
 	// Mutex-protected methods of abstract versions below
 
-	bool openConnection(const std::string & url, bool overwritten = false);
+	bool openConnection(const std::string & url, bool overwritten = false, bool readOnly = false);
 	void closeConnection(bool save = true, const std::string & outputUrl = "");
 	bool isConnected() const;
 	unsigned long getMemoryUsed() const; // In bytes
@@ -193,7 +193,7 @@ public:
 protected:
 	DBDriver(const ParametersMap & parameters = ParametersMap());
 
-	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
+	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false, bool readOnly = false) = 0;
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "") = 0;
 	virtual bool isConnectedQuery() const = 0;
 	virtual unsigned long getMemoryUsedQuery() const = 0; // In bytes
@@ -300,6 +300,18 @@ protected:
 	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const = 0;
 	virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const = 0;
 
+protected:
+	std::vector<unsigned char> serializeFeatures(
+		const std::vector<cv::KeyPoint> & keypoints, 
+		const std::vector<cv::Point3f> & points3D,
+		const cv::Mat & descriptors) const;
+	bool deserializeFeatures(
+		const unsigned char * compressedData,
+		unsigned int compressedDataSize,
+		std::vector<cv::KeyPoint> & keypoints,
+		std::vector<cv::Point3f> & points3D,
+		cv::Mat & descriptors) const;
+
 private:
 	//non-abstract methods
 	void saveOrUpdate(const std::vector<Signature *> & signatures);

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -51,7 +51,7 @@ public:
 	void setTempStore(int tempStore);
 
 protected:
-	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
+	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false, bool readOnly = false);
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
 	virtual bool isConnectedQuery() const;
 	virtual unsigned long getMemoryUsedQuery() const; // In bytes
@@ -182,6 +182,7 @@ private:
 	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
+	void stepKeypoint(sqlite3_stmt * ppStmt, int nodeId, int wordId, int kptIndex) const;
 	void stepKeypoint(sqlite3_stmt * ppStmt, int nodeID, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
 	void stepGlobalDescriptor(sqlite3_stmt * ppStmt, int nodeId, const GlobalDescriptor & descriptor) const;
 	void stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,

corelib/include/rtabmap/core/DBReader.h

@@ -59,6 +59,7 @@ public:
 			 int startMapId = 0,
 			 int stopMapId = -1,
 			 bool priorsIgnored = false,
+			 bool imuIgnored = 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
@@ -74,6 +75,7 @@ public:
 			 int startMapId = 0,
 			 int stopMapId = -1,
 			 bool priorsIgnored = false,
+			 bool imuIgnored = false,
 			 const std::vector<Transform> & cameraLocalTransformOverrides = std::vector<Transform>());
 	virtual ~DBReader();
 
@@ -107,6 +109,7 @@ private:
 	bool _landmarksIgnored;
 	bool _featuresIgnored;
 	bool _priorsIgnored;
+	bool _imuIgnored;
 	int _startMapId;
 	int _stopMapId;
 	std::vector<Transform> _cameraLocalTransformOverrides;

corelib/include/rtabmap/core/Features2d.h

@@ -104,6 +104,7 @@ namespace rtabmap {
 
 class ORBextractor;
 class SPDetector;
+class SPDetectorRpautrat;
 
 class Stereo;
 #if CV_MAJOR_VERSION < 3
@@ -129,7 +130,8 @@ public:
 		kFeatureSurfFreak=12, //new 0.20.4
 		kFeatureGfttDaisy=13, //new 0.20.6
 		kFeatureSurfDaisy=14,  //new 0.20.6
-		kFeaturePyDetector=15}; //new 0.20.8
+		kFeaturePyDetector=15, //new 0.20.8
+		kFeatureSuperPointRpautrat=16}; // new 0.23.3
 
 	static std::string typeName(Type type)
 	{
@@ -164,6 +166,8 @@ public:
 			return "GFTT+Daisy";
 		case kFeatureSurfDaisy:
 			return "SURF+Daisy";
+		case kFeatureSuperPointRpautrat:
+			return "SUPERPOINT-RPAUTRAT";
 		default:
 			return "Unknown";
 		}
@@ -305,7 +309,8 @@ private:
 	bool preciseUpscale_;
 	bool rootSIFT_;
 	bool gpu_;
-	float guaussianThreshold_;
+	float gaussianThreshold_;
+	float maxGaussianThreshold_;
 	bool upscale_;
 
 	cv::Ptr<CV_SIFT> sift_;
@@ -626,6 +631,31 @@ private:
 	bool cuda_;
 };
 
+//SuperPointRpautrat
+class RTABMAP_CORE_EXPORT SuperPointRpautrat : public Feature2D
+{
+public:
+	SuperPointRpautrat(const ParametersMap & parameters = ParametersMap());
+	virtual ~SuperPointRpautrat();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const { return kFeatureSuperPointRpautrat; }
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+	cv::Ptr<SPDetectorRpautrat> superPoint_;
+
+	std::string superpointWeightsPath_;
+	std::string superpointModelPath_;
+	std::string outputDir_;
+	float threshold_;
+	bool nms_;
+	int minDistance_;
+	bool cuda_;
+};
+
 //GFTT_DAISY
 class RTABMAP_CORE_EXPORT GFTT_DAISY : public GFTT
 {

corelib/include/rtabmap/core/Graph.h

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

corelib/include/rtabmap/core/Memory.h

@@ -144,6 +144,7 @@ public:
 	void saveLocationData(int locationId);
 	void removeLink(int idA, int idB);
 	void removeRawData(int id, bool image = true, bool scan = true, bool userData = true);
+	int reduceNode(int id, float maxDistance = 0.0f, bool keepLinkedInDb = false, int direction = 0);
 
 	//getters
 	const std::map<int, double> & getWorkingMem() const {return _workingMem;}
@@ -211,6 +212,7 @@ public:
 	std::set<int> getAllSignatureIds(bool ignoreChildren = true) const;
 	bool memoryChanged() const {return _memoryChanged;}
 	bool isIncremental() const {return _incrementalMemory;}
+	bool isReadOnly() const {return !_incrementalMemory && _localizationReadOnly;}
 	bool isLocalizationDataSaved() const {return _localizationDataSaved;}
 	const Signature * getSignature(int id) const;
 	bool isInSTM(int signatureId) const {return _stMem.find(signatureId) != _stMem.end();}
@@ -276,6 +278,7 @@ private:
 	void initCountId();
 	void rehearsal(Signature * signature, Statistics * stats = 0);
 	bool rehearsalMerge(int oldId, int newId);
+	bool canBeReduced(const Link & link, float maxDistance, int direction);
 
 	const std::map<int, Signature*> & getSignatures() const {return _signatures;}
 
@@ -307,6 +310,7 @@ private:
 	std::string _rgbCompressionFormat;
 	std::string _depthCompressionFormat;
 	bool _incrementalMemory;
+	bool _localizationReadOnly;
 	bool _localizationDataSaved;
 	bool _flannIndexSaved;
 	bool _reduceGraph;

corelib/include/rtabmap/core/Parameters.h

@@ -213,6 +213,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM_STR(Mem, DepthCompressionFormat,   ".rvl",        "Depth image compression format for 16UC1 depth type. It should be \".png\" or \".rvl\". If depth type is 32FC1, \".png\" is used.");
     RTABMAP_PARAM(Mem, STMSize,                   unsigned int, 10, "Short-term memory size.");
     RTABMAP_PARAM(Mem, IncrementalMemory,           bool, true,     "SLAM mode, otherwise it is Localization mode.");
+    RTABMAP_PARAM(Mem, LocalizationReadOnly,        bool, false,    uFormat("In localization mode, open the database in read-only mode (ignored if %s=true). Currrenty incompatible with memory management (%s and %s cannot be used) and if there are disjoint sessions in working memory. Last localization pose won't be saved back in the database at the end of the session, so the robot will always restart to original last localization pose, unless %s is used or an external initial pose is provided on initialization.", kMemIncrementalMemory().c_str(), kRtabmapLoopThr().c_str(), kRtabmapMemoryThr().c_str(), kRGBDStartAtOrigin().c_str()).c_str());
     RTABMAP_PARAM(Mem, LocalizationDataSaved,       bool, false,    uFormat("Save localization data during localization session (when %s=false). When enabled, the database will then also grow in localization mode. This mode would be used only for debugging purpose.", kMemIncrementalMemory().c_str()).c_str());
     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.");
@@ -223,7 +224,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Mem, BadSignaturesIgnored,        bool, false,    "Bad signatures are ignored.");
     RTABMAP_PARAM(Mem, InitWMWithAllNodes,          bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
     RTABMAP_PARAM(Mem, DepthAsMask,                 bool, true,     "Use depth image as mask when extracting features for vocabulary.");
-    RTABMAP_PARAM(Mem, DepthMaskFloorThr,           float, 0.0,     uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kMemDepthAsMask().c_str()));
+    RTABMAP_PARAM(Mem, DepthMaskFloorThr,           float, 0.0,     uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled. Ignored if %s is false.", kMemDepthAsMask().c_str()));
     RTABMAP_PARAM(Mem, StereoFromMotion,            bool, false,    uFormat("Triangulate features without depth using stereo from motion (odometry). It would be ignored if %s is true and the feature detector used supports masking.", kMemDepthAsMask().c_str()));
     RTABMAP_PARAM(Mem, ImagePreDecimation,          unsigned int, 1, uFormat("Decimation of the RGB image before visual feature detection. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.",kMemDepthAsMask().c_str()));
     RTABMAP_PARAM(Mem, ImagePostDecimation,         unsigned int, 1, uFormat("Decimation of the RGB image before saving it to database. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. Decimation is done from the original image. If set to same value than %s, data already decimated is saved (no need to re-decimate the image).", kMemImagePreDecimation().c_str()));
@@ -252,16 +253,16 @@ class RTABMAP_CORE_EXPORT Parameters
     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
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 8,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 8,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector 16=SuperPoint-Rpautrat");
 #else
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector 16=SuperPoint-Rpautrat");
 #endif
     RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed,      bool, true,   "Use of the td-idf strategy to compute the likelihood.");
     RTABMAP_PARAM(Kp, Parallelized,             bool, true,   "If the dictionary update and signature creation were parallelized.");
     RTABMAP_PARAM_STR(Kp, RoiRatios,       "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
     RTABMAP_PARAM_STR(Kp, DictionaryPath,       "",           "Path of the pre-computed dictionary");
     RTABMAP_PARAM(Kp, NewWordsComparedTogether, bool, true,   "When adding new words to dictionary, they are compared also with each other (to detect same words in the same signature).");
-    RTABMAP_PARAM(Kp, FlannIndexSaved,          bool, false,  uFormat("Save FLANN index during localization session (when %s=false). The FLANN index will be saved to database after the first time localization mode is used, then on next sessions, the index is reloaded from the database instead of being rebuilt again. This can save significant loading time when the visual word dictionary is big (>1M words). Note that if the dictionary is modified (parameters or data), the index will be rebuilt and saved again on the next session.", kMemIncrementalMemory().c_str()).c_str());
+    RTABMAP_PARAM(Kp, FlannIndexSaved,          bool, false,  uFormat("Save FLANN index during localization session (when %s=false). The FLANN index will be saved to database after the first time localization mode is used, then on next sessions, the index is reloaded from the database instead of being rebuilt again. This can save significant loading time when the visual word dictionary is big (>1M words). Note that if the dictionary is modified (parameters or data), the index will be rebuilt and saved again on the next session. Ignored on initialization if %s is enabled.", kMemIncrementalMemory().c_str(), kMemInitWMWithAllNodes().c_str()).c_str());
     RTABMAP_PARAM(Kp, SerializeWithChecksum,    bool, true,   "On serialization of the FLANN index, compute checksum of the data used by the FLANN index. This adds a slight overhead on serialization/deserialization to make sure that the dictionary data correspond to same data used when the index was built.");
     RTABMAP_PARAM(Kp, SubPixWinSize,            int, 3,       "See cv::cornerSubPix().");
     RTABMAP_PARAM(Kp, SubPixIterations,         int, 0,       "See cv::cornerSubPix(). 0 disables sub pixel refining.");
@@ -293,7 +294,8 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(SIFT, PreciseUpscale,    bool, false,  "Whether to enable precise upscaling in the scale pyramid (OpenCV >= 4.8).");
     RTABMAP_PARAM(SIFT, RootSIFT,          bool, false,  "Apply RootSIFT normalization of the descriptors.");
     RTABMAP_PARAM(SIFT, Gpu,               bool, false,  "CudaSift: Use GPU version of SIFT. This option is enabled only if RTAB-Map is built with CudaSift dependency and GPUs are detected.");
-    RTABMAP_PARAM(SIFT, GaussianThreshold, float, 2.0,   "CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features are produced by the detector.");
+    RTABMAP_PARAM(SIFT, GaussianThreshold, float, 2.0,   "CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features with low response/hessian are produced by the detector.");
+    RTABMAP_PARAM(SIFT, MaxGaussianThreshold, float, 0.0,   uFormat("CudaSift: Maximum threshold on difference of Gaussians for feature pruning (ignored if smaller or equal than %s). The lower the threshold, the less features with high response/hessian are produced by the detector.", kSIFTGaussianThreshold().c_str()));
     RTABMAP_PARAM(SIFT, Upscale,           bool, false,  "CudaSift: Whether to enable upscaling.");
 
     RTABMAP_PARAM(BRIEF, Bytes,            int, 32,      "Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");
@@ -346,6 +348,13 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(SuperPoint, NMSRadius,     int,  4,      uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSuperPointNMS().c_str()));
     RTABMAP_PARAM(SuperPoint, Cuda,          bool, true,   "Use Cuda device for Torch, otherwise CPU device is used by default.");
 
+    RTABMAP_PARAM_STR(SuperPointRpautrat, WeightsPath, "",           "[Required] SuperPoint weights file (*.pth).");
+    RTABMAP_PARAM_STR(SuperPointRpautrat, ModelPath, "",           "[Required] SuperPoint python model file (superpoint_pytorch.py).");
+    RTABMAP_PARAM(SuperPointRpautrat, Threshold,     float, 0.005, "Detector response threshold to accept keypoint.");
+    RTABMAP_PARAM(SuperPointRpautrat, NMS,           bool,  true,  "If true, non-maximum suppression is applied to detected keypoints.");
+    RTABMAP_PARAM(SuperPointRpautrat, NMSRadius,     int,  4,      uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSuperPointRpautratNMS().c_str()));
+    RTABMAP_PARAM(SuperPointRpautrat, Cuda,          bool, true,   "Use Cuda device for Torch, otherwise CPU device is used by default.");
+
     RTABMAP_PARAM_STR(PyDetector, Path,       "",           "Path to python script file (see available ones in rtabmap/corelib/src/python/*). See the header to see where the script should be copied.");
 	RTABMAP_PARAM(PyDetector, Cuda,           bool, true,   "Use cuda.");
 
@@ -362,8 +371,8 @@ class RTABMAP_CORE_EXPORT Parameters
 
     // RGB-D SLAM
     RTABMAP_PARAM(RGBD, Enabled,                  bool, true,  "Activate metric SLAM. If set to false, classic RTAB-Map loop closure detection is done using only images and without any metric information.");
-    RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.1,  "Minimum linear displacement (m) to update the map. Rehearsal is done prior to this, so weights are still updated.");
-    RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated.");
+    RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.1,  uFormat("Minimum linear displacement (m) to update the map. Rehearsal is done prior to this, so weights are still updated. To update the map when not moving, both %s and %s should be set to 0.", Parameters::kRGBDLinearUpdate().c_str(), Parameters::kRGBDAngularUpdate().c_str()));
+    RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  uFormat("Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated. To update the map when not moving, both %s and %s should be set to 0.", Parameters::kRGBDLinearUpdate().c_str(), Parameters::kRGBDAngularUpdate().c_str()));
     RTABMAP_PARAM(RGBD, LinearSpeedUpdate,        float, 0.0,  "Maximum linear speed (m/s) to update the map (0 means not limit).");
     RTABMAP_PARAM(RGBD, AngularSpeedUpdate,       float, 0.0,  "Maximum angular speed (rad/s) to update the map (0 means not limit).");
     RTABMAP_PARAM(RGBD, AggressiveLoopThr,        float, 0.05, uFormat("Loop closure threshold used (overriding %s) when a new mapping session is not yet linked to a map of the highest loop closure hypothesis. In localization mode, this threshold is used when there are no loop closure constraints with any map in the cache (%s). In all cases, the goal is to aggressively loop on a previous map in the database. Only used when %s is enabled. Set 1 to disable.", kRtabmapLoopThr().c_str(), kRGBDMaxOdomCacheSize().c_str(), kRGBDEnabled().c_str()));
@@ -456,7 +465,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_SLAM2 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");
     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).");
@@ -551,7 +560,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(OdomViso2, BucketWidth,               double, 50,  "Width of bucket.");
     RTABMAP_PARAM(OdomViso2, BucketHeight,              double, 50,  "Height of bucket.");
 
-    // Odometry ORB_SLAM2
+    // Odometry ORB_SLAM
     RTABMAP_PARAM_STR(OdomORBSLAM, VocPath,             "",       "Path to ORB vocabulary (*.txt).");
     RTABMAP_PARAM(OdomORBSLAM, Bf,              double, 0.076,    "Fake IR projector baseline (m) used only when stereo is not used.");
     RTABMAP_PARAM(OdomORBSLAM, ThDepth,         double, 40.0,     "Close/Far threshold. Baseline times.");
@@ -675,6 +684,9 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(OdomOpen3D, MaxDepth,         float, 3.0,  "Maximum depth.");
     RTABMAP_PARAM(OdomOpen3D, Method,           int, 0,  "Registration method: 0=PointToPlane, 1=Intensity, 2=Hybrid.");
 
+    // Odometry cuVSLAM
+    RTABMAP_PARAM(OdomCuVSLAM, MulticamMode,        int, 0,  "cuVSLAM multicam_mode setting: 0=moderate, 1=performance, 2=precision.");
+
     // 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");
@@ -704,16 +716,16 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Vis, Iterations,               int,    300,   "Maximum iterations to compute the transform.");
 #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
     // OpenCV>2 without xFeatures2D module doesn't have BRIEF
-    RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
+    RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector 16=SuperPoint-Rpautrat");
 #else
-    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
+    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector 16=SuperPoint-Rpautrat");
 #endif
     RTABMAP_PARAM(Vis, MaxFeatures,               int,   1000,  "0 no limits.");
     RTABMAP_PARAM(Vis, SSC,                       bool,  false, "If true, SSC (Suppression via Square Covering) is applied to limit keypoints.");
     RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,     "Max depth of the features (0 means no limit).");
     RTABMAP_PARAM(Vis, MinDepth,                  float, 0,     "Min depth of the features (0 means no limit).");
     RTABMAP_PARAM(Vis, DepthAsMask,               bool,  true,  "Use depth image as mask when extracting features.");
-    RTABMAP_PARAM(Vis, DepthMaskFloorThr,         float, 0.0,    uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kVisDepthAsMask().c_str()));
+    RTABMAP_PARAM(Vis, DepthMaskFloorThr,         float, 0.0,    uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled. Ignored if %s is false.", kVisDepthAsMask().c_str()));
     RTABMAP_PARAM_STR(Vis, RoiRatios,        "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
     RTABMAP_PARAM(Vis, SubPixWinSize,             int,   3,     "See cv::cornerSubPix().");
     RTABMAP_PARAM(Vis, SubPixIterations,          int,   0,     "See cv::cornerSubPix(). 0 disables sub pixel refining.");
@@ -729,8 +741,11 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Vis, CorFlowIterations,         int,   30,    uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowEps,                float, 0.01,  uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowMaxLevel,           int,   3,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
-    RTABMAP_PARAM(Vis, CorFlowGpu,                bool,  false, uFormat("[%s=1] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA).", kVisCorType().c_str()));
-#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
+    RTABMAP_PARAM(Vis, CorFlowUseMinEigenVals,    bool,  true,  uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach. Use minimum eigen values as an error measure, otherwise L1 distance between patches is used as an error measure.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowMinEigThreshold,    float, 1e-4,  uFormat("[%s=true] If the minimum eigenvalue of a feature's spatial gradient matrix is less than this threshold, then the feature is filtered out.", kVisCorFlowUseMinEigenVals().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowErrorThreshold,     float, 20,    uFormat("[%s=false] Filter out features with error greater than this threshold.", kVisCorFlowUseMinEigenVals().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowGpu,                bool,  false, uFormat("[%s=1] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA). Note that %s is not used in the GPU implementation.", kVisCorType().c_str(), kVisCorFlowUseMinEigenVals().c_str()));
+    #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
     RTABMAP_PARAM(Vis, BundleAdjustment,          int,   1,     "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
 #else
     RTABMAP_PARAM(Vis, BundleAdjustment,          int,   0,     "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
@@ -807,7 +822,10 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Stereo, OpticalFlow,           bool, true,    "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
     RTABMAP_PARAM(Stereo, SSD,                   bool, true,    uFormat("[%s=false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.", kStereoOpticalFlow().c_str()));
     RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  uFormat("[%s=true] Epsilon stop criterion.", kStereoOpticalFlow().c_str()));
-    RTABMAP_PARAM(Stereo, Gpu,                   bool, false,   uFormat("[%s=true] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA).", kStereoOpticalFlow().c_str()));
+    RTABMAP_PARAM(Stereo, UseMinEigenVals,       bool, true,    uFormat("[%s=true] Use minimum eigen values as an error measure, otherwise L1 distance between patches is used as an error measure.", kStereoOpticalFlow().c_str()));
+    RTABMAP_PARAM(Stereo, MinEigThreshold,       double, 1e-4,  uFormat("[%s=true] If the minimum eigenvalue of a feature's spatial gradient matrix is less than this threshold, then the feature is filtered out.", kStereoUseMinEigenVals().c_str()));
+    RTABMAP_PARAM(Stereo, ErrorThreshold,        double, 50,    uFormat("[%s=false] Filter out features with error greater than this threshold.", kStereoUseMinEigenVals().c_str()));
+    RTABMAP_PARAM(Stereo, Gpu,                   bool, false,   uFormat("[%s=true] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA). Note that %s is not used in the GPU implementation.", kStereoOpticalFlow().c_str(), kStereoUseMinEigenVals().c_str()));
 
     RTABMAP_PARAM(Stereo, DenseStrategy,         int, 0,  "0=cv::StereoBM, 1=cv::StereoSGBM");
 

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -41,6 +41,7 @@ public:
 		inliersMeanDistance(0.0f),
 		inliersDistribution(0.0f),
 		matches(0),
+		variance(0.0f),
 		icpInliersRatio(0),
 		icpTranslation(0.0f),
 		icpRotation(0.0f),
@@ -64,6 +65,7 @@ public:
 		output.inliersDistribution = inliersDistribution;
 		output.matches = matches;
 		output.matchesPerCam = matchesPerCam;
+		output.variance = variance;
 		output.icpInliersRatio = icpInliersRatio;
 		output.icpTranslation = icpTranslation;
 		output.icpRotation = icpRotation;
@@ -85,6 +87,7 @@ public:
 	float inliersDistribution;
 	std::vector<int> inliersIDs;
 	int matches;
+	float variance;
 	std::vector<int> matchesIDs;
 	std::vector<int> projectedIDs; // "From" IDs
 	std::vector<int> inliersPerCam;

corelib/include/rtabmap/core/RegistrationVis.h

@@ -91,6 +91,9 @@ private:
 	float _flowEps;
 	int _flowMaxLevel;
 	bool _flowGpu;
+	bool _flowUseMinEigenVals;
+	float _flowMinEigThreshold;
+	float _flowErrorThreshold;
 	float _nndr;
 	int _nnType;
 	bool _gmsWithRotation;

corelib/include/rtabmap/core/Rtabmap.h

@@ -209,7 +209,8 @@ public:
 			bool intraSession = true,
 			bool interSession = true,
 			const ProgressState * state = 0,
-			float clusterRadiusMin = 0.0f);
+			float clusterRadiusMin = 0.0f,
+			int toFromMapId = -1);
 	bool globalBundleAdjustment(
 			int optimizerType = 1 /*g2o*/,
 			bool rematchFeatures = true,

corelib/include/rtabmap/core/Statistics.h

@@ -67,6 +67,7 @@ class RTABMAP_CORE_EXPORT Statistics
 	RTABMAP_STATS(Loop, Visual_inliers,);
 	RTABMAP_STATS(Loop, Visual_inliers_ratio,);
 	RTABMAP_STATS(Loop, Visual_matches,);
+	RTABMAP_STATS(Loop, Visual_variance,);
 	RTABMAP_STATS(Loop, Distance_since_last_loc, m);
 	RTABMAP_STATS(Loop, Last_id,);
 	RTABMAP_STATS(Loop, Optimization_max_error, m);

corelib/include/rtabmap/core/Stereo.h

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

corelib/include/rtabmap/core/VWDictionary.h

@@ -89,7 +89,7 @@ public:
 	std::vector<int> findNN(const std::list<VisualWord *> & vws) const;
 	std::vector<int> findNN(const cv::Mat & descriptors) const;
 
-	void addWordRef(int wordId, int signatureId);
+	bool addWordRef(int wordId, int signatureId);
 	void removeAllWordRef(int wordId, int signatureId);
 	const VisualWord * getWord(int id) const;
 	VisualWord * getUnusedWord(int id) const;

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

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

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

@@ -52,10 +52,10 @@ public:
     // deviceId can be either an index (e.g., "0"), an UID (e.g, "2-1-2" or "gmsl-1") or a serial ("AAA6454S")
     CameraOrbbecSDK(
         std::string deviceId = "",
-        int colorWidth = 800,
-        int colorHeight = 600,
-        int depthWidth = 800,
-        int depthHeight = 600,
+        unsigned int colorWidth = 800,
+        unsigned int colorHeight = 600,
+        unsigned int depthWidth = 800,
+        unsigned int depthHeight = 600,
         float imageRate = 0.0f,
         const Transform & localTransform = Transform::getIdentity());
     virtual ~CameraOrbbecSDK();
@@ -79,10 +79,10 @@ private:
 
 #ifdef RTABMAP_ORBBEC_SDK
     std::string deviceId_;
-    int colorWidth_;
-    int colorHeight_;
-    int depthWidth_;
-    int depthHeight_;
+    unsigned int colorWidth_;
+    unsigned int colorHeight_;
+    unsigned int depthWidth_;
+    unsigned int depthHeight_;
     ob::Pipeline * pipeline_;
     ob::Pipeline * imuPipeline_;
     ob::Align * alignFilter_;

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

@@ -146,6 +146,7 @@ private:
 	Transform dualExtrinsics_;
 	std::string jsonConfig_;
 	bool closing_;
+	bool playback_;
 
 	static Transform realsense2PoseRotation_;
 	static Transform realsense2PoseRotationInv_;

corelib/include/rtabmap/core/global_map/OccupancyGrid.h

@@ -57,7 +57,6 @@ protected:
 private:
 	cv::Mat map_;
 	cv::Mat mapInfo_;
-	std::map<int, std::pair<int, int> > cellCount_; //<node Id, cells>
 
 	float minMapSize_;
 	bool erode_;

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

@@ -31,6 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/Odometry.h>
 #include <memory>
+#include <deque>
+#include <array>
 
 #ifdef RTABMAP_CUVSLAM
 #include <cuvslam.h>
@@ -51,6 +53,7 @@ public:
 
 private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+	virtual void cleanupCuVSLAMResources();
 
 private:
 #ifdef RTABMAP_CUVSLAM
@@ -65,9 +68,21 @@ private:
 	bool lost_;
 	bool tracking_;
 	bool planar_constraints_;
+	int multicam_mode_;
 	Transform previous_pose_;
 	double last_timestamp_;
 
+	// Configuration Thresholds
+	double velocity_ratio_threshold_high_ = 1.5;			// The maximum velocity ratio of guess / estimated velocity needed to detect lost state.
+	double velocity_ratio_threshold_low_ = 0.5;				// The minimum velocity ratio of guess / estimated velocity needed to detect lost state.
+	double velocity_difference_threshold_ = 0.1;			// The maximum velocity difference between the guess and the estimated velocity needed to detect lost state.
+	double zero_estimated_velocity_threshold_ = 0.00001;	// The minimum cuVSLAM estimated velocity needed to detect lost state.
+	double min_landmarks_threshold_ = 30; 					// The minimum number of landmarks needed to start tracking after an initialization.
+	
+	// Forward cuVLSAM covariance directly to RTAB-Map.
+	// When true this disables covariance based lost detection.
+	bool use_raw_covariance_  = false;
+
 	//visualization
 	std::vector<CUVSLAM_Observation> observations_;
 	std::vector<CUVSLAM_Landmark> landmarks_;

corelib/include/rtabmap/core/util3d_features.h

@@ -80,6 +80,7 @@ std::map<int, cv::Point3f> RTABMAP_CORE_EXPORT generateWords3DMono(
 		Transform & cameraTransform,
 		float ransacReprojThreshold = 3.0f,
 		float ransacConfidence = 0.99f,
+		int varianceMedianRatio = 4,
 		const std::map<int, cv::Point3f> & refGuess3D = std::map<int, cv::Point3f>(),
 		double * variance = 0,
 		std::vector<int> * matchesOut = 0);

corelib/src/CMakeLists.txt

@@ -215,11 +215,21 @@ IF(TORCH_FOUND)
 		${SRC_FILES}
 		superpoint_torch/SuperPoint.cc
 	)
-	 SET(INCLUDE_DIRS
+	SET(INCLUDE_DIRS
 	    ${TORCH_INCLUDE_DIRS}
 	    ${CMAKE_CURRENT_SOURCE_DIR}/superpoint_torch
 		${INCLUDE_DIRS}
 	)
+	IF(WITH_PYTHON AND Python3_FOUND)
+		SET(SRC_FILES
+			${SRC_FILES}
+			superpoint_rpautrat/SuperpointRpautrat.cpp
+		)
+		SET(INCLUDE_DIRS
+	    	${CMAKE_CURRENT_SOURCE_DIR}/superpoint_rpautrat
+			${INCLUDE_DIRS}
+		)
+	ENDIF(WITH_PYTHON AND Python3_FOUND)
 ENDIF(TORCH_FOUND)
 
 IF(WITH_PYTHON AND Python3_FOUND)
@@ -781,37 +791,14 @@ IF(CUVSLAM_FOUND)
 ENDIF(CUVSLAM_FOUND)
 
 IF(GTSAM_FOUND)
-	# Make sure GTSAM is built with system Eigen, not the included one in its package
-	IF(GTSAM_INCLUDE_DIR)
-		SET(INCLUDE_DIRS 
-			${INCLUDE_DIRS} 
-			${GTSAM_INCLUDE_DIR} 
-		)
-	ELSE()
-		SET(INCLUDE_DIRS 
-			${INCLUDE_DIRS} 
-			${GTSAM_INCLUDE_DIRS}
-		)
-	ENDIF()
 	SET(SRC_FILES
     	${SRC_FILES}
 		optimizer/gtsam/GravityFactor.cpp
 	)
-	IF(WIN32)
-		# GTSAM should be built in STATIC on Windows to avoid "error C2338: THIS_METHOD_IS_ONLY_FOR_1x1_EXPRESSIONS" when building GTSAM
-		add_definitions("-DGTSAM_IMPORT_STATIC") 
-	ENDIF(WIN32)
 	SET(LIBRARIES
 		${LIBRARIES}
-		gtsam # Windows: Place static libs at the end
+		gtsam
 	)
-	IF(WIN32)
-		#explicitly add metis target on windows (after gtsam target)
-		SET(LIBRARIES
-			${LIBRARIES}
-			metis
-		)
-	ENDIF(WIN32)
 ENDIF(GTSAM_FOUND)
 
 IF(WITH_MADGWICK)
@@ -828,6 +815,7 @@ CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql.in ${CMA
 
 SET(RESOURCES
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_23_0.sql
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_22_0.sql
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_20_0.sql
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_3.sql
@@ -838,6 +826,13 @@ SET(RESOURCES
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_16_0.sql
 )
 
+IF(TORCH_FOUND AND WITH_PYTHON AND Python3_FOUND)
+	SET(RESOURCES
+		${RESOURCES}
+		${CMAKE_CURRENT_SOURCE_DIR}/superpoint_rpautrat/superpoint_to_torchscript.py
+	)
+ENDIF()
+
 foreach(arg ${RESOURCES})
    get_filename_component(filename ${arg} NAME)
    string(REPLACE "." "_" output ${filename})

corelib/src/Camera.cpp

@@ -39,7 +39,8 @@ namespace rtabmap
 Camera::Camera(float imageRate, const Transform & localTransform) :
 		SensorCapture(imageRate, localTransform*CameraModel::opticalRotation()),
 		imuFilter_(0),
-		publishInterIMU_(false)
+		publishInterIMU_(false),
+		imuBaseFrameConversion_(false)
 {}
 
 Camera::~Camera()
@@ -52,15 +53,23 @@ bool Camera::initFromFile(const std::string & calibrationPath)
 	return init(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front());
 }
 
-void Camera::setInterIMUPublishing(bool enabled, IMUFilter * filter)
+void Camera::setInterIMUPublishing(bool enabled, IMUFilter * filter, bool baseFrameConversion)
 {
 	publishInterIMU_ = enabled;
 	delete imuFilter_;
 	imuFilter_ = filter;
+	imuBaseFrameConversion_ = baseFrameConversion;
 }
 
-void Camera::postInterIMU(const IMU & imu, double stamp)
+void Camera::postInterIMU(const IMU & imu_in, double stamp)
 {
+	IMU imu = imu_in;
+	if(imuBaseFrameConversion_)
+	{
+		UASSERT(!imu.localTransform().isNull());
+		imu.convertToBaseFrame();
+	}
+
 	if(imuFilter_)
 	{
 		imuFilter_->update(

corelib/src/CameraModel.cpp

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

corelib/src/DBDriver.cpp

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/VisualWord.h"
 #include "rtabmap/core/DBDriverSqlite3.h"
+#include "rtabmap/core/Compression.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UMath.h"
 #include "rtabmap/utilite/ULogger.h"
@@ -45,7 +46,7 @@ DBDriver * DBDriver::create(const ParametersMap & parameters)
 
 DBDriver::DBDriver(const ParametersMap & parameters) :
 	_emptyTrashesTime(0),
-	_timestampUpdate(true)
+	_timestampUpdate(false)
 {
 	this->parseParameters(parameters);
 }
@@ -73,7 +74,15 @@ void DBDriver::closeConnection(bool save, const std::string & outputUrl)
 	else
 	{
 		_trashesMutex.lock();
+		for(auto & iter: _trashSignatures)
+		{
+			delete iter.second;
+		}
 		_trashSignatures.clear();
+		for(auto & iter: _trashVisualWords)
+		{
+			delete iter.second;
+		}
 		_trashVisualWords.clear();
 		_trashesMutex.unlock();
 	}
@@ -83,12 +92,12 @@ void DBDriver::closeConnection(bool save, const std::string & outputUrl)
 	UDEBUG("");
 }
 
-bool DBDriver::openConnection(const std::string & url, bool overwritten)
+bool DBDriver::openConnection(const std::string & url, bool overwritten, bool readOnly)
 {
 	UDEBUG("");
 	_url = url;
 	_dbSafeAccessMutex.lock();
-	if(this->connectDatabaseQuery(url, overwritten))
+	if(this->connectDatabaseQuery(url, overwritten, readOnly))
 	{
 		_dbSafeAccessMutex.unlock();
 		return true;
@@ -695,11 +704,11 @@ void DBDriver::getNodeData(
 	if(uContains(_trashSignatures, signatureId))
 	{
 		const Signature * s = _trashSignatures.at(signatureId);
-		if((!s->isSaved() ||
+		if(!s->isSaved() ||
 			((!images || !s->sensorData().imageCompressed().empty()) &&
 			 (!scan || !s->sensorData().laserScanCompressed().isEmpty()) &&
 			 (!userData || !s->sensorData().userDataCompressed().empty()) &&
-			 (!occupancyGrid || s->sensorData().gridCellSize() != 0.0f))))
+			 (!occupancyGrid || s->sensorData().gridCellSize() != 0.0f)))
 		{
 			data = (SensorData)s->sensorData();
 			if(!images)
@@ -1505,4 +1514,132 @@ void DBDriver::generateGraph(
 	}
 }
 
+
+std::vector<unsigned char> DBDriver::serializeFeatures(
+	const std::vector<cv::KeyPoint> & keypoints, 
+	const std::vector<cv::Point3f> & points3D,
+	const cv::Mat & descriptors) const
+{
+	UTimer timer;
+	const int headerSize = 13;
+	int header[headerSize] = {
+			RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH, // 0,1,2
+			CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION,             // 3,4,5 (In case the format/order/size of KeyPoint and/or Point3f changes in the future)
+			sizeof(cv::KeyPoint), (int)keypoints.size(),                         // 6,7
+			sizeof(cv::Point3f), (int)points3D.size(),                           // 8,9
+			descriptors.type(), descriptors.cols, descriptors.rows};             // 10,11,12
+	UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d %d %d",
+		header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10],header[11],header[12]);
+	std::vector<unsigned char> data(
+			sizeof(int)*headerSize +
+			keypoints.size()*sizeof(cv::KeyPoint) + // pos_x, pos_y, size, dir, response, octave
+			points3D.size()*sizeof(cv::Point3f) + // depth_x, depth_y, depth_z
+			descriptors.total()*descriptors.elemSize());
+	UDEBUG("Serialized total size = %ld bytes (header=%ld)", data.size(), sizeof(int)*headerSize);
+	memcpy(data.data(), header, sizeof(int)*headerSize);
+	size_t index = sizeof(int)*headerSize;
+	if(!keypoints.empty())
+	{
+		memcpy(data.data()+index, keypoints.data(), sizeof(cv::KeyPoint)*keypoints.size());
+		index += sizeof(cv::KeyPoint)*(keypoints.size());
+	}
+	if(!points3D.empty())
+	{
+		memcpy(data.data()+index, points3D.data(), sizeof(cv::Point3f)*points3D.size());
+		index += sizeof(cv::Point3f)*(points3D.size());
+	}
+	if(!descriptors.empty())
+	{
+		memcpy(data.data()+index, descriptors.data, descriptors.elemSize()*descriptors.total());
+		index+=descriptors.elemSize()*(descriptors.total());
+	}
+	double serializationTime = timer.ticks();
+	UASSERT_MSG(index == data.size(), uFormat("wrote=%ld expected=%ld", index, data.size()).c_str());
+	std::vector<unsigned char> compressedData = compressData(cv::Mat(1, data.size(), CV_8UC1, (void *)data.data()));
+	UWARN("Serialized %ld bytes in %f ms, Compressed %ld bytes in %f ms",
+		data.size(), serializationTime*1000.0f,
+		compressedData.size(), timer.ticks()*1000.0f);
+	return compressedData;
+}
+
+bool DBDriver::deserializeFeatures(
+	const unsigned char * compressedData,
+	unsigned int compressedDataSize,
+	std::vector<cv::KeyPoint> & keypoints,
+	std::vector<cv::Point3f> & points3D,
+	cv::Mat & descriptors) const
+{
+	UTimer timer;
+	cv::Mat serializedData = uncompressData(compressedData, compressedDataSize);
+	double uncompressionTime = timer.ticks();
+	if(serializedData.empty())
+	{
+		return false;
+	}
+	UDEBUG("Decompressed serialized data = %dx%d type=%d",
+		serializedData.cols, serializedData.rows, serializedData.type());
+	UASSERT(serializedData.type() == CV_8UC1);
+	int headerSize = 13;
+	if(serializedData.total() >= sizeof(int)*headerSize)
+	{
+		const int * header = (const int *)serializedData.data;
+		UASSERT(header[6] == sizeof(cv::KeyPoint));
+		int n_kpts = header[7];
+		UASSERT(header[8] == sizeof(cv::Point3f));
+		int n_pts = header[9];
+		int d_type = header[10];
+		int d_cols = header[11];
+		int d_rows = header[12];
+
+		UDEBUG("Serialized features header: version %d.%d.%d cv=%d.%d.%d kpts=%d (size=%d) pts=%d (size=%d) descriptors=%dx%d type=%d",
+			header[0], header[1], header[2], 
+			header[3], header[4], header[5],
+			header[7], header[6],
+			header[9], header[8],
+			header[11], header[12], header[10]);
+
+		keypoints.resize(n_kpts);
+		points3D.resize(n_pts);
+		descriptors = cv::Mat(d_rows, d_cols, d_type);
+		unsigned int requiredDataSize = sizeof(int)*headerSize +
+				sizeof(cv::KeyPoint)*n_kpts +
+				sizeof(cv::Point3f)*n_pts +
+				descriptors.total() * descriptors.elemSize();
+		UASSERT_MSG(serializedData.total() == requiredDataSize,
+				uFormat("dataSize=%d != required=%d (header: version %d.%d.%d cv=%d.%d.%d kpts=%d (size=%d) pts=%d (size=%d) descriptors=%dx%d type=%d",
+						serializedData.total(),
+						requiredDataSize,
+						header[0], header[1], header[2], 
+						header[3], header[4], header[5],
+						header[7], header[6],
+						header[9], header[8],
+						header[11], header[12], header[10]).c_str());
+		unsigned int index = sizeof(int)*headerSize;
+		if(n_kpts != 0)
+		{
+			memcpy(keypoints.data(), (void*)(serializedData.data+index), n_kpts*sizeof(cv::KeyPoint));
+			index += n_kpts*sizeof(cv::KeyPoint);
+		}
+		if(n_pts != 0)
+		{
+			memcpy(points3D.data(), (void*)(serializedData.data+index), n_pts*sizeof(cv::Point3f));
+			index += n_pts*sizeof(cv::Point3f);
+		}
+		if(d_rows > 0)
+		{
+			cv::Mat(d_rows, d_cols, d_type, (void*)(serializedData.data+index)).copyTo(descriptors);
+			index+=descriptors.elemSize()*(descriptors.total());
+		}
+		UASSERT(index == serializedData.total());
+
+		UWARN("Uncompressed %ld bytes in %f ms, deserialized %ld bytes in %f ms",
+			compressedDataSize, uncompressionTime*1000.0f,
+			serializedData.total(), timer.ticks()*1000.0f);
+
+		return true;
+	}
+	UERROR("Wrong serialized features format detected (size in bytes=%ld)! Cannot deserialize the data.", serializedData.size());
+	return false;
+}
+
 } // namespace rtabmap

corelib/src/DBDriverSqlite3.cpp

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/Compression.h"
 #include "DatabaseSchema_sql.h"
+#include "DatabaseSchema_0_23_0_sql.h"
 #include "DatabaseSchema_0_22_0_sql.h"
 #include "DatabaseSchema_0_20_0_sql.h"
 #include "DatabaseSchema_0_18_3_sql.h"
@@ -320,7 +321,7 @@ bool DBDriverSqlite3::getDatabaseVersionQuery(std::string & version) const
 	return false;
 }
 
-bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwritten)
+bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwritten, bool readOnly)
 {
 	this->disconnectDatabaseQuery();
 	// Open a database connection
@@ -332,7 +333,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 	if(!url.empty())
 	{
 		dbFileExist = UFile::exists(url.c_str());
-		if(dbFileExist && overwritten)
+		if(dbFileExist && overwritten && !readOnly)
 		{
 			UINFO("Deleting database %s...", url.c_str());
 			UASSERT(UFile::erase(url.c_str()) == 0);
@@ -354,12 +355,12 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 		{
 			ULOGGER_INFO("Using empty database in the memory.");
 		}
-		rc = sqlite3_open_v2(":memory:", &_ppDb, SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
+		rc = sqlite3_open_v2(":memory:", &_ppDb, readOnly ? SQLITE_OPEN_READONLY : SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
 	}
 	else
 	{
 		ULOGGER_INFO("Using database \"%s\" from the hard drive.", url.c_str());
-		rc = sqlite3_open_v2(url.c_str(), &_ppDb, SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
+		rc = sqlite3_open_v2(url.c_str(), &_ppDb, readOnly ? SQLITE_OPEN_READONLY : SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
 	}
 	if(rc != SQLITE_OK)
 	{
@@ -406,6 +407,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 			schemas.push_back(std::make_pair("0.18.3", DATABASESCHEMA_0_18_3_SQL));
 			schemas.push_back(std::make_pair("0.20.0", DATABASESCHEMA_0_20_0_SQL));
 			schemas.push_back(std::make_pair("0.22.0", DATABASESCHEMA_0_22_0_SQL));
+			schemas.push_back(std::make_pair("0.23.0", DATABASESCHEMA_0_23_0_SQL));
 			schemas.push_back(std::make_pair(uNumber2Str(RTABMAP_VERSION_MAJOR)+"."+uNumber2Str(RTABMAP_VERSION_MINOR), DATABASESCHEMA_SQL));
 			for(size_t i=0; i<schemas.size(); ++i)
 			{
@@ -881,7 +883,15 @@ long DBDriverSqlite3::getFeaturesMemoryUsedQuery() const
 	if(_ppDb)
 	{
 		std::string query;
-		if(uStrNumCmp(_version, "0.13.0") >= 0)
+		if(uStrNumCmp(_version, "0.24.0") >= 0)
+		{
+			query = "SELECT ("
+					 "(SELECT sum(length(node_id) + length(word_id) + length(feature_index)) FROM Feature)"
+					 " + "
+					 "(SELECT total(length(features)) FROM Node)"
+					 ")";
+		}
+		else if(uStrNumCmp(_version, "0.13.0") >= 0)
 		{
 			query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(octave) + ifnull(length(depth_x),0) + ifnull(length(depth_y),0) + ifnull(length(depth_z),0) + ifnull(length(descriptor_size),0) + ifnull(length(descriptor),0)) "
 					 "FROM Feature";
@@ -1319,6 +1329,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 		{
 			std::stringstream fields;
 
+			bool fieldAdded = false;
 			if(images)
 			{
 				if(uStrNumCmp(_version, "0.22.0") >= 0)
@@ -1329,30 +1340,33 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 				{
 					fields << "image, depth, calibration";
 				}
-				
-				if(scan || userData || occupancyGrid)
-				{
-					fields << ", ";
-				}
+				fieldAdded = true;
 			}
 			if(scan)
 			{
-				fields << "scan_info, scan";
-				if(userData || occupancyGrid)
+				if(fieldAdded)
 				{
 					fields << ", ";
 				}
+				fieldAdded = true;
+				fields << "scan_info, scan";
 			}
 			if(userData)
 			{
-				fields << "user_data";
-				if(occupancyGrid)
+				if(fieldAdded)
 				{
 					fields << ", ";
 				}
+				fieldAdded = true;
+				fields << "user_data";
 			}
 			if(occupancyGrid)
 			{
+				if(fieldAdded)
+				{
+					fields << ", ";
+				}
+				fieldAdded = true;
 				if(uStrNumCmp(_version, "0.16.0") >= 0)
 				{
 					fields << "ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z";
@@ -3846,13 +3860,22 @@ void DBDriverSqlite3::loadWordIdsQuery(std::list<Signature *> & signatures) cons
 
 void DBDriverSqlite3::loadWordsQuery(std::list<Signature *> & signatures) const
 {
+	UTimer totalTime;
 	if(_ppDb)
 	{
+		bool before_v0_24 = uStrNumCmp(_version, "0.24.0") < 0;
+		
 		int rc = SQLITE_OK;
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		if(uStrNumCmp(_version, "0.13.0") >= 0)
+		if(uStrNumCmp(_version, "0.24.0") >= 0)
+		{
+			query << "SELECT word_id, feature_index "
+					 "FROM Feature "
+					 "WHERE node_id = ? ";
+		}
+		else if(uStrNumCmp(_version, "0.13.0") >= 0)
 		{
 			query << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
 					 "FROM Feature "
@@ -3876,7 +3899,6 @@ void DBDriverSqlite3::loadWordsQuery(std::list<Signature *> & signatures) const
 					 "FROM Map_Node_Word "
 					 "WHERE node_id = ? ";
 		}
-
 		query << " ORDER BY word_id"; // Needed for fast insertion below
 		query << ";";
 
@@ -3884,7 +3906,7 @@ void DBDriverSqlite3::loadWordsQuery(std::list<Signature *> & signatures) const
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
 		float nanFloat = std::numeric_limits<float>::quiet_NaN ();
-
+		std::vector<std::multimap<int, int> > allVisualWords;
 		for(std::list<Signature*>::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
 		{
 			//ULOGGER_DEBUG("Loading words of %d...", (*iter)->id());
@@ -3893,6 +3915,7 @@ void DBDriverSqlite3::loadWordsQuery(std::list<Signature *> & signatures) const
 			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
 			int visualWordId = 0;
+			int featureIndex = 0;
 			int descriptorSize = 0;
 			const void * descriptor = 0;
 			int dRealSize = 0;
@@ -3910,82 +3933,90 @@ void DBDriverSqlite3::loadWordsQuery(std::list<Signature *> & signatures) const
 			{
 				int index = 0;
 				visualWordId = sqlite3_column_int(ppStmt, index++);
-				kpt.pt.x = sqlite3_column_double(ppStmt, index++);
-				kpt.pt.y = sqlite3_column_double(ppStmt, index++);
-				kpt.size = sqlite3_column_int(ppStmt, index++);
-				kpt.angle = sqlite3_column_double(ppStmt, index++);
-				kpt.response = sqlite3_column_double(ppStmt, index++);
-				if(uStrNumCmp(_version, "0.12.0") >= 0)
+				if(!before_v0_24)
 				{
-					kpt.octave = sqlite3_column_int(ppStmt, index++);
+					featureIndex = sqlite3_column_int(ppStmt, index++);
+					visualWords.insert(visualWords.end(), std::make_pair(visualWordId, featureIndex));
 				}
-
-				if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
+				else if(before_v0_24)
 				{
-					depth.x = nanFloat;
-					++index;
-				}
-				else
-				{
-					depth.x = sqlite3_column_double(ppStmt, index++);
-				}
-
-				if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
-				{
-					depth.y = nanFloat;
-					++index;
-				}
-				else
-				{
-					depth.y = sqlite3_column_double(ppStmt, index++);
-				}
-
-				if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
-				{
-					depth.z = nanFloat;
-					++index;
-				}
-				else
-				{
-					depth.z = sqlite3_column_double(ppStmt, index++);
-				}
-
-				visualWordsKpts.push_back(kpt);
-				visualWords.insert(visualWords.end(), std::make_pair(visualWordId, visualWordsKpts.size()-1));
-				visualWords3.push_back(depth);
-
-				if(allWords3NaN && util3d::isFinite(depth))
-				{
-					allWords3NaN = false;
-				}
-
-				if(uStrNumCmp(_version, "0.11.2") >= 0)
-				{
-					descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
-					descriptor = sqlite3_column_blob(ppStmt, index); 	// VisualWord descriptor array
-					dRealSize = sqlite3_column_bytes(ppStmt, index++);
-
-					if(descriptor && descriptorSize>0 && dRealSize>0)
+					kpt.pt.x = sqlite3_column_double(ppStmt, index++);
+					kpt.pt.y = sqlite3_column_double(ppStmt, index++);
+					kpt.size = sqlite3_column_int(ppStmt, index++);
+					kpt.angle = sqlite3_column_double(ppStmt, index++);
+					kpt.response = sqlite3_column_double(ppStmt, index++);
+					if(uStrNumCmp(_version, "0.12.0") >= 0)
 					{
-						cv::Mat d;
-						if(dRealSize == descriptorSize)
-						{
-							// CV_8U binary descriptors
-							d = cv::Mat(1, descriptorSize, CV_8U);
-						}
-						else if(dRealSize/int(sizeof(float)) == descriptorSize)
-						{
-							// CV_32F
-							d = cv::Mat(1, descriptorSize, CV_32F);
-						}
-						else
-						{
-							UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
-						}
+						kpt.octave = sqlite3_column_int(ppStmt, index++);
+					}
 
-						memcpy(d.data, descriptor, dRealSize);
+					if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
+					{
+						depth.x = nanFloat;
+						++index;
+					}
+					else
+					{
+						depth.x = sqlite3_column_double(ppStmt, index++);
+					}
 
-						descriptors.push_back(d);
+					if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
+					{
+						depth.y = nanFloat;
+						++index;
+					}
+					else
+					{
+						depth.y = sqlite3_column_double(ppStmt, index++);
+					}
+
+					if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
+					{
+						depth.z = nanFloat;
+						++index;
+					}
+					else
+					{
+						depth.z = sqlite3_column_double(ppStmt, index++);
+					}
+
+					visualWordsKpts.push_back(kpt);
+					visualWords.insert(visualWords.end(), std::make_pair(visualWordId, visualWordsKpts.size()-1));
+					visualWords3.push_back(depth);
+
+					if(allWords3NaN && util3d::isFinite(depth))
+					{
+						allWords3NaN = false;
+					}
+
+					if(uStrNumCmp(_version, "0.11.2") >= 0)
+					{
+						descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
+						descriptor = sqlite3_column_blob(ppStmt, index); 	// VisualWord descriptor array
+						dRealSize = sqlite3_column_bytes(ppStmt, index++);
+
+						if(descriptor && descriptorSize>0 && dRealSize>0)
+						{
+							cv::Mat d;
+							if(dRealSize == descriptorSize)
+							{
+								// CV_8U binary descriptors
+								d = cv::Mat(1, descriptorSize, CV_8U);
+							}
+							else if(dRealSize/int(sizeof(float)) == descriptorSize)
+							{
+								// CV_32F
+								d = cv::Mat(1, descriptorSize, CV_32F);
+							}
+							else
+							{
+								UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
+							}
+
+							memcpy(d.data, descriptor, dRealSize);
+
+							descriptors.push_back(d);
+						}
 					}
 				}
 
@@ -3993,18 +4024,25 @@ void DBDriverSqlite3::loadWordsQuery(std::list<Signature *> & signatures) const
 			}
 			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
-			if(visualWords.size()==0)
+			if(before_v0_24)
 			{
-				UDEBUG("Empty signature detected! (id=%d)", (*iter)->id());
+				if(visualWords.size()==0)
+				{
+					UDEBUG("Empty signature detected! (id=%d)", (*iter)->id());
+				}
+				else
+				{
+					if(allWords3NaN)
+					{
+						visualWords3.clear();
+					}
+					(*iter)->setWords(visualWords, visualWordsKpts, visualWords3, descriptors);
+					//ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id());
+				}
 			}
 			else
 			{
-				if(allWords3NaN)
-				{
-					visualWords3.clear();
-				}
-				(*iter)->setWords(visualWords, visualWordsKpts, visualWords3, descriptors);
-				//ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id());
+				allVisualWords.push_back(visualWords);
 			}
 
 			//reset
@@ -4015,7 +4053,73 @@ void DBDriverSqlite3::loadWordsQuery(std::list<Signature *> & signatures) const
 		// Finalize (delete) the statement
 		rc = sqlite3_finalize(ppStmt);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		if(!before_v0_24)
+		{
+			// Features are now in compressed field "features" of table Node
+			std::string queryStr = "SELECT features FROM Node WHERE id = ?;";
+
+			rc = sqlite3_prepare_v2(_ppDb, queryStr.c_str(), -1, &ppStmt, 0);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			UASSERT(allVisualWords.size() == signatures.size());
+			int w=0;
+			for(std::list<Signature*>::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter, ++w)
+			{
+				if(allVisualWords[w].empty())
+				{
+					continue;
+				}
+				ULOGGER_DEBUG("Loading compressed features of %d...", (*iter)->id());
+				// bind id
+				rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+				std::multimap<int, int> & visualWords = allVisualWords[w];
+				std::vector<cv::KeyPoint> visualWordsKpts;
+				std::vector<cv::Point3f> visualWords3;
+				cv::Mat descriptors;
+
+				// Process the result if one
+				rc = sqlite3_step(ppStmt);
+				if(rc == SQLITE_ROW)
+				{
+					int index = 0;
+					const void * data = sqlite3_column_blob(ppStmt, index);
+					int dataSize = sqlite3_column_bytes(ppStmt, index++);
+					if(dataSize > 0 && data)
+					{
+						if(!deserializeFeatures((const unsigned char *)data, dataSize, visualWordsKpts, visualWords3, descriptors))
+						{
+							UERROR("Failed deserializing features for node %d! (dataSize=%d)", (*iter)->id(), dataSize);
+						}
+					}
+
+					rc = sqlite3_step(ppStmt);
+				}
+				UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+				UASSERT_MSG(visualWords.size() == visualWordsKpts.size(), 
+					uFormat("visualWords=%ld visualWordsKpts=%ld", visualWords.size(), visualWordsKpts.size()).c_str());
+				UASSERT_MSG(visualWords3.empty() || visualWords.size() == visualWords3.size(),
+					uFormat("visualWords=%ld visualWordsKpts=%ld", visualWords.size(), visualWords3.size()).c_str());
+				UASSERT_MSG(descriptors.empty() || (int)visualWords.size() == descriptors.rows,
+					uFormat("visualWords=%ld visualWordsKpts=%d", visualWords.size(), descriptors.rows).c_str());
+
+				(*iter)->setWords(visualWords, visualWordsKpts, visualWords3, descriptors);
+				ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), (int)visualWords3.size(), (int)descriptors.rows, (*iter)->id());
+
+				//reset
+				rc = sqlite3_reset(ppStmt);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			}
+
+			// Finalize (delete) the statement
+			rc = sqlite3_finalize(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
 	}
+	UWARN("totalTime=%f ms", totalTime.ticks() *1000.0f);
 }
 
 void DBDriverSqlite3::loadLinksQuery(
@@ -4343,7 +4447,7 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 
 void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool updateTimestamp) const
 {
-	UDEBUG("nodes = %d", nodes.size());
+	UDEBUG("nodes = %d, updateTimestamp = %s", nodes.size(), updateTimestamp?"true":"false");
 	if(_ppDb && nodes.size())
 	{
 		UTimer timer;
@@ -4382,7 +4486,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
 		{
 			s = *i;
 			int index = 1;
-			if(s)
+			if(s && (s->isModified() || updateTimestamp))
 			{
 				rc = sqlite3_bind_int(ppStmt, index++, s->getWeight());
 				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -4406,7 +4510,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
 
 				//step
 				rc=sqlite3_step(ppStmt);
-				UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+				UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s (node id = %d map=%d)", _version.c_str(), sqlite3_errmsg(_ppDb), s->id(), s->mapId()).c_str());
 
 				rc = sqlite3_reset(ppStmt);
 				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -4419,14 +4523,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
 		ULOGGER_DEBUG("Update Node table, Time=%fs", timer.ticks());
 
 		// Update links part1
-		if(uStrNumCmp(_version, "0.18.3") >= 0)
-		{
-			query = uFormat("DELETE FROM Link WHERE from_id=? and type!=%d;", (int)Link::kLandmark);
-		}
-		else
-		{
-			query = uFormat("DELETE FROM Link WHERE from_id=?;");
-		}
+		query = uFormat("DELETE FROM Link WHERE from_id=?;");
 		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 		for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
@@ -4461,6 +4558,12 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
 				{
 					stepLink(ppStmt, i->second);
 				}
+				// Save landmarks
+				const std::map<int, Link> & landmarks = (*j)->getLandmarks();
+				for(std::map<int, Link>::const_iterator i=landmarks.begin(); i!=landmarks.end(); ++i)
+				{
+					stepLink(ppStmt, i->second);
+				}
 			}
 		}
 		// Finalize (delete) the statement
@@ -4600,24 +4703,35 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
 		for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
 		{
 			UASSERT((*i)->getWords().size() == (*i)->getWordsKpts().size());
-			UASSERT((*i)->getWords3().empty() || (*i)->getWords().size() == (*i)->getWords3().size());
-			UASSERT((*i)->getWordsDescriptors().empty() || (int)(*i)->getWords().size() == (*i)->getWordsDescriptors().rows);
-
-			for(std::multimap<int, int>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
+			if(uStrNumCmp(_version, "0.24.0") >= 0)
 			{
-				cv::Point3f pt(nanFloat,nanFloat,nanFloat);
-				if(!(*i)->getWords3().empty())
+				// Only node -> word -> index are saved in Feature
+				for(std::multimap<int, int>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
 				{
-					pt = (*i)->getWords3()[w->second];
+					stepKeypoint(ppStmt, (*i)->id(), w->first, w->second);
 				}
+			}
+			else
+			{
+				UASSERT((*i)->getWords3().empty() || (*i)->getWords().size() == (*i)->getWords3().size());
+				UASSERT((*i)->getWordsDescriptors().empty() || (int)(*i)->getWords().size() == (*i)->getWordsDescriptors().rows);
 
-				cv::Mat descriptor;
-				if(!(*i)->getWordsDescriptors().empty())
+				for(std::multimap<int, int>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
 				{
-					descriptor = (*i)->getWordsDescriptors().row(w->second);
-				}
+					cv::Point3f pt(nanFloat,nanFloat,nanFloat);
+					if(!(*i)->getWords3().empty())
+					{
+						pt = (*i)->getWords3()[w->second];
+					}
 
-				stepKeypoint(ppStmt, (*i)->id(), w->first, (*i)->getWordsKpts()[w->second], pt, descriptor);
+					cv::Mat descriptor;
+					if(!(*i)->getWordsDescriptors().empty())
+					{
+						descriptor = (*i)->getWordsDescriptors().row(w->second);
+					}
+
+					stepKeypoint(ppStmt, (*i)->id(), w->first, (*i)->getWordsKpts()[w->second], pt, descriptor);
+				}
 			}
 		}
 		// Finalize (delete) the statement
@@ -5751,7 +5865,7 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
 
 void DBDriverSqlite3::saveFlannIndexQuery(const std::vector<unsigned char> & data) const
 {
-	UDEBUG("");
+	UDEBUG("data size = %ld bytes", data.size());
 	if(_ppDb && uStrNumCmp(_version, "0.23.0") >= 0)
 	{
 		UTimer timer;
@@ -5792,7 +5906,11 @@ void DBDriverSqlite3::saveFlannIndexQuery(const std::vector<unsigned char> & dat
 
 std::string DBDriverSqlite3::queryStepNode() const
 {
-	if(uStrNumCmp(_version, "0.18.0") >= 0)
+	if(uStrNumCmp(_version, "0.24.0") >= 0)
+	{
+		return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps, env_sensors, features) VALUES(?,?,?,?,?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.18.0") >= 0)
 	{
 		return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps, env_sensors) VALUES(?,?,?,?,?,?,?,?,?,?);";
 	}
@@ -5824,6 +5942,7 @@ std::string DBDriverSqlite3::queryStepNode() const
 }
 void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
 {
+	UTimer totalTime;
 	UDEBUG("Save node %d", s->id());
 	if(!ppStmt || !s)
 	{
@@ -5860,6 +5979,7 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
 
 	std::vector<double> gps;
 	std::vector<double> envSensors;
+	std::vector<unsigned char> serializedFeatures;
 	if(uStrNumCmp(_version, "0.10.1") >= 0)
 	{
 		// ignore user_data
@@ -5943,12 +6063,30 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
 		}
 	}
 
+	if(uStrNumCmp(_version, "0.24.0") >= 0)
+	{
+		//features
+		serializedFeatures = serializeFeatures(s->getWordsKpts(), s->getWords3(), s->getWordsDescriptors());
+		if(serializedFeatures.empty())
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+		else
+		{
+			rc = sqlite3_bind_blob(ppStmt, index++, serializedFeatures.data(), (int)serializedFeatures.size(), SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+	}
+
 	//step
 	rc=sqlite3_step(ppStmt);
 	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
 	rc = sqlite3_reset(ppStmt);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	UWARN("totalTime=%f ms", totalTime.ticks()*1000.0f);
 }
 
 std::string DBDriverSqlite3::queryStepImage() const
@@ -6894,7 +7032,11 @@ void DBDriverSqlite3::stepWordsChanged(sqlite3_stmt * ppStmt, int nodeId, int ol
 
 std::string DBDriverSqlite3::queryStepKeypoint() const
 {
-	if(uStrNumCmp(_version, "0.13.0") >= 0)
+	if(uStrNumCmp(_version, "0.24.0") >= 0)
+	{
+		return "INSERT INTO Feature(node_id, word_id, feature_index) VALUES(?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.13.0") >= 0)
 	{
 		return "INSERT INTO Feature(node_id, word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?);";
 	}
@@ -6908,6 +7050,32 @@ std::string DBDriverSqlite3::queryStepKeypoint() const
 	}
 	return "INSERT INTO Map_Node_Word(node_id, word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z) VALUES(?,?,?,?,?,?,?,?,?,?);";
 }
+void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
+		int nodeId,
+		int wordId,
+		int kptIndex) const
+{
+	// Used with version >= 0.24
+	UASSERT(uStrNumCmp(_version, "0.24.0") >= 0);
+	if(!ppStmt)
+	{
+		UFATAL("");
+	}
+	int rc = SQLITE_OK;
+	int index = 1;
+	rc = sqlite3_bind_int(ppStmt, index++, nodeId);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+	rc = sqlite3_bind_int(ppStmt, index++, wordId);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+	rc = sqlite3_bind_double(ppStmt, index++, kptIndex);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+	
+	rc=sqlite3_step(ppStmt);
+	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	rc = sqlite3_reset(ppStmt);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+}
 void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
 		int nodeId,
 		int wordId,
@@ -6915,6 +7083,8 @@ void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
 		const cv::Point3f & pt,
 		const cv::Mat & descriptor) const
 {
+	// Used with version < 0.24
+	UASSERT(uStrNumCmp(_version, "0.24.0") < 0);
 	if(!ppStmt)
 	{
 		UFATAL("");

corelib/src/DBReader.cpp

@@ -56,6 +56,7 @@ DBReader::DBReader(const std::string & databasePath,
 				   int startMapId,
 				   int stopMapId,
 				   bool priorsIgnored,
+				   bool imuIgnored,
 				   const std::vector<Transform> & cameraLocalTransformOverrides) :
 	Camera(frameRate),
 	_paths(uSplit(databasePath, ';')),
@@ -69,6 +70,7 @@ DBReader::DBReader(const std::string & databasePath,
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
 	_priorsIgnored(priorsIgnored),
+	_imuIgnored(imuIgnored),
 	_startMapId(startMapId),
 	_stopMapId(stopMapId),
 	_cameraLocalTransformOverrides(cameraLocalTransformOverrides),
@@ -96,6 +98,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   int startMapId,
 				   int stopMapId,
 				   bool priorsIgnored,
+				   bool imuIgnored,
 				   const std::vector<Transform> & cameraLocalTransformOverrides) :
 	Camera(frameRate),
    _paths(databasePaths),
@@ -109,6 +112,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
 	_priorsIgnored(priorsIgnored),
+	_imuIgnored(imuIgnored),
 	_startMapId(startMapId),
 	_stopMapId(stopMapId),
 	_cameraLocalTransformOverrides(cameraLocalTransformOverrides),
@@ -463,14 +467,17 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 			}
 
 			Transform gravityTransform;
-			std::multimap<int, Link> gravityLinks;
-			_dbDriver->loadLinks(*_currentId, gravityLinks, Link::kGravity);
-			if( gravityLinks.size() &&
-				!gravityLinks.begin()->second.transform().isNull() &&
-				gravityLinks.begin()->second.infMatrix().cols == 6 &&
-				gravityLinks.begin()->second.infMatrix().rows == 6)
+			if(!_imuIgnored)
 			{
-				gravityTransform = gravityLinks.begin()->second.transform();
+				std::multimap<int, Link> gravityLinks;
+				_dbDriver->loadLinks(*_currentId, gravityLinks, Link::kGravity);
+				if( gravityLinks.size() &&
+					!gravityLinks.begin()->second.transform().isNull() &&
+					gravityLinks.begin()->second.infMatrix().cols == 6 &&
+					gravityLinks.begin()->second.infMatrix().rows == 6)
+				{
+					gravityTransform = gravityLinks.begin()->second.transform();
+				}
 			}
 
 			Landmarks landmarks;

corelib/src/Features2d.cpp

@@ -47,6 +47,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifdef RTABMAP_TORCH
 #include "superpoint_torch/SuperPoint.h"
 #endif
+#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
+#include "superpoint_rpautrat/SuperpointRpautrat.h"
+#endif
 
 #ifdef RTABMAP_PYTHON
 #include "python/PyDetector.h"
@@ -300,7 +303,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vecto
 		cv::Mat descriptorsTmp;
 		if(ssc)
 		{
-			ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
+			ULOGGER_DEBUG("too many words (%d), removing words with SSC", keypoints.size());
 
 			// Sorting keypoints by deacreasing order of strength
 			std::vector<float> responseVector;
@@ -416,7 +419,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
 		inliers.resize(keypoints.size(), false);
 		if(ssc)
 		{
-			ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
+			ULOGGER_DEBUG("too many words (%d), removing words with SSC", keypoints.size());
 
 			// Sorting keypoints by deacreasing order of strength
 			std::vector<float> responseVector;
@@ -463,7 +466,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
 				minimumHessian = iter->first;
 			}
 		}
-		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, maxKeypoints, minimumHessian);
+		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size()-removed, minimumHessian);
 		ULOGGER_DEBUG("filter keypoints time = %f s", timer.ticks());
 	}
 	else
@@ -730,9 +733,14 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 		feature2D = new ORBOctree(parameters);
 		break;
 #ifdef RTABMAP_TORCH
-	case Feature2D::kFeatureSuperPointTorch:
-		feature2D = new SuperPointTorch(parameters);
-		break;
+case Feature2D::kFeatureSuperPointTorch:
+    feature2D = new SuperPointTorch(parameters);
+    break;
+#endif
+#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
+case Feature2D::kFeatureSuperPointRpautrat:
+    feature2D = new SuperPointRpautrat(parameters);
+    break;
 #endif
 	case Feature2D::kFeatureSurfFreak:
 		feature2D = new SURF_FREAK(parameters);
@@ -831,7 +839,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 			cv::Rect roi(globalRoi.x + j*colSize, globalRoi.y + i*rowSize, colSize, rowSize);
 			std::vector<cv::KeyPoint> subKeypoints;
 			subKeypoints = this->generateKeypointsImpl(image, roi, mask);
-			if (this->getType() != Feature2D::Type::kFeaturePyDetector)
+			if (this->getType() != Feature2D::Type::kFeaturePyDetector && this->getType() != Feature2D::Type::kFeatureSuperPointRpautrat)
 			{
 				limitKeypoints(subKeypoints, maxFeatures, roi.size(), this->getSSC());
 			}
@@ -1243,7 +1251,8 @@ SIFT::SIFT(const ParametersMap & parameters) :
 	preciseUpscale_(Parameters::defaultSIFTPreciseUpscale()),
 	rootSIFT_(Parameters::defaultSIFTRootSIFT()),
 	gpu_(Parameters::defaultSIFTGpu()),
-	guaussianThreshold_(Parameters::defaultSIFTGaussianThreshold()),
+	gaussianThreshold_(Parameters::defaultSIFTGaussianThreshold()),
+	maxGaussianThreshold_(Parameters::defaultSIFTMaxGaussianThreshold()),
 	upscale_(Parameters::defaultSIFTUpscale()),
 	cudaSiftData_(0),
 	cudaSiftMemory_(0),
@@ -1276,23 +1285,25 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kSIFTPreciseUpscale(), preciseUpscale_);
 	Parameters::parse(parameters, Parameters::kSIFTRootSIFT(), rootSIFT_);
 	Parameters::parse(parameters, Parameters::kSIFTGpu(), gpu_);
-	Parameters::parse(parameters, Parameters::kSIFTGaussianThreshold(), guaussianThreshold_);
+	Parameters::parse(parameters, Parameters::kSIFTGaussianThreshold(), gaussianThreshold_);
+	Parameters::parse(parameters, Parameters::kSIFTMaxGaussianThreshold(), maxGaussianThreshold_);
 	Parameters::parse(parameters, Parameters::kSIFTUpscale(), upscale_);
 	
 	if(gpu_)
 	{
 #ifdef RTABMAP_CUDASIFT
 		// Check if there is a cuda device
-		if(InitCuda(0, ULogger::level() == ULogger::kDebug)) {
-			UDEBUG("Init SiftData");
-			if(cudaSiftData_ == 0) {
+		if(cudaSiftData_==0)
+		{
+			if(InitCuda(0, ULogger::level() == ULogger::kDebug)) {
+				UDEBUG("Init SiftData");
 				cudaSiftData_ = new SiftData();
 				InitSiftData(*cudaSiftData_, 8192, true, true);
 			}
-		}
-		else{
-			UWARN("No cuda device(s) detected, CudaSift is not available! Using SIFT CPU version instead.");
-			gpu_ = false;
+			else{
+				UWARN("No cuda device(s) detected, CudaSift is not available! Using SIFT CPU version instead.");
+				gpu_ = false;
+			}
 		}
 #else
 		UWARN("RTAB-Map is not built with CudaSift so %s cannot be used!", Parameters::kSIFTGpu().c_str());
@@ -1355,7 +1366,7 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 			numOctaves = 7; // hard-coded limit in CudaSift
 		}
 		float initBlur = sigma_; /* Amount of initial Gaussian blurring in standard deviations */
-		float thresh = guaussianThreshold_;   /* Threshold on difference of Gaussians for feature pruning */
+		float thresh = gaussianThreshold_;   /* Threshold on difference of Gaussians for feature pruning */
 		float edgeLimit = edgeThreshold_;   
 		float minScale = 0.0f; /* Minimum acceptable scale to remove fine-scale features */
 		UDEBUG("numOctaves=%d initBlur=%f thresh=%f edgeLimit=%f minScale=%f upScale=%s w=%d h=%d", numOctaves, initBlur, thresh, edgeLimit, minScale, upscale_?"true":"false", w, h);
@@ -1380,15 +1391,9 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 		cudaSiftDescriptors_ = cv::Mat();
 		if(cudaSiftData_->numPts)
 		{
-			int maxKeypoints = this->getMaxFeatures();
-			if(maxKeypoints == 0 || maxKeypoints > cudaSiftData_->numPts)
-			{
-				maxKeypoints = cudaSiftData_->numPts;
-			}
-
-			// Re-using same implementation of limitKeypoints() directly here to avoid doubling memory copies
-			// Sort words by hessian
-			std::multimap<float, int> hessianMap; // <hessian,id>
+			keypoints.resize(cudaSiftData_->numPts);
+			cudaSiftDescriptors_ = cv::Mat(cudaSiftData_->numPts, 128, CV_32FC1);
+			size_t k=0;
 			for(int i=0; i<cudaSiftData_->numPts; ++i)
 			{
 				// Ignore keypoints with invalid descriptors
@@ -1405,29 +1410,40 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 					continue;
 				}
 
-				//Keep track of the data, to be easier to manage the data in the next step
-				hessianMap.insert(std::pair<float, int>(cudaSiftData_->h_data[i].sharpness, i));
-			}
+				if(i>0 && 
+					cudaSiftData_->h_data[i].subsampling == cudaSiftData_->h_data[i-1].subsampling &&
+					fabs(cudaSiftData_->h_data[i].xpos-cudaSiftData_->h_data[i-1].xpos) +
+					fabs(cudaSiftData_->h_data[i].xpos-cudaSiftData_->h_data[i-1].ypos) < 0.1f)
+				{
+					// Same feature, skip doubles
+					continue;
+				}
 
-			if((int)hessianMap.size() < maxKeypoints)
-			{
-				maxKeypoints = hessianMap.size();
-			}
+				float response = abs(cudaSiftData_->h_data[i].sharpness);
+				if(maxGaussianThreshold_>gaussianThreshold_ && response > maxGaussianThreshold_)
+				{
+					continue;
+				}
 
-			std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
-			keypoints.resize(maxKeypoints);
-			cudaSiftDescriptors_ = cv::Mat(maxKeypoints, 128, CV_32FC1);
-			for(unsigned int k=0; k<keypoints.size() && iter!=hessianMap.rend(); ++k, ++iter)
-			{
-				int i = iter->second;
-				float *desc = cudaSiftData_->h_data[i].data;
 				cv::Mat(1, 128, CV_32FC1, desc).copyTo(cudaSiftDescriptors_.row(k));
 				keypoints[k].pt.x = cudaSiftData_->h_data[i].xpos;
 				keypoints[k].pt.y = cudaSiftData_->h_data[i].ypos;
 				keypoints[k].size = 2.0f*cudaSiftData_->h_data[i].scale; // x2 because the scale is more like a radius than a diameter, see CudaSift's ExtractSiftDescriptors function to see how they convert scale to patch size
 				keypoints[k].angle = cudaSiftData_->h_data[i].orientation;
-				keypoints[k].response = cudaSiftData_->h_data[i].sharpness; 
+				keypoints[k].response = response; 
 				keypoints[k].octave = log2(cudaSiftData_->h_data[i].subsampling)-(upscale_?1:0);
+				++k;
+			}
+			if(k < keypoints.size())
+			{
+				UDEBUG("keypoints extracted = %d, valid=%d", keypoints.size(), k);
+				keypoints.resize(k);
+				cudaSiftDescriptors_.resize(k);
+			}
+			if(this->getMaxFeatures() != 0 && this->getMaxFeatures() < (int)keypoints.size())
+			{
+				// Call limitKeypoints() now to filter the descriptors.
+				this->limitKeypoints(keypoints, cudaSiftDescriptors_, this->getMaxFeatures(), cv::Size(w,h), this->getSSC());
 			}
 		}
 	}
@@ -1449,12 +1465,13 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 
 cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
+	cv::Mat descriptors;
 #ifdef RTABMAP_CUDASIFT
 	if(gpu_)
 	{
 		if((int)keypoints.size() == cudaSiftDescriptors_.rows)
 		{
-			return cudaSiftDescriptors_.clone();
+			descriptors = cudaSiftDescriptors_.clone();
 		}
 		else
 		{
@@ -1462,19 +1479,25 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 			return cv::Mat();
 		}
 	}
+	else
+	{
 #endif
 
-	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
-	cv::Mat descriptors;
+		UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 #ifdef RTABMAP_NONFREE
-	sift_->compute(image, keypoints, descriptors);
+		sift_->compute(image, keypoints, descriptors);
 #else
-	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
+		UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
 #else // >=4.4, >=3.4.11
-	sift_->compute(image, keypoints, descriptors);
+		sift_->compute(image, keypoints, descriptors);
 #endif
+
+#ifdef RTABMAP_CUDASIFT
+	}
+#endif
+
 	if( rootSIFT_ && !descriptors.empty())
 	{
 		UDEBUG("Performing RootSIFT...");
@@ -2615,6 +2638,108 @@ cv::Mat SuperPointTorch::generateDescriptorsImpl(const cv::Mat & image, std::vec
 }
 
 
+//////////////////////////
+//SuperPointRpautrat
+//////////////////////////
+SuperPointRpautrat::SuperPointRpautrat(const ParametersMap & parameters) :
+		superpointWeightsPath_(Parameters::defaultSuperPointRpautratWeightsPath()),
+		superpointModelPath_(Parameters::defaultSuperPointRpautratModelPath()),
+		outputDir_(""),
+		threshold_(Parameters::defaultSuperPointRpautratThreshold()),
+		nms_(Parameters::defaultSuperPointRpautratNMS()),
+		minDistance_(Parameters::defaultSuperPointRpautratNMSRadius()),
+		cuda_(Parameters::defaultSuperPointRpautratCuda())
+{
+	parseParameters(parameters);
+}
+
+SuperPointRpautrat::~SuperPointRpautrat()
+{
+}
+
+void SuperPointRpautrat::parseParameters(const ParametersMap & parameters)
+{
+	Feature2D::parseParameters(parameters);
+
+#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
+	std::string previousWeightsPath = superpointWeightsPath_;
+	std::string previousModelPath = superpointModelPath_;
+	bool previousCuda = cuda_;
+	float previousThreshold = threshold_;
+	bool previousNms = nms_;
+	int previousMinDistance = minDistance_;
+	
+	Parameters::parse(parameters, Parameters::kSuperPointRpautratWeightsPath(), superpointWeightsPath_);
+	Parameters::parse(parameters, Parameters::kSuperPointRpautratModelPath(), superpointModelPath_);
+	Parameters::parse(parameters, Parameters::kSuperPointRpautratThreshold(), threshold_);
+	Parameters::parse(parameters, Parameters::kSuperPointRpautratNMS(), nms_);
+	Parameters::parse(parameters, Parameters::kSuperPointRpautratNMSRadius(), minDistance_);
+	Parameters::parse(parameters, Parameters::kSuperPointRpautratCuda(), cuda_);
+	Parameters::parse(parameters, Parameters::kRtabmapWorkingDirectory(), outputDir_);
+
+	// If working directory is not set, use the default
+	if(outputDir_.empty())
+	{
+		outputDir_ = Parameters::createDefaultWorkingDirectory();
+	}
+
+	// Reinitialize detector if model-affecting parameters changed
+	if(superPoint_.get() == 0 || 
+	   superpointWeightsPath_.compare(previousWeightsPath) != 0 || 
+	   superpointModelPath_.compare(previousModelPath) != 0 ||
+	   previousCuda != cuda_ ||
+	   previousThreshold != threshold_ ||
+	   previousNms != nms_ ||
+	   previousMinDistance != minDistance_)
+	{
+		superPoint_ = cv::Ptr<SPDetectorRpautrat>(new SPDetectorRpautrat(superpointWeightsPath_, superpointModelPath_, outputDir_, threshold_, nms_, minDistance_, cuda_, this->getMaxFeatures(), this->getSSC()));
+	}
+	else if(superPoint_.get() != 0)
+	{
+		// Update post-processing parameters without reinitializing
+		superPoint_->setMaxFeatures(this->getMaxFeatures());
+		superPoint_->setSSC(this->getSSC());
+	}
+#else
+	UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!");
+#endif
+}
+
+std::vector<cv::KeyPoint> SuperPointRpautrat::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
+{
+#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	if(roi.x!=0 || roi.y !=0)
+	{
+		UERROR("SuperPoint Rpautrat: Not supporting ROI (%d,%d,%d,%d). Make sure %s, %s, %s, %s, %s, %s are all set to default values.",
+				roi.x, roi.y, roi.width, roi.height,
+				Parameters::kKpRoiRatios().c_str(),
+				Parameters::kVisRoiRatios().c_str(),
+				Parameters::kVisGridRows().c_str(),
+				Parameters::kVisGridCols().c_str(),
+				Parameters::kKpGridRows().c_str(),
+				Parameters::kKpGridCols().c_str());
+		return std::vector<cv::KeyPoint>();
+	}
+	return superPoint_->detect(image, mask);
+#else
+	UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!");
+	return std::vector<cv::KeyPoint>();
+#endif
+}
+
+cv::Mat SuperPointRpautrat::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	return superPoint_->compute(keypoints);
+#else
+	UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!");
+	return cv::Mat();
+#endif
+}
+
+
 //////////////////////////
 //GFTT-DAISY
 //////////////////////////

corelib/src/FlannIndex.cpp

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

corelib/src/GlobalMap.cpp

@@ -139,14 +139,25 @@ bool GlobalMap::update(const std::map<int, Transform> & poses)
 	std::list<std::pair<int, Transform> > orderedPoses;
 
 	// add old poses that were not in the current map (they were just retrieved from LTM)
+	int nodesNotAssembled = 0;
+	int nodesNotInCache = 0;
 	for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
 	{
 		if(!isNodeAssembled(iter->first))
 		{
-			UDEBUG("Pose %d not found in current added poses, it will be added to map", iter->first);
-			orderedPoses.push_back(*iter);
+			if(uContains(cache(), iter->first))
+			{
+				++nodesNotAssembled;
+				//UDEBUG("Pose %d not found in current added poses, it will be added to map", iter->first);
+				orderedPoses.push_back(*iter);
+			}
+			else
+			{
+				++nodesNotInCache;
+			}
 		}
 	}
+	UDEBUG("%d nodes will be assembled in the map and %d nodes won't (no local grids in cache for them)", nodesNotAssembled, nodesNotInCache);
 
 	// insert zero after
 	if(poses.find(0) != poses.end())

corelib/src/Graph.cpp

@@ -451,9 +451,12 @@ bool importPoses(
 					}
 					double stamp = uStr2Double(strList.front());
 					strList.pop_front();
-					if(format==11)
+					if(strList.size() == 8 && (format==10 || format==11 || format==12))
 					{
-						id = uStr2Int(strList.back());
+						if(format==11)
+						{
+							id = uStr2Int(strList.back());
+						}
 						strList.pop_back();
 					}
 					str = uJoin(strList, " ");
@@ -2017,19 +2020,21 @@ std::list<std::pair<int, Transform> > computePath(
 		bool lookInDatabase,
 		bool updateNewCosts,
 		float linearVelocity,  // m/sec
-		float angularVelocity) // rad/sec
+		float angularVelocity, // rad/sec
+		bool ignoreDirectLinks) 
 {
 	UASSERT(memory!=0);
 	UASSERT(fromId>=0);
 	UASSERT(toId!=0);
 	std::list<std::pair<int, Transform> > path;
-	UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f",
+	UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f ignoreDirectLinks=%d",
 			fromId,
 			toId,
 			lookInDatabase?1:0,
 			updateNewCosts?1:0,
 			linearVelocity,
-			angularVelocity);
+			angularVelocity,
+			ignoreDirectLinks?1:0);
 
 	std::multimap<int, Link> allLinks;
 	if(lookInDatabase)
@@ -2107,7 +2112,9 @@ std::list<std::pair<int, Transform> > computePath(
 		}
 		for(std::multimap<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
 		{
-			if(iter->second.from() != iter->second.to())
+			if(iter->second.from() != iter->second.to() &&
+			  (!ignoreDirectLinks || 
+				(!(iter->second.from()==fromId && iter->second.to()==toId) && !(iter->second.to()==fromId && iter->second.from()==toId))))
 			{
 				Transform nextPose = currentNode->pose()*iter->second.transform();
 				float cost = 0.0f;
@@ -2393,26 +2400,15 @@ std::map<int, Transform> getPosesInRadius(const Transform & targetPose, const st
 
 
 float computePathLength(
-		const std::vector<std::pair<int, Transform> > & path,
-		unsigned int fromIndex,
-		unsigned int toIndex)
+		const std::vector<std::pair<int, Transform> > & path)
 {
 	float length = 0.0f;
 	if(path.size() > 1)
 	{
-		UASSERT(fromIndex  < path.size() && toIndex < path.size() && fromIndex <= toIndex);
-		if(fromIndex >= toIndex)
+		for(unsigned int i=0; i<path.size()-1; ++i)
 		{
-			toIndex = (unsigned int)path.size()-1;
+			length+=path[i].second.getDistance(path[i+1].second);
 		}
-		float x=0, y=0, z=0;
-		for(unsigned int i=fromIndex; i<toIndex-1; ++i)
-		{
-			x += fabs(path[i].second.x() - path[i+1].second.x());
-			y += fabs(path[i].second.y() - path[i+1].second.y());
-			z += fabs(path[i].second.z() - path[i+1].second.z());
-		}
-		length = sqrt(x*x + y*y + z*z);
 	}
 	return length;
 }
@@ -2423,19 +2419,15 @@ float computePathLength(
 	float length = 0.0f;
 	if(path.size() > 1)
 	{
-		float x=0, y=0, z=0;
 		std::map<int, Transform>::const_iterator iter=path.begin();
 		Transform previousPose = iter->second;
 		++iter;
 		for(; iter!=path.end(); ++iter)
 		{
 			const Transform & currentPose = iter->second;
-			x += fabs(previousPose.x() - currentPose.x());
-			y += fabs(previousPose.y() - currentPose.y());
-			z += fabs(previousPose.z() - currentPose.z());
+			length+=previousPose.getDistance(currentPose);
 			previousPose = currentPose;
 		}
-		length = sqrt(x*x + y*y + z*z);
 	}
 	return length;
 }

corelib/src/IMUThread.cpp

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

corelib/src/Memory.cpp

@@ -83,6 +83,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_rgbCompressionFormat(Parameters::defaultMemImageCompressionFormat()),
 	_depthCompressionFormat(Parameters::defaultMemDepthCompressionFormat()),
 	_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
+	_localizationReadOnly(Parameters::defaultMemLocalizationReadOnly()),
 	_localizationDataSaved(Parameters::defaultMemLocalizationDataSaved()),
 	_flannIndexSaved(Parameters::defaultKpFlannIndexSaved()),
 	_reduceGraph(Parameters::defaultMemReduceGraph()),
@@ -185,10 +186,6 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 			_dbDriver = 0; // HACK for the clear() below to think that there is no db
 		}
 	}
-	else if(!_memoryChanged && _linksChanged)
-	{
-		_dbDriver->setTimestampUpdateEnabled(false); // update links only
-	}
 	this->clear();
 	if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
 
@@ -212,10 +209,10 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 	bool success = true;
 	if(_dbDriver)
 	{
-		_dbDriver->setTimestampUpdateEnabled(true); // make sure that timestamp update is enabled (may be disabled above)
+
 		success = false;
 		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database \"") + dbUrl + "\"..."));
-		if(_dbDriver->openConnection(dbUrl, dbOverwritten))
+		if(_dbDriver->openConnection(dbUrl, dbOverwritten, isReadOnly()))
 		{
 			success = true;
 			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database \"") + dbUrl + "\", done!"));
@@ -245,6 +242,7 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 
 		if(loadAllNodesInWM)
 		{
+			UDEBUG("Loading all nodes to WM...");
 			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM...")));
 			std::set<int> ids;
 			_dbDriver->getAllNodeIds(ids, true);
@@ -252,6 +250,7 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 		}
 		else
 		{
+			UDEBUG("Loading last nodes to WM...");
 			// load previous session working memory
 			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM...")));
 			_dbDriver->loadLastNodes(dbSignatures, !_loadVisualLocalFeaturesOnInit);
@@ -438,7 +437,8 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 		UTimer timer;
 		// Enable loaded signatures
 		const std::map<int, Signature *> & signatures = this->getSignatures();
-		for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
+		bool corruptedDictionary = false;
+		for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end() && !corruptedDictionary; ++i)
 		{
 			Signature * s = this->_getSignature(i->first);
 			UASSERT(s != 0);
@@ -451,12 +451,114 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 				{
 					if(iter->first > 0)
 					{
-						_vwd->addWordRef(iter->first, i->first);
+						if(!_vwd->addWordRef(iter->first, s->id()))
+						{
+							corruptedDictionary = true;
+							break;
+						}
+					}
+				}
+				s->setEnabled(!corruptedDictionary);
+				if(corruptedDictionary)
+				{
+					//revert all changes from that signature till it broke above
+					for(std::multimap<int, int>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
+					{
+						if(iter->first > 0)
+						{
+							_vwd->removeAllWordRef(iter->first, s->id());
+						}
 					}
 				}
-				s->setEnabled(true);
 			}
 		}
+		if(corruptedDictionary)
+		{
+			if(!_vwd->isIncremental())
+			{
+				UERROR("The dictionary is empty or missing some words from nodes in WM, "
+					"we cannot repair it because it is a fixed dictionary. Make sure you "
+					"are using the right fixed dictionary that was used to generate the map.");
+			}
+			else
+			{
+				std::string msg = uFormat(
+					"The dictionary is empty or missing some words from nodes in WM, "
+					"we will try to repair it. This can be caused by rtabmap closing before it has time "
+					"to save the dictionary. Re-creating the dictionary from %ld nodes...",
+					signatures.size());
+				UWARN("%s", msg.c_str());
+				if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(msg));
+
+				//remove all words ref
+
+				const std::map<int, VisualWord *> & addedWords = _vwd->getVisualWords();
+				int nodesRepaired = 0;
+				size_t oldSize = addedWords.size();
+				std::string assertMsg = 
+					"If we assert here, the problem is maybe deeper. Try "
+					"to use rtabmap-recovery tool instead to fix the database.";
+				for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
+				{
+					Signature * s = this->_getSignature(i->first);
+					UASSERT_MSG(s != 0, assertMsg.c_str());
+
+					if(s->isEnabled())
+					{
+						// Words already in dictionary and references added
+						continue;
+					}
+
+					const std::multimap<int, int> * words = &s->getWords();
+					if(words->size())
+					{
+						cv::Mat descriptors = s->getWordsDescriptors();
+						std::multimap<int, int> loadedWords;
+						if(descriptors.empty())
+						{
+							// We may have started rtabmap without loading features, check in the database
+							std::multimap<int, int> w;
+							std::vector<cv::KeyPoint> k;
+							std::vector<cv::Point3f> p;
+							_dbDriver->getLocalFeatures(s->id(), loadedWords, k, p, descriptors);
+							UASSERT_MSG(loadedWords.size() == words->size(), assertMsg.c_str()); // Just doublecheck
+							words = &loadedWords; // The index will be set
+							UASSERT_MSG(!descriptors.empty(), assertMsg.c_str());
+						}
+						bool repaired = false;
+						for(std::multimap<int, int>::const_iterator iter = words->begin(); iter!=words->end(); ++iter)
+						{
+							if(iter->first > 0)
+							{
+								if(addedWords.find(iter->first) == addedWords.end())
+								{
+									UASSERT_MSG(iter->second >= 0 && iter->second < descriptors.rows, 
+										uFormat("iter->second=%d descriptors.rows=%d (signature=%d word=%d). %s",
+										iter->second, descriptors.rows, s->id(), iter->first, assertMsg.c_str()).c_str());
+									_vwd->addWord(new VisualWord(iter->first, descriptors.row(iter->second).clone()));
+									repaired = true;
+								}
+								UASSERT_MSG(_vwd->addWordRef(iter->first, s->id()), assertMsg.c_str());
+							}
+						}
+						nodesRepaired += (repaired?1:0);
+						s->setEnabled(true);
+					}
+				}
+
+				msg = uFormat(
+					"Regenerated the dictionary with %ld missing words (%ld -> %ld) from %d nodes.",
+					addedWords.size() - oldSize,
+					oldSize,
+					addedWords.size(),
+					nodesRepaired);
+				UWARN("%s", msg.c_str());
+				if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(msg));
+				_memoryChanged = true; // This will force rtabmap to save back the dictionary even if we don't process any new data
+				_vwd->update();
+			}
+		}
+
 		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Adding word references, done! (%d)", _vwd->getTotalActiveReferences())));
 
 		if(_vwd->getUnusedWordsSize() && _vwd->isIncremental())
@@ -531,16 +633,23 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::st
 
 	UDEBUG("_memoryChanged=%d _linksChanged=%d databaseNameChanged=%d", _memoryChanged?1:0, _linksChanged?1:0, databaseNameChanged?1:0);
 
-	if(!databaseSaved || (!_memoryChanged && !_linksChanged && !databaseNameChanged))
+	if(!databaseSaved || (!_memoryChanged && !_linksChanged && !databaseNameChanged) || this->isReadOnly())
 	{
 		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("No changes added to database.")));
 
 		UINFO("No changes added to database.");
 		if(_dbDriver)
 		{
-			saveFlannIndex(postInitClosingEvents);
+			if(!this->isReadOnly()) {
+				saveFlannIndex(postInitClosingEvents);
+			}
+			else if(_memoryChanged || _linksChanged || databaseNameChanged)
+			{
+				UWARN("Memory has been modified (nodes=%s links=%s name=%s) but the database is read-only, changes are not saved to database.",
+					_memoryChanged?"true":"false", _linksChanged?"true":"false", databaseNameChanged?"true":"false");
+			}
 			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
-			_dbDriver->closeConnection(false, ouputDatabasePath);
+			_dbDriver->closeConnection(false);
 			delete _dbDriver;
 			_dbDriver = 0;
 			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
@@ -556,12 +665,6 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::st
 		if(!_memoryChanged && _dbDriver)
 		{
 			saveFlannIndex(postInitClosingEvents);
-
-			if(_linksChanged) {
-				// don't update the time stamps!
-				UDEBUG("");
-				_dbDriver->setTimestampUpdateEnabled(false);
-			}
 		}
 		this->clear();
 		if(_dbDriver)
@@ -663,6 +766,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMarkerVarianceOrientationIgnored(), _markerOrientationIgnored);
 	Parameters::parse(params, Parameters::kMemLocalizationDataSaved(), _localizationDataSaved);
 	Parameters::parse(params, Parameters::kKpFlannIndexSaved(), _flannIndexSaved);
+	Parameters::parse(params, Parameters::kMemLocalizationReadOnly(), _localizationReadOnly);
 
 	if(_markerAngVariance>=9999)
 	{
@@ -1184,114 +1288,180 @@ void Memory::addSignatureToWmFromLTM(Signature * signature)
 	}
 }
 
-void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
+bool Memory::canBeReduced(const Link & link, float maxDistance, int direction)
 {
-	UDEBUG("Inserting node %d from STM in WM...", id);
-	UASSERT(_stMem.find(id) != _stMem.end());
+	return  link.to() != link.from() &&
+			link.type() != Link::kNeighbor &&
+			link.type() != Link::kNeighborMerged &&
+			link.userDataCompressed().empty() &&
+			link.type() != Link::kUndef &&
+			link.type() != Link::kVirtualClosure &&
+			(maxDistance == 0.0f || link.transform().getNorm() < maxDistance) &&
+			(direction == 0 || (direction==-1 && link.to() < link.from()) || (direction==1 && link.to() > link.from()));
+}
+
+int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb, int direction)
+{
+	UDEBUG("Reducing %d (max distance=%f, keep linked in db=%s, direction=%d)",
+		id, maxDistance, keepLinkedInDb?"true":"false", direction);
 	Signature * s = this->_getSignature(id);
-	UASSERT(s!=0);
-
-	if(_reduceGraph)
+	if(s==0)
 	{
-		bool merge = false;
-		const std::multimap<int, Link> & links = s->getLinks();
-		std::map<int, Link> neighbors;
-		for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
-		{
-			if(!merge)
-			{
-				merge = iter->second.to() < s->id() && // should be a parent->child link
-						iter->second.to() != iter->second.from() &&
-						iter->second.type() != Link::kNeighbor &&
-						iter->second.type() != Link::kNeighborMerged &&
-						iter->second.userDataCompressed().empty() &&
-						iter->second.type() != Link::kUndef &&
-						iter->second.type() != Link::kVirtualClosure;
-				if(merge)
-				{
-					UDEBUG("Reduce %d to %d", s->id(), iter->second.to());
-					if(reducedTo)
-					{
-						*reducedTo = iter->second.to();
-					}
-				}
+		UWARN("Node %d is not in WM/STM, cannot reduce it.", id);
+		return 0;
+	}
 
-			}
-			if(iter->second.type() == Link::kNeighbor)
+	if(!s->getLabel().empty())
+	{
+		// We currently not remove nodes with labels
+		return 0;
+	}
+
+	std::multimap<int, Link> links = s->getLinks();
+	std::map<int, Link> neighbors;
+	int reducedTo = 0;
+	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		if(canBeReduced(iter->second, maxDistance, direction))
+		{
+			float distance = iter->second.transform().getNorm();
+			reducedTo = iter->second.to();
+			UDEBUG("Reduce %d to %d (distance=%f)",
+				s->id(), iter->second.to(), distance);
+		}
+		
+		if(iter->second.type() == Link::kNeighbor)
+		{
+			neighbors.insert(*iter);
+		}
+	}
+	if(reducedTo>0)
+	{
+		if(maxDistance > 0.0f)
+		{
+			// Only reduce if all neighbor merged links are also below maxDistance
+			for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 			{
-				neighbors.insert(*iter);
+				if( iter->second.type() == Link::kNeighborMerged &&
+					iter->second.transform().getNorm() > maxDistance)
+				{
+					return 0;
+				}
 			}
 		}
-		if(merge)
+
+		for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 		{
-			if(s->getLabel().empty())
+			Signature * sTo = this->_getSignature(iter->first);
+			if(sTo->id()!=s->id()) // Not Prior/Gravity links...
 			{
-				for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+				UASSERT_MSG(sTo!=0, uFormat("id=%d", iter->first).c_str());
+				sTo->removeLink(s->id());
+				if(iter->second.type() != Link::kNeighbor &&
+					iter->second.type() != Link::kUndef)
 				{
-					Signature * sTo = this->_getSignature(iter->first);
-					if(sTo->id()!=s->id()) // Not Prior/Gravity links...
+					if(iter->second.type() == Link::kNeighborMerged)
 					{
-						UASSERT_MSG(sTo!=0, uFormat("id=%d", iter->first).c_str());
-						sTo->removeLink(s->id());
-						if(iter->second.type() != Link::kNeighbor &&
-						   iter->second.type() != Link::kNeighborMerged &&
-						   iter->second.type() != Link::kUndef)
+						s->removeLink(sTo->id());
+						if(maxDistance == 0.0f)
 						{
-							// link to all neighbors
-							for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
+							// online graph reduction, always skip these links
+							continue;
+						}
+					}
+					// link to all neighbors
+					for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
+					{
+						if(!sTo->hasLink(jter->second.to()))
+						{
+							Link l = iter->second.inverse().merge(
+									jter->second,
+									iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type());
+							UDEBUG("Merging link %d->%d (type=%d) to with %d->%d (type %d). Adding %d->%d (type %d) to %d and %d",
+								iter->second.to(), iter->second.from(), iter->second.type(),
+								jter->second.from(), jter->second.to(), jter->second.type(),
+								l.from(), l.to(), l.type(), sTo->id(), l.to());
+							sTo->addLink(l);
+							Signature * sB = this->_getSignature(l.to());
+							UASSERT(sB!=0);
+							UASSERT_MSG(!sB->hasLink(l.from()), uFormat("%d->%d type=%d", sB->id(), l.to(), l.type()).c_str());
+							sB->addLink(l.inverse());
+						}
+					}
+					// link to all landmarks
+					for(std::map<int, Link>::const_iterator jter=s->getLandmarks().begin(); jter!=s->getLandmarks().end(); ++jter)
+					{
+						if(!uContains(sTo->getLandmarks(), jter->first))
+						{
+							UDEBUG("Move landmark observation %d from %d to %d",
+									jter->first, s->id(), sTo->id());
+							Link l = iter->second.inverse().merge(
+									jter->second,
+									jter->second.type());
+							sTo->addLandmark(l);
+							// Update landmark index
+							std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(jter->first);
+							if(nter!=_landmarksIndex.end())
 							{
-								if(!sTo->hasLink(jter->second.to()))
-								{
-									UDEBUG("Merging link %d->%d (type=%d) to link %d->%d (type %d)",
-											iter->second.from(), iter->second.to(), iter->second.type(),
-											jter->second.from(), jter->second.to(), jter->second.type());
-									Link l = iter->second.inverse().merge(
-											jter->second,
-											iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type());
-									sTo->addLink(l);
-									Signature * sB = this->_getSignature(l.to());
-									UASSERT(sB!=0);
-									UASSERT_MSG(!sB->hasLink(l.from()), uFormat("%d->%d", sB->id(), l.to()).c_str());
-									sB->addLink(l.inverse());
-								}
+								nter->second.insert(sTo->id());
+							}
+							else
+							{
+								std::set<int> tmp;
+								tmp.insert(sTo->id());
+								_landmarksIndex.insert(std::make_pair(jter->first, tmp));
 							}
 						}
 					}
 				}
+			}
+		}
 
-				//remove neighbor links
-				std::multimap<int, Link> linksCopy = links;
-				for(std::multimap<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
+		this->moveToTrash(s, keepLinkedInDb);
+		s = 0;
+		_linksChanged = true;
+		_memoryChanged = true;
+	}
+	return reducedTo;
+}
+
+void Memory::moveSignatureToWMFromSTM(int id, int * reducedToOut)
+{
+	UDEBUG("Inserting node %d from STM in WM...", id);
+	UASSERT(_stMem.find(id) != _stMem.end());
+	int reducedId = 0;
+	if(_reduceGraph)
+	{
+		Signature * s = this->_getSignature(id);
+		UASSERT(s!=0);
+		std::multimap<int, Link> links = s->getLinks();
+		// Setting true to make sure we save all visual
+		// words that could be referenced in a previously
+		// transferred node in LTM (#979)
+		reducedId = reduceNode(s->id(), 0, true);
+		if(reducedToOut) {
+			*reducedToOut = reducedId;
+		}
+		if(reducedId>0)
+		{
+			for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
+			{
+				if(iter->second.type() == Link::kNeighbor)
 				{
-					if(iter->second.type() == Link::kNeighborMerged)
+					if(_lastGlobalLoopClosureId == s->id())
 					{
-						// Removing only merged neighbor links, we keep original neighbor 
-						// links to be able to reprocess databases with correct odometry covariance.
-						s->removeLink(iter->first);
-					}
-					if(iter->second.type() == Link::kNeighbor)
-					{
-						if(_lastGlobalLoopClosureId == s->id())
-						{
-							_lastGlobalLoopClosureId = iter->first;
-						}
+						_lastGlobalLoopClosureId = iter->first;
 					}
 				}
-
-				// Setting true to make sure we save all visual
-				// words that could be referenced in a previously
-				// transferred node in LTM (#979)
-				this->moveToTrash(s, true);
-				s = 0;
 			}
 		}
 	}
-	if(s != 0)
+	if(reducedId == 0)
 	{
 		_workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now()));
 		_stMem.erase(*_stMem.begin());
 	}
-	// else already removed from STM/WM in moveToTrash()
+	// else already removed from STM/WM in reduceNode()
 }
 
 const Signature * Memory::getSignature(int id) const
@@ -1865,6 +2035,7 @@ void Memory::clear()
 			uInsert(parameters, parameters_);
 			parameters.erase(Parameters::kRtabmapWorkingDirectory()); // don't save working directory as it is machine dependent
 			UDEBUG("");
+			_dbDriver->setTimestampUpdateEnabled(true); // Only re-stamp if we updated the memory
 			_dbDriver->addInfoAfterRun(memSize,
 					_lastSignature?_lastSignature->id():0,
 					UProcessInfo::getMemoryUsage(),
@@ -1938,6 +2109,7 @@ void Memory::clear()
 		_dbDriver->join(true);
 		cleanUnusedWords();
 		_dbDriver->emptyTrashes();
+		_dbDriver->setTimestampUpdateEnabled(false);
 	}
 	_vwd->clear(_dbDriver!=NULL);
 	UDEBUG("");
@@ -2504,9 +2676,10 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 		// If not saved to database
 		if(!keepLinkedToGraph)
 		{
-			UASSERT_MSG(this->isInSTM(s->id()),
+			UASSERT_MSG(this->isInSTM(s->id()) || this->isInWM(s->id()),
 						uFormat("Deleting location (%d) outside the "
-								"STM is not implemented!", s->id()).c_str());
+								"WM/STM is not implemented! STM size=%ld WM size=%ld",
+								s->id(), this->getStMem().size(), this->getWorkingMem().size()).c_str());
 			const std::multimap<int, Link> & links = s->getLinks();
 			for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 			{
@@ -2517,7 +2690,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 					UASSERT_MSG(sTo!=0,
 								uFormat("A neighbor (%d) of the deleted location %d is "
 										"not found in WM/STM! Are you deleting a location "
-										"outside the STM?", iter->first, s->id()).c_str());
+										"outside the WM/STM?", iter->first, s->id()).c_str());
 
 					if(iter->first > s->id() && links.size()>1 && sTo->hasLink(s->id()))
 					{
@@ -2527,7 +2700,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 					}
 
 					// child
-					if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id() && s->getWeight()>0)
+					if(iter->second.type() == Link::kGlobalClosure && s->getWeight()>0)
 					{
 						sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight
 					}
@@ -3626,7 +3799,7 @@ void Memory::updateLink(const Link & link, bool updateInDatabase)
 
 			if(oldType!=Link::kVirtualClosure || link.type()!=Link::kVirtualClosure)
 			{
-				_linksChanged = true;
+				_linksChanged = _incrementalMemory || (fromS->isSaved() && toS->isSaved());
 			}
 		}
 		else
@@ -4276,6 +4449,29 @@ void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
 		words3 = s->getWords3();
 		wordsDescriptors = s->getWordsDescriptors();
 		globalDescriptors = s->sensorData().globalDescriptors();
+
+		if(!words.empty() && wordsKpts.empty() && _dbDriver)
+		{
+			std::multimap<int, int> tmpWords;
+			_dbDriver->getLocalFeatures(nodeId, tmpWords, wordsKpts, words3, wordsDescriptors);
+			if(!tmpWords.empty() && !wordsKpts.empty())
+			{
+				UASSERT(tmpWords.size() == words.size());
+				std::map<int, int> wordsChanged = s->getWordsChanged();
+				for(const auto & iter: wordsChanged) {
+					std::list<int> subwords = uValues(tmpWords, iter.first); // old id
+					if(subwords.size())
+					{
+						tmpWords.erase(iter.first);
+						for(std::list<int>::const_iterator jter=subwords.begin(); jter!=subwords.end(); ++jter)
+						{
+							tmpWords.insert(std::pair<int, int>(iter.second, (*jter))); // new id
+						}
+					}
+				}
+				words = tmpWords;
+			}
+		}
 	}
 	else if(_dbDriver)
 	{
@@ -4303,11 +4499,6 @@ void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
 			}
 		}
 	}
-	if(!words.empty() && wordsKpts.empty() && _dbDriver)
-	{
-		std::multimap<int, int> tmpWords;
-		_dbDriver->getLocalFeatures(nodeId, tmpWords, wordsKpts, words3, wordsDescriptors);
-	}
 }
 
 void Memory::getNodeCalibration(int nodeId,
@@ -5044,16 +5235,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					{
 						UASSERT(!decimatedData.cameraModels().empty());
 						UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
-						if(_maskFloorThreshold<0.0f)
-						{
-							cv::Mat depthBelow;
-							util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
-							depthMask = depthBelow;
-						}
-						else
-						{
-							depthMask = util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold);
-						}
+						depthMask = util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold);
 						UDEBUG("Masking floor done.");
 					}
 
@@ -5103,6 +5285,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			else
 			{
 				int oldMaxFeatures = _feature2D->getMaxFeatures();
+				bool oldSSC = _feature2D->getSSC();
 				UDEBUG("rawDescriptorsKept=%d, pose=%d, maxFeatures=%d, visMaxFeatures=%d", _rawDescriptorsKept?1:0, pose.isNull()?0:1, _feature2D->getMaxFeatures(), _visMaxFeatures);
 				ParametersMap tmpMaxFeatureParameter;
 				if(_rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures)
@@ -5110,6 +5293,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					// The total extracted features should match the number of features used for transformation estimation
 					UDEBUG("Changing temporary max features from %d to %d", _feature2D->getMaxFeatures(), _visMaxFeatures);
 					tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(_visMaxFeatures)));
+					tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpSSC(), uNumber2Str(_visSSC)));
 					_feature2D->parseParameters(tmpMaxFeatureParameter);
 				}
 
@@ -5120,6 +5304,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				if(tmpMaxFeatureParameter.size())
 				{
 					tmpMaxFeatureParameter.at(Parameters::kKpMaxFeatures()) = uNumber2Str(oldMaxFeatures);
+					tmpMaxFeatureParameter.at(Parameters::kKpSSC()) = uBool2Str(oldSSC);
 					_feature2D->parseParameters(tmpMaxFeatureParameter); // reset back
 				}
 				t = timer.ticks();
@@ -5320,8 +5505,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		bool ssc = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visSSC:_feature2D->getSSC();
 		if((int)keypoints.size() > maxFeatures)
 		{
-			if(data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)
-				_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures, data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(), ssc);
+			if(data.cameraModels().size()>=1 || data.stereoCameraModels().size()>=1)
+				_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures, data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(), data.cameraModels()[0].imageHeight()):cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(), data.stereoCameraModels()[0].left().imageHeight()), ssc);
 			else
 				_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
 		}
@@ -5554,13 +5739,17 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					UWARN("Ignored %s and %s parameters as they cannot be used for multi-cameras setup or uncalibrated camera.",
 							Parameters::kKpGridCols().c_str(), Parameters::kKpGridRows().c_str());
 				}
-				if(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
-					data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)
+				if(decimatedData.cameraModels().size()>=1 || decimatedData.stereoCameraModels().size()>=1 ||
+					data.cameraModels().size()>=1 || data.stereoCameraModels().size()>=1)
 				{
-					Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(),
-						decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
-						decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
-						data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
+					Feature2D::limitKeypoints(
+						keypoints,
+						inliers,
+						_feature2D->getMaxFeatures(),
+						decimatedData.cameraModels().size()?cv::Size(decimatedData.cameraModels()[0].imageWidth()*decimatedData.cameraModels().size(), decimatedData.cameraModels()[0].imageHeight()):
+						decimatedData.stereoCameraModels().size()?cv::Size(decimatedData.stereoCameraModels()[0].left().imageWidth()*decimatedData.stereoCameraModels().size(), decimatedData.stereoCameraModels()[0].left().imageWidth()):
+						data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(), data.cameraModels()[0].imageHeight()):
+						cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(), data.stereoCameraModels()[0].left().imageHeight()),
 						_feature2D->getSSC());
 				}
 				else
@@ -5818,7 +6007,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		cameraModels.size() == 1 &&
 		words.size() &&
 		(words3D.size() == 0 || (words.size() == words3D.size() && words3DValid!=(int)words3D.size())) &&
-		_registrationPipeline->isImageRequired() &&
 		_signatures.size() &&
 		_signatures.rbegin()->second->mapId() == _idMapCount) // same map
 	{
@@ -5862,11 +6050,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 
 			// The following is used only to re-estimate the correspondences, the returned transform is ignored
 			Transform tmpt;
-			RegistrationVis reg(parameters_);
+			ParametersMap tmpParams = parameters_;
+			// Pure 2D-2D without guess would generate variance=1
+			uInsert(tmpParams, ParametersPair(Parameters::kVisEpipolarGeometryVar(), "1"));
+			RegistrationVis reg(tmpParams);
 			if(_registrationPipeline->isScanRequired())
 			{
 				// If icp is used, remove it to just do visual registration
-				RegistrationVis vis(parameters_);
+				RegistrationVis vis(tmpParams);
 				tmpt = vis.computeTransformationMod(cpCurrent, cpPrevious, cameraTransform);
 			}
 			else
@@ -5888,11 +6079,18 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			{
 				previousWords.insert(std::make_pair(iter->first, cpPrevious.getWordsKpts()[iter->second]));
 			}
+			float reprojError = Parameters::defaultVisPnPReprojError();
+			int varianceMedianRatio = Parameters::defaultVisPnPVarianceMedianRatio();
+			Parameters::parse(parameters_, Parameters::kVisPnPReprojError(), reprojError);
+			Parameters::parse(parameters_, Parameters::kVisPnPVarianceMedianRatio(), varianceMedianRatio);
 			std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
 					currentWords,
 					previousWords,
 					cameraModels[0],
-					cameraTransform);
+					cameraTransform,
+					reprojError,
+					0.99f,
+					varianceMedianRatio);
 
 			UDEBUG("inliers=%d", (int)inliers.size());
 
@@ -6569,7 +6767,10 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
 			{
 				if(keys.at(i)>0)
 				{
-					_vwd->addWordRef(keys.at(i), (*j)->id());
+					if(_vwd->addWordRef(keys.at(i), (*j)->id()))
+					{
+						UERROR("Could not add word ref %d to node %d!?", keys.at(i), (*j)->id());
+					}
 				}
 			}
 			(*j)->setEnabled(true);

corelib/src/OdometryThread.cpp

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

corelib/src/Optimizer.cpp

@@ -185,6 +185,52 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame
 	return optimizer;
 }
 
+class LinkIdKey
+{
+public:
+LinkIdKey(int id, Link::Type type) :
+		id_(id),
+		type_(type) {}
+	bool operator<(const LinkIdKey & k) const
+	{
+		// landmark, sort by smallest to largest landmark id, after normal links
+		if(id_ < 0 && k.id_ < 0)
+		{
+			return id_ > k.id_;
+		}
+		else if(id_ < 0) {
+			return false;
+		}
+		else if(k.id_ < 0) {
+			return true;
+		}
+
+		if(type_ == Link::kNeighbor && k.type_ != Link::kNeighbor)
+		{
+			return true;
+		}
+		else if(type_ != Link::kNeighbor && k.type_ == Link::kNeighbor)
+		{
+			return false;
+		}
+		else if(type_ == Link::kNeighborMerged && k.type_ != Link::kNeighbor && k.type_ != Link::kNeighborMerged)
+		{
+			return true;
+		}
+		else if(k.type_ == Link::kNeighborMerged && type_ != Link::kNeighbor && type_ != Link::kNeighborMerged)
+		{
+			return false;
+		}
+		else
+		{
+			// normal link, sort by smallest to largest id
+			return id_ < k.id_;
+		}
+	}
+	int id_;
+	Link::Type type_;
+};
+
 void Optimizer::getConnectedGraph(
 		int fromId,
 		const std::map<int, Transform> & posesIn,
@@ -199,8 +245,8 @@ void Optimizer::getConnectedGraph(
 	posesOut.clear();
 	linksOut.clear();
 
-	std::set<int> nextPoses;
-	nextPoses.insert(fromId);
+	std::map<LinkIdKey, Transform> nextPoses;
+	nextPoses.insert(std::make_pair(LinkIdKey(fromId, Link::kUndef), posesIn.find(fromId)->second));
 	std::multimap<int, std::pair<int, Link::Type> > biLinks;
 	for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
 	{
@@ -214,22 +260,27 @@ void Optimizer::getConnectedGraph(
 		}
 	}
 
-	while(nextPoses.size())
+	while(!nextPoses.empty())
 	{
-		int currentId = *nextPoses.rbegin(); // fill up all nodes before landmarks
-		nextPoses.erase(*nextPoses.rbegin());
+		// Fill up all nodes before landmarks
+		// For nodes, fill up all neightbor nodes before loop closure ones
+		int currentId = nextPoses.begin()->first.id_; 
+		Transform currentPose = nextPoses.begin()->second;
+		nextPoses.erase(nextPoses.begin());
 
-		if(posesOut.empty())
+		if(posesOut.find(currentId) != posesOut.end()) {
+			// Already added from priority list
+			continue;
+		}
+
+		posesOut.insert(std::make_pair(currentId, currentPose));
+
+		// add prior links
+		for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
 		{
-			posesOut.insert(std::make_pair(currentId, posesIn.find(currentId)->second));
-
-			// add prior links
-			for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
+			if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
 			{
-				if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
-				{
-					linksOut.insert(*pter);
-				}
+				linksOut.insert(*pter);
 			}
 		}
 
@@ -240,52 +291,42 @@ void Optimizer::getConnectedGraph(
 			if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
 			{
 				std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId, true, type);
-				if(nextPoses.find(toId) == nextPoses.end())
+				UASSERT(kter!=linksIn.end());
+				if(!uContains(posesOut, toId))
 				{
-					if(!uContains(posesOut, toId))
+					const Transform & poseToIn = posesIn.at(toId);
+					Transform t = kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse();
+					Transform pose;
+					if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0 && (poseToIn.is3DoF() || poseToIn.is4DoF()))
 					{
-						const Transform & poseToIn = posesIn.at(toId);
-						Transform t = kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse();
-						if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0 && (poseToIn.is3DoF() || poseToIn.is4DoF()))
+						if(poseToIn.is3DoF())
 						{
-							if(poseToIn.is3DoF())
-							{
-								posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
-							}
-							else
-							{
-								posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to4DoF()));
-							}
+							pose = (posesOut.at(currentId) * t).to3DoF();
 						}
 						else
 						{
-							posesOut.insert(std::make_pair(toId, posesOut.at(currentId)* t));
+							pose = (posesOut.at(currentId) * t).to4DoF();
 						}
-
-						// add prior links
-						for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
-						{
-							if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
-							{
-								linksOut.insert(*pter);
-							}
-						}
-
-						nextPoses.insert(toId);
+					}
+					else
+					{
+						pose = posesOut.at(currentId)* t;
 					}
 
-					// only add unique links
-					if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
+					nextPoses.insert(std::make_pair(LinkIdKey(toId, type), pose));
+				}
+
+				// only add unique links
+				if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
+				{
+					if(kter->second.to() < 0)
 					{
-						if(kter->second.to() < 0)
-						{
-							// For landmarks, make sure fromId is the landmark
-							linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse()));
-						}
-						else
-						{
-							linksOut.insert(*kter);
-						}
+						// For landmarks, make sure fromId is the landmark
+						linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse()));
+					}
+					else
+					{
+						linksOut.insert(*kter);
 					}
 				}
 			}

corelib/src/Parameters.cpp

@@ -168,6 +168,7 @@ bool Parameters::isFeatureParameter(const std::string & parameter)
 			group.compare("BRISK") == 0 ||
 			group.compare("KAZE") == 0 ||
 			group.compare("SuperPoint") == 0 ||
+			group.compare("SuperPointRpautrat") == 0 ||
 			group.compare("PyDetector") == 0;
 }
 
@@ -182,6 +183,7 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, boo
 			(stereo && group.compare("Stereo") == 0) ||
 			(icp && group.compare("Icp") == 0) ||
 			(vis && Parameters::isFeatureParameter(iter->first)) ||
+			group.compare("OdomCuVSLAM") == 0 ||
 			group.compare("Reg") == 0 ||
 			group.compare("Optimizer") == 0 ||
 			group.compare("g2o") == 0 ||
@@ -661,6 +663,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 SuperPoint Rpautrat:"; 
+#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON) 
+				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 Python3:";
 #ifdef RTABMAP_PYTHON
@@ -1112,8 +1120,8 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 						ignore = true;
 					}
 #endif
-#ifndef RTABMAP_ORBSLAM2
-					if(group.compare("OdomORBSLAM2") == 0)
+#ifndef RTABMAP_ORB_SLAM
+					if(group.compare("OdomORBSLAM") == 0)
 					{
 						ignore = true;
 					}

corelib/src/RegistrationIcp.cpp

@@ -529,6 +529,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 				double toComplexity = util3d::computeNormalsComplexity(toScan, guess, &complexityVectorsTo, &complexityValuesTo);
 				float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
 				info.icpStructuralComplexity = complexity;
+				UDEBUG("structural complexity: from=%f to=%f", fromComplexity, toComplexity);
 				if(complexity < _pointToPlaneMinComplexity)
 				{
 					tooLowComplexityForPlaneToPlane = true;

corelib/src/RegistrationVis.cpp

@@ -84,6 +84,9 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_flowEps(Parameters::defaultVisCorFlowEps()),
 		_flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
 		_flowGpu(Parameters::defaultVisCorFlowGpu()),
+		_flowUseMinEigenVals(Parameters::defaultVisCorFlowUseMinEigenVals()),
+		_flowMinEigThreshold(Parameters::defaultVisCorFlowMinEigThreshold()),
+		_flowErrorThreshold(Parameters::defaultVisCorFlowErrorThreshold()),
 		_nndr(Parameters::defaultVisCorNNDR()),
 		_nnType(Parameters::defaultVisCorNNType()),
 		_gmsWithRotation(Parameters::defaultGMSWithRotation()),
@@ -145,6 +148,9 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisCorFlowEps(), _flowEps);
 	Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
 	Parameters::parse(parameters, Parameters::kVisCorFlowGpu(), _flowGpu);
+	Parameters::parse(parameters, Parameters::kVisCorFlowUseMinEigenVals(), _flowUseMinEigenVals);
+	Parameters::parse(parameters, Parameters::kVisCorFlowMinEigThreshold(), _flowMinEigThreshold);
+	Parameters::parse(parameters, Parameters::kVisCorFlowErrorThreshold(), _flowErrorThreshold);
 	Parameters::parse(parameters, Parameters::kVisCorNNDR(), _nndr);
 	Parameters::parse(parameters, Parameters::kVisCorNNType(), _nnType);
 	Parameters::parse(parameters, Parameters::kGMSWithRotation(), _gmsWithRotation);
@@ -286,6 +292,10 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	{
 		uInsert(_featureParameters, ParametersPair(Parameters::kKpGridCols(), parameters.at(Parameters::kVisGridCols())));
 	}
+	if(uContains(parameters, Parameters::kRtabmapWorkingDirectory()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kRtabmapWorkingDirectory(), parameters.at(Parameters::kRtabmapWorkingDirectory())));
+	}
 
 	delete _detectorFrom;
 	delete _detectorTo;
@@ -317,6 +327,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
 	UDEBUG("%s=%f", Parameters::kVisPnPMaxVariance().c_str(), _PnPMaxVar);
 	UDEBUG("%s=%f", Parameters::kVisPnPSplitLinearCovComponents().c_str(), _PnPSplitLinearCovarianceComponents);
+	UDEBUG("%s=%f", Parameters::kVisPnPVarianceMedianRatio().c_str(), _PnPVarMedianRatio);
 	UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
 	UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
 	UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
@@ -329,11 +340,12 @@ Transform RegistrationVis::computeTransformationImpl(
 	UDEBUG("Feature Detector = %d", (int)_detectorFrom->getType());
 	UDEBUG("guess=%s", guess.prettyPrint().c_str());
 
-	UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors,  %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
+	UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors, %d words kpts, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
 			fromSignature.id(),
 			(int)fromSignature.getWords().size(),
 			(int)fromSignature.getWords3().size(),
 			(int)fromSignature.getWordsDescriptors().rows,
+			(int)fromSignature.getWordsKpts().size(),
 			(int)fromSignature.sensorData().keypoints().size(),
 			(int)fromSignature.sensorData().keypoints3D().size(),
 			fromSignature.sensorData().descriptors().rows,
@@ -342,11 +354,12 @@ Transform RegistrationVis::computeTransformationImpl(
 			(int)fromSignature.sensorData().cameraModels().size(),
 			(int)fromSignature.sensorData().stereoCameraModels().size());
 
-	UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
+	UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d words kpts, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
 			toSignature.id(),
 			(int)toSignature.getWords().size(),
 			(int)toSignature.getWords3().size(),
 			(int)toSignature.getWordsDescriptors().rows,
+			(int)toSignature.getWordsKpts().size(),
 			(int)toSignature.sensorData().keypoints().size(),
 			(int)toSignature.sensorData().keypoints3D().size(),
 			toSignature.sensorData().descriptors().rows,
@@ -434,16 +447,7 @@ Transform RegistrationVis::computeTransformationImpl(
 							{
 								UASSERT(!fromSignature.sensorData().cameraModels().empty());
 								UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
-								if(_maskFloorThreshold<0.0f)
-								{
-									cv::Mat depthBelow;
-									util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
-									depthMask = depthBelow;
-								}
-								else
-								{
-									depthMask = util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold);
-								}
+								depthMask = util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold);
 								UDEBUG("Masking floor done.");
 							}
 
@@ -649,6 +653,7 @@ Transform RegistrationVis::computeTransformationImpl(
 				// Find features in the new left image
 				UDEBUG("guessSet = %d", guessSet?1:0);
 				std::vector<unsigned char> status;
+				std::vector<float> err;
 #ifdef HAVE_OPENCV_CUDAOPTFLOW
 				if (_flowGpu)
 				{
@@ -678,7 +683,6 @@ Transform RegistrationVis::computeTransformationImpl(
 				else
 #endif
 				{
-					std::vector<float> err;
 					UDEBUG("cv::calcOpticalFlowPyrLK() begin");
 					cv::calcOpticalFlowPyrLK(
 						imageFrom,
@@ -690,7 +694,8 @@ Transform RegistrationVis::computeTransformationImpl(
 						cv::Size(_flowWinSize, _flowWinSize),
 						guessSet ? 0 : _flowMaxLevel,
 						cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, _flowIterations, _flowEps),
-						cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet ? cv::OPTFLOW_USE_INITIAL_FLOW : 0), 1e-4);
+						(_flowUseMinEigenVals ? cv::OPTFLOW_LK_GET_MIN_EIGENVALS : 0) | (guessSet ? cv::OPTFLOW_USE_INITIAL_FLOW : 0),
+						_flowMinEigThreshold);
 					UDEBUG("cv::calcOpticalFlowPyrLK() end");
 				}
 
@@ -699,11 +704,14 @@ Transform RegistrationVis::computeTransformationImpl(
 				std::vector<cv::Point3f> kptsFrom3DKept(kptsFrom3D.size());
 				std::vector<int> orignalWordsFromIdsCpy = orignalWordsFromIds;
 				int ki = 0;
+				UASSERT((status.empty() || cornersTo.size() == status.size()) &&
+						(err.empty() || cornersTo.size() == err.size()));
 				for(unsigned int i=0; i<status.size(); ++i)
 				{
 					if(status[i] &&
 					   uIsInBounds(cornersTo[i].x, 0.0f, float(imageTo.cols)) &&
-					   uIsInBounds(cornersTo[i].y, 0.0f, float(imageTo.rows)))
+					   uIsInBounds(cornersTo[i].y, 0.0f, float(imageTo.rows)) &&
+					   (_flowUseMinEigenVals || err.empty() || err[i] < _flowErrorThreshold))
 					{
 						if(orignalWordsFromIdsCpy.size())
 						{
@@ -800,16 +808,7 @@ Transform RegistrationVis::computeTransformationImpl(
 							{
 								UASSERT(!toSignature.sensorData().cameraModels().empty());
 								UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
-								if(_maskFloorThreshold<0.0f)
-								{
-									cv::Mat depthBelow;
-									util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
-									depthMask = depthBelow;
-								}
-								else
-								{
-									depthMask = util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold);
-								}
+								depthMask = util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold);
 								UDEBUG("Masking floor done.");
 							}
 
@@ -1626,6 +1625,7 @@ Transform RegistrationVis::computeTransformationImpl(
 						cameraTransform,
 						_PnPReprojError,
 						0.99f,
+						_PnPVarMedianRatio,
 						words3A, // for scale estimation
 						&variance,
 						&matchesV);
@@ -1666,14 +1666,10 @@ Transform RegistrationVis::computeTransformationImpl(
 					UINFO(msg.c_str());
 				}
 			}
-			else if(fromSignature.getWords().size() == 0)
+			else 
 			{
-				msg = uFormat("No enough features (%d)", (int)fromSignature.getWords().size());
-				UWARN(msg.c_str());
-			}
-			else
-			{
-				msg = uFormat("No camera model");
+				msg = uFormat("No enough features < %s=%d (from=%d to=%d)", 
+					Parameters::kVisMinInliers().c_str(), _minInliers, (int)fromSignature.getWords().size(), (int)toSignature.getWords().size());
 				UWARN(msg.c_str());
 			}
 		}
@@ -2180,6 +2176,8 @@ Transform RegistrationVis::computeTransformationImpl(
 				// We take the second eigen value
 				info.inliersDistribution = pca_analysis.eigenvalues.at<float>(0, 1);
 
+				UDEBUG("Visual distribution: %f (eigen values = %f %f)", info.inliersDistribution, pca_analysis.eigenvalues.at<float>(0, 0), pca_analysis.eigenvalues.at<float>(0, 1));
+
 				if(info.inliersDistribution < _minInliersDistributionThr)
 				{
 					msg = uFormat("The distribution (%f) of inliers is under %s threshold (%f)",
@@ -2202,6 +2200,10 @@ Transform RegistrationVis::computeTransformationImpl(
 	info.matches = matchesCount;
 	info.rejectedMsg = msg;
 	info.covariance = covariance;
+	if(!covariance.empty())
+	{
+		info.variance = covariance.at<double>(0,0);
+	}
 
 	UDEBUG("inliers=%d/%d", info.inliers, info.matches);
 	UDEBUG("transform=%s", transform.prettyPrint().c_str());

corelib/src/Rtabmap.cpp

@@ -385,19 +385,19 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 					!_optimizeFromGraphEnd?_memory->getWorkingMem().lower_bound(1)->first:_memory->getWorkingMem().rbegin()->first,
 					false, _optimizedPoses, cov, &_constraints);
 		}
-		if(!_optimizedPoses.empty())
+		if(_optimizedPoses.lower_bound(1) != _optimizedPoses.end())
 		{
 			if(_restartAtOrigin)
 			{
-				UWARN("last localization pose is ignored (%s=true), assuming we start at the origin of the map.", Parameters::kRGBDStartAtOrigin().c_str());
-				lastPose = _optimizedPoses.begin()->second;
+				UWARN("last localization pose is ignored (%s=true), assuming we start at the first node of the map.", Parameters::kRGBDStartAtOrigin().c_str());
+				lastPose = _optimizedPoses.lower_bound(1)->second;
 			}
 			_lastLocalizationPose = lastPose;
 
 			UINFO("Loaded optimizedPoses=%d firstPose %d=%s lastLocalizationPose=%s",
 					_optimizedPoses.size(),
-					_optimizedPoses.begin()->first,
-					_optimizedPoses.begin()->second.prettyPrint().c_str(),
+					_optimizedPoses.lower_bound(1)->first,
+					_optimizedPoses.lower_bound(1)->second.prettyPrint().c_str(),
 					_lastLocalizationPose.prettyPrint().c_str());
 
 			if(_constraints.empty())
@@ -411,7 +411,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 			UTimer time;
 			std::map<int, float> likelihood;
 			likelihood.insert(std::make_pair(Memory::kIdVirtual, 1));
-			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
+			for(std::map<int, Transform>::iterator iter=_optimizedPoses.lower_bound(1); iter!=_optimizedPoses.end(); ++iter)
 			{
 				if(_memory->getSignature(iter->first))
 				{
@@ -507,6 +507,11 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	}
 	if(_memory)
 	{
+		if(_memory->isReadOnly() && databaseSaved)
+		{
+			UWARN("Database is read-only, latest optimized poses, latest localization pose and latest state of the memory are not saved.");
+			databaseSaved = false;
+		}
 		if(databaseSaved)
 		{
 			if(_memory->isGraphReduced() && _memory->isIncremental())
@@ -723,29 +728,24 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 			isMemIncremental != _memory->isIncremental())
 		{
 			// Mode has changed from Mapping to Localization, cleanup the local graph
-			if(_memory->isGraphReduced() && _memory->isIncremental())
+			if(_memory->isIncremental())
 			{
-				// Force reducing graph, then remove filtered nodes from the optimized poses
-				std::map<int, int> reducedIds;
-				_memory->incrementMapId(&reducedIds);
-				for(std::map<int, int>::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter)
+				if(_memory->isGraphReduced())
 				{
-					_optimizedPoses.erase(iter->first);
+					// Force reducing graph, then remove filtered nodes from the optimized poses
+					std::map<int, int> reducedIds;
+					_memory->incrementMapId(&reducedIds);
+					for(std::map<int, int>::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter)
+					{
+						_optimizedPoses.erase(iter->first);
+					}
 				}
+				_odomCachePoses.clear();
+				_odomCacheConstraints.clear();
 			}
 
 			// In both cases, we save the latest optimized graph and latest localization pose
 			_memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose);
-
-			// Mode changed from Localization to Mapping, clear local graph
-			if(!_memory->isIncremental()) {
-				_optimizedPoses.clear();
-				_lastLocalizationPose.setNull();
-				_mapCorrection.setIdentity();
-				_mapCorrectionBackup.setNull();
-				_localizationCovariance = cv::Mat();
-				_lastLocalizationNodeId = 0;
-			}
 		}
 
 		_memory->parseParameters(parameters);
@@ -760,12 +760,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 		{
 			this->createGlobalScanMap();
 		}
-
-		if(_memory->isIncremental())
-		{
-			_odomCachePoses.clear();
-			_odomCacheConstraints.clear();
-		}
 	}
 
 	if(!_epipolarGeometry)
@@ -1544,9 +1538,10 @@ bool Rtabmap::process(
 				{
 					float x,y,z, roll,pitch,yaw;
 					t.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-					bool isMoving = fabs(x) > _rgbdLinearUpdate ||
-									fabs(y) > _rgbdLinearUpdate ||
-									fabs(z) > _rgbdLinearUpdate ||
+					bool isMoving = (_rgbdLinearUpdate > 0.0f && (
+										fabs(x) > _rgbdLinearUpdate ||
+										fabs(y) > _rgbdLinearUpdate ||
+										fabs(z) > _rgbdLinearUpdate)) ||
 									(_rgbdAngularUpdate>0.0f && (
 										fabs(roll) > _rgbdAngularUpdate ||
 										fabs(pitch) > _rgbdAngularUpdate ||
@@ -1788,7 +1783,7 @@ bool Rtabmap::process(
 			}
 		}
 		_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
-		if(!_memory->isIncremental() && signature->getWeight() >= 0)
+		if(signature->getWeight() >= 0)
 		{
 			UDEBUG("Update odometry localization cache (size=%d/%d)", (int)_odomCachePoses.size(), _maxOdomCacheSize);
 			if(!_odomCachePoses.empty())
@@ -2612,6 +2607,7 @@ bool Rtabmap::process(
 	int loopClosureVisualInliers = 0; // for statistics
 	float loopClosureVisualInliersRatio = 0.0f;
 	int loopClosureVisualMatches = 0;
+	float loopClosureVisualVariance = 0.0f;
 	float loopClosureLinearVariance = 0.0f;
 	float loopClosureAngularVariance = 0.0f;
 	float loopClosureVisualInliersMeanDist = 0;
@@ -2665,7 +2661,9 @@ bool Rtabmap::process(
 				std::map<int, float> nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
 				UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size());
 				std::map<int, Transform> nearestPoses;
+				std::map<int, Transform> optimizedPosesWithOdomCache;
 				std::multimap<int, int> links;
+				std::map<int, Transform> * refPoses = &_optimizedPoses;
 				if(_memory->isIncremental() && _proximityMaxGraphDepth>0)
 				{
 					// get bidirectional links
@@ -2677,6 +2675,25 @@ bool Rtabmap::process(
 							links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <->
 						}
 					}
+					if(_odomCachePoses.size() > 1)
+					{
+						// Add odometry cache if it contains a loop closure
+						// That could happen when we just switched from localization mode to 
+						// mapping mode while being localized on the previous session.
+						optimizedPosesWithOdomCache = _optimizedPoses;
+						optimizedPosesWithOdomCache.insert(_odomCachePoses.begin(), _odomCachePoses.end());
+						refPoses = &optimizedPosesWithOdomCache;
+						for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end(); ++iter)
+						{
+							if(uContains(optimizedPosesWithOdomCache, iter->second.from()) && 
+							   uContains(optimizedPosesWithOdomCache, iter->second.to()) && 
+							   iter->second.from() != iter->second.to())
+							{
+								links.insert(std::make_pair(iter->second.from(), iter->second.to()));
+								links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <->
+							}
+						}
+					}
 				}
 				for(std::map<int, float>::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter)
 				{
@@ -2684,7 +2701,7 @@ bool Rtabmap::process(
 					{
 						if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
 						{
-							std::list<std::pair<int, Transform> > path = graph::computePath(_optimizedPoses, links, signature->id(), iter->first);
+							std::list<std::pair<int, Transform> > path = graph::computePath(*refPoses, links, signature->id(), iter->first);
 							UDEBUG("Graph depth to %d = %ld", iter->first, path.size());
 							if(!path.empty() && (int)path.size() <= _proximityMaxGraphDepth)
 							{
@@ -2794,6 +2811,7 @@ bool Rtabmap::process(
 									loopClosureVisualInliers = info.inliers;
 									loopClosureVisualInliersRatio = info.inliersRatio;
 									loopClosureVisualMatches = info.matches;
+									loopClosureVisualVariance = info.variance;
 
 									cv::Mat information = getInformation(info.covariance);
 									loopClosureLinearVariance = 1.0/information.at<double>(0,0);
@@ -3061,6 +3079,7 @@ bool Rtabmap::process(
 				loopClosureVisualInliers = info.inliers;
 				loopClosureVisualInliersRatio = info.inliersRatio;
 				loopClosureVisualMatches = info.matches;
+				loopClosureVisualVariance = info.variance;
 				rejectedLoopClosure = transform.isNull();
 				if(rejectedLoopClosure)
 				{
@@ -3257,6 +3276,7 @@ bool Rtabmap::process(
 				{
 					constraints.insert(std::make_pair(iter->second.from(), iter->second));
 				}
+
 				cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf;
 				for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 				{
@@ -3264,6 +3284,7 @@ bool Rtabmap::process(
 					if(iterPose != _optimizedPoses.end() && poses.find(iterPose->first) == poses.end())
 					{
 						poses.insert(*iterPose);
+
 						// make the poses in the map fixed
 						constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat)));
 						UDEBUG("Constraint %d->%d: %s (type=%s, var=%f)", iterPose->first, iterPose->first, iterPose->second.prettyPrint().c_str(), Link::typeName(Link::kPosePrior).c_str(), 1./_localizationPriorInf);
@@ -3273,11 +3294,14 @@ bool Rtabmap::process(
 
 				std::map<int, Transform> posesOut;
 				std::multimap<int, Link> edgeConstraintsOut;
+
 				bool priorsIgnored = _graphOptimizer->priorsIgnored();
 				UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false");
 				_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
+
 				// If slam2d: get connected graph while keeping original roll,pitch,z values.
 				_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut);
+				
 				if(ULogger::level() == ULogger::kDebug)
 				{
 					for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
@@ -4043,6 +4067,7 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers_ratio(), loopClosureVisualInliersRatio);
 			statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches);
+			statistics_.addStatistic(Statistics::kLoopVisual_variance(), loopClosureVisualVariance);
 			statistics_.addStatistic(Statistics::kLoopLinear_variance(), loopClosureLinearVariance);
 			statistics_.addStatistic(Statistics::kLoopAngular_variance(), loopClosureAngularVariance);
 			statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
@@ -4303,6 +4328,20 @@ bool Rtabmap::process(
 			// If there is a too small displacement, remove the node
 			signaturesRemoved.push_back(signature->id());
 			_memory->deleteLocation(signature->id());
+
+			// Update odom cache (if we just switched from mapping mode to localization mode)
+			_odomCachePoses.erase(signature->id());
+			for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();)
+			{
+				if(iter->second.from() == signature->id() || iter->second.to() == signature->id())
+				{
+					_odomCacheConstraints.erase(iter++);
+				}
+				else
+				{
+					++iter;
+				}
+			}
 		}
 		else
 		{
@@ -5630,8 +5669,10 @@ int Rtabmap::detectMoreLoopClosures(
 		bool intraSession,
 		bool interSession,
 		const ProgressState * processState,
-		float clusterRadiusMin)
+		float clusterRadiusMin,
+		int toFromMapId)
 {
+	UDEBUG("");
 	UASSERT(iterations>0);
 
 	if(_graphOptimizer->iterations() <= 0)
@@ -5656,17 +5697,23 @@ int Rtabmap::detectMoreLoopClosures(
 	std::map<int, Transform> posesToCheckLoopClosures;
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> links;
-	std::map<int, Signature> signatures; // some signatures may be in LTM, get them all
-	this->getGraph(poses, links, true, true, &signatures);
+	this->getGraph(poses, links, true, true);
 
 	std::map<int, int> mapIds;
 	UDEBUG("remove all invalid or intermediate nodes, fill mapIds");
 	for(std::map<int, Transform>::iterator iter=poses.upper_bound(0); iter!=poses.end();++iter)
 	{
-		if(signatures.at(iter->first).getWeight() >= 0)
+		Transform odom, gt;
+		int mapId, weight;
+		std::string l;
+		double s;
+		std::vector<float> v;
+		GPS gps;
+		EnvSensors srs;
+		if(_memory->getNodeInfo(iter->first, odom, mapId, weight, l, s, gt, v, gps, srs, true) && weight >= 0)
 		{
 			posesToCheckLoopClosures.insert(*iter);
-			mapIds.insert(std::make_pair(iter->first, signatures.at(iter->first).mapId()));
+			mapIds.insert(std::make_pair(iter->first, mapId));
 		}
 	}
 
@@ -5680,7 +5727,56 @@ int Rtabmap::detectMoreLoopClosures(
 				clusterRadiusMax,
 				clusterAngle);
 
-		UINFO("Looking for more loop closures, clustering poses... found %d clusters.", (int)clusters.size());
+		UINFO("Looking for more loop closures: clustering poses... found %ld clusters.", clusters.size());
+
+		if(toFromMapId >=0)
+		{
+			size_t clustersBefore = clusters.size();
+			for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end();)
+			{
+				int mapId = uValue(mapIds, iter->first, 0);
+				if(mapId != toFromMapId)
+				{
+					iter = clusters.erase(iter);
+				}
+				else {
+					++iter;
+				}
+			}
+			UINFO("Looking for more loop closures: filtered %ld/%ld clusters for map session %d.", clustersBefore-clusters.size(), clustersBefore, toFromMapId);
+			if(clusters.empty())
+			{
+				UERROR("No clusters belong to mapId %d, aborting.", toFromMapId);
+				break;
+			}
+		}
+
+		if(_memory->getMaxStMemSize() > 1)
+		{
+			size_t clustersBefore = clusters.size();
+			for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end();)
+			{
+				if(abs(iter->first - iter->second) < _memory->getMaxStMemSize())
+				{
+					iter = clusters.erase(iter);
+				}
+				else
+				{
+					// compute path to know how far we are in terms of graph length
+					std::map<int, int> ids = _memory->getNeighborsId(iter->first, _memory->getMaxStMemSize(), -1, true, true, true);
+					if(ids.find(iter->second) != ids.end())
+					{
+						iter = clusters.erase(iter);
+					}
+					else
+					{
+						++iter;
+					}
+				}
+			}
+			UINFO("Looking for more loop closures: filtered %ld/%ld clusters for too close nodes (below %s=%d).",
+				clustersBefore-clusters.size(), clustersBefore, Parameters::kMemSTMSize().c_str(), _memory->getMaxStMemSize());
+		}
 
 		int i=0;
 		std::set<int> addedLinks;
@@ -5732,8 +5828,10 @@ int Rtabmap::detectMoreLoopClosures(
 						{
 							checkedLoopClosures.insert(std::make_pair(from, to));
 
-							UASSERT(signatures.find(from) != signatures.end());
-							UASSERT(signatures.find(to) != signatures.end());
+							Signature fromS = getSignatureCopy(from, false, true, false, false, true, false);
+							Signature toS = getSignatureCopy(to, false, true, false, false, true, false);
+							UASSERT(fromS.getWeight()>=0);
+							UASSERT(toS.getWeight()>=0);
 
 							Transform guess;
 							if(_proximityBySpace && uContains(poses, from) && uContains(poses, to))
@@ -5743,7 +5841,7 @@ int Rtabmap::detectMoreLoopClosures(
 
 							RegistrationInfo info;
 							// use signatures instead of IDs because some signatures may not be in WM
-							Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), guess, &info);
+							Transform t = _memory->computeTransform(fromS, toS, guess, &info);
 
 							if(!t.isNull())
 							{
@@ -5752,11 +5850,11 @@ int Rtabmap::detectMoreLoopClosures(
 								//optimize the graph to see if the new constraint is globally valid
 
 								int fromId = from;
-								int mapId = signatures.at(from).mapId();
+								int mapId = fromS.mapId();
 								// use first node of the map containing from
-								for(std::map<int, Signature>::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster)
+								for(std::map<int, Transform>::iterator ster=posesToCheckLoopClosures.begin(); ster!=posesToCheckLoopClosures.end(); ++ster)
 								{
-									if(ster->second.mapId() == mapId)
+									if(uValue(mapIds, ster->first, 0) == mapId)
 									{
 										fromId = ster->first;
 										break;
@@ -5771,22 +5869,22 @@ int Rtabmap::detectMoreLoopClosures(
 								float maxLinearErrorRatio = 0.0f;
 								float maxAngularErrorRatio = 0.0f;
 								std::map<int, Transform> optimizedPoses;
-								std::multimap<int, Link> links;
+								std::multimap<int, Link> linksOut;
 								UASSERT(poses.find(fromId) != poses.end());
 								UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
 								UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
-								_graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links);
+								_graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, linksOut);
 								UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end());
-								UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
-								UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str());
-								UASSERT(graph::findLink(links, from, to) != links.end());
-								optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
+								UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)linksOut.size()).c_str());
+								UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)linksOut.size()).c_str());
+								UASSERT(graph::findLink(linksOut, from, to) != linksOut.end());
+								optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, linksOut);
 								std::string msg;
 								if(optimizedPoses.size())
 								{
 									graph::computeMaxGraphErrors(
 											optimizedPoses,
-											links,
+											linksOut,
 											maxLinearErrorRatio,
 											maxAngularErrorRatio,
 											maxLinearError,
@@ -6314,6 +6412,7 @@ bool Rtabmap::addLink(const Link & link)
 		std::map<int, Transform> poses = _odomCachePoses;
 		std::multimap<int, Link> constraints = _odomCacheConstraints;
 		constraints.insert(std::make_pair(link.from(), link));
+		cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf;
 		for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 		{
 			std::map<int, Transform>::iterator iterPose = _optimizedPoses.find(iter->second.to());
@@ -6321,7 +6420,7 @@ bool Rtabmap::addLink(const Link & link)
 			{
 				poses.insert(*iterPose);
 				// make the poses in the map fixed
-				constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, cv::Mat::eye(6,6, CV_64FC1)*999999)));
+				constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat)));
 			}
 		}
 

corelib/src/SensorCaptureThread.cpp

@@ -960,7 +960,6 @@ void SensorCaptureThread::postUpdate(SensorData * dataPtr, SensorCaptureInfo * i
 			{
 				UASSERT(!data.imu().localTransform().isNull());
 				imu.convertToBaseFrame();
-
 			}
 			_imuFilter->update(
 					imu.angularVelocity()[0],

corelib/src/Stereo.cpp

@@ -113,6 +113,9 @@ std::vector<cv::Point2f> Stereo::computeCorrespondences(
 StereoOpticalFlow::StereoOpticalFlow(const ParametersMap & parameters) :
 		Stereo(parameters),
 		epsilon_(Parameters::defaultStereoEps()),
+		useMinEigenVals_(Parameters::defaultStereoUseMinEigenVals()),
+		minEigThreshold_(Parameters::defaultStereoMinEigThreshold()),
+		errorThreshold_(Parameters::defaultStereoErrorThreshold()),
 		gpu_(Parameters::defaultStereoGpu())
 {
 	this->parseParameters(parameters);
@@ -122,6 +125,9 @@ void StereoOpticalFlow::parseParameters(const ParametersMap & parameters)
 {
 	Stereo::parseParameters(parameters);
 	Parameters::parse(parameters, Parameters::kStereoEps(), epsilon_);
+	Parameters::parse(parameters, Parameters::kStereoUseMinEigenVals(), useMinEigenVals_);
+	Parameters::parse(parameters, Parameters::kStereoMinEigThreshold(), minEigThreshold_);
+	Parameters::parse(parameters, Parameters::kStereoErrorThreshold(), errorThreshold_);
 	Parameters::parse(parameters, Parameters::kStereoGpu(), gpu_);
 #ifndef HAVE_OPENCV_CUDAOPTFLOW
 	if(gpu_)
@@ -171,11 +177,18 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
 				err,
 				this->winSize(),
 				this->maxLevel(),
-				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), epsilon_),
-				cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), this->epsilon()),
+				this->usingMinEigenVals() ? cv::OPTFLOW_LK_GET_MIN_EIGENVALS : 0, this->minEigThreshold());
 		UDEBUG("util2d::calcOpticalFlowPyrLKStereo() end");
 	}
-	updateStatus(leftCorners, rightCorners, status);
+	if(this->usingMinEigenVals())
+	{
+		updateStatus(leftCorners, rightCorners, status);
+	}
+	else
+	{
+		updateStatus(leftCorners, rightCorners, status, err);
+	}
 	return rightCorners;
 }
 
@@ -227,14 +240,17 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
 void StereoOpticalFlow::updateStatus(
 		const std::vector<cv::Point2f> & leftCorners,
 		const std::vector<cv::Point2f> & rightCorners,
-		std::vector<unsigned char> & status) const
+		std::vector<unsigned char> & status,
+		std::vector<float> err) const
 {
-	UASSERT(leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size());
+	UASSERT(
+		leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size() &&
+		(err.empty() || err.size() == leftCorners.size()));
 	int countFlowRejected = 0;
 	int countDisparityRejected = 0;
 	for(unsigned int i=0; i<status.size(); ++i)
 	{
-		if(status[i]!=0)
+		if(status[i]!=0 && (err.empty() || err[i] < this->errorThreshold()))
 		{
 			float disparity = leftCorners[i].x - rightCorners[i].x;
 			if(disparity <= this->minDisparity() || disparity > this->maxDisparity())

corelib/src/Transform.cpp

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

corelib/src/VWDictionary.cpp

@@ -571,19 +571,36 @@ void VWDictionary::update()
 		else if(_strategy >= kNNBruteForce &&
 				_notIndexedWords.size() &&
 				_removedIndexedWords.size() == 0 &&
-				_visualWords.size() &&
-				_dataTree.rows)
+				_visualWords.size())
 		{
+			const int IMGIDX_SHIFT = 18;
+    		const int IMGIDX_ONE = (1 << IMGIDX_SHIFT); // a limit defined in https://github.com/opencv/opencv/blob/4.x/modules/features2d/src/matchers.cpp
+			if(_dataTree.rows >= IMGIDX_ONE)
+			{
+				UWARN("%s=%d is not a FLANN strategy and the number of words in the vocabulary (%d) is over %d (IMGIDX_ONE), so opencv may "
+					"assert on an IMGIDX_ONE check when adding new words. Use a FLANN strategy instead (%s<%d).",
+					Parameters::kKpNNStrategy().c_str(), _strategy, _dataTree.rows, IMGIDX_ONE, Parameters::kKpNNStrategy().c_str(), kNNBruteForce);
+			}
+
 			//just add not indexed words
 			int i = _dataTree.rows;
-			_dataTree.reserve(_dataTree.rows + _notIndexedWords.size());
+			if(!_dataTree.empty()) {
+				_dataTree.reserve(_dataTree.rows + _notIndexedWords.size());
+			}
 			for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
 			{
 				VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
 				UASSERT(w);
-				UASSERT(w->getDescriptor().cols == _dataTree.cols);
-				UASSERT(w->getDescriptor().type() == _dataTree.type());
-				_dataTree.push_back(w->getDescriptor());
+				if(_dataTree.empty())
+				{
+					_dataTree = w->getDescriptor().clone();
+				}
+				else
+				{
+					UASSERT(w->getDescriptor().cols == _dataTree.cols);
+					UASSERT(w->getDescriptor().type() == _dataTree.type());
+					_dataTree.push_back(w->getDescriptor());
+				}
 				_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, w->id()));
 				std::pair<std::map<int, int>::iterator, bool> inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), i));
 				UASSERT(inserted.second);
@@ -860,7 +877,7 @@ int VWDictionary::getNextId()
 	return ++_lastWordId;
 }
 
-void VWDictionary::addWordRef(int wordId, int signatureId)
+bool VWDictionary::addWordRef(int wordId, int signatureId)
 {
 	VisualWord * vw = 0;
 	vw = uValue(_visualWords, wordId, vw);
@@ -870,10 +887,12 @@ void VWDictionary::addWordRef(int wordId, int signatureId)
 		_totalActiveReferences += 1;
 
 		_unusedWords.erase(vw->id());
+		return true;
 	}
 	else
 	{
-		UERROR("Not found word %d (dict size=%d)", wordId, (int)_visualWords.size());
+		UWARN("Not found word %d (dict size=%d)", wordId, (int)_visualWords.size());
+		return false;
 	}
 }
 
@@ -996,7 +1015,7 @@ std::list<int> VWDictionary::addNewWords(
 	if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k))
 	{
 		//Find nearest neighbors
-		UDEBUG("newPts.total()=%d ", descriptors.rows);
+		UDEBUG("newPts.total()=%d _strategy=%d", descriptors.rows, _strategy);
 
 		if(_strategy == kNNFlannNaive || _strategy == kNNFlannKdTree || _strategy == kNNFlannLSH)
 		{

corelib/src/camera/CameraImages.cpp

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

corelib/src/camera/CameraOrbbecSDK.cpp

@@ -155,10 +155,10 @@ bool CameraOrbbecSDK::available()
 
 CameraOrbbecSDK::CameraOrbbecSDK(
         std::string deviceId,
-        int colorWidth,
-        int colorHeight,
-        int depthWidth,
-        int depthHeight,
+        unsigned int colorWidth,
+        unsigned int colorHeight,
+        unsigned int depthWidth,
+        unsigned int depthHeight,
         float imageRate,
         const Transform & localTransform) :
             Camera(imageRate, localTransform)
@@ -327,22 +327,36 @@ bool CameraOrbbecSDK::init(const std::string & calibrationFolder, const std::str
                 UINFO("Resolution: %ldx%ld, FPS: %ld, Format: %d", 
                     profile->getWidth(), profile->getHeight(), profile->getFps(), profile->getFormat(), j==0?" (default)":"");
                 
+                // Get maximum frame rate based on resolution selected
                 if(sensors->getSensorType(i) == OB_SENSOR_DEPTH) {
-                    if(profile->getFps() > maxDepthFps) {
+                    if( profile->getFps() > maxDepthFps &&
+                        depthWidth_ == profile->getWidth() &&
+                        depthHeight_ == profile->getHeight())
+                    {
                         maxDepthFps = profile->getFps();
                     }
                 }
                 else
                 {
-                    if(profile->getFps() > maxColorFps) {
+                    if( profile->getFps() > maxColorFps &&
+                        colorWidth_ == profile->getWidth() &&
+                        colorHeight_ == profile->getHeight())
+                    {
                         maxColorFps = profile->getFps();
                     }
-                }
-                
+                }            
             }
         }
     }
 
+    // Note that for TOF camera, we want maximum frame rate to better 
+    // sync rgb and depth. For stereo cameras, use the specified frame rate.
+    if(this->getImageRate()!=0.0f && device->getDeviceInfo()->getDeviceType() != OB_TOF_CAMERA)
+    {
+        maxColorFps = maxDepthFps = (unsigned int)this->getImageRate();
+        this->setImageRate(0);
+    }
+
     std::shared_ptr<ob::Config> imuConfig;
     if(imuPublished_)
     {

corelib/src/camera/CameraRealSense2.cpp

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UThreadC.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UDirectory.h>
 #include <opencv2/imgproc/types_c.h>
 
 #ifdef RTABMAP_REALSENSE2
@@ -78,7 +79,8 @@ CameraRealSense2::CameraRealSense2(
 	cameraDepthFps_(30),
 	globalTimeSync_(true),
 	dualMode_(false),
-	closing_(false)
+	closing_(false),
+	playback_(false)
 #endif
 {
 	UDEBUG("");
@@ -130,6 +132,7 @@ void CameraRealSense2::close()
 	}
 
 	closing_ = false;
+	playback_ = false;
 }
 
 void CameraRealSense2::imu_callback(rs2::frame frame)
@@ -492,64 +495,76 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 	clockSyncWarningShown_ = false;
 	imuGlobalSyncWarningShown_ = false;
 
-	rs2::device_list list = ctx_.query_devices();
-	if (0 == list.size())
+	if(uStrContains(deviceId_, ".bag"))
 	{
-		UERROR("No RealSense2 devices were found!");
-		return false;
+		// playback (bag recorded by realsense-viewer)
+		dev_.resize(1);
+		dev_[0] = ctx_.load_device(uReplaceChar(deviceId_, '~', UDirectory::homeDir()));
+		playback_ = true;
+		UINFO("Device ID is a bag (\"%s\"), using playback mode", deviceId_.c_str());
 	}
-
-	bool found=false;
-	try
+	else
 	{
-		for (rs2::device dev : list)
+		rs2::device_list list = ctx_.query_devices();
+		if (0 == list.size())
 		{
-			auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
-			auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
-			auto name = dev.get_info(RS2_CAMERA_INFO_NAME);
+			UERROR("No RealSense2 devices were found!");
+			return false;
+		}
 
-			uint16_t pid;
-			std::stringstream ss;
-			ss << std::hex << pid_str;
-			ss >> pid;
-			UINFO("Device \"%s\" with serial number %s was found with product ID=%d.", name, sn, (int)pid);
-			if(dualMode_ && pid == 0x0B37)
+		bool found=false;
+		try
+		{
+			for (rs2::device dev : list)
 			{
-				// Dual setup: device[0] = D400, device[1] = T265
-				// T265
-				dev_.resize(2);
-				dev_[1] = dev;
-			}
-			else if (!found && (deviceId_.empty() || deviceId_ == sn || uStrContains(name, uToUpperCase(deviceId_))))
-			{
-				if(dev_.empty())
+				auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
+				auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
+				auto name = dev.get_info(RS2_CAMERA_INFO_NAME);
+
+				uint16_t pid;
+				std::stringstream ss;
+				ss << std::hex << pid_str;
+				ss >> pid;
+				UINFO("Device \"%s\" with serial number %s was found with product ID=%d.", name, sn, (int)pid);
+				if(dualMode_ && pid == 0x0B37)
 				{
-					dev_.resize(1);
+					// Dual setup: device[0] = D400, device[1] = T265
+					// T265
+					dev_.resize(2);
+					dev_[1] = dev;
+				}
+				else if (!found && (deviceId_.empty() || deviceId_ == sn || uStrContains(name, uToUpperCase(deviceId_))))
+				{
+					if(dev_.empty())
+					{
+						dev_.resize(1);
+					}
+					dev_[0] = dev;
+					found=true;
 				}
-				dev_[0] = dev;
-				found=true;
 			}
 		}
-	}
-	catch(const rs2::error & error)
-	{
-		UWARN("%s. Is the camera already used with another app?", error.what());
+		catch(const rs2::error & error)
+		{
+			UWARN("%s. Is the camera already used with another app?", error.what());
+		}
+
+		if (!found)
+		{
+			if(dualMode_ && dev_.size()==2)
+			{
+				UERROR("Dual setup is enabled, but a D400 camera is not detected!");
+				dev_.clear();
+			}
+			else
+			{
+				UERROR("The requested device \"%s\" is NOT found!", deviceId_.c_str());
+			}
+			return false;
+		}
 	}
 
-	if (!found)
-	{
-		if(dualMode_ && dev_.size()==2)
-		{
-			UERROR("Dual setup is enabled, but a D400 camera is not detected!");
-			dev_.clear();
-		}
-		else
-		{
-			UERROR("The requested device \"%s\" is NOT found!", deviceId_.c_str());
-		}
-		return false;
-	}
-	else if(dualMode_ && dev_.size()!=2)
+	if(dualMode_ && dev_.size()!=2)
 	{
 		UERROR("Dual setup is enabled, but a T265 camera is not detected!");
 		dev_.clear();
@@ -634,7 +649,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			sensors[1] = elem;
 			if(sensors[1].supports(rs2_option::RS2_OPTION_EMITTER_ENABLED))
 			{
-				sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_);
+				if(!sensors[1].is_option_read_only(rs2_option::RS2_OPTION_EMITTER_ENABLED))
+				{
+					sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_);
+				}
+				else if(!emitterEnabled_)
+				{
+					UWARN("rs2_option::RS2_OPTION_EMITTER_ENABLED option is read-only, cannot disable IR emitter.");
+				}
 			}
 		}
 		else if ("Coded-Light Depth Sensor" == module_name)
@@ -712,14 +734,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			for (auto& profile : profiles)
 			{
 				auto video_profile = profile.as<rs2::video_stream_profile>();
-				UINFO("%s %d %d %d %d %s type=%d", rs2_format_to_string(
-						video_profile.format()),
-						video_profile.width(),
-						video_profile.height(),
-						video_profile.fps(),
-						video_profile.stream_index(),
-						video_profile.stream_name().c_str(),
-						video_profile.stream_type());
+				UINFO("%s %d %d %d %d %s type=%d", 
+						rs2_format_to_string(profile.format()),
+						video_profile.get()?video_profile.width():-1,
+						video_profile.get()?video_profile.height():-1,
+						profile.fps(),
+						profile.stream_index(),
+						profile.stream_name().c_str(),
+						profile.stream_type());
 			}
 		}
 		int pi = 0;
@@ -728,10 +750,12 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			auto video_profile = profile.as<rs2::video_stream_profile>();
 			if(!stereo)
 			{
-				if( (video_profile.width()  == cameraWidth_ &&
+				if( (video_profile.get() &&
+					 video_profile.width()  == cameraWidth_ &&
 					 video_profile.height() == cameraHeight_ &&
 					 video_profile.fps()    == cameraFps_) ||
-						(strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 &&
+						((strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 || 
+						  (playback_ && strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "Stereo Module")==0)) &&
 							video_profile.width()  == cameraDepthWidth_ &&
 							video_profile.height() == cameraDepthHeight_ &&
 							video_profile.fps()    == cameraDepthFps_))
@@ -740,7 +764,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 
 					// rgb or ir left
 					if((!ir_ && video_profile.format() == RS2_FORMAT_RGB8 && video_profile.stream_type() == RS2_STREAM_COLOR) ||
-					  (ir_ && video_profile.format() == RS2_FORMAT_Y8 && (video_profile.stream_index() == 1 || isL500)))
+					   (ir_ && video_profile.format() == RS2_FORMAT_Y8 && (video_profile.stream_index() == 1 || isL500)))
 					{
 						if(!profilesPerSensor[i].empty())
 						{
@@ -778,7 +802,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 						}
 					}
 				}
-				else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF)
+				else if(profile.format() == RS2_FORMAT_MOTION_XYZ32F || profile.format() == RS2_FORMAT_6DOF)
 				{
 					//D435i:
 					//MOTION_XYZ32F 0 0 200 (gyro)
@@ -817,6 +841,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			{
 				//T265:
 				if(!dualMode_ &&
+					video_profile.get() &&
 					video_profile.format() == RS2_FORMAT_Y8 &&
 					video_profile.width()  == 848 &&
 					video_profile.height() == 800 &&
@@ -865,7 +890,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 					}
 					added = true;
 				}
-				else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF)
+				else if(profile.format() == RS2_FORMAT_MOTION_XYZ32F || profile.format() == RS2_FORMAT_6DOF)
 				{
 					//MOTION_XYZ32F 0 0 200
 					//MOTION_XYZ32F 0 0 62
@@ -878,20 +903,29 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		}
 		if (!added)
 		{
-			UERROR("Given stream configuration is not supported by the device! "
-					"Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraWidth_, cameraHeight_, cameraFps_);
+			if(strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 || 
+			   (playback_ && strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "Stereo Module")==0))
+			{
+				UERROR("Given stream configuration is not supported by the device! "
+						"Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraDepthWidth_, cameraDepthHeight_, cameraDepthFps_);
+			}
+			else
+			{
+				UERROR("Given stream configuration is not supported by the device! "
+						"Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraWidth_, cameraHeight_, cameraFps_);
+			}
 			UERROR("Available configurations:");
 			for (auto& profile : profiles)
 			{
 				auto video_profile = profile.as<rs2::video_stream_profile>();
-				UERROR("%s %d %d %d %d %s type=%d", rs2_format_to_string(
-						video_profile.format()),
-						video_profile.width(),
-						video_profile.height(),
-						video_profile.fps(),
-						video_profile.stream_index(),
-						video_profile.stream_name().c_str(),
-						video_profile.stream_type());
+				UERROR("%s %d %d %d %d %s type=%d",
+						rs2_format_to_string(profile.format()),
+						video_profile.get()?video_profile.width():-1,
+						video_profile.get()?video_profile.height():-1,
+						profile.fps(),
+						profile.stream_index(),
+						profile.stream_name().c_str(),
+						profile.stream_type());
 			}
 			return false;
 		}
@@ -1075,19 +1109,26 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			 {
 				 auto video_profile = profilesPerSensor[i][j].as<rs2::video_stream_profile>();
 				 UINFO("Opening: %s %d %d %d %d %s type=%d", rs2_format_to_string(
-						 video_profile.format()),
-						 video_profile.width(),
-						 video_profile.height(),
-						 video_profile.fps(),
-						 video_profile.stream_index(),
-						 video_profile.stream_name().c_str(),
-						 video_profile.stream_type());
+						 profilesPerSensor[i][j].format()),
+						 video_profile.get()?video_profile.width():-1,
+						 video_profile.get()?video_profile.height():-1,
+						 profilesPerSensor[i][j].fps(),
+						 profilesPerSensor[i][j].stream_index(),
+						 profilesPerSensor[i][j].stream_name().c_str(),
+						 profilesPerSensor[i][j].stream_type());
 			 }
 			 if(globalTimeSync_ && sensors[i].supports(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED))
 			 {
-				 float value = sensors[i].get_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED);
-				 UINFO("Set RS2_OPTION_GLOBAL_TIME_ENABLED=1 (was %f) for sensor %d", value, (int)i);
-				 sensors[i].set_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED, 1);
+				float value = sensors[i].get_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED);
+				UINFO("Set RS2_OPTION_GLOBAL_TIME_ENABLED=1 (was %f) for sensor %d", value, (int)i);
+				if(!sensors[i].is_option_read_only(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED))
+				{
+					sensors[i].set_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED, 1);
+				}
+				else if(value != 1)
+				{
+					UWARN("rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED option is read-only, cannot enable it.");
+				}
 			 }
 			 sensors[i].open(profilesPerSensor[i]);
 			 if(sensors[i].is<rs2::depth_sensor>())
@@ -1525,11 +1566,21 @@ SensorData CameraRealSense2::captureImage(SensorCaptureInfo * info)
 		else
 		{
 			UERROR("Missing frames (received %d, needed=%d)", (int)frameset.size(), desiredFramesetSize);
+			if(frameset.size()>0)
+			{
+				for (auto it = frameset.begin(); it != frameset.end(); ++it)
+				{
+					UERROR("Received frame only from %s", (*it).get_profile().stream_name().c_str());
+				}
+			}
 		}
 	}
 	catch(const std::exception& ex)
 	{
-		UERROR("An error has occurred during frame callback: %s", ex.what());
+		if(!playback_)
+		{
+			UERROR("An error has occurred during frame callback: %s", ex.what());
+		}
 	}
 #else
 	UERROR("CameraRealSense2: RTAB-Map is not built with RealSense2 support!");

corelib/src/global_map/OccupancyGrid.cpp

@@ -92,7 +92,6 @@ void OccupancyGrid::clear()
 {
 	map_ = cv::Mat();
 	mapInfo_ = cv::Mat();
-	cellCount_.clear();
 	GlobalMap::clear();
 }
 
@@ -433,11 +432,6 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
 				if(iter != emptyLocalMaps.end() || jter!=occupiedLocalMaps.end())
 				{
 					addAssembledNode(kter->first, kter->second);
-					std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
-					if(cter == cellCount_.end() && kter->first > 0)
-					{
-						cter = cellCount_.insert(std::make_pair(kter->first, std::pair<int,int>(0,0))).first;
-					}
 					if(iter!=emptyLocalMaps.end())
 					{
 						for(int i=0; i<iter->second.cols; ++i)
@@ -459,30 +453,12 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
 										// cannot rewrite on cells referred by more recent nodes
 										continue;
 									}
-									if(nodeId > 0)
-									{
-										std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
-										UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
-										if(value == 0)
-										{
-											eter->second.first -= 1;
-										}
-										else if(value == 100)
-										{
-											eter->second.second -= 1;
-										}
-										if(kter->first < 0)
-										{
-											eter->second.first += 1;
-										}
-									}
 								}
 								if(kter->first > 0)
 								{
 									info[0] = (float)kter->first;
 									info[1] = ptf[0];
 									info[2] = ptf[1];
-									cter->second.first+=1;
 								}
 								value = 0; // free space
 
@@ -533,23 +509,6 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
 										// cannot rewrite on cells referred by more recent nodes
 										continue;
 									}
-									if(nodeId>0)
-									{
-										std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
-										UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
-										if(value == 0)
-										{
-											eter->second.first -= 1;
-										}
-										else if(value == 100)
-										{
-											eter->second.second -= 1;
-										}
-										if(kter->first < 0)
-										{
-											eter->second.first += 1;
-										}
-									}
 								}
 								if(kter->first > 0)
 								{
@@ -557,7 +516,6 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
 									info[1] = float(i) * cellSize_ + xMin;
 									info[2] = float(j) * cellSize_ + yMin;
 									info[3] = logOddsClampingMin_;
-									cter->second.first+=1;
 								}
 								value = -2; // free space (footprint)
 							}
@@ -585,30 +543,12 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
 										// cannot rewrite on cells referred by more recent nodes
 										continue;
 									}
-									if(nodeId>0)
-									{
-										std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
-										UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
-										if(value == 0)
-										{
-											eter->second.first -= 1;
-										}
-										else if(value == 100)
-										{
-											eter->second.second -= 1;
-										}
-										if(kter->first < 0)
-										{
-											eter->second.second += 1;
-										}
-									}
 								}
 								if(kter->first > 0)
 								{
 									info[0] = (float)kter->first;
 									info[1] = ptf[0];
 									info[2] = ptf[1];
-									cter->second.second+=1;
 								}
 
 								// update odds
@@ -651,20 +591,6 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
 			mapInfo_ = mapInfo;
 			minValues_[0] = xMin;
 			minValues_[1] = yMin;
-
-			// clean cellCount_
-			for(std::map<int, std::pair<int, int> >::iterator iter= cellCount_.begin(); iter!=cellCount_.end();)
-			{
-				UASSERT(iter->second.first >= 0 && iter->second.second >= 0);
-				if(iter->second.first == 0 && iter->second.second == 0)
-				{
-					cellCount_.erase(iter++);
-				}
-				else
-				{
-					++iter;
-				}
-			}
 		}
 	}
 
@@ -677,7 +603,6 @@ unsigned long OccupancyGrid::getMemoryUsed() const
 
 	memoryUsage += map_.total() * map_.elemSize();
 	memoryUsage += mapInfo_.total() * mapInfo_.elemSize();
-	memoryUsage += cellCount_.size()*(sizeof(int)*3 + sizeof(std::pair<int, int>) + sizeof(std::map<int, std::pair<int, int> >::iterator)) + sizeof(std::map<int, std::pair<int, int> >);
 
 	return memoryUsage;
 }

corelib/src/odometry/OdometryCuVSLAM.cpp

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
+#include <cmath>
 
 #ifdef RTABMAP_CUVSLAM
 #include "rtabmap/core/CameraModel.h"
@@ -95,6 +96,7 @@ bool initializeCuVSLAM(const SensorData & data,
                        CUVSLAM_TrackerHandle & cuvslam_handle,
                        CUVSLAM_GroundConstraintHandle & ground_constraint_handle,
                        bool planar_constraints,
+                       int multicam_mode,
                        std::vector<uint8_t *> & gpu_left_image_data,
                        std::vector<uint8_t *> & gpu_right_image_data,
                        std::vector<size_t> & gpu_left_image_sizes,
@@ -103,7 +105,7 @@ bool initializeCuVSLAM(const SensorData & data,
 	                   std::vector<std::array<float, 12>> & intrinsics,
                        cudaStream_t & cuda_stream);
     
-CUVSLAM_Configuration CreateConfiguration(const SensorData & data);
+CUVSLAM_Configuration CreateConfiguration(const SensorData & data, int multicam_mode);
 
 bool prepareImages(const SensorData & data, 
                     std::vector<CUVSLAM_Image> & cuvslam_images,
@@ -113,10 +115,7 @@ bool prepareImages(const SensorData & data,
                     std::vector<size_t> & gpu_right_image_sizes,
                     cudaStream_t & cuda_stream);
 
-cv::Mat convertCuVSLAMCovariance(const float * cuvslam_covariance);
-
-void printCovarianceMatrix(const cv::Mat & cov, const std::string & label);
-void printRawCuvslamCovariance(const float * cuvslam_covariance, const std::string & label);
+cv::Mat convertCuVSLAMCovariance(const float * cuvslam_covariance, bool use_raw_covariance);
 
 
 // ============================================================================
@@ -161,24 +160,6 @@ Transform FromcuVSLAMPose(const CUVSLAM_Pose & cuvslam_pose)
   return rtabmap_transform;
 }
 
-void PrintConfiguration(const CUVSLAM_Configuration & cfg)
-{
-  UINFO("Use use_gpu: %s", cfg.use_gpu ? "true" : "false");
-  UINFO("Enable IMU Fusion: %s", cfg.enable_imu_fusion ? "true" : "false");
-  if (cfg.enable_imu_fusion) {
-    UINFO("gyroscope_noise_density: %f",
-      cfg.imu_calibration.gyroscope_noise_density);
-    UINFO("gyroscope_random_walk: %f",
-      cfg.imu_calibration.gyroscope_random_walk);
-    UINFO("accelerometer_noise_density: %f",
-      cfg.imu_calibration.accelerometer_noise_density);
-    UINFO("accelerometer_random_walk: %f",
-      cfg.imu_calibration.accelerometer_random_walk);
-    UINFO("frequency: %f",
-      cfg.imu_calibration.frequency);
-  }
-}
-
 } // namespace rtabmap
 
 #endif
@@ -199,6 +180,7 @@ OdometryCuVSLAM::OdometryCuVSLAM(const ParametersMap & parameters) :
     lost_(false),
     tracking_(false),
     planar_constraints_(false),
+    multicam_mode_(0),
     previous_pose_(Transform::getIdentity()),
     last_timestamp_(-1.0),
     observations_(5000),
@@ -212,6 +194,12 @@ OdometryCuVSLAM::OdometryCuVSLAM(const ParametersMap & parameters) :
 {
 #ifdef RTABMAP_CUVSLAM
     Parameters::parse(parameters, Parameters::kRegForce3DoF(), planar_constraints_);
+	Parameters::parse(parameters, Parameters::kOdomCuVSLAMMulticamMode(), multicam_mode_);
+    UASSERT(multicam_mode_ >= 0 && multicam_mode_ <= 2);
+	UINFO("%s=%d", Parameters::kOdomCuVSLAMMulticamMode().c_str(), multicam_mode_);
+    // Warm up GPU and create CUDA context before tracker initialization
+    // Supposedly this will speed up the tracker initialization
+    CUVSLAM_WarmUpGPU();
 #endif
 }
 
@@ -248,7 +236,14 @@ OdometryCuVSLAM::~OdometryCuVSLAM()
 void OdometryCuVSLAM::reset(const Transform & initialPose)
 {
     Odometry::reset(initialPose);
-    
+   
+#ifdef RTABMAP_CUVSLAM
+    this->cleanupCuVSLAMResources();
+#endif
+}
+
+void OdometryCuVSLAM::cleanupCuVSLAMResources()
+{
 #ifdef RTABMAP_CUVSLAM
     // Clean up cuVSLAM handles
     if(cuvslam_handle_)
@@ -300,9 +295,19 @@ Transform OdometryCuVSLAM::computeTransform(
 #ifdef RTABMAP_CUVSLAM
     UTimer timer;
 
+    UDEBUG("=== computeTransform ENTRY === lost_=%s, tracking_=%s, initialized_=%s",
+          lost_ ? "true" : "false",
+          tracking_ ? "true" : "false",
+          initialized_ ? "true" : "false");
+
     // If we are lost after tracking has begun, return null transform
     // We wait until a reset is triggered.
     if(lost_ && tracking_) {
+        UDEBUG("EARLY EXIT: lost_ && tracking_ is true, returning null");
+        if(info) {
+            info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0;
+            info->timeEstimation = timer.ticks();
+        }
         return Transform();
     }
     
@@ -330,6 +335,7 @@ Transform OdometryCuVSLAM::computeTransform(
             cuvslam_handle_,
             ground_constraint_handle_,
             planar_constraints_,
+            multicam_mode_,
             gpu_left_image_data_,
             gpu_right_image_data_,
             gpu_left_image_sizes_,
@@ -341,18 +347,8 @@ Transform OdometryCuVSLAM::computeTransform(
             UERROR("Failed to initialize cuVSLAM tracker");
             return Transform();
         }
-        
-        initialized_ = true;
-        if(info)
-        {
-            info->type = 0;
-            info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0;
-            info->timeEstimation = timer.ticks();
-        }
-        last_timestamp_ = data.stamp();
-        return Transform();
     }
-    
+        
     // Prepare images for cuVSLAM
     std::vector<CUVSLAM_Image> cuvslam_image_objects;
     if(!prepareImages(
@@ -368,10 +364,9 @@ Transform OdometryCuVSLAM::computeTransform(
         return Transform();
     }
     
-    // Process IMU data if available
+    // Not using the IMU yet
     if(!data.imu().empty())
     {
-        // TODO: Implement IMU processing
         UWARN("IMU data available but processing not implemented yet");
     }
     
@@ -394,13 +389,14 @@ Transform OdometryCuVSLAM::computeTransform(
         predicted_pose = TocuVSLAMPose(absolute_guess);
         predicted_pose_ptr = &predicted_pose;
     }
-
+    
     CUVSLAM_PoseEstimate vo_pose_estimate;
     const CUVSLAM_Status vo_status = CUVSLAM_TrackGpuMem(
         cuvslam_handle_, 
         cuvslam_image_objects.data(), 
         cuvslam_image_objects.size(), 
-        predicted_pose_ptr,  // can safely handle nullptr if no guess is provided
+        nullptr,                        // depth_image (not used in this mode)
+        predicted_pose_ptr,             // can safely handle nullptr if no guess is provided
         &vo_pose_estimate
     );
 
@@ -409,23 +405,37 @@ Transform OdometryCuVSLAM::computeTransform(
         // Provide specific error message
         const char * error_msg = "Unknown error";
         switch(vo_status) {
-            case 1: error_msg = "CUVSLAM_TRACKING_LOST"; break;
-            case 2: error_msg = "CUVSLAM_INVALID_PARAMETER"; break;
-            case 3: error_msg = "CUVSLAM_INVALID_IMAGE_FORMAT or CUVSLAM_INVALID_CAMERA_CONFIG"; break;
-            case 4: error_msg = "CUVSLAM_GPU_MEMORY_ERROR"; break;
-            case 5: error_msg = "CUVSLAM_INITIALIZATION_ERROR"; break;
-            default: error_msg = "Unknown cuVSLAM error"; break;
+            case CUVSLAM_TRACKING_LOST:                 error_msg = "CUVSLAM_TRACKING_LOST"; break;
+            case CUVSLAM_INVALID_ARG:                   error_msg = "CUVSLAM_INVALID_PARAMETER"; break;
+            case CUVSLAM_CAN_NOT_LOCALIZE:              error_msg = "CUVSLAM_CAN_NOT_LOCALIZE"; break;
+            case CUVSLAM_GENERIC_ERROR:                 error_msg = "CUVSLAM_GENERIC_ERROR"; break;
+            case CUVSLAM_UNSUPPORTED_NUMBER_OF_CAMERAS: error_msg = "CUVSLAM_UNSUPPORTED_NUMBER_OF_CAMERAS"; break;
+            case CUVSLAM_SLAM_IS_NOT_INITIALIZED:       error_msg = "CUVSLAM_SLAM_IS_NOT_INITIALIZED"; break;
+            default:                                    error_msg = "Unknown cuVSLAM error"; break;
         }
-        
-        UERROR("cuVSLAM tracking error: %d (%s)", vo_status, error_msg);
-        
+
+        // Update timing information even on failure
+        last_timestamp_ = data.stamp();
+
         if(info)
         {
+            // Report very high uncertainty to upstream consumers
             info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0;
             info->timeEstimation = timer.ticks();
         }
-    
-        last_timestamp_ = data.stamp();    
+
+        // The cuVSLAM tracking status never reports lost in my testing.
+        // Thus we use covariance to detect lost state.
+        if(vo_status == CUVSLAM_TRACKING_LOST)
+        {
+            UWARN("LOST: cuVSLAM reported CUVSLAM_TRACKING_LOST");
+            lost_ = true;
+        }
+        else
+        {
+            UWARN("cuVSLAM tracking error: %d (%s)", vo_status, error_msg);
+        }
+
         return Transform();
     }
 
@@ -434,51 +444,41 @@ Transform OdometryCuVSLAM::computeTransform(
     for(int i = 0; i < 6; i++)
     {
         float & diag_val = vo_pose_estimate.covariance[i*6+i];
-        // We allow 1.0 as a valid value, since cuVSLAM sends identity covariance for the first few frames.
-        if(!std::isfinite(diag_val) || diag_val <= 0.0 || (diag_val > 0.1 && diag_val != 1.0))
+
+        // conditions for immediate failure and tracking loss
+        if(!std::isfinite(diag_val) || diag_val < 0.0)
         {
-            diag_val = 9999.0; // Set to high uncertainty
+            diag_val = 9999.0;
             valid_covariance = false;
         }
+        // Tracker returns identity covariance and 0.0 values after initialization before motion.
+        if(std::abs(diag_val) < 1e-7f)
+        {
+            diag_val = 0.0001;
+        }
+        if(diag_val > 0.1) {
+            valid_covariance = false;
+
+            // If we don't have a guess, we can't use velocity difference to detect lost state.
+            // Thus at this point, we are lost. Warn the user that cuVSLAM probably needs a guess to work well.
+            if(guess.isNull()) {
+                UWARN("No guess provided, but covariance is invalid: %.8f", diag_val);
+                UWARN("We cannot use velocity difference to detect lost state without a guess!");
+                UWARN("Without a guess cuVSLAM is prone to getting lost easily!");
+                UWARN("It is highly recommended to provide a guess to cuVSLAM!");
+                lost_ = true;
+                if(info) {
+                    info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0;
+                    info->timeEstimation = timer.ticks();
+                }
+                return Transform();
+            }
+        }
     }
 
     // Convert to RTABMAP covariance format and scale to meet RTABMAP expectations
-    cv::Mat covMat = convertCuVSLAMCovariance(vo_pose_estimate.covariance);
-
-    // Handle invalid covariance. Protect against low velocity cases.
-    if(!valid_covariance) {
-        double velocity_ms = 9999.0;
-        double angular_velocity_rad_s = 9999.0;
-        if(!guess.isNull()) {
-            double time_s = data.stamp() - last_timestamp_;
-            velocity_ms = guess.getNorm() / time_s;
-            angular_velocity_rad_s = guess.getAngle(Transform::getIdentity()) / time_s;
-        }
-        if(velocity_ms < 0.1 && angular_velocity_rad_s < 0.1 && last_timestamp_ != -1.0) {
-            covMat = cv::Mat::eye(6, 6, CV_64FC1) * 0.0001;
-        } else {
-            // If we have already begun tracking, now we are lost.
-            if(tracking_) {
-                UWARN("LOST: Velocity is high and covariance is invalid, setting lost to true");
-                lost_ = true;
-            }
-            // Still send covariance for debugging
-            if(info) {
-                info->reg.covariance = covMat;
-            }
-            return Transform();
-        }
-    }
+    cv::Mat covMat = convertCuVSLAMCovariance(vo_pose_estimate.covariance, use_raw_covariance_);
     
-    // Tracking was successful and the covariance is valid, set tracking to true
-    tracking_ = true;
-
-    if(info)
-    {
-        info->reg.covariance = covMat;
-        info->timeEstimation = timer.ticks();
-    }
-
     // Apply ground constraint
     if(planar_constraints_) {
         if(CUVSLAM_GroundConstraintAddNextPose(ground_constraint_handle_, &vo_pose_estimate.pose) != CUVSLAM_SUCCESS) {
@@ -491,14 +491,62 @@ Transform OdometryCuVSLAM::computeTransform(
         }
     }
 
-    // Convert cuVSLAM pose to RTAB-Map Transform
+    // Convert cuVSLAM absolute pose to incremental RTAB-Map Transform
     Transform current_pose = FromcuVSLAMPose(vo_pose_estimate.pose);
     current_pose = canonical_pose_cuvslam * current_pose * cuvslam_pose_canonical;
-
-    // Calculate incremental transform
     UASSERT(!previous_pose_.isNull());
     Transform transform = previous_pose_.inverse() * current_pose;
 
+    // Compute guess and estimated velocity and report lost if velocity ratio is high and covariance is invalid
+    double time_delta_s = 0.0;
+    double guess_velocity_ms = 0.0;
+    double estimated_velocity_ms = 0.0;
+    
+    if(!guess.isNull() && last_timestamp_ > 0.0 && !use_raw_covariance_ && !valid_covariance) {
+        time_delta_s = data.stamp() - last_timestamp_;
+        
+        guess_velocity_ms = guess.getNorm() / time_delta_s;
+        estimated_velocity_ms = transform.getNorm() / time_delta_s;
+        double velocity_ratio = estimated_velocity_ms / guess_velocity_ms;
+        double velocity_difference = std::abs(estimated_velocity_ms - guess_velocity_ms);
+
+        // Check if the expected and predicted velocities are divergent.
+        // Also ensure estimated velocity is not zero.
+        // In rapid deceleration cases, estimated velocity zeros out faster then the guess but we aren't lost yet. So we need to check for this.
+        bool zero_estimated_velocity = estimated_velocity_ms < zero_estimated_velocity_threshold_; 
+        bool invalid_velocity_ratio = velocity_ratio > velocity_ratio_threshold_high_ || velocity_ratio < velocity_ratio_threshold_low_;
+        bool invalid_velocity_difference = velocity_difference > velocity_difference_threshold_;
+        
+        if(invalid_velocity_ratio && invalid_velocity_difference && !zero_estimated_velocity) {
+            UWARN("Velocity ratio is high and covariance is invalid: %.4f, returning null transform", velocity_ratio);
+            lost_ = true;
+            if(info) {
+                info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0;
+                info->timeEstimation = timer.ticks();
+            }
+            return Transform();
+        } else {
+            covMat = cv::Mat::eye(6, 6, CV_64FC1) * 0.0001;
+        }
+    }
+
+    // At this point we have passed the covariance lost checks, so we are tracking.
+    tracking_ = true;
+
+    if(info)
+    {
+        info->reg.covariance = covMat;
+        info->timeEstimation = timer.ticks();
+    }
+
+    // extract 3D VO landmarks for visualization
+    // This will be used to determine if we have enough features to start tracking.
+    CUVSLAM_LandmarkVector landmark_vector;
+    landmark_vector.max = landmarks_.size();
+    landmark_vector.landmarks = landmarks_.data();
+    CUVSLAM_Status landmark_status = CUVSLAM_GetLastLandmarks(cuvslam_handle_, &landmark_vector);
+    int landmarks_num = landmark_vector.num;
+
     // Fill info with visualization data
     if(info) {
         if(data.stereoCameraModels().size()==1) {
@@ -522,11 +570,6 @@ Transform OdometryCuVSLAM::computeTransform(
             info->type = kTypeF2M;
         }
         
-        // extract 3D VO landmarks for visualization
-        CUVSLAM_LandmarkVector landmark_vector;
-        landmark_vector.max = landmarks_.size();
-        landmark_vector.landmarks = landmarks_.data();
-        CUVSLAM_Status landmark_status = CUVSLAM_GetLastLandmarks(cuvslam_handle_, &landmark_vector);
         std::vector<Transform> local_transform_inv(data.stereoCameraModels().size());
         for(size_t i=0; i<data.stereoCameraModels().size(); ++i) {
             local_transform_inv[i] = data.stereoCameraModels()[i].localTransform().inverse();
@@ -552,11 +595,36 @@ Transform OdometryCuVSLAM::computeTransform(
                                 info->reg.inliersIDs.push_back(landmark.id);
                                 break;
                             }
+
+                            // Update landmarks number based on which landmarks were successfully reprojected in the current frame
+                            landmarks_num = info->words.size();
                         }
                     }
                 }
             }
         }
+
+        // If we are in a multi-camera setup and successfully reprojected landmarks into camera frames,
+        // use the number of successfully reprojected landmarks instead of the raw cuVSLAM landmark count.
+        if(data.stereoCameraModels().size() > 1) {
+            landmarks_num = (int)info->words.size();
+        }
+    }
+    
+    // Check if we have enough features to start tracking. Otherwise we are lost.
+    if(landmarks_num < min_landmarks_threshold_ && !initialized_) {
+        if(info) {
+            info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0;
+            info->timeEstimation = timer.ticks();
+        }
+        // Free GPU resources and reset state. Prevent memory leaks on init loops.
+        cleanupCuVSLAMResources();
+        lost_ = true;
+        tracking_ = false;
+        initialized_ = false;
+        return Transform();
+    } else {
+        initialized_ = true;
     }
 
     previous_pose_ = current_pose;
@@ -566,7 +634,7 @@ Transform OdometryCuVSLAM::computeTransform(
     UERROR("cuVSLAM support not compiled in RTAB-Map");\
     return Transform();
 #endif
-    
+   
 }
 
 #ifdef RTABMAP_CUVSLAM
@@ -579,6 +647,7 @@ bool initializeCuVSLAM(const SensorData & data,
                        CUVSLAM_TrackerHandle & cuvslam_handle,
                        CUVSLAM_GroundConstraintHandle & ground_constraint_handle,
                        bool planar_constraints,
+                       int multicam_mode,
                        std::vector<uint8_t *> & gpu_left_image_data,
                        std::vector<uint8_t *> & gpu_right_image_data,
                        std::vector<size_t> & gpu_left_image_sizes,
@@ -627,9 +696,9 @@ bool initializeCuVSLAM(const SensorData & data,
         rtabmap::Transform extrinsics = cuvslam_pose_canonical * stereoModel.localTransform() * optical_pose_cuvslam;
         cam_left.pose = TocuVSLAMPose(extrinsics);
         cam_left.border_top = 0;
-        cam_left.border_bottom = leftModel.imageHeight();
+        cam_left.border_bottom = 0;
         cam_left.border_left = 0;
-        cam_left.border_right = leftModel.imageWidth();
+        cam_left.border_right = 0;
 
         // Right camera
         cam_right.parameters = intrinsics_right.data();
@@ -649,9 +718,9 @@ bool initializeCuVSLAM(const SensorData & data,
         extrinsics = cuvslam_pose_canonical * stereoModel.localTransform() * baseline_transform * optical_pose_cuvslam;
         cam_right.pose = TocuVSLAMPose(extrinsics);
         cam_right.border_top = 0;
-        cam_right.border_bottom = rightModel.imageHeight();
+        cam_right.border_bottom = 0;
         cam_right.border_left = 0;
-        cam_right.border_right = rightModel.imageWidth();
+        cam_right.border_right = 0;
     }
     
     // Set up camera rig
@@ -659,12 +728,12 @@ bool initializeCuVSLAM(const SensorData & data,
     camera_rig.cameras = cuvslam_cameras.data();
     camera_rig.num_cameras = cuvslam_cameras.size();
 
-    const CUVSLAM_Configuration configuration = CreateConfiguration(data);
-    PrintConfiguration(configuration);
+    const CUVSLAM_Configuration configuration = CreateConfiguration(data, multicam_mode);
 
     // Create tracker
     CUVSLAM_TrackerHandle tracker_handle;
     UTimer create_timer; create_timer.start();
+    
     const CUVSLAM_Status status_tracker = CUVSLAM_CreateTracker(&tracker_handle, &camera_rig, &configuration);
     
     if (status_tracker != CUVSLAM_SUCCESS) {
@@ -706,12 +775,11 @@ Implementation based on Isaac ROS VisualSlamNode::VisualSlamImpl::CreateConfigur
 Source: isaac_ros_visual_slam/isaac_ros_visual_slam/src/impl/visual_slam_impl.cpp:379-422
 https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam/blob/19be8c781a55dee9cfbe9f097adca3986638feb1/isaac_ros_visual_slam/src/impl/visual_slam_impl.cpp#L379-L422    
 */
-CUVSLAM_Configuration CreateConfiguration(const SensorData & data)
+CUVSLAM_Configuration CreateConfiguration(const SensorData & data, int multicam_mode)
 {
     CUVSLAM_Configuration configuration;
     CUVSLAM_InitDefaultConfiguration(&configuration);
     
-    configuration.multicam_mode = data.stereoCameraModels().size()>1?1:0;
     
     // Core Visual Odometry Settings 
     configuration.use_motion_model = 1;                 // Enable motion model for better tracking
@@ -726,19 +794,12 @@ CUVSLAM_Configuration CreateConfiguration(const SensorData & data)
     configuration.enable_landmarks_export = 0;          // SLAM feature (optional)
     configuration.enable_reading_slam_internals = 0;    // SLAM feature (optional)
     
-    // IMU Configuration (If we later implement IMU support)
-    configuration.enable_imu_fusion = 0;                //data.imu().empty()?0:1;
-    configuration.debug_imu_mode = 0;                   // Disable IMU debug mode
-    // imu_calibration.gyroscope_noise_density = 0.0002f;    
-    // imu_calibration.gyroscope_random_walk = 0.00003f;      
-    // imu_calibration.accelerometer_noise_density = 0.01f;   
-    // imu_calibration.accelerometer_random_walk = 0.001f;    
-    // imu_calibration.frequency = 200.0f; 
-    // configuration.imu_calibration = imu_calibration;
-    
-    // configuration.max_frame_delta_ms = 100.0;        // Maximum frame interval (100ms default)
-    
-    // SLAM-specific parameters (disabled)
+    // Odometry configuration (Vision-only, no IMU)
+    configuration.odometry_mode = CUVSLAM_OdometryMode::Multicamera;
+    configuration.multicam_mode = multicam_mode;    // moderate (0), performance (1) or precision (2).
+    configuration.debug_imu_mode = 0;
+        
+    // SLAM parameters (disabled)
     configuration.planar_constraints = 0;
     configuration.slam_throttling_time_ms = 0;
     configuration.slam_max_map_size = 0;
@@ -907,6 +968,11 @@ bool prepareImages(const SensorData & data,
         left_cuvslam_image.camera_index = camera_index;
         left_cuvslam_image.pitch = left_image_slice.step;
         left_cuvslam_image.image_encoding = left_encoding;
+        // Mask fields (not used in this implementation)
+        left_cuvslam_image.input_mask = nullptr;
+        left_cuvslam_image.mask_width = 0;
+        left_cuvslam_image.mask_height = 0;
+        left_cuvslam_image.mask_pitch = 0;
         
         cuvslam_images.push_back(left_cuvslam_image);
 
@@ -931,6 +997,11 @@ bool prepareImages(const SensorData & data,
         right_cuvslam_image.camera_index = camera_index;
         right_cuvslam_image.pitch = right_image_slice.step;
         right_cuvslam_image.image_encoding = right_encoding;
+        // Mask fields (not used in this implementation)
+        right_cuvslam_image.input_mask = nullptr;
+        right_cuvslam_image.mask_width = 0;
+        right_cuvslam_image.mask_height = 0;
+        right_cuvslam_image.mask_pitch = 0;
 
         cuvslam_images.push_back(right_cuvslam_image);
         
@@ -953,10 +1024,11 @@ Convert cuVSLAM covariance to RTAB-Map format.
 Based on Isaac ROS implementation: FromcuVSLAMCovariance()
 Source: isaac_ros_visual_slam/src/impl/cuvslam_ros_conversion.cpp:275-299
 */
-cv::Mat convertCuVSLAMCovariance(const float * cuvslam_covariance)
+cv::Mat convertCuVSLAMCovariance(const float * cuvslam_covariance, bool use_raw_covariance)
 {
+    
     // Scale cuvslam covariance to make it more realistic
-    const double scaling_factor = 10.0;
+    const double scaling_factor = use_raw_covariance ? 1.0 : 10.0;
 
     // Handle null covariance pointer
     if(cuvslam_covariance == nullptr)
@@ -982,32 +1054,36 @@ cv::Mat convertCuVSLAMCovariance(const float * cuvslam_covariance)
     block_canonical_pose_cuvslam.block<3, 3>(0, 0) = canonical_pose_cuvslam_mat;
     block_canonical_pose_cuvslam.block<3, 3>(3, 3) = canonical_pose_cuvslam_mat;
     
-    // Map cuVSLAM covariance array to Eigen matrix
-    Eigen::Matrix<float, 6, 6> covariance_mat = 
+    // Map cuVSLAM covariance array to Eigen matrix and convert to double for numerical stability
+    Eigen::Matrix<float, 6, 6> covariance_mat_float = 
         Eigen::Map<Eigen::Matrix<float, 6, 6, Eigen::StorageOptions::AutoAlign>>(const_cast<float*>(covariance));
+    Eigen::Matrix<double, 6, 6> covariance_mat = covariance_mat_float.cast<double>();
     
-    // Reorder covariance matrix elements
+    // Reorder covariance matrix elements (in double precision)
     // The covariance matrix from cuVSLAM arranges elements as follows:
     // (rotation about X axis, rotation about Y axis, rotation about Z axis, x, y, z)
     // However, in RTAB-Map, the order is:
     // (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
-    Eigen::Matrix<float, 6, 6> rtabmap_covariance_mat = Eigen::Matrix<float, 6, 6>::Zero();
+    Eigen::Matrix<double, 6, 6> rtabmap_covariance_mat = Eigen::Matrix<double, 6, 6>::Zero();
     rtabmap_covariance_mat.block<3, 3>(0, 0) = covariance_mat.block<3, 3>(3, 3);  // translation-translation
     rtabmap_covariance_mat.block<3, 3>(0, 3) = covariance_mat.block<3, 3>(3, 0);  // translation-rotation
     rtabmap_covariance_mat.block<3, 3>(3, 0) = covariance_mat.block<3, 3>(0, 3);  // rotation-translation
     rtabmap_covariance_mat.block<3, 3>(3, 3) = covariance_mat.block<3, 3>(0, 0);  // rotation-rotation
     
-    // Apply coordinate system transformation
-    Eigen::Matrix<float, 6, 6> covariance_mat_change_basis = 
-        block_canonical_pose_cuvslam * rtabmap_covariance_mat * block_canonical_pose_cuvslam.transpose();
+    // Convert transformation matrix to double for numerical stability in matrix operations
+    Eigen::Matrix<double, 6, 6> block_canonical_pose_cuvslam_double = block_canonical_pose_cuvslam.cast<double>();
     
-    // Convert Eigen matrix to OpenCV Mat
+    // Apply coordinate system transformation (in double precision)
+    Eigen::Matrix<double, 6, 6> covariance_mat_change_basis = 
+        block_canonical_pose_cuvslam_double * rtabmap_covariance_mat * block_canonical_pose_cuvslam_double.transpose();
+    
+    // Convert Eigen matrix to OpenCV Mat (already in double precision)
     cv::Mat cv_covariance(6, 6, CV_64FC1);
     for(int i = 0; i < 6; i++)
     {
         for(int j = 0; j < 6; j++)
         {
-            cv_covariance.at<double>(i, j) = static_cast<double>(covariance_mat_change_basis(i, j));
+            cv_covariance.at<double>(i, j) = covariance_mat_change_basis(i, j);
             // for angular values, scale again to make it more realistic
             if(i > 2 || j > 2) {
                 cv_covariance.at<double>(i, j) *= scaling_factor;

corelib/src/odometry/OdometryMono.cpp

@@ -775,6 +775,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 					cameraTransform,
 					fundMatrixReprojError_,
 					fundMatrixConfidence_,
+					4,
 					refWords3Guess);  // for scale estimation
 
 			if(cameraTransform.getNorm() < minTranslation_*5)

corelib/src/odometry/OdometryORBSLAM3.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UStl.h"
 #include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
 #include <pcl/common/transforms.h>
 #include <opencv2/imgproc/types_c.h>
 #include <rtabmap/core/odometry/OdometryORBSLAM3.h>
@@ -116,6 +117,13 @@ bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap::
 	}
 	//Load ORB Vocabulary
 	vocabularyPath = uReplaceChar(vocabularyPath, '~', UDirectory::homeDir());
+	if(!UFile::exists(vocabularyPath))
+	{
+		UERROR("ORB_SLAM vocabulary path \"%s\" doesn't exist! (Parameter name=\"%s\")",
+			vocabularyPath.c_str(),
+			rtabmap::Parameters::kOdomORBSLAMVocPath().c_str());
+		return false;
+	}
 	UWARN("Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str());
 
 	// Create configuration file
@@ -240,7 +248,7 @@ bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap::
 		//# IMU Parameters TODO: hard-coded, not used
 		//#--------------------------------------------------------------------------------------------
 		//  Transformation from camera 0 to body-frame (imu)
-		rtabmap::Transform camImuT = model1.localTransform()*imuLocalTransform_;
+		rtabmap::Transform camImuT = imuLocalTransform_.inverse()*model1.localTransform();
 		ofs << "IMU.T_b_c1: !!opencv-matrix" << std::endl;
 		ofs << "   rows: 4" << std::endl;
 		ofs << "   cols: 4" << std::endl;
@@ -340,14 +348,16 @@ bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap::
 
 	ofs.close();
 
+	ORB_SLAM3::System::eSensor sensor = 
+		stereo?(withIMU?ORB_SLAM3::System::IMU_STEREO:ORB_SLAM3::System::STEREO):
+			   (withIMU?ORB_SLAM3::System::IMU_RGBD:ORB_SLAM3::System::RGBD);
+	UINFO("Initializing ORB_SLAM3 system with sensor %d...", (int)sensor);
 	orbslam_ = new ORB_SLAM3::System(
 			vocabularyPath,
 			configPath,
-			stereo && withIMU?ORB_SLAM3::System::IMU_STEREO:
-			stereo?ORB_SLAM3::System::STEREO:
-			withIMU?ORB_SLAM3::System::IMU_RGBD:
-					ORB_SLAM3::System::RGBD,
+			sensor,
 			false);
+	UINFO("Initializing ORB_SLAM3 system with sensor %d... done!", (int)sensor);
 	return true;
 #else
 	UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");
@@ -373,6 +383,7 @@ Transform OdometryORBSLAM3::computeTransform(
 		{
 			if(lastImuStamp_ == 0.0 || lastImuStamp_ < data.stamp())
 			{
+				UDEBUG("Adding IMU %f", data.stamp());
 				orbslamImus_.push_back(ORB_SLAM3::IMU::Point(
 						data.imu().linearAcceleration().val[0],
 						data.imu().linearAcceleration().val[1],
@@ -419,7 +430,14 @@ Transform OdometryORBSLAM3::computeTransform(
 		(data.stereoCameraModels().size() == 1 &&
 			data.stereoCameraModels()[0].isValidForProjection())))
 	{
-		UERROR("Invalid camera model!");
+		if(data.cameraModels().size() > 1 || data.stereoCameraModels().size() > 1)
+		{
+			UERROR("Multi-camera not supported with ORB_SLAM integration!");
+		}
+		else
+		{
+			UERROR("Invalid camera model!");
+		}
 		return t;
 	}
 
@@ -432,6 +450,7 @@ Transform OdometryORBSLAM3::computeTransform(
 		if(lastImageStamp_ == 0.0)
 		{
 			lastImageStamp_ = data.stamp();
+			UDEBUG("Waiting for another image to initialize...");
 			return t;
 		}
 
@@ -457,6 +476,7 @@ Transform OdometryORBSLAM3::computeTransform(
 			rightMono = cv::Mat();
 			cv::cvtColor(data.imageRaw(), rightMono, CV_BGR2GRAY);
 		}
+		UDEBUG("Adding Stereo Frame %f", data.stamp());
 		Tcw = orbslam_->TrackStereo(leftMono, rightMono, data.stamp(), orbslamImus_);
 		orbslamImus_.clear();
 	}
@@ -472,15 +492,22 @@ Transform OdometryORBSLAM3::computeTransform(
 		{
 			depth = util2d::cvtDepthToFloat(data.depthRaw());
 		}
+		UDEBUG("Adding RGBD Frame %f", data.stamp());
 		Tcw = orbslam_->TrackRGBD(data.imageRaw(), depth, data.stamp(), orbslamImus_);
 		orbslamImus_.clear();
 	}
 
 	Transform previousPoseInv = previousPose_.inverse();
-	std::vector<ORB_SLAM3::MapPoint*> mapPoints = orbslam_->GetTrackedMapPoints();
-	if(orbslam_->isLost() || mapPoints.empty())
+	std::vector<ORB_SLAM3::MapPoint*> trackedMapPoints = orbslam_->GetTrackedMapPoints();
+	if(orbslam_->isLost() || trackedMapPoints.empty())
 	{
 		covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
+		if(!imuLocalTransform_.isNull()) {
+			UWARN("ORBSLAM lost tracking! If it is on initialization, try moving the sensor in a circle for a couple of seconds.");
+		}
+		else {
+			UWARN("ORBSLAM lost tracking!");
+		}
 	}
 	else
 	{
@@ -490,14 +517,16 @@ Transform OdometryORBSLAM3::computeTransform(
 
 		if(!p.isNull())
 		{
-			if(!localTransform.isNull())
+			if(!imuLocalTransform_.isNull())
 			{
-				if(originLocalTransform_.isNull())
-				{
-					originLocalTransform_ = localTransform;
-				}
-				// transform in base frame
-				p = originLocalTransform_ * p.inverse() * localTransform.inverse();
+				// Transform p from optical-imu system (x->left, y->back and z->up) to ros system, then remove camera local transform
+				p = Transform(0,0,0,0,0,-M_PI/2) * p.inverse() * localTransform.inverse();
+			}
+			else
+			{				
+				UASSERT(!localTransform.isNull());
+				// Transform p from optical system (x->right, y->down and z->forward) to ros system, then remove camera local transform
+				p = CameraModel::opticalRotation() * p.inverse() * localTransform.inverse();
 			}
 			t = previousPoseInv*p;
 		}
@@ -534,12 +563,14 @@ Transform OdometryORBSLAM3::computeTransform(
 		}
 	}
 
+	size_t mapPointsSize = 0;
 	if(info)
 	{
 		info->lost = t.isNull();
 		info->type = (int)kTypeORBSLAM;
 		info->reg.covariance = covariance;
-		info->localMapSize = mapPoints.size();
+		std::vector<ORB_SLAM3::MapPoint*> mapPoints = orbslam_->GetAllMapPoints();
+		info->localMapSize = mapPointsSize = mapPoints.size();
 		info->localKeyFrames = 0;
 
 		if(this->isInfoDataFilled())
@@ -549,20 +580,20 @@ Transform OdometryORBSLAM3::computeTransform(
 			info->reg.inliersIDs.resize(kpts.size());
 			int oi = 0;
 
-			UASSERT(mapPoints.size() == kpts.size());
+			UASSERT(trackedMapPoints.size() == kpts.size());
 			for (unsigned int i = 0; i < kpts.size(); ++i)
 			{
 				int wordId;
-				if(mapPoints[i] != 0)
+				if(trackedMapPoints[i] != 0)
 				{
-					wordId = mapPoints[i]->mnId;
+					wordId = trackedMapPoints[i]->mnId;
 				}
 				else
 				{
 					wordId = -(i+1);
 				}
 				info->words.insert(std::make_pair(wordId, kpts[i]));
-				if(mapPoints[i] != 0)
+				if(trackedMapPoints[i] != 0)
 				{
 					info->reg.matchesIDs[oi] = wordId;
 					info->reg.inliersIDs[oi] = wordId;
@@ -574,7 +605,15 @@ Transform OdometryORBSLAM3::computeTransform(
 			info->reg.inliers = oi;
 			info->reg.matches = oi;
 
-			Eigen::Affine3f fixRot = (this->getPose()*previousPoseInv*originLocalTransform_).toEigen3f();
+			Eigen::Affine3f fixRot;
+			if(!imuLocalTransform_.isNull())
+			{
+				fixRot = (this->getPose()*previousPoseInv*Transform(0,0,0,0,0,-M_PI/2)).toEigen3f();
+			}
+			else
+			{
+				fixRot = (this->getPose()*previousPoseInv*CameraModel::opticalRotation()).toEigen3f();
+			}
 			for (unsigned int i = 0; i < mapPoints.size(); ++i)
 			{
 				if(mapPoints[i])
@@ -587,7 +626,8 @@ Transform OdometryORBSLAM3::computeTransform(
 		}
 	}
 
-	UINFO("Odom update time = %fs, map points=%ld, lost=%s", timer.elapsed(), mapPoints.size(), t.isNull()?"true":"false");
+	UINFO("Odom update time = %fs, tracked points=%ld, map points=%ld, lost=%s",
+		timer.elapsed(), trackedMapPoints.size(), mapPointsSize, t.isNull()?"true":"false");
 
 #else
 	UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");

corelib/src/odometry/OdometryVINSFusion.cpp

@@ -39,7 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <estimator/estimator.h>
 #include <estimator/parameters.h>
 #include <camodocal/camera_models/PinholeCamera.h>
-#include <camodocal/camera_models/EquidistantCamera.h>
+#include <camodocal/camera_models/PinholeFullCamera.h>
 #include <utility/visualization.h>
 #endif
 
@@ -49,10 +49,12 @@ namespace rtabmap {
 class VinsFusionEstimator: public Estimator
 {
 public:
-	VinsFusionEstimator(
-			const Transform & imuLocalTransform,
+	VinsFusionEstimator() : Estimator()
+	{}
+
+	bool init(const Transform & imuLocalTransform,
 			const StereoCameraModel & model,
-			bool rectified) : Estimator()
+			bool rectified)
 	{
 		MULTIPLE_THREAD = 0;
 		setParameter();
@@ -63,71 +65,133 @@ public:
 		//overwrite camera calibration only if received model is radtan, otherwise use config
 		UASSERT(NUM_OF_CAM >= 1 && NUM_OF_CAM <=2);
 
-		if( (NUM_OF_CAM == 2 && model.left().D_raw().cols == 4 && model.right().D_raw().cols == 4) ||
-			(NUM_OF_CAM == 1 && model.left().D_raw().cols == 4))
+		if( (NUM_OF_CAM == 2 && (rectified || (model.left().D_raw().cols >= 4 && model.right().D_raw().cols >= 4))) ||
+			(NUM_OF_CAM == 1 && (rectified || model.left().D_raw().cols >= 4)))
 		{
-			UWARN("Overwriting VINS camera calibration config with received pinhole model... rectified=%d", rectified?1:0);
+			UINFO("Setting up VINS camera calibration config with received pinhole model... rectified=%d distortion coefficients=%d",
+				rectified?1:0, model.left().D_raw().cols);
 			featureTracker.m_camera.clear();
 
-			camodocal::PinholeCameraPtr camera( new camodocal::PinholeCamera );
-			camodocal::PinholeCamera::Parameters params(
-					model.name(),
-					model.left().imageWidth(), model.left().imageHeight(),
-					rectified?0:model.left().D_raw().at<double>(0,0),
-					rectified?0:model.left().D_raw().at<double>(0,1),
-					rectified?0:model.left().D_raw().at<double>(0,2),
-					rectified?0:model.left().D_raw().at<double>(0,3),
-					rectified?model.left().fx():model.left().K_raw().at<double>(0,0),
-					rectified?model.left().fy():model.left().K_raw().at<double>(1,1),
-					rectified?model.left().cx():model.left().K_raw().at<double>(0,2),
-					rectified?model.left().cy():model.left().K_raw().at<double>(1,2));
-			camera->setParameters(params);
-			featureTracker.m_camera.push_back(camera);
+			double fx = 0.0;
+			if(!rectified && model.left().D_raw().cols >= 8)
+			{
+				if(model.left().D_raw().cols > 8)
+				{
+					UWARN("Received %d distortion coefficients, but only the first 8 are supported, ignoring the last coefficents.",
+						model.left().D_raw().cols);
+				}
+				camodocal::PinholeFullCameraPtr camera( new camodocal::PinholeFullCamera );
+				camodocal::PinholeFullCamera::Parameters params(
+						model.name(),
+						model.left().imageWidth(), model.left().imageHeight(),
+						model.left().D_raw().at<double>(0,0), // k1
+						model.left().D_raw().at<double>(0,1), // k1
+						model.left().D_raw().at<double>(0,4), // k3
+						model.left().D_raw().at<double>(0,5), // k4
+						model.left().D_raw().at<double>(0,6), // k5
+						model.left().D_raw().at<double>(0,7), // k6
+						model.left().D_raw().at<double>(0,2), // p1
+						model.left().D_raw().at<double>(0,3), // p1
+						model.left().K_raw().at<double>(0,0), // fx
+						model.left().K_raw().at<double>(1,1), // fy
+						model.left().K_raw().at<double>(0,2), // cx
+						model.left().K_raw().at<double>(1,2)); // cy
+				camera->setParameters(params);
+				featureTracker.m_camera.push_back(camera);
+				fx = params.fx();
+				if(NUM_OF_CAM == 2)
+				{
+					UASSERT(model.left().D_raw().cols == model.right().D_raw().cols);
+					camodocal::PinholeFullCameraPtr camera2( new camodocal::PinholeFullCamera );
+					camodocal::PinholeFullCamera::Parameters params2(
+							model.name(),
+							model.right().imageWidth(), model.right().imageHeight(),
+							model.right().D_raw().at<double>(0,0), // k1
+							model.right().D_raw().at<double>(0,1), // k2
+							model.right().D_raw().at<double>(0,4), // k3
+							model.right().D_raw().at<double>(0,5), // k4
+							model.right().D_raw().at<double>(0,6), // k5
+							model.right().D_raw().at<double>(0,7), // k6
+							model.right().D_raw().at<double>(0,2), // p1
+							model.right().D_raw().at<double>(0,3), // p2
+							model.right().K_raw().at<double>(0,0), // fx
+							model.right().K_raw().at<double>(1,1), // fy
+							model.right().K_raw().at<double>(0,2), // cx
+							model.right().K_raw().at<double>(1,2)); // cy
+					camera2->setParameters(params2);
+					featureTracker.m_camera.push_back(camera2);
+				}
+			}
+			else
+			{
+				if(!rectified)
+				{
+					if(model.left().D_raw().cols == 6) {
+						UERROR("Fisheye camera model support not implemented! Provide rectified images instead (see %s).",
+							Parameters::kRtabmapImagesAlreadyRectified().c_str());
+						return false;
+					}
+					if(model.left().D_raw().cols > 4)
+					{
+						UWARN("Received %d distortion coefficients, but only 4 or 8 are supported, ignoring the last coefficents.",
+								model.left().D_raw().cols);
+					}
+				}
+
+				camodocal::PinholeCameraPtr camera( new camodocal::PinholeCamera );
+				camodocal::PinholeCamera::Parameters params(
+						model.name(),
+						model.left().imageWidth(), model.left().imageHeight(),
+						rectified?0:model.left().D_raw().at<double>(0,0), // k1
+						rectified?0:model.left().D_raw().at<double>(0,1), // k2
+						rectified?0:model.left().D_raw().at<double>(0,2), // p1
+						rectified?0:model.left().D_raw().at<double>(0,3), // p2
+						rectified?model.left().fx():model.left().K_raw().at<double>(0,0),
+						rectified?model.left().fy():model.left().K_raw().at<double>(1,1),
+						rectified?model.left().cx():model.left().K_raw().at<double>(0,2),
+						rectified?model.left().cy():model.left().K_raw().at<double>(1,2));
+				camera->setParameters(params);
+				featureTracker.m_camera.push_back(camera);
+				fx = params.fx();
+				if(NUM_OF_CAM == 2)
+				{
+					UASSERT(model.left().D_raw().cols == model.right().D_raw().cols);
+					camodocal::PinholeCameraPtr camera2( new camodocal::PinholeCamera );
+					camodocal::PinholeCamera::Parameters params2(
+							model.name(),
+							model.right().imageWidth(), model.right().imageHeight(),
+							rectified?0:model.right().D_raw().at<double>(0,0), // k1
+							rectified?0:model.right().D_raw().at<double>(0,1), // k2
+							rectified?0:model.right().D_raw().at<double>(0,2), // p1
+							rectified?0:model.right().D_raw().at<double>(0,3), // p2
+							rectified?model.right().fx():model.right().K_raw().at<double>(0,0),
+							rectified?model.right().fy():model.right().K_raw().at<double>(1,1),
+							rectified?model.right().cx():model.right().K_raw().at<double>(0,2),
+							rectified?model.right().cy():model.right().K_raw().at<double>(1,2));
+					camera2->setParameters(params2);
+					featureTracker.m_camera.push_back(camera2);
+				}
+			}
 
 			double originalParalax = MIN_PARALLAX * FOCAL_LENGTH;
-			// If you have compiler error about FOCAL_LENGTH being const, make sure to use the following patch:
+			// If you have compiler error about FOCAL_LENGTH being const, make sure to use the following patch for ROS1:
 			// https://gist.github.com/matlabbe/795ab37067367dca58bbadd8201d986c#file-vins-fusion_pull136-patch
-			FOCAL_LENGTH = params.fx();
+			// Use this patch for ROS2: https://gist.github.com/matlabbe/ebbb343cd744da9d6d6d6ded2e1557fd
+			FOCAL_LENGTH = fx;
 			MIN_PARALLAX = originalParalax / FOCAL_LENGTH;
 			ProjectionTwoFrameOneCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
 			ProjectionTwoFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
 			ProjectionOneFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
-
-			if(NUM_OF_CAM == 2)
-			{
-				camodocal::PinholeCameraPtr camera( new camodocal::PinholeCamera );
-				camodocal::PinholeCamera::Parameters params(
-						model.name(),
-						model.right().imageWidth(), model.right().imageHeight(),
-						rectified?0:model.right().D_raw().at<double>(0,0),
-						rectified?0:model.right().D_raw().at<double>(0,1),
-						rectified?0:model.right().D_raw().at<double>(0,2),
-						rectified?0:model.right().D_raw().at<double>(0,3),
-						rectified?model.right().fx():model.right().K_raw().at<double>(0,0),
-						rectified?model.right().fy():model.right().K_raw().at<double>(1,1),
-						rectified?model.right().cx():model.right().K_raw().at<double>(0,2),
-						rectified?model.right().cy():model.right().K_raw().at<double>(1,2));
-				camera->setParameters(params);
-				featureTracker.m_camera.push_back(camera);
-			}
 		}
-		else if(rectified)
+		else
 		{
-			UWARN("Images are rectified but received calibration cannot be "
-					"used, make sure calibration in config file doesn't have "
-					"distortion or send raw images to VINS odometry.");
-			if(!featureTracker.m_camera.empty())
-			{
-				if(featureTracker.m_camera.front()->imageWidth() != model.left().imageWidth() ||
-				   featureTracker.m_camera.front()->imageHeight() != model.left().imageHeight())
-				{
-					UERROR("Received images don't have same size (%dx%d) than in the config file (%dx%d)!",
-							model.left().imageWidth(),
-							model.left().imageHeight(),
-							featureTracker.m_camera.front()->imageWidth(),
-							featureTracker.m_camera.front()->imageHeight());
-				}
+			UERROR("Received stereo camera model is not compatible with VINS-Fusion.");
+			if(!rectified && model.left().D_raw().cols != 4) {
+				UERROR("When raw images are provided (%s=false), we expect 4 distortion coefficients (k1,k2,p1,p2), received %d",
+					Parameters::kRtabmapImagesAlreadyRectified().c_str(),
+					model.left().D_raw().cols);
 			}
+			return false;
 		}
 
 		Transform imuCam0 = imuLocalTransform.inverse() * model.localTransform();
@@ -165,6 +229,7 @@ public:
 			cout << " new intrinsic cam " << i << endl  << featureTracker.m_camera[i]->parametersToString() << endl;
 		}
 		f_manager.setRic(ric);
+		return true;
 	}
 
 	// Copy of original inputImage() so that overridden processMeasurements() is used and threading is disabled.
@@ -350,10 +415,16 @@ Transform OdometryVINSFusion::computeTransform(
 		{
 			// intialize
 			UINFO("Initializing with image %f", data.stamp());
-			vinsEstimator_ = new VinsFusionEstimator(
-					lastImu_.localTransform().isNull()?Transform::getIdentity():lastImu_.localTransform(),
-					data.stereoCameraModels()[0],
-					this->imagesAlreadyRectified());
+			vinsEstimator_ = new VinsFusionEstimator();
+			if(!vinsEstimator_->init(
+				lastImu_.localTransform().isNull()?Transform::getIdentity():lastImu_.localTransform(),
+				data.stereoCameraModels()[0],
+				this->imagesAlreadyRectified()))
+			{
+				delete vinsEstimator_;
+				vinsEstimator_ = 0;
+				return Transform();
+			}
 
 			if(USE_IMU) {
 				double dx = lastImu_.linearAcceleration().val[0];

corelib/src/optimizer/OptimizerG2O.cpp

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

corelib/src/optimizer/OptimizerTORO.cpp

@@ -543,7 +543,7 @@ bool OptimizerTORO::loadGraph(
 				}
 				else
 				{
-					UFATAL("Referred poses from the link not exist!");
+					UERROR("Referred poses from the link (%d->%d) don't exist! Link ignored!", idFrom, idTo);
 				}
 			}
 			else if(strList.size())

corelib/src/optimizer/ceres/pose_graph_2d/angle_manifold.h

@@ -77,7 +77,7 @@ class AngleManifold {
 
 #else
 
-class AngleManfold {
+class AngleManifold {
  public:
 
   template <typename T>
@@ -90,7 +90,7 @@ class AngleManfold {
   }
 
   static ceres::LocalParameterization* Create() {
-    return (new ceres::AutoDiffLocalParameterization<AngleManfold, 1, 1>);
+    return (new ceres::AutoDiffLocalParameterization<AngleManifold, 1, 1>);
   }
 };
 

corelib/src/optimizer/g2o/edge_sbacam_gravity.h

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

corelib/src/optimizer/g2o/edge_sbacam_prior.h

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

corelib/src/optimizer/g2o/edge_se3_expmap.h

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

corelib/src/optimizer/gtsam/GravityFactor.h

@@ -25,6 +25,9 @@
 #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>
 

corelib/src/python/PyDetector.cpp

@@ -1,230 +1,242 @@
-/**
- * Python interface for SuperGlue: https://github.com/magicleap/SuperGluePretrainedNetwork
- */
-
-#include "PyDetector.h"
-#include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/utilite/UDirectory.h>
-#include <rtabmap/utilite/UFile.h>
-#include <rtabmap/utilite/UStl.h>
-#include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/utilite/UTimer.h>
-
-#include <pybind11/embed.h>
-
-#define NPY_NO_DEPRECATED_API NPY_API_VERSION
-#include <numpy/arrayobject.h>
-
-namespace rtabmap
-{
-
-PyDetector::PyDetector(const ParametersMap & parameters) :
-		pModule_(0),
-		pFunc_(0),
-		path_(Parameters::defaultPyDetectorPath()),
-		cuda_(Parameters::defaultPyDetectorCuda())
-{
-	this->parseParameters(parameters);
-
-	UDEBUG("path = %s", path_.c_str());
-	if(!UFile::exists(path_) || UFile::getExtension(path_).compare("py") != 0)
-	{
-		UERROR("Cannot initialize Python detector, the path is not valid: \"%s\"=\"%s\"",
-				Parameters::kPyDetectorPath().c_str(), path_.c_str());
-		return;
-	}
-
-	pybind11::gil_scoped_acquire acquire;
-	
-	std::string matcherPythonDir = UDirectory::getDir(path_);
-	if(!matcherPythonDir.empty())
-	{
-		PyRun_SimpleString("import sys");
-		PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str());
-	}
-
-	_import_array();
-
-	std::string scriptName = uSplit(UFile::getName(path_), '.').front();
-	PyObject * pName = PyUnicode_FromString(scriptName.c_str());
-	UDEBUG("PyImport_Import() beg");
-	pModule_ = PyImport_Import(pName);
-	UDEBUG("PyImport_Import() end");
-
-	Py_DECREF(pName);
-
-	if(!pModule_)
-	{
-		UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str());
-		UERROR("%s", getPythonTraceback().c_str());
-	}
-}
-
-PyDetector::~PyDetector()
-{
-	pybind11::gil_scoped_acquire acquire;
-
-	if(pFunc_)
-	{
-		Py_DECREF(pFunc_);
-	}
-	if(pModule_)
-	{
-		Py_DECREF(pModule_);
-	}
-}
-
-void PyDetector::parseParameters(const ParametersMap & parameters)
-{
-	Feature2D::parseParameters(parameters);
-
-	Parameters::parse(parameters, Parameters::kPyDetectorPath(), path_);
-	Parameters::parse(parameters, Parameters::kPyDetectorCuda(), cuda_);
-
-	path_ = uReplaceChar(path_, '~', UDirectory::homeDir());
-}
-
-std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
-{
-	UDEBUG("");
-	descriptors_ = cv::Mat();
-	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
-	std::vector<cv::KeyPoint> keypoints;
-	cv::Mat imgRoi(image, roi);
-
-	UTimer timer;
-
-	if(!pModule_)
-	{
-		UERROR("Python detector module not loaded!");
-		return keypoints;
-	}
-
-	pybind11::gil_scoped_acquire acquire;
-
-	if(!pFunc_)
-	{
-		PyObject * pFunc = PyObject_GetAttrString(pModule_, "init");
-		if(pFunc)
-		{
-			if(PyCallable_Check(pFunc))
-			{
-				PyObject * result = PyObject_CallFunction(pFunc, "i", cuda_?1:0);
-
-				if(result == NULL)
-				{
-					UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str());
-					UERROR("%s", getPythonTraceback().c_str());
-					return keypoints;
-				}
-				Py_DECREF(result);
-
-				pFunc_ = PyObject_GetAttrString(pModule_, "detect");
-				if(pFunc_ && PyCallable_Check(pFunc_))
-				{
-					// we are ready!
-				}
-				else
-				{
-					UERROR("Cannot find method \"detect(...)\" in %s", path_.c_str());
-					UERROR("%s", getPythonTraceback().c_str());
-					if(pFunc_)
-					{
-						Py_DECREF(pFunc_);
-						pFunc_ = 0;
-					}
-					return keypoints;
-				}
-			}
-			else
-			{
-				UERROR("Cannot call method \"init(...)\" in %s", path_.c_str());
-				UERROR("%s", getPythonTraceback().c_str());
-				return keypoints;
-			}
-			Py_DECREF(pFunc);
-		}
-		else
-		{
-			UERROR("Cannot find method \"init(...)\"");
-			UERROR("%s", getPythonTraceback().c_str());
-			return keypoints;
-		}
-		UDEBUG("init time = %fs", timer.ticks());
-	}
-
-	if(pFunc_)
-	{
-		npy_intp dims[2] = {imgRoi.rows, imgRoi.cols};
-		PyObject* pImageBuffer = PyArray_SimpleNewFromData(2, dims, NPY_UBYTE, (void*)imgRoi.data);
-		UASSERT(pImageBuffer);
-
-		UDEBUG("Preparing data time = %fs", timer.ticks());
-
-		PyObject *pReturn = PyObject_CallFunctionObjArgs(pFunc_, pImageBuffer, NULL);
-		if(pReturn == NULL)
-		{
-			UERROR("Failed to call match() function!");
-			UERROR("%s", getPythonTraceback().c_str());
-		}
-		else
-		{
-			UDEBUG("Python detector time = %fs", timer.ticks());
-
-			if (PyTuple_Check(pReturn) && PyTuple_GET_SIZE(pReturn) == 2)
-			{
-				PyObject *kptsPtr = PyTuple_GET_ITEM(pReturn, 0);
-				PyObject *descPtr = PyTuple_GET_ITEM(pReturn, 1);
-				if(PyArray_Check(kptsPtr) && PyArray_Check(descPtr))
-				{
-					PyArrayObject *arrayPtr = reinterpret_cast<PyArrayObject*>(kptsPtr);
-					int nKpts = PyArray_SHAPE(arrayPtr)[0];
-					int kptSize = PyArray_SHAPE(arrayPtr)[1];
-					int type = PyArray_TYPE(arrayPtr);
-					UDEBUG("Kpts array %dx%d (type=%d)", nKpts, kptSize, type);
-					UASSERT(kptSize == 3);
-					UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned matches should type FLOAT=11, received type=%d", type).c_str());
-
-					float* c_out = reinterpret_cast<float*>(PyArray_DATA(arrayPtr));
-					keypoints.reserve(nKpts);
-					for (int i = 0; i < nKpts*kptSize; i+=kptSize)
-					{
-						cv::KeyPoint kpt(c_out[i], c_out[i+1], 8, -1, c_out[i+2]);
-						keypoints.push_back(kpt);
-					}
-
-					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; i < nDesc*dim; i+=dim)
-					{
-						cv::Mat descriptor = cv::Mat(1, dim, CV_32FC1, &c_out[i]).clone();
-						descriptors_.push_back(descriptor);
-					}
-				}
-			}
-			else
-			{
-				UWARN("Expected tuple (Kpts 3 x N, Descriptors dim x N), returning empty features.");
-			}
-			Py_DECREF(pReturn);
-		}
-		Py_DECREF(pImageBuffer);
-	}
-
-	return keypoints;
-}
-
-cv::Mat PyDetector::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
-{
-	UASSERT((int)keypoints.size() == descriptors_.rows);
-	return descriptors_;
-}
-
-}
+/**
+ * Python interface for SuperGlue: https://github.com/magicleap/SuperGluePretrainedNetwork
+ */
+
+#include "PyDetector.h"
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#include <pybind11/embed.h>
+
+#define NPY_NO_DEPRECATED_API NPY_API_VERSION
+#include <numpy/arrayobject.h>
+
+namespace rtabmap
+{
+
+PyDetector::PyDetector(const ParametersMap & parameters) :
+		pModule_(0),
+		pFunc_(0),
+		path_(Parameters::defaultPyDetectorPath()),
+		cuda_(Parameters::defaultPyDetectorCuda())
+{
+	this->parseParameters(parameters);
+
+	UDEBUG("path = %s", path_.c_str());
+	if(!UFile::exists(path_) || UFile::getExtension(path_).compare("py") != 0)
+	{
+		UERROR("Cannot initialize Python detector, the path is not valid: \"%s\"=\"%s\"",
+				Parameters::kPyDetectorPath().c_str(), path_.c_str());
+		return;
+	}
+
+	pybind11::gil_scoped_acquire acquire;
+	
+	std::string matcherPythonDir = UDirectory::getDir(path_);
+	if(!matcherPythonDir.empty())
+	{
+		PyRun_SimpleString("import sys");
+		PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str());
+	}
+
+	_import_array();
+
+	std::string scriptName = uSplit(UFile::getName(path_), '.').front();
+	PyObject * pName = PyUnicode_FromString(scriptName.c_str());
+	UDEBUG("PyImport_Import() beg");
+	pModule_ = PyImport_Import(pName);
+	UDEBUG("PyImport_Import() end");
+
+	Py_DECREF(pName);
+
+	if(!pModule_)
+	{
+		UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str());
+		UERROR("%s", getPythonTraceback().c_str());
+	}
+}
+
+PyDetector::~PyDetector()
+{
+	pybind11::gil_scoped_acquire acquire;
+
+	if(pFunc_)
+	{
+		Py_DECREF(pFunc_);
+	}
+	if(pModule_)
+	{
+		Py_DECREF(pModule_);
+	}
+}
+
+void PyDetector::parseParameters(const ParametersMap & parameters)
+{
+	Feature2D::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kPyDetectorPath(), path_);
+	Parameters::parse(parameters, Parameters::kPyDetectorCuda(), cuda_);
+
+	path_ = uReplaceChar(path_, '~', UDirectory::homeDir());
+}
+
+std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
+{
+	UDEBUG("");
+	descriptors_ = cv::Mat();
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	std::vector<cv::KeyPoint> keypoints;
+	cv::Mat imgRoi(image, roi);
+
+	UTimer timer;
+
+	if(!pModule_)
+	{
+		UERROR("Python detector module not loaded!");
+		return keypoints;
+	}
+
+	pybind11::gil_scoped_acquire acquire;
+
+	if(!pFunc_)
+	{
+		PyObject * pFunc = PyObject_GetAttrString(pModule_, "init");
+		if(pFunc)
+		{
+			if(PyCallable_Check(pFunc))
+			{
+				PyObject * result = PyObject_CallFunction(pFunc, "i", cuda_?1:0);
+
+				if(result == NULL)
+				{
+					UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str());
+					UERROR("%s", getPythonTraceback().c_str());
+					return keypoints;
+				}
+				Py_DECREF(result);
+
+				pFunc_ = PyObject_GetAttrString(pModule_, "detect");
+				if(pFunc_ && PyCallable_Check(pFunc_))
+				{
+					// we are ready!
+				}
+				else
+				{
+					UERROR("Cannot find method \"detect(...)\" in %s", path_.c_str());
+					UERROR("%s", getPythonTraceback().c_str());
+					if(pFunc_)
+					{
+						Py_DECREF(pFunc_);
+						pFunc_ = 0;
+					}
+					return keypoints;
+				}
+			}
+			else
+			{
+				UERROR("Cannot call method \"init(...)\" in %s", path_.c_str());
+				UERROR("%s", getPythonTraceback().c_str());
+				return keypoints;
+			}
+			Py_DECREF(pFunc);
+		}
+		else
+		{
+			UERROR("Cannot find method \"init(...)\"");
+			UERROR("%s", getPythonTraceback().c_str());
+			return keypoints;
+		}
+		UDEBUG("init time = %fs", timer.ticks());
+	}
+
+	if(pFunc_)
+	{
+		npy_intp dims[2] = {imgRoi.rows, imgRoi.cols};
+		PyObject * pImageBuffer = PyArray_SimpleNewFromData(2, dims, NPY_UBYTE, (void*)imgRoi.data);
+		UASSERT(pImageBuffer);
+
+		UDEBUG("Preparing data time = %fs", timer.ticks());
+
+		PyObject * pReturn = PyObject_CallFunctionObjArgs(pFunc_, pImageBuffer, NULL);
+		if(pReturn == NULL)
+		{
+			UERROR("Failed to call match() function!");
+			UERROR("%s", getPythonTraceback().c_str());
+		}
+		else
+		{
+			UDEBUG("Python detector time = %fs", timer.ticks());
+
+			if (PyTuple_Check(pReturn) && PyTuple_GET_SIZE(pReturn) == 2)
+			{
+				PyObject * kptsPtr = PyTuple_GET_ITEM(pReturn, 0);
+				PyObject * descPtr = PyTuple_GET_ITEM(pReturn, 1);
+				if(PyArray_Check(kptsPtr) && PyArray_Check(descPtr))
+				{
+					PyArrayObject *arrayPtr = reinterpret_cast<PyArrayObject*>(kptsPtr);
+					int nKpts = PyArray_SHAPE(arrayPtr)[0];
+					int kptSize = PyArray_SHAPE(arrayPtr)[1];
+					int type = PyArray_TYPE(arrayPtr);
+					UDEBUG("Kpts array %dx%d (type=%d)", nKpts, kptSize, type);
+					UASSERT(kptSize == 3);
+					UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned matches should type FLOAT=11, received type=%d", type).c_str());
+
+					float* c_out = reinterpret_cast<float*>(PyArray_DATA(arrayPtr));
+					std::vector<bool> keep_kpt(nKpts);
+					keypoints.reserve(nKpts);
+					for (int i = 0, kpt_idx = 0; i < nKpts*kptSize; i+=kptSize, kpt_idx++)
+					{
+						// x,y in full image coordinates. Mask is in full image coordinates too.
+						int full_x = (int)(c_out[i] + roi.x);
+						int full_y = (int)(c_out[i+1] + roi.y);
+						keep_kpt[kpt_idx] = mask.empty() || (full_x >= 0 && full_x < mask.cols && full_y >= 0 && full_y < mask.rows && mask.at<unsigned char>(full_y, full_x) != 0);
+						if(keep_kpt[kpt_idx]) {
+							cv::KeyPoint kpt(c_out[i], c_out[i+1], 8, -1, c_out[i+2]);
+							keypoints.push_back(kpt);
+						}
+					}
+
+					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(keep_kpt[kpt_idx]) {
+							cv::Mat descriptor = cv::Mat(1, dim, CV_32FC1, &c_out[i]).clone();
+							descriptors_.push_back(descriptor);
+						}
+					}
+				}
+			}
+			else
+			{
+				UWARN("Expected tuple (Kpts 3 x N, Descriptors dim x N), returning empty features.");
+			}
+			Py_DECREF(pReturn);
+		}
+		Py_DECREF(pImageBuffer);
+	}
+
+	// Apply limitKeypoints to enforce maxFeatures and SSC
+	this->limitKeypoints(keypoints, descriptors_, this->getMaxFeatures(), cv::Size(roi.width, roi.height), this->getSSC());
+
+	return keypoints;
+}
+
+cv::Mat PyDetector::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+	UASSERT((int)keypoints.size() == descriptors_.rows);
+	return descriptors_;
+}
+
+}

corelib/src/python/PyMatcher.cpp

@@ -228,16 +228,32 @@ std::vector<cv::DMatch> PyMatcher::match(
 				int len2 = PyArray_SHAPE(np_ret)[1];
 				int type = PyArray_TYPE(np_ret);
 				UDEBUG("Matches array %dx%d (type=%d)", len1, len2, type);
-				UASSERT_MSG(type == NPY_LONG || type == NPY_INT, uFormat("Returned matches should type INT=5 or LONG=7, received type=%d", type).c_str());
-				if(type == NPY_LONG)
+				UASSERT_MSG(type == NPY_INT32 || type == NPY_UINT32 || type == NPY_INT64 || type == NPY_UINT64, uFormat("Returned matches should type INT32=%d UINT32=%d, INT64=%d or UINT64=%d, received type=%d", NPY_INT, NPY_UINT32, NPY_INT64, NPY_UINT64, type).c_str());
+				if(type == NPY_UINT64)
 				{
-					long* c_out = reinterpret_cast<long*>(PyArray_DATA(np_ret));
+					long long* c_out = reinterpret_cast<long long*>(PyArray_DATA(np_ret));
 					for (int i = 0; i < len1*len2; i+=2)
 					{
 						matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0));
 					}
 				}
-				else // INT
+				if(type == NPY_INT64)
+				{
+					unsigned long long* c_out = reinterpret_cast<unsigned long long*>(PyArray_DATA(np_ret));
+					for (int i = 0; i < len1*len2; i+=2)
+					{
+						matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0));
+					}
+				}
+				else if(type == NPY_UINT32)
+				{
+					unsigned int* c_out = reinterpret_cast<unsigned int*>(PyArray_DATA(np_ret));
+					for (int i = 0; i < len1*len2; i+=2)
+					{
+						matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0));
+					}
+				}
+				else // NPY_INT
 				{
 					int* c_out = reinterpret_cast<int*>(PyArray_DATA(np_ret));
 					for (int i = 0; i < len1*len2; i+=2)

corelib/src/python/PythonInterface.cpp

@@ -9,6 +9,7 @@
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UThread.h>
 #include <pybind11/embed.h>
+#include <filesystem>
 
 namespace rtabmap {
 
@@ -16,6 +17,12 @@ PythonInterface::PythonInterface()
 {
 	UINFO("Initialize python interpreter");
 	guard_ = new pybind11::scoped_interpreter();
+
+	// Tell Python to look in this directory for DLLs
+	std::string exe_dir = std::filesystem::current_path().string();
+    pybind11::module_ os = pybind11::module_::import("os");
+    os.attr("add_dll_directory")(exe_dir);
+
 	pybind11::module::import("threading");
 	release_ = new pybind11::gil_scoped_release();
 }

corelib/src/python/rtabmap_superglue.py

@@ -38,7 +38,6 @@ def init(descriptorDim, matchThreshold, iterations, cuda, model):
     global superglue
     superglue = SuperGlue(config.get('superglue', {})).eval().to(device)
 
-
 def match(kptsFrom, kptsTo, scoresFrom, scoresTo, descriptorsFrom, descriptorsTo, imageWidth, imageHeight):
     #print("SuperGlue python match()")
     global device
@@ -77,6 +76,8 @@ def match(kptsFrom, kptsTo, scoresFrom, scoresTo, descriptorsFrom, descriptorsTo
        
     matchesArray = np.stack((matchesFrom, matchesTo), axis=1);
     
+    # rtabmap expects format:
+    #   matches: array Nx2 (type=9 or uint64)
     return matchesArray
 
 

corelib/src/python/rtabmap_superpoint.py

@@ -8,6 +8,7 @@
 import random
 import numpy as np
 import torch
+import os
 
 #import sys
 #import os
@@ -21,15 +22,19 @@ torch.set_grad_enabled(False)
 device = 'cpu'
 superpoint = []
 
+script_dir = os.path.dirname(os.path.abspath(__file__))
+
 def init(cuda):
     #print("SuperPoint python init()")
     
     global device
     device = 'cuda' if torch.cuda.is_available() and cuda else 'cpu'
     
+    weights_abs_path = os.path.join(script_dir, "superpoint_v1.pth")
+    
     # This class runs the SuperPoint network and processes its outputs.
     global superpoint
-    superpoint = SuperPointFrontend(weights_path="superpoint_v1.pth",
+    superpoint = SuperPointFrontend(weights_path=weights_abs_path,
                           nms_dist=4,
                           conf_thresh=0.015,
                           nn_thresh=1,
@@ -47,10 +52,14 @@ def detect(imageBuffer):
     # use copy to make sure memory is correctly re-ordered
     pts = np.float32(np.transpose(pts)).copy()
     desc = np.float32(np.transpose(desc)).copy()
+    
+    # rtabmap expects format:
+    #   pts:         array Nx3 (type=11 or float)
+    #   descriptors: array NxDIM 35x256 (type=11 or float)
     return pts, desc
 
 
 if __name__ == '__main__':
     #test
-    init(True)
+    init(False)
     detect(np.random.rand(640,480)*255)

corelib/src/python/rtabmap_superpoint_rpautrat.py

@@ -0,0 +1,71 @@
+#! /usr/bin/env python3
+#
+# Drop this file in the root folder of SuperPoint git: https://github.com/rpautrat/SuperPoint
+# To use with rtabmap:
+#   --Vis/FeatureType 15 --Kp/DetectorStrategy 15 --PyDetector/Path "~/SuperPoint/rtabmap_superpoint_rpautrat.py"
+#
+import numpy as np
+import os
+import torch
+from superpoint_pytorch import SuperPoint
+
+superpoint = []
+device = 'cpu'
+
+def init(cuda):
+    global superpoint, device
+
+    superpoint = SuperPoint().eval()
+
+    # set up device, gpu or cpu depending on the availability and the user's choice
+    device = 'cuda' if torch.cuda.is_available() and cuda else 'cpu'
+    
+    # Load weights directly to target device
+    # Get the directory where this script is located
+    script_dir = os.path.dirname(os.path.abspath(__file__))
+    weights_path = os.path.join(script_dir, 'weights', 'superpoint_v6_from_tf.pth')
+    
+    # Load model weights with proper error handling
+    try:
+        state_dict = torch.load(weights_path, map_location=device, weights_only=True)
+        superpoint.load_state_dict(state_dict)
+    except Exception as e:
+        print(f"Error loading weights: {e}")
+        raise
+    
+    # Move the model to the target device
+    superpoint.to(device)
+    
+    # Ensure model is in eval mode for inference
+    superpoint.eval()
+
+def detect(imageBuffer):
+    global superpoint, device
+    
+    image = np.asarray(imageBuffer)
+    image = (image.astype('float32') / 255.)
+    
+    try:
+        image_with_dims = image[None, None]  # Add batch and channel dims
+        image_tensor = torch.from_numpy(image_with_dims).float()
+        image_tensor = image_tensor.to(device)
+    except Exception as e:
+        print(f"Error creating tensor: {e}")
+        raise
+    # Result: (1, 1, H, W) - PyTorch tensor on correct device (CPU or GPU).
+    
+    with torch.no_grad():
+        pred = superpoint({'image': image_tensor})
+    
+    # Extract keypoints and descriptors
+    keypoints = pred['keypoints'][0].cpu().numpy()  # Shape: (N, 2)
+    keypoints_response = pred['keypoint_scores'][0].cpu().numpy()
+    keypoints_with_response = np.column_stack([keypoints, keypoints_response]).astype(np.float32)
+    # Result: (N, 3) with [x, y, response]
+
+    descriptors = pred['descriptors'][0].cpu().numpy()  
+    # Result: (N, descriptor_dim)    
+
+    desc = np.float32(descriptors).copy()
+    pts = np.float32(keypoints_with_response).copy()
+    return pts, desc

corelib/src/python/rtabmap_trace_superpoint.py

@@ -0,0 +1,13 @@
+import os
+import sys
+from pathlib import Path
+
+import torch
+import torchvision
+from demo_superpoint  import SuperPointNet
+model = SuperPointNet()
+model.load_state_dict(torch.load("superpoint_v1.pth"))
+model.eval()
+example = torch.rand(1, 1, 640, 480)
+traced_script_module = torch.jit.trace(model, example, check_trace=False)
+traced_script_module.save("superpoint_v1.pt")

corelib/src/resources/DatabaseSchema.sql.in

@@ -24,6 +24,7 @@ CREATE TABLE Node (
 	label TEXT,
 	gps BLOB,                 -- 1x6 double: stamp, longitude (DD), latitude (DD), altitude (m), accuracy (m), bearing (North 0->360 deg clockwise)
 	env_sensors BLOB,         -- Variable 3xdouble: (sensorId1, value, stamp, sensorId2, value, stamp, ...)
+	features BLOB,            -- compressed serialized data (pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor)
 	time_enter DATE,
 	PRIMARY KEY (id)
 );
@@ -45,7 +46,7 @@ CREATE TABLE Data (
 	view_point_x FLOAT,
 	view_point_y FLOAT,
 	view_point_z FLOAT,
-	
+
 	user_data BLOB,           -- compressed data (User data)
 	time_enter DATE,
 	PRIMARY KEY (id)
@@ -74,17 +75,7 @@ CREATE TABLE Word (
 CREATE TABLE Feature (
 	node_id INTEGER NOT NULL,
 	word_id INTEGER NOT NULL,
-	pos_x FLOAT NOT NULL,
-	pos_y FLOAT NOT NULL,
-	size INTEGER NOT NULL,
-	dir FLOAT NOT NULL,
-	response FLOAT NOT NULL,
-	octave INTEGER NOT NULL,
-	depth_x FLOAT,
-	depth_y FLOAT,
-	depth_z FLOAT,
-	descriptor_size INTEGER,
-	descriptor BLOB,
+	feature_index INTEGER NOT NULL,   -- index of the feature in "features" field of Node
 	FOREIGN KEY (node_id) REFERENCES Node(id)
 );
 

corelib/src/resources/backward_compatibility/DatabaseSchema_0_23_0.sql

@@ -0,0 +1,185 @@
+-- *******************************************************************
+--  DatabaseSchema: Script for creating the database
+--   Usage:
+--       $ sqlite3 LTM.db < DatabaseSchema.sql
+--
+-- *******************************************************************
+
+-- *******************************************************************
+-- CLEAN
+-- *******************************************************************
+/*DROP TABLE Node;*/
+
+-- *******************************************************************
+-- CREATE
+-- *******************************************************************
+CREATE TABLE Node (
+	id INTEGER NOT NULL,
+	map_id INTEGER NOT NULL,
+	weight INTEGER,
+	stamp FLOAT,
+	pose BLOB,                -- 3x4 float
+	ground_truth_pose BLOB,   -- 3x4 float
+	velocity BLOB,            -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s
+	label TEXT,
+	gps BLOB,                 -- 1x6 double: stamp, longitude (DD), latitude (DD), altitude (m), accuracy (m), bearing (North 0->360 deg clockwise)
+	env_sensors BLOB,         -- Variable 3xdouble: (sensorId1, value, stamp, sensorId2, value, stamp, ...)
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Data (
+	id INTEGER NOT NULL,
+	image BLOB,               -- compressed image (Grayscale or RGB)
+	depth BLOB,               -- compressed image (Depth or Right image)
+	depth_confidence BLOB,    -- compressed data (low=0 high=100)
+	calibration BLOB,         -- fx, fy, cx, cy, [baseline,] width, height, local_transform
+	
+	scan BLOB,                -- compressed data (Laser scan)
+	scan_info BLOB,           -- scan_max_pts, scan_max_range, scan_format, local_transform
+	
+	ground_cells BLOB,        -- compressed data (occupancy grid)
+	obstacle_cells BLOB,      -- compressed data (occupancy grid)
+	empty_cells BLOB,         -- compressed data (occupancy grid)
+	cell_size FLOAT,
+	view_point_x FLOAT,
+	view_point_y FLOAT,
+	view_point_z FLOAT,
+	
+	user_data BLOB,           -- compressed data (User data)
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Link (
+	from_id INTEGER NOT NULL,
+	to_id INTEGER NOT NULL,
+	type INTEGER NOT NULL,    -- kNeighbor=0, kGlobalClosure=1, kLocalSpaceClosure=2, kLocalTimeClosure=3, kUserClosure=4, kVirtualClosure=5, kNeighborMerged=6, kPosePrior=7, kLandmark=8
+	information_matrix BLOB NOT NULL, -- 6x6 double (inverse covariance)
+	transform BLOB,           -- 3x4 float
+	user_data BLOB,           -- compressed data (User data)
+	FOREIGN KEY (from_id) REFERENCES Node(id),
+	FOREIGN KEY (to_id) REFERENCES Node(id)
+);
+
+-- 
+CREATE TABLE Word (
+	id INTEGER NOT NULL,
+	descriptor_size INTEGER NOT NULL,
+	descriptor BLOB NOT NULL,
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Feature (
+	node_id INTEGER NOT NULL,
+	word_id INTEGER NOT NULL,
+	pos_x FLOAT NOT NULL,
+	pos_y FLOAT NOT NULL,
+	size INTEGER NOT NULL,
+	dir FLOAT NOT NULL,
+	response FLOAT NOT NULL,
+	octave INTEGER NOT NULL,
+	depth_x FLOAT,
+	depth_y FLOAT,
+	depth_z FLOAT,
+	descriptor_size INTEGER,
+	descriptor BLOB,
+	FOREIGN KEY (node_id) REFERENCES Node(id)
+);
+
+CREATE TABLE GlobalDescriptor (
+	node_id INTEGER NOT NULL,
+	type INTEGER NOT NULL,
+	info BLOB,
+	data BLOB NOT NULL,
+	FOREIGN KEY (node_id) REFERENCES Node(id)
+);
+
+--
+
+CREATE TABLE Info (
+	STM_size INTEGER,
+	last_sign_added INTEGER,
+	process_mem_used INTEGER,
+	database_mem_used INTEGER,
+	dictionary_size INTEGER,
+	parameters TEXT,
+	time_enter DATE
+);
+
+CREATE TABLE Statistics (
+	id INTEGER NOT NULL,
+	stamp FLOAT,
+	data BLOB,              -- compressed string
+	wm_state BLOB,	        -- compressed data
+	FOREIGN KEY (id) REFERENCES Node(id)
+);
+
+CREATE TABLE Admin (
+	version TEXT,
+	preview_image BLOB,      -- compressed image
+	
+	opt_cloud BLOB,          -- compressed data
+	opt_ids BLOB,            -- Node ids used to generate the optimized cloud/mesh
+	opt_poses BLOB,          -- compressed N*3x4 float 
+	opt_last_localization BLOB, -- 3x4 float
+	opt_polygons_size INTEGER, -- e.g., 3
+	opt_polygons BLOB,       -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
+	opt_tex_coords BLOB,     -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
+	opt_tex_materials BLOB,  -- compressed image
+	opt_map BLOB,            -- compressed CV_8SC1 occupancy grid
+	opt_map_x_min FLOAT,
+	opt_map_y_min FLOAT, 
+	opt_map_resolution FLOAT,
+
+	dictionary_index BLOB,   -- serialized dictionary index
+
+	time_enter DATE
+);
+
+-- *******************************************************************
+-- TRIGGERS
+-- *******************************************************************
+CREATE TRIGGER insert_Feature BEFORE INSERT ON Feature 
+WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key constraint failed in Feature table');
+END;
+
+ --   Creating a trigger for time_enter
+CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Data_timeEnter AFTER INSERT ON Data
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word
+BEGIN
+ UPDATE Word SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Info_timeEnter AFTER INSERT ON Info
+BEGIN
+ UPDATE Info SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+-- *******************************************************************
+-- INDEXES
+-- *******************************************************************
+CREATE UNIQUE INDEX IDX_Node_id on Node (id);
+CREATE INDEX IDX_Feature_node_id on Feature (node_id);
+CREATE INDEX IDX_GlobalDescriptor_node_id on GlobalDescriptor (node_id);
+CREATE INDEX IDX_Link_from_id on Link (from_id);
+CREATE UNIQUE INDEX IDX_node_label on Node (label);
+CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id);
+
+-- *******************************************************************
+-- VERSION
+-- *******************************************************************
+INSERT INTO Admin(version) VALUES('0.23.0');
+

corelib/src/superpoint_rpautrat/SuperpointRpautrat.cpp

@@ -0,0 +1,234 @@
+/**
+ * SuperPoint implementation based on the PyTorch version by Rémi Pautrat, Paul-Edouard Sarlin
+ * Adapted for RTAB-Map integration
+ */
+
+#include "SuperpointRpautrat.h"
+#include <rtabmap/core/Features2d.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <pybind11/embed.h>
+#include <torch/torch.h>
+#include <torch/script.h>
+#include <opencv2/opencv.hpp>
+
+#include <fstream>
+#include <sstream>
+#include "superpoint_to_torchscript_py.h"
+
+namespace rtabmap
+{
+
+// Run the python script to export the SuperPoint model file with the desired parameters
+static std::string exportSuperPointTorchScript(
+    const std::string & superpointWeightsPath,
+    const std::string & superpointModelPath,
+    const std::string & outputDir,
+    const int & width,
+    const int & height,
+    const float & threshold,
+    const int & nms_radius,
+    const bool & cuda)
+{
+    // Validate output directory is explicitly set and exists
+    if(outputDir.empty())
+	{
+        UERROR("Output directory is not set.");
+		return std::string("");
+	}
+	if(!UDirectory::exists(outputDir))
+	{
+		UERROR("Output directory does not exist: %s", outputDir.c_str());
+		return std::string("");
+	}
+
+    // Resolve paths (no dependency on source tree)
+	const std::string weightsPath = superpointWeightsPath;
+    const std::string modelPath = superpointModelPath;
+	const std::string output = std::string(outputDir + "/superpoint_v6_from_tf.pt");
+    
+	// Sanity checks
+    if(!UFile::exists(weightsPath)) {
+        UERROR("Weights not found: %s", weightsPath.c_str());
+        return "";
+    }
+    if(!UFile::exists(modelPath)) {
+        UERROR("Model not found: %s", modelPath.c_str());
+        return "";
+    }
+    
+    // Execute the script inside the embedded Python interpreter
+    try
+    {
+        pybind11::gil_scoped_acquire acquire;
+        pybind11::dict scope;
+        scope["__builtins__"] = pybind11::module_::import("builtins");
+        
+        // set sys.path to the location of the model definition so it can be imported
+        std::string model_dir = UDirectory::getDir(modelPath);
+        auto sys = pybind11::module_::import("sys");
+        pybind11::list sys_path = sys.attr("path");
+        sys_path.attr("insert")(0, model_dir);
+        
+        try {
+            // execute the script to generate the model
+            pybind11::exec(uHex2Str(SUPERPOINT_TO_TORCHSCRIPT_PY), scope, scope);
+            pybind11::function generate_model = scope["generate_model"].cast<pybind11::function>();
+            pybind11::object result = generate_model(weightsPath, output, cuda, nms_radius, threshold, width, height);
+            sys_path.attr("remove")(model_dir);
+        }
+        catch(...) {
+            // Ensure sys.path cleanup on any exception
+            sys_path.attr("remove")(model_dir);
+            throw;
+        }
+
+    }
+    // pybind11 throws std::exception for RuntimeError
+    catch (const std::exception &e)
+    {
+        UERROR("Python export failed: %s", e.what());
+        return "";
+    }
+    
+	return output;
+}
+
+SPDetectorRpautrat::SPDetectorRpautrat(std::string superpointWeightsPath, std::string superpointModelPath, std::string outputDir, float threshold, bool nms, int minDistance, bool cuda, int maxFeatures, bool ssc) :
+		device_(torch::kCPU),
+		superpointWeightsPath_(superpointWeightsPath),
+        superpointModelPath_(superpointModelPath),
+        outputDir_(outputDir),
+        threshold_(threshold),
+		nms_(nms),
+		minDistance_(minDistance),
+        maxFeatures_(maxFeatures),
+        ssc_(ssc),
+		detected_(false)
+{
+	if(cuda && !torch::cuda::is_available())
+	{
+		UWARN("Cuda option is enabled but torch doesn't have cuda support on this platform, using CPU instead.");
+	}
+	cuda_ = cuda && torch::cuda::is_available();
+    
+    if(!UFile::exists(superpointWeightsPath_)) {
+        UERROR("Superpoint weights not found: %s", superpointWeightsPath_.c_str());
+    }
+
+    // Update device based on cuda availability
+    device_ = torch::Device(cuda_ ? torch::kCUDA : torch::kCPU);
+}
+
+SPDetectorRpautrat::~SPDetectorRpautrat()
+{
+}
+
+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().");
+		return cv::Mat();
+	}
+    if(keypoints.empty())
+	{
+		return cv::Mat();
+	}
+
+    // These should have the same size
+    UASSERT(static_cast<size_t>(desc_.rows) == keypoints.size());
+
+    return desc_;
+}
+
+std::vector<cv::KeyPoint> SPDetectorRpautrat::detect(const cv::Mat &img, const cv::Mat & mask)
+{    
+    // On first frame, run a trace of the model with the desired parameters and load the model file
+    if(!detected_)
+    {        
+        // effectively disable nms if it is not enabled by setting radius to 0
+        int nms_radius = nms_ ? minDistance_ : 0;
+        
+        std::string modelPath = exportSuperPointTorchScript(
+            superpointWeightsPath_,
+            superpointModelPath_,
+            outputDir_,
+            img.cols,
+            img.rows,
+            threshold_,
+            nms_radius,
+            cuda_
+        );
+
+        UDEBUG("Initializing SuperPoint Rpautrat detector with model: %s", modelPath.c_str());
+        UDEBUG("modelPath=%s thr=%f nms=%d minDistance=%d cuda=%d", modelPath.c_str(), threshold_, nms_?1:0, minDistance_, cuda_?1:0);
+        if(modelPath.empty())
+        {
+            UERROR("Model's path is empty! The model was not exported correctly.");
+            return std::vector<cv::KeyPoint>();
+        }
+        if(!UFile::exists(modelPath))
+        {
+            UERROR("Model's path \"%s\" doesn't exist!", modelPath.c_str());
+            return std::vector<cv::KeyPoint>();
+        }
+        
+        // Load TorchScript model
+        model_ = torch::jit::load(modelPath);
+        model_.eval(); // put in evaluation mode
+        model_.to(device_);
+    }
+    
+    // format the input tensor for the model
+    torch::NoGradGuard no_grad_guard;
+    auto x = torch::from_blob(img.data, {1, 1, img.rows, img.cols}, torch::kByte);
+    x = x.to(torch::kFloat) / 255;
+    x = x.set_requires_grad(false).to(device_);
+    
+    auto outputs = model_.forward({x}).toTuple();
+    auto kpts_tensor = outputs->elements()[0].toTensor();  // [N, 2] keypoint coordinates
+    auto scores_tensor = outputs->elements()[1].toTensor();  // [N] keypoint scores
+    torch::Tensor desc_tensor = outputs->elements()[2].toTensor();  // [N, 256] descriptors    
+
+    // Convert to CPU for processing
+    auto keypoints_cpu = kpts_tensor.to(torch::kCPU);
+    auto scores_cpu = scores_tensor.to(torch::kCPU);
+
+    std::vector<cv::KeyPoint> filtered_keypoints;
+    std::vector<int64_t> keep_indices_vec;
+    
+    // Apply mask filtering
+    for(int i = 0; i < keypoints_cpu.size(0); i++) {
+        float score = scores_cpu[i].item<float>();
+        float x = keypoints_cpu[i][0].item<float>();  // x coordinate
+        float y = keypoints_cpu[i][1].item<float>();  // y coordinate
+            
+        // Check mask if provided
+        if(mask.empty() || mask.at<unsigned char>((int)y, (int)x) != 0) {
+            keep_indices_vec.push_back(i);
+            filtered_keypoints.emplace_back(cv::KeyPoint(x, y, 8, -1, score));
+        }
+    }
+    
+    // Filter descriptors based on mask
+    auto keep_indices = torch::from_blob(keep_indices_vec.data(), {(long int)keep_indices_vec.size()}, torch::kLong);
+    keep_indices = keep_indices.to(desc_tensor.device());
+    auto filtered_descriptors = desc_tensor.index_select(0, keep_indices);
+    
+    // Convert descriptors to cv::Mat
+    auto filtered_descriptors_cpu = filtered_descriptors.to(torch::kCPU);
+    cv::Mat descriptors_mat(filtered_descriptors_cpu.size(0), filtered_descriptors_cpu.size(1), CV_32FC1, filtered_descriptors_cpu.data_ptr<float>());
+    cv::Mat descriptors_clone = descriptors_mat.clone(); // Clone to own the memory
+    
+    // Apply limitKeypoints to enforce maxFeatures and SSC
+    Feature2D::limitKeypoints(filtered_keypoints, descriptors_clone, maxFeatures_, cv::Size(img.cols, img.rows), ssc_);
+        
+    desc_ = descriptors_clone;
+    detected_ = true;
+    return filtered_keypoints;
+}
+
+} // namespace rtabmap

corelib/src/superpoint_rpautrat/SuperpointRpautrat.h

@@ -0,0 +1,59 @@
+/**
+ * SuperPoint implementation based on the PyTorch version by Rémi Pautrat, Paul-Edouard Sarlin
+ * Adapted for RTAB-Map integration
+ */
+
+#ifndef SUPERPOINT_RPAUTRAT_H
+#define SUPERPOINT_RPAUTRAT_H
+
+#include <torch/torch.h>
+#include <opencv2/opencv.hpp>
+#include <vector>
+#include <memory>
+
+namespace rtabmap
+{
+
+class SPDetectorRpautrat {
+    public:
+        SPDetectorRpautrat(
+            std::string superpointWeightsPath, 
+            std::string superpointModelPath,
+            std::string outputDir, 
+            float threshold = 0.005f, 
+            bool nms = true, 
+            int nmsRadius = 4, 
+            bool cuda = false,
+            int maxFeatures = 1000,
+            bool ssc = false
+        );
+        virtual ~SPDetectorRpautrat();
+        std::vector<cv::KeyPoint> detect(const cv::Mat &img, const cv::Mat & mask = cv::Mat());
+        cv::Mat compute(const std::vector<cv::KeyPoint> &keypoints);
+        
+        // Setters for post-processing parameters that don't require model reinitialization
+        void setMaxFeatures(int maxFeatures) { maxFeatures_ = maxFeatures; }
+        void setSSC(bool ssc) { ssc_ = ssc; }
+    
+    private:
+        torch::jit::script::Module model_;
+        torch::Device device_;
+        cv::Mat desc_;
+        
+        std::string superpointWeightsPath_;
+        std::string superpointModelPath_;
+        std::string outputDir_;
+        float threshold_;
+        bool nms_;
+        int minDistance_;
+        bool cuda_;
+        int maxFeatures_;
+        bool ssc_;
+        
+        bool detected_;
+    };    
+}
+
+
+
+#endif // SUPERPOINT_RPAUTRAT_H

corelib/src/superpoint_rpautrat/superpoint_to_torchscript.py

@@ -0,0 +1,107 @@
+#!/usr/bin/env python3
+"""
+Convert PyTorch weights to TorchScript format for C++ usage.
+"""
+
+import argparse
+import os
+
+import torch
+import torch.nn as nn
+from superpoint_pytorch import SuperPoint
+
+
+def wrap_model(model: nn.Module):
+    """
+    Simple wrapper to fix SuperPoint input format for TorchScript.
+    Easier to call from C++ code since the input isn't a dictionary.
+    """
+    class Wrapper(nn.Module):
+        def __init__(self, net: nn.Module):
+            super().__init__()
+            self.net = net
+
+        def forward(self, x: torch.Tensor):
+            # SuperPoint expects {"image": tensor} but TorchScript doesn't like dict indexing
+            out = self.net.forward({"image": x})
+            
+            # Return the format expected by C++ code: keypoints, scores, descriptors
+            # For single batch item, take the first (and only) element
+            keypoints = out["keypoints"][0] if out["keypoints"] else torch.empty(0, 2)
+            scores = out["keypoint_scores"][0] if out["keypoint_scores"] else torch.empty(0)
+            descriptors = out["descriptors"][0] if out["descriptors"] else torch.empty(0, 256)
+            
+            return (keypoints, scores, descriptors)
+    
+    return Wrapper(model)
+
+def generate_model(
+    weights_path: str,
+    output_path: str,
+    cuda: bool,
+    nms_radius: int,
+    threshold: float,
+    width: int,
+    height: int,
+):
+    # Check if weights are already TorchScript
+    try:
+        scripted = torch.jit.load(weights_path, map_location="cpu")
+        scripted.eval()
+        torch.jit.save(scripted, output_path)
+        print(f"Converted TorchScript file: {output_path}")
+        return
+    except:
+        pass
+
+    device = "cuda" if cuda else "cpu"
+
+    # Load SuperPoint model and weights
+    model = SuperPoint(
+        nms_radius=nms_radius,
+        detection_threshold=threshold,
+    ).eval().to(device)
+    
+    # Load weights without forcing CPU location to allow CUDA usage
+    weights = torch.load(weights_path, map_location=None)
+    if isinstance(weights, dict) and "state_dict" in weights:
+        weights = weights["state_dict"]
+    
+    model.load_state_dict(weights, strict=False)
+    
+    wrapped = wrap_model(model)
+    dummy = torch.randn(1, 1, height, width, device=device)  # Dummy input, grayscale, using cuda.
+    
+    # Convert to TorchScript using trace (SuperPoint has dynamic behavior that scripting can't handle)
+    print("Using torch.jit.trace (SuperPoint has dynamic behavior)...")
+    scripted = torch.jit.trace(wrapped, (dummy,), strict=False)
+    print("Successfully traced SuperPoint model")
+    
+    # Save output
+    os.makedirs(os.path.dirname(output_path), exist_ok=True)
+    torch.jit.save(scripted, output_path)
+    print(f"Converted SuperPoint weights to TorchScript: {output_path}")
+
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="Convert SuperPoint weights to TorchScript")
+    parser.add_argument("--weights", required=True, help="Path to weights file")
+    parser.add_argument("--output", required=True, help="Output TorchScript file")
+    parser.add_argument("--cuda", action="store_true", help="Use CUDA")
+    parser.add_argument("--width", type=int, default=1920, help="Width of the input image")
+    parser.add_argument("--height", type=int, default=288, help="Height of the input image")
+    parser.add_argument("--nms_radius", type=int, default=4, help="NMS radius")
+    parser.add_argument("--threshold", type=float, default=0.005, help="Confidence threshold")
+    args = parser.parse_args()
+    print(f"Generating model from weights: {args.weights} to output: {args.output}")
+    
+    generate_model(
+        weights_path=args.weights,
+        output_path=args.output,
+        cuda=args.cuda,
+        nms_radius=args.nms_radius,
+        threshold=args.threshold,
+        width=args.width,
+        height=args.height,
+    )

corelib/src/util2d.cpp

@@ -2296,6 +2296,7 @@ std::vector<int> SSC(
 	const std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, float tolerance, int cols, int rows, const std::vector<int> & indx)
 {
 	bool useIndx = keypoints.size() == indx.size();
+	maxKeypoints = maxKeypoints - round(maxKeypoints * tolerance); // Just the make sure the solution will always be <= input maxKeypoints
 
 	// several temp expression variables to simplify solution equation
 	int exp1 = rows + cols + 2*maxKeypoints;

corelib/src/util3d_features.cpp

@@ -213,6 +213,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
 		Transform & cameraTransform,
 		float ransacReprojThreshold,
 		float ransacConfidence,
+		int varianceMedianRatio,
 		const std::map<int, cv::Point3f> & refGuess3D,
 		double * varianceOut,
 		std::vector<int> * matchesOut)
@@ -345,7 +346,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
 							errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
 						}
 						std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
-						double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 2];
+						double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> varianceMedianRatio];
 						float var = 2.1981 * median_error_sqr;
 						//UDEBUG("scale %d = %f variance = %f", (int)i, s, variance);
 
@@ -369,7 +370,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
 						errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
 					}
 					std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
-					double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 2];
+					double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> varianceMedianRatio];
 					variance = 2.1981 * median_error_sqr;
 				}
 			}

corelib/src/util3d_filtering.cpp

@@ -954,7 +954,8 @@ pcl::IndicesPtr cropBoxImpl(
 		const Transform & transform,
 		bool negative)
 {
-	UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
+	UASSERT_MSG(min[0] < max[0] && min[1] < max[1] && min[2] <= max[2], // z can be equal in 2D case
+		uFormat("x=%f->%f y=%f->%f z=%f->%f", min[0], max[0], min[1], max[1], min[2], max[2]).c_str()); 
 
 	pcl::IndicesPtr output(new std::vector<int>);
 	pcl::CropBox<PointT> filter;
@@ -973,7 +974,8 @@ pcl::IndicesPtr cropBoxImpl(
 
 pcl::IndicesPtr cropBox(const pcl::PCLPointCloud2::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
 {
-	UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
+	UASSERT_MSG(min[0] < max[0] && min[1] < max[1] && min[2] <= max[2], // z can be equal in 2D case
+		uFormat("x=%f->%f y=%f->%f z=%f->%f", min[0], max[0], min[1], max[1], min[2], max[2]).c_str());
 
 	pcl::IndicesPtr output(new std::vector<int>);
 	pcl::CropBox<pcl::PCLPointCloud2> filter;

docker/noble/android/Dockerfile

@@ -3,14 +3,17 @@
 FROM ubuntu:24.04
 
 # Install build dependencies
-RUN apt-get update && apt-get install -y --no-install-recommends apt-utils
+RUN apt-get update && apt-get install -y --no-install-recommends \
+      apt-utils && apt-get clean && \
+      rm -rf /var/lib/apt/lists/
 RUN apt-get update && apt-get install -y \
       git unzip wget ant cmake \
       g++ lib32stdc++6 lib32z1 \
       software-properties-common \
       freeglut3-dev \
       openjdk-8-jdk openjdk-8-jre \
-      curl
+      curl && \
+      apt-get clean && rm -rf /var/lib/apt/lists/
 
 ENV ANDROID_HOME=/opt/android-sdk
 ENV PATH=$PATH:/opt/android-sdk/cmdline-tools/latest/bin:/opt/android-sdk/tools:/opt/android-sdk/platform-tools:/opt/android-sdk/ndk/21.4.7075529
@@ -53,6 +56,8 @@ RUN echo "Install boost..." && \
     cd build && \
     cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" ..  && \
     make -j4 && \
+    find . -type f -name "*.a" -print0 \
+       | xargs -0 $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug && \
     make install && \
     cd /root  && \
     rm -r boost_1_59_0.tar.gz boost_1_59_0
@@ -80,6 +85,8 @@ RUN echo "Install flann..." && \
     cd build && \
     cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DBUILD_PYTHON_BINDINGS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" .. && \
     make -j4 && \
+    find . -type f -name "*.a" -print0 \
+       | xargs -0 $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug && \
     make install && \
     cd /root && \
     rm -rf flann
@@ -95,6 +102,8 @@ RUN echo "Install gtsam..." && \
     cd build && \
     cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DMETIS_SHARED=OFF -DGTSAM_BUILD_STATIC_LIBRARY=ON -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON .. && \
     make -j4 && \
+    find . -type f -name "*.a" -print0 \
+       | xargs -0 $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug && \
     make install && \
     cd /root && \
     rm -rf gtsam
@@ -108,6 +117,8 @@ RUN echo "Install g2o..." && \
     cd build && \
     cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DG2O_USE_OPENGL=OFF .. && \
     make -j4 && \
+    find "../lib" -type f -name "*.a" -print0 \
+       | xargs -0 $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug && \
     make install && \
     cd /root && \
     rm -rf g2o
@@ -117,12 +128,14 @@ RUN echo "Install VTK..." && \
     git clone https://github.com/Kitware/VTK.git && \
     cd VTK && \
     git checkout tags/v8.2.0 && \
-    wget https://gist.github.com/matlabbe/e217259fb8ece9ee6daf5a8f70e896a0/raw/2214b503a537d6431d764526b5b780f07d6f168d/vtk_8_2_0_android_r21_fix.patch && \
+    wget https://gist.github.com/matlabbe/e217259fb8ece9ee6daf5a8f70e896a0/raw/36d879f0827289e75de969afb7b6515d9a1c71d7/vtk_8_2_0_android_r21_fix.patch && \
     git apply vtk_8_2_0_android_r21_fix.patch && \
     mkdir build && \
     cd build && \
     cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DANDROID_ARCH_ABI=arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" .. && \
     make -j4 && \
+    find . -type f -name "*.a" -print0 \
+       | xargs -0 $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug && \
     cp -r CMakeExternals/Install/vtk-android/* /opt/android/arm64-v8a/. && \
     cd /root && \
     rm -rf VTK
@@ -140,6 +153,8 @@ RUN echo "Install pcl..." && \
     cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_tools=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF .. || true && \
     cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_tools=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF .. && \
     make -j4 && \
+    find . -type f -name "*.a" -print0 \
+       | xargs -0 $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug && \
     make install && \
     cd /root && \
     rm -rf pcl
@@ -161,14 +176,12 @@ RUN echo "Install OpenCV..." && \
     cd build && \
     cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=23 -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_CUDA=OFF -DBUILD_opencv_structured_light=OFF -DBUILD_ANDROID_PROJECTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DBUILD_ANDROID_EXAMPLES=OFF -DWITH_PROTOBUF=OFF -DBUILD_opencv_stereo=OFF -DBUILD_JAVA=OFF -DWITH_QUIRC=OFF -DBUILD_opencv_js_bindings_generator=OFF -DBUILD_opencv_objc_bindings_generator=OFF -DBUILD_opencv_objdetect=OFF -DBUILD_opencv_xobjdetect=OFF .. && \
     make -j4 && \
+    find . -type f -name "*.a" -print0 \
+       | xargs -0 $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug && \
     make install && \
     cd /root && \
     rm -rf opencv opencv_contrib
 
-RUN echo "Strip libraries..." && \
-    $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug --verbose /opt/android/arm64-v8a/lib/*.a && \
-    $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug --verbose /opt/android/arm64-v8a/sdk/native/staticlibs/arm64-v8a/*.a
-
 RUN mkdir /opt/android/lib
 
 # tango

docker/noble/android/rtabmap_apiXX/rtabmap.bash

@@ -21,7 +21,7 @@ make
 # rtabmap
 mkdir arm64-v8a
 cd arm64-v8a
-cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$api -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DOpenCV_DIR=$prefix/arm64-v8a/sdk/native/jni ../..
+cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$api -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DWITH_OPENGV=OFF -DOpenCV_DIR=$prefix/arm64-v8a/sdk/native/jni ../..
 make
 make clean
 

guilib/include/rtabmap/gui/CameraViewer.h

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/utilite/UEventsHandler.h>
 #include <QDialog>
+#include <QElapsedTimer>
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/core/Parameters.h>
 
@@ -73,6 +74,9 @@ private:
 	QCheckBox * showScanCheckbox_;
 	QCheckBox * markerCheckbox_;
 	MarkerDetector * markerDetector_;
+	QElapsedTimer fpsTimer_;
+	double lastCapturePeriod_;
+	double previousCaptureStamp_;
 };
 
 } /* namespace rtabmap */

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -220,6 +220,7 @@ private:
 	std::map<int, int> mapIds_;
 	std::map<int, int> weights_;
 	std::map<int, std::vector<int> > wmStates_;
+	std::map<int, EnvSensors> envSensors_;
 	QMap<int, int> idToIndex_;
 	QList<rtabmap::Link> neighborLinks_;
 	QList<rtabmap::Link> loopLinks_;

guilib/include/rtabmap/gui/GraphViewer.h

@@ -77,6 +77,7 @@ public:
 	// Use updateNodeColorByValue() instead with valueName="Posterior".
 	RTABMAP_DEPRECATED void updatePosterior(const std::map<int, float> & posterior, float fixedMax = 0.0f, int zValueOffset = 0);
 	void updateNodeColorByValue(const std::string & valueName, const std::map<int, float> & values, float fixedMax = 0.0f, bool invertedColorScale = false, int zValueOffset = 0);
+	void updateNodeColorByValue(const std::string & valueName, const std::map<int, float> & values, float fixedMin, float fixedMax, bool invertedColorScale = false, unsigned short hueMin=0, unsigned short hueMax=180, int zValueOffset = 0);
 	void updateLocalPath(const std::vector<int> & localPath);
 	void setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath);
 	void setCurrentGoalID(int id, const Transform & pose = Transform());
@@ -118,8 +119,9 @@ public:
 	bool isReferentialVisible() const;
 	bool isLocalRadiusVisible() const;
 	float getLoopClosureOutlierThr() const {return _loopClosureOutlierThr;}
-	float getMaxLinkLength() const {return _maxLinkLength;}
+	float getMinLinkLength() const {return _minLinkLength;}
 	bool isGraphVisible() const;
+	bool isNodeVisible() const;
 	bool isGlobalPathVisible() const;
 	bool isLocalPathVisible() const;
 	bool isGtGraphVisible() const;
@@ -158,7 +160,7 @@ public:
 	void setReferentialVisible(bool visible);
 	void setLocalRadiusVisible(bool visible);
 	void setLoopClosureOutlierThr(float value);
-	void setMaxLinkLength(float value);
+	void setMinLinkLength(float value);
 	void setGraphVisible(bool visible);
 	void setGlobalPathVisible(bool visible);
 	void setLocalPathVisible(bool visible);
@@ -184,6 +186,9 @@ protected:
 	virtual void mousePressEvent(QMouseEvent * event);
 	virtual void contextMenuEvent(QContextMenuEvent * event);
 
+private:
+	void setupGraphicsScene();
+
 private:
 	QString _workingDirectory;
 	QColor _nodeColor;
@@ -237,7 +242,7 @@ private:
 	QGraphicsEllipseItem * _localRadius;
 	QGraphicsRectItem * _odomCacheOverlay;
 	float _loopClosureOutlierThr;
-	float _maxLinkLength;
+	float _minLinkLength;
 	bool _orientationENU;
 	bool _mouseTracking;
 	ViewPlane _viewPlane;

guilib/include/rtabmap/gui/MainWindow.h

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

guilib/include/rtabmap/gui/MapVisibilityWidget.h

@@ -44,6 +44,7 @@ public:
 
 	void setMap(const std::map<int, Transform> & poses, const std::map<int, bool> & mask);
 	std::map<int, Transform> getVisiblePoses() const;
+	bool isEmpty() const {return _poses.empty();}
 
 	void clear();
 

guilib/include/rtabmap/gui/PostProcessingDialog.h

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

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -293,6 +293,7 @@ public:
 	double getSourceScanForceGroundNormalsUp() const;
 	Transform getSourceLocalTransform() const;    //Openni group
 	Transform getLaserLocalTransform() const; // directory images
+	Transform getGroundTruthLocalTransform() const; // directory images
 	Transform getIMULocalTransform() const; // directory images
 	QString getIMUPath() const;
 	int getIMURate() const;
@@ -374,6 +375,8 @@ private Q_SLOTS:
 	void changeOdometryOpenVINSRightMask();
 	void changeIcpPMConfigPath();
 	void changeSuperPointModelPath();
+	void changeSuperPointRpautratWeightsPath();
+	void changeSuperPointRpautratModelPath();
 	void changePyMatcherPath();
 	void changePyMatcherModel();
 	void changePyDescriptorPath();

guilib/include/rtabmap/gui/StatsToolBox.h

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

guilib/src/3rdParty/QMultiComboBox.cpp

@@ -40,7 +40,7 @@ QMultiComboBox::QMultiComboBox(QWidget *widget ) :
 
 QMultiComboBox::~QMultiComboBox()
 {
-    disconnect(&vlist_,0,0,0);
+	vlist_.disconnect(SIGNAL(itemChanged(QListWidgetItem*)));
 }
 
 

guilib/src/AboutDialog.cpp

@@ -86,6 +86,13 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_sptorch->setText("No");
 	_ui->label_sptorch_license->setEnabled(false);
 #endif
+#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
+	_ui->label_sprpautrat->setText("Yes");
+	_ui->label_sprpautrat_license->setEnabled(true);
+#else
+	_ui->label_sprpautrat->setText("No");
+	_ui->label_sprpautrat_license->setEnabled(false);
+#endif
 #ifdef RTABMAP_PYTHON
 	_ui->label_pymatcher->setText("Yes");
 	_ui->label_pymatcher_license->setEnabled(true);
@@ -114,6 +121,13 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_liblas->setText("No");
 	_ui->label_liblas_license->setEnabled(false);
 #endif
+#ifdef RTABMAP_OPENGV
+	_ui->label_opengv->setText("Yes");
+	_ui->label_opengv_license->setEnabled(true);
+#else
+	_ui->label_opengv->setText("No");
+	_ui->label_opengv_license->setEnabled(false);
+#endif
 #ifdef RTABMAP_CUDASIFT
 	_ui->label_cudasift->setText("Yes");
 	_ui->label_cudasift_license->setEnabled(true);