merged master->branch

2026-10-03 16:47:47 +08:00 · 2026-04-05 14:20:35 -07:00
parent ffb3be7ba3 8fd701aabe
commit 0405949ff4
103 changed files with 7621 additions and 3191 deletions
@@ -1,160 +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 -DBUILD_TESTING=ON ..
-
-build_script:
-  - cd c:\projects\rtabmap\build
-  - cmake --build . --config Release --target ALL_BUILD
-
-after_build :
-  - cmake --build . --config Release --target package
-
-test_script:
-  - cd c:\projects\rtabmap\build
-  - ctest -C Release --output-on-failure --timeout 300
-
-artifacts:
-  - path: build\RTABMap-*
-  
-notifications:
-  - provider: Email
-    to:
-      - [email protected]
-    on_build_success: false
-    on_build_failure: false
-    on_build_status_changed: true
@@ -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/[email protected]
+      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
@@ -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
@@ -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-*
@@ -1,4 +1,4 @@
-name: CMake
+name: CMake-Linux

 on:
  push:
@@ -11,34 +11,44 @@ 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 }}
    concurrency:
      group: ${{ github.workflow }}-${{ github.ref }}-${{ matrix.os }}
      cancel-in-progress: true
    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 }}
@@ -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 }}
    concurrency:
      group: ${{ github.workflow }}-${{ github.ref }}-${{ matrix.ros_distribution }}-${{ matrix.os }}
@@ -0,0 +1,112 @@
+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 }} `
+          -DBUILD_TESTING=ON `
+          -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}} --target ALL_BUILD
+
+    - name: Test
+      working-directory: ${{github.workspace}}/build
+      run: ctest -C ${{env.BUILD_TYPE}} --output-on-failure --timeout 300
+
+    - 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}}
+      
@@ -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")
@@ -20,7 +22,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 23)
-SET(RTABMAP_PATCH_VERSION 3)
+SET(RTABMAP_PATCH_VERSION 4)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -235,6 +237,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()
@@ -656,7 +659,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)
@@ -801,8 +805,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)
@@ -866,12 +868,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)
@@ -1302,7 +1350,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}")
@@ -1622,7 +1680,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()
@@ -1737,7 +1799,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)")
@@ -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>
@@ -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
@@ -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))
  
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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;
@@ -309,7 +309,8 @@ private:
 	bool preciseUpscale_;
 	bool rootSIFT_;
 	bool gpu_;
-	float guaussianThreshold_;
+	float gaussianThreshold_;
+	float maxGaussianThreshold_;
 	bool upscale_;

 	cv::Ptr<CV_SIFT> sift_;
@@ -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(
@@ -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;
@@ -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()));
@@ -261,7 +262,7 @@ class RTABMAP_CORE_EXPORT Parameters
    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.");
@@ -723,7 +725,7 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,     "Max depth of the features (0 means no limit).");
    RTABMAP_PARAM(Vis, MinDepth,                  float, 0,     "Min depth of the features (0 means no limit).");
    RTABMAP_PARAM(Vis, DepthAsMask,               bool,  true,  "Use depth image as mask when extracting features.");
-    RTABMAP_PARAM(Vis, DepthMaskFloorThr,         float, 0.0,    uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kVisDepthAsMask().c_str()));
+    RTABMAP_PARAM(Vis, DepthMaskFloorThr,         float, 0.0,    uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled. Ignored if %s is false.", kVisDepthAsMask().c_str()));
    RTABMAP_PARAM_STR(Vis, RoiRatios,        "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
    RTABMAP_PARAM(Vis, SubPixWinSize,             int,   3,     "See cv::cornerSubPix().");
    RTABMAP_PARAM(Vis, SubPixIterations,          int,   0,     "See cv::cornerSubPix(). 0 disables sub pixel refining.");
@@ -739,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.");
@@ -817,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");

@@ -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;
@@ -91,6 +91,9 @@ private:
 	float _flowEps;
 	int _flowMaxLevel;
 	bool _flowGpu;
+	bool _flowUseMinEigenVals;
+	float _flowMinEigThreshold;
+	float _flowErrorThreshold;
 	float _nndr;
 	int _nnType;
 	bool _gmsWithRotation;
@@ -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,
@@ -117,6 +117,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);
@@ -311,6 +311,10 @@ public:
 	 */
 	virtual bool isGpuEnabled() const;

+	bool usingMinEigenVals() const {return useMinEigenVals_;}
+	float minEigThreshold() const {return minEigThreshold_;}
+	float errorThreshold() const {return errorThreshold_;}
+
 private:
 	/**
 	 * @brief Update status vector based on disparity constraints
@@ -329,10 +333,14 @@ 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_; ///< Convergence threshold for optical flow (default: from Parameters::defaultStereoEps())
+	bool useMinEigenVals_;
+	float minEigThreshold_;
+	float errorThreshold_;
 	bool gpu_;      ///< Enable GPU acceleration (default: from Parameters::defaultStereoGpu(), requires OpenCV CUDA)
 };

@@ -187,11 +187,12 @@ public:
 	 * @brief Add a reference from a visual word to a signature
 	 * @param wordId ID of the visual word
 	 * @param signatureId ID of the signature (image)
+	 * @return true if the word exists in the dictionary and the reference has been added, false otherwise
 	 * 
 	 * Tracks which signatures use which visual words. If the word was unused,
 	 * it is removed from the unused words list.
 	 */
-	void addWordRef(int wordId, int signatureId);
+	bool addWordRef(int wordId, int signatureId);
 	
 	/**
 	 * @brief Remove all references from a visual word to a signature
@@ -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;
@@ -146,6 +146,7 @@ private:
 	Transform dualExtrinsics_;
 	std::string jsonConfig_;
 	bool closing_;
+	bool playback_;

 	static Transform realsense2PoseRotation_;
 	static Transform realsense2PoseRotationInv_;
@@ -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_;
@@ -160,6 +160,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);
@@ -791,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)
@@ -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
 			{
@@ -45,7 +45,7 @@ DBDriver * DBDriver::create(const ParametersMap & parameters)

 DBDriver::DBDriver(const ParametersMap & parameters) :
 	_emptyTrashesTime(0),
-	_timestampUpdate(true)
+	_timestampUpdate(false)
 {
 	this->parseParameters(parameters);
 }
@@ -73,7 +73,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 +91,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;
@@ -320,7 +320,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 +332,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 +354,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)
 	{
@@ -4350,7 +4350,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;
@@ -4389,7 +4389,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());
@@ -4413,7 +4413,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());
@@ -4426,14 +4426,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)
@@ -4468,6 +4461,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
@@ -5758,7 +5757,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;
@@ -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;
@@ -303,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;
@@ -419,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;
@@ -466,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
@@ -1251,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),
@@ -1284,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());
@@ -1363,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);
@@ -1388,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
@@ -1413,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());
 			}
 		}
 	}
@@ -1457,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
 		{
@@ -1470,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...");
@@ -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) {
@@ -2020,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)
@@ -2110,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;
@@ -2396,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;
 }
@@ -2426,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;
 }
@@ -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_;
@@ -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
@@ -5062,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.");
 					}

@@ -5121,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)
@@ -5128,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);
 				}

@@ -5138,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();
@@ -5338,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);
 		}
@@ -5572,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
@@ -5836,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
 	{
@@ -5880,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
@@ -5906,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());

@@ -6587,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);
@@ -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;
 			}
@@ -217,6 +217,10 @@ LinkIdKey(int id, Link::Type type) :
 		{
 			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
@@ -256,7 +260,7 @@ void Optimizer::getConnectedGraph(
 		}
 	}

-	while(nextPoses.size())
+	while(!nextPoses.empty())
 	{
 		// Fill up all nodes before landmarks
 		// For nodes, fill up all neightbor nodes before loop closure ones
@@ -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;
@@ -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);
@@ -321,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);
@@ -440,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.");
 							}

@@ -655,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)
 				{
@@ -684,7 +683,6 @@ Transform RegistrationVis::computeTransformationImpl(
 				else
 #endif
 				{
-					std::vector<float> err;
 					UDEBUG("cv::calcOpticalFlowPyrLK() begin");
 					cv::calcOpticalFlowPyrLK(
 						imageFrom,
@@ -696,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");
 				}

@@ -705,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())
 						{
@@ -806,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.");
 							}

@@ -1632,6 +1625,7 @@ Transform RegistrationVis::computeTransformationImpl(
 						cameraTransform,
 						_PnPReprojError,
 						0.99f,
+						_PnPVarMedianRatio,
 						words3A, // for scale estimation
 						&variance,
 						&matchesV);
@@ -2182,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)",
@@ -2204,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());
@@ -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)
@@ -1789,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())
@@ -2613,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;
@@ -2666,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
@@ -2678,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)
 				{
@@ -2685,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)
 							{
@@ -2795,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);
@@ -3062,6 +3079,7 @@ bool Rtabmap::process(
 				loopClosureVisualInliers = info.inliers;
 				loopClosureVisualInliersRatio = info.inliersRatio;
 				loopClosureVisualMatches = info.matches;
+				loopClosureVisualVariance = info.variance;
 				rejectedLoopClosure = transform.isNull();
 				if(rejectedLoopClosure)
 				{
@@ -4049,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());
@@ -4309,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
 		{
@@ -5636,7 +5669,8 @@ int Rtabmap::detectMoreLoopClosures(
 		bool intraSession,
 		bool interSession,
 		const ProgressState * processState,
-		float clusterRadiusMin)
+		float clusterRadiusMin,
+		int toFromMapId)
 {
 	UDEBUG("");
 	UASSERT(iterations>0);
@@ -5663,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));
 		}
 	}

@@ -5687,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;
@@ -5739,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))
@@ -5750,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())
 							{
@@ -5759,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;
@@ -5778,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,
@@ -120,6 +120,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);
@@ -129,6 +132,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_)
@@ -185,11 +191,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;
 }

@@ -241,14 +254,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())
@@ -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;
 }

@@ -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);
@@ -865,7 +882,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);
@@ -875,10 +892,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;
 	}
 }

@@ -1001,7 +1020,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)
 		{
@@ -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();
 					}
@@ -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)
@@ -732,7 +754,8 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 					 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_))
@@ -741,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())
 						{
@@ -880,8 +903,17 @@ 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)
 			{
@@ -1087,9 +1119,16 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			 }
 			 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>())
@@ -1538,7 +1577,10 @@ SensorData CameraRealSense2::captureImage(SensorCaptureInfo * info)
 	}
 	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!");
@@ -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;
 }
@@ -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)
@@ -430,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;
 	}

@@ -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;
@@ -1414,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,
@@ -1559,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(
@@ -1579,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)
@@ -1596,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
@@ -1619,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,
@@ -1632,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());
 				}
 			}
 		}
@@ -1652,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())
 					{
@@ -1666,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
@@ -1683,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
 				{
@@ -1839,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_;
@@ -1944,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);
@@ -1978,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)
@@ -2043,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
 					{
@@ -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())
@@ -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>);
  }
 };

@@ -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:
@@ -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
@@ -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;
+};
@@ -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)
@@ -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();
 }
@@ -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


@@ -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)
@@ -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")
@@ -2325,6 +2325,7 @@ std::vector<int> SSC(
 	UASSERT(keypoints.empty() || indx.empty() || keypoints.size() == indx.size());

 	bool useIndx = !indx.empty();
+	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;
@@ -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;
 				}
 			}
@@ -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

@@ -330,6 +330,7 @@ protected:
 			int iterations,
 			bool interSession,
 			bool intraSession,
+			int minGraphDistance,
 			// SBA params:
 			bool sba,
 			int sbaIterations,
@@ -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);
@@ -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;
@@ -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;
 };

 }
@@ -40,7 +40,7 @@ QMultiComboBox::QMultiComboBox(QWidget *widget ) :

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


@@ -121,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);
@@ -72,13 +72,15 @@ bool DataRecorder::init(const QString & path, bool recordInRAM)
 		ParametersMap customParameters;
 		customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // deactivate rehearsal
 		customParameters.insert(ParametersPair(Parameters::kKpMaxFeatures(), "-1")); // deactivate keypoints extraction
-		customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "true")); // to keep images
+		customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "true")); // make sure we keep images
 		customParameters.insert(ParametersPair(Parameters::kMemMapLabelsAdded(), "false")); // don't create map labels
+		customParameters.insert(ParametersPair(Parameters::kRGBDCreateOccupancyGrid(), "false")); // make sure we don't create local grids
 		customParameters.insert(ParametersPair(Parameters::kMemBadSignaturesIgnored(), "true")); // make sure memory cleanup is done
-		customParameters.insert(ParametersPair(Parameters::kMemIntermediateNodeDataKept(), "true"));
-		if(!recordInRAM)
+		customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "true")); // make sure we save every frame
+
+		if(recordInRAM)
 		{
-			customParameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false"));
+			customParameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "true"));
 		}
 		memory_ = new Memory();
 		if(!memory_->init(path.toStdString(), true, customParameters))
@@ -239,6 +239,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopClosureReextractFeatures()));
 	parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopCovLimited()));
 	parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDProximityPathFilteringRadius()));
+	parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kMemSTMSize()));
 	ui_->parameters_toolbox->setupUi(parameters);
 	exportDialog_->setObjectName("ExportCloudsDialog");
 	restoreDefaultSettings();
@@ -481,6 +482,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_detectMore_intraSession, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_detectMore_interSession, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
+	connect(ui_->spinBox_minGraphDistance, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_opt_graph_as_guess, SIGNAL(stateChanged(int)), this, SLOT(configModified()));

 	connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
@@ -678,6 +680,7 @@ void DatabaseViewer::readSettings()
 	ui_->checkBox_detectMore_intraSession->setChecked(settings.value("intra_session", ui_->checkBox_detectMore_intraSession->isChecked()).toBool());
 	ui_->checkBox_detectMore_interSession->setChecked(settings.value("inter_session", ui_->checkBox_detectMore_interSession->isChecked()).toBool());
 	ui_->checkBox_opt_graph_as_guess->setChecked(settings.value("opt_graph_as_guess", ui_->checkBox_opt_graph_as_guess->isChecked()).toBool());
+	ui_->spinBox_minGraphDistance->setValue(settings.value("min_graph_distance", ui_->spinBox_minGraphDistance->value()).toInt());
 	settings.endGroup();
 	settings.endGroup();

@@ -775,6 +778,7 @@ void DatabaseViewer::writeSettings()
 	settings.setValue("intra_session", ui_->checkBox_detectMore_intraSession->isChecked());
 	settings.setValue("inter_session", ui_->checkBox_detectMore_interSession->isChecked());
 	settings.setValue("opt_graph_as_guess", ui_->checkBox_opt_graph_as_guess->isChecked());
+	settings.setValue("min_graph_distance", ui_->spinBox_minGraphDistance->value());
 	settings.endGroup();
 	settings.endGroup();

@@ -855,6 +859,8 @@ void DatabaseViewer::restoreDefaultSettings()
 	ui_->checkBox_detectMore_intraSession->setChecked(true);
 	ui_->checkBox_detectMore_interSession->setChecked(true);
 	ui_->checkBox_opt_graph_as_guess->setChecked(true);
+	ui_->spinBox_fromToMapId->setValue(-1);
+	ui_->spinBox_minGraphDistance->setValue(10);
 }

 void DatabaseViewer::openDatabase()
@@ -4284,10 +4290,12 @@ void DatabaseViewer::detectMoreLoopClosures()
 	const ParametersMap & parameters = ui_->parameters_toolbox->getParameters();
 	bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
 	Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
+	std::multimap<int, Link> links = updateLinksWithModifications(links_);
 	if(loopCovLimited)
 	{
-		odomMaxInf_ = graph::getMaxOdomInf(updateLinksWithModifications(links_));
+		odomMaxInf_ = graph::getMaxOdomInf(links);
 	}
+	links = graph::filterLinks(links, Link::kNeighbor, true); // keep only neighbor links

 	int iterations = ui_->spinBox_detectMore_iterations->value();
 	UASSERT(iterations > 0);
@@ -4297,6 +4305,8 @@ void DatabaseViewer::detectMoreLoopClosures()
 	bool intraSession = ui_->checkBox_detectMore_intraSession->isChecked();
 	bool interSession = ui_->checkBox_detectMore_interSession->isChecked();
 	bool useOptimizedGraphAsGuess = ui_->checkBox_opt_graph_as_guess->isChecked();
+	int fromToMapId = ui_->spinBox_fromToMapId->value();
+	int minimumGraphDistance = ui_->spinBox_minGraphDistance->value();
 	if(!interSession && !intraSession)
 	{
 		QMessageBox::warning(this, tr("Cannot detect more loop closures"), tr("Intra and inter session parameters are disabled! Enable one or both."));
@@ -4313,14 +4323,68 @@ void DatabaseViewer::detectMoreLoopClosures()
 				ui_->doubleSpinBox_detectMore_radius->value(),
 				ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0);

-		progressDialog->setMaximumSteps(progressDialog->maximumSteps()+(int)clusters.size());
-		progressDialog->appendText(tr("Looking for more loop closures, %1 clusters found.").arg(clusters.size()));
 		QApplication::processEvents();
 		if(progressDialog->isCanceled())
 		{
 			break;
 		}

+		progressDialog->appendText(tr("Looking for more loop closures: %1 clusters found.").arg(clusters.size()));
+		if(fromToMapId >=0)
+		{
+			int clusterBefore = clusters.size();
+			for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end();)
+			{
+				int mapId = uValue(mapIds_, iter->first, 0);
+				if(mapId != fromToMapId)
+				{
+					iter = clusters.erase(iter);
+				}
+				else {
+					++iter;
+				}
+			}
+			progressDialog->appendText(tr("Looking for more loop closures: filtered %1/%2 clusters for map session %3.")
+				.arg(clusterBefore-clusters.size()).arg(clusterBefore).arg(fromToMapId));
+			if(clusters.empty())
+			{
+				progressDialog->appendText(tr("No clusters belong to mapId %1, aborting!").arg(fromToMapId));
+				QApplication::processEvents();
+				break;
+			}
+		}
+
+		if(minimumGraphDistance > 1)
+		{
+			int clusterBefore = clusters.size();
+			for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end();)
+			{
+				if(abs(iter->first - iter->second) < minimumGraphDistance)
+				{
+					iter = clusters.erase(iter);
+				}
+				else
+				{
+					// compute path to know how far we are in terms of graph length
+					std::list<int> path = graph::computePath(links, iter->first, iter->second);
+					if(!path.empty() && (int)path.size() <= minimumGraphDistance)
+					{
+						iter = clusters.erase(iter);
+					}
+					else
+					{
+						++iter;
+					}
+				}
+			}
+			progressDialog->appendText(tr("Filtered %1/%2 clusters for too close nodes (below minimum graph distance=%3).")
+				.arg(clusterBefore-clusters.size()).arg(clusterBefore).arg(minimumGraphDistance));
+			QApplication::processEvents();
+		}
+
+		progressDialog->setMaximumSteps(progressDialog->maximumSteps()+(int)clusters.size());
+		QApplication::processEvents();
+
 		std::set<int> addedLinks;
 		int i=0;
 		for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end() && !progressDialog->isCanceled(); ++iter, ++i)
@@ -4674,10 +4738,17 @@ void DatabaseViewer::graphNodeSelected(int id)

 void DatabaseViewer::graphLinkSelected(int from, int to)
 {
-	if(from>0 && idToIndex_.contains(from))
-		ui_->horizontalSlider_A->setValue(idToIndex_.value(from));
-	if(to>0 && idToIndex_.contains(to))
-		ui_->horizontalSlider_B->setValue(idToIndex_.value(to));
+	if(from < 0 || to < 0)
+	{
+		updateLoopClosuresSlider(from, to);
+	}
+	else
+	{
+		if(idToIndex_.contains(from))
+			ui_->horizontalSlider_A->setValue(idToIndex_.value(from));
+		if(idToIndex_.contains(to))
+			ui_->horizontalSlider_B->setValue(idToIndex_.value(to));
+	}
 }

 void DatabaseViewer::sliderAValueChanged(int value)
@@ -6137,6 +6208,14 @@ void DatabaseViewer::updateWordsMatching(const std::vector<int> & inliers)
 							kptB->keypoint().pt.y,
 							cB);
 				}
+				else if(ids[i]<0)
+				{
+					ui_->graphicsView_A->setFeatureColor(ids[i], Qt::gray);
+				}
+			}
+			for(auto iter = wordsB.begin(); iter.key()<0 && iter!=wordsB.end(); ++iter)
+			{
+				ui_->graphicsView_B->setFeatureColor(iter.key(), Qt::gray);
 			}
 			ui_->graphicsView_A->update();
 			ui_->graphicsView_B->update();
@@ -8017,6 +8096,17 @@ void DatabaseViewer::updateGraphView()
 				ui_->label_timeOptimization->setNum(0);
 				ui_->label_poses->setNum((int)optPoses.size());
 				graphes_.push_back(optPoses);
+				// Just get the links:
+				std::map<int, rtabmap::Transform> posesOut;
+				UINFO("Get connected graph from %d (%d poses, %d links)", fromId, (int)poses.size(), (int)links.size());
+				std::shared_ptr<Optimizer> optimizer(Optimizer::create(parameters));
+				optimizer->getConnectedGraph(
+						fromId,
+						optPoses,
+						links,
+						posesOut,
+						graphLinks_);
+				UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)graphLinks_.size());
 			}
 			ui_->horizontalSlider_rotation->setEnabled(false);
 			ui_->pushButton_applyRotation->setEnabled(false);
@@ -3703,7 +3703,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 							0,
 							0,
 							0,
-							_ui->checkBox_fromDepth->isChecked()?&confidence:0);
+							_ui->checkBox_fromDepth->isChecked()&&_ui->spinBox_depthConfidence->value()>0?&confidence:0);
 				}
 				else if(_dbDriver)
 				{
@@ -3717,7 +3717,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 							0,
 							0,
 							0,
-							_ui->checkBox_fromDepth->isChecked()?&confidence:0);
+							_ui->checkBox_fromDepth->isChecked()&&_ui->spinBox_depthConfidence->value()>0?&confidence:0);
 				}

 				if(_ui->checkBox_fromDepth->isChecked() && !data.imageRaw().empty() && !data.depthOrRightRaw().empty())
@@ -1255,11 +1255,11 @@ void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, c
 	{
 		if (xRatio > 0 && yRatio > 0)
 		{
-			addFeature(iter->first, iter->second, util2d::getDepth(depth, iter->second.pt.x*xRatio, iter->second.pt.y*yRatio, false), color);
+			addFeature(iter->first, iter->second, util2d::getDepth(depth, iter->second.pt.x*xRatio, iter->second.pt.y*yRatio, false), iter->first<0?Qt::gray:color);
 		}
 		else
 		{
-			addFeature(iter->first, iter->second, 0, color);
+			addFeature(iter->first, iter->second, 0, iter->first<0?Qt::gray:color);
 		}
 	}

@@ -1401,7 +1401,7 @@ void ImageView::setImageDepth(const cv::Mat & imageDepth, const cv::Mat & imageD
 							cv::Point3f pt;
 							int cameraIndex = u/subImageWidth;
 							UASSERT(cameraIndex>=0 && cameraIndex < (int)models.size() && subImageWidth == models[cameraIndex].imageWidth());
-							models[cameraIndex].project(u,v,float(val)/1000.0f, pt.x, pt.y, pt.z);
+							models[cameraIndex].project(u-(cameraIndex*subImageWidth),v,float(val)/1000.0f, pt.x, pt.y, pt.z);
 							pt = util3d::transformPoint(pt, models[cameraIndex].localTransform());
 							val = (unsigned short)(pt.z*1000.0f);
 						}
@@ -1417,7 +1417,7 @@ void ImageView::setImageDepth(const cv::Mat & imageDepth, const cv::Mat & imageD
 							cv::Point3f pt;
 							int cameraIndex = u/subImageWidth;
 							UASSERT(cameraIndex>=0 && cameraIndex < (int)models.size() && subImageWidth == models[cameraIndex].imageWidth());
-							models[cameraIndex].project(u,v,val, pt.x, pt.y, pt.z);
+							models[cameraIndex].project(u-(cameraIndex*subImageWidth),v,val, pt.x, pt.y, pt.z);
 							pt = util3d::transformPoint(pt, models[cameraIndex].localTransform());
 							val = pt.z;
 						}
@@ -3313,7 +3313,7 @@ void MainWindow::updateMapCloud(
 							{
 								std::string gtFrustumId = uFormat("f_gt_%d", iter->first);
 								color = Qt::gray;
-								_cloudViewer->addOrUpdateFrustum(gtFrustumId, _currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
+								_cloudViewer->addOrUpdateFrustum(gtFrustumId, mapToGt*_currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
 							}
 						}
 					}
@@ -6563,6 +6563,7 @@ void MainWindow::showPostProcessingDialog()
 			_postProcessingDialog->iterations(),
 			_postProcessingDialog->interSession(),
 			_postProcessingDialog->intraSession(),
+			_postProcessingDialog->minGraphDistance(),
 			_postProcessingDialog->isSBA(),
 			_postProcessingDialog->sbaIterations(),
 			_postProcessingDialog->sbaVariance(),
@@ -6579,6 +6580,7 @@ void MainWindow::postProcessing(
 		int iterations,
 		bool interSession,
 		bool intraSession,
+		int minGraphDistance,
 		bool sba,
 		int sbaIterations,
 		double sbaVariance,
@@ -6687,6 +6689,7 @@ void MainWindow::postProcessing(
 		{
 			odomMaxInf = graph::getMaxOdomInf(_currentLinksMap);
 		}
+		std::multimap<int, Link> neigborLinks = graph::filterLinks(_currentLinksMap, Link::kNeighbor, true);

 		std::shared_ptr<Registration> registration(Registration::create(parameters));

@@ -6705,6 +6708,34 @@ void MainWindow::postProcessing(
 			_progressDialog->appendText(tr("Looking for more loop closures, clustering poses... found %1 clusters.").arg(clusters.size()));
 			QApplication::processEvents();

+			if(minGraphDistance > 1)
+			{
+				int clustersBefore = clusters.size();
+				for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end();)
+				{
+					if(abs(iter->first - iter->second) < minGraphDistance)
+					{
+						iter = clusters.erase(iter);
+					}
+					else
+					{
+						// compute path to know how far we are in terms of graph length
+						std::list<int> path = graph::computePath(neigborLinks, iter->first, iter->second);
+						if(!path.empty() && (int)path.size() <= minGraphDistance)
+						{
+							iter = clusters.erase(iter);
+						}
+						else
+						{
+							++iter;
+						}
+					}
+				}
+				_progressDialog->appendText(tr("Filtered %1/%2 clusters for too close nodes (below minimum graph distance=%3).")
+					.arg(clustersBefore-clusters.size()).arg(clustersBefore).arg(minGraphDistance));
+				QApplication::processEvents();
+			}
+
 			int i=0;
 			std::set<int> addedLinks;
 			for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end() && !_progressCanceled; ++iter, ++i)
@@ -82,6 +82,7 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
 	connect(_ui->iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->intraSession, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->interSession, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->minGraphDistance, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));

@@ -150,6 +151,7 @@ void PostProcessingDialog::saveSettings(QSettings & settings, const QString & gr
 	settings.setValue("iterations", this->iterations());
 	settings.setValue("intra_session", this->intraSession());
 	settings.setValue("inter_session", this->interSession());
+	settings.setValue("min_graph_distance", this->minGraphDistance());
 	settings.setValue("refine_neigbors", this->isRefineNeighborLinks());
 	settings.setValue("refine_lc", this->isRefineLoopClosureLinks());
 	settings.setValue("sba", this->isSBA());
@@ -175,6 +177,7 @@ void PostProcessingDialog::loadSettings(QSettings & settings, const QString & gr
 	this->setIterations(settings.value("iterations", this->iterations()).toInt());
 	this->setIntraSession(settings.value("intra_session", this->intraSession()).toBool());
 	this->setInterSession(settings.value("inter_session", this->interSession()).toBool());
+	this->setMinGraphDistance(settings.value("min_graph_distance", this->minGraphDistance()).toInt());
 	this->setRefineNeighborLinks(settings.value("refine_neigbors", this->isRefineNeighborLinks()).toBool());
 	this->setRefineLoopClosureLinks(settings.value("refine_lc", this->isRefineLoopClosureLinks()).toBool());
 	this->setSBA(settings.value("sba", this->isSBA()).toBool());
@@ -197,6 +200,7 @@ void PostProcessingDialog::restoreDefaults()
 	setIterations(5);
 	setIntraSession(true);
 	setInterSession(true);
+	setMinGraphDistance(10);
 	setRefineNeighborLinks(false);
 	setRefineLoopClosureLinks(false);
 	setSBA(false);
@@ -254,6 +258,11 @@ bool PostProcessingDialog::interSession() const
 	return _ui->interSession->isChecked();
 }

+int PostProcessingDialog::minGraphDistance() const
+{
+	return _ui->minGraphDistance->value();
+}
+
 bool PostProcessingDialog::isRefineNeighborLinks() const
 {
 	return _ui->refineNeighborLinks->isChecked();
@@ -311,6 +320,10 @@ void PostProcessingDialog::setInterSession(bool enabled)
 {
 	_ui->interSession->setChecked(enabled);
 }
+void PostProcessingDialog::setMinGraphDistance(int value)
+{
+	_ui->minGraphDistance->setValue(value);
+}
 void PostProcessingDialog::setRefineNeighborLinks(bool on)
 {
 	_ui->refineNeighborLinks->setChecked(on);
@@ -171,6 +171,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->sift_label_gpu->setEnabled(false);
 	_ui->sift_doubleSpinBox_gaussianDiffThreshold->setEnabled(false);
 	_ui->sift_label_gaussianThreshold->setEnabled(false);
+	_ui->sift_doubleSpinBox_maxGaussianDiffThreshold->setEnabled(false);
+	_ui->sift_label_maxGaussianThreshold->setEnabled(false);
 	_ui->sift_checkBox_upscale->setEnabled(false);
 	_ui->sift_label_upscale->setEnabled(false);
 #endif
@@ -755,10 +757,13 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->source_checkBox_ignoreLandmarks, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_ignoreFeatures, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_ignorePriors, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->source_checkBox_ignoreIMU, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_databaseStartId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_databaseStopId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_lineEdit_databaseCameraIndex, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->source_checkBox_overrideLocalTransforms, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->source_lineEdit_databaseLocalTransformOffset, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_stereoToDepthDB, SIGNAL(toggled(bool)), _ui->checkbox_stereo_depthGenerated, SLOT(setChecked(bool)));
 	connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(toggled(bool)), _ui->source_checkBox_stereoToDepthDB, SLOT(setChecked(bool)));

@@ -847,6 +852,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->comboBox_cameraImages_odomFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraImages_gt, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_cameraImages_gtFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_cameraImages_gt_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_maxPoseTimeDiff, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraImages_path_imu, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraImages_imu_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1067,6 +1073,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :

 	// Database
 	_ui->checkBox_dbInMemory->setObjectName(Parameters::kDbSqlite3InMemory().c_str());
+	_ui->checkBox_dbReadOnly->setObjectName(Parameters::kMemLocalizationReadOnly().c_str());
 	_ui->spinBox_dbCacheSize->setObjectName(Parameters::kDbSqlite3CacheSize().c_str());
 	_ui->comboBox_dbJournalMode->setObjectName(Parameters::kDbSqlite3JournalMode().c_str());
 	_ui->comboBox_dbSynchronous->setObjectName(Parameters::kDbSqlite3Synchronous().c_str());
@@ -1140,6 +1147,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->sift_checkBox_rootsift->setObjectName(Parameters::kSIFTRootSIFT().c_str());
 	_ui->sift_checkBox_gpu->setObjectName(Parameters::kSIFTGpu().c_str());
 	_ui->sift_doubleSpinBox_gaussianDiffThreshold->setObjectName(Parameters::kSIFTGaussianThreshold().c_str());
+	_ui->sift_doubleSpinBox_maxGaussianDiffThreshold->setObjectName(Parameters::kSIFTMaxGaussianThreshold().c_str());
 	_ui->sift_checkBox_upscale->setObjectName(Parameters::kSIFTUpscale().c_str());

 	//BRIEF descriptor
@@ -1354,6 +1362,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->odom_flow_iterations->setObjectName(Parameters::kVisCorFlowIterations().c_str());
 	_ui->odom_flow_eps->setObjectName(Parameters::kVisCorFlowEps().c_str());
 	_ui->odom_flow_gpu->setObjectName(Parameters::kVisCorFlowGpu().c_str());
+	_ui->odom_flow_useMinEigenVals->setObjectName(Parameters::kVisCorFlowUseMinEigenVals().c_str());
+	_ui->odom_flow_minEigThreshold->setObjectName(Parameters::kVisCorFlowMinEigThreshold().c_str());
+	_ui->odom_flow_errorThreshold->setObjectName(Parameters::kVisCorFlowErrorThreshold().c_str());
 	_ui->loopClosure_bundle->setObjectName(Parameters::kVisBundleAdjustment().c_str());

 	//RegistrationIcp
@@ -1674,6 +1685,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->stereo_maxDisparity->setObjectName(Parameters::kStereoMaxDisparity().c_str());
 	_ui->stereo_ssd->setObjectName(Parameters::kStereoSSD().c_str());
 	_ui->stereo_flow_eps->setObjectName(Parameters::kStereoEps().c_str());
+	_ui->stereo_flow_useMinEigenVals->setObjectName(Parameters::kStereoUseMinEigenVals().c_str());
+	_ui->stereo_flow_minEigThreshold->setObjectName(Parameters::kStereoMinEigThreshold().c_str());
+	_ui->stereo_flow_errorThreshold->setObjectName(Parameters::kStereoErrorThreshold().c_str());
 	_ui->stereo_opticalFlow->setObjectName(Parameters::kStereoOpticalFlow().c_str());
 	_ui->stereo_flow_gpu->setObjectName(Parameters::kStereoGpu().c_str());

@@ -2194,10 +2208,13 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->source_checkBox_ignoreLandmarks->setChecked(true);
 		_ui->source_checkBox_ignoreFeatures->setChecked(true);
 		_ui->source_checkBox_ignorePriors->setChecked(false);
+		_ui->source_checkBox_ignoreIMU->setChecked(false);
 		_ui->source_spinBox_databaseStartId->setValue(0);
 		_ui->source_spinBox_databaseStopId->setValue(0);
 		_ui->source_lineEdit_databaseCameraIndex->setText("");
 		_ui->source_checkBox_useDbStamps->setChecked(true);
+		_ui->source_checkBox_overrideLocalTransforms->setChecked(false);
+		_ui->source_lineEdit_databaseLocalTransformOffset->setText("");

 #ifdef _WIN32
 		_ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI2-kSrcRGBD); // openni2
@@ -2347,6 +2364,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->comboBox_cameraImages_odomFormat->setCurrentIndex(0);
 		_ui->lineEdit_cameraImages_gt->setText("");
 		_ui->comboBox_cameraImages_gtFormat->setCurrentIndex(0);
+		_ui->lineEdit_cameraImages_gt_transform->setText("0 0 0 0 0 0");
 		_ui->doubleSpinBox_maxPoseTimeDiff->setValue(0.02);
 		_ui->lineEdit_cameraImages_path_imu->setText("");
 		_ui->lineEdit_cameraImages_imu_transform->setText("0 0 1 0 -1 0 1 0 0");
@@ -2881,6 +2899,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->comboBox_cameraImages_odomFormat->setCurrentIndex(settings.value("odom_format", _ui->comboBox_cameraImages_odomFormat->currentIndex()).toInt());
 	_ui->lineEdit_cameraImages_gt->setText(settings.value("gt_path", _ui->lineEdit_cameraImages_gt->text()).toString());
 	_ui->comboBox_cameraImages_gtFormat->setCurrentIndex(settings.value("gt_format", _ui->comboBox_cameraImages_gtFormat->currentIndex()).toInt());
+	_ui->lineEdit_cameraImages_gt_transform->setText(settings.value("gt_transform", _ui->lineEdit_cameraImages_gt_transform->text()).toString());
 	_ui->doubleSpinBox_maxPoseTimeDiff->setValue(settings.value("max_pose_time_diff", _ui->doubleSpinBox_maxPoseTimeDiff->value()).toDouble());

 	_ui->lineEdit_cameraImages_path_imu->setText(settings.value("imu_path", _ui->lineEdit_cameraImages_path_imu->text()).toString());
@@ -2951,10 +2970,14 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->source_checkBox_ignoreLandmarks->setChecked(settings.value("ignoreLandmarks", _ui->source_checkBox_ignoreLandmarks->isChecked()).toBool());
 	_ui->source_checkBox_ignoreFeatures->setChecked(settings.value("ignoreFeatures", _ui->source_checkBox_ignoreFeatures->isChecked()).toBool());
 	_ui->source_checkBox_ignorePriors->setChecked(settings.value("ignorePriors", _ui->source_checkBox_ignorePriors->isChecked()).toBool());
+	_ui->source_checkBox_ignoreIMU->setChecked(settings.value("ignoreImu", _ui->source_checkBox_ignoreIMU->isChecked()).toBool());
+	
 	_ui->source_spinBox_databaseStartId->setValue(settings.value("startId", _ui->source_spinBox_databaseStartId->value()).toInt());
 	_ui->source_spinBox_databaseStopId->setValue(settings.value("stopId", _ui->source_spinBox_databaseStopId->value()).toInt());
 	_ui->source_lineEdit_databaseCameraIndex->setText(settings.value("cameraIndices", _ui->source_lineEdit_databaseCameraIndex->text()).toString());
 	_ui->source_checkBox_useDbStamps->setChecked(settings.value("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked()).toBool());
+	_ui->source_checkBox_overrideLocalTransforms->setChecked(settings.value("overrideLocalTransforms", _ui->source_checkBox_overrideLocalTransforms->isChecked()).toBool());
+	_ui->source_lineEdit_databaseLocalTransformOffset->setText(settings.value("localTransformOffsets", _ui->source_lineEdit_databaseLocalTransformOffset->text()).toString());
 	settings.endGroup(); // Database

 	settings.endGroup(); // Camera
@@ -3497,6 +3520,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("odom_format",         _ui->comboBox_cameraImages_odomFormat->currentIndex());
 	settings.setValue("gt_path",             _ui->lineEdit_cameraImages_gt->text());
 	settings.setValue("gt_format",           _ui->comboBox_cameraImages_gtFormat->currentIndex());
+	settings.setValue("gt_transform",        _ui->lineEdit_cameraImages_gt_transform->text());
 	settings.setValue("max_pose_time_diff",  _ui->doubleSpinBox_maxPoseTimeDiff->value());
 	settings.setValue("imu_path",            _ui->lineEdit_cameraImages_path_imu->text());
 	settings.setValue("imu_local_transform", _ui->lineEdit_cameraImages_imu_transform->text());
@@ -3566,10 +3590,13 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("ignoreLandmarks", _ui->source_checkBox_ignoreLandmarks->isChecked());
 	settings.setValue("ignoreFeatures",  _ui->source_checkBox_ignoreFeatures->isChecked());
 	settings.setValue("ignorePriors",  _ui->source_checkBox_ignorePriors->isChecked());
+	settings.setValue("ignoreImu",  _ui->source_checkBox_ignoreIMU->isChecked());
 	settings.setValue("startId",          _ui->source_spinBox_databaseStartId->value());
 	settings.setValue("stopId",          _ui->source_spinBox_databaseStopId->value());
 	settings.setValue("cameraIndices",       _ui->source_lineEdit_databaseCameraIndex->text());
 	settings.setValue("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked());
+	settings.setValue("overrideLocalTransforms", _ui->source_checkBox_overrideLocalTransforms->isChecked());
+	settings.setValue("localTransformOffsets", _ui->source_lineEdit_databaseLocalTransformOffset->text());
 	settings.endGroup(); // Database

 	settings.endGroup(); // Camera
@@ -6505,6 +6532,15 @@ Transform PreferencesDialog::getLaserLocalTransform() const
 	}
 	return t;
 }
+Transform PreferencesDialog::getGroundTruthLocalTransform() const
+{
+	Transform t = Transform::fromString(_ui->lineEdit_cameraImages_gt_transform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString());
+	if(t.isNull())
+	{
+		return Transform::getIdentity();
+	}
+	return t;
+}

 QString PreferencesDialog::getIMUPath() const
 {
@@ -6871,7 +6907,7 @@ Camera * PreferencesDialog::createCamera(
 		((CameraRGBDImages*)camera)->setMaxFrames(_ui->spinBox_cameraRGBDImages_maxFrames->value());
 		((CameraRGBDImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
 		((CameraRGBDImages*)camera)->setOdometryPath(_ui->lineEdit_cameraImages_odom->text().toStdString(), _ui->comboBox_cameraImages_odomFormat->currentIndex());
-		((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
+		((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex(), this->getGroundTruthLocalTransform());
 		((CameraRGBDImages*)camera)->setMaxPoseTimeDiff(_ui->doubleSpinBox_maxPoseTimeDiff->value());
 		((CameraRGBDImages*)camera)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
@@ -6917,7 +6953,7 @@ Camera * PreferencesDialog::createCamera(
 		((CameraStereoImages*)camera)->setMaxFrames(_ui->spinBox_cameraStereoImages_maxFrames->value());
 		((CameraStereoImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
 		((CameraStereoImages*)camera)->setOdometryPath(_ui->lineEdit_cameraImages_odom->text().toStdString(), _ui->comboBox_cameraImages_odomFormat->currentIndex());
-		((CameraStereoImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
+		((CameraStereoImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex(), this->getGroundTruthLocalTransform());
 		((CameraStereoImages*)camera)->setMaxPoseTimeDiff(_ui->doubleSpinBox_maxPoseTimeDiff->value());
 		((CameraStereoImages*)camera)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
@@ -7117,7 +7153,8 @@ Camera * PreferencesDialog::createCamera(
 				_ui->comboBox_cameraImages_odomFormat->currentIndex());
 		((CameraImages*)camera)->setGroundTruthPath(
 				_ui->lineEdit_cameraImages_gt->text().toStdString(),
-				_ui->comboBox_cameraImages_gtFormat->currentIndex());
+				_ui->comboBox_cameraImages_gtFormat->currentIndex(),
+				this->getGroundTruthLocalTransform());
 		((CameraImages*)camera)->setMaxPoseTimeDiff(_ui->doubleSpinBox_maxPoseTimeDiff->value());
 		((CameraImages*)camera)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
@@ -7171,6 +7208,54 @@ Camera * PreferencesDialog::createCamera(
 				}
 			}
 		}
+
+		std::vector<Transform> localTransformOverrides;
+		if(_ui->source_checkBox_overrideLocalTransforms->isChecked())
+		{
+			if(!_ui->lineEdit_sourceLocalTransform->text().isEmpty())
+			{
+				std::list<std::string> transforms = uSplit(_ui->lineEdit_sourceLocalTransform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString(), ';');
+				for(auto t: transforms)
+				{
+					localTransformOverrides.push_back(Transform::fromString(t));
+				}
+
+				// offset(s)?
+				if(!_ui->source_lineEdit_databaseLocalTransformOffset->text().isEmpty())
+				{
+					std::vector<float> localTransformOffsetOverrides;
+					std::list<std::string> offsetStr = uSplit(_ui->source_lineEdit_databaseLocalTransformOffset->text().toStdString(), ' ');
+					for(std::list<std::string>::iterator iter=offsetStr.begin(); iter!=offsetStr.end(); ++iter)
+					{
+						localTransformOffsetOverrides.push_back(uStr2Float(*iter));
+						UINFO("Camera offset = %f", localTransformOffsetOverrides.back());
+					}
+					if(!localTransformOffsetOverrides.empty())
+					{
+						if(!localTransformOverrides.empty() && localTransformOffsetOverrides.size() > 1 && localTransformOffsetOverrides.size() != localTransformOverrides.size())
+						{
+							QMessageBox::warning(this, tr("DBReader"),
+								tr( "Camera lens offset vector size (%1) is not equal to local transform overrides (%2). "
+									"Camera lens offset vector should be one to affect all cameras or the same size than local transforms overrides.").arg(localTransformOffsetOverrides.size()).arg(localTransformOverrides.size()), QMessageBox::Ok);
+							return 0;
+						}
+						else {
+							for(size_t i=0; i<localTransformOverrides.size(); ++i)
+							{
+								float offset = localTransformOffsetOverrides.size()==1?localTransformOffsetOverrides[0]:localTransformOffsetOverrides[i];
+								localTransformOverrides[i] *= Transform(0, offset, 0);
+								UINFO("Overriding camera's local transform %ld to %s (offset=%f)", i, localTransformOverrides[i].prettyPrint().c_str(), offset);
+							}
+						}
+					}
+				}
+			}
+			else if(!_ui->source_lineEdit_databaseLocalTransformOffset->text().isEmpty())
+			{
+				UWARN("Overriding camera offsets can only be used when camera local transforms are overriden. Ignoring offsets :\"%s\"", 
+					_ui->source_lineEdit_databaseLocalTransformOffset->text().toStdString().c_str());
+			}
+		}
 		
 		camera = new DBReader(_ui->source_database_lineEdit_path->text().toStdString(),
 				_ui->source_checkBox_useDbStamps->isChecked()?-1:this->getGeneralInputRate(),
@@ -7185,7 +7270,9 @@ Camera * PreferencesDialog::createCamera(
 				_ui->source_checkBox_ignoreFeatures->isChecked(),
 				0,
 				-1,
-				_ui->source_checkBox_ignorePriors->isChecked());
+				_ui->source_checkBox_ignorePriors->isChecked(),
+				_ui->source_checkBox_ignoreIMU->isChecked(),
+				localTransformOverrides);
 	}
 	else
 	{
@@ -69,6 +69,7 @@ StatItem::StatItem(const QString & name, bool cacheOn, const std::vector<qreal>
 		_y = y;
 	}
 	_unit->setText(unit);
+	_value->setTextFormat(Qt::PlainText);
 	this->updateMenu(menu);
 }

@@ -90,7 +91,6 @@ void StatItem::addValue(qreal y)
 	{
 		_y.push_back(y);
 	}
-	_value->setText(QString::number(y, 'g', 3));
 	Q_EMIT valueAdded(y);
 }

@@ -107,7 +107,6 @@ void StatItem::addValue(qreal x, qreal y)
 		_x.push_back(x);
 	}

-	_value->setText(QString::number(y, 'g', 3));
 	Q_EMIT valueAdded(x,y);
 }

@@ -117,18 +116,23 @@ void StatItem::setValues(const std::vector<qreal> & x, const std::vector<qreal>
 	{
 		_x = x;
 		_y = y;
-		if(y.size())
-		{
-			_value->setNum(y[y.size()-1]);
-		}
-	}
-	else
-	{
-		_value->setText("*");
 	}
 	Q_EMIT valuesChanged(x,y);
 }

+void StatItem::updateLabel()
+{
+	QString newText;
+	if(_y.size())
+	{
+		newText = QString::number(_y.back(), 'g', 3);
+	}
+	if(newText != _value->text())
+	{
+		_value->setText(newText);
+	}
+}
+
 QString StatItem::value() const
 {
 	return _value->text();
@@ -227,6 +231,8 @@ StatsToolBox::StatsToolBox(QWidget * parent) :
 	_plotMenu->addAction(tr("<New figure>"));
 	_workingDirectory = QDir::homePath();
 	_newFigureMaxItems = 0;
+	_updateLabelsTimer.setSingleShot(true);
+	connect(&_updateLabelsTimer, &QTimer::timeout, this, &StatsToolBox::updateLabels);
 }

 StatsToolBox::~StatsToolBox()
@@ -263,6 +269,7 @@ void StatsToolBox::updateStat(const QString & statFullName, qreal y, bool cacheO
 	std::vector<qreal> vx,vy(1);
 	vy[0] = y;
 	updateStat(statFullName, vx, vy, cacheOn);
+	requestLabelsUpdate();
 }

 void StatsToolBox::updateStat(const QString & statFullName, qreal x, qreal y, bool cacheOn)
@@ -271,6 +278,7 @@ void StatsToolBox::updateStat(const QString & statFullName, qreal x, qreal y, bo
 	vx[0] = x;
 	vy[0] = y;
 	updateStat(statFullName, vx, vy, cacheOn);
+	requestLabelsUpdate();
 }

 void StatsToolBox::updateStat(const QString & statFullName, const std::vector<qreal> & x, const std::vector<qreal> & y, bool cacheOn)
@@ -295,6 +303,7 @@ void StatsToolBox::updateStat(const QString & statFullName, const std::vector<qr
 		{
 			item->setValues(x, y);
 		}
+		requestLabelsUpdate();
 	}
 	else
 	{
@@ -378,6 +387,22 @@ void StatsToolBox::updateStat(const QString & statFullName, const std::vector<qr
 	}
 }

+void StatsToolBox::requestLabelsUpdate()
+{
+	if(!_updateLabelsTimer.isActive())
+	{
+		_updateLabelsTimer.start(100); // Max 10 Hz
+	}
+}
+void StatsToolBox::updateLabels()
+{
+	QList<StatItem *> items = _statBox->findChildren<StatItem *>();
+	for(int i=0; i<items.size(); ++i)
+	{
+		items[i]->updateLabel();
+	}
+}
+
 void StatsToolBox::plot(const StatItem * stat, const QString & plotName)
 {
 	QWidget * fig = _figures.value(plotName, (QWidget*)0);
@@ -61,7 +61,7 @@
          <rect>
           <x>0</x>
           <y>0</y>
-           <width>307</width>
+           <width>364</width>
           <height>389</height>
          </rect>
         </property>
@@ -392,7 +392,7 @@
          <rect>
           <x>0</x>
           <y>0</y>
-           <width>306</width>
+           <width>364</width>
           <height>389</height>
          </rect>
         </property>
@@ -1722,14 +1722,14 @@
     <item>
      <widget class="QToolBox" name="toolBox">
       <property name="currentIndex">
-        <number>1</number>
+        <number>2</number>
       </property>
       <widget class="QWidget" name="page_3">
        <property name="geometry">
         <rect>
          <x>0</x>
          <y>0</y>
-          <width>314</width>
+          <width>403</width>
          <height>188</height>
         </rect>
        </property>
@@ -2604,15 +2604,133 @@
        <property name="geometry">
         <rect>
          <x>0</x>
-          <y>0</y>
-          <width>222</width>
-          <height>306</height>
+          <y>-175</y>
+          <width>403</width>
+          <height>319</height>
         </rect>
        </property>
        <attribute name="label">
         <string>Detect more loop closures</string>
        </attribute>
        <layout class="QGridLayout" name="gridLayout_10" columnstretch="0,1">
+         <item row="2" column="1">
+          <widget class="QLabel" name="label_30">
+           <property name="text">
+            <string>Angle</string>
+           </property>
+          </widget>
+         </item>
+         <item row="5" column="1">
+          <widget class="QLabel" name="label_34">
+           <property name="text">
+            <string>Inter-session</string>
+           </property>
+          </widget>
+         </item>
+         <item row="5" column="0">
+          <widget class="QCheckBox" name="checkBox_detectMore_interSession">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
+         <item row="0" column="1">
+          <widget class="QLabel" name="label_29">
+           <property name="text">
+            <string>Radius Max</string>
+           </property>
+          </widget>
+         </item>
+         <item row="4" column="1">
+          <widget class="QLabel" name="label_32">
+           <property name="text">
+            <string>Intra-session</string>
+           </property>
+          </widget>
+         </item>
+         <item row="9" column="0">
+          <spacer name="verticalSpacer_5">
+           <property name="orientation">
+            <enum>Qt::Vertical</enum>
+           </property>
+           <property name="sizeHint" stdset="0">
+            <size>
+             <width>20</width>
+             <height>40</height>
+            </size>
+           </property>
+          </spacer>
+         </item>
+         <item row="2" column="0">
+          <widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_angle">
+           <property name="suffix">
+            <string> degrees</string>
+           </property>
+           <property name="decimals">
+            <number>0</number>
+           </property>
+           <property name="maximum">
+            <double>180.000000000000000</double>
+           </property>
+           <property name="singleStep">
+            <double>1.000000000000000</double>
+           </property>
+           <property name="value">
+            <double>30.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="1" column="1">
+          <widget class="QLabel" name="label_36">
+           <property name="text">
+            <string>Radius Min</string>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="0">
+          <widget class="QSpinBox" name="spinBox_detectMore_iterations">
+           <property name="minimum">
+            <number>1</number>
+           </property>
+           <property name="maximum">
+            <number>100</number>
+           </property>
+           <property name="value">
+            <number>5</number>
+           </property>
+          </widget>
+         </item>
+         <item row="7" column="1">
+          <widget class="QLabel" name="label_38">
+           <property name="text">
+            <string>From/to map ID only (-1 is all)</string>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="1">
+          <widget class="QLabel" name="label_31">
+           <property name="text">
+            <string>Iterations</string>
+           </property>
+          </widget>
+         </item>
+         <item row="6" column="0">
+          <widget class="QCheckBox" name="checkBox_opt_graph_as_guess">
+           <property name="text">
+            <string/>
+           </property>
+           <property name="checked">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="4" column="0">
+          <widget class="QCheckBox" name="checkBox_detectMore_intraSession">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
         <item row="1" column="0">
          <widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_radiusMin">
           <property name="suffix">
@@ -2632,63 +2750,13 @@
           </property>
          </widget>
         </item>
-         <item row="1" column="1">
-          <widget class="QLabel" name="label_36">
+         <item row="6" column="1">
+          <widget class="QLabel" name="label_37">
           <property name="text">
-            <string>Radius Min</string>
+            <string>Use optimized graph as guess</string>
           </property>
-          </widget>
-         </item>
-         <item row="4" column="1">
-          <widget class="QLabel" name="label_32">
-           <property name="text">
-            <string>Intra-session</string>
-           </property>
-          </widget>
-         </item>
-         <item row="7" column="0">
-          <spacer name="verticalSpacer_5">
-           <property name="orientation">
-            <enum>Qt::Vertical</enum>
-           </property>
-           <property name="sizeHint" stdset="0">
-            <size>
-             <width>20</width>
-             <height>40</height>
-            </size>
-           </property>
-          </spacer>
-         </item>
-         <item row="0" column="1">
-          <widget class="QLabel" name="label_29">
-           <property name="text">
-            <string>Radius Max</string>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="1">
-          <widget class="QLabel" name="label_30">
-           <property name="text">
-            <string>Angle</string>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="0">
-          <widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_angle">
-           <property name="suffix">
-            <string> degrees</string>
-           </property>
-           <property name="decimals">
-            <number>0</number>
-           </property>
-           <property name="maximum">
-            <double>180.000000000000000</double>
-           </property>
-           <property name="singleStep">
-            <double>1.000000000000000</double>
-           </property>
-           <property name="value">
-            <double>30.000000000000000</double>
+           <property name="wordWrap">
+            <bool>true</bool>
           </property>
          </widget>
         </item>
@@ -2711,64 +2779,36 @@
           </property>
          </widget>
         </item>
-         <item row="3" column="0">
-          <widget class="QSpinBox" name="spinBox_detectMore_iterations">
+         <item row="7" column="0">
+          <widget class="QSpinBox" name="spinBox_fromToMapId">
+           <property name="minimum">
+            <number>-1</number>
+           </property>
+           <property name="maximum">
+            <number>9999</number>
+           </property>
+           <property name="value">
+            <number>-1</number>
+           </property>
+          </widget>
+         </item>
+         <item row="8" column="1">
+          <widget class="QLabel" name="label_40">
+           <property name="text">
+            <string>Minimum graph distance</string>
+           </property>
+          </widget>
+         </item>
+         <item row="8" column="0">
+          <widget class="QSpinBox" name="spinBox_minGraphDistance">
           <property name="minimum">
            <number>1</number>
           </property>
           <property name="maximum">
-            <number>100</number>
+            <number>9999</number>
           </property>
           <property name="value">
-            <number>5</number>
-           </property>
-          </widget>
-         </item>
-         <item row="4" column="0">
-          <widget class="QCheckBox" name="checkBox_detectMore_intraSession">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="1">
-          <widget class="QLabel" name="label_31">
-           <property name="text">
-            <string>Iterations</string>
-           </property>
-          </widget>
-         </item>
-         <item row="5" column="1">
-          <widget class="QLabel" name="label_34">
-           <property name="text">
-            <string>Inter-session</string>
-           </property>
-          </widget>
-         </item>
-         <item row="5" column="0">
-          <widget class="QCheckBox" name="checkBox_detectMore_interSession">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
-         <item row="6" column="0">
-          <widget class="QCheckBox" name="checkBox_opt_graph_as_guess">
-           <property name="text">
-            <string/>
-           </property>
-           <property name="checked">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="6" column="1">
-          <widget class="QLabel" name="label_37">
-           <property name="text">
-            <string>Use optimized graph as guess</string>
-           </property>
-           <property name="wordWrap">
-            <bool>true</bool>
+            <number>10</number>
           </property>
          </widget>
         </item>
@@ -2779,8 +2819,8 @@
         <rect>
          <x>0</x>
          <y>0</y>
-          <width>181</width>
-          <height>485</height>
+          <width>428</width>
+          <height>196</height>
         </rect>
        </property>
        <attribute name="label">
@@ -18,7 +18,7 @@
    <normaloff>:/images/RTAB-Map.ico</normaloff>:/images/RTAB-Map.ico</iconset>
  </property>
  <property name="dockOptions">
-   <set>QMainWindow::AllowNestedDocks|QMainWindow::AllowTabbedDocks|QMainWindow::AnimatedDocks|QMainWindow::VerticalTabs</set>
+   <set>QMainWindow::DockOption::AllowNestedDocks|QMainWindow::DockOption::AllowTabbedDocks|QMainWindow::DockOption::AnimatedDocks|QMainWindow::DockOption::VerticalTabs</set>
  </property>
  <widget class="QWidget" name="widget_mainWindow"/>
  <widget class="QMenuBar" name="menubar">
@@ -27,7 +27,7 @@
     <x>0</x>
     <y>0</y>
     <width>1012</width>
-     <height>22</height>
+     <height>21</height>
    </rect>
   </property>
   <widget class="QMenu" name="menuFile">
@@ -494,7 +494,7 @@
  </widget>
  <widget class="QDockWidget" name="dockWidget_statsV2">
   <property name="floating">
-    <bool>true</bool>
+    <bool>false</bool>
   </property>
   <property name="windowTitle">
    <string>Statistics</string>
@@ -522,7 +522,7 @@
     <item>
      <layout class="QGridLayout" name="gridLayout" columnstretch="10,0">
       <property name="sizeConstraint">
-        <enum>QLayout::SetMinimumSize</enum>
+        <enum>QLayout::SizeConstraint::SetMinimumSize</enum>
       </property>
       <property name="verticalSpacing">
        <number>2</number>
@@ -530,7 +530,7 @@
       <item row="3" column="1">
        <widget class="QDoubleSpinBox" name="doubleSpinBox_stats_timeLimit">
         <property name="alignment">
-          <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+          <set>Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter</set>
         </property>
         <property name="suffix">
          <string> ms</string>
@@ -562,14 +562,14 @@
          <string>Unknown</string>
         </property>
         <property name="alignment">
-          <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+          <set>Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter</set>
         </property>
        </widget>
       </item>
       <item row="1" column="1">
        <widget class="QDoubleSpinBox" name="doubleSpinBox_stats_imgRate">
         <property name="alignment">
-          <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+          <set>Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter</set>
         </property>
         <property name="suffix">
          <string> Hz</string>
@@ -604,7 +604,7 @@
          <string>Unknown</string>
         </property>
         <property name="alignment">
-          <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+          <set>Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter</set>
         </property>
        </widget>
       </item>
@@ -621,7 +621,7 @@
          <string>Unknown</string>
         </property>
         <property name="alignment">
-          <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+          <set>Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter</set>
         </property>
        </widget>
       </item>
@@ -638,7 +638,7 @@
          <string>0</string>
         </property>
         <property name="alignment">
-          <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+          <set>Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter</set>
         </property>
        </widget>
       </item>
@@ -656,7 +656,7 @@
          <string>0</string>
         </property>
         <property name="alignment">
-          <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+          <set>Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter</set>
         </property>
        </widget>
       </item>
@@ -673,7 +673,7 @@
          <string>0</string>
         </property>
         <property name="alignment">
-          <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+          <set>Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter</set>
         </property>
        </widget>
       </item>
@@ -697,7 +697,7 @@
       <item row="2" column="1">
        <widget class="QDoubleSpinBox" name="doubleSpinBox_stats_detectionRate">
         <property name="alignment">
-          <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+          <set>Qt::AlignmentFlag::AlignRight|Qt::AlignmentFlag::AlignTrailing|Qt::AlignmentFlag::AlignVCenter</set>
         </property>
         <property name="suffix">
          <string> Hz</string>
@@ -731,7 +731,7 @@
     <item>
      <spacer name="verticalSpacer_2">
       <property name="orientation">
-        <enum>Qt::Vertical</enum>
+        <enum>Qt::Orientation::Vertical</enum>
       </property>
       <property name="sizeHint" stdset="0">
        <size>
@@ -893,7 +893,7 @@
  </widget>
  <widget class="QDockWidget" name="dockWidget_mapVisibility">
   <property name="floating">
-    <bool>true</bool>
+    <bool>false</bool>
   </property>
   <property name="windowTitle">
    <string>Map visibility</string>
@@ -956,7 +956,7 @@
  </widget>
  <widget class="QDockWidget" name="dockWidget_imageView">
   <property name="allowedAreas">
-    <set>Qt::AllDockWidgetAreas</set>
+    <set>Qt::DockWidgetArea::AllDockWidgetAreas</set>
   </property>
   <property name="windowTitle">
    <string>Loop closure detection</string>
@@ -998,7 +998,7 @@
          <string/>
         </property>
         <property name="alignment">
-          <set>Qt::AlignCenter</set>
+          <set>Qt::AlignmentFlag::AlignCenter</set>
         </property>
        </widget>
       </item>
@@ -1024,7 +1024,7 @@
          <string/>
         </property>
         <property name="alignment">
-          <set>Qt::AlignCenter</set>
+          <set>Qt::AlignmentFlag::AlignCenter</set>
         </property>
        </widget>
       </item>
@@ -1422,6 +1422,9 @@
   </property>
  </action>
  <action name="actionClose_database">
+   <property name="enabled">
+    <bool>true</bool>
+   </property>
   <property name="icon">
    <iconset resource="../GuiLib.qrc">
     <normaloff>:/images/document-save.png</normaloff>:/images/document-save.png</iconset>
@@ -6,8 +6,8 @@
   <rect>
    <x>0</x>
    <y>0</y>
-    <width>552</width>
-    <height>633</height>
+    <width>553</width>
+    <height>662</height>
   </rect>
  </property>
  <property name="windowTitle">
@@ -67,10 +67,10 @@
          </property>
         </widget>
        </item>
-        <item row="0" column="1">
-         <widget class="QLabel" name="label_3">
+        <item row="4" column="1">
+         <widget class="QLabel" name="label_9">
          <property name="text">
-           <string>Cluster radius</string>
+           <string>Inter-session</string>
          </property>
          <property name="wordWrap">
           <bool>true</bool>
@@ -87,10 +87,10 @@
          </property>
         </widget>
        </item>
-        <item row="2" column="1">
-         <widget class="QLabel" name="label_6">
+        <item row="0" column="1">
+         <widget class="QLabel" name="label_3">
          <property name="text">
-           <string>Iterations</string>
+           <string>Cluster radius</string>
          </property>
          <property name="wordWrap">
           <bool>true</bool>
@@ -120,16 +120,6 @@
          </property>
         </widget>
        </item>
-        <item row="4" column="1">
-         <widget class="QLabel" name="label_9">
-          <property name="text">
-           <string>Inter-session</string>
-          </property>
-          <property name="wordWrap">
-           <bool>true</bool>
-          </property>
-         </widget>
-        </item>
        <item row="3" column="0">
         <widget class="QCheckBox" name="intraSession">
          <property name="text">
@@ -137,6 +127,16 @@
          </property>
         </widget>
        </item>
+        <item row="2" column="1">
+         <widget class="QLabel" name="label_6">
+          <property name="text">
+           <string>Iterations</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
        <item row="4" column="0">
         <widget class="QCheckBox" name="interSession">
          <property name="text">
@@ -144,6 +144,32 @@
          </property>
         </widget>
        </item>
+        <item row="5" column="1">
+         <widget class="QLabel" name="label_11">
+          <property name="text">
+           <string>Minimum graph distance</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
+        <item row="5" column="0">
+         <widget class="QSpinBox" name="minGraphDistance">
+          <property name="suffix">
+           <string> nodes</string>
+          </property>
+          <property name="minimum">
+           <number>1</number>
+          </property>
+          <property name="maximum">
+           <number>9999</number>
+          </property>
+          <property name="value">
+           <number>10</number>
+          </property>
+         </widget>
+        </item>
       </layout>
      </item>
      <item>
@@ -0,0 +1,18 @@
+# Try to apply the patch in reverse first to see if it's already there
+execute_process(
+    COMMAND git apply --reverse --check "${PATCH_FILE}"
+    RESULT_VARIABLE PATCH_APPLIED
+    OUTPUT_QUIET
+    ERROR_QUIET
+)
+
+if(NOT PATCH_APPLIED EQUAL 0)
+    # If not applied, apply it now with whitespace fixes for Windows
+    execute_process(
+        COMMAND git apply --ignore-whitespace --whitespace=fix "${PATCH_FILE}"
+        RESULT_VARIABLE RESULT
+    )
+    if(NOT RESULT EQUAL 0)
+        message(FATAL_ERROR "Failed to apply patch: ${PATCH_FILE}")
+    endif()
+endif()
@@ -0,0 +1,13 @@
+diff --git a/cmake/Config.cmake.in b/cmake/Config.cmake.in
+index 338ff8500..0b738ea3c 100644
+--- a/cmake/Config.cmake.in
+++ b/cmake/Config.cmake.in
+@@ -28,7 +28,7 @@ if(@GTSAM_USE_TBB@)
+ endif()
+ 
+ if(@GTSAM_USE_SYSTEM_EIGEN@)
+-find_dependency(Eigen3 REQUIRED)
+find_dependency(Eigen3 REQUIRED CONFIG)
+ endif()
+ 
+ # Load exports
@@ -0,0 +1,58 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index 2e9de0c..6a031c7 100644
+--- a/CMakeLists.txt
+++ b/CMakeLists.txt
+@@ -103,16 +103,7 @@ endif ()
+ include(GNUInstallDirs)
+ 
+ # eigen 2 or 3
+-find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+-  HINTS  ENV EIGEN3_INC_DIR
+-         ENV EIGEN3_DIR
+-  PATHS  Eigen/Core
+-         /usr/local/include
+-         /usr/include
+-         /opt/local/include
+-  PATH_SUFFIXES include eigen3 eigen2 eigen
+-  DOC "Directory containing the Eigen3 header files"
+-)
+find_package(Eigen3 REQUIRED)
+ 
+ # optionally, opencl
+ # OpenCL disabled as its code is not up-to-date with API
+@@ -165,10 +156,10 @@ endif ()
+ set_target_properties(${LIB_NAME} PROPERTIES VERSION "${PROJECT_VERSION}" SOVERSION 1)
+ 
+ target_include_directories(${LIB_NAME} PUBLIC
+-	${EIGEN_INCLUDE_DIR}
+ 	$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
+ 	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
+   )
+target_link_libraries(${LIB_NAME} PUBLIC Eigen3::Eigen)
+ 
+ # openmp
+ set(USE_OPEN_MP TRUE CACHE BOOL "Set to FALSE to not use OpenMP")
+diff --git a/libnaboConfig.cmake.in b/libnaboConfig.cmake.in
+index 0de01c3..8bb8194 100644
+--- a/libnaboConfig.cmake.in
+++ b/libnaboConfig.cmake.in
+@@ -1,5 +1,7 @@
+ # - Config file for the libnabo package
+ 
+find_dependency(Eigen3 CONFIG)
+
+ include(${CMAKE_CURRENT_LIST_DIR}/libnabo-targets.cmake)
+ 
+ # This causes catkin_simple to link against these libraries
+diff --git a/nabo/kdtree_cpu.cpp b/nabo/kdtree_cpu.cpp
+index cb1f8d1..52a444f 100644
+--- a/nabo/kdtree_cpu.cpp
+++ b/nabo/kdtree_cpu.cpp
+@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ #include <queue>
+ #include <algorithm>
+ #include <utility>
+#include <cassert>
+ #ifdef HAVE_OPENMP
+ #include <omp.h>
+ #endif
@@ -0,0 +1,314 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index 9660f55..c4a076f 100644
+--- a/CMakeLists.txt
+++ b/CMakeLists.txt
+@@ -18,6 +18,7 @@ set(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib)
+ 
+ OPTION(BUILD_TESTS "Build tests" ON)
+ OPTION(BUILD_PYTHON "Build Python extension" OFF)
+OPTION(BUILD_WITH_MARCHNATIVE "Build with -march=native" ON)
+ 
+ IF(MSVC)
+   set(BUILD_SHARED_LIBS OFF)
+@@ -35,7 +36,7 @@ ELSE()
+   ELSEIF (CMAKE_SYSTEM_PROCESSOR MATCHES 
+           "(arm)|(ARM)|(armhf)|(ARMHF)|(armel)|(ARMEL)")
+     add_definitions (-march=armv7-a)
+-  ELSE ()
+  ELSEIF (BUILD_WITH_MARCHNATIVE)
+     add_definitions (-march=native) #TODO use correct c++11 def once everybody has moved to gcc 4.7 # for now I even removed std=gnu++0x
+   ENDIF()
+   add_definitions (
+@@ -54,8 +55,9 @@ IF(BUILD_POSITION_INDEPENDENT_CODE)
+ ENDIF()
+ 
+ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/modules/")
+-find_package(Eigen REQUIRED)
+-set(ADDITIONAL_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS} ${EIGEN_INCLUDE_DIR}/unsupported)
+find_package(Eigen3 REQUIRED)
+get_target_property(EIGEN3_INCLUDE_DIR Eigen3::Eigen INTERFACE_INCLUDE_DIRECTORIES)
+set(ADDITIONAL_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}/unsupported)
+ 
+ set( OPENGV_SOURCE_FILES
+   src/absolute_pose/modules/main.cpp
+@@ -187,13 +189,10 @@ set_target_properties( opengv random_generators PROPERTIES
+ 
+ target_include_directories( opengv PUBLIC 
+                 # only when building from the source tree
+-                $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+                "$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include;${ADDITIONAL_INCLUDE_DIRS}>"
+                 # only when using the lib from the install path
+-                $<INSTALL_INTERFACE:include>
+-                ${ADDITIONAL_INCLUDE_DIRS} )
+-
+-target_include_directories( random_generators PRIVATE ${ADDITIONAL_INCLUDE_DIRS} )
+-
+                "$<INSTALL_INTERFACE:include>" )
+target_link_libraries(opengv PUBLIC Eigen3::Eigen)
+ target_link_libraries( random_generators opengv )
+ 
+ IF (BUILD_TESTS)
+diff --git a/modules/Config.cmake.in b/modules/Config.cmake.in
+index 9b4c9ee..6963356 100644
+--- a/modules/Config.cmake.in
+++ b/modules/Config.cmake.in
+@@ -1,4 +1,4 @@
+ @PACKAGE_INIT@
+-
+find_dependency(Eigen3 CONFIG)
+ include("${CMAKE_CURRENT_LIST_DIR}/@[email protected]")
+ check_required_components("@PROJECT_NAME@")
+diff --git a/src/absolute_pose/CentralAbsoluteAdapter.cpp b/src/absolute_pose/CentralAbsoluteAdapter.cpp
+index 684fa7e..ead54ac 100644
+--- a/src/absolute_pose/CentralAbsoluteAdapter.cpp
+++ b/src/absolute_pose/CentralAbsoluteAdapter.cpp
+@@ -30,6 +30,7 @@
+ 
+ 
+ #include <opengv/absolute_pose/CentralAbsoluteAdapter.hpp>
+#include <cassert>
+ 
+ 
+ opengv::absolute_pose::CentralAbsoluteAdapter::CentralAbsoluteAdapter(
+diff --git a/src/absolute_pose/MACentralAbsolute.cpp b/src/absolute_pose/MACentralAbsolute.cpp
+index 6edbabc..1164b30 100644
+--- a/src/absolute_pose/MACentralAbsolute.cpp
+++ b/src/absolute_pose/MACentralAbsolute.cpp
+@@ -30,6 +30,7 @@
+ 
+ 
+ #include <opengv/absolute_pose/MACentralAbsolute.hpp>
+#include <cassert>
+ 
+ 
+ opengv::absolute_pose::MACentralAbsolute::MACentralAbsolute(
+diff --git a/src/absolute_pose/MANoncentralAbsolute.cpp b/src/absolute_pose/MANoncentralAbsolute.cpp
+index d9b5b09..1d2041c 100644
+--- a/src/absolute_pose/MANoncentralAbsolute.cpp
+++ b/src/absolute_pose/MANoncentralAbsolute.cpp
+@@ -30,6 +30,7 @@
+ 
+ 
+ #include <opengv/absolute_pose/MANoncentralAbsolute.hpp>
+#include <cassert>
+ 
+ opengv::absolute_pose::MANoncentralAbsolute::MANoncentralAbsolute(
+     const double * points,
+diff --git a/src/absolute_pose/NoncentralAbsoluteAdapter.cpp b/src/absolute_pose/NoncentralAbsoluteAdapter.cpp
+index 30176aa..399699b 100644
+--- a/src/absolute_pose/NoncentralAbsoluteAdapter.cpp
+++ b/src/absolute_pose/NoncentralAbsoluteAdapter.cpp
+@@ -30,6 +30,7 @@
+ 
+ 
+ #include <opengv/absolute_pose/NoncentralAbsoluteAdapter.hpp>
+#include <cassert>
+ 
+ opengv::absolute_pose::NoncentralAbsoluteAdapter::NoncentralAbsoluteAdapter(
+     const bearingVectors_t & bearingVectors,
+diff --git a/src/absolute_pose/NoncentralAbsoluteMultiAdapter.cpp b/src/absolute_pose/NoncentralAbsoluteMultiAdapter.cpp
+index 88c237a..b88bbb2 100644
+--- a/src/absolute_pose/NoncentralAbsoluteMultiAdapter.cpp
+++ b/src/absolute_pose/NoncentralAbsoluteMultiAdapter.cpp
+@@ -30,6 +30,7 @@
+ 
+ 
+ #include <opengv/absolute_pose/NoncentralAbsoluteMultiAdapter.hpp>
+#include <cassert>
+ 
+ opengv::absolute_pose::NoncentralAbsoluteMultiAdapter::NoncentralAbsoluteMultiAdapter(
+     std::vector<std::shared_ptr<bearingVectors_t> > bearingVectors,
+diff --git a/src/absolute_pose/methods.cpp b/src/absolute_pose/methods.cpp
+index b1f0889..6d7a73c 100644
+--- a/src/absolute_pose/methods.cpp
+++ b/src/absolute_pose/methods.cpp
+@@ -34,6 +34,7 @@
+ 
+ #include <Eigen/NonLinearOptimization>
+ #include <Eigen/NumericalDiff>
+#include <cassert>
+ 
+ #include <opengv/absolute_pose/modules/main.hpp>
+ #include <opengv/absolute_pose/modules/Epnp.hpp>
+diff --git a/src/absolute_pose/modules/main.cpp b/src/absolute_pose/modules/main.cpp
+index ed0c271..011dbcc 100644
+--- a/src/absolute_pose/modules/main.cpp
+++ b/src/absolute_pose/modules/main.cpp
+@@ -46,6 +46,8 @@
+ #include <opengv/math/arun.hpp>
+ #include <opengv/math/cayley.hpp>
+ 
+#include <cassert>
+
+ void
+ opengv::absolute_pose::modules::p3p_kneip_main(
+     const bearingVectors_t & f,
+diff --git a/src/math/arun.cpp b/src/math/arun.cpp
+index a0d6296..bc729f6 100644
+--- a/src/math/arun.cpp
+++ b/src/math/arun.cpp
+@@ -30,6 +30,7 @@
+ 
+ 
+ #include <opengv/math/arun.hpp>
+#include <cassert>
+ 
+ opengv::rotation_t
+ opengv::math::arun( const Eigen::MatrixXd & Hcross )
+diff --git a/src/point_cloud/MAPointCloud.cpp b/src/point_cloud/MAPointCloud.cpp
+index 81fd5dd..a216857 100644
+--- a/src/point_cloud/MAPointCloud.cpp
+++ b/src/point_cloud/MAPointCloud.cpp
+@@ -30,6 +30,7 @@
+ 
+ 
+ #include <opengv/point_cloud/MAPointCloud.hpp>
+#include <cassert>
+ 
+ opengv::point_cloud::MAPointCloud::MAPointCloud(
+     const double * points1,
+diff --git a/src/point_cloud/PointCloudAdapter.cpp b/src/point_cloud/PointCloudAdapter.cpp
+index f9faaeb..1f8e951 100644
+--- a/src/point_cloud/PointCloudAdapter.cpp
+++ b/src/point_cloud/PointCloudAdapter.cpp
+@@ -30,6 +30,7 @@
+ 
+ 
+ #include <opengv/point_cloud/PointCloudAdapter.hpp>
+#include <cassert>
+ 
+ opengv::point_cloud::PointCloudAdapter::PointCloudAdapter(
+     const points_t & points1,
+diff --git a/src/point_cloud/methods.cpp b/src/point_cloud/methods.cpp
+index 5409eeb..f098a1d 100644
+--- a/src/point_cloud/methods.cpp
+++ b/src/point_cloud/methods.cpp
+@@ -39,6 +39,8 @@
+ #include <opengv/math/arun.hpp>
+ #include <opengv/math/cayley.hpp>
+ 
+#include <cassert>
+
+ namespace opengv
+ {
+ namespace point_cloud
+diff --git a/src/relative_pose/CentralRelativeAdapter.cpp b/src/relative_pose/CentralRelativeAdapter.cpp
+index 38e9a62..b2e9d98 100644
+--- a/src/relative_pose/CentralRelativeAdapter.cpp
+++ b/src/relative_pose/CentralRelativeAdapter.cpp
+@@ -30,6 +30,7 @@
+ 
+ 
+ #include <opengv/relative_pose/CentralRelativeAdapter.hpp>
+#include <cassert>
+ 
+ opengv::relative_pose::CentralRelativeAdapter::CentralRelativeAdapter(
+     const bearingVectors_t & bearingVectors1,
+diff --git a/src/relative_pose/CentralRelativeMultiAdapter.cpp b/src/relative_pose/CentralRelativeMultiAdapter.cpp
+index 2ab7476..e522205 100644
+--- a/src/relative_pose/CentralRelativeMultiAdapter.cpp
+++ b/src/relative_pose/CentralRelativeMultiAdapter.cpp
+@@ -30,6 +30,7 @@
+ 
+ 
+ #include <opengv/relative_pose/CentralRelativeMultiAdapter.hpp>
+#include <cassert>
+ 
+ opengv::relative_pose::CentralRelativeMultiAdapter::CentralRelativeMultiAdapter(
+     std::vector<std::shared_ptr<bearingVectors_t> > bearingVectors1,
+diff --git a/src/relative_pose/CentralRelativeWeightingAdapter.cpp b/src/relative_pose/CentralRelativeWeightingAdapter.cpp
+index a6ab478..9401dc2 100644
+--- a/src/relative_pose/CentralRelativeWeightingAdapter.cpp
+++ b/src/relative_pose/CentralRelativeWeightingAdapter.cpp
+@@ -30,6 +30,7 @@
+ 
+ 
+ #include <opengv/relative_pose/CentralRelativeWeightingAdapter.hpp>
+#include <cassert>
+ 
+ opengv::relative_pose::CentralRelativeWeightingAdapter::CentralRelativeWeightingAdapter(
+     const bearingVectors_t & bearingVectors1,
+diff --git a/src/relative_pose/MACentralRelative.cpp b/src/relative_pose/MACentralRelative.cpp
+index ec2959f..c1feb7c 100644
+--- a/src/relative_pose/MACentralRelative.cpp
+++ b/src/relative_pose/MACentralRelative.cpp
+@@ -30,6 +30,7 @@
+ 
+ 
+ #include <opengv/relative_pose/MACentralRelative.hpp>
+#include <cassert>
+ 
+ opengv::relative_pose::MACentralRelative::MACentralRelative(
+     const double * bearingVectors1,
+diff --git a/src/relative_pose/MANoncentralRelative.cpp b/src/relative_pose/MANoncentralRelative.cpp
+index cea9c14..fc65c64 100644
+--- a/src/relative_pose/MANoncentralRelative.cpp
+++ b/src/relative_pose/MANoncentralRelative.cpp
+@@ -30,6 +30,7 @@
+ 
+ 
+ #include <opengv/relative_pose/MANoncentralRelative.hpp>
+#include <cassert>
+ 
+ opengv::relative_pose::MANoncentralRelative::MANoncentralRelative(
+       const double * bearingVectors1,
+diff --git a/src/relative_pose/MANoncentralRelativeMulti.cpp b/src/relative_pose/MANoncentralRelativeMulti.cpp
+index 49f8ecf..1e58fde 100644
+--- a/src/relative_pose/MANoncentralRelativeMulti.cpp
+++ b/src/relative_pose/MANoncentralRelativeMulti.cpp
+@@ -30,6 +30,7 @@
+ 
+ 
+ #include <opengv/relative_pose/MANoncentralRelativeMulti.hpp>
+#include <cassert>
+ 
+ opengv::relative_pose::MANoncentralRelativeMulti::MANoncentralRelativeMulti(
+     const std::vector<double*> & bearingVectors1,
+diff --git a/src/relative_pose/NoncentralRelativeAdapter.cpp b/src/relative_pose/NoncentralRelativeAdapter.cpp
+index 552f180..9edf294 100644
+--- a/src/relative_pose/NoncentralRelativeAdapter.cpp
+++ b/src/relative_pose/NoncentralRelativeAdapter.cpp
+@@ -30,6 +30,7 @@
+ 
+ 
+ #include <opengv/relative_pose/NoncentralRelativeAdapter.hpp>
+#include <cassert>
+ 
+ opengv::relative_pose::NoncentralRelativeAdapter::NoncentralRelativeAdapter(
+     const bearingVectors_t & bearingVectors1,
+diff --git a/src/relative_pose/NoncentralRelativeMultiAdapter.cpp b/src/relative_pose/NoncentralRelativeMultiAdapter.cpp
+index f41edbe..c720a5a 100644
+--- a/src/relative_pose/NoncentralRelativeMultiAdapter.cpp
+++ b/src/relative_pose/NoncentralRelativeMultiAdapter.cpp
+@@ -30,6 +30,7 @@
+ 
+ 
+ #include <opengv/relative_pose/NoncentralRelativeMultiAdapter.hpp>
+#include <cassert>
+ 
+ opengv::relative_pose::NoncentralRelativeMultiAdapter::NoncentralRelativeMultiAdapter(
+     std::vector<std::shared_ptr<bearingVectors_t> > bearingVectors1,
+diff --git a/src/relative_pose/methods.cpp b/src/relative_pose/methods.cpp
+index 0027dae..e2e26b1 100644
+--- a/src/relative_pose/methods.cpp
+++ b/src/relative_pose/methods.cpp
+@@ -42,6 +42,7 @@
+ #include <opengv/triangulation/methods.hpp>
+ 
+ #include <iostream>
+#include <cassert>
+ 
+ opengv::translation_t
+ opengv::relative_pose::twopt(
+diff --git a/src/relative_pose/modules/fivept_nister/modules.cpp b/src/relative_pose/modules/fivept_nister/modules.cpp
+index 4b134c5..f24e3f1 100644
+--- a/src/relative_pose/modules/fivept_nister/modules.cpp
+++ b/src/relative_pose/modules/fivept_nister/modules.cpp
+@@ -34,6 +34,7 @@
+ #include <Eigen/NumericalDiff>
+ 
+ #include <opengv/OptimizationFunctor.hpp>
+#include <cassert>
+ 
+ void
+ opengv::relative_pose::modules::fivept_nister::composeA(
@@ -0,0 +1,589 @@
+diff --git a/.gitignore b/.gitignore
+index 85152fb..da016a7 100644
+--- a/.gitignore
+++ b/.gitignore
+@@ -5,3 +5,4 @@
+ *.cur_trans
+ build
+ .ipynb_checkpoints/
+pointmatcher/pm_export.h
+\ No newline at end of file
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index 819a834..d72850a 100644
+--- a/CMakeLists.txt
+++ b/CMakeLists.txt
+@@ -233,7 +233,7 @@ else()
+         #get_property(yaml-cpp-pm_INCLUDE TARGET yaml-cpp-pm PROPERTY INCLUDE_DIRECTORIES)
+         #include_directories(${yaml-cpp-pm_INCLUDE})
+ 
+-        list(APPEND EXTERNAL_LIBS $<TARGET_FILE:yaml-cpp-pm>)
+        list(APPEND EXTERNAL_LIBS $<TARGET_LINKER_FILE:yaml-cpp-pm>)
+         list(APPEND EXTRA_DEPS yaml-cpp-pm)
+         set(yamlcpp_FOUND)
+ 
+@@ -291,6 +291,8 @@ else ()
+ 	set (CMAKE_CXX_STANDARD 11)
+ endif ()
+ 
+option(BUILD_SHARED_LIBS "Set to ON to build shared libraries" OFF)
+
+ # SOURCE
+ 
+ # Pointmatcher lib and install
+@@ -363,12 +365,18 @@ set(POINTMATCHER_SRC
+ 
+ file(GLOB_RECURSE POINTMATCHER_HEADERS "pointmatcher/*.h")
+ 
+-
+ # In CMake >=3.4 we can easily build shared libraries in Mac and Windows.
+ # No need to distinguish between operating systems while building targets
+ 
+ add_library(pointmatcher ${POINTMATCHER_SRC} ${POINTMATCHER_HEADERS} )
+ 
+include(GenerateExportHeader)
+generate_export_header(pointmatcher
+  BASE_NAME PM
+  EXPORT_FILE_NAME "${CMAKE_CURRENT_SOURCE_DIR}/pointmatcher/pm_export.h"
+  DEFINE_NO_DEPRECATED
+)
+
+ target_include_directories(pointmatcher PUBLIC
+   $<INSTALL_INTERFACE:>
+   $<INSTALL_INTERFACE:pointmatcher>
+@@ -417,6 +425,7 @@ install(FILES
+ 	pointmatcher/Timer.h
+ 	pointmatcher/Functions.h
+ 	pointmatcher/IO.h
+	pointmatcher/pm_export.h
+ 	DESTINATION ${INSTALL_INCLUDE_DIR}/pointmatcher
+ )
+ 
+@@ -515,7 +524,7 @@ add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS pointmatcher)
+ get_property(CONF_INCLUDE_DIRS DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES)
+ 
+ # Create variable with the library location
+-set(POINTMATCHER_LIB  $<TARGET_FILE:pointmatcher>)
+set(POINTMATCHER_LIB  $<TARGET_LINKER_FILE:pointmatcher>)
+ 
+ # Configure config file for local build tree
+ configure_file(libpointmatcherConfig.cmake.in
+diff --git a/pointmatcher/Bibliography.h b/pointmatcher/Bibliography.h
+index 17a353d..c86def5 100644
+--- a/pointmatcher/Bibliography.h
+++ b/pointmatcher/Bibliography.h
+@@ -40,6 +40,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ #include <string>
+ #include <vector>
+ 
+#include "pm_export.h"
+
+ namespace PointMatcherSupport
+ {
+ 	typedef std::vector<std::string> StringVector;
+@@ -48,7 +50,7 @@ namespace PointMatcherSupport
+ 	typedef StringMapMap Bibliography;
+ 	typedef std::map<std::string, unsigned> BibIndices;
+ 	
+-	struct CurrentBibliography
+	struct PM_EXPORT CurrentBibliography
+ 	{
+ 		enum Mode
+ 		{
+@@ -68,7 +70,7 @@ namespace PointMatcherSupport
+ 		void dumpBibtex(std::ostream& os) const;
+ 	};
+ 	
+-	std::string getAndReplaceBibEntries(const std::string&, CurrentBibliography& curBib);
+	PM_EXPORT std::string getAndReplaceBibEntries(const std::string&, CurrentBibliography& curBib);
+ 	
+ }; // PointMatcherSupport
+ 
+diff --git a/pointmatcher/DeprecationWarnings.h b/pointmatcher/DeprecationWarnings.h
+index b1ca8ec..dc84582 100644
+--- a/pointmatcher/DeprecationWarnings.h
+++ b/pointmatcher/DeprecationWarnings.h
+@@ -31,7 +31,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ #ifndef __POINTMATCHER_DEPRECATION_WARNINGS_H
+ #define __POINTMATCHER_DEPRECATION_WARNINGS_H
+ 
+-
+ #if __cplusplus >= 201402L
+ #define PM_DEPRECATED(msg) [[deprecated(msg)]]
+ #define PM_DEPRECATION_SUPPORTED
+diff --git a/pointmatcher/IO.cpp b/pointmatcher/IO.cpp
+index c4288c9..8722747 100644
+--- a/pointmatcher/IO.cpp
+++ b/pointmatcher/IO.cpp
+@@ -51,7 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ #include "boost/lexical_cast.hpp"
+ #include "boost/foreach.hpp"
+ 
+-#ifdef WIN32
+#ifdef _WIN32
+ #define strtok_r strtok_s
+ #endif // WIN32
+ 
+@@ -359,7 +359,9 @@ void PointMatcherSupport::validateFile(const std::string& fileName)
+ 	ifstream ifs(fileName.c_str());
+ 	if (!ifs.good() || !boost::filesystem::is_regular_file(fullPath))
+ 	#if BOOST_FILESYSTEM_VERSION >= 3
+-		#if BOOST_VERSION >= 105000
+		#if BOOST_VERSION >= 109000
+				throw runtime_error(string("Cannot open file ") + boost::filesystem::absolute(fullPath).generic_string());
+		#elif BOOST_VERSION >= 105000
+ 				throw runtime_error(string("Cannot open file ") + boost::filesystem::complete(fullPath).generic_string());
+ 		#else
+ 				throw runtime_error(string("Cannot open file ") + boost::filesystem3::complete(fullPath).generic_string());
+@@ -375,7 +377,11 @@ template<typename T>
+ typename PointMatcher<T>::DataPoints PointMatcher<T>::DataPoints::load(const std::string& fileName)
+ {
+ 	const boost::filesystem::path path(fileName);
+#if BOOST_VERSION >= 109000
+	const string ext = path.extension().string();
+#else
+ 	const string& ext(boost::filesystem::extension(path));
+#endif
+ 	if (boost::iequals(ext, ".vtk"))
+ 		return PointMatcherIO<T>::loadVTK(fileName);
+ 	else if (boost::iequals(ext, ".csv"))
+@@ -809,7 +815,11 @@ template<typename T>
+ void PointMatcher<T>::DataPoints::save(const std::string& fileName, bool binary) const
+ {
+ 	const boost::filesystem::path path(fileName);
+#if BOOST_VERSION >= 109000
+	const string ext = path.extension().string();
+#else
+ 	const string& ext(boost::filesystem::extension(path));
+#endif
+ 	if (boost::iequals(ext, ".vtk"))
+ 		return PointMatcherIO<T>::saveVTK(*this, fileName, binary);
+ 
+diff --git a/pointmatcher/IO.h b/pointmatcher/IO.h
+index 162dc21..8a0cefa 100644
+--- a/pointmatcher/IO.h
+++ b/pointmatcher/IO.h
+@@ -58,7 +58,7 @@ struct PointMatcherIO
+ 	//! ex: nx, ny, nz are associated with (0,normals) (1,normals) (2,normals) respectively
+ 	typedef std::map<std::string, LabelAssociationPair > SublabelAssociationMap;
+ 	
+-	static std::string getColLabel(const Label& label, const int row); //!< convert a descriptor label to an appropriate sub-label
+	PM_EXPORT static std::string getColLabel(const Label& label, const int row); //!< convert a descriptor label to an appropriate sub-label
+ 	
+ 	//! Type of information in a DataPoints. Each type is stored in its own dense matrix.
+ 	enum PMPropTypes
+@@ -70,7 +70,7 @@ struct PointMatcherIO
+ 		};
+ 
+ 	//! Structure containing all information required to map external information to PointMatcher internal representation
+-	struct SupportedLabel
+	struct PM_EXPORT SupportedLabel
+ 	{
+ 		std::string internalName; //!< name used in PointMatcher
+ 		std::string externalName; //!< name used in external format
+@@ -84,7 +84,7 @@ struct PointMatcherIO
+ 	typedef std::vector<SupportedLabel> SupportedLabels;
+ 
+ 	//! Helper structure designed to parse file headers
+-	struct GenericInputHeader
+	struct PM_EXPORT GenericInputHeader
+ 	{
+ 		std::string name; //!< name found in the file
+ 		unsigned int matrixRowId; //!< on which row the information will be loaded
+@@ -159,7 +159,7 @@ struct PointMatcherIO
+ 	}
+ 
+ 	//! Generate a vector of Labels by checking for collision is the same name is reused.
+-	class LabelGenerator
+	class PM_EXPORT LabelGenerator
+ 	{
+ 		Labels labels; //!< vector of labels used to cumulat information
+ 
+@@ -180,11 +180,11 @@ struct PointMatcherIO
+ 	//static PMPropTypes getPMType(const std::string& externalName); //! Return the type of information specific to a DataPoints based on a sulabel name
+ 
+ 	// CSV
+-	static DataPoints loadCSV(const std::string& fileName);
+-	static DataPoints loadCSV(std::istream& is);
+	PM_EXPORT static DataPoints loadCSV(const std::string& fileName);
+	PM_EXPORT static DataPoints loadCSV(std::istream& is);
+ 
+-	static void saveCSV(const DataPoints& data, const std::string& fileName);
+-	static void saveCSV(const DataPoints& data, std::ostream& os);
+	PM_EXPORT static void saveCSV(const DataPoints& data, const std::string& fileName);
+	PM_EXPORT static void saveCSV(const DataPoints& data, std::ostream& os);
+ 
+ 	// VTK
+ 	//! Enumeration of legacy VTK data types that can be parsed
+@@ -209,25 +209,25 @@ struct PointMatcherIO
+ 
+ 	};
+ 
+-	static DataPoints loadVTK(const std::string& fileName);
+-	static DataPoints loadVTK(std::istream& is);
+	PM_EXPORT static DataPoints loadVTK(const std::string& fileName);
+	PM_EXPORT static DataPoints loadVTK(std::istream& is);
+ 
+-	static void saveVTK(const DataPoints& data, const std::string& fileName, bool binary = false);
+	PM_EXPORT static void saveVTK(const DataPoints& data, const std::string& fileName, bool binary = false);
+ 
+ 	// PLY
+-	static DataPoints loadPLY(const std::string& fileName);
+-	static DataPoints loadPLY(std::istream& is);
+	PM_EXPORT static DataPoints loadPLY(const std::string& fileName);
+	PM_EXPORT static DataPoints loadPLY(std::istream& is);
+ 
+-	static void savePLY(const DataPoints& data, const std::string& fileName); //!< save datapoints to PLY point cloud format
+	PM_EXPORT static void savePLY(const DataPoints& data, const std::string& fileName); //!< save datapoints to PLY point cloud format
+ 
+ 	// PCD
+-	static DataPoints loadPCD(const std::string& fileName);
+-	static DataPoints loadPCD(std::istream& is);
+	PM_EXPORT static DataPoints loadPCD(const std::string& fileName);
+	PM_EXPORT static DataPoints loadPCD(std::istream& is);
+ 
+-	static void savePCD(const DataPoints& data, const std::string& fileName); //!< save datapoints to PCD point cloud format
+	PM_EXPORT static void savePCD(const DataPoints& data, const std::string& fileName); //!< save datapoints to PCD point cloud format
+ 
+ 	//! Information to exploit a reading from a file using this library. Fields might be left blank if unused.
+-	struct FileInfo
+	struct PM_EXPORT FileInfo
+ 	{
+ 		typedef Eigen::Matrix<T, 3, 1> Vector3; //!< alias
+ 
+@@ -242,7 +242,7 @@ struct PointMatcherIO
+ 	};
+ 
+ 	//! A vector of file info, to be used in batch processing
+-	struct FileInfoVector: public std::vector<FileInfo>
+	struct PM_EXPORT FileInfoVector: public std::vector<FileInfo>
+ 	{
+ 		FileInfoVector();
+ 		FileInfoVector(const std::string& fileName, std::string dataPath = "", std::string configPath = "");
+@@ -264,7 +264,7 @@ struct PointMatcherIO
+ 	static bool plyPropTypeValid (const std::string& type);
+ 
+ 	//! Interface for PLY property
+-	struct PLYProperty
+	struct PM_EXPORT PLYProperty
+ 	{
+ 		//PLY information:
+ 		std::string name; //!< name of PLY property
+@@ -299,7 +299,7 @@ struct PointMatcherIO
+ 	typedef typename PLYProperties::iterator it_PLYProp;
+ 
+ 	//! Interface for all PLY elements. 
+-	class PLYElement
+	class PM_EXPORT PLYElement
+ 	{
+ 	public:
+ 		std::string name; //!< name identifying the PLY element
+@@ -331,7 +331,7 @@ struct PointMatcherIO
+ 
+ 
+ 	//! Implementation of PLY vertex element
+-	class PLYVertex : public PLYElement
+	class PM_EXPORT PLYVertex : public PLYElement
+ 	{
+ 	public:
+ 		//! Constructor
+@@ -346,7 +346,7 @@ struct PointMatcherIO
+ 	};
+ 
+ 	//! Factory for PLY elements
+-	class PLYElementF
+	class PM_EXPORT PLYElementF
+ 	{
+ 		enum ElementTypes
+ 		{
+diff --git a/pointmatcher/Parametrizable.h b/pointmatcher/Parametrizable.h
+index 318e7f1..edc1355 100644
+--- a/pointmatcher/Parametrizable.h
+++ b/pointmatcher/Parametrizable.h
+@@ -46,6 +46,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ #define BOOST_ASSIGN_MAX_PARAMS 6
+ #include <boost/assign/list_inserter.hpp>
+ 
+#include "pm_export.h"
+ 
+ namespace PointMatcherSupport
+ {	
+@@ -95,7 +96,7 @@ namespace PointMatcherSupport
+ 	}
+ 	
+ 	//! The superclass of classes that are constructed using generic parameters. This class provides the parameter storage and fetching mechanism
+-	struct Parametrizable
+	struct PM_EXPORT Parametrizable
+ 	{
+ 		//! An exception thrown when one tries to fetch the value of an unexisting parameter
+ 		struct InvalidParameter: std::runtime_error
+@@ -114,7 +115,7 @@ namespace PointMatcherSupport
+ 		}
+ 		
+ 		//! The documentation of a parameter
+-		struct ParameterDoc
+		struct PM_EXPORT ParameterDoc
+ 		{
+ 			std::string name; //!< name
+ 			std::string doc; //!< short documentation
+@@ -173,7 +174,7 @@ namespace PointMatcherSupport
+ 		
+ 		friend std::ostream& operator<< (std::ostream& o, const Parametrizable& p);
+ 	};
+-	std::ostream& operator<< (std::ostream& o, const Parametrizable::ParametersDoc& p);
+	PM_EXPORT std::ostream& operator<< (std::ostream& o, const Parametrizable::ParametersDoc& p);
+ } // namespace PointMatcherSupport
+ 
+ #endif // __POINTMATCHER_PARAMETRIZABLE_H
+diff --git a/pointmatcher/PointMatcher.h b/pointmatcher/PointMatcher.h
+index f26aa2c..435a464 100644
+--- a/pointmatcher/PointMatcher.h
+++ b/pointmatcher/PointMatcher.h
+@@ -101,7 +101,7 @@ namespace PointMatcherSupport
+ 
+ 	
+ 	//! The logger interface, used to output warnings and informations
+-	struct Logger: public Parametrizable
+	struct PM_EXPORT Logger: public Parametrizable
+ 	{
+ 		Logger();
+ 		Logger(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
+@@ -127,7 +127,7 @@ namespace PointMatcherSupport
+ 
+ //! Functions and classes that are dependant on scalar type are defined in this templatized class
+ template<typename T>
+-struct PointMatcher
+struct PM_EXPORT PointMatcher
+ {
+ 	// ---------------------------------
+ 	// macros for constants
+@@ -145,7 +145,7 @@ struct PointMatcher
+ 	//TODO: gather exceptions here and in Exceptions.cpp
+ 
+ 	//! Point matcher did not converge
+-	struct ConvergenceError: std::runtime_error
+	struct PM_EXPORT ConvergenceError: std::runtime_error
+ 	{
+ 		ConvergenceError(const std::string& reason);
+ 	};
+@@ -204,7 +204,7 @@ struct PointMatcher
+ 		Moreover, the position of the points is in homogeneous coordinates because they need both translation and rotation, while the normals need only rotation.
+ 		All channels contain scalar values of type ScalarType.
+ 	*/
+-	struct DataPoints
+	struct PM_EXPORT DataPoints
+ 	{
+ 		//! A view on a feature or descriptor
+ 		typedef Eigen::Block<Matrix> View;
+@@ -218,7 +218,7 @@ struct PointMatcher
+ 		typedef typename Matrix::Index Index;
+ 		
+ 		//! The name for a certain number of dim
+-		struct Label
+		struct PM_EXPORT Label
+ 		{
+ 			std::string text; //!< name of the label
+ 			size_t span; //!< number of data dimensions the label spans
+@@ -226,7 +226,7 @@ struct PointMatcher
+ 			bool operator ==(const Label& that) const;
+ 		};
+ 		//! A vector of Label
+-		struct Labels: std::vector<Label>
+		struct PM_EXPORT Labels: std::vector<Label>
+ 		{
+ 			typedef typename std::vector<Label>::const_iterator const_iterator; //!< alias
+ 			Labels();
+@@ -368,7 +368,7 @@ struct PointMatcher
+ 		This class holds a list of associated reference identifiers, along with the corresponding \e squared distance, for all points in the reading.
+ 		A single point in the reading can have one or multiple matches.
+ 	*/
+-	struct Matches
+	struct PM_EXPORT Matches
+ 	{
+ 		typedef Matrix Dists; //!< Squared distances to closest points, dense matrix of ScalarType
+ 		typedef IntMatrix Ids; //!< Identifiers of closest points, dense matrix of integers
+@@ -401,7 +401,7 @@ struct PointMatcher
+ 	// ---------------------------------
+ 	
+ 	//! A function that transforms points and their descriptors given a transformation matrix
+-	struct Transformation: public Parametrizable
+	struct PM_EXPORT Transformation: public Parametrizable
+ 	{
+ 		Transformation();
+ 		Transformation(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
+@@ -422,7 +422,7 @@ struct PointMatcher
+ 	};
+ 	
+ 	//! A chain of Transformation
+-	struct Transformations: public std::vector<std::shared_ptr<Transformation> >
+	struct PM_EXPORT Transformations: public std::vector<std::shared_ptr<Transformation> >
+ 	{
+ 		void apply(DataPoints& cloud, const TransformationParameters& parameters) const;
+ 	};
+@@ -437,7 +437,7 @@ struct PointMatcher
+ 	/**
+ 		The filter might add information, for instance surface normals, or might change the number of points, for instance by randomly removing some of them.
+ 	*/
+-	struct DataPointsFilter: public Parametrizable
+	struct PM_EXPORT DataPointsFilter: public Parametrizable
+ 	{
+ 		DataPointsFilter();
+ 		DataPointsFilter(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
+@@ -452,7 +452,7 @@ struct PointMatcher
+ 	};
+ 	
+ 	//! A chain of DataPointsFilter
+-	struct DataPointsFilters: public std::vector<std::shared_ptr<DataPointsFilter> >
+	struct PM_EXPORT DataPointsFilters: public std::vector<std::shared_ptr<DataPointsFilter> >
+ 	{
+ 		DataPointsFilters();
+ 		DataPointsFilters(std::istream& in);
+@@ -470,7 +470,7 @@ struct PointMatcher
+ 	/**
+ 		This typically uses a space-partitioning structure such as a kd-tree for performance optimization.
+ 	*/
+-	struct Matcher: public Parametrizable
+	struct PM_EXPORT Matcher: public Parametrizable
+ 	{
+ 		unsigned long visitCounter; //!< number of points visited
+ 		
+@@ -496,7 +496,7 @@ struct PointMatcher
+ 		Criteria can be a fixed maximum authorized distance, a factor of the median distance, etc. 
+ 		Points with zero weights are ignored in the subsequent minimization step.
+ 	*/
+-	struct OutlierFilter: public Parametrizable
+	struct PM_EXPORT OutlierFilter: public Parametrizable
+ 	{
+ 		OutlierFilter();
+ 		OutlierFilter(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
+@@ -509,7 +509,7 @@ struct PointMatcher
+ 	
+ 	
+ 	//! A chain of OutlierFilter
+-	struct OutlierFilters: public std::vector<std::shared_ptr<OutlierFilter> >
+	struct PM_EXPORT OutlierFilters: public std::vector<std::shared_ptr<OutlierFilter> >
+ 	{
+ 		
+ 		OutlierWeights compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const Matches& input);
+@@ -527,7 +527,7 @@ struct PointMatcher
+ 	/**
+ 		Typical error minimized are point-to-point and point-to-plane.
+ 	*/
+-	struct ErrorMinimizer: public Parametrizable
+	struct PM_EXPORT ErrorMinimizer: public Parametrizable
+ 	{
+ 		//! A structure holding data ready for minimization. The data are "normalized", for instance there are no points with 0 weight, etc.
+ 		struct ErrorElements
+@@ -580,7 +580,7 @@ struct PointMatcher
+ 		For example, a condition can be the number of times the loop was executed, or it can be related to the matching error.
+ 		Because the modules can be chained, we defined that the relation between modules must agree through an OR-condition, while all AND-conditions are defined within a single module.
+ 	*/
+-	struct TransformationChecker: public Parametrizable
+	struct PM_EXPORT TransformationChecker: public Parametrizable
+ 	{
+ 	protected:
+ 		typedef std::vector<std::string> StringVector; //!< a vector of strings
+@@ -608,7 +608,7 @@ struct PointMatcher
+ 	};
+ 	
+ 	//! A chain of TransformationChecker
+-	struct TransformationCheckers: public std::vector<std::shared_ptr<TransformationChecker> >
+	struct PM_EXPORT TransformationCheckers: public std::vector<std::shared_ptr<TransformationChecker> >
+ 	{
+ 		void init(const TransformationParameters& parameters, bool& iterate);
+ 		void check(const TransformationParameters& parameters, bool& iterate);
+@@ -621,7 +621,7 @@ struct PointMatcher
+ 	// ---------------------------------
+ 	
+ 	//! An inspector allows to log data at the different steps, for analysis.
+-	struct Inspector: public Parametrizable
+	struct PM_EXPORT Inspector: public Parametrizable
+ 	{
+ 		
+ 		Inspector();
+@@ -652,7 +652,7 @@ struct PointMatcher
+ 	// algorithms
+ 	
+ 	//! Stuff common to all ICP algorithms
+-	struct ICPChainBase
+	struct PM_EXPORT ICPChainBase
+ 	{
+ 	public:
+ 		DataPointsFilters readingDataPointsFilters; //!< filters for reading, applied once
+@@ -699,7 +699,7 @@ struct PointMatcher
+ 	};
+ 	
+ 	//! ICP algorithm
+-	struct ICP: ICPChainBase
+	struct PM_EXPORT ICP: ICPChainBase
+ 	{
+ 		TransformationParameters operator()(
+ 			const DataPoints& readingIn,
+@@ -730,7 +730,7 @@ struct PointMatcher
+ 	
+ 	//! ICP alogrithm, taking a sequence of clouds and using a map
+ 	//! Warning: used with caution, you need to set the map manually.
+-	struct ICPSequence: public ICP
+	struct PM_EXPORT ICPSequence: public ICP
+ 	{
+ 		TransformationParameters operator()(
+ 			const DataPoints& cloudIn);
+diff --git a/pointmatcher/Registrar.h b/pointmatcher/Registrar.h
+index 706211f..84557f8 100644
+--- a/pointmatcher/Registrar.h
+++ b/pointmatcher/Registrar.h
+@@ -63,7 +63,7 @@ namespace PointMatcherSupport
+ #endif
+ 
+ 	//! Retrieve name and parameters from a yaml node
+-	void getNameParamsFromYAML(const YAML::Node& module, std::string& name, Parametrizable::Parameters& params);
+	PM_EXPORT void getNameParamsFromYAML(const YAML::Node& module, std::string& name, Parametrizable::Parameters& params);
+ 
+ 	//! An exception thrown when one tries to instanciate an element that does not exist in the registrar
+ 	struct InvalidElement: std::runtime_error
+@@ -73,13 +73,13 @@ namespace PointMatcherSupport
+ 
+ 	//! A factor for subclasses of Interface
+ 	template<typename Interface>
+-	struct Registrar
+	struct PM_EXPORT Registrar
+ 	{
+ 	public:
+ 		typedef Interface TargetType; //!< alias to recover the template parameter
+ 
+ 		//! The interface for class descriptors
+-		struct ClassDescriptor
+		struct PM_EXPORT ClassDescriptor
+ 		{
+ 			//! Virtual destructor, do nothing
+ 			virtual ~ClassDescriptor() {}
+@@ -93,7 +93,7 @@ namespace PointMatcherSupport
+ 
+ 		//! A descriptor for a class C that provides parameters
+ 		template<typename C>
+-		struct GenericClassDescriptor: public ClassDescriptor
+		struct PM_EXPORT GenericClassDescriptor: public ClassDescriptor
+ 		{
+ 			virtual std::shared_ptr<Interface> createInstance(const std::string& className, const Parametrizable::Parameters& params) const
+ 			{
+@@ -124,7 +124,7 @@ namespace PointMatcherSupport
+ 
+ 		//! A descriptor for a class C that does not provide any parameter
+ 		template<typename C>
+-		struct GenericClassDescriptorNoParam: public ClassDescriptor
+		struct PM_EXPORT GenericClassDescriptorNoParam: public ClassDescriptor
+ 		{
+ 			virtual std::shared_ptr<Interface> createInstance(const std::string& className, const Parametrizable::Parameters& params) const
+ 			{
+diff --git a/pointmatcher/Timer.h b/pointmatcher/Timer.h
+index 316dbe2..6a9898a 100644
+--- a/pointmatcher/Timer.h
+++ b/pointmatcher/Timer.h
+@@ -37,7 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ #define __POINTMATCHER_TIMER_H
+ 
+ #include <time.h>
+-#ifndef WIN32
+#ifndef _WIN32
+ #include <unistd.h>
+ #endif // WIN32
+ 
@@ -7,6 +7,7 @@ ADD_SUBDIRECTORY( StereoEval )
 ADD_SUBDIRECTORY( KittiDataset )
 ADD_SUBDIRECTORY( RgbdDataset )
 ADD_SUBDIRECTORY( EurocDataset )
+ADD_SUBDIRECTORY( CidSimsDataset )
 ADD_SUBDIRECTORY( Recovery )
 ADD_SUBDIRECTORY( Reprocess )
 ADD_SUBDIRECTORY( DetectMoreLoopClosures )
@@ -15,6 +16,7 @@ ADD_SUBDIRECTORY( Report )
 ADD_SUBDIRECTORY( Info )
 ADD_SUBDIRECTORY( CleanupLocalGrids )
 ADD_SUBDIRECTORY( GlobalBundleAdjustment )
+ADD_SUBDIRECTORY( ReduceGraph )

 IF(OPENCV_NONFREE_FOUND)
 ADD_SUBDIRECTORY( VocabularyComparison )
@@ -0,0 +1,11 @@
+
+ADD_EXECUTABLE(cidsims_dataset main.cpp)
+  
+TARGET_LINK_LIBRARIES(cidsims_dataset rtabmap_core)
+
+SET_TARGET_PROPERTIES( cidsims_dataset 
+    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-cidsims_dataset)
+    
+INSTALL(TARGETS cidsims_dataset
+	RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+	BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
@@ -0,0 +1,688 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/Odometry.h>
+#include "rtabmap/core/Rtabmap.h"
+#include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/Graph.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/Memory.h"
+#include "rtabmap/core/util3d_registration.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UMath.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UProcessInfo.h"
+#include <pcl/common/common.h>
+#include <rtabmap/core/SensorCaptureThread.h>
+#include <stdio.h>
+#include <signal.h>
+#include <fstream>
+
+using namespace rtabmap;
+
+void showUsage(const char * appName)
+{
+	printf("\nUsage:\n"
+			"%s [options] path\n"
+			"  path               Folder of the sequence (e.g., \"~/apartment1_1\")\n"
+			"                        containing color, depth, groundtruth.txt, imu.txt and odom.txt.\n"
+			"  --output           Output directory. By default, results are saved in \"path\".\n"
+			"  --output_name      Output database name (default \"rtabmap\").\n"
+			"  --max_time_diff #.#  Maximum time difference with frame to attribute a valid odometry and/or ground truth pose (default 0.1 s).\n"
+			"  --quiet            Don't show log messages and iteration updates.\n"
+			"  --use_imu          Use IMU.\n"
+			"  --gt               Record ground truth.\n"
+			"  --odom             Use wheel odometry as input guess to visual odometry.\n"
+			"  --imu #            Use IMU and set filter: 0=madgwick, 1=complementary.\n"
+			"  --quiet            Don't show log messages and iteration updates.\n"
+			"%s\n"
+			"Example:\n\n"
+			"   $ %s \\\n"
+			"       --Rtabmap/DetectionRate 2\\\n"
+			"       --RGBD/OptimizeMaxError 5\\\n"
+			"       --Mem/STMSize 30\\\n"
+			"       --gt\\\n"
+			"       --odom\\\n"
+			"       --imu 1\\\n"
+			"       ~/apartment1_1\n\n", appName, rtabmap::Parameters::showUsage(), appName);
+	exit(1);
+}
+
+// catch ctrl-c
+bool g_forever = true;
+void sighandler(int sig)
+{
+	printf("\nSignal %d caught...\n", sig);
+	g_forever = false;
+}
+
+int main(int argc, char * argv[])
+{
+	signal(SIGABRT, &sighandler);
+	signal(SIGTERM, &sighandler);
+	signal(SIGINT, &sighandler);
+
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	ParametersMap parameters;
+	std::string path;
+	std::string output;
+	std::string outputName = "rtabmap";
+	int skipFrames = 0;
+	float maxTimeDiff = 0.1f;
+	bool quiet = false;
+	int imuFilter = 1;
+	bool useImu = false;
+	bool useOdom = false;
+	bool recordGt = false;
+	if(argc < 2)
+	{
+		showUsage(argv[0]);
+	}
+	else
+	{
+		for(int i=1; i<argc; ++i)
+		{
+			if(std::strcmp(argv[i], "--output") == 0)
+			{
+				output = argv[++i];
+			}
+			else if(std::strcmp(argv[i], "--output_name") == 0)
+			{
+				outputName = argv[++i];
+			}
+			else if(std::strcmp(argv[i], "--max_time_diff") == 0)
+			{
+				maxTimeDiff = atof(argv[++i]);
+				UASSERT(maxTimeDiff > 0.0f);
+			}
+			else if(std::strcmp(argv[i], "--odom") == 0)
+			{
+				useOdom = true;
+			}
+			else if(std::strcmp(argv[i], "--imu") == 0)
+			{
+				useImu = true;
+				imuFilter = atoi(argv[++i]);
+			}
+			else if(std::strcmp(argv[i], "--gt") == 0)
+			{
+				recordGt = true;
+			}
+			else if(std::strcmp(argv[i], "--quiet") == 0)
+			{
+				quiet = true;
+			}
+		}
+		parameters = Parameters::parseArguments(argc, argv);
+		path = argv[argc-1];
+		path = uReplaceChar(path, '~', UDirectory::homeDir());
+		path = uReplaceChar(path, '\\', '/');
+		if(output.empty())
+		{
+			output = path;
+		}
+		else
+		{
+			output = uReplaceChar(output, '~', UDirectory::homeDir());
+			UDirectory::makeDir(output);
+		}
+		parameters.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), output));
+		parameters.insert(ParametersPair(Parameters::kRtabmapPublishRAMUsage(), "true"));
+	}
+
+	std::string seq = uSplit(path, '/').back();
+	std::string pathRgbImages  = path+"/color";
+	std::string pathDepthImages = path+"/depth";
+	std::string pathGt = recordGt?path+"/groundtruth.txt":"";
+	if(recordGt && !UFile::exists(pathGt))
+	{
+		UWARN("Ground truth file path doesn't exist: \"%s\", benchmark values won't be computed.", pathGt.c_str());
+		pathGt.clear();
+	}
+	std::string pathOdom = useOdom?path+"/rtabmap_odom.txt":"";
+	if(useOdom && !UFile::exists(pathOdom))
+	{
+		std::string orgOdom = path+"/odom.txt";
+		if(UFile::exists(orgOdom))
+		{
+			printf("Converting odom.txt to rtabmap_odom.txt...");
+			std::ifstream inputFile(orgOdom);
+			std::ofstream outputFile(pathOdom);
+			std::string line;
+
+			if (!inputFile.is_open()) {
+				UERROR("Error: Could not open input file: %s", orgOdom.c_str());
+				return 1;
+			}
+
+			double previousStamp = 0.0;
+			float odomX = 0.0f;
+			float odomY = 0.0f;
+			float odomTheta = 0.0f;
+			while (std::getline(inputFile, line)) {
+				auto strList = uListToVector(uSplit(line, ' '));
+				if(line.empty())
+				{
+					break;
+				}
+				if(strList.size() != 14)
+				{
+					UERROR("Odometry shoud, have 14 entries per line, got %ld: \"%s\"", strList.size(), line.c_str());
+					return 1;
+				}
+				// Recompute wheel odometry based on velocity
+				double stamp = uStr2Double(strList[0]);
+				if(previousStamp==0)
+				{
+					previousStamp = uStr2Double(strList[0]);
+				}
+				float dt = stamp - previousStamp;
+				float vx = uStr2Float(strList[8]);
+				float vtheta = uStr2Float(strList[13]);
+				
+				odomX += vx * cos(odomTheta) * dt;
+				odomY += vx * sin(odomTheta) * dt;
+				odomTheta = odomTheta + vtheta * dt;
+				
+				Transform t(odomX, odomY, odomTheta);
+				Eigen::Quaternionf q = t.getQuaternionf();
+				outputFile << strList[0] << ' ' << t.x() << ' ' << t.y() << ' ' << t.z() << ' ' << q.x() << ' ' << q.y() << ' ' << q.z() << ' ' << q.w() << std::endl;
+				previousStamp = stamp;
+			}
+
+			inputFile.close();
+			outputFile.close();
+			printf("Converting odom.txt to rtabmap_odom.txt...done!");
+		}
+		else 
+		{
+			pathOdom.clear();
+		}
+	}
+	std::string pathImu = useImu?path+"/imu.txt":"";
+	if(useImu && !UFile::exists(pathImu))
+	{
+		pathImu.clear();
+	}
+
+	if(quiet)
+	{
+		ULogger::setLevel(ULogger::kError);
+	}
+
+	printf("Paths:\n"
+			"   Dataset name:    %s\n"
+			"   Dataset path:    %s\n"
+			"   Color path:      %s\n"
+			"   Depth path:      %s\n"
+			"   Output:          %s\n"
+			"   Output name:     %s\n"
+			"   Max time diff:   %f\n",
+			seq.c_str(),
+			path.c_str(),
+			pathRgbImages.c_str(),
+			pathDepthImages.c_str(),
+			output.c_str(),
+			outputName.c_str(),
+			maxTimeDiff);
+	printf("   Ground Truth: %s\n", !pathGt.empty()?pathGt.c_str():"Set --gt to record ground truth");
+	printf("   Odometry:     %s\n", !pathOdom.empty()?pathOdom.c_str():"Set --odom use wheel odometry");
+	printf("   IMU:          %s\n", !pathImu.empty()?pathImu.c_str():"Set --imu 1 to use IMU");
+	printf("   IMU Filter:   %d\n", imuFilter);
+	if(!parameters.empty())
+	{
+		printf("Parameters:\n");
+		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+		{
+			printf("   %s=%s\n", iter->first.c_str(), iter->second.c_str());
+		}
+	}
+	printf("RTAB-Map version: %s\n", RTABMAP_VERSION);
+
+	// setup calibration file, based on https://cid-sims.github.io/calibration/calibration.yaml
+	CameraModel model(outputName+"_calib", 
+			386.52199190267083, 387.32300428823663, 
+			326.5103569741365, 237.40293732598795, 
+			Transform::getIdentity(), 0, cv::Size(640,480));
+	std::string sequenceName = UFile(path).getName();
+	// The ground truth corresponds to camera frame, thus make the camera the base frame
+	Transform cameraHigh(
+		0.013246,  0.0521412, 0.9982324, pathGt.empty()?0.28:0,
+		-0.9962321, 0.0610127, 0.0024175, 0,
+		-0.05647427, -0.9950674, 0.0591213, pathGt.empty()?0.20:0);
+	Transform cameraLow(
+		-0.00477153, 0.0888742, 0.995711, pathGt.empty()?0.369117:0, 
+		-0.99571, 0.0665738, -0.018348, pathGt.empty()?0.0130432:0, 
+		-0.0637357, -0.99188, 0.094639, pathGt.empty()?0.016175:0);
+	Transform camImu(
+		0.9999691, 0.00720362, -0.00314765, -0.02707507,
+		-0.0071841, 0.99995517, 0.0061682, -0.004337,
+		0.00319195, -0.0061454, 0.99997602, -0.01595186);
+	Transform imuHigh = cameraHigh * camImu; // base->IMU
+	Transform imuLow = cameraLow * camImu; // base->IMU
+	Transform baseToImu;
+
+	// Based on https://cid-sims.github.io/overview/index.html
+	if( sequenceName.find("apartment1_1") != std::string::npos ||
+		sequenceName.find("apartment2_1") != std::string::npos ||
+		sequenceName.find("apartment2_3") != std::string::npos ||
+		sequenceName.find("apartment3_1") != std::string::npos ||
+		sequenceName.find("apartment3_1") != std::string::npos)
+	{
+		// using camera low
+		model.setLocalTransform(cameraLow);
+		baseToImu = imuLow;
+	}
+	else
+	{
+		// using camera high
+		model.setLocalTransform(cameraHigh);
+		baseToImu = imuHigh;
+	}
+	model.save(path);
+
+	SensorCaptureThread cameraThread(new
+		CameraRGBDImages(
+				pathRgbImages,
+				pathDepthImages), parameters);
+	((CameraRGBDImages*)cameraThread.camera())->setTimestamps(true, "", false);
+	if(!pathGt.empty())
+	{
+		((CameraRGBDImages*)cameraThread.camera())->setGroundTruthPath(pathGt, 1);
+	}
+	if(!pathOdom.empty())
+	{
+		((CameraRGBDImages*)cameraThread.camera())->setOdometryPath(pathOdom, 10);
+	}
+	((CameraRGBDImages*)cameraThread.camera())->setMaxPoseTimeDiff(maxTimeDiff);
+	if(!pathImu.empty())
+	{
+		cameraThread.enableIMUFiltering(imuFilter, parameters);
+	}
+
+	bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
+	float detectionRate = Parameters::defaultRtabmapDetectionRate();
+	int odomStrategy = Parameters::defaultOdomStrategy();
+	Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
+	Parameters::parse(parameters, Parameters::kOdomStrategy(), odomStrategy);
+	Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
+	std::string databasePath = output+"/"+outputName+".db";
+	UFile::erase(databasePath);
+	if(cameraThread.camera()->init(path, outputName+"_calib"))
+	{
+		int totalImages = (int)((CameraRGBDImages*)cameraThread.camera())->filenames().size();
+
+		if(skipFrames>0)
+		{
+			totalImages /= skipFrames+1;
+		}
+
+		printf("Processing %d images...\n", totalImages);
+
+		ParametersMap odomParameters = parameters;
+		odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
+		Odometry * odom = Odometry::create(odomParameters);
+		Rtabmap rtabmap;
+		rtabmap.init(parameters, databasePath);
+
+		std::ifstream imu_file;
+
+		if(!pathImu.empty())
+		{
+			// open the IMU file
+			std::string line;
+			imu_file.open(pathImu.c_str());
+			if (!imu_file.good()) {
+				UERROR("no imu file found at %s",pathImu.c_str());
+				return -1;
+			}
+			int number_of_lines = 0;
+			while (std::getline(imu_file, line))
+				++number_of_lines;
+			printf("No. IMU measurements: %d\n", number_of_lines-1);
+			if (number_of_lines - 1 <= 0) {
+				UERROR("no imu messages present in %s", pathImu.c_str());
+				return -1;
+			}
+			// set reading position to second line
+			imu_file.clear();
+			imu_file.seekg(0, std::ios::beg);
+			std::getline(imu_file, line);
+		}
+
+		UTimer totalTime;
+		UTimer timer;
+		SensorCaptureInfo cameraInfo;
+		SensorData data = cameraThread.camera()->takeData(&cameraInfo);
+		int iteration = 0;
+		double start = data.stamp();
+
+		/////////////////////////////
+		// Processing dataset begin
+		/////////////////////////////
+		int odomKeyFrames = 0;
+		double previousStamp = 0.0;
+		Transform previousOdomPose;
+		
+		while(data.isValid() && g_forever)
+		{
+			// get all IMU measurements till then
+			double t_imu = start;
+			do {
+				std::string line;
+				if (!std::getline(imu_file, line)) {
+					std::cout << std::endl << "Finished parsing IMU." << std::endl << std::flush;
+					break;
+				}
+
+				std::stringstream stream(line);
+				std::string s;
+				std::getline(stream, s, ' ');
+				t_imu = uStr2Double(s);
+
+				cv::Vec3d gyr;
+				for (int j = 0; j < 3; ++j) {
+					std::getline(stream, s, ' ');
+					gyr[j] = uStr2Double(s);
+				}
+
+				cv::Vec3d acc;
+				for (int j = 0; j < 3; ++j) {
+					std::getline(stream, s, ' ');
+					acc[j] = uStr2Double(s);
+				}
+
+				if (t_imu - start + 1 > 0) {
+
+					SensorData dataImu(IMU(gyr, cv::Mat(3,3,CV_64FC1), acc, cv::Mat(3,3,CV_64FC1), baseToImu), 0, t_imu);
+					cameraThread.postUpdate(&dataImu);
+					odom->process(dataImu);
+				}
+
+			} while (t_imu <= data.stamp());
+
+			cameraThread.postUpdate(&data, &cameraInfo);
+			cameraInfo.timeTotal = timer.ticks();
+
+			Transform guess = (!pathOdom.empty() && !cameraInfo.odomPose.isNull() && !previousOdomPose.isNull())?previousOdomPose.inverse() * cameraInfo.odomPose:Transform();	
+
+			OdometryInfo odomInfo;
+			Transform previous = odom->getPose();
+			Transform pose = odom->process(data,
+				guess,
+				&odomInfo);
+
+			if(!pose.isNull() && odomInfo.reg.covariance.total() == 36)
+			{
+				previousOdomPose = cameraInfo.odomPose;
+				if(uIsFinite(odomInfo.reg.covariance.at<double>(0,0)) &&
+					odomInfo.reg.covariance.at<double>(0,0)>0.0)
+				{
+					if( !pathOdom.empty() && 
+						odomInfo.reg.covariance.at<double>(0,0) >= 9999 && 
+						!previousOdomPose.isNull() &&
+						(pose.x() != 0.0f || pose.y() != 0.0f || pose.z() != 0.0f)) // not the first frame
+					{
+						// In case of external guess and auto reset, keep reporting lost till we
+						// process the second frame with valid covariance. This way it
+						// won't trigger a new map.
+						pose = Transform();
+					}
+				}
+			}
+
+			if(odomStrategy == 2)
+			{
+				//special case for FOVIS, set covariance 1 if 9999 is detected
+				if(!odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at<double>(0,0) >= 9999)
+				{
+					odomInfo.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
+				}
+			}
+			
+			if(iteration!=0 && !pose.isNull() && !odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at<double>(0,0)>=9999)
+            {
+				UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomInfo.reg.covariance.at<double>(0,0));
+				rtabmap.triggerNewMap();
+            }
+
+			if(odomInfo.keyFrameAdded)
+			{
+				++odomKeyFrames;
+			}
+
+			bool processData = true;
+			if(detectionRate>0.0f &&
+				previousStamp>0.0 &&
+				data.stamp()>previousStamp && data.stamp() - previousStamp < 1.0/detectionRate)
+			{
+				processData = false;
+			}
+
+			if(processData)
+			{
+				previousStamp = data.stamp();
+			}
+
+			if(!processData)
+			{
+				// set negative id so rtabmap will detect it as an intermediate node
+				data.setId(-1);
+				data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
+				processData = intermediateNodes;
+			}
+
+			timer.restart();
+			if(processData)
+			{
+				std::map<std::string, float> externalStats;
+				// save camera statistics to database
+				externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
+				externalStats.insert(std::make_pair("Camera/HistogramEqualization/ms", cameraInfo.timeHistogramEqualization*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
+				externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
+				externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
+				// save odometry statistics to database
+				externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
+				externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
+				externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
+				externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
+				externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
+				externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
+				externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
+				externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
+				externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
+
+				OdometryEvent e(SensorData(), Transform(), odomInfo);
+				rtabmap.process(data, pose, odomInfo.reg.covariance, e.velocity(), externalStats);
+			}
+
+			++iteration;
+			if(!quiet || iteration == totalImages)
+			{
+				double slamTime = timer.ticks();
+
+				float rmse = -1;
+				if(rtabmap.getStatistics().data().find(Statistics::kGtTranslational_rmse()) != rtabmap.getStatistics().data().end())
+				{
+					rmse = rtabmap.getStatistics().data().at(Statistics::kGtTranslational_rmse());
+				}
+
+				if(rmse >= 0.0f)
+				{
+					printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
+							iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
+				}
+				else
+				{
+					printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms",
+							iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
+				}
+				if(processData && rtabmap.getLoopClosureId()>0)
+				{
+					printf(" *");
+				}
+				printf("\n");
+			}
+			else if(iteration % (totalImages/10) == 0)
+			{
+				printf(".");
+				fflush(stdout);
+			}
+
+			cameraInfo = SensorCaptureInfo();
+			timer.restart();
+			data = cameraThread.camera()->takeData(&cameraInfo);
+		}
+		delete odom;
+		printf("Total time=%fs\n", totalTime.ticks());
+		/////////////////////////////
+		// Processing dataset end
+		/////////////////////////////
+
+		// Save trajectory
+		printf("Saving trajectory...\n");
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> links;
+		std::map<int, Signature> signatures;
+		std::map<int, double> stamps;
+		rtabmap.getGraph(poses, links, true, true, &signatures);
+		for(std::map<int, Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
+		{
+			stamps.insert(std::make_pair(iter->first, iter->second.getStamp()));
+		}
+		std::string pathTrajectory = output+"/"+outputName+"_poses.txt";
+		if(poses.size() && graph::exportPoses(pathTrajectory, 1, poses, links, stamps))
+		{
+			printf("Saving %s... done!\n", pathTrajectory.c_str());
+		}
+		else
+		{
+			printf("Saving %s... failed!\n", pathTrajectory.c_str());
+		}
+
+		if(!pathGt.empty())
+		{
+			// Log ground truth statistics
+			std::map<int, Transform> groundTruth;
+
+			for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				Transform o, gtPose;
+				int m,w;
+				std::string l;
+				double s;
+				std::vector<float> v;
+				GPS gps;
+				EnvSensors sensors;
+				rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, sensors, true);
+				if(!gtPose.isNull())
+				{
+					groundTruth.insert(std::make_pair(iter->first, gtPose));
+				}
+			}
+
+
+			// compute RMSE statistics
+			float translational_rmse = 0.0f;
+			float translational_mean = 0.0f;
+			float translational_median = 0.0f;
+			float translational_std = 0.0f;
+			float translational_min = 0.0f;
+			float translational_max = 0.0f;
+			float rotational_rmse = 0.0f;
+			float rotational_mean = 0.0f;
+			float rotational_median = 0.0f;
+			float rotational_std = 0.0f;
+			float rotational_min = 0.0f;
+			float rotational_max = 0.0f;
+			graph::calcRMSE(
+					groundTruth,
+					poses,
+					translational_rmse,
+					translational_mean,
+					translational_median,
+					translational_std,
+					translational_min,
+					translational_max,
+					rotational_rmse,
+					rotational_mean,
+					rotational_median,
+					rotational_std,
+					rotational_min,
+					rotational_max);
+
+			printf("   translational_rmse=   %f m\n", translational_rmse);
+			printf("   rotational_rmse=      %f deg\n", rotational_rmse);
+
+			FILE * pFile = 0;
+			std::string pathErrors = output+"/"+outputName+"_rmse.txt";
+			pFile = fopen(pathErrors.c_str(),"w");
+			if(!pFile)
+			{
+				UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
+			}
+			fprintf(pFile, "Ground truth comparison:\n");
+			fprintf(pFile, "  translational_rmse=   %f\n", translational_rmse);
+			fprintf(pFile, "  translational_mean=   %f\n", translational_mean);
+			fprintf(pFile, "  translational_median= %f\n", translational_median);
+			fprintf(pFile, "  translational_std=    %f\n", translational_std);
+			fprintf(pFile, "  translational_min=    %f\n", translational_min);
+			fprintf(pFile, "  translational_max=    %f\n", translational_max);
+			fprintf(pFile, "  rotational_rmse=      %f\n", rotational_rmse);
+			fprintf(pFile, "  rotational_mean=      %f\n", rotational_mean);
+			fprintf(pFile, "  rotational_median=    %f\n", rotational_median);
+			fprintf(pFile, "  rotational_std=       %f\n", rotational_std);
+			fprintf(pFile, "  rotational_min=       %f\n", rotational_min);
+			fprintf(pFile, "  rotational_max=       %f\n", rotational_max);
+			fclose(pFile);
+		}
+	}
+	else
+	{
+		UERROR("Camera init failed!");
+	}
+
+	printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+outputName+".db").c_str());
+	printf("Do:\n"
+			" $ rtabmap-databaseViewer %s\n\n", (output+"/"+outputName+".db").c_str());
+
+	return 0;
+}
@@ -52,7 +52,8 @@ void showUsage()
 			"  after modifying Source settings.\n"
 			"Options:\n"
 			"  -debug    Show debug log.\n"
-			"  -hide     Don't display the current cloud recorded.\n");
+			"  -hide     Don't display the current cloud recorded.\n"
+			"  -ram      Record all frames in RAM before dumping on hard-drive on exit.\n");
 	exit(1);
 }

@@ -105,6 +106,7 @@ int main (int argc, char * argv[])
 	// parse arguments
 	std::string fileName;
 	bool show = true;
+	bool recordInRAM = false;
 	std::string configFile;

 	if(argc < 3)
@@ -123,6 +125,11 @@ int main (int argc, char * argv[])
 			show = false;
 			continue;
 		}
+		if(strcmp(argv[i], "-ram") == 0)
+		{
+			recordInRAM = true;
+			continue;
+		}
 		printf("Unrecognized option : %s\n", argv[i]);
 		showUsage();
 	}
@@ -139,6 +146,7 @@ int main (int argc, char * argv[])

 	UINFO("Output = %s", fileName.c_str());
 	UINFO("Show = %s", show?"true":"false");
+	UINFO("RecordInRAM = %s", recordInRAM?"true":"false");
 	UINFO("Config = %s", configFile.c_str());

 	app = new QApplication(argc, argv);
@@ -207,7 +215,7 @@ int main (int argc, char * argv[])

 	DataRecorder recorder;

-	if(recorder.init(fileName.c_str()))
+	if(recorder.init(fileName.c_str(), recordInRAM))
 	{
 		recorder.registerToEventsManager();
 		if(show)
@@ -58,29 +58,37 @@ void showUsage()
 			"    -i #          Iterations (default 1).\n"
 			"    --intra       Add only intra-session loop closures.\n"
 			"    --inter       Add only inter-session loop closures.\n"
+			"    --session #   Add loop closures only from/to that map session ID (use -1 for last session).\n"
 			"\n%s", Parameters::showUsage());
 	exit(1);
 }

-// catch ctrl-c
 bool g_loopForever = true;
+class PrintProgressState : public ProgressState
+{
+public:
+	PrintProgressState() :
+		stamp_(UTimer::now())
+	{}
+	virtual bool callback(const std::string & msg) const
+	{
+		if(!msg.empty())
+			printf("[%f] %s \n", UTimer::now()-stamp_, msg.c_str());
+		return g_loopForever;
+	}
+private:
+	double stamp_;
+};
+PrintProgressState progress;
+
+// catch ctrl-c
 void sighandler(int sig)
 {
 	printf("\nSignal %d caught...\n", sig);
 	g_loopForever = false;
+	progress.setCanceled(true);
 }

-class PrintProgressState : public ProgressState
-{
-public:
-	virtual bool callback(const std::string & msg) const
-	{
-		if(!msg.empty())
-			printf("%s \n", msg.c_str());
-		return g_loopForever;
-	}
-};
-
 int main(int argc, char * argv[])
 {
 	signal(SIGABRT, &sighandler);
@@ -88,7 +96,7 @@ int main(int argc, char * argv[])
 	signal(SIGINT, &sighandler);

 	ULogger::setType(ULogger::kTypeConsole);
-	ULogger::setLevel(ULogger::kError);
+	ULogger::setLevel(ULogger::kWarning);

 	if(argc < 2)
 	{
@@ -101,6 +109,7 @@ int main(int argc, char * argv[])
 	int iterations = 1;
 	bool intraSession = false;
 	bool interSession = false;
+	int fromToMapId = -2;
 	for(int i=1; i<argc-1; ++i)
 	{
 		if(std::strcmp(argv[i], "--help") == 0)
@@ -171,9 +180,25 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(std::strcmp(argv[i], "--session") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				fromToMapId = uStr2Int(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
 	}
 	ParametersMap inputParams = Parameters::parseArguments(argc,  argv);

+	// Add some optimizations (soft set, can be overriden by arguments)
+	inputParams.insert(ParametersPair(Parameters::kMemLoadVisualLocalFeaturesOnInit(), "false")); // don't need features already loaded in RAM
+	inputParams.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // should be incremental to update links
+
 	std::string dbPath = argv[argc-1];
 	if(!UFile::exists(dbPath))
 	{
@@ -221,15 +246,32 @@ int main(int argc, char * argv[])
 	// Get the global optimized map
 	Rtabmap rtabmap;
 	printf("Initialization...\n");
+	UTimer timer;
+	ParametersMap originalParameters = parameters;
 	uInsert(parameters, inputParams);
 	rtabmap.init(parameters, dbPath);
+	printf("Initialization... done! (%f sec)\n", timer.ticks());

 	float xMin, yMin, cellSize;
 	bool haveOptimizedMap = !rtabmap.getMemory()->load2DMap(xMin, yMin, cellSize).empty();

-	PrintProgressState progress;
+	if(fromToMapId <= -2)
+	{
+		fromToMapId = -1; // means all below
+	}
+	else
+	{
+		bool last = false;
+		if(fromToMapId == -1)
+		{
+			// get last session ID
+			fromToMapId = rtabmap.getMemory()->getMapId(rtabmap.getMemory()->getLastSignatureId(), true);
+		}
+		printf("From/To Session ID = %d%s\n", fromToMapId, last?" (last session)":"");
+	}
+
 	printf("Detecting...\n");
-	int detected = rtabmap.detectMoreLoopClosures(clusterRadiusMax, clusterAngle, iterations, intraSession, interSession, &progress, clusterRadiusMin);
+	int detected = rtabmap.detectMoreLoopClosures(clusterRadiusMax, clusterAngle, iterations, intraSession, interSession, &progress, clusterRadiusMin, fromToMapId);
 	if(detected < 0)
 	{
 		if(!g_loopForever)
@@ -266,6 +308,9 @@ int main(int argc, char * argv[])
 		}
 	}

+	// Restore original parameters before saving back the database
+	rtabmap.parseParameters(originalParameters);
+
 	rtabmap.close();

 	return 0;
@@ -1691,15 +1691,24 @@ int main(int argc, char * argv[])
 			{
 				if(voxelSize>0.0f)
 				{
-					if(cloud.get() && !cloud->empty())
+					if(cloud.get() && !cloud->empty()) {
 						cloud = rtabmap::util3d::voxelize(cloud, indices, voxelSize);
-					else if(cloudI.get() && !cloudI->empty())
+						if(!cloud->empty())
+							cloud = rtabmap::util3d::transformPointCloud(cloud, iter->second);
+					}
+					else if(cloudI.get() && !cloudI->empty()) {
 						cloudI = rtabmap::util3d::voxelize(cloudI, indices, voxelSize);
+						if(!cloudI->empty())
+							cloudI = rtabmap::util3d::transformPointCloud(cloudI, iter->second);
+					}
+				}
+				else
+				{
+					if(cloud.get() && !cloud->empty())
+						cloud = rtabmap::util3d::transformPointCloud(cloud, indices, iter->second);
+					else if(cloudI.get() && !cloudI->empty())
+						cloudI = rtabmap::util3d::transformPointCloud(cloudI, indices, iter->second);
 				}
-				if(cloud.get() && !cloud->empty())
-					cloud = rtabmap::util3d::transformPointCloud(cloud, iter->second);
-				else if(cloudI.get() && !cloudI->empty())
-					cloudI = rtabmap::util3d::transformPointCloud(cloudI, iter->second);

 				if(filter_ceiling != 0.0 || filter_floor != 0.0f)
 				{
@@ -87,6 +87,11 @@ void showUsage()
 			"   --to_depth \"to_depth.png\"        Depth or right image file of the second image.\n"
 			"                                        For 3D->3D estimation, from_depth and to_depth\n"
 			"                                        should be both set.\n"
+			"   --raw                              Provided images are raw and should be rectified.\n"
+			"                                        Doesn't need to be explicitly set if calibration\n"
+			"                                        is not provided. For RGB-D data, only the RGB image\n"
+			"                                        is rectified, the depth is assumed already matching\n"
+			"                                        the rectified one.\n"
 			"\n\n"
 			"%s\n",
 			Parameters::showUsage());
@@ -107,6 +112,7 @@ int main(int argc, char * argv[])
 	std::string toDepthPath;
 	std::string calibrationPath;
 	std::string calibrationToPath;
+	bool imagesRectified = true;
 	for(int i=1; i<argc-2; ++i)
 	{
 		if(strcmp(argv[i], "--from_depth") == 0)
@@ -157,6 +163,10 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(strcmp(argv[i], "--raw") == 0)
+		{
+			imagesRectified = false;
+		}
 		else if(strcmp(argv[i], "--help") == 0)
 		{
 			showUsage();
@@ -171,6 +181,10 @@ int main(int argc, char * argv[])
 	}
 	printf("  --from_depth  = \"%s\"\n", fromDepthPath.c_str());
 	printf("  --to_depth    = \"%s\"\n", toDepthPath.c_str());
+	if(!imagesRectified)
+	{
+		printf("  --raw (images will be rectified)\n");
+	}

 #ifdef RTABMAP_PYTHON
 	rtabmap::PythonInterface pythonInterface;
@@ -311,12 +325,57 @@ int main(int argc, char * argv[])
 		if(model.isValidForProjection())
 		{
 			printf("Mono calibration model detected.\n");
+
+			if(!imagesRectified)
+			{
+				if(!model.isValidForRectification())
+				{
+					printf("ERROR: calibration model \"%s\" is not valid for rectification and --raw option was set. Aborting.\n", calibrationPath.c_str());
+					exit(-1);
+				}
+				if(!model.isRectificationMapInitialized()) {
+					model.initRectificationMap();
+				}
+				if(!modelTo.isValidForRectification())
+				{
+					printf("ERROR: calibration model \"%s\" is not valid for rectification and --raw option was set. Aborting.\n", calibrationToPath.c_str());
+					exit(-1);
+				}
+				if(!modelTo.isRectificationMapInitialized()) {
+					modelTo.initRectificationMap();
+				}
+				imageFrom = model.rectifyImage(imageFrom);
+				imageTo = modelTo.rectifyImage(imageTo);
+			}
 			dataFrom = SensorData(imageFrom, fromDepth, model, 1);
 			dataTo = SensorData(imageTo, toDepth, modelTo, 2);
 		}
 		else //stereo
 		{
 			printf("Stereo calibration model detected.\n");
+			if(!imagesRectified)
+			{
+				if(!stereoModel.isValidForRectification())
+				{
+					printf("ERROR: stereo calibration model \"%s\" is not valid for rectification and --raw option was set. Aborting.\n", calibrationPath.c_str());
+					exit(-1);
+				}
+				if(!stereoModel.isRectificationMapInitialized()) {
+					stereoModel.initRectificationMap();
+				}
+				if(!stereoModelTo.isValidForRectification())
+				{
+					printf("ERROR: stereo calibration model \"%s\" is not valid for rectification and --raw option was set. Aborting.\n", calibrationToPath.c_str());
+					exit(-1);
+				}
+				if(!stereoModelTo.isRectificationMapInitialized()) {
+					stereoModelTo.initRectificationMap();
+				}
+				imageFrom = stereoModel.left().rectifyImage(imageFrom);
+				fromDepth = stereoModel.right().rectifyImage(fromDepth);
+				imageTo = stereoModelTo.left().rectifyImage(imageTo);
+				toDepth = stereoModelTo.right().rectifyImage(toDepth);
+			}
 			dataFrom = SensorData(imageFrom, fromDepth, stereoModel, 1);
 			dataTo = SensorData(imageTo, toDepth, stereoModelTo, 2);
 		}
@@ -329,7 +388,7 @@ int main(int argc, char * argv[])
 		{
 			parameters.insert(ParametersPair(Parameters::kVisEstimationType(), "2")); // Set 2D->2D estimation for mono images
 			parameters.insert(ParametersPair(Parameters::kVisEpipolarGeometryVar(), "1")); //Unknown scale
-			printf("Calibration not set, setting %s=1 and %s=2 by default (2D->2D estimation)\n", Parameters::kVisEpipolarGeometryVar().c_str(), Parameters::kVisEstimationType().c_str());
+			printf("Depth/Stereo not set, setting %s=1 and %s=2 by default (2D->2D estimation)\n", Parameters::kVisEpipolarGeometryVar().c_str(), Parameters::kVisEstimationType().c_str());
 		}
 		RegistrationVis reg(parameters);
 		RegistrationInfo info;
@@ -0,0 +1,13 @@
+
+ADD_EXECUTABLE(reduceGraph main.cpp)
+  
+TARGET_LINK_LIBRARIES(reduceGraph rtabmap_core)
+
+SET_TARGET_PROPERTIES( reduceGraph 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-reduceGraph)
+
+INSTALL(TARGETS reduceGraph
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
+
+
@@ -0,0 +1,281 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/DBDriver.h>
+#include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/Memory.h>
+#include <rtabmap/core/global_map/OccupancyGrid.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_surface.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+#include <pcl/filters/filter.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/obj_io.h>
+#include <pcl/common/common.h>
+#include <pcl/surface/poisson.h>
+#include <stdio.h>
+#include <signal.h>
+
+using namespace rtabmap;
+
+void showUsage(const char * exec)
+{
+	printf("\nUsage:\n"
+			"%s [Options] database.db\n"
+			"Options:\n"
+			"    --keep_latest  Merge old nodes to newer nodes, thus keeping only latest nodes.\n"
+			"    --keep_linked  Keep reduced nodes linked to graph.\n"
+			"    --pre_cleanup  Remove all user loop closures linking nodes closer than %s in the graph before reducing the graph.\n"
+			"    --radius #.#   Maximum loop closure distance that can be merged. Default is 1 m. Should be > 0.\n"
+			"    --udebug/--uinfo/--warn can also be used to change verbosity.\n"
+			"\n", exec, Parameters::kMemSTMSize().c_str());
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	if(argc < 2)
+	{
+		showUsage(argv[0]);
+	}
+
+	bool keepLatest = false;
+	bool keepLinked = false;
+	float radius = 1.0f;
+	bool preCleanup = false;
+	for(int i=1; i<argc; ++i)
+	{
+		if(std::strcmp(argv[i], "--help") == 0)
+		{
+			showUsage(argv[0]);
+		}
+		else if(std::strcmp(argv[i], "--keep_latest") == 0)
+		{
+			keepLatest = true;
+		}
+		else if(std::strcmp(argv[i], "--keep_linked") == 0)
+		{
+			keepLinked = true;
+		}
+		else if(std::strcmp(argv[i], "--pre_cleanup") == 0)
+		{
+			preCleanup = true;
+		}
+		else if(std::strcmp(argv[i], "--radius") == 0)
+		{
+			++i;
+			if(i < argc-1)
+			{
+				radius = uStr2Float(argv[i]);
+				if(radius <= 0.0f)
+				{
+					printf("--radius should be > 0, parsed %f\n", radius);
+					showUsage(argv[0]);
+				}
+			}
+			else {
+				showUsage(argv[0]);
+			}
+		}
+	}
+	printf("Parameters:\n");
+	printf("  radius = %f m\n", radius);
+	printf("  keep_latest = %s\n", keepLatest?"true":"false");
+	printf("  keep_linked = %s\n", keepLinked?"true":"false");
+	printf("  pre_cleanup = %s\n", preCleanup?"true":"false");
+	
+	// Just parse logging options
+	Parameters::parseArguments(argc, argv);
+
+	// Add some optimizations
+	ParametersMap inputParams;
+	inputParams.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true")); // load the whole map in RAM
+	inputParams.insert(ParametersPair(Parameters::kMemLoadVisualLocalFeaturesOnInit(), "false")); // don't need features already loaded in RAM
+
+	std::string dbPath = argv[argc-1];
+	if(!UFile::exists(dbPath))
+	{
+		printf("Database %s doesn't exist!\n", dbPath.c_str());
+		return 1;
+	}
+
+	printf("Database: %s\n", dbPath.c_str());
+
+	// Get parameters
+	ParametersMap parameters;
+	DBDriver * driver = DBDriver::create();
+	if(driver->openConnection(dbPath))
+	{
+		parameters = driver->getLastParameters();
+		driver->closeConnection(false);
+	}
+	else
+	{
+		UERROR("Cannot open database %s!", dbPath.c_str());
+	}
+	delete driver;
+
+	Memory memory;
+	printf("Initialization...\n");
+	UTimer timer;
+	ParametersMap originalParameters = parameters;
+	uInsert(parameters, inputParams);
+	if(!memory.init(dbPath, false, parameters))
+	{
+		printf("Initialization... failed! Aborting!\n");
+		return 1;
+	}
+	std::set<int> ids = memory.getAllSignatureIds();
+	printf("Initialization... done! %ld nodes loaded. (%f sec)\n", ids.size(), timer.ticks());
+
+	if(ids.empty())
+	{
+		printf("IDs are empty?! Aborting.\n");
+		return 1;
+	}
+
+	Transform lastLocalizationPose;
+	std::map<int, Transform> optimizedPoses = memory.loadOptimizedPoses(&lastLocalizationPose);
+	float xMin, yMin, cellSize;
+	bool hasOptimizedMap = !memory.load2DMap(xMin, yMin, cellSize).empty();
+
+	int totalNodesReduced = 0;
+	std::vector<int> vids;
+	vids.reserve(ids.size());
+	if(keepLatest)
+	{
+		// we process older to newer nodes, merging to new nodes
+		vids.insert(vids.end(), ids.begin(), ids.end());
+	}
+	else
+	{
+		// we process newer to older nodes, merging to old nodes
+		vids.insert(vids.end(), ids.rbegin(), ids.rend());
+	}
+
+	if(preCleanup)
+	{
+		if(memory.getMaxStMemSize() <= 1)
+		{
+			printf("--pre_cleanup is used but %s <= 1, skipping pre cleanup...\n", Parameters::kMemSTMSize().c_str());
+		}
+		else
+		{
+			int totalRemoved = 0;
+			for(auto id: vids)
+			{
+				auto nids = memory.getNeighborsId(id, memory.getMaxStMemSize(), -1, true, true, true);
+				auto links = memory.getLinks(id, true, false);
+				for(auto link:links)
+				{
+					if( link.second.type() == Link::kUserClosure &&
+						nids.find(link.first)!=nids.end())
+					{
+						memory.removeLink(id, link.first);
+						++totalRemoved;
+					}
+				}
+			}
+			printf("Removed %d user links that were linking nodes that were close in the graph (below %s=%d)\n",
+				totalRemoved, Parameters::kMemSTMSize().c_str(), memory.getMaxStMemSize());
+		}
+	}
+
+	for(auto id: vids)
+	{
+		// Nodes can be already reduced by other nodes, check if they are still there
+		if(memory.getSignature(id) != 0)
+		{
+			int reducedId = memory.reduceNode(id, radius, keepLinked, keepLatest?1:-1);
+			if(reducedId > 0)
+			{
+				printf("Reduced node %d to node %d!\n", id, reducedId);
+				++totalNodesReduced;
+			}
+		}
+	}
+	printf("Reduced a total of %d nodes out of %ld nodes\n", totalNodesReduced, ids.size());
+
+	if(!optimizedPoses.empty())
+	{
+		size_t removed = 0;
+		// cleanup reduced nodes from the optimized poses
+		for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(0); iter!=optimizedPoses.end();)
+		{
+			if(memory.getSignature(iter->first) == 0)
+			{
+				iter = optimizedPoses.erase(iter);
+				++removed;
+			}
+			else
+			{
+				++iter;
+			}
+		}
+		printf("Updated optimized graph from %ld poses to %ld poses\n", optimizedPoses.size()+removed, optimizedPoses.size());
+		memory.saveOptimizedPoses(optimizedPoses, lastLocalizationPose);
+	}
+	if(hasOptimizedMap)
+	{
+		printf("The database has a global occupancy grid, regenerating one with the remaining nodes of the optimized graph!\n");
+		LocalGridCache cache;
+		OccupancyGrid grid(&cache, parameters);
+		for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(0); iter!=optimizedPoses.end(); ++iter)
+		{
+			SensorData data = memory.getNodeData(iter->first, false, false, false, true);
+			data.uncompressData();
+			cache.add(iter->first, data.gridGroundCellsRaw(), data.gridObstacleCellsRaw(), data.gridEmptyCellsRaw(), data.gridCellSize(), data.gridViewPoint());
+		}
+		grid.update(optimizedPoses);
+		cv::Mat map = grid.getMap(xMin, yMin);
+		if(map.empty())
+		{
+			printf("Could not regenerate the global occupancy grid! The grid is not updated.\n");
+		}
+		else
+		{
+			memory.save2DMap(map, xMin, yMin, grid.getCellSize());
+			printf("Saved the new global occupancy grid!\n");
+		}
+	}
+
+	// Restore original parameters before saving back the database
+	memory.parseParameters(originalParameters);
+	
+	printf("Saving all changes to database...\n");
+	memory.close(true);
+
+	return 0;
+}
--- a/Show More
+++ b/Show More