missing ressources

adding 0_23_0 schema
functionnal
2026-09-11 14:00:20 +08:00 · 2026-04-05 19:01:08 -07:00 · 2026-04-05 18:59:56 -07:00 · 2026-04-05 18:27:53 -07:00 · 2026-04-05 14:53:11 -07:00 · 2026-04-05 14:06:48 -07:00
198 changed files with 18751 additions and 6948 deletions
@@ -1,152 +0,0 @@
-
-branches:
-  only:
-    - master
-    - devel
-
-os: Visual Studio 2015
-
-clone_folder: c:\projects\rtabmap
-
-platform: x64
-configuration: Release
-
-init:
-  - cmake --version
-  - call "C:\Program Files\Microsoft SDKs\Windows\v7.1\Bin\SetEnv.cmd" /x64
-  - call "C:\Program Files (x86)\Microsoft Visual Studio 14.0\VC\vcvarsall.bat" x86_amd64
-
-install:
-  # To download from google drive
-  - set PATH=C:\Python38-x64;C:\Python38-x64\Scripts;%PATH%
-  - ps: py -m pip --disable-pip-version-check install gdown>=5.1.0 
-  # Qt
-  - set QTDIR=C:\Qt\5.10.1\msvc2015_64
-  # make sure Qt bin path is before cmake bin path to avoid copying qt5 dlls from cmake before qt installation
-  - set PATH=%QTDIR%\bin;%PATH%
-  # Boost
-  - set PATH=%PATH%;C:\Libraries\boost_1_62_0\lib64-msvc-14.0
-  # Openni2
-  - ps: wget 'https://dl.dropboxusercontent.com/s/d98jv79l6oy9fxz/OpenNI2.exe?dl=0' -outfile OpenNI2.exe
-  - cmd: OpenNI2.exe -o"C:\Program Files" -y
-  - ECHO "Installed OpenNI2:"
-  - ps: "ls \"C:/Program Files/OpenNI2\""
-  - set PATH=%PATH%;C:\Program Files\OpenNI2\Redist
-  - set OPENNI2_INCLUDE64=C:\Program Files\OpenNI2\Include
-  - set OPENNI2_LIB64=C:\Program Files\OpenNI2\Lib
-  - set OPENNI2_REDIST64=C:\Program Files\OpenNI2\Redist
-  # OpenCV
-  #- appveyor-retry appveyor DownloadFile http://downloads.sourceforge.net/project/opencvlibrary/4.5.2/opencv-4.5.2-vc14_vc15.exe
-  #- cmd: opencv-4.5.2-vc14_vc15.exe -o"C:\Program Files" -y
-  #- ECHO "Installed OpenCV:"
-  #- ps: "ls \"C:/Program Files/opencv/build\""
-  #- set PATH=%PATH%;C:\Program Files\opencv\build\x64\vc14\bin
-  - ps: wget 'https://dl.dropboxusercontent.com/s/o6ofn491bc0jso1/opencv450_vc14.exe?dl=0' -outfile opencv.exe
-  - cmd: opencv.exe -o"C:\Program Files" -y
-  - ECHO "Installed OpenCV:"
-  - ps: "ls \"C:/Program Files/opencv\""
-  - set PATH=%PATH%;C:\Program Files\opencv\x64\vc14\bin
-  # VTK (including QVTK)
-  - ps: wget 'https://dl.dropboxusercontent.com/s/1l33b5l3f3y52gf/VTK-6_3-msvc140.exe?dl=0' -outfile VTK-6_3.exe
-  - cmd: VTK-6_3.exe -o"C:\Program Files" -y
-  - ECHO "Installed VTK:"
-  - ps: "ls \"C:/Program Files/VTK\""
-  - set PATH=%PATH%;C:\Program Files\VTK\bin
-  # QHull 
-  - ps: wget 'https://dl.dropboxusercontent.com/s/9widnk9msdsh2b8/Qhull-msvc140.exe?dl=0' -outfile Qhull.exe
-  - cmd: Qhull.exe -o"C:\Program Files" -y
-  - ECHO "Installed QHull:"
-  - ps: "ls \"C:/Program Files/Qhull\""
-  - set PATH=%PATH%;C:\Program Files\Qhull\bin
-  # FLANN 
-  - ps: wget 'https://dl.dropboxusercontent.com/s/7k58jbmqa51sxmh/FLANN-msvc140.exe?dl=0' -outfile FLANN.exe
-  - cmd: FLANN.exe -o"C:\Program Files" -y
-  - ECHO "Installed FLANN:"
-  - ps: "ls \"C:/Program Files/FLANN\""
-  - set PATH=%PATH%;C:\Program Files\FLANN\bin
-  # Eigen 
-  - ps: wget 'https://dl.dropboxusercontent.com/s/3v6i9i8dxj4o8ji/Eigen.exe?dl=0' -outfile Eigen.exe
-  - cmd: Eigen.exe -o"C:\Program Files" -y
-  - ECHO "Installed Eigen:"
-  - ps: "ls \"C:/Program Files/Eigen\""
-  # PCL
-  - ps: wget 'https://dl.dropboxusercontent.com/s/2iayr4lyqa50i9j/PCL_181_August2018_x64_vc14.exe?dl=0' -outfile PCL_1.8.1.exe
-  - cmd: PCL_1.8.1.exe -o"C:\Program Files" -y
-  - ECHO "Installed PCL:"
-  - ps: "ls \"C:/Program Files/PCL\""
-  - set PATH=%PATH%;C:\Program Files\PCL\bin
-  # zlib
-  - ps: gdown -q 0B46akLGdg-uaYm9MTTI4MUtUcmc
-  - ps: Expand-Archive zlib-1.2.8-vc2010-x64.zip -DestinationPath 'C:\Program Files'
-  - ECHO "Installed zlib:"
-  - ps: "ls \"C:/Program Files/zlib\""
-  - set PATH=%PATH%;C:\Program Files\zlib\bin
-  # g2o
-  - ps: wget 'https://dl.dropboxusercontent.com/s/ht74s5pa21wokzw/g2o.exe?dl=0' -outfile g2o.exe
-  - cmd: g2o.exe -o"C:\Program Files" -y
-  - ECHO "Installed g2o:"
-  - ps: "ls \"C:/Program Files/g2o\""
-  - set PATH=%PATH%;C:\Program Files\g2o\bin
-  # GTSAM
-  - ps: wget 'https://dl.dropboxusercontent.com/s/0fpr6r4cgsqmvhf/GTSAM-4_0_0_alpha2-msvc140.exe?dl=0' -outfile GTSAM.exe
-  - cmd: GTSAM.exe -o"C:\Program Files" -y
-  - ECHO "Installed GTSAM:"
-  - ps: "ls \"C:/Program Files/GTSAM\""
-  - set PATH=%PATH%;C:\Program Files\GTSAM\bin
-  # OctoMap
-  - ps: wget 'https://dl.dropboxusercontent.com/s/6jpxu0nm8ne6e54/octomap_x64_vc14.exe?dl=0' -outfile octomap.exe
-  - cmd: octomap.exe -o"C:\Program Files" -y
-  - ECHO "Installed OctoMap:"
-  - ps: "ls \"C:/Program Files/octomap-distribution\""
-  - set PATH=%PATH%;C:\Program Files\octomap-distribution\bin
-  # CPU-TSDF
-  - ps: wget 'https://dl.dropboxusercontent.com/s/mgges9va1uzxr0q/cpu_tsdf_sept2015_x64_vc14.exe?dl=0' -outfile cpu_tsdf.exe
-  - cmd: cpu_tsdf.exe -o"C:\Program Files" -y
-  - ECHO "Installed CPU-TSDF:"
-  - ps: "ls \"C:/Program Files/cpu_tsdf\""
-  - set PATH=%PATH%;C:\Program Files\cpu_tsdf\bin
-  # Open Chisel
-  - ps: wget 'https://dl.dropboxusercontent.com/s/0aaphcde4acrinm/open_chisel_x64_vc14.exe?dl=0' -outfile open_chisel.exe
-  - cmd: open_chisel.exe -o"C:\Program Files" -y
-  - ECHO "Installed Open Chisel:"
-  - ps: "ls \"C:/Program Files/open_chisel\""
-  - set PATH=%PATH%;C:\Program Files\open_chisel\bin
-  # yaml-cpp
-  - ps: wget 'https://dl.dropboxusercontent.com/s/22qfvftwj6zq8tj/yaml-cpp_x64_vc14.exe?dl=0' -outfile yaml-cpp.exe
-  - cmd: yaml-cpp.exe -o"C:\Program Files" -y
-  - ECHO "Installed yaml-cpp:"
-  - ps: "ls \"C:/Program Files/yaml-cpp\""
-  # RealSense2
-  - ps: wget 'https://github.com/IntelRealSense/librealsense/releases/download/v2.40.0/Intel.RealSense.SDK-WIN10-2.40.0.2482.exe' -outfile realsense2.exe
-  - cmd: realsense2.exe /VERYSILENT
-  - ECHO "Installed RealSense2:"
-  - ps: "ls \"C:/Program Files (x86)/Intel RealSense SDK 2.0\""
-  - set PATH=%PATH%;C:\Program Files (x86)\Intel RealSense SDK 2.0\bin\x64
-  - set RealSense2_ROOT_DIR=C:\Program Files (x86)\Intel RealSense SDK 2.0
-  # Kinect 4 Azure
-  - ps: wget 'https://download.microsoft.com/download/3/d/6/3d6d9e99-a251-4cf3-8c6a-8e108e960b4b/Azure%20Kinect%20SDK%201.4.1.exe' -outfile azure.exe
-  - cmd: azure.exe /quiet
-  - ECHO "Installed Kinect For Azure:"
-  - ps: "ls \"C:/Program Files/Azure Kinect SDK v1.4.1\""
-  - set PATH=%PATH%;C:\Program Files\Azure Kinect SDK v1.4.1\tools
-  - set K4A_ROOT_DIR=C:\Program Files\Azure Kinect SDK v1.4.1
-  
-before_build:
-  - cd c:\projects\rtabmap\build
-  - ECHO %PROGRAMFILES%
-  - ECHO %PATH%
-  - cmake -G "Visual Studio 14 2015 Win64" -DOpenCV_DIR="C:\Program Files\opencv\build" -DPCL_DIR="C:\Program Files\PCL\cmake" -DCPUTSDF_DIR="C:\Program Files\cpu_tsdf\share\cpu_tsdf" -Dyaml-cpp_DIR="C:\Program Files\yaml-cpp\CMake" -DBUILD_AS_BUNDLE=ON ..
-  
-after_build :
-  - cmake --build . --config Release --target package
-
-artifacts:
-  - path: build\RTABMap-*
-  
-notifications:
-  - provider: Email
-    to:
-      - matlabbe@gmail.com
-    on_build_success: false
-    on_build_failure: false
-    on_build_status_changed: true
@@ -0,0 +1,24 @@
+FROM introlab3it/rtabmap:android-noble-deps
+
+# remove ubuntu user
+RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu
+
+RUN apt-get update && apt-get install -y sudo && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG USERNAME=vscode
+ARG USER_UID=1000
+ARG USER_GID=1000
+
+RUN set -ex && \
+    groupadd --gid ${USER_GID} ${USERNAME} && \
+    useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
+    usermod -a -G sudo ${USERNAME} && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
+    chmod 0440 /etc/sudoers.d/${USERNAME}
+
+RUN chmod +x /opt/android-sdk/tools/android
+
+RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
+
+
@@ -1,9 +1,21 @@
 {
-    "image": "introlab3it/rtabmap:android-deps",
+    "build": {
+      "dockerfile": "Dockerfile"
+    },
    "customizations": {
        "vscode": {
          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"]
        }
      },
+    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
+    "workspaceFolder": "/home/vscode/rtabmap",
+    "postStartCommand": "./.devcontainer/android/init.sh",
+    "settings": {
+      "terminal.integrated.defaultProfile.linux": "bash"
+    },
+    "remoteUser": "vscode",
    "runArgs": ["--privileged", "--network=host"]
 }
+
+
+  
@@ -0,0 +1,17 @@
+#!/bin/bash
+set -e
+
+echo "Running post-start initialization..."
+
+# copy required jars
+cp /opt/android/lib/*.jar app/android/libs/.
+
+mkdir -p build_android/arm64-v8a
+
+# resource tool
+cd build_android
+cmake -DANDROID_PREBUILD=ON ..
+make
+
+echo -e "\nTo build the APK, do (adjust API number):"
+echo -e '\nexport ANDROID_API=30 && cd build_android/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=$ANDROID_API -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DOpenCV_DIR=/opt/android/arm64-v8a/sdk/native/jni ../..\nmake -j6\n'
@@ -0,0 +1,21 @@
+FROM introlab3it/rtabmap:focal-deps
+
+RUN apt-get update && apt-get install -y sudo && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG USERNAME=vscode
+ARG USER_UID=1000
+ARG USER_GID=1000
+
+RUN set -ex && \
+    groupadd --gid ${USER_GID} ${USERNAME} && \
+    useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
+    usermod -a -G sudo ${USERNAME} && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
+    chmod 0440 /etc/sudoers.d/${USERNAME}
+
+RUN mkdir -p /home/${USERNAME}/Documents/RTAB-Map && chown -R ${USERNAME} /home/${USERNAME}
+
+RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
+
+
@@ -1,8 +1,18 @@
 {
-    "image": "introlab3it/rtabmap:20.04",
+    "build": {
+      "dockerfile": "Dockerfile"
+    },
    "customizations": {
        "vscode": {
          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
        }
-      }
+      },
+    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
+    "workspaceFolder": "/home/vscode/rtabmap",
+    //"mounts": ["source=${localEnv:HOME}/Documents/RTAB-Map,target=/home/vscode/Documents/RTAB-Map,type=bind,consistency=cached"],
+    "settings": {
+      "terminal.integrated.defaultProfile.linux": "bash"
+    },
+    "remoteUser": "vscode",
+    "runArgs": ["--privileged", "--network=host"]
 }
@@ -0,0 +1,21 @@
+FROM introlab3it/rtabmap:jammy-deps
+
+RUN apt-get update && apt-get install -y sudo && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG USERNAME=vscode
+ARG USER_UID=1000
+ARG USER_GID=1000
+
+RUN set -ex && \
+    groupadd --gid ${USER_GID} ${USERNAME} && \
+    useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
+    usermod -a -G sudo ${USERNAME} && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
+    chmod 0440 /etc/sudoers.d/${USERNAME}
+
+RUN mkdir -p /home/${USERNAME}/Documents/RTAB-Map && chown -R ${USERNAME} /home/${USERNAME}
+
+RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
+
+
@@ -1,8 +1,18 @@
 {
-    "image": "introlab3it/rtabmap:22.04",
+    "build": {
+      "dockerfile": "Dockerfile"
+    },
    "customizations": {
        "vscode": {
          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
        }
-      }
+      },
+    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
+    "workspaceFolder": "/home/vscode/rtabmap",
+    //"mounts": ["source=${localEnv:HOME}/Documents/RTAB-Map,target=/home/vscode/Documents/RTAB-Map,type=bind,consistency=cached"],
+    "settings": {
+      "terminal.integrated.defaultProfile.linux": "bash"
+    },
+    "remoteUser": "vscode",
+    "runArgs": ["--privileged", "--network=host"]
 }
@@ -0,0 +1,104 @@
+ARG ROS_DISTRO=jazzy
+
+FROM osrf/ros:${ROS_DISTRO}-desktop
+
+# Install build dependencies with Qt6 (issue: rtabmap has black window)
+#RUN apt-get update && \
+#    apt-get install -y git software-properties-common ros-${ROS_DISTRO}-rtabmap-ros libqt6* qt6* qml6* && \
+#    apt-get remove -y ros-${ROS_DISTRO}-rtabmap* ros-${ROS_DISTRO}-gtsam ros-${ROS_DISTRO}-libg2o libpcl* libqt5* qt5* libvtk* libopencv* && \
+#    apt-get clean && rm -rf /var/lib/apt/lists/
+
+# Install build dependencies with Qt5
+RUN apt-get update && \
+    apt-get install -y git software-properties-common ros-${ROS_DISTRO}-rtabmap-ros && \
+    apt-get remove -y ros-${ROS_DISTRO}-rtabmap* ros-${ROS_DISTRO}-gtsam ros-${ROS_DISTRO}-libg2o libpcl* libvtk* libopencv* && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+# remove ubuntu user
+RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu
+
+RUN apt-get update && apt-get install -y sudo && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG USERNAME=vscode
+ARG USER_UID=1000
+ARG USER_GID=1000
+
+RUN set -ex && \
+    groupadd --gid ${USER_GID} ${USERNAME} && \
+    useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
+    usermod -a -G sudo ${USERNAME} && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
+    chmod 0440 /etc/sudoers.d/${USERNAME}
+
+RUN mkdir -p /home/${USERNAME}/Documents/RTAB-Map
+
+RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
+
+WORKDIR /home/${USERNAME}/
+
+RUN rm /bin/sh && ln -s /bin/bash /bin/sh
+
+# We build main dependencies from source, cleaning up the
+# build directory after install but keeping the source code
+# to change version/reinstall from inside the dev container
+# if needed.
+
+# Build latest VTK with Qt6
+RUN git clone https://github.com/Kitware/VTK.git
+RUN cd VTK && \
+    mkdir build && \
+    cd build && \
+    cmake -DVTK_GROUP_ENABLE_Qt=YES .. && \
+    make -j$(nproc) && \
+    make install && \
+    make clean
+    
+# Build latest PCL with latest VTK
+# Make sure all libraries depending on Eigen are built with same CXX standard (17)
+RUN git clone https://github.com/PointCloudLibrary/pcl.git
+RUN cd pcl && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_CXX_STANDARD=17 -DBUILD_tools=ON -DPCL_ENABLE_AVX=OFF -DPCL_ENABLE_MARCHNATIVE=OFF -DPCL_ENABLE_SSE=OFF .. && \
+    make -j$(nproc) && \
+    make install && \
+    make clean
+    
+# Build latest OpenCV
+RUN git clone https://github.com/opencv/opencv.git
+RUN git clone https://github.com/opencv/opencv_contrib.git
+RUN cd opencv && \
+    mkdir build && \
+    cd build && \
+    cmake -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=../../opencv_contrib/modules .. && \
+    make -j$(nproc) && \
+    make install && \
+    make clean
+
+# Build latest gtsam
+RUN git clone https://github.com/borglab/gtsam.git
+RUN cd gtsam && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_CXX_STANDARD=17 -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_BUILD_UNSTABLE=OFF -DGTSAM_INSTALL_CPPUNILITE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    make clean
+
+# Build latest g2o
+RUN git clone https://github.com/RainerKuemmerle/g2o.git
+RUN cd g2o && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_CXX_STANDARD=17 -DBUILD_WITH_MARCH_NATIVE=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DG2O_USE_OPENGL=OFF -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    make clean
+
+# ros2 seems not sourcing by default its multi-arch folders
+ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/${ROS_DISTRO}/lib/x86_64-linux-gnu
+
+RUN ldconfig
+
+RUN chown -R ${USERNAME} /home/${USERNAME}
@@ -0,0 +1,21 @@
+{
+    "build": {
+      "dockerfile": "Dockerfile",
+      "args": {
+        "ROS_DISTRO": "jazzy"
+      }
+    },
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
+        }
+      },
+    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
+    "workspaceFolder": "/home/vscode/rtabmap",
+    //"mounts": ["source=${localEnv:HOME}/Documents/RTAB-Map,target=/home/vscode/Documents/RTAB-Map,type=bind,consistency=cached"],
+    "settings": {
+      "terminal.integrated.defaultProfile.linux": "bash"
+    },
+    "remoteUser": "vscode",
+    "runArgs": ["--privileged", "--network=host"]
+}
@@ -0,0 +1,25 @@
+FROM introlab3it/rtabmap:noble-deps
+
+# For devcontainer
+# remove ubuntu user
+RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu
+
+RUN apt-get update && apt-get install -y sudo && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG USERNAME=vscode
+ARG USER_UID=1000
+ARG USER_GID=1000
+
+RUN set -ex && \
+    groupadd --gid ${USER_GID} ${USERNAME} && \
+    useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
+    usermod -a -G sudo ${USERNAME} && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
+    chmod 0440 /etc/sudoers.d/${USERNAME}
+
+RUN mkdir -p /home/${USERNAME}/Documents/RTAB-Map && chown -R ${USERNAME} /home/${USERNAME}
+
+RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
+
+
@@ -1,8 +1,18 @@
 {
-    "image": "introlab3it/rtabmap:24.04",
+    "build": {
+      "dockerfile": "Dockerfile"
+    },
    "customizations": {
        "vscode": {
          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
        }
-      }
+      },
+    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
+    "workspaceFolder": "/home/vscode/rtabmap",
+    //"mounts": ["source=${localEnv:HOME}/Documents/RTAB-Map,target=/home/vscode/Documents/RTAB-Map,type=bind,consistency=cached"],
+    "settings": {
+      "terminal.integrated.defaultProfile.linux": "bash"
+    },
+    "remoteUser": "vscode",
+    "runArgs": ["--privileged", "--network=host"]
 }
@@ -0,0 +1,79 @@
+
+FROM ubuntu:24.04
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+# Install ROS2
+RUN apt update && \
+    apt install software-properties-common -y && \
+    add-apt-repository universe && \
+    apt update && \
+    apt install curl -y && \
+    curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \
+    echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+# Install build dependencies
+RUN apt-get update && \
+    apt upgrade -y && \
+    apt-get install -y \
+      git \
+      wget \
+      libtbb-dev \
+      libproj-dev \
+      libpcl-dev \
+      liboctomap-dev \
+      libfreenect-dev \
+      ros-rolling-ros-base \
+      ros-dev-tools \
+      ros-rolling-cv-bridge \
+      ros-rolling-image-geometry \
+      ros-rolling-laser-geometry \
+      ros-rolling-pcl-conversions \
+      ros-rolling-rviz-common \
+      ros-rolling-rviz-rendering \
+      ros-rolling-rviz-default-plugins \
+      ros-rolling-pcl-ros \
+      ros-rolling-imu-filter-madgwick \
+      ros-rolling-image-transport \
+      ros-rolling-octomap-msgs \
+      ros-rolling-libg2o \
+      ros-rolling-gtsam \
+      ros-rolling-libpointmatcher \
+      ros-rolling-qt-gui-cpp \
+      ros-rolling-diagnostic-updater && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+WORKDIR /root/
+
+RUN rm /bin/sh && ln -s /bin/bash /bin/sh
+
+RUN echo -e '#!/bin/bash\nset -e\n\n# setup ros2 environment\nsource "/opt/ros/rolling/setup.bash" --\nexec "$@"' > /ros_entrypoint.sh
+RUN chmod +x /ros_entrypoint.sh
+ENTRYPOINT [ "/ros_entrypoint.sh" ]
+
+# ros2 seems not sourcing by default its multi-arch folders
+ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/rolling/lib/x86_64-linux-gnu
+
+# For devcontainer
+# remove ubuntu user
+RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu
+
+RUN apt-get update && apt-get install -y sudo && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG USERNAME=vscode
+ARG USER_UID=1000
+ARG USER_GID=1000
+
+RUN set -ex && \
+    groupadd --gid ${USER_GID} ${USERNAME} && \
+    useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
+    usermod -a -G sudo ${USERNAME} && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
+    chmod 0440 /etc/sudoers.d/${USERNAME}
+
+RUN mkdir -p /home/${USERNAME}/Documents/RTAB-Map && chown -R ${USERNAME} /home/${USERNAME}
+
+RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
+
@@ -0,0 +1,18 @@
+{
+    "build": {
+      "dockerfile": "Dockerfile"
+    },
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
+        }
+      },
+    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
+    "workspaceFolder": "/home/vscode/rtabmap",
+    //"mounts": ["source=${localEnv:HOME}/Documents/RTAB-Map,target=/home/vscode/Documents/RTAB-Map,type=bind,consistency=cached"],
+    "settings": {
+      "terminal.integrated.defaultProfile.linux": "bash"
+    },
+    "remoteUser": "vscode",
+    "runArgs": ["--privileged", "--network=host"]
+}
@@ -0,0 +1,53 @@
+name: 'Install Windows Dependencies with CUDA'
+description: 'Installs PCL, Qt, VTK, g2o and others'
+runs:
+  using: "composite"
+  steps:
+    - name: Set up MSVC Developer Command Prompt
+      uses: ilammy/msvc-dev-cmd@v1
+      with:
+        arch: x64 
+  
+    - name: Install CUDA
+      uses: Jimver/cuda-toolkit@v0.2.30
+      id: cuda-toolkit
+      with:
+        cuda: '13.0.0'
+        use-github-cache: True
+
+    - name: Verify CUDA
+      shell: bash
+      run: |
+        nvcc --version
+        echo "CUDA Path: $CUDA_PATH"
+  
+    - name: Cache vcpkg
+      id: cache-vcpkg
+      uses: actions/cache@v4
+      with:
+        path: ${{ runner.workspace }}/vcpkg_installed
+        key: ${{ runner.os }}-vcpkg-export-66c0373d-x64-vs2022-cuda130_v1
+
+    - name: Download and Install vcpkg
+      if: steps.cache-vcpkg.outputs.cache-hit != 'true'
+      shell: pwsh
+      run: |
+        $install_dir = "${{ runner.workspace }}\vcpkg_installed"
+        $archivePath = "${{ runner.workspace }}\vcpkg-export.7z"
+
+        # The file has been built locally with bundle-windows-deps.bat
+        $url = "https://github.com/introlab/rtabmap/releases/download/0.23.1/vcpkg-export-66c0373d-x64-vs2022-cuda130.7z"
+
+        Invoke-WebRequest -Uri $url -OutFile $archivePath
+        & 7z x $archivePath "-o$install_dir" -y
+
+    - name: Add vcpkg to PATH and env variable
+      shell: pwsh
+      run: |
+        $vcpkg_path = "${{ runner.workspace }}\vcpkg_installed"
+        echo "VCPKG_EXPORT_PATH=$vcpkg_path" | Out-File -FilePath $env:GITHUB_ENV -Encoding utf8 -Append
+        echo "$vcpkg_path\installed\x64-windows-release\bin" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append     
+        echo "${{env.CUDA_PATH}}\bin\x64" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append
+        echo "${{env.CUDA_PATH}}\extras\CUPTI\lib64" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append
+        echo "$vcpkg_path\installed\x64-windows-release\tools\python3\Lib\site-packages\torch\lib" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append
+        echo "$vcpkg_path\installed\x64-windows-release\tools\python3\Lib\site-packages\numpy.libs" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append
@@ -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-*
@@ -0,0 +1,66 @@
+name: CMake-Linux
+
+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: [ubuntu-22.04, ubuntu-24.04, ubuntu-24.04-with-opengv]
+        include:
+        - build_name: ubuntu-22.04
+          os: ubuntu-22.04
+          extra_deps: "libunwind-dev libceres-dev"
+          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:
+    - 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 }}
+
+    - name: Configure CMake
+      run: |
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{ matrix.extra_cmake_def }}
+
+    - name: Build
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+    - name: Info
+      working-directory: ${{github.workspace}}/build/bin
+      run: |
+        ./rtabmap-console --version
+
+#    - name: 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}}
+      
@@ -12,13 +12,17 @@ env:
  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
  BUILD_TYPE: Release

+concurrency:
+  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
+  cancel-in-progress: ${{ github.event_name == 'pull_request' }}
+
 jobs:
  build:
    # The CMake configure and build commands are platform agnostic and should work equally
    # well on Windows or Mac.  You can convert this to a matrix build if you need
    # cross-platform coverage.
    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
-    name: Build on ros ${{ matrix.ros_distribution }} and ${{ matrix.os }}
+    name: ${{ matrix.ros_distribution }}-${{ matrix.os }}
    runs-on: ${{ matrix.os }}
    strategy:
      fail-fast: false
@@ -0,0 +1,107 @@
+name: CMake-Windows
+
+on:
+  push:
+    branches: 
+      - master
+  pull_request:
+    branches: 
+      - '**'
+
+env:
+  BUILD_TYPE: Release
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
+  cancel-in-progress: ${{ github.event_name == 'pull_request' }}
+
+jobs:
+  build:
+    name: ${{ matrix.build_name }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      fail-fast: true
+      matrix:
+        build_name: [windows-2022, windows-2022-cuda]
+        include:
+        - build_name: windows-2022
+          os: windows-2022
+          extra_deps: ""
+          extra_cmake_def: '-DBUILD_AS_BUNDLE=ON -DWITH_PYTHON=ON -DWITH_TORCH=OFF'
+        - build_name: windows-2022-cuda
+          os: windows-2022
+          extra_deps: ""
+          extra_cmake_def: '-DBUILD_AS_BUNDLE=ON -DWITH_PYTHON=ON -DWITH_TORCH=ON'
+
+    steps:
+    - uses: actions/checkout@v4
+
+    - name: Install Windows Dependencies
+      if: matrix.build_name == 'windows-2022'
+      uses: ./.github/actions/install-windows-deps
+ 
+    - name: Install Windows Dependencies with CUDA
+      if: matrix.build_name == 'windows-2022-cuda'
+      uses: ./.github/actions/install-windows-cuda-deps
+
+    - name: Configure CMake
+      run: |
+        cmake `
+          -B ${{github.workspace}}/build `
+          -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} `
+          ${{ matrix.extra_cmake_def }} `
+          -DVCPKG_MANIFEST_INSTALL=OFF `
+          -DVCPKG_TARGET_TRIPLET=x64-windows-release `
+          -DVCPKG_INSTALLED_DIR="${{env.VCPKG_EXPORT_PATH}}/installed" `
+          -DCMAKE_TOOLCHAIN_FILE=${{env.VCPKG_EXPORT_PATH}}/scripts/buildsystems/vcpkg.cmake `
+          -DTorch_DIR=${{env.VCPKG_EXPORT_PATH}}/installed/x64-windows-release/tools/python3/Lib/site-packages/torch/share/cmake/Torch
+
+    - name: Build
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+    - name: Build Windows Package
+      shell: pwsh
+      run: |
+        if ("${{ github.event_name }}" -eq "pull_request") {
+          cpack --config build/CPackConfig.cmake -G ZIP -B build
+        } else {
+          cmake --build ${{ github.workspace }}/build --config ${{ env.BUILD_TYPE }} --target package
+        }
+
+    - name: Rename CUDA artifacts
+      if: matrix.build_name == 'windows-2022-cuda'
+      shell: pwsh
+      run: Get-ChildItem -Path "build" -Filter "RTABMap-*" | Rename-Item -NewName { $_.BaseName + "_cuda" + $_.Extension }
+
+    - name: Info
+      working-directory: ${{github.workspace}}/build/bin
+      run: |
+        ./rtabmap-console --version
+
+    - name: Upload RTABMap Artifacts (ZIP)
+      uses: actions/upload-artifact@v4
+      with:
+        name: RTABMap-Binaries-${{ matrix.build_name }}-zip
+        path: |
+          build/RTABMap-*.zip
+        compression-level: 0
+        if-no-files-found: warn
+        retention-days: ${{ github.event_name == 'pull_request' && 1 || 90 }}
+
+    - name: Upload RTABMap Artifacts (Installer)
+      if: github.event_name != 'pull_request'
+      uses: actions/upload-artifact@v4
+      with:
+        name: RTABMap-Binaries-${{ matrix.build_name }}-exe
+        path: |
+          build/RTABMap-*.exe
+        compression-level: 0
+        if-no-files-found: warn
+        retention-days: ${{ github.event_name == 'pull_request' && 1 || 90 }}
+
+#    - name: Test
+#      working-directory: ${{github.workspace}}/build
+#      # Execute tests defined by the CMake configuration.  
+#      # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail
+#      run: ctest -C ${{env.BUILD_TYPE}}
+      
@@ -1,49 +0,0 @@
-name: CMake
-
-on:
-  push:
-    branches: 
-      - master
-  pull_request:
-    branches: 
-      - '**'
-
-env:
-  BUILD_TYPE: Release
-
-jobs:
-  build:
-    name: ${{ matrix.os }}
-    runs-on: ${{ matrix.os }}
-    strategy:
-      fail-fast: false
-      matrix:
-        os: [ubuntu-24.04, ubuntu-22.04]
-
-    steps:    
-    - name: Install 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
-
-    - uses: actions/checkout@v4
-
-    - name: Configure CMake
-      run: |
-        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
-
-    - name: Build
-      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
-
-    - name: Info
-      working-directory: ${{github.workspace}}/build/bin
-      run: |
-        ./rtabmap-console --version
-
-#    - 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}}
-      
@@ -150,7 +150,7 @@ jobs:
            API_VERSION=23
          docker_platforms: |
            linux/amd64
-          docker_path: 'bionic/android/rtabmap_apiXX'
+          docker_path: 'noble/android/rtabmap_apiXX'
        - docker_tag: android24
          docker_tags: |
            introlab3it/rtabmap:android24
@@ -158,7 +158,7 @@ jobs:
            API_VERSION=24
          docker_platforms: |
            linux/amd64
-          docker_path: 'bionic/android/rtabmap_apiXX'
+          docker_path: 'noble/android/rtabmap_apiXX'
        - docker_tag: android26
          docker_tags: |
            introlab3it/rtabmap:android26
@@ -166,7 +166,7 @@ jobs:
            API_VERSION=26
          docker_platforms: |
            linux/amd64
-          docker_path: 'bionic/android/rtabmap_apiXX'
+          docker_path: 'noble/android/rtabmap_apiXX'
        - docker_tag: android30
          docker_tags: |
            introlab3it/rtabmap:android30
@@ -174,7 +174,7 @@ jobs:
            API_VERSION=30
          docker_platforms: |
            linux/amd64
-          docker_path: 'bionic/android/rtabmap_apiXX'
+          docker_path: 'noble/android/rtabmap_apiXX'
    
    steps:
      -
@@ -1,5 +1,6 @@
 # Top-Level CmakeLists.txt
 cmake_minimum_required(VERSION 3.14)
+
 PROJECT( RTABMap )
 SET(PROJECT_PREFIX rtabmap)

@@ -11,6 +12,7 @@ IF(NOT MULTI_ARCH AND NOT DEFINED CMAKE_INSTALL_LIBDIR)
 ENDIF(NOT MULTI_ARCH AND NOT DEFINED CMAKE_INSTALL_LIBDIR)

 INCLUDE(GNUInstallDirs)
+INCLUDE(FetchContent)

 ####### local cmake modules #######
 SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
@@ -19,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 22)
+SET(RTABMAP_MINOR_VERSION 24)
 SET(RTABMAP_PATCH_VERSION 0)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
@@ -83,14 +85,6 @@ IF(MINGW)
    SET(CMAKE_SHARED_LINKER_FLAGS "-Wl,--enable-auto-import")
 ENDIF(MINGW)

-# GCC 4 required
-IF(UNIX OR MINGW)
-    EXEC_PROGRAM( gcc ARGS "-dumpversion" OUTPUT_VARIABLE GCC_VERSION )
-    IF(GCC_VERSION VERSION_LESS "4.0.0")
-        MESSAGE(FATAL_ERROR "GCC ${GCC_VERSION} found, but version 4.x.x minimum is required")
-    ENDIF(GCC_VERSION VERSION_LESS "4.0.0")
-ENDIF(UNIX OR MINGW)
-
 #The CDT Error Parser cannot handle error messages that span 
 #more than one line, which is the default gcc behavior. 
 #In order to force gcc to generate single line error messages with no line wrapping
@@ -203,10 +197,11 @@ option(WITH_ZED           "Include ZED sdk support"              ON)
 option(WITH_ZEDOC         "Include ZED Open Capture support"     ON)
 option(WITH_REALSENSE     "Include RealSense support"            ON)
 option(WITH_REALSENSE_SLAM "Include RealSenseSlam support"       ON)
-option(WITH_REALSENSE2    "Include RealSense support"            ON)
+option(WITH_REALSENSE2    "Include RealSense2 support"           ON)
 option(WITH_MYNTEYE       "Include mynteye-s support"            ON)
 option(WITH_DEPTHAI       "Include depthai-core support"         OFF)
 option(WITH_XVSDK         "Include XVisio SDK support"           OFF)
+option(WITH_ORBBEC_SDK    "Include Orbbec SDK v2 support"        OFF)
 option(WITH_OCTOMAP       "Include OctoMap support"              ON)
 option(WITH_GRIDMAP       "Include GridMap support"              OFF)
 option(WITH_CPUTSDF       "Include CPUTSDF support"              OFF)
@@ -218,12 +213,14 @@ option(WITH_DVO           "Include DVO support"                  OFF)
 option(WITH_ORB_SLAM      "Include ORB_SLAM2 or ORB_SLAM3  support" OFF)
 option(WITH_OKVIS         "Include OKVIS support"                OFF)
 option(WITH_MSCKF_VIO     "Include MSCKF_VIO support"            OFF)
-option(WITH_VINS          "Include VINS-Fusion support"          OFF)
+option(WITH_VINS_FUSION   "Include VINS-Fusion support"          OFF)
 option(WITH_OPENVINS      "Include OpenVINS support"             OFF)
+option(WITH_CUVSLAM       "Include cuVSLAM support"              OFF)
 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()
@@ -396,7 +393,7 @@ IF(NOT VTK_FOUND)
 ENDIF(NOT VTK_FOUND)

 IF(WITH_TORCH)
-    FIND_PACKAGE(Torch QUIET)
+    FIND_PACKAGE(Torch)
    IF(TORCH_FOUND)
        MESSAGE(STATUS "Found Torch: ${TORCH_INCLUDE_DIRS}")
    ENDIF(TORCH_FOUND)
@@ -418,14 +415,14 @@ IF(WITH_PDAL)
 ENDIF(WITH_PDAL)

 IF(WITH_LIBLAS)
-    FIND_PACKAGE(libLAS QUIET)
+    FIND_PACKAGE(libLAS)
    IF(libLAS_FOUND)
       MESSAGE(STATUS "Found libLAS ${libLAS_VERSION}: ${libLAS_INCLUDE_DIRS}")
    ENDIF(libLAS_FOUND)
 ENDIF(WITH_LIBLAS)

 IF(WITH_CUDASIFT)
-    FIND_PACKAGE(CudaSift 3 QUIET)
+    FIND_PACKAGE(CudaSift 3)
    IF(CudaSift_FOUND)
       MESSAGE(STATUS "Found CudaSift")
    ENDIF(CudaSift_FOUND)
@@ -496,22 +493,30 @@ ENDIF(WITH_DC1394)
 IF(WITH_G2O)
    FIND_PACKAGE(g2o NO_MODULE)
    IF(g2o_FOUND)
-       MESSAGE(STATUS "Found g2o (targets)")
-         SET(G2O_FOUND ${g2o_FOUND})
-         get_target_property(G2O_INCLUDES g2o::core INTERFACE_INCLUDE_DIRECTORIES)
-         MESSAGE(STATUS "g2o include dir: ${G2O_INCLUDES}")
-         FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h 
-         PATHS ${G2O_INCLUDES}
-         NO_DEFAULT_PATH)
-       FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
-       STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
-       IF(${matchres} EQUAL -1)
-           MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
-           SET(G2O_CPP11 2)
-       ELSE()
-           MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
-           SET(G2O_CPP11 1)
-       ENDIF()
+        MESSAGE(STATUS "Found g2o (targets)")
+        SET(G2O_FOUND ${g2o_FOUND})
+        get_target_property(G2O_INCLUDES g2o::core INTERFACE_INCLUDE_DIRECTORIES)
+        MESSAGE(STATUS "g2o include dir: ${G2O_INCLUDES}")
+        FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h
+            PATHS ${G2O_INCLUDES}
+            NO_DEFAULT_PATH)
+        FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
+        STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
+        IF(${matchres} EQUAL -1)
+            MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+            SET(G2O_CPP11 2)
+        ELSE()
+            MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+            SET(G2O_CPP11 1)
+        ENDIF()
+        FIND_FILE(G2O_SBA_UTILS_FILE g2o/types/sba/sba_utils.h
+            PATHS ${G2O_INCLUDES}
+            NO_DEFAULT_PATH)
+        IF(G2O_SBA_UTILS_FILE)
+            SET(G2O_WITH_SBA_UTILS 1)
+        ELSE()
+            SET(G2O_WITH_SBA_UTILS 0)
+        ENDIF()
    ELSE()
        FIND_PACKAGE(G2O QUIET)
        IF(G2O_FOUND)
@@ -523,6 +528,16 @@ ENDIF(WITH_G2O)
 IF(WITH_GTSAM)
    # Force config mode to ignore PCL's findGTSAM.cmake file
    FIND_PACKAGE(GTSAM CONFIG QUIET)
+    IF(GTSAM_FOUND)
+      # For issue https://github.com/introlab/rtabmap/pull/1626
+      FIND_FILE(GTSAM_NOISE_MODEL_FACTOR_N_FILE gtsam/nonlinear/NoiseModelFactorN.h
+         PATHS ${GTSAM_INCLUDE_DIR}
+         NO_DEFAULT_PATH)
+      IF(GTSAM_NOISE_MODEL_FACTOR_N_FILE)
+         MESSAGE(STATUS "GTSAM with NoiseModelFactorN.h")
+         ADD_DEFINITIONS("-DGTSAM_WITH_NOISE_MODEL_FACTOR_N")
+      ENDIF(GTSAM_NOISE_MODEL_FACTOR_N_FILE)
+    ENDIF(GTSAM_FOUND)
 ENDIF(WITH_GTSAM)

 IF(WITH_MRPT)
@@ -541,7 +556,7 @@ IF(WITH_FLYCAPTURE2)
 ENDIF(WITH_FLYCAPTURE2)

 IF(WITH_CVSBA)
-    FIND_PACKAGE(cvsba QUIET)
+    FIND_PACKAGE(cvsba)
    IF(cvsba_FOUND)
       MESSAGE(STATUS "Found cvsba: ${cvsba_INCLUDE_DIRS}")
    ENDIF(cvsba_FOUND)
@@ -567,10 +582,14 @@ IF(WITH_POINTMATCHER)
 ENDIF(WITH_POINTMATCHER)

 IF(libpointmatcher_FOUND OR GTSAM_FOUND)
-    find_package(Boost COMPONENTS thread filesystem system program_options date_time REQUIRED)
-    IF(Boost_MINOR_VERSION GREATER 47)
+    find_package(Boost COMPONENTS thread filesystem program_options date_time REQUIRED)
+    IF(Boost_MINOR_VERSION GREATER 80)
+        find_package(Boost COMPONENTS thread filesystem program_options date_time chrono timer serialization REQUIRED)
+    ELSEIF(Boost_MINOR_VERSION GREATER 47)
        find_package(Boost COMPONENTS thread filesystem system program_options date_time chrono timer serialization REQUIRED)
-    ENDIF(Boost_MINOR_VERSION GREATER 47)
+    ELSE()
+        find_package(Boost COMPONENTS thread filesystem system program_options date_time REQUIRED)
+    ENDIF()
    IF(WIN32)
        MESSAGE(STATUS "Boost_LIBRARY_DIRS=${Boost_LIBRARY_DIRS}")
        link_directories(${Boost_LIBRARY_DIRS})
@@ -578,7 +597,7 @@ IF(libpointmatcher_FOUND OR GTSAM_FOUND)
 ENDIF(libpointmatcher_FOUND OR GTSAM_FOUND)

 IF(WITH_CCCORELIB)
-    find_package(CCCoreLib QUIET)
+    find_package(CCCoreLib)
    IF(CCCoreLib_FOUND)
       MESSAGE(STATUS "Found CCCoreLib: ${CCCoreLib_INCLUDE_DIRS}")
    ENDIF(CCCoreLib_FOUND)
@@ -590,7 +609,7 @@ IF(WITH_OPEN3D)
    ELSE()
       # Build Open3D like this to avoid linker errors in rtabmap:
       # cmake -DBUILD_SHARED_LIBS=ON -DGLIBCXX_USE_CXX11_ABI=ON -DCMAKE_BUILD_TYPE=Release ..
-       find_package(Open3D QUIET)
+       find_package(Open3D)
       IF(Open3D_FOUND)
          MESSAGE(STATUS "Found Open3D: ${Open3DINCLUDE_DIRS}")
       ENDIF(Open3D_FOUND)
@@ -598,17 +617,17 @@ IF(WITH_OPEN3D)
 ENDIF(WITH_OPEN3D)

 IF(WITH_LOAM)
-    find_package(loam_velodyne QUIET)
+    find_package(loam_velodyne)
    IF(loam_velodyne_FOUND)
       MESSAGE(STATUS "Found loam_velodyne: ${loam_velodyne_INCLUDE_DIRS}")
    ENDIF(loam_velodyne_FOUND)
 ENDIF(WITH_LOAM)

 IF(WITH_FLOAM)
-    find_package(floam QUIET)
+    find_package(floam)
    IF(floam_FOUND)
       MESSAGE(STATUS "Found floam: ${floam_INCLUDE_DIRS}")
-       FIND_PACKAGE(Ceres QUIET REQUIRED)
+       FIND_PACKAGE(Ceres REQUIRED)
    ENDIF(floam_FOUND)
 ENDIF(WITH_FLOAM)

@@ -623,7 +642,8 @@ IF(WITH_ZED)
       IF(CUDA_FOUND)
        MESSAGE(STATUS "Found CUDA: ${CUDA_INCLUDE_DIRS}")
       ELSE()
-        MESSAGE(FATAL_ERROR "CUDA is required to build with Zed sdk! Set -DWITH_ZED=OFF if you don't have CUDA.")
+        MESSAGE(WARNING "CUDA is required to build with Zed sdk! Set -DWITH_ZED=OFF if you don't have CUDA.")
+		SET(ZED_FOUND FALSE)
       ENDIF()
    ENDIF(ZED_FOUND)
 ENDIF(WITH_ZED)
@@ -675,19 +695,26 @@ IF(WITH_MYNTEYE)
 ENDIF(WITH_MYNTEYE)

 IF(WITH_DEPTHAI)
-    FIND_PACKAGE(depthai 2.24 QUIET)
+    FIND_PACKAGE(depthai 2.24)
    IF(depthai_FOUND)
       MESSAGE(STATUS "Found depthai-core (targets)")
    ENDIF(depthai_FOUND)
 ENDIF(WITH_DEPTHAI)

 IF(WITH_XVSDK)
-    FIND_PACKAGE(xvsdk QUIET)
+    FIND_PACKAGE(xvsdk)
    IF(xvsdk_FOUND)
       MESSAGE(STATUS "Found xvsdk (targets)")
    ENDIF(xvsdk_FOUND)
 ENDIF(WITH_XVSDK)

+IF(WITH_ORBBEC_SDK)
+    FIND_PACKAGE(OrbbecSDK 2)
+    IF(OrbbecSDK_FOUND)
+       MESSAGE(STATUS "Found OrbbecSDK v2 (targets)")
+    ENDIF(OrbbecSDK_FOUND)
+ENDIF(WITH_ORBBEC_SDK)
+
 IF(WITH_OCTOMAP)
    FIND_PACKAGE(octomap QUIET)
    IF(octomap_FOUND)
@@ -699,35 +726,35 @@ IF(WITH_OCTOMAP)
 ENDIF(WITH_OCTOMAP)

 IF(WITH_GRIDMAP)
-    FIND_PACKAGE(grid_map_core QUIET)
+    FIND_PACKAGE(grid_map_core)
    IF(grid_map_core_FOUND)
       MESSAGE(STATUS "Found grid_map_core ${grid_map_core_VERSION}: ${grid_map_core_INCLUDE_DIRS}")
    ENDIF(grid_map_core_FOUND)
 ENDIF(WITH_GRIDMAP)

 IF(WITH_CPUTSDF)
-    FIND_PACKAGE(CPUTSDF QUIET)
+    FIND_PACKAGE(CPUTSDF)
    IF(CPUTSDF_FOUND)
       MESSAGE(STATUS "Found CPUTSDF: ${CPUTSDF_INCLUDE_DIRS}")
    ENDIF(CPUTSDF_FOUND)
 ENDIF(WITH_CPUTSDF)

 IF(WITH_OPENCHISEL)
-    find_package(open_chisel QUIET)
+    find_package(open_chisel)
    if(open_chisel_FOUND)
        MESSAGE(STATUS "Found open_chisel: ${open_chisel_INCLUDE_DIRS}")
    endif(open_chisel_FOUND)
 ENDIF(WITH_OPENCHISEL)

 IF(WITH_ALICE_VISION)
-    find_package(AliceVision CONFIG QUIET)
+    find_package(AliceVision CONFIG)
    IF(AliceVision_FOUND)
        IF(${AliceVision_VERSION} VERSION_LESS_EQUAL "2.2")
           find_package(Boost COMPONENTS log log_setup container REQUIRED)
        ENDIF(${AliceVision_VERSION} VERSION_LESS_EQUAL "2.2")
        SET(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/local/lib/cmake/modules")
-        find_package(Geogram REQUIRED QUIET)
-        find_package(assimp QUIET)
+        find_package(Geogram REQUIRED)
+        find_package(assimp)
        add_definitions("-DRTABMAP_ALICE_VISION_MAJOR=${AliceVision_VERSION_MAJOR}")
        add_definitions("-DRTABMAP_ALICE_VISION_MINOR=${AliceVision_VERSION_MINOR}")
        add_definitions("-DRTABMAP_ALICE_VISION_PATCH=${AliceVision_VERSION_PATCH}")
@@ -735,34 +762,32 @@ IF(WITH_ALICE_VISION)
 ENDIF(WITH_ALICE_VISION)

 IF(WITH_FOVIS)
-    FIND_PACKAGE(libfovis QUIET)
+    FIND_PACKAGE(libfovis)
    IF(libfovis_FOUND)
       MESSAGE(STATUS "Found libfovis: ${libfovis_INCLUDE_DIRS}")
    ENDIF(libfovis_FOUND)
 ENDIF(WITH_FOVIS)

 IF(WITH_VISO2)
-    FIND_PACKAGE(libviso2 QUIET)
+    FIND_PACKAGE(libviso2)
    IF(libviso2_FOUND)
       MESSAGE(STATUS "Found libviso2: ${libviso2_INCLUDE_DIRS}")
    ENDIF(libviso2_FOUND)
 ENDIF(WITH_VISO2)

 IF(WITH_DVO)
-    FIND_PACKAGE(dvo_core QUIET)
+    FIND_PACKAGE(dvo_core)
    IF(dvo_core_FOUND)
       MESSAGE(STATUS "Found dvo_core: ${dvo_core_INCLUDE_DIRS}")
    ENDIF(dvo_core_FOUND)
 ENDIF(WITH_DVO)

 IF(WITH_OKVIS)
-    FIND_PACKAGE(okvis 1.1 QUIET)
+    FIND_PACKAGE(okvis 1.1)
    IF(okvis_FOUND)
       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)
@@ -771,7 +796,7 @@ ENDIF(WITH_OKVIS)
 # If built with okvis, we found already ceres above
 IF(WITH_CERES)
  IF(NOT okvis_FOUND AND NOT floam_FOUND)
-    FIND_PACKAGE(Ceres QUIET)
+    FIND_PACKAGE(Ceres)
    MESSAGE(STATUS "Found ceres ${Ceres_VERSION}: ${CERES_INCLUDE_DIRS}")
  ENDIF(NOT okvis_FOUND AND NOT floam_FOUND)
 ELSEIF(Ceres_FOUND)
@@ -779,28 +804,33 @@ ELSEIF(Ceres_FOUND)
 ENDIF()

 IF(WITH_MSCKF_VIO)
-    FIND_PACKAGE(msckf_vio QUIET)
+    FIND_PACKAGE(msckf_vio)
    IF(msckf_vio_FOUND)
       MESSAGE(STATUS "Found msckf_vio: ${msckf_vio_INCLUDE_DIRS}")
    ENDIF(msckf_vio_FOUND)
 ENDIF(WITH_MSCKF_VIO)

-IF(WITH_VINS)
-    FIND_PACKAGE(vins QUIET)
+IF(WITH_VINS AND NOT WITH_VINS_FUSION)
+message(DEPRECATION "The option WITH_VINS is deprecated and will be removed in a future version. Please use WITH_VINS_FUSION instead.")
+set(WITH_VINS_FUSION ON)
+ENDIF(WITH_VINS AND NOT WITH_VINS_FUSION)
+
+IF(WITH_VINS_FUSION)
+    FIND_PACKAGE(vins)
    IF(vins_FOUND)
-       MESSAGE(STATUS "Found vins: ${vins_INCLUDE_DIRS}")
+       MESSAGE(STATUS "Found vins-fusion: ${vins_INCLUDE_DIRS}")
       IF(okvis_FOUND)
-           MESSAGE(WARNING "VINS and OKVIS will be both linked to project, make sure VINS has been built with against same Ceres version than OKVIS to avoid some crashes.")
+           MESSAGE(WARNING "VINS-Fusion and OKVIS will be both linked to project, make sure VINS-Fusion has been built with against same Ceres version than OKVIS to avoid some crashes.")
       ENDIF(okvis_FOUND)
    ENDIF(vins_FOUND)
-ENDIF(WITH_VINS)
+ENDIF(WITH_VINS_FUSION)

 IF(WITH_OPENVINS)
-    FIND_PACKAGE(ov_msckf QUIET)
+    FIND_PACKAGE(ov_msckf)
    # On ROS2, the indirect includes and libraries
    # are not forwarded by ov_msckf target, append them manually
-    FIND_PACKAGE(ov_core QUIET)
-    FIND_PACKAGE(ov_init QUIET)
+    FIND_PACKAGE(ov_core)
+    FIND_PACKAGE(ov_init)
    IF(ov_msckf_FOUND AND ov_core_FOUND AND ov_init_FOUND)
      SET(ov_msckf_INCLUDE_DIRS
         ${ov_msckf_INCLUDE_DIRS}
@@ -821,20 +851,73 @@ 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 QUIET)
+    FIND_PACKAGE(ORB_SLAM)
    IF(ORB_SLAM_FOUND)
       MESSAGE(STATUS "Found ORB_SLAM${ORB_SLAM_VERSION}: ${ORB_SLAM_INCLUDE_DIRS}")
    ENDIF(ORB_SLAM_FOUND)
 ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND)

+IF(WITH_CUVSLAM)
+    FIND_PACKAGE(CuVSLAM 14.0.0)
+    IF(CUVSLAM_FOUND)
+        MESSAGE(STATUS "Found cuVSLAM: ${CUVSLAM_INCLUDE_DIRS}")
+    ENDIF()
+ENDIF(WITH_CUVSLAM)
+
 SET(DISABLE_NEW_DTAGS_FLAG "--disable-new-dtags")
 IF(NOT (APPLE OR WIN32) AND BUILD_WITH_RPATH_NOT_RUNPATH)
    ADD_LINK_OPTIONS(LINKER:${DISABLE_NEW_DTAGS_FLAG})
@@ -929,10 +1012,14 @@ ENDIF()
 IF(NOT G2O_FOUND)
   SET(G2O "//")
   SET(G2O_CPP_CONF "//")
+   SET(G2O_WITH_SBA_UTILS "//")
 ELSE()
   IF(NOT G2O_CPP11)
     SET(G2O_CPP_CONF "//")
   ENDIF(NOT G2O_CPP11)
+   IF(NOT G2O_WITH_SBA_UTILS)
+     SET(G2O_WITH_SBA_UTILS_CONF "//")
+   ENDIF(NOT G2O_WITH_SBA_UTILS)
 ENDIF()
 IF(NOT GTSAM_FOUND)
   SET(GTSAM "//")
@@ -1058,6 +1145,9 @@ IF(NOT xvsdk_FOUND)
 ELSE()
   SET(CONF_WITH_XVSDK 1)
 ENDIF()
+IF(NOT OrbbecSDK_FOUND)
+   SET(ORBBEC_SDK "//")
+ENDIF(NOT OrbbecSDK_FOUND)
 IF(NOT octomap_FOUND)
   SET(OCTOMAP "//")
   SET(CONF_WITH_OCTOMAP 0)
@@ -1092,11 +1182,14 @@ IF(NOT msckf_vio_FOUND)
   SET(MSCKF_VIO "//")
 ENDIF()
 IF(NOT vins_FOUND)
-   SET(VINS "//")
+   SET(VINSFUSION "//")
 ENDIF()
 IF(NOT ov_msckf_FOUND)
   SET(OPENVINS "//")
 ENDIF()
+IF(NOT CUVSLAM_FOUND)
+   SET(CUVSLAM "//")
+ENDIF()
 IF(NOT ORB_SLAM_FOUND)
   SET(ORB_SLAM "//")
 ENDIF()
@@ -1240,7 +1333,17 @@ install(FILES package.xml DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_PR
 #######################
 IF(BUILD_AS_BUNDLE)
  SET(CMAKE_INSTALL_SYSTEM_RUNTIME_COMPONENT runtime)
+  IF(WIN32)
+    set(CMAKE_INSTALL_SYSTEM_RUNTIME_LIBS_SKIP TRUE)
+	set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS "
+      ExecWait '\\\"$INSTDIR\\\\vc_redist.x64.exe\\\" /quiet /norestart'
+    ")
+  ENDIF()
  INCLUDE(InstallRequiredSystemLibraries)
+  set(CPACK_NSIS_COMPONENT_INSTALL OFF)
+  set(CPACK_ARCHIVE_COMPONENT_INSTALL ON)
+  set(CPACK_COMPONENTS_GROUPING ALL_COMPONENTS_IN_ONE)
+  set(CPACK_COMPONENTS_ALL runtime)
 ENDIF(BUILD_AS_BUNDLE)

 SET(CPACK_PACKAGE_NAME "${PROJECT_NAME}")
@@ -1410,15 +1513,26 @@ MESSAGE(STATUS "  With ORB OcTree           = NO (WITH_ORB_OCTREE=OFF)")
 ENDIF()

 IF(TORCH_FOUND)
-MESSAGE(STATUS "  With SuperPoint          = YES (License: GPLv3) libtorch=${Torch_VERSION}")
+MESSAGE(STATUS "  With SuperPoint           = YES (License: GPLv3) libtorch=${Torch_VERSION}")
 ELSEIF(NOT WITH_TORCH)
-MESSAGE(STATUS "  With SuperPoint          = NO (WITH_TORCH=OFF)")
+MESSAGE(STATUS "  With SuperPoint           = NO (WITH_TORCH=OFF)")
 ELSE()
-MESSAGE(STATUS "  With SuperPoint          = NO (libtorch not found)")
+MESSAGE(STATUS "  With SuperPoint           = NO (libtorch not found)")
 ENDIF()

+IF(TORCH_FOUND AND WITH_PYTHON AND Python3_FOUND)
+MESSAGE(STATUS "  With Superpoint Rpautrat  = YES (Liscense: MIT) libtorch=${Torch_VERSION}")
+ELSEIF(NOT WITH_TORCH)
+MESSAGE(STATUS "  With Superpoint Rpautrat  = NO (WITH_TORCH=OFF)")
+ELSEIF(NOT WITH_PYTHON)
+MESSAGE(STATUS "  With Superpoint Rpautrat  = NO (WITH_PYTHON=OFF)")
+ELSE()
+MESSAGE(STATUS "  Wtih Superpoint Rpautrat  = NO (libtorch and/or python3 not found)")
+ENDIF()
+
+
 IF(WITH_PYTHON AND Python3_FOUND)
-MESSAGE(STATUS "  With Python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}            = YES (License: PSF)")
+MESSAGE(STATUS "  With Python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}           = YES (License: PSF)")
 ELSEIF(NOT WITH_PYTHON)
 MESSAGE(STATUS "  With Python3              = NO (WITH_PYTHON=OFF)")
 ELSE()
@@ -1548,7 +1662,11 @@ MESSAGE(STATUS "  With Open3D               = NO (Open3D not found)")
 ENDIF()

 IF(opengv_FOUND AND WITH_OPENGV)
+IF(opengv_VERSION STREQUAL "internal")
+MESSAGE(STATUS "  With OpenGV (internal)    = YES (License: BSD)")
+ELSE()
 MESSAGE(STATUS "  With OpenGV ${opengv_VERSION}           = YES (License: BSD)")
+ENDIF()
 ELSEIF(NOT WITH_OPENGV)
 MESSAGE(STATUS "  With OpenGV               = NO (WITH_OPENGV=OFF)")
 ELSE()
@@ -1567,7 +1685,7 @@ ENDIF()

 IF(grid_map_core_FOUND)
 MESSAGE(STATUS "  With GridMap ${grid_map_core_VERSION}        = YES (License: BSD)")
-ELSEIF(NOT WITH_OCTOMAP)
+ELSEIF(NOT WITH_GRIDMAP)
 MESSAGE(STATUS "  With GridMap              = NO (WITH_GRIDMAP=OFF)")
 ELSE()
 MESSAGE(STATUS "  With GridMap              = NO (grid_map_core not found)")
@@ -1663,7 +1781,7 @@ ELSE()
 MESSAGE(STATUS "  With FlyCapture2/Triclops = NO (Point Grey SDK not found)")
 ENDIF()

-IF(ZED_FOUND AND CUDA_FOUND)
+IF(ZED_FOUND)
 MESSAGE(STATUS "  With ZED                  = YES")
 ELSEIF(NOT WITH_ZED)
 MESSAGE(STATUS "  With ZED                  = NO (WITH_ZED=OFF)")
@@ -1730,6 +1848,14 @@ ELSE()
 MESSAGE(STATUS "  With XVisio SDK           = NO (xvsdk not found)")
 ENDIF()

+IF(OrbbecSDK_FOUND)
+MESSAGE(STATUS "  With Orbbec SDK ${OrbbecSDK_VERSION}     = YES (License: MIT)")
+ELSEIF(NOT WITH_ORBBEC_SDK)
+MESSAGE(STATUS "  With Orbbec SDK           = NO (WITH_ORBBEC_SDK=OFF)")
+ELSE()
+MESSAGE(STATUS "  With Orbbec SDK           = NO (OrbbecSDK v2 not found)")
+ENDIF()
+
 MESSAGE(STATUS "")
 MESSAGE(STATUS " Odometry Approaches:")
 IF(loam_velodyne_FOUND)
@@ -1791,7 +1917,7 @@ ENDIF()
 IF(vins_FOUND)
 MESSAGE(STATUS "  With VINS-Fusion          = YES (License: GPLv3)")
 ELSEIF(NOT WITH_VINS)
-MESSAGE(STATUS "  With VINS-Fusion          = NO (WITH_VINS=OFF)")
+MESSAGE(STATUS "  With VINS-Fusion          = NO (WITH_VINS_FUSION=OFF)")
 ELSE()
 MESSAGE(STATUS "  With VINS-Fusion          = NO (VINS-Fusion not found)")
 ENDIF()
@@ -1814,6 +1940,14 @@ ELSE()
 MESSAGE(STATUS "  With ORB_SLAM             = NO (ORB_SLAM2 and ORB_SLAM3 not found, make sure environment variable ORB_SLAM_ROOT_DIR is set)")
 ENDIF()

+IF(CUVSLAM_FOUND)
+MESSAGE(STATUS "  With cuVSLAM              = YES (License: NVIDIA ISAAC ROS SOFTWARE LICENSE)")
+ELSEIF(NOT WITH_CUVSLAM)
+MESSAGE(STATUS "  With cuVSLAM              = NO (WITH_CUVSLAM=OFF)")
+ELSE()
+MESSAGE(STATUS "  With cuVSLAM              = NO (cuVSLAM not found, make sure cuVSLAM is installed)")
+ENDIF()
+
 MESSAGE(STATUS "Show all options with: cmake -LA | grep WITH_")
 MESSAGE(STATUS "--------------------------------------------")

@@ -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>
@@ -41,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@TORO@#define RTABMAP_TORO
@G2O@#define RTABMAP_G2O
@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11 @G2O_CPP11@
+@G2O_WITH_SBA_UTILS_CONF@#define RTABMAP_G2O_WITH_SBA_UTILS
@GTSAM@#define RTABMAP_GTSAM
@CERES@#define RTABMAP_CERES
@MRPT@#define RTABMAP_MRPT
@@ -72,6 +73,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@MYNTEYE@#define RTABMAP_MYNTEYE
@DEPTHAI@#define RTABMAP_DEPTHAI
@XVSDK@#define RTABMAP_XVSDK
+@ORBBEC_SDK@#define RTABMAP_ORBBEC_SDK
@OCTOMAP@#define RTABMAP_OCTOMAP
@GRIDMAP@#define RTABMAP_GRIDMAP
@CPUTSDF@#define RTABMAP_CPUTSDF
@@ -82,8 +84,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@DVO@#define RTABMAP_DVO
@OKVIS@#define RTABMAP_OKVIS
@MSCKF_VIO@#define RTABMAP_MSCKF_VIO
-@VINS@#define RTABMAP_VINS
+@VINSFUSION@#define RTABMAP_VINS_FUSION
@OPENVINS@#define RTABMAP_OPENVINS
+@CUVSLAM@#define RTABMAP_CUVSLAM
@ORB_SLAM@#define RTABMAP_ORB_SLAM @ORB_SLAM_VERSION@
@ORB_OCTREE@#define RTABMAP_ORB_OCTREE
@TORCH@#define RTABMAP_TORCH
@@ -1078,7 +1078,7 @@
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.22\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.23\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",
@@ -1139,7 +1139,7 @@
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.22\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.23\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",
@@ -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
@@ -34,7 +34,7 @@ ENDIF()
 IF(APPLE AND BUILD_AS_BUNDLE)
  ADD_EXECUTABLE(rtabmap_app MACOSX_BUNDLE ${SRC_FILES})
 ELSEIF(WIN32 AND BUILD_AS_BUNDLE)
-  ADD_EXECUTABLE(rtabmap_app WIN32 ${SRC_FILES})
+  ADD_EXECUTABLE(rtabmap_app ${SRC_FILES})
 ELSE()
  ADD_EXECUTABLE(rtabmap_app ${SRC_FILES})
 ENDIF()
@@ -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)
@@ -110,24 +112,135 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
 	ENDIF(WIN32)
  ENDIF(k4a_FOUND)
  
+  IF(ZED_FOUND)
+	# Install needed zlibwapi.dll
+    IF(WIN32)
+		file(TO_CMAKE_PATH "$ENV{ZED_SDK_ROOT_DIR}" ENV_ZED_SDK_ROOT_DIR)
+		INSTALL(FILES "${ENV_ZED_SDK_ROOT_DIR}/bin/zlibwapi.dll" 
+          DESTINATION ${thirdparty_dest_dir} 
+          COMPONENT runtime)
+	ENDIF(WIN32)
+  ENDIF(ZED_FOUND)
+
+  IF(OrbbecSDK_FOUND)
+	# Install needed "extensions" folder
+    IF(WIN32)
+        find_path(OrbbecSDK_BIN_DIR NAMES OrbbecSDK.dll)
+        IF(NOT OrbbecSDK_BIN_DIR)
+            MESSAGE(FATAL_ERROR "OrbbecSDK.dll not found! Verify your PATH.")
+        ENDIF(NOT OrbbecSDK_BIN_DIR)
+		MESSAGE(FATAL "OrbbecSDK_BIN_DIR=${OrbbecSDK_BIN_DIR}")
+        INSTALL(DIRECTORY "${OrbbecSDK_BIN_DIR}/extensions" 
+          DESTINATION ${thirdparty_dest_dir} 
+          COMPONENT runtime 
+          FILES_MATCHING
+          PATTERN "*.lib" EXCLUDE
+          PATTERN "*")
+	ENDIF(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
@@ -221,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  ...).
@@ -230,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))
  
@@ -15,7 +15,7 @@ RAMaddOverhead = 0;
 % Inliers_ratio = 'Loop/Visual_inliers/' ./ 'Keypoint/Current_frame/words'
 % Odometry_average = 'Memory/Distance_travelled/m'(2:end) - 'Memory/Distance_travelled/m'(1:end-1)

-statNames = {'Loop/Odom_correction_norm/m', 'Loop/Visual_inliers/', 'Inliers_ratio_%', 'Timing/Total/ms', 'Memory/RAM_usage/MB', 'Memory/RAM_estimated/MB', 'Keypoint/Current_frame/words', 'Loop/Map_id/', 'Memory/Local_graph_size/', 'Keypoint/Dictionary_size/words', 'Loop/Distance_since_last_loc/'}; % 'Odometry_average'
+statNames = {'Loop/Odom_correction_norm/m', 'Loop/Visual_inliers/', 'Inliers_ratio_%', 'Timing/Total/ms', 'Memory/RAM_usage/MB', 'Memory/RAM_estimated/MB', 'Keypoint/Current_frame/words', 'Loop/Map_id/', 'Memory/Local_graph_size/', 'Keypoint/Dictionary_size/words', 'Loop/Distance_since_last_loc/m'}; % 'Odometry_average'

 
 datasets = [ 0 1 6 7 9 14 11 111 ]; % 0 1 6 7 9 12 14 11
@@ -26,7 +26,7 @@ if resultsToShow == 2
  sep = [0, 1000, 3000, 5000, 7000, 9000];
  sepName = {'17:27', '17:54', '18:27', '18:56', '19:35'};
  prefix = 'Consecutive';
-  statNames = {'Loop/Distance_since_last_loc/', 'Distance_since_last_loc_under_50cm'};
+  statNames = {'Loop/Distance_since_last_loc/m', 'Distance_since_last_loc_under_50cm'};
 endif

 MapsN = length(sepName);
@@ -52,7 +52,7 @@ if strcmp(statName,'Inliers_ratio_%')
 elseif strcmp(statName, 'Odometry_average')
  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Memory-Distance_travelled-m' '.txt'], '\t', 1, 0, "emptyvalue", 0);
 elseif strcmp(statName, 'Distance_since_last_loc_under_50cm')
-  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Loop-Distance_since_last_loc-' '.txt'], '\t', 1, 0, "emptyvalue", 0);
+  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Loop-Distance_since_last_loc-m' '.txt'], '\t', 1, 0, "emptyvalue", 0);
 else
  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' statName '.txt'], '\t', 1, 0, "emptyvalue", 0);
 endif
@@ -13,7 +13,7 @@ source rtabmap_latest.bash

 for d in "${DETECTOR[@]}"
 do
-  rtabmap-report --export --export_prefix "Stat$d" --loc 32  Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms Timing/Proximity_by_space_visual/ms Timing/Likelihood_computation/ms Timing/Posterior_computation/ms TimingMem/Keypoints_detection/ms TimingMem/Descriptors_extraction/ms TimingMem/Add_new_words/ms Loop/Map_id/ Keypoint/Current_frame/words Memory/RAM_usage/MB Memory/RAM_estimated/MB Memory/Distance_travelled/m Loop/Distance_since_last_loc/ Memory/Local_graph_size/ Keypoint/Dictionary_size/words "$DATA/$d/loc"
-  rtabmap-report --export --export_prefix "Consecutive$d" --loc 32 Loop/Map_id/ Loop/Distance_since_last_loc/ "$DATA/$d/consecutive_loc"
+  rtabmap-report --export --export_prefix "Stat$d" --loc 32  Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms Timing/Proximity_by_space_visual/ms Timing/Likelihood_computation/ms Timing/Posterior_computation/ms TimingMem/Keypoints_detection/ms TimingMem/Descriptors_extraction/ms TimingMem/Add_new_words/ms Loop/Map_id/ Keypoint/Current_frame/words Memory/RAM_usage/MB Memory/RAM_estimated/MB Memory/Distance_travelled/m Loop/Distance_since_last_loc/m Memory/Local_graph_size/ Keypoint/Dictionary_size/words "$DATA/$d/loc"
+  rtabmap-report --export --export_prefix "Consecutive$d" --loc 32 Loop/Map_id/ Loop/Distance_since_last_loc/m "$DATA/$d/consecutive_loc"
 done

@@ -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
@@ -0,0 +1,105 @@
+# - Find cuVSLAM library (https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam)
+#
+#  CUVSLAM_ROOT_DIR environment variable can be set to find the library.
+#
+# It sets the following variables:
+#  CUVSLAM_FOUND         - Set to false, or undefined, if cuVSLAM isn't found.
+#  CUVSLAM_VERSION       - The version of cuVSLAM found (e.g., "14.0.0").
+#  CUVSLAM_INCLUDE_DIRS  - The cuVSLAM include directory.
+#  CUVSLAM_LIBRARIES     - The cuVSLAM library to link against.
+
+find_package(CUDA REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+find_path(CUVSLAM_INCLUDE_DIRS 
+    NAMES cuvslam.h
+    PATHS
+        /usr/include
+        /usr/local/include
+        /opt/cuvslam/include
+        /opt/ros/humble/share/isaac_ros_nitros/cuvslam/include
+        $ENV{CUVSLAM_ROOT}/include
+        $ENV{CUVSLAM_ROOT_DIR}/include
+)
+
+find_library(CUVSLAM_LIBRARY 
+    NAMES cuvslam
+    PATHS
+        /usr/lib
+        /usr/local/lib
+        /opt/cuvslam/lib
+        /opt/ros/humble/share/isaac_ros_nitros/cuvslam/lib
+        $ENV{CUVSLAM_ROOT}/lib
+        $ENV{CUVSLAM_ROOT_DIR}/lib
+)
+
+if(CUVSLAM_INCLUDE_DIRS AND CUVSLAM_LIBRARY)
+    # Extract version from cuvslam.h header
+    file(STRINGS "${CUVSLAM_INCLUDE_DIRS}/cuvslam.h" CUVSLAM_VERSION_MAJOR_LINE 
+         REGEX "^#define CUVSLAM_API_VERSION_MAJOR")
+    file(STRINGS "${CUVSLAM_INCLUDE_DIRS}/cuvslam.h" CUVSLAM_VERSION_MINOR_LINE 
+         REGEX "^#define CUVSLAM_API_VERSION_MINOR")
+    
+    if(CUVSLAM_VERSION_MAJOR_LINE AND CUVSLAM_VERSION_MINOR_LINE)
+        string(REGEX MATCH "[0-9]+" CUVSLAM_VERSION_MAJOR "${CUVSLAM_VERSION_MAJOR_LINE}")
+        string(REGEX MATCH "[0-9]+" CUVSLAM_VERSION_MINOR "${CUVSLAM_VERSION_MINOR_LINE}")
+        set(CUVSLAM_VERSION "${CUVSLAM_VERSION_MAJOR}.${CUVSLAM_VERSION_MINOR}.0")
+    endif()
+    
+    set(CUVSLAM_LIBRARIES 
+        ${CUVSLAM_LIBRARY} 
+        ${CUDA_LIBRARIES}
+        # Eigen3 is header-only, so we don't need to link to it
+    )
+    set(CUVSLAM_INCLUDE_DIRS 
+        ${CUVSLAM_INCLUDE_DIRS} 
+        ${CUDA_INCLUDE_DIRS} 
+        ${EIGEN3_INCLUDE_DIR}
+    )
+endif()
+
+# Version compatibility check - cuVSLAM only guarantees API compatibility within the same major version
+set(CUVSLAM_VERSION_MISMATCH_REASON "")
+if(CuVSLAM_FIND_VERSION AND CUVSLAM_VERSION)
+    string(REGEX MATCH "^[0-9]+" REQUESTED_MAJOR_VERSION "${CuVSLAM_FIND_VERSION}")
+    if(NOT CUVSLAM_VERSION_MAJOR EQUAL REQUESTED_MAJOR_VERSION)
+        set(CUVSLAM_VERSION_MISMATCH_REASON "Major version mismatch: found ${CUVSLAM_VERSION_MAJOR}.x but requested ${REQUESTED_MAJOR_VERSION}.x.\ncuVSLAM only guarantees API compatibility within the same major version.\nPlease install cuVSLAM ${REQUESTED_MAJOR_VERSION}.x or update CMakeLists.txt to request version ${CUVSLAM_VERSION_MAJOR}.0.0")
+        
+        if(CuVSLAM_FIND_REQUIRED)
+            message(FATAL_ERROR 
+                "cuVSLAM major version mismatch: found version ${CUVSLAM_VERSION} but version ${CuVSLAM_FIND_VERSION} is required.\n"
+                "cuVSLAM only guarantees API compatibility within the same major version.\n"
+                "Found major version ${CUVSLAM_VERSION_MAJOR} is not compatible with requested major version ${REQUESTED_MAJOR_VERSION}.\n"
+                "Please install cuVSLAM ${REQUESTED_MAJOR_VERSION}.x or update CMakeLists.txt to request version ${CUVSLAM_VERSION_MAJOR}.x."
+            )
+        else()
+            # Clear the found variables to indicate incompatibility
+            unset(CUVSLAM_LIBRARIES)
+            unset(CUVSLAM_INCLUDE_DIRS)
+        endif()
+    endif()
+endif()
+
+# Handle the QUIET and REQUIRED arguments
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(CuVSLAM
+    FOUND_VAR CUVSLAM_FOUND
+    REQUIRED_VARS CUVSLAM_LIBRARIES CUVSLAM_INCLUDE_DIRS
+    VERSION_VAR CUVSLAM_VERSION
+    REASON_FAILURE_MESSAGE "${CUVSLAM_VERSION_MISMATCH_REASON}"
+    HANDLE_COMPONENTS
+)
+
+if(CUVSLAM_FOUND)
+    # Create imported target for modern CMake usage
+    if(NOT TARGET cuvslam::cuvslam)
+        add_library(cuvslam::cuvslam UNKNOWN IMPORTED)
+        set_target_properties(cuvslam::cuvslam PROPERTIES
+            IMPORTED_LOCATION "${CUVSLAM_LIBRARY}"
+            INTERFACE_INCLUDE_DIRECTORIES "${CUVSLAM_INCLUDE_DIRS}"
+            INTERFACE_LINK_LIBRARIES "${CUVSLAM_LIBRARIES};Eigen3::Eigen"
+        )
+    endif()
+endif()
+
+mark_as_advanced(CUVSLAM_INCLUDE_DIRS CUVSLAM_LIBRARY)
@@ -27,10 +27,10 @@

 if(NOT Eigen3_FIND_VERSION)
  if(NOT Eigen3_FIND_VERSION_MAJOR)
-    set(Eigen3_FIND_VERSION_MAJOR 2)
+    set(Eigen3_FIND_VERSION_MAJOR 3)
  endif()
  if(NOT Eigen3_FIND_VERSION_MINOR)
-    set(Eigen3_FIND_VERSION_MINOR 91)
+    set(Eigen3_FIND_VERSION_MINOR 0)
  endif()
  if(NOT Eigen3_FIND_VERSION_PATCH)
    set(Eigen3_FIND_VERSION_PATCH 0)
@@ -26,6 +26,10 @@ FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h
  PATHS ${G2O_INCLUDE_DIR}
  NO_DEFAULT_PATH)

+FIND_FILE(G2O_SBA_UTILS_FILE g2o/types/sba/sba_utils.h
+  PATHS ${G2O_INCLUDE_DIR}
+  NO_DEFAULT_PATH)
+
 #ifdef G2O_NUMBER_FORMAT_STR
 #define G2O_CPP11 // we assume that if G2O_NUMBER_FORMAT_STR is defined, this is the new g2o code with c++11 interface
 #endif
@@ -118,22 +122,29 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FIL
 	  ${CHOLMOD_LIB})
  ENDIF(G2O_SOLVER_CHOLMOD)

-  FILE(READ ${G2O_CONFIG_FILE} TMPTXT)
-  STRING(FIND "${TMPTXT}" "G2O_NUMBER_FORMAT_STR" matchres)
+  FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
+  STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
  IF(${matchres} EQUAL -1)
-    MESSAGE(STATUS "Old g2o version detected with c++03 interface (config file: ${G2O_CONFIG_FILE}).")
-    SET(G2O_CPP11 0)
-  ELSE()
-    MESSAGE(WARNING "Latest g2o version detected with c++11 interface (config file: ${G2O_CONFIG_FILE}). Make sure g2o is built with \"-DBUILD_WITH_MARCH_NATIVE=OFF\" to avoid segmentation faults caused by Eigen.")
-    FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
-    STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
+    FILE(READ ${G2O_CONFIG_FILE} TMPTXT)
+    STRING(FIND "${TMPTXT}" "G2O_NUMBER_FORMAT_STR" matchres)
    IF(${matchres} EQUAL -1)
+      MESSAGE(STATUS "Old g2o version detected with c++03 interface (config file: ${G2O_CONFIG_FILE}).")
+      SET(G2O_CPP11 0)
+    ELSE()
+      MESSAGE(WARNING "Latest g2o version detected with c++11 interface (config file: ${G2O_CONFIG_FILE}). Make sure g2o is built with \"-DBUILD_WITH_MARCH_NATIVE=OFF\" to avoid segmentation faults caused by Eigen.")
      MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
      SET(G2O_CPP11 2)
-    ELSE()
-      MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
-      SET(G2O_CPP11 1)
    ENDIF()
+  ELSE()
+    MESSAGE(WARNING "Latest g2o version detected with c++11 interface (config file: ${G2O_CONFIG_FILE}). Make sure g2o is built with \"-DBUILD_WITH_MARCH_NATIVE=OFF\" to avoid segmentation faults caused by Eigen.")
+    MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+    SET(G2O_CPP11 1)
+  ENDIF()
+
+  IF(G2O_SBA_UTILS_FILE)
+    SET(G2O_WITH_SBA_UTILS 1)
+  ELSE()
+    SET(G2O_WITH_SBA_UTILS 0)
  ENDIF()

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

 IF (ORB_SLAM_INCLUDE_DIR AND ORB_SLAM_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
@@ -10,8 +10,6 @@
 	<string>${MACOSX_BUNDLE_INFO_STRING}</string>
 	<key>CFBundleIconFile</key>
 	<string>${MACOSX_BUNDLE_ICON_FILE}</string>
-	<key>CFBundleIdentifier</key>
-	<string>${MACOSX_BUNDLE_GUI_IDENTIFIER}</string>
 	<key>CFBundleInfoDictionaryVersion</key>
 	<string>6.0</string>
 	<key>CFBundleLongVersionString</key>
@@ -32,6 +30,14 @@
 	<true/>
 	<key>NSHumanReadableCopyright</key>
 	<string>${MACOSX_BUNDLE_COPYRIGHT}</string>
+    <key>com.apple.security.app-sandbox</key>
+    <true/>
+    <key>com.apple.security.files.downloads.read-write</key>
+    <true/>
+    <key>com.apple.security.files.downloads.read-only</key>
+    <false/>
+    <key>com.apple.security.device.camera</key>
+    <true/>

 <!-- File type associations -->
 <key>CFBundleDocumentTypes</key>
@@ -38,7 +38,7 @@ class IMUFilter;

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

 	bool initFromFile(const std::string & calibrationPath);
@@ -73,6 +73,7 @@ private:
 private:
 	IMUFilter * imuFilter_;
 	bool publishInterIMU_;
+	bool imuBaseFrameConversion_;
 };


@@ -38,3 +38,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/camera/CameraRGBDImages.h>
 #include <rtabmap/core/camera/CameraK4A.h>
 #include <rtabmap/core/camera/CameraSeerSense.h>
+#include <rtabmap/core/camera/CameraOrbbecSDK.h>
@@ -129,11 +129,12 @@ public:
 			std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
 #endif
 			cv::Mat * textures = 0) const;
+	void saveFlannIndex(const std::vector<unsigned char> & indexData) const;

 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
@@ -161,19 +162,20 @@ public:
 	void executeNoResult(const std::string & sql) const;

 	// Load objects
-	void load(VWDictionary * dictionary, bool lastStateOnly = true) const;
-	void loadLastNodes(std::list<Signature *> & signatures) const; // returned signatures must be freed after usage
+	void load(VWDictionary & dictionary, bool lastStateOnly = true) const;
+	void loadLastNodes(std::list<Signature *> & signatures, bool loadWordIdsOnly = false) const; // returned signatures must be freed after usage
 	Signature * loadSignature(int id, bool * loadedFromTrash = 0); // returned signature must be freed after usage, call loadSignatures() instead if more than one signature should be loaded
-	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0); // returned signatures must be freed after usage
+	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0, bool loadWordIdsOnly = false); // returned signatures must be freed after usage
 	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws); // returned words must be freed after usage

 	// Specific queries...
-	void loadNodeData(Signature * signature, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
+	void loadNodeData(Signature & signature, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	bool getCalibration(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const;
 	bool getLaserScanInfo(int signatureId, LaserScan & info) const;
 	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
+	void getLocalFeatures(int signatureId, std::multimap<int, int> & words, std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & points, cv::Mat & descriptors) const;
 	void loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
 	void getLastNodeIds(std::set<int> & ids) const;
@@ -191,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
@@ -274,11 +276,12 @@ protected:
 				std::vector<std::vector<Eigen::Vector2f> > * texCoords,
 #endif
 				cv::Mat * textures) const = 0;
+	virtual void saveFlannIndexQuery(const std::vector<unsigned char> & indexData) const = 0;

 	// Load objects
-	virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const = 0;
-	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
-	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
+	virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true) const = 0;
+	virtual void loadLastNodesQuery(std::list<Signature *> & signatures, bool loadWordIdsOnly) const = 0;
+	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures, bool loadWordIdsOnly) const = 0;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
 	virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const = 0;

@@ -286,6 +289,7 @@ protected:
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const = 0;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const = 0;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const = 0;
+	virtual void getLocalFeaturesQuery(int signatureId, std::multimap<int, int> & words, std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & points, cv::Mat & descriptors) const = 0;
 	virtual void getLastNodeIdsQuery(std::set<int> & ids) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const = 0;
 	virtual void getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const = 0;
@@ -296,6 +300,18 @@ protected:
 	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const = 0;
 	virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const = 0;

+protected:
+	std::vector<unsigned char> serializeFeatures(
+		const std::vector<cv::KeyPoint> & keypoints, 
+		const std::vector<cv::Point3f> & points3D,
+		const cv::Mat & descriptors) const;
+	bool deserializeFeatures(
+		const unsigned char * compressedData,
+		unsigned int compressedDataSize,
+		std::vector<cv::KeyPoint> & keypoints,
+		std::vector<cv::Point3f> & points3D,
+		cv::Mat & descriptors) const;
+
 private:
 	//non-abstract methods
 	void saveOrUpdate(const std::vector<Signature *> & signatures);
@@ -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
@@ -135,10 +135,12 @@ protected:
 #endif
 			cv::Mat * textures) const;

+	virtual void saveFlannIndexQuery(const std::vector<unsigned char> & indexData) const;
+
 	// Load objects
-	virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const;
-	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
-	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
+	virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true) const;
+	virtual void loadLastNodesQuery(std::list<Signature *> & signatures, bool loadWordIdsOnly) const;
+	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures, bool loadWordIdsOnly) const;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
 	virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;

@@ -146,6 +148,7 @@ protected:
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
+	virtual void getLocalFeaturesQuery(int signatureId, std::multimap<int, int> & words, std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & points, cv::Mat & descriptors) const;
 	virtual void getLastNodeIdsQuery(std::set<int> & ids) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const;
 	virtual void getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const;
@@ -179,6 +182,7 @@ private:
 	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
+	void stepKeypoint(sqlite3_stmt * ppStmt, int nodeId, int wordId, int kptIndex) const;
 	void stepKeypoint(sqlite3_stmt * ppStmt, int nodeID, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
 	void stepGlobalDescriptor(sqlite3_stmt * ppStmt, int nodeId, const GlobalDescriptor & descriptor) const;
 	void stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
@@ -190,6 +194,8 @@ private:
 			const cv::Point3f & viewpoint) const;

 private:
+	void loadWordsQuery(std::list<Signature *> & signatures) const;
+	void loadWordIdsQuery(std::list<Signature *> & signatures) const;
 	void loadLinksQuery(std::list<Signature *> & signatures) const;
 	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;

@@ -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;
@@ -104,6 +104,7 @@ namespace rtabmap {

 class ORBextractor;
 class SPDetector;
+class SPDetectorRpautrat;

 class Stereo;
 #if CV_MAJOR_VERSION < 3
@@ -129,7 +130,8 @@ public:
 		kFeatureSurfFreak=12, //new 0.20.4
 		kFeatureGfttDaisy=13, //new 0.20.6
 		kFeatureSurfDaisy=14,  //new 0.20.6
-		kFeaturePyDetector=15}; //new 0.20.8
+		kFeaturePyDetector=15, //new 0.20.8
+		kFeatureSuperPointRpautrat=16}; // new 0.23.3

 	static std::string typeName(Type type)
 	{
@@ -164,6 +166,8 @@ public:
 			return "GFTT+Daisy";
 		case kFeatureSurfDaisy:
 			return "SURF+Daisy";
+		case kFeatureSuperPointRpautrat:
+			return "SUPERPOINT-RPAUTRAT";
 		default:
 			return "Unknown";
 		}
@@ -305,7 +309,8 @@ private:
 	bool preciseUpscale_;
 	bool rootSIFT_;
 	bool gpu_;
-	float guaussianThreshold_;
+	float gaussianThreshold_;
+	float maxGaussianThreshold_;
 	bool upscale_;

 	cv::Ptr<CV_SIFT> sift_;
@@ -626,6 +631,31 @@ private:
 	bool cuda_;
 };

+//SuperPointRpautrat
+class RTABMAP_CORE_EXPORT SuperPointRpautrat : public Feature2D
+{
+public:
+	SuperPointRpautrat(const ParametersMap & parameters = ParametersMap());
+	virtual ~SuperPointRpautrat();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const { return kFeatureSuperPointRpautrat; }
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+	cv::Ptr<SPDetectorRpautrat> superPoint_;
+
+	std::string superpointWeightsPath_;
+	std::string superpointModelPath_;
+	std::string outputDir_;
+	float threshold_;
+	bool nms_;
+	int minDistance_;
+	bool cuda_;
+};
+
 //GFTT_DAISY
 class RTABMAP_CORE_EXPORT GFTT_DAISY : public GFTT
 {
@@ -37,37 +37,48 @@ namespace rtabmap {
 class RTABMAP_CORE_EXPORT FlannIndex
 {
 public:
+	// A forward of the internal enum, indexes should match. See src/rtflann/defines.h
+	enum flann_algorithm_t
+	{
+		FLANN_INDEX_LINEAR 			= 0,
+		FLANN_INDEX_KDTREE 			= 1,
+		FLANN_INDEX_KDTREE_SINGLE   = 4,
+		FLANN_INDEX_LSH 			= 6,
+	};
+
 	FlannIndex();
 	virtual ~FlannIndex();

 	void release();
+	std::vector<unsigned char> serializeIndex(bool computeChecksum = true) const;
+
 	size_t indexedFeatures() const;

 	// return Bytes
 	size_t memoryUsed() const;

 	// Note that useDistanceL1 doesn't have any effect if LSH is used
-	void buildLinearIndex(
+	void buildIndex(
+			flann_algorithm_t algorithm,
 			const cv::Mat & features,
 			bool useDistanceL1 = false,
 			float rebalancingFactor = 2.0f);
-	void buildKDTreeIndex(
-			const cv::Mat & features,
-			int trees = 4,
-			bool useDistanceL1 = false,
-			float rebalancingFactor = 2.0f);
-	void buildKDTreeSingleIndex(
-			const cv::Mat & features,
-			int leafMaxSize = 10,
-			bool reorder = true,
-			bool useDistanceL1 = false,
-			float rebalancingFactor = 2.0f);
-	void buildLSHIndex(
-			const cv::Mat & features,
-			unsigned int table_number = 12,
-			unsigned int key_size = 20,
-			unsigned int multi_probe_level = 2,
-			float rebalancingFactor = 2.0f);
+	// Return false if the indexData doesn't correspond to expected features used and parameters.
+	bool loadIndex(
+		const std::vector<unsigned char> & indexData,
+		flann_algorithm_t algorithm,
+		const cv::Mat & features,
+		bool useDistanceL1 = false,
+		float rebalancingFactor = 2.0f,
+		std::string * errorMsg = NULL);
+	bool loadIndex(
+		const unsigned char * indexData,
+		size_t indexDataSize,
+		flann_algorithm_t algorithm,
+		const cv::Mat & features,
+		bool useDistanceL1 = false,
+		float rebalancingFactor = 2.0f,
+		std::string * errorMsg = NULL);

 	bool isBuilt();

@@ -104,9 +115,9 @@ private:
 	unsigned int nextIndex_;
 	int featuresType_;
 	int featuresDim_;
-	bool isLSH_;
 	bool useDistanceL1_; // true=EUCLEDIAN_L2 false=MANHATTAN_L1
 	float rebalancingFactor_;
+	flann_algorithm_t algorithm_;

 	// keep feature in memory until the tree is rebuilt
 	// (in case the word is deleted when removed from the VWDictionary)
@@ -53,6 +53,7 @@ public:
 public:
 	virtual ~GlobalMap();

+	bool fullUpdateNeeded(const std::map<int, Transform> & poses) const;
 	bool update(const std::map<int, Transform> & poses); // return true if map has changed

 	virtual void clear();
@@ -56,7 +56,7 @@ bool RTABMAP_CORE_EXPORT exportPoses(

 bool RTABMAP_CORE_EXPORT importPoses(
 		const std::string & filePath,
-		int format, // 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
+		int format, // 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
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
 		std::map<int, double> * stamps = 0); // optional for format 1 and 9
@@ -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();}
@@ -264,6 +266,7 @@ private:
 	void addSignatureToStm(Signature * signature, const cv::Mat & covariance);
 	void clear();
 	void loadDataFromDb(bool postInitClosingEvents);
+	void saveFlannIndex(bool postInitClosingEvents);
 	void moveToTrash(Signature * s, bool keepLinkedToGraph = true, std::list<int> * deletedWords = 0);

 	void moveSignatureToWMFromSTM(int id, int * reducedTo = 0);
@@ -275,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;}

@@ -299,13 +303,16 @@ private:
 	float _similarityThreshold;
 	bool _binDataKept;
 	bool _rawDescriptorsKept;
+	bool _loadVisualLocalFeaturesOnInit;
 	bool _saveDepth16Format;
 	bool _notLinkedNodesKeptInDb;
 	bool _saveIntermediateNodeData;
 	std::string _rgbCompressionFormat;
 	std::string _depthCompressionFormat;
 	bool _incrementalMemory;
+	bool _localizationReadOnly;
 	bool _localizationDataSaved;
+	bool _flannIndexSaved;
 	bool _reduceGraph;
 	int _maxStMemSize;
 	float _recentWmRatio;
@@ -353,6 +360,7 @@ private:
 	bool _linksChanged; // False by default, become true when links are modified.
 	int _signaturesAdded;
 	bool _allNodesInWM;
+	bool _receivingOdometryFeatures;
 	GPS _gpsOrigin;
 	std::vector<CameraModel> _rectCameraModels;
 	std::vector<StereoCameraModel> _rectStereoCameraModels;
@@ -53,10 +53,11 @@ public:
 		kTypeOkvis = 6,
 		kTypeLOAM = 7,
 		kTypeMSCKF = 8,
-		kTypeVINS = 9,
+		kTypeVINSFusion = 9,
 		kTypeOpenVINS = 10,
 		kTypeFLOAM = 11,
-		kTypeOpen3D = 12
+		kTypeOpen3D = 12,
+		kTypeCuVSLAM = 13
 	};

 public:
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define ODOMETRYTHREAD_H_

 #include <rtabmap/core/rtabmap_core_export.h>
+#include <rtabmap/core/SensorEvent.h>
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UEventsHandler.h>
@@ -55,20 +56,21 @@ private:
 	// MAIN LOOP
 	//============================================================
 	virtual void mainLoop();
-	void addData(const SensorData & data);
-	bool getData(SensorData & data);
+	void addData(const SensorEvent & data);
+	bool getData(SensorEvent & data);

 private:
 	USemaphore _dataAdded;
 	UMutex _dataMutex;
-	std::list<SensorData> _dataBuffer;
+	std::list<SensorEvent> _dataBuffer;
 	std::list<SensorData> _imuBuffer;
 	Odometry * _odometry;
 	unsigned int _dataBufferMaxSize;
 	bool _resetOdometry;
 	Transform _resetPose;
-	double _lastImuStamp;
-	double _imuEstimatedDelay;
+	Transform _previousGuessPose;
+	double _oldestAsyncImuStamp;
+	double _newestAsyncImuStamp;
 };

 } // namespace rtabmap
@@ -204,6 +204,7 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(Mem, ImageKept,                   bool, false,    "Keep raw images in RAM.");
    RTABMAP_PARAM(Mem, BinDataKept,                 bool, true,     "Keep binary data in db.");
    RTABMAP_PARAM(Mem, RawDescriptorsKept,          bool, true,     "Raw descriptors kept in memory.");
+    RTABMAP_PARAM(Mem, LoadVisualLocalFeaturesOnInit, bool, true,   "Load all local visual features (keypoints, descriptors and 3D points) in RAM when loading an existing database. This can add significant time to initialize the memory but the features will be already loaded before computing loop closure transforms. If false, the features are loaded on-demand from the database when a loop closure transformation should be estimated.");
    RTABMAP_PARAM(Mem, MapLabelsAdded,              bool, true,     "Create map labels. The first node of a map will be labeled as \"map#\" where # is the map ID.");
    RTABMAP_PARAM(Mem, SaveDepth16Format,           bool, false,    "Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
    RTABMAP_PARAM(Mem, NotLinkedNodesKept,          bool, true,     "Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
@@ -212,8 +213,9 @@ 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, 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,    "Reduce graph. Merge nodes when loop closures are added (ignoring those with user data set).");
+    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.");
    RTABMAP_PARAM(Mem, TransferSortingByWeightId,   bool, false,    "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");
    RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne,  bool, false,    "On merge, update to new id. When false, no copy.");
@@ -222,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()));
@@ -251,15 +253,17 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(Kp, NndrRatio,                float, 0.8,   "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
 #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
    // OpenCV>2 without xFeatures2D module doesn't have BRIEF
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 8,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 8,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector 16=SuperPoint-Rpautrat");
 #else
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector 16=SuperPoint-Rpautrat");
 #endif
    RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed,      bool, true,   "Use of the td-idf strategy to compute the likelihood.");
    RTABMAP_PARAM(Kp, Parallelized,             bool, true,   "If the dictionary update and signature creation were parallelized.");
    RTABMAP_PARAM_STR(Kp, RoiRatios,       "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
    RTABMAP_PARAM_STR(Kp, DictionaryPath,       "",           "Path of the pre-computed dictionary");
    RTABMAP_PARAM(Kp, NewWordsComparedTogether, bool, true,   "When adding new words to dictionary, they are compared also with each other (to detect same words in the same signature).");
+    RTABMAP_PARAM(Kp, FlannIndexSaved,          bool, false,  uFormat("Save FLANN index during localization session (when %s=false). The FLANN index will be saved to database after the first time localization mode is used, then on next sessions, the index is reloaded from the database instead of being rebuilt again. This can save significant loading time when the visual word dictionary is big (>1M words). Note that if the dictionary is modified (parameters or data), the index will be rebuilt and saved again on the next session. 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.");
    RTABMAP_PARAM(Kp, SubPixEps,                double, 0.02, "See cv::cornerSubPix().");
@@ -290,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.");
@@ -343,6 +348,13 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(SuperPoint, NMSRadius,     int,  4,      uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSuperPointNMS().c_str()));
    RTABMAP_PARAM(SuperPoint, Cuda,          bool, true,   "Use Cuda device for Torch, otherwise CPU device is used by default.");

+    RTABMAP_PARAM_STR(SuperPointRpautrat, WeightsPath, "",           "[Required] SuperPoint weights file (*.pth).");
+    RTABMAP_PARAM_STR(SuperPointRpautrat, ModelPath, "",           "[Required] SuperPoint python model file (superpoint_pytorch.py).");
+    RTABMAP_PARAM(SuperPointRpautrat, Threshold,     float, 0.005, "Detector response threshold to accept keypoint.");
+    RTABMAP_PARAM(SuperPointRpautrat, NMS,           bool,  true,  "If true, non-maximum suppression is applied to detected keypoints.");
+    RTABMAP_PARAM(SuperPointRpautrat, NMSRadius,     int,  4,      uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSuperPointRpautratNMS().c_str()));
+    RTABMAP_PARAM(SuperPointRpautrat, Cuda,          bool, true,   "Use Cuda device for Torch, otherwise CPU device is used by default.");
+
    RTABMAP_PARAM_STR(PyDetector, Path,       "",           "Path to python script file (see available ones in rtabmap/corelib/src/python/*). See the header to see where the script should be copied.");
 	RTABMAP_PARAM(PyDetector, Cuda,           bool, true,   "Use cuda.");

@@ -359,14 +371,14 @@ class RTABMAP_CORE_EXPORT Parameters

    // RGB-D SLAM
    RTABMAP_PARAM(RGBD, Enabled,                  bool, true,  "Activate metric SLAM. If set to false, classic RTAB-Map loop closure detection is done using only images and without any metric information.");
-    RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.1,  "Minimum linear displacement (m) to update the map. Rehearsal is done prior to this, so weights are still updated.");
-    RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated.");
+    RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.1,  uFormat("Minimum linear displacement (m) to update the map. Rehearsal is done prior to this, so weights are still updated. To update the map when not moving, both %s and %s should be set to 0.", Parameters::kRGBDLinearUpdate().c_str(), Parameters::kRGBDAngularUpdate().c_str()));
+    RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  uFormat("Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated. To update the map when not moving, both %s and %s should be set to 0.", Parameters::kRGBDLinearUpdate().c_str(), Parameters::kRGBDAngularUpdate().c_str()));
    RTABMAP_PARAM(RGBD, LinearSpeedUpdate,        float, 0.0,  "Maximum linear speed (m/s) to update the map (0 means not limit).");
    RTABMAP_PARAM(RGBD, AngularSpeedUpdate,       float, 0.0,  "Maximum angular speed (rad/s) to update the map (0 means not limit).");
    RTABMAP_PARAM(RGBD, AggressiveLoopThr,        float, 0.05, uFormat("Loop closure threshold used (overriding %s) when a new mapping session is not yet linked to a map of the highest loop closure hypothesis. In localization mode, this threshold is used when there are no loop closure constraints with any map in the cache (%s). In all cases, the goal is to aggressively loop on a previous map in the database. Only used when %s is enabled. Set 1 to disable.", kRtabmapLoopThr().c_str(), kRGBDMaxOdomCacheSize().c_str(), kRGBDEnabled().c_str()));
    RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
    RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
-    RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 3.0,   uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
+    RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 3.0,   uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. If used with \"%s\", the disabled loop closure links will be removed.", kOptimizerRobust().c_str()));
    RTABMAP_PARAM(RGBD, MaxLoopClosureDistance,   float, 0.0,   "Reject loop closures/localizations if the distance from the map is over this distance (0=disabled).");
    RTABMAP_PARAM(RGBD, ForceOdom3DoF,            bool, true,  uFormat("Force odometry pose to be 3DoF if %s=true.", kRegForce3DoF().c_str()));
    RTABMAP_PARAM(RGBD, StartAtOrigin,            bool, false, uFormat("If true, rtabmap will assume the robot is starting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode (%s=false).", kMemIncrementalMemory().c_str()));
@@ -428,7 +440,7 @@ class RTABMAP_CORE_EXPORT Parameters
 #endif
 #endif
    RTABMAP_PARAM(Optimizer, VarianceIgnored, bool, false,     "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
-    RTABMAP_PARAM(Optimizer, Robust,          bool, false,     uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
+    RTABMAP_PARAM(Optimizer, Robust,          bool, false,     "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies).");
    RTABMAP_PARAM(Optimizer, PriorsIgnored,   bool, true,      "Ignore prior constraints (global pose or GPS) while optimizing. Currently only g2o and gtsam optimization supports this.");
    RTABMAP_PARAM(Optimizer, LandmarksIgnored,   bool, false,  "Ignore landmark constraints while optimizing. Currently only g2o and gtsam optimization supports this.");
 #if defined(RTABMAP_G2O) || defined(RTABMAP_GTSAM)
@@ -453,8 +465,8 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(GTSAM, IncRelinearizeSkip,       int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.");

    // Odometry
-    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D");
-    RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,       "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
+    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D 13=cuVSLAM");
+    RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,       "Automatically reset odometry after X consecutive images where odometry cannot be computed (a value of 0 disables auto-reset). When a reset occurs, odometry resumes from the last successfully computed pose with large covariance to trigger a new map. If external odometry is used, it will also be reset based on the motion estimated relative to the last computed pose but no large covariance will be received, so that a new map won't be triggered.");
    RTABMAP_PARAM(Odom, Holonomic,              bool, true,   "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
    RTABMAP_PARAM(Odom, FillInfoData,           bool, true,   "Fill info with data (inliers/outliers features).");
    RTABMAP_PARAM(Odom, ImageBufferSize,     unsigned int, 1, "Data buffer size (0 min inf).");
@@ -548,7 +560,7 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(OdomViso2, BucketWidth,               double, 50,  "Width of bucket.");
    RTABMAP_PARAM(OdomViso2, BucketHeight,              double, 50,  "Height of bucket.");

-    // Odometry ORB_SLAM2
+    // Odometry ORB_SLAM
    RTABMAP_PARAM_STR(OdomORBSLAM, VocPath,             "",       "Path to ORB vocabulary (*.txt).");
    RTABMAP_PARAM(OdomORBSLAM, Bf,              double, 0.076,    "Fake IR projector baseline (m) used only when stereo is not used.");
    RTABMAP_PARAM(OdomORBSLAM, ThDepth,         double, 40.0,     "Close/Far threshold. Baseline times.");
@@ -603,8 +615,8 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(OdomMSCKF, InitCovExTrans,  double,  0.000025,  "");
    RTABMAP_PARAM(OdomMSCKF, MaxCamStateSize,  int,  20,  "");

-    // Odometry VINS
-    RTABMAP_PARAM_STR(OdomVINS, ConfigPath,     "",  "Path of VINS config file.");
+    // Odometry VINS-Fusion
+    RTABMAP_PARAM_STR(OdomVINSFusion, ConfigPath,     "",  "Path of VINS-Fusion config file.");

    // Odometry OpenVINS
    RTABMAP_PARAM(OdomOpenVINS, UseStereo,                 bool,   true,   "If we have more than 1 camera, if we should try to track stereo constraints between pairs");
@@ -672,6 +684,9 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(OdomOpen3D, MaxDepth,         float, 3.0,  "Maximum depth.");
    RTABMAP_PARAM(OdomOpen3D, Method,           int, 0,  "Registration method: 0=PointToPlane, 1=Intensity, 2=Hybrid.");

+    // Odometry cuVSLAM
+    RTABMAP_PARAM(OdomCuVSLAM, MulticamMode,        int, 0,  "cuVSLAM multicam_mode setting: 0=moderate, 1=performance, 2=precision.");
+
    // Common registration parameters
    RTABMAP_PARAM(Reg, RepeatOnce,               bool, true,    "Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration (like on loop closure). It can be useful if the registration approach used can use a guess to get better matches.");
    RTABMAP_PARAM(Reg, Strategy,                 int, 0,        "0=Vis, 1=Icp, 2=VisIcp");
@@ -701,16 +716,16 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(Vis, Iterations,               int,    300,   "Maximum iterations to compute the transform.");
 #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
    // OpenCV>2 without xFeatures2D module doesn't have BRIEF
-    RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
+    RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector 16=SuperPoint-Rpautrat");
 #else
-    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
+    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector 16=SuperPoint-Rpautrat");
 #endif
    RTABMAP_PARAM(Vis, MaxFeatures,               int,   1000,  "0 no limits.");
    RTABMAP_PARAM(Vis, SSC,                       bool,  false, "If true, SSC (Suppression via Square Covering) is applied to limit keypoints.");
    RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,     "Max depth of the features (0 means no limit).");
    RTABMAP_PARAM(Vis, MinDepth,                  float, 0,     "Min depth of the features (0 means no limit).");
    RTABMAP_PARAM(Vis, DepthAsMask,               bool,  true,  "Use depth image as mask when extracting features.");
-    RTABMAP_PARAM(Vis, DepthMaskFloorThr,         float, 0.0,    uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kVisDepthAsMask().c_str()));
+    RTABMAP_PARAM(Vis, DepthMaskFloorThr,         float, 0.0,    uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled. Ignored if %s is false.", kVisDepthAsMask().c_str()));
    RTABMAP_PARAM_STR(Vis, RoiRatios,        "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
    RTABMAP_PARAM(Vis, SubPixWinSize,             int,   3,     "See cv::cornerSubPix().");
    RTABMAP_PARAM(Vis, SubPixIterations,          int,   0,     "See cv::cornerSubPix(). 0 disables sub pixel refining.");
@@ -726,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.");
@@ -804,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");

@@ -8,6 +8,7 @@
 #ifndef CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_
 #define CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_

+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines

 #include <string>
 #include <rtabmap/utilite/UMutex.h>
@@ -23,7 +24,7 @@ namespace rtabmap {
 * Create a single PythonInterface on main thread at
 * global scope before any Python classes.
 */
-class PythonInterface
+class RTABMAP_CORE_EXPORT PythonInterface
 {
 public:
 	PythonInterface();
@@ -34,7 +35,7 @@ private:
 	pybind11::gil_scoped_release* release_;
 };

-std::string getPythonTraceback();
+std::string RTABMAP_CORE_EXPORT getPythonTraceback();

 }

@@ -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,
@@ -67,6 +67,7 @@ class RTABMAP_CORE_EXPORT Statistics
 	RTABMAP_STATS(Loop, Visual_inliers,);
 	RTABMAP_STATS(Loop, Visual_inliers_ratio,);
 	RTABMAP_STATS(Loop, Visual_matches,);
+	RTABMAP_STATS(Loop, Visual_variance,);
 	RTABMAP_STATS(Loop, Distance_since_last_loc, m);
 	RTABMAP_STATS(Loop, Last_id,);
 	RTABMAP_STATS(Loop, Optimization_max_error, m);
@@ -96,16 +96,23 @@ public:
 #endif

 	float epsilon() const {return epsilon_;}
+	bool usingMinEigenVals() const {return useMinEigenVals_;}
+	float minEigThreshold() const {return minEigThreshold_;}
+	float errorThreshold() const {return errorThreshold_;}
 	virtual bool isGpuEnabled() const;

 private:
 	void updateStatus(
 		const std::vector<cv::Point2f> & leftCorners,
 		const std::vector<cv::Point2f> & rightCorners,
-		std::vector<unsigned char> & status) const;
+		std::vector<unsigned char> & status,
+		std::vector<float> err = {}) const;

 private:
 	float epsilon_;
+	bool useMinEigenVals_;
+	float minEigThreshold_;
+	float errorThreshold_;
 	bool gpu_;
 };

@@ -89,7 +89,7 @@ public:
 	std::vector<int> findNN(const std::list<VisualWord *> & vws) const;
 	std::vector<int> findNN(const cv::Mat & descriptors) const;

-	void addWordRef(int wordId, int signatureId);
+	bool addWordRef(int wordId, int signatureId);
 	void removeAllWordRef(int wordId, int signatureId);
 	const VisualWord * getWord(int id) const;
 	VisualWord * getUnusedWord(int id) const;
@@ -107,7 +107,11 @@ public:
 	bool isIncrementalFlann() const {return _incrementalFlann;}
 	void setIncrementalDictionary();
 	void setFixedDictionary(const std::string & dictionaryPath);
+	bool isModified() const;

+	std::vector<unsigned char> serializeIndex() const;
+	void deserializeIndex(const std::vector<unsigned char> & data);
+	void deserializeIndex(const unsigned char * data, size_t size);
 	void exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const;

 	void clear(bool printWarningsIfNotEmpty = true);
@@ -137,10 +141,12 @@ private:
 	std::string _dictionaryPath; // a pre-computed dictionary (.txt or .db)
 	std::string _newDictionaryPath; // a pre-computed dictionary (.txt or .db)
 	bool _newWordsComparedTogether;
+	bool _serializeWithChecksum;
 	int _lastWordId;
 	bool useDistanceL1_;
 	FlannIndex * _flannIndex;
 	cv::Mat _dataTree;
+	bool _modified;
 	NNStrategy _strategy;
 	std::map<int ,int> _mapIndexId;
 	std::map<int ,int> _mapIdIndex;
@@ -94,18 +94,19 @@ public:
 		_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
 	}

-	// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
+	// 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 setOdometryPath(const std::string & filePath, int format = 0)
 	{
 		_odometryPath = filePath;
 		_odometryFormat = format;
 	}

-	// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
-	void setGroundTruthPath(const std::string & filePath, int format = 0)
+	// 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, 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;
@@ -0,0 +1,105 @@
+/*
+Copyright (c) 2010-2025, 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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+#ifdef RTABMAP_ORBBEC_SDK
+namespace ob
+{
+    class Pipeline;
+    class Align;
+}
+#endif
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT CameraOrbbecSDK :
+    public Camera
+{
+public:
+    static bool available();
+
+public:
+    // deviceId can be either an index (e.g., "0"), an UID (e.g, "2-1-2" or "gmsl-1") or a serial ("AAA6454S")
+    CameraOrbbecSDK(
+        std::string deviceId = "",
+        unsigned int colorWidth = 800,
+        unsigned int colorHeight = 600,
+        unsigned int depthWidth = 800,
+        unsigned int depthHeight = 600,
+        float imageRate = 0.0f,
+        const Transform & localTransform = Transform::getIdentity());
+    virtual ~CameraOrbbecSDK();
+
+    virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+    virtual bool isCalibrated() const;
+    virtual std::string getSerial() const;
+
+    void close();
+
+    // Should be set before initializing
+    void enableColorRectification(bool enabled);
+    void enableImu(bool enabled);
+
+    void enableDepthMM(bool enabled);
+
+protected:
+    virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+
+#ifdef RTABMAP_ORBBEC_SDK
+    std::string deviceId_;
+    unsigned int colorWidth_;
+    unsigned int colorHeight_;
+    unsigned int depthWidth_;
+    unsigned int depthHeight_;
+    ob::Pipeline * pipeline_;
+    ob::Pipeline * imuPipeline_;
+    ob::Align * alignFilter_;
+    CameraModel model_;
+    Transform imuLocalTransform_;
+    bool imuLocalTransformInitialized_;
+    uint64_t lastAccStamp_;
+    uint64_t lastImageStamp_;
+    bool globalTimestampAvailable_;
+    bool rectifyColor_;
+    bool convertDepthToMM_;
+    bool imuPublished_;
+    std::map<double, cv::Vec6f> imuBuffer_;
+    UMutex imuMutex_;
+#endif
+
+};
+
+
+} // namespace rtabmap
@@ -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_;
@@ -0,0 +1,101 @@
+/*
+Copyright (c) 2025 Felix Toft
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef ODOMETRYCUVSLAM_H_
+#define ODOMETRYCUVSLAM_H_
+
+#include <rtabmap/core/Odometry.h>
+#include <memory>
+#include <deque>
+#include <array>
+
+#ifdef RTABMAP_CUVSLAM
+#include <cuvslam.h>
+#include <ground_constraint.h>
+#include <cuda_runtime.h>
+#endif
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT OdometryCuVSLAM : public Odometry
+{
+public:
+	OdometryCuVSLAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryCuVSLAM();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeCuVSLAM;}
+
+private:
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+	virtual void cleanupCuVSLAMResources();
+
+private:
+#ifdef RTABMAP_CUVSLAM
+	CUVSLAM_TrackerHandle cuvslam_handle_;
+	CUVSLAM_GroundConstraintHandle ground_constraint_handle_;
+
+	std::vector<CUVSLAM_Camera> cuvslam_cameras_;
+	std::vector<std::array<float, 12>> intrinsics_;
+	
+	// State tracking
+	bool initialized_;
+	bool lost_;
+	bool tracking_;
+	bool planar_constraints_;
+	int multicam_mode_;
+	Transform previous_pose_;
+	double last_timestamp_;
+
+	// Configuration Thresholds
+	double velocity_ratio_threshold_high_ = 1.5;			// The maximum velocity ratio of guess / estimated velocity needed to detect lost state.
+	double velocity_ratio_threshold_low_ = 0.5;				// The minimum velocity ratio of guess / estimated velocity needed to detect lost state.
+	double velocity_difference_threshold_ = 0.1;			// The maximum velocity difference between the guess and the estimated velocity needed to detect lost state.
+	double zero_estimated_velocity_threshold_ = 0.00001;	// The minimum cuVSLAM estimated velocity needed to detect lost state.
+	double min_landmarks_threshold_ = 30; 					// The minimum number of landmarks needed to start tracking after an initialization.
+	
+	// Forward cuVLSAM covariance directly to RTAB-Map.
+	// When true this disables covariance based lost detection.
+	bool use_raw_covariance_  = false;
+
+	//visualization
+	std::vector<CUVSLAM_Observation> observations_;
+	std::vector<CUVSLAM_Landmark> landmarks_;
+	
+	// GPU memory management
+	std::vector<uint8_t *> gpu_left_image_data_; // pointers to all gpu images
+	std::vector<uint8_t *> gpu_right_image_data_;
+	std::vector<size_t> gpu_left_image_sizes_; // size of one image
+	std::vector<size_t> gpu_right_image_sizes_;
+	cudaStream_t cuda_stream_;
+#endif
+};
+
+}
+
+#endif /* ODOMETRYCUVSLAM_H_ */
@@ -25,39 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

-#ifndef ODOMETRYVINS_H_
-#define ODOMETRYVINS_H_
+#pragma once
+#pragma message("Warning: OdometryVINS.h is deprecated. Please use OdometryVINSFusion.h instead.")

-#include <rtabmap/core/Odometry.h>
-
-namespace rtabmap {
-
-class VinsEstimator;
-
-class RTABMAP_CORE_EXPORT OdometryVINS : public Odometry
-{
-public:
-	OdometryVINS(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
-	virtual ~OdometryVINS();
-
-	virtual void reset(const Transform & initialPose = Transform::getIdentity());
-	virtual Odometry::Type getType() {return Odometry::kTypeVINS;}
-	virtual bool canProcessRawImages() const {return true;}
-	virtual bool canProcessAsyncIMU() const {return true;}
-
-private:
-	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
-
-private:
-#ifdef RTABMAP_VINS
-	VinsEstimator * vinsEstimator_;
-	bool initGravity_;
-	Transform previousPose_;
-	Transform previousLocalTransform_;
-	IMU lastImu_;
-#endif
-};
-
-}
-
-#endif /* ODOMETRYVINS_H_ */
+#include "rtabmap/core/odometry/OdometryVINSFusion.h"
@@ -0,0 +1,64 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef ODOMETRYVINSFUSION_H_
+#define ODOMETRYVINSFUSION_H_
+
+#include <rtabmap/core/Odometry.h>
+
+namespace rtabmap {
+
+class VinsFusionEstimator;
+
+class RTABMAP_CORE_EXPORT OdometryVINSFusion : public Odometry
+{
+public:
+	OdometryVINSFusion(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryVINSFusion();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeVINSFusion;}
+	virtual bool canProcessRawImages() const {return true;}
+	virtual bool canProcessAsyncIMU() const {return true;}
+
+private:
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+private:
+#ifdef RTABMAP_VINS_FUSION
+	VinsFusionEstimator * vinsEstimator_;
+	bool initGravity_;
+	Transform previousPose_;
+	Transform previousLocalTransform_;
+	IMU lastImu_;
+	double lastImuStamp_;
+#endif
+};
+
+}
+
+#endif /* ODOMETRYVINSFUSION_H_ */
@@ -80,6 +80,7 @@ std::map<int, cv::Point3f> RTABMAP_CORE_EXPORT generateWords3DMono(
 		Transform & cameraTransform,
 		float ransacReprojThreshold = 3.0f,
 		float ransacConfidence = 0.99f,
+		int varianceMedianRatio = 4,
 		const std::map<int, cv::Point3f> & refGuess3D = std::map<int, cv::Point3f>(),
 		double * variance = 0,
 		std::vector<int> * matchesOut = 0);
@@ -41,6 +41,7 @@ SET(SRC_FILES
    camera/CameraMyntEye.cpp
    camera/CameraDepthAI.cpp
 	camera/CameraSeerSense.cpp
+	camera/CameraOrbbecSDK.cpp
    
    EpipolarGeometry.cpp
    VisualWord.cpp
@@ -98,9 +99,10 @@ SET(SRC_FILES
    odometry/OdometryLOAM.cpp
    odometry/OdometryFLOAM.cpp
    odometry/OdometryMSCKF.cpp
-    odometry/OdometryVINS.cpp
+    odometry/OdometryVINSFusion.cpp
    odometry/OdometryOpenVINS.cpp
    odometry/OdometryOpen3D.cpp
+	odometry/OdometryCuVSLAM.cpp
    
    IMU.cpp
    IMUThread.cpp
@@ -163,10 +165,6 @@ IF(MSVC)
 ENDIF(MSVC)

 SET(INCLUDE_DIRS
-    ${CMAKE_CURRENT_SOURCE_DIR}
-    ${CMAKE_CURRENT_SOURCE_DIR}/../include
-    ${CMAKE_CURRENT_BINARY_DIR}
-    ${CMAKE_CURRENT_BINARY_DIR}/include
 	${ZLIB_INCLUDE_DIRS}
 )

@@ -217,11 +215,21 @@ IF(TORCH_FOUND)
 		${SRC_FILES}
 		superpoint_torch/SuperPoint.cc
 	)
-	 SET(INCLUDE_DIRS
+	SET(INCLUDE_DIRS
 	    ${TORCH_INCLUDE_DIRS}
 	    ${CMAKE_CURRENT_SOURCE_DIR}/superpoint_torch
 		${INCLUDE_DIRS}
 	)
+	IF(WITH_PYTHON AND Python3_FOUND)
+		SET(SRC_FILES
+			${SRC_FILES}
+			superpoint_rpautrat/SuperpointRpautrat.cpp
+		)
+		SET(INCLUDE_DIRS
+	    	${CMAKE_CURRENT_SOURCE_DIR}/superpoint_rpautrat
+			${INCLUDE_DIRS}
+		)
+	ENDIF(WITH_PYTHON AND Python3_FOUND)
 ENDIF(TORCH_FOUND)

 IF(WITH_PYTHON AND Python3_FOUND)
@@ -394,6 +402,13 @@ IF(xvsdk_FOUND)
 	)
 ENDIF(xvsdk_FOUND)

+IF(OrbbecSDK_FOUND)
+	SET(LIBRARIES
+		${LIBRARIES}
+		ob::OrbbecSDK
+	)
+ENDIF(OrbbecSDK_FOUND)
+
 IF(TARGET OpenMP::OpenMP_CXX)
 	SET(LIBRARIES
 		${LIBRARIES}
@@ -768,38 +783,22 @@ IF(ORB_SLAM_FOUND)
 	)
 ENDIF(ORB_SLAM_FOUND)

+IF(CUVSLAM_FOUND)
+    SET(LIBRARIES
+		${LIBRARIES}
+		cuvslam::cuvslam
+	)
+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)
@@ -816,6 +815,8 @@ CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql.in ${CMA

 SET(RESOURCES
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_23_0.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_22_0.sql
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_20_0.sql
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_3.sql
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_0.sql
@@ -825,6 +826,13 @@ SET(RESOURCES
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_16_0.sql
 )

+IF(TORCH_FOUND AND WITH_PYTHON AND Python3_FOUND)
+	SET(RESOURCES
+		${RESOURCES}
+		${CMAKE_CURRENT_SOURCE_DIR}/superpoint_rpautrat/superpoint_to_torchscript.py
+	)
+ENDIF()
+
 foreach(arg ${RESOURCES})
   get_filename_component(filename ${arg} NAME)
   string(REPLACE "." "_" output ${filename})
@@ -857,8 +865,12 @@ generate_export_header(rtabmap_core
 DEPRECATED_MACRO_NAME RTABMAP_DEPRECATED)

 target_include_directories(rtabmap_core PUBLIC 
-  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../include;${CMAKE_CURRENT_BINARY_DIR}/include;${PUBLIC_INCLUDE_DIRS};${INCLUDE_DIRS}>"
-  "$<INSTALL_INTERFACE:${INSTALL_INCLUDE_DIR};${PUBLIC_INCLUDE_DIRS}>")
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR};${CMAKE_CURRENT_SOURCE_DIR}/../include;${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/include>"
+  "$<INSTALL_INTERFACE:${INSTALL_INCLUDE_DIR}>")
+
+target_include_directories(rtabmap_core SYSTEM PUBLIC 
+  "$<BUILD_INTERFACE:${PUBLIC_INCLUDE_DIRS};${INCLUDE_DIRS}>"
+  "$<INSTALL_INTERFACE:${PUBLIC_INCLUDE_DIRS}>")
  
 TARGET_LINK_LIBRARIES(rtabmap_core 
  PUBLIC 
@@ -39,7 +39,8 @@ namespace rtabmap
 Camera::Camera(float imageRate, const Transform & localTransform) :
 		SensorCapture(imageRate, localTransform*CameraModel::opticalRotation()),
 		imuFilter_(0),
-		publishInterIMU_(false)
+		publishInterIMU_(false),
+		imuBaseFrameConversion_(false)
 {}

 Camera::~Camera()
@@ -52,15 +53,23 @@ bool Camera::initFromFile(const std::string & calibrationPath)
 	return init(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front());
 }

-void Camera::setInterIMUPublishing(bool enabled, IMUFilter * filter)
+void Camera::setInterIMUPublishing(bool enabled, IMUFilter * filter, bool baseFrameConversion)
 {
 	publishInterIMU_ = enabled;
 	delete imuFilter_;
 	imuFilter_ = filter;
+	imuBaseFrameConversion_ = baseFrameConversion;
 }

-void Camera::postInterIMU(const IMU & imu, double stamp)
+void Camera::postInterIMU(const IMU & imu_in, double stamp)
 {
+	IMU imu = imu_in;
+	if(imuBaseFrameConversion_)
+	{
+		UASSERT(!imu.localTransform().isNull());
+		imu.convertToBaseFrame();
+	}
+
 	if(imuFilter_)
 	{
 		imuFilter_->update(
@@ -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
 			{
@@ -554,7 +570,7 @@ unsigned int CameraModel::deserialize(const unsigned char * data, unsigned int d
 			int iR = 8;
 			int iP = 9;
 			int iL = 10;
-			UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]);
+			//UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]);
 			unsigned int requiredDataSize = sizeof(int)*headerSize +
 					sizeof(double)*(header[iK]+header[iD]+header[iR]+header[iP]) +
 					sizeof(float)*header[iL];
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/VisualWord.h"
 #include "rtabmap/core/DBDriverSqlite3.h"
+#include "rtabmap/core/Compression.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UMath.h"
 #include "rtabmap/utilite/ULogger.h"
@@ -45,7 +46,7 @@ DBDriver * DBDriver::create(const ParametersMap & parameters)

 DBDriver::DBDriver(const ParametersMap & parameters) :
 	_emptyTrashesTime(0),
-	_timestampUpdate(true)
+	_timestampUpdate(false)
 {
 	this->parseParameters(parameters);
 }
@@ -73,7 +74,15 @@ void DBDriver::closeConnection(bool save, const std::string & outputUrl)
 	else
 	{
 		_trashesMutex.lock();
+		for(auto & iter: _trashSignatures)
+		{
+			delete iter.second;
+		}
 		_trashSignatures.clear();
+		for(auto & iter: _trashVisualWords)
+		{
+			delete iter.second;
+		}
 		_trashVisualWords.clear();
 		_trashesMutex.unlock();
 	}
@@ -83,12 +92,12 @@ void DBDriver::closeConnection(bool save, const std::string & outputUrl)
 	UDEBUG("");
 }

-bool DBDriver::openConnection(const std::string & url, bool overwritten)
+bool DBDriver::openConnection(const std::string & url, bool overwritten, bool readOnly)
 {
 	UDEBUG("");
 	_url = url;
 	_dbSafeAccessMutex.lock();
-	if(this->connectDatabaseQuery(url, overwritten))
+	if(this->connectDatabaseQuery(url, overwritten, readOnly))
 	{
 		_dbSafeAccessMutex.unlock();
 		return true;
@@ -383,7 +392,7 @@ void DBDriver::asyncSave(Signature * s)
 {
 	if(s)
 	{
-		UDEBUG("s=%d", s->id());
+		//UDEBUG("s=%d", s->id());
 		_trashesMutex.lock();
 		{
 			_trashSignatures.insert(std::pair<int, Signature*>(s->id(), s));
@@ -531,17 +540,17 @@ void DBDriver::updateLaserScan(int nodeId, const LaserScan & scan)
 	_dbSafeAccessMutex.unlock();
 }

-void DBDriver::load(VWDictionary * dictionary, bool lastStateOnly) const
+void DBDriver::load(VWDictionary & dictionary, bool lastStateOnly) const
 {
 	_dbSafeAccessMutex.lock();
 	this->loadQuery(dictionary, lastStateOnly);
 	_dbSafeAccessMutex.unlock();
 }

-void DBDriver::loadLastNodes(std::list<Signature *> & signatures) const
+void DBDriver::loadLastNodes(std::list<Signature *> & signatures, bool loadWordIdsOnly) const
 {
 	_dbSafeAccessMutex.lock();
-	this->loadLastNodesQuery(signatures);
+	this->loadLastNodesQuery(signatures, loadWordIdsOnly);
 	_dbSafeAccessMutex.unlock();
 }

@@ -564,7 +573,8 @@ Signature * DBDriver::loadSignature(int id, bool * loadedFromTrash)
 }
 void DBDriver::loadSignatures(const std::list<int> & signIds,
 		std::list<Signature *> & signatures,
-		std::set<int> * loadedFromTrash)
+		std::set<int> * loadedFromTrash,
+		bool loadWordIdsOnly)
 {
 	UDEBUG("");
 	// look up in the trash before the database
@@ -609,7 +619,7 @@ void DBDriver::loadSignatures(const std::list<int> & signIds,
 	if(ids.size())
 	{
 		_dbSafeAccessMutex.lock();
-		this->loadSignaturesQuery(ids, signatures);
+		this->loadSignaturesQuery(ids, signatures, loadWordIdsOnly);
 		_dbSafeAccessMutex.unlock();
 	}
 }
@@ -656,10 +666,10 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
 	}
 }

-void DBDriver::loadNodeData(Signature * signature, bool images, bool scan, bool userData, bool occupancyGrid) const
+void DBDriver::loadNodeData(Signature & signature, bool images, bool scan, bool userData, bool occupancyGrid) const
 {
 	std::list<Signature *> signatures;
-	signatures.push_back(signature);
+	signatures.push_back(&signature);
 	this->loadNodeData(signatures, images, scan, userData, occupancyGrid);
 }

@@ -694,11 +704,11 @@ void DBDriver::getNodeData(
 	if(uContains(_trashSignatures, signatureId))
 	{
 		const Signature * s = _trashSignatures.at(signatureId);
-		if((!s->isSaved() ||
+		if(!s->isSaved() ||
 			((!images || !s->sensorData().imageCompressed().empty()) &&
 			 (!scan || !s->sensorData().laserScanCompressed().isEmpty()) &&
 			 (!userData || !s->sensorData().userDataCompressed().empty()) &&
-			 (!occupancyGrid || s->sensorData().gridCellSize() != 0.0f))))
+			 (!occupancyGrid || s->sensorData().gridCellSize() != 0.0f)))
 		{
 			data = (SensorData)s->sensorData();
 			if(!images)
@@ -823,6 +833,45 @@ bool DBDriver::getNodeInfo(
 	return found;
 }

+void DBDriver::getLocalFeatures(
+	int signatureId,
+	std::multimap<int, int> & words,
+	std::vector<cv::KeyPoint> & keypoints,
+	std::vector<cv::Point3f> & points,
+	cv::Mat & descriptors) const
+{
+	bool found = false;
+	// look in the trash
+	_trashesMutex.lock();
+	if(uContains(_trashSignatures, signatureId))
+	{
+		const Signature * s = _trashSignatures.at(signatureId);
+		UASSERT(s != 0);
+		found = true;
+		if(!s->getWords().empty())
+		{
+			words = s->getWords();
+			if(s->getWordsKpts().empty()){
+				found = false; // Force checking the database in case the local features were not loaded in RAM
+			}
+			else
+			{
+				words = s->getWords();
+				keypoints = s->getWordsKpts();
+				points = s->getWords3();
+				descriptors = s->getWordsDescriptors().clone();
+			}
+		}
+	}
+	_trashesMutex.unlock();
+
+	if(!found)
+	{
+		UScopeMutex lock(_dbSafeAccessMutex);
+		getLocalFeaturesQuery(signatureId, words, keypoints, points, descriptors);
+	}
+}
+
 void DBDriver::loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type) const
 {
 	bool found = false;
@@ -1287,6 +1336,13 @@ cv::Mat DBDriver::loadOptimizedMesh(
 	return cloud;
 }

+void DBDriver::saveFlannIndex(const std::vector<unsigned char> & indexData) const
+{
+	_dbSafeAccessMutex.lock();
+	saveFlannIndexQuery(indexData);
+	_dbSafeAccessMutex.unlock();
+}
+
 void DBDriver::generateGraph(
 		const std::string & fileName,
 		const std::set<int> & idsInput,
@@ -1458,4 +1514,132 @@ void DBDriver::generateGraph(
 	}
 }

+
+std::vector<unsigned char> DBDriver::serializeFeatures(
+	const std::vector<cv::KeyPoint> & keypoints, 
+	const std::vector<cv::Point3f> & points3D,
+	const cv::Mat & descriptors) const
+{
+	UTimer timer;
+	const int headerSize = 13;
+	int header[headerSize] = {
+			RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH, // 0,1,2
+			CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION,             // 3,4,5 (In case the format/order/size of KeyPoint and/or Point3f changes in the future)
+			sizeof(cv::KeyPoint), (int)keypoints.size(),                         // 6,7
+			sizeof(cv::Point3f), (int)points3D.size(),                           // 8,9
+			descriptors.type(), descriptors.cols, descriptors.rows};             // 10,11,12
+	UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d %d %d",
+		header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10],header[11],header[12]);
+	std::vector<unsigned char> data(
+			sizeof(int)*headerSize +
+			keypoints.size()*sizeof(cv::KeyPoint) + // pos_x, pos_y, size, dir, response, octave
+			points3D.size()*sizeof(cv::Point3f) + // depth_x, depth_y, depth_z
+			descriptors.total()*descriptors.elemSize());
+	UDEBUG("Serialized total size = %ld bytes (header=%ld)", data.size(), sizeof(int)*headerSize);
+	memcpy(data.data(), header, sizeof(int)*headerSize);
+	size_t index = sizeof(int)*headerSize;
+	if(!keypoints.empty())
+	{
+		memcpy(data.data()+index, keypoints.data(), sizeof(cv::KeyPoint)*keypoints.size());
+		index += sizeof(cv::KeyPoint)*(keypoints.size());
+	}
+	if(!points3D.empty())
+	{
+		memcpy(data.data()+index, points3D.data(), sizeof(cv::Point3f)*points3D.size());
+		index += sizeof(cv::Point3f)*(points3D.size());
+	}
+	if(!descriptors.empty())
+	{
+		memcpy(data.data()+index, descriptors.data, descriptors.elemSize()*descriptors.total());
+		index+=descriptors.elemSize()*(descriptors.total());
+	}
+	double serializationTime = timer.ticks();
+	UASSERT_MSG(index == data.size(), uFormat("wrote=%ld expected=%ld", index, data.size()).c_str());
+	std::vector<unsigned char> compressedData = compressData(cv::Mat(1, data.size(), CV_8UC1, (void *)data.data()));
+	UWARN("Serialized %ld bytes in %f ms, Compressed %ld bytes in %f ms",
+		data.size(), serializationTime*1000.0f,
+		compressedData.size(), timer.ticks()*1000.0f);
+	return compressedData;
+}
+
+bool DBDriver::deserializeFeatures(
+	const unsigned char * compressedData,
+	unsigned int compressedDataSize,
+	std::vector<cv::KeyPoint> & keypoints,
+	std::vector<cv::Point3f> & points3D,
+	cv::Mat & descriptors) const
+{
+	UTimer timer;
+	cv::Mat serializedData = uncompressData(compressedData, compressedDataSize);
+	double uncompressionTime = timer.ticks();
+	if(serializedData.empty())
+	{
+		return false;
+	}
+	UDEBUG("Decompressed serialized data = %dx%d type=%d",
+		serializedData.cols, serializedData.rows, serializedData.type());
+	UASSERT(serializedData.type() == CV_8UC1);
+	int headerSize = 13;
+	if(serializedData.total() >= sizeof(int)*headerSize)
+	{
+		const int * header = (const int *)serializedData.data;
+		UASSERT(header[6] == sizeof(cv::KeyPoint));
+		int n_kpts = header[7];
+		UASSERT(header[8] == sizeof(cv::Point3f));
+		int n_pts = header[9];
+		int d_type = header[10];
+		int d_cols = header[11];
+		int d_rows = header[12];
+
+		UDEBUG("Serialized features header: version %d.%d.%d cv=%d.%d.%d kpts=%d (size=%d) pts=%d (size=%d) descriptors=%dx%d type=%d",
+			header[0], header[1], header[2], 
+			header[3], header[4], header[5],
+			header[7], header[6],
+			header[9], header[8],
+			header[11], header[12], header[10]);
+
+		keypoints.resize(n_kpts);
+		points3D.resize(n_pts);
+		descriptors = cv::Mat(d_rows, d_cols, d_type);
+		unsigned int requiredDataSize = sizeof(int)*headerSize +
+				sizeof(cv::KeyPoint)*n_kpts +
+				sizeof(cv::Point3f)*n_pts +
+				descriptors.total() * descriptors.elemSize();
+		UASSERT_MSG(serializedData.total() == requiredDataSize,
+				uFormat("dataSize=%d != required=%d (header: version %d.%d.%d cv=%d.%d.%d kpts=%d (size=%d) pts=%d (size=%d) descriptors=%dx%d type=%d",
+						serializedData.total(),
+						requiredDataSize,
+						header[0], header[1], header[2], 
+						header[3], header[4], header[5],
+						header[7], header[6],
+						header[9], header[8],
+						header[11], header[12], header[10]).c_str());
+		unsigned int index = sizeof(int)*headerSize;
+		if(n_kpts != 0)
+		{
+			memcpy(keypoints.data(), (void*)(serializedData.data+index), n_kpts*sizeof(cv::KeyPoint));
+			index += n_kpts*sizeof(cv::KeyPoint);
+		}
+		if(n_pts != 0)
+		{
+			memcpy(points3D.data(), (void*)(serializedData.data+index), n_pts*sizeof(cv::Point3f));
+			index += n_pts*sizeof(cv::Point3f);
+		}
+		if(d_rows > 0)
+		{
+			cv::Mat(d_rows, d_cols, d_type, (void*)(serializedData.data+index)).copyTo(descriptors);
+			index+=descriptors.elemSize()*(descriptors.total());
+		}
+		UASSERT(index == serializedData.total());
+
+		UWARN("Uncompressed %ld bytes in %f ms, deserialized %ld bytes in %f ms",
+			compressedDataSize, uncompressionTime*1000.0f,
+			serializedData.total(), timer.ticks()*1000.0f);
+
+		return true;
+	}
+	UERROR("Wrong serialized features format detected (size in bytes=%ld)! Cannot deserialize the data.", serializedData.size());
+	return false;
+}
+
 } // namespace rtabmap
@@ -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),
@@ -260,7 +264,7 @@ bool DBReader::init(
 			else
 			{
 				Signature * s = _dbDriver->loadSignature(*_ids.begin());
-				_dbDriver->loadNodeData(s);
+				_dbDriver->loadNodeData(*s);
 				if( s->sensorData().imageCompressed().empty() &&
 					s->getWords().empty() &&
 					!s->sensorData().laserScanCompressed().empty())
@@ -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;
@@ -510,22 +517,41 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 					}
 					else
 					{
-						// if localization data saved in database, covariance will be set in a prior link
-						_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
-						if(links.size())
-						{
-							// assume the first is the backward neighbor, take its variance
-							infMatrix = links.begin()->second.infMatrix();
-							_previousInfMatrix = infMatrix;
-						}
-						else
-						{
-							if(_previousInfMatrix.empty())
+						// In case the graph was reduced, look for forward neighbor link from previous id
+						bool covAdded = false;
+						if(_currentId != _ids.begin()) {
+							std::set<int>::iterator previousId = _currentId;
+							--previousId;
+							std::multimap<int, Link> previousLinks;
+							_dbDriver->loadLinks(*previousId, previousLinks, Link::kNeighbor);
+							if(previousLinks.size() && previousLinks.rbegin()->first == *_currentId)
 							{
-								_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
+								// assume the last is the forward neighbor pointing to current ID, take its covariance
+								infMatrix = previousLinks.rbegin()->second.infMatrix();
+								_previousInfMatrix = infMatrix;
+								covAdded = true;
+							}
+						}
+
+						if(!covAdded) {
+							// if localization data saved in database, covariance will be set in a prior link
+							_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
+							if(links.size())
+							{
+								// assume the first is the backward neighbor, take its variance
+								infMatrix = links.begin()->second.infMatrix();
+								_previousInfMatrix = infMatrix;
+							}
+							else
+							{
+								if(_previousInfMatrix.empty())
+								{
+									_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
+								}
+								// we have a node not linked to map, use last variance
+								UWARN("The node loaded (%d) doesn't have neighbor, re-using the covariance of the previous link for odometry.", s->id());
+								infMatrix = _previousInfMatrix;
 							}
-							// we have a node not linked to map, use last variance
-							infMatrix = _previousInfMatrix;
 						}
 					}
 				}
@@ -837,6 +863,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 				if(info)
 				{
 					info->odomPose = pose;
+					UASSERT(!infMatrix.empty());
 					info->odomCovariance = infMatrix.inv();
 					info->odomVelocity = s->getVelocity();
 					UDEBUG("odom variance = %f/%f", info->odomCovariance.at<double>(0,0), info->odomCovariance.at<double>(5,5));
@@ -47,6 +47,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifdef RTABMAP_TORCH
 #include "superpoint_torch/SuperPoint.h"
 #endif
+#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
+#include "superpoint_rpautrat/SuperpointRpautrat.h"
+#endif

 #ifdef RTABMAP_PYTHON
 #include "python/PyDetector.h"
@@ -300,7 +303,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vecto
 		cv::Mat descriptorsTmp;
 		if(ssc)
 		{
-			ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
+			ULOGGER_DEBUG("too many words (%d), removing words with SSC", keypoints.size());

 			// Sorting keypoints by deacreasing order of strength
 			std::vector<float> responseVector;
@@ -416,7 +419,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
 		inliers.resize(keypoints.size(), false);
 		if(ssc)
 		{
-			ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
+			ULOGGER_DEBUG("too many words (%d), removing words with SSC", keypoints.size());

 			// Sorting keypoints by deacreasing order of strength
 			std::vector<float> responseVector;
@@ -463,7 +466,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
 				minimumHessian = iter->first;
 			}
 		}
-		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, maxKeypoints, minimumHessian);
+		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size()-removed, minimumHessian);
 		ULOGGER_DEBUG("filter keypoints time = %f s", timer.ticks());
 	}
 	else
@@ -730,9 +733,14 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 		feature2D = new ORBOctree(parameters);
 		break;
 #ifdef RTABMAP_TORCH
-	case Feature2D::kFeatureSuperPointTorch:
-		feature2D = new SuperPointTorch(parameters);
-		break;
+case Feature2D::kFeatureSuperPointTorch:
+    feature2D = new SuperPointTorch(parameters);
+    break;
+#endif
+#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
+case Feature2D::kFeatureSuperPointRpautrat:
+    feature2D = new SuperPointRpautrat(parameters);
+    break;
 #endif
 	case Feature2D::kFeatureSurfFreak:
 		feature2D = new SURF_FREAK(parameters);
@@ -831,7 +839,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 			cv::Rect roi(globalRoi.x + j*colSize, globalRoi.y + i*rowSize, colSize, rowSize);
 			std::vector<cv::KeyPoint> subKeypoints;
 			subKeypoints = this->generateKeypointsImpl(image, roi, mask);
-			if (this->getType() != Feature2D::Type::kFeaturePyDetector)
+			if (this->getType() != Feature2D::Type::kFeaturePyDetector && this->getType() != Feature2D::Type::kFeatureSuperPointRpautrat)
 			{
 				limitKeypoints(subKeypoints, maxFeatures, roi.size(), this->getSSC());
 			}
@@ -1243,7 +1251,8 @@ SIFT::SIFT(const ParametersMap & parameters) :
 	preciseUpscale_(Parameters::defaultSIFTPreciseUpscale()),
 	rootSIFT_(Parameters::defaultSIFTRootSIFT()),
 	gpu_(Parameters::defaultSIFTGpu()),
-	guaussianThreshold_(Parameters::defaultSIFTGaussianThreshold()),
+	gaussianThreshold_(Parameters::defaultSIFTGaussianThreshold()),
+	maxGaussianThreshold_(Parameters::defaultSIFTMaxGaussianThreshold()),
 	upscale_(Parameters::defaultSIFTUpscale()),
 	cudaSiftData_(0),
 	cudaSiftMemory_(0),
@@ -1276,23 +1285,25 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kSIFTPreciseUpscale(), preciseUpscale_);
 	Parameters::parse(parameters, Parameters::kSIFTRootSIFT(), rootSIFT_);
 	Parameters::parse(parameters, Parameters::kSIFTGpu(), gpu_);
-	Parameters::parse(parameters, Parameters::kSIFTGaussianThreshold(), guaussianThreshold_);
+	Parameters::parse(parameters, Parameters::kSIFTGaussianThreshold(), gaussianThreshold_);
+	Parameters::parse(parameters, Parameters::kSIFTMaxGaussianThreshold(), maxGaussianThreshold_);
 	Parameters::parse(parameters, Parameters::kSIFTUpscale(), upscale_);
 	
 	if(gpu_)
 	{
 #ifdef RTABMAP_CUDASIFT
 		// Check if there is a cuda device
-		if(InitCuda(0, ULogger::level() == ULogger::kDebug)) {
-			UDEBUG("Init SiftData");
-			if(cudaSiftData_ == 0) {
+		if(cudaSiftData_==0)
+		{
+			if(InitCuda(0, ULogger::level() == ULogger::kDebug)) {
+				UDEBUG("Init SiftData");
 				cudaSiftData_ = new SiftData();
 				InitSiftData(*cudaSiftData_, 8192, true, true);
 			}
-		}
-		else{
-			UWARN("No cuda device(s) detected, CudaSift is not available! Using SIFT CPU version instead.");
-			gpu_ = false;
+			else{
+				UWARN("No cuda device(s) detected, CudaSift is not available! Using SIFT CPU version instead.");
+				gpu_ = false;
+			}
 		}
 #else
 		UWARN("RTAB-Map is not built with CudaSift so %s cannot be used!", Parameters::kSIFTGpu().c_str());
@@ -1355,7 +1366,7 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 			numOctaves = 7; // hard-coded limit in CudaSift
 		}
 		float initBlur = sigma_; /* Amount of initial Gaussian blurring in standard deviations */
-		float thresh = guaussianThreshold_;   /* Threshold on difference of Gaussians for feature pruning */
+		float thresh = gaussianThreshold_;   /* Threshold on difference of Gaussians for feature pruning */
 		float edgeLimit = edgeThreshold_;   
 		float minScale = 0.0f; /* Minimum acceptable scale to remove fine-scale features */
 		UDEBUG("numOctaves=%d initBlur=%f thresh=%f edgeLimit=%f minScale=%f upScale=%s w=%d h=%d", numOctaves, initBlur, thresh, edgeLimit, minScale, upscale_?"true":"false", w, h);
@@ -1380,15 +1391,9 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 		cudaSiftDescriptors_ = cv::Mat();
 		if(cudaSiftData_->numPts)
 		{
-			int maxKeypoints = this->getMaxFeatures();
-			if(maxKeypoints == 0 || maxKeypoints > cudaSiftData_->numPts)
-			{
-				maxKeypoints = cudaSiftData_->numPts;
-			}
-
-			// Re-using same implementation of limitKeypoints() directly here to avoid doubling memory copies
-			// Sort words by hessian
-			std::multimap<float, int> hessianMap; // <hessian,id>
+			keypoints.resize(cudaSiftData_->numPts);
+			cudaSiftDescriptors_ = cv::Mat(cudaSiftData_->numPts, 128, CV_32FC1);
+			size_t k=0;
 			for(int i=0; i<cudaSiftData_->numPts; ++i)
 			{
 				// Ignore keypoints with invalid descriptors
@@ -1405,29 +1410,40 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 					continue;
 				}

-				//Keep track of the data, to be easier to manage the data in the next step
-				hessianMap.insert(std::pair<float, int>(cudaSiftData_->h_data[i].sharpness, i));
-			}
+				if(i>0 && 
+					cudaSiftData_->h_data[i].subsampling == cudaSiftData_->h_data[i-1].subsampling &&
+					fabs(cudaSiftData_->h_data[i].xpos-cudaSiftData_->h_data[i-1].xpos) +
+					fabs(cudaSiftData_->h_data[i].xpos-cudaSiftData_->h_data[i-1].ypos) < 0.1f)
+				{
+					// Same feature, skip doubles
+					continue;
+				}

-			if((int)hessianMap.size() < maxKeypoints)
-			{
-				maxKeypoints = hessianMap.size();
-			}
+				float response = abs(cudaSiftData_->h_data[i].sharpness);
+				if(maxGaussianThreshold_>gaussianThreshold_ && response > maxGaussianThreshold_)
+				{
+					continue;
+				}

-			std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
-			keypoints.resize(maxKeypoints);
-			cudaSiftDescriptors_ = cv::Mat(maxKeypoints, 128, CV_32FC1);
-			for(unsigned int k=0; k<keypoints.size() && iter!=hessianMap.rend(); ++k, ++iter)
-			{
-				int i = iter->second;
-				float *desc = cudaSiftData_->h_data[i].data;
 				cv::Mat(1, 128, CV_32FC1, desc).copyTo(cudaSiftDescriptors_.row(k));
 				keypoints[k].pt.x = cudaSiftData_->h_data[i].xpos;
 				keypoints[k].pt.y = cudaSiftData_->h_data[i].ypos;
 				keypoints[k].size = 2.0f*cudaSiftData_->h_data[i].scale; // x2 because the scale is more like a radius than a diameter, see CudaSift's ExtractSiftDescriptors function to see how they convert scale to patch size
 				keypoints[k].angle = cudaSiftData_->h_data[i].orientation;
-				keypoints[k].response = cudaSiftData_->h_data[i].sharpness; 
+				keypoints[k].response = response; 
 				keypoints[k].octave = log2(cudaSiftData_->h_data[i].subsampling)-(upscale_?1:0);
+				++k;
+			}
+			if(k < keypoints.size())
+			{
+				UDEBUG("keypoints extracted = %d, valid=%d", keypoints.size(), k);
+				keypoints.resize(k);
+				cudaSiftDescriptors_.resize(k);
+			}
+			if(this->getMaxFeatures() != 0 && this->getMaxFeatures() < (int)keypoints.size())
+			{
+				// Call limitKeypoints() now to filter the descriptors.
+				this->limitKeypoints(keypoints, cudaSiftDescriptors_, this->getMaxFeatures(), cv::Size(w,h), this->getSSC());
 			}
 		}
 	}
@@ -1449,12 +1465,13 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con

 cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
+	cv::Mat descriptors;
 #ifdef RTABMAP_CUDASIFT
 	if(gpu_)
 	{
 		if((int)keypoints.size() == cudaSiftDescriptors_.rows)
 		{
-			return cudaSiftDescriptors_.clone();
+			descriptors = cudaSiftDescriptors_.clone();
 		}
 		else
 		{
@@ -1462,19 +1479,25 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 			return cv::Mat();
 		}
 	}
+	else
+	{
 #endif

-	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
-	cv::Mat descriptors;
+		UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 #ifdef RTABMAP_NONFREE
-	sift_->compute(image, keypoints, descriptors);
+		sift_->compute(image, keypoints, descriptors);
 #else
-	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
+		UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
 #else // >=4.4, >=3.4.11
-	sift_->compute(image, keypoints, descriptors);
+		sift_->compute(image, keypoints, descriptors);
 #endif
+
+#ifdef RTABMAP_CUDASIFT
+	}
+#endif
+
 	if( rootSIFT_ && !descriptors.empty())
 	{
 		UDEBUG("Performing RootSIFT...");
@@ -2615,6 +2638,108 @@ cv::Mat SuperPointTorch::generateDescriptorsImpl(const cv::Mat & image, std::vec
 }


+//////////////////////////
+//SuperPointRpautrat
+//////////////////////////
+SuperPointRpautrat::SuperPointRpautrat(const ParametersMap & parameters) :
+		superpointWeightsPath_(Parameters::defaultSuperPointRpautratWeightsPath()),
+		superpointModelPath_(Parameters::defaultSuperPointRpautratModelPath()),
+		outputDir_(""),
+		threshold_(Parameters::defaultSuperPointRpautratThreshold()),
+		nms_(Parameters::defaultSuperPointRpautratNMS()),
+		minDistance_(Parameters::defaultSuperPointRpautratNMSRadius()),
+		cuda_(Parameters::defaultSuperPointRpautratCuda())
+{
+	parseParameters(parameters);
+}
+
+SuperPointRpautrat::~SuperPointRpautrat()
+{
+}
+
+void SuperPointRpautrat::parseParameters(const ParametersMap & parameters)
+{
+	Feature2D::parseParameters(parameters);
+
+#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
+	std::string previousWeightsPath = superpointWeightsPath_;
+	std::string previousModelPath = superpointModelPath_;
+	bool previousCuda = cuda_;
+	float previousThreshold = threshold_;
+	bool previousNms = nms_;
+	int previousMinDistance = minDistance_;
+	
+	Parameters::parse(parameters, Parameters::kSuperPointRpautratWeightsPath(), superpointWeightsPath_);
+	Parameters::parse(parameters, Parameters::kSuperPointRpautratModelPath(), superpointModelPath_);
+	Parameters::parse(parameters, Parameters::kSuperPointRpautratThreshold(), threshold_);
+	Parameters::parse(parameters, Parameters::kSuperPointRpautratNMS(), nms_);
+	Parameters::parse(parameters, Parameters::kSuperPointRpautratNMSRadius(), minDistance_);
+	Parameters::parse(parameters, Parameters::kSuperPointRpautratCuda(), cuda_);
+	Parameters::parse(parameters, Parameters::kRtabmapWorkingDirectory(), outputDir_);
+
+	// If working directory is not set, use the default
+	if(outputDir_.empty())
+	{
+		outputDir_ = Parameters::createDefaultWorkingDirectory();
+	}
+
+	// Reinitialize detector if model-affecting parameters changed
+	if(superPoint_.get() == 0 || 
+	   superpointWeightsPath_.compare(previousWeightsPath) != 0 || 
+	   superpointModelPath_.compare(previousModelPath) != 0 ||
+	   previousCuda != cuda_ ||
+	   previousThreshold != threshold_ ||
+	   previousNms != nms_ ||
+	   previousMinDistance != minDistance_)
+	{
+		superPoint_ = cv::Ptr<SPDetectorRpautrat>(new SPDetectorRpautrat(superpointWeightsPath_, superpointModelPath_, outputDir_, threshold_, nms_, minDistance_, cuda_, this->getMaxFeatures(), this->getSSC()));
+	}
+	else if(superPoint_.get() != 0)
+	{
+		// Update post-processing parameters without reinitializing
+		superPoint_->setMaxFeatures(this->getMaxFeatures());
+		superPoint_->setSSC(this->getSSC());
+	}
+#else
+	UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!");
+#endif
+}
+
+std::vector<cv::KeyPoint> SuperPointRpautrat::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
+{
+#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	if(roi.x!=0 || roi.y !=0)
+	{
+		UERROR("SuperPoint Rpautrat: Not supporting ROI (%d,%d,%d,%d). Make sure %s, %s, %s, %s, %s, %s are all set to default values.",
+				roi.x, roi.y, roi.width, roi.height,
+				Parameters::kKpRoiRatios().c_str(),
+				Parameters::kVisRoiRatios().c_str(),
+				Parameters::kVisGridRows().c_str(),
+				Parameters::kVisGridCols().c_str(),
+				Parameters::kKpGridRows().c_str(),
+				Parameters::kKpGridCols().c_str());
+		return std::vector<cv::KeyPoint>();
+	}
+	return superPoint_->detect(image, mask);
+#else
+	UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!");
+	return std::vector<cv::KeyPoint>();
+#endif
+}
+
+cv::Mat SuperPointRpautrat::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	return superPoint_->compute(keypoints);
+#else
+	UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!");
+	return cv::Mat();
+#endif
+}
+
+
 //////////////////////////
 //GFTT-DAISY
 //////////////////////////
@@ -27,8 +27,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include <rtabmap/core/FlannIndex.h>
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/core/Version.h>
+#ifdef WIN32
+#include <rtabmap/core/Parameters.h>
+#endif

 #include "rtflann/flann.hpp"
+#include <boost/crc.hpp>

 namespace rtabmap {

@@ -37,7 +45,6 @@ FlannIndex::FlannIndex():
 		nextIndex_(0),
 		featuresType_(0),
 		featuresDim_(0),
-		isLSH_(false),
 		useDistanceL1_(false),
 		rebalancingFactor_(2.0f)
 {
@@ -49,9 +56,9 @@ FlannIndex::~FlannIndex()

 void FlannIndex::release()
 {
-	UDEBUG("");
 	if(index_)
 	{
+		UDEBUG("Clearing flann index...");
 		if(featuresType_ == CV_8UC1)
 		{
 			delete (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
@@ -72,12 +79,139 @@ void FlannIndex::release()
 			}
 		}
 		index_ = 0;
+		UDEBUG("Clearing flann index... done!");
 	}
 	nextIndex_ = 0;
-	isLSH_ = false;
 	addedDescriptors_.clear();
 	removedIndexes_.clear();
-	UDEBUG("");
+}
+
+#define FLANN_INDEX_HEADER_SIZE 12
+
+std::vector<unsigned char> FlannIndex::serializeIndex(bool computeChecksum) const {
+	if(index_ && !addedDescriptors_.empty())
+	{
+#ifdef WIN32
+		UERROR("FLANN index serialization is not yet implemented on Windows. Parameter \"%s\" cannot be used.", Parameters::kKpFlannIndexSaved().c_str());
+#else
+		UTimer timer;
+		const int headerSizeBytes = sizeof(int)*FLANN_INDEX_HEADER_SIZE;
+		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) {
+			if(featuresType_ == CV_8UC1)
+			{
+				((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->save(indexDataPtr);
+			}
+			else
+			{
+				if(useDistanceL1_)
+				{
+					((rtflann::Index<rtflann::L1<float> >*)index_)->save(indexDataPtr);;
+				}
+				else if(featuresDim_ <= 3)
+				{
+					((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->save(indexDataPtr);;
+				}
+				else
+				{
+					((rtflann::Index<rtflann::L2<float> >*)index_)->save(indexDataPtr);;
+				}
+			}
+			bytes_written = ftell(indexDataPtr);
+            fclose(indexDataPtr);
+        }
+		if(bytes_written < long(indexData.size()-headerSizeBytes))
+		{
+			//Expected data size and type
+			int dataRows = 0;
+			int dataCols = 0;
+			int dataType = -1;
+			cv::Mat dataset;
+			std::set<int> removedDescriptors;
+			if(computeChecksum){
+				removedDescriptors.insert(removedIndexes_.begin(), removedIndexes_.end());
+			}
+			for(const auto & iter: addedDescriptors_)
+			{
+				UASSERT(!iter.second.empty());
+				dataRows += iter.second.rows;
+				if(dataCols <= 0) {
+					dataCols = iter.second.cols;
+				}
+				else {
+					UASSERT(dataCols == iter.second.cols);
+				}
+				if(dataType < 0) {
+					dataType = iter.second.type();
+				}
+				else {
+					UASSERT(dataType == iter.second.type());
+				}
+				if(computeChecksum){
+					if(removedDescriptors.find(iter.first) == removedDescriptors.end()) {
+						if(dataset.empty()) {
+							dataset = iter.second.clone();
+						}
+						else {
+							dataset.push_back(iter.second);
+						}
+					}
+					else {
+						dataRows -= iter.second.rows;
+					}
+				}
+			}
+			if(!computeChecksum) {
+				for(const auto & index: removedIndexes_)
+				{
+					dataRows -= addedDescriptors_.at(index).rows;
+				}
+			}
+
+			unsigned int crcValue = 0;
+			if(computeChecksum) {
+				boost::crc_32_type result;
+				result.process_bytes(dataset.data, dataset.total()*dataset.elemSize());
+				crcValue = result.checksum(); 
+			}
+			
+			indexData.resize(bytes_written+headerSizeBytes);
+			indexData.shrink_to_fit();
+			int rebalancingFactorAsInt;
+			memcpy(&rebalancingFactorAsInt, &rebalancingFactor_, sizeof(rebalancingFactor_));
+			int crcValueAsInt;
+			memcpy(&crcValueAsInt, &crcValue, sizeof(crcValue));
+			int header[FLANN_INDEX_HEADER_SIZE] = {
+					RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH, // 0,1,2
+					algorithm_,                                                          // 3,
+					featuresDim_,                                                        // 4,
+					useDistanceL1_?1:0,                                                  // 5,
+					rebalancingFactorAsInt,                                              // 6,
+					dataRows,                                                            // 7,
+					dataCols,                                                            // 8,			
+					dataType,                                                            // 9,
+					crcValueAsInt,                                                       // 10         
+					(int)bytes_written};                                                 // 11
+			UDEBUG("Header: \"%d.%d.%d\" alg=%d dim=%d L1=%d factor=%f data(%dx%d type=%d, crc=%X) %d", 
+				header[0],header[1],header[2],
+				header[3],
+				header[4],
+				header[5],
+				rebalancingFactor_,
+				header[7], header[8], header[9], crcValueAsInt, 
+				header[11]);
+			memcpy(indexData.data(), header, headerSizeBytes);
+			return indexData;
+		}
+		else {
+			UERROR("Target buffer too small to serialize index, aborting.");
+		}
+		UDEBUG("Flann serialization: %fs", timer.ticks());
+#endif
+	}
+	return std::vector<unsigned char>();
 }

 size_t FlannIndex::indexedFeatures() const
@@ -139,12 +273,13 @@ size_t FlannIndex::memoryUsed() const
 	return memoryUsage;
 }

-void FlannIndex::buildLinearIndex(
+void FlannIndex::buildIndex(
+		flann_algorithm_t algorithm,
 		const cv::Mat & features,
 		bool useDistanceL1,
 		float rebalancingFactor)
 {
-	UDEBUG("");
+	UDEBUG("algorithm=%d", (int)algorithm);
 	this->release();
 	UASSERT(index_ == 0);
 	UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
@@ -152,8 +287,29 @@ void FlannIndex::buildLinearIndex(
 	featuresDim_ = features.cols;
 	useDistanceL1_ = useDistanceL1;
 	rebalancingFactor_ = rebalancingFactor;
+	algorithm_ = algorithm;

-	rtflann::LinearIndexParams params;
+	rtflann::IndexParams params;
+
+	switch (algorithm)
+	{
+	case FLANN_INDEX_LINEAR:
+		params = rtflann::LinearIndexParams();
+		break;
+	case FLANN_INDEX_KDTREE:
+		params = rtflann::KDTreeIndexParams(4);
+		break;
+	case FLANN_INDEX_KDTREE_SINGLE:
+		params = rtflann::KDTreeSingleIndexParams(10, true);
+		break;
+	case FLANN_INDEX_LSH:
+		UASSERT(features.type() == CV_8UC1);
+		params = rtflann::LshIndexParams(12, 20, 2);
+		break;
+	default:
+		UFATAL("The flann algorithm type %d is not supported!", (int)algorithm);
+		break;
+	}

 	if(featuresType_ == CV_8UC1)
 	{
@@ -199,13 +355,140 @@ void FlannIndex::buildLinearIndex(
 	UDEBUG("");
 }

-void FlannIndex::buildKDTreeIndex(
-		const cv::Mat & features,
-		int trees,
-		bool useDistanceL1,
-		float rebalancingFactor)
+bool FlannIndex::loadIndex(
+	const std::vector<unsigned char> & indexData,
+	flann_algorithm_t algorithm,
+	const cv::Mat & features,
+	bool useDistanceL1,
+	float rebalancingFactor,
+	std::string * error)
 {
-	UDEBUG("");
+	return loadIndex(
+		indexData.data(),
+		indexData.size(),
+		algorithm,
+		features,
+		useDistanceL1,
+		rebalancingFactor),
+		error;
+}
+bool FlannIndex::loadIndex(
+	const unsigned char * indexData,
+	size_t indexDataSize,
+	flann_algorithm_t algorithm,
+	const cv::Mat & features,
+	bool useDistanceL1,
+	float rebalancingFactor,
+	std::string * error)
+{
+	UASSERT(indexData!=NULL);
+	if(indexDataSize == 0) {
+		UWARN("Trying to load empty index....");
+		return false;
+	}
+
+#ifdef WIN32
+	UERROR("FLANN index deserialization is not yet implemented on Windows. Index cannot be loaded from memory buffer.");
+	return false;
+#else
+
+	// Check if the features match the expected data from the index
+	size_t headerSizeBytes = sizeof(int)*FLANN_INDEX_HEADER_SIZE;
+	if(indexDataSize < headerSizeBytes) {
+		if(error) {
+			*error = uFormat("Wrong header size detected (%ld vs expected %ld).", indexDataSize, headerSizeBytes);
+		}
+		return false;
+	}
+	const int * header = (const int *)indexData;
+	
+	int savedAlgorithm = header[3];
+	int savedDim = header[4];
+	bool savedDistanceL1 = header[5]==1;
+	float savedRebalancingFactor;
+	memcpy(&savedRebalancingFactor, &header[6], sizeof(header[6]));
+	int savedRows = header[7];
+	int savedCols = header[8];
+	int savedType = header[9];
+	unsigned int savedCrc;
+	 memcpy(&savedCrc, &header[10], sizeof(header[10]));
+	int savedIndexSize = header[11];
+
+	UDEBUG("Header: \"%d.%d.%d\" alg=%d dim=%d L1=%d factor=%f data(%dx%d type=%d, crc=%X) %d", 
+				header[0],header[1],header[2],
+				header[3],
+				header[4],
+				header[5],
+				savedRebalancingFactor,
+				header[7], header[8], header[9], savedCrc, 
+				header[11]);
+
+	if(savedAlgorithm != algorithm) {
+		if(error) {
+			*error = uFormat("Serialized flann algorithm (%d) doesn't match the expected one (%d).", savedAlgorithm, algorithm);
+		}
+		return false;
+	}
+	if(savedDim != features.cols) {
+		if(error) {
+			*error = uFormat("Serialized feature dimension (%d) doesn't match the expected one (%d).", savedDim, features.cols);
+		}
+		return false;
+	}
+	if(savedDistanceL1 != useDistanceL1) {
+		if(error) {
+			*error = uFormat("Serialized \"use distance L1\" (%s) doesn't match the expected one (%s).", savedDistanceL1?"true":"false", useDistanceL1?"true":"false");
+		}
+		return false;
+	}
+	if(savedRebalancingFactor != rebalancingFactor) {
+		if(error) {
+			*error = uFormat("Serialized \"rebalancing factor\" (%f) doesn't match the expected one (%f).", savedRebalancingFactor, rebalancingFactor);
+		}
+		return false;
+	}
+	if(savedRows != features.rows) {
+		if(error) {
+			*error = uFormat("Serialized feature count (%d) doesn't match the expected one (%d).", savedRows, features.rows);
+		}
+		return false;
+	}
+	if(savedCols != features.cols) {
+		if(error) {
+			*error = uFormat("Serialized feature dimension (%d) doesn't match the expected one (%d).", savedCols, features.cols);
+		}
+		return false;
+	}
+	if(savedType != features.type()) {
+		if(error) {
+			*error = uFormat("Serialized feature type (%d) doesn't match the expected one (%d).", savedType, features.type());
+		}
+		return false;
+	}
+	if(savedCrc != 0) {
+		// Compute checksum and compare
+		boost::crc_32_type result;
+		result.process_bytes(features.data, features.total()*features.elemSize());
+		if(savedCrc != result.checksum()) {
+			if(error) {
+				*error = uFormat("Serialized feature crc (%X) doesn't match the expected one (%X).", savedCrc, result.checksum());
+			}
+			return false;
+		}
+	}
+	if(savedIndexSize != int(indexDataSize - headerSizeBytes)) {
+		if(error) {
+			*error = uFormat("Serialized flann index size (%ld) doesn't match the expected one (%ld).", savedIndexSize, indexDataSize - headerSizeBytes);
+		}
+		return false;
+	}
+	if(savedIndexSize == 0) {
+		if(error) {
+			*error = "Serialized flann index is empty.";
+		}
+		return false;
+	}
+
 	this->release();
 	UASSERT(index_ == 0);
 	UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
@@ -213,14 +496,39 @@ void FlannIndex::buildKDTreeIndex(
 	featuresDim_ = features.cols;
 	useDistanceL1_ = useDistanceL1;
 	rebalancingFactor_ = rebalancingFactor;
+	algorithm_ = algorithm;

-	rtflann::KDTreeIndexParams params(trees);
+	UDEBUG("algorithm=%d", (int)algorithm);
+
+	rtflann::IndexParams params;
+
+	switch (algorithm)
+	{
+	case FLANN_INDEX_LINEAR:
+		params = rtflann::LinearIndexParams();
+		break;
+	case FLANN_INDEX_KDTREE:
+		params = rtflann::KDTreeIndexParams(4);
+		break;
+	case FLANN_INDEX_KDTREE_SINGLE:
+		params = rtflann::KDTreeSingleIndexParams(10, true);
+		break;
+	case FLANN_INDEX_LSH:
+		UASSERT(features.type() == CV_8UC1);
+		params = rtflann::LshIndexParams(12, 20, 2);
+		break;
+	default:
+		UFATAL("The flann algorithm type %d is not supported!", (int)algorithm);
+		break;
+	}
+
+	FILE* indexDataPtr = fmemopen((void*)(indexData+headerSizeBytes), indexDataSize - headerSizeBytes, "r");

 	if(featuresType_ == CV_8UC1)
 	{
 		rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
 		index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
-		((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
+		((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->load_saved_index(indexDataPtr);
 	}
 	else
 	{
@@ -228,22 +536,24 @@ void FlannIndex::buildKDTreeIndex(
 		if(useDistanceL1_)
 		{
 			index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
-			((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
+			((rtflann::Index<rtflann::L1<float> >*)index_)->load_saved_index(indexDataPtr);
 		}
 		else if(featuresDim_ <=3)
 		{
 			index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
-			((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->buildIndex();
+			((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->load_saved_index(indexDataPtr);
 		}
 		else
 		{
 			index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
-			((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
+			((rtflann::Index<rtflann::L2<float> >*)index_)->load_saved_index(indexDataPtr);
 		}
 	}
+	fclose(indexDataPtr);

 	// incremental FLANN: we should add all headers separately in case we remove
 	// some indexes (to keep underlying matrix data allocated)
+	
 	if(rebalancingFactor_ > 1.0f)
 	{
 		for(int i=0; i<features.rows; ++i)
@@ -257,107 +567,8 @@ void FlannIndex::buildKDTreeIndex(
 		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
 		nextIndex_ += features.rows;
 	}
-	UDEBUG("");
-}
-
-void FlannIndex::buildKDTreeSingleIndex(
-		const cv::Mat & features,
-		int leafMaxSize,
-		bool reorder,
-		bool useDistanceL1,
-		float rebalancingFactor)
-{
-	UDEBUG("");
-	this->release();
-	UASSERT(index_ == 0);
-	UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
-	featuresType_ = features.type();
-	featuresDim_ = features.cols;
-	useDistanceL1_ = useDistanceL1;
-	rebalancingFactor_ = rebalancingFactor;
-
-	rtflann::KDTreeSingleIndexParams params(leafMaxSize, reorder);
-
-	if(featuresType_ == CV_8UC1)
-	{
-		rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
-		index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
-		((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
-	}
-	else
-	{
-		rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
-		if(useDistanceL1_)
-		{
-			index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
-			((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
-		}
-		else if(featuresDim_ <=3)
-		{
-			index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
-			((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->buildIndex();
-		}
-		else
-		{
-			index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
-			((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
-		}
-	}
-
-	// incremental FLANN: we should add all headers separately in case we remove
-	// some indexes (to keep underlying matrix data allocated)
-	if(rebalancingFactor_ > 1.0f)
-	{
-		for(int i=0; i<features.rows; ++i)
-		{
-			addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
-		}
-	}
-	else
-	{
-		// tree won't ever be rebalanced, so just keep only one header for the data
-		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
-		nextIndex_ += features.rows;
-	}
-	UDEBUG("");
-}
-
-void FlannIndex::buildLSHIndex(
-		const cv::Mat & features,
-		unsigned int table_number,
-		unsigned int key_size,
-		unsigned int multi_probe_level,
-		float rebalancingFactor)
-{
-	UDEBUG("");
-	this->release();
-	UASSERT(index_ == 0);
-	UASSERT(features.type() == CV_8UC1);
-	featuresType_ = features.type();
-	featuresDim_ = features.cols;
-	useDistanceL1_ = true;
-	rebalancingFactor_ = rebalancingFactor;
-
-	rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
-	index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, rtflann::LshIndexParams(12, 20, 2));
-	((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
-
-	// incremental FLANN: we should add all headers separately in case we remove
-	// some indexes (to keep underlying matrix data allocated)
-	if(rebalancingFactor_ > 1.0f)
-	{
-		for(int i=0; i<features.rows; ++i)
-		{
-			addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
-		}
-	}
-	else
-	{
-		// tree won't ever be rebalanced, so just keep only one header for the data
-		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
-		nextIndex_ += features.rows;
-	}
-	UDEBUG("");
+	return true;
+#endif
 }

 bool FlannIndex::isBuilt()
@@ -99,15 +99,12 @@ unsigned long GlobalMap::getMemoryUsed() const
 	return memoryUsage;
 }

-bool GlobalMap::update(const std::map<int, Transform> & poses)
+bool GlobalMap::fullUpdateNeeded(const std::map<int, Transform> & poses) const
 {
-	UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
-
-	// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
 	bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
 	bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
 	float updateErrorSqrd = updateError_*updateError_;
-	for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
+	for(std::map<int, Transform>::const_iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
 	{
 		std::map<int, Transform>::const_iterator jter = poses.find(iter->first);
 		if(jter != poses.end())
@@ -125,7 +122,15 @@ bool GlobalMap::update(const std::map<int, Transform> & poses)
 		}
 	}

-	if(graphOptimized || graphChanged)
+	return graphOptimized || graphChanged;
+}
+
+bool GlobalMap::update(const std::map<int, Transform> & poses)
+{
+	UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
+
+	// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
+	if(fullUpdateNeeded(poses))
 	{
 		// clear all but keep cache
 		clear();
@@ -134,14 +139,25 @@ bool GlobalMap::update(const std::map<int, Transform> & poses)
 	std::list<std::pair<int, Transform> > orderedPoses;

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

 	// insert zero after
 	if(poses.find(0) != poses.end())
@@ -196,7 +196,7 @@ bool exportPoses(

 bool importPoses(
 		const std::string & filePath,
-		int format, // 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
+		int format, // 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
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints, // optional for formats 3 and 4
 		std::map<int, double> * stamps) // optional for format 1 and 9
@@ -440,16 +440,23 @@ bool importPoses(
 					UERROR("Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y, found %d)", str.c_str(), (int)strList.size());
 				}
 			}
-			else if(format == 1 || format==10 || format==11) // rgbd-slam format
+			else if(format == 1 || format==10 || format==11 || format==12) // rgbd-slam format
 			{
 				std::list<std::string> strList = uSplit(str);
 				if((strList.size() >=  8 && format!=11) || (strList.size() ==  9 && format==11))
 				{
+					if(!uIsNumber(strList.front())) {
+						UWARN("Skipping \"%s\"", str.c_str());
+						continue;
+					}
 					double stamp = uStr2Double(strList.front());
 					strList.pop_front();
-					if(format==11)
+					if(strList.size() == 8 && (format==10 || format==11 || format==12))
 					{
-						id = uStr2Int(strList.back());
+						if(format==11)
+						{
+							id = uStr2Int(strList.back());
+						}
 						strList.pop_back();
 					}
 					str = uJoin(strList, " ");
@@ -477,6 +484,20 @@ bool importPoses(
 										   1, 0, 0, 0);
 							pose = t*pose;
 						}
+						else if(format == 12)
+						{
+							// See https://www.ipb.uni-bonn.de/data/rgbd-dynamic-dataset/index.html
+							Transform T_ros(-1, 0, 0, 0,
+											 0, 0, 1, 0,
+											 0, 1, 0, 0);
+							Transform T_m(
+										1.0157,    0.1828,   -0.2389,    0.0113,
+										0.0009,   -0.8431,   -0.6413,   -0.00980,
+										-0.3009,    0.6147,   -0.8085,    0.0111);
+
+							// we remove the optical rotation
+							pose = T_ros*pose*T_ros*T_m*CameraModel::opticalRotation().inverse();
+						}
 						poses.insert(std::make_pair(id, pose));
 					}
 				}
@@ -1999,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)
@@ -2089,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;
@@ -2375,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;
 }
@@ -2405,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_;
@@ -64,8 +64,8 @@ void LocalGridCache::add(int nodeId,

 void LocalGridCache::add(int nodeId, const LocalGrid & localGrid)
 {
-	UDEBUG("nodeId=%d (ground=%d/%d obstacles=%d/%d empty=%d/%d)",
-			nodeId, localGrid.groundCells.cols,  localGrid.groundCells.channels(),  localGrid.obstacleCells.cols,  localGrid.obstacleCells.channels(), localGrid.emptyCells.cols,  localGrid.emptyCells.channels());
+	//UDEBUG("nodeId=%d (ground=%d/%d obstacles=%d/%d empty=%d/%d)",
+	//		nodeId, localGrid.groundCells.cols,  localGrid.groundCells.channels(),  localGrid.obstacleCells.cols,  localGrid.obstacleCells.channels(), localGrid.emptyCells.cols,  localGrid.emptyCells.channels());
 	if(nodeId < 0)
 	{
 		UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
@@ -36,9 +36,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/odometry/OdometryLOAM.h"
 #include "rtabmap/core/odometry/OdometryFLOAM.h"
 #include "rtabmap/core/odometry/OdometryMSCKF.h"
-#include "rtabmap/core/odometry/OdometryVINS.h"
+#include "rtabmap/core/odometry/OdometryVINSFusion.h"
 #include "rtabmap/core/odometry/OdometryOpenVINS.h"
 #include "rtabmap/core/odometry/OdometryOpen3D.h"
+#include "rtabmap/core/odometry/OdometryCuVSLAM.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_mapping.h"
@@ -103,8 +104,8 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeMSCKF:
 		odometry = new OdometryMSCKF(parameters);
 		break;
-	case Odometry::kTypeVINS:
-		odometry = new OdometryVINS(parameters);
+	case Odometry::kTypeVINSFusion:
+		odometry = new OdometryVINSFusion(parameters);
 		break;
 	case Odometry::kTypeOpenVINS:
 		odometry = new OdometryOpenVINS(parameters);
@@ -112,6 +113,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeOpen3D:
 		odometry = new OdometryOpen3D(parameters);
 		break;
+	case Odometry::kTypeCuVSLAM:
+		odometry = new OdometryCuVSLAM(parameters);
+		break;
 	default:
 		UERROR("Unknown odometry type %d, using F2M instead...", (int)type);
 		odometry = new OdometryF2M(parameters);
@@ -322,7 +326,9 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 				Transform previous = this->getPose();
 				Transform newFramePose = Transform(previous.x(), previous.y(), previous.z(), imuQuat.x(), imuQuat.y(), imuQuat.z(), imuQuat.w());
 				UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), newFramePose.prettyPrint().c_str());
+				std::map<double, rtabmap::Transform> imus = imus_;
 				this->reset(newFramePose);
+				imus_ = imus;
 			}

 			imus_.insert(std::make_pair(data.stamp(), imuT));
@@ -623,6 +629,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 	if(!guessIn.isNull())
 	{
 		guess = guessIn;
+		UDEBUG("Using provided guess %s", guessIn.prettyPrint().c_str());
 	}
 	else if(!imus_.empty())
 	{
@@ -639,12 +646,16 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			{
 				guess = guess.to3DoF();
 			}
+			UDEBUG("Adjusting guess from motion with IMU %s", guess.prettyPrint().c_str());
 		}
 		else if(!imuLastTransform_.isNull())
 		{
 			UWARN("Could not find imu transform at %f", data.stamp());
 		}
 	}
+	else if(!guess.isNull()) {
+		UDEBUG("Using guess from motion %s", guess.prettyPrint().c_str());
+	}

 	UTimer time;

@@ -1009,21 +1020,28 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		--_resetCurrentCount;
 		if(_resetCurrentCount == 0)
 		{
-			UWARN("Odometry automatically reset to latest pose!");
-			this->reset(_pose);
+			if(!guess.isNull() && !guessIn.isNull()) {
+				UWARN("Odometry automatically reset to latest pose (%s) + guess (%s)!", _pose.prettyPrint().c_str(), guess.prettyPrint().c_str());
+				this->reset(_pose * guess);
+			}
+			else {
+				UWARN("Odometry automatically reset to latest pose (%s)!", _pose.prettyPrint().c_str());
+				this->reset(_pose);
+			}
 			_resetCurrentCount = _resetCountdown;
 			if(info)
 			{
 				*info = OdometryInfo();
 			}
-			return this->computeTransform(data, Transform(), info);
+			this->computeTransform(data, Transform(), info);
+			return _pose;
 		}
-	}

 	previousVelocities_.clear();
 	velocityGuess_.setNull();
 	previousStamp_ = 0;

+}
 	return Transform();
 }

@@ -1053,6 +1071,7 @@ void Odometry::initKalmanFilter(const Transform & initialPose, float vx, float v
 	    0, 0, 0, 0, 0, 0.17 } };
 	static const boost::array<double, 36> STANDARD_TWIST_COVARIANCE =
 	{ { 0.05, 0, 0, 0, 0, 0,
+	}
 	    0, 0.05, 0, 0, 0, 0,
 	    0, 0, 0.05, 0, 0, 0,
 	    0, 0, 0, 0.09, 0, 0,
@@ -126,10 +126,10 @@ std::map<std::string, float> OdometryInfo::statistics(const Transform & pose)
 	stats.insert(std::make_pair("Odometry/ICPStructuralComplexity/", reg.icpStructuralComplexity));
 	stats.insert(std::make_pair("Odometry/ICPStructuralDistribution/", reg.icpStructuralDistribution));
 	stats.insert(std::make_pair("Odometry/ICPCorrespondences/", reg.icpCorrespondences));
-	stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)reg.covariance.at<double>(0,0))));
-	stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)reg.covariance.at<double>(5,5))));
-	stats.insert(std::make_pair("Odometry/VarianceLin/", (float)reg.covariance.at<double>(0,0)));
-	stats.insert(std::make_pair("Odometry/VarianceAng/", (float)reg.covariance.at<double>(5,5)));
+	stats.insert(std::make_pair("Odometry/StdDevLin/", reg.covariance.empty()?0:sqrt((float)reg.covariance.at<double>(0,0))));
+	stats.insert(std::make_pair("Odometry/StdDevAng/", reg.covariance.empty()?0:sqrt((float)reg.covariance.at<double>(5,5))));
+	stats.insert(std::make_pair("Odometry/VarianceLin/", reg.covariance.empty()?0:(float)reg.covariance.at<double>(0,0)));
+	stats.insert(std::make_pair("Odometry/VarianceAng/", reg.covariance.empty()?0:(float)reg.covariance.at<double>(5,5)));
 	stats.insert(std::make_pair("Odometry/TimeEstimation/ms", timeEstimation*1000.0f));
 	stats.insert(std::make_pair("Odometry/TimeFiltering/ms", timeParticleFiltering*1000.0f));
 	stats.insert(std::make_pair("Odometry/LocalMapSize/", localMapSize));
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"

 namespace rtabmap {

@@ -40,8 +41,8 @@ OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSi
 	_dataBufferMaxSize(dataBufferMaxSize),
 	_resetOdometry(false),
 	_resetPose(Transform::getIdentity()),
-	_lastImuStamp(0.0),
-	_imuEstimatedDelay(0.0)
+	_oldestAsyncImuStamp(0.0),
+	_newestAsyncImuStamp(0.0)
 {
 	UASSERT(_odometry != 0);
 }
@@ -63,7 +64,7 @@ bool OdometryThread::handleEvent(UEvent * event)
 			SensorEvent * sensorEvent = (SensorEvent*)event;
 			if(sensorEvent->getCode() == SensorEvent::kCodeData)
 			{
-				this->addData(sensorEvent->data());
+				this->addData(*sensorEvent);
 			}
 		}
 		else if(event->getClassName().compare("IMUEvent") == 0)
@@ -110,32 +111,53 @@ void OdometryThread::mainLoop()
 		UScopeMutex lock(_dataMutex);
 		_dataBuffer.clear();
 		_imuBuffer.clear();
-		_lastImuStamp = 0.0f;
+		_oldestAsyncImuStamp = 0.0;
+		_newestAsyncImuStamp = 0.0;
+		_previousGuessPose.setNull();
 	}

-	SensorData data;
-	if(getData(data))
+	SensorEvent event;
+	if(getData(event))
 	{
 		OdometryInfo info;
 		UDEBUG("Processing data...");
-		Transform pose = _odometry->process(data, &info);
+		Transform guess;
+		UDEBUG("event.info().odomPose=%s", event.info().odomPose.prettyPrint().c_str());
+		if(!_previousGuessPose.isNull() && !event.info().odomPose.isNull()) {
+			guess = _previousGuessPose.inverse() * event.info().odomPose;
+		}
+
+		SensorData data = event.data();
+		Transform pose = _odometry->process(data, guess , &info);
 		if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (pose.isNull() && data.imu().empty()))
 		{
 			UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
+			if(!pose.isNull()) {
+				_previousGuessPose = event.info().odomPose;
+				UASSERT(event.info().odomPose.isNull() || !info.reg.covariance.empty());
+				if(!event.info().odomPose.isNull() && info.reg.covariance.at<double>(0,0) >= 9999 &&
+					(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();
+				}
+			}
 			// a null pose notify that odometry could not be computed
 			this->post(new OdometryEvent(data, pose, info));
 		}
 	}
 }

-void OdometryThread::addData(const SensorData & data)
+void OdometryThread::addData(const SensorEvent & event)
 {
-	if(data.imu().empty())
+	if(event.data().imu().empty())
 	{
 		if(dynamic_cast<OdometryMono*>(_odometry) == 0)
 		{
-			if((data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty())) &&
-					data.laserScanRaw().empty())
+			if((event.data().imageRaw().empty() || event.data().depthOrRightRaw().empty() || (event.data().cameraModels().empty() && event.data().stereoCameraModels().empty())) &&
+					event.data().laserScanRaw().empty())
 			{
 				ULOGGER_ERROR("Missing some information (images/scans empty or missing calibration)!?");
 				return;
@@ -144,7 +166,7 @@ void OdometryThread::addData(const SensorData & data)
 		else
 		{
 			// Mono can accept RGB only
-			if(data.imageRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty()))
+			if(event.data().imageRaw().empty() || (event.data().cameraModels().empty() && event.data().stereoCameraModels().empty()))
 			{
 				ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");
 				return;
@@ -155,28 +177,47 @@ void OdometryThread::addData(const SensorData & data)
 	bool notify = true;
 	_dataMutex.lock();
 	{
-		if( !data.imageRaw().empty() ||
-			!data.imageCompressed().empty() ||
-			!data.laserScanRaw().isEmpty() ||
-			!data.laserScanCompressed().empty() || 
-			data.imu().empty())
+		if( !event.data().imageRaw().empty() ||
+			!event.data().imageCompressed().empty() ||
+			!event.data().laserScanRaw().isEmpty() ||
+			!event.data().laserScanCompressed().empty() || 
+			event.data().imu().empty())
 		{
-			_dataBuffer.push_back(data);
-			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
-			{
-				UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
-				_dataBuffer.erase(_dataBuffer.begin());
+			if(_oldestAsyncImuStamp > 0.0 && event.data().stamp() < _oldestAsyncImuStamp) {
+				UWARN("Received image/lidar with stamp (%f) older than oldest received imu "
+					"(%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 with system time is %fs.",
+					event.data().stamp(), _oldestAsyncImuStamp, _imuBuffer.size(), UTimer::now() - event.data().stamp());
 				notify = false;
 			}
+			else if(_newestAsyncImuStamp > 0.0 && event.data().stamp()>=_newestAsyncImuStamp) {
+				UWARN("Received image/lidar with stamp (%f) newer than latest received imu "
+					"(%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 with system time is %fs.",
+					event.data().stamp(), _newestAsyncImuStamp, _imuBuffer.size(), UTimer::now() - event.data().stamp());
+				notify = false;
+			}
+			else {
+				_dataBuffer.push_back(event);
+				while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
+				{
+					UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
+					_dataBuffer.erase(_dataBuffer.begin());
+					notify = false;
+				}
+			}
 		}
 		else
 		{
-			_imuBuffer.push_back(data);
-			if(_lastImuStamp != 0.0 && data.stamp() > _lastImuStamp)
-			{
-				_imuEstimatedDelay = data.stamp() - _lastImuStamp;
+			_imuBuffer.push_back(event.data());
+			if(_oldestAsyncImuStamp == 0) {
+				_oldestAsyncImuStamp = event.data().stamp();
 			}
-			_lastImuStamp = data.stamp();
+			_newestAsyncImuStamp = event.data().stamp();
 		}
 	}
 	_dataMutex.unlock();
@@ -187,7 +228,7 @@ void OdometryThread::addData(const SensorData & data)
 	}
 }

-bool OdometryThread::getData(SensorData & data)
+bool OdometryThread::getData(SensorEvent & event)
 {
 	bool dataFilled = false;
 	_dataAdded.acquire();
@@ -195,22 +236,18 @@ bool OdometryThread::getData(SensorData & data)
 	{
 		if(!_dataBuffer.empty())
 		{
-			if(!_imuBuffer.empty())
+			// Send IMU up to stamp greater than image (OpenVINS needs this).
+			while(!_imuBuffer.empty())
 			{
-				// Send IMU up to stamp greater than image (OpenVINS needs this).
-				while(!_imuBuffer.empty())
-				{
-					_odometry->process(_imuBuffer.front());
-					double stamp = _imuBuffer.front().stamp();
-					_imuBuffer.pop_front();
-					if(stamp > _dataBuffer.front().stamp())
-					{
-						break;
-					}
+				_odometry->process(_imuBuffer.front());
+				double stamp =_imuBuffer.front().stamp();
+				_imuBuffer.pop_front();
+				if(stamp > _dataBuffer.front().data().stamp()) {
+					break;
 				}
 			}

-			data = _dataBuffer.front();
+			event = _dataBuffer.front();
 			_dataBuffer.pop_front();
 			dataFilled = true;
 		}
@@ -185,6 +185,52 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame
 	return optimizer;
 }

+class LinkIdKey
+{
+public:
+LinkIdKey(int id, Link::Type type) :
+		id_(id),
+		type_(type) {}
+	bool operator<(const LinkIdKey & k) const
+	{
+		// landmark, sort by smallest to largest landmark id, after normal links
+		if(id_ < 0 && k.id_ < 0)
+		{
+			return id_ > k.id_;
+		}
+		else if(id_ < 0) {
+			return false;
+		}
+		else if(k.id_ < 0) {
+			return true;
+		}
+
+		if(type_ == Link::kNeighbor && k.type_ != Link::kNeighbor)
+		{
+			return true;
+		}
+		else if(type_ != Link::kNeighbor && k.type_ == Link::kNeighbor)
+		{
+			return false;
+		}
+		else if(type_ == Link::kNeighborMerged && k.type_ != Link::kNeighbor && k.type_ != Link::kNeighborMerged)
+		{
+			return true;
+		}
+		else if(k.type_ == Link::kNeighborMerged && type_ != Link::kNeighbor && type_ != Link::kNeighborMerged)
+		{
+			return false;
+		}
+		else
+		{
+			// normal link, sort by smallest to largest id
+			return id_ < k.id_;
+		}
+	}
+	int id_;
+	Link::Type type_;
+};
+
 void Optimizer::getConnectedGraph(
 		int fromId,
 		const std::map<int, Transform> & posesIn,
@@ -199,8 +245,8 @@ void Optimizer::getConnectedGraph(
 	posesOut.clear();
 	linksOut.clear();

-	std::set<int> nextPoses;
-	nextPoses.insert(fromId);
+	std::map<LinkIdKey, Transform> nextPoses;
+	nextPoses.insert(std::make_pair(LinkIdKey(fromId, Link::kUndef), posesIn.find(fromId)->second));
 	std::multimap<int, std::pair<int, Link::Type> > biLinks;
 	for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
 	{
@@ -214,22 +260,27 @@ void Optimizer::getConnectedGraph(
 		}
 	}

-	while(nextPoses.size())
+	while(!nextPoses.empty())
 	{
-		int currentId = *nextPoses.rbegin(); // fill up all nodes before landmarks
-		nextPoses.erase(*nextPoses.rbegin());
+		// Fill up all nodes before landmarks
+		// For nodes, fill up all neightbor nodes before loop closure ones
+		int currentId = nextPoses.begin()->first.id_; 
+		Transform currentPose = nextPoses.begin()->second;
+		nextPoses.erase(nextPoses.begin());

-		if(posesOut.empty())
+		if(posesOut.find(currentId) != posesOut.end()) {
+			// Already added from priority list
+			continue;
+		}
+
+		posesOut.insert(std::make_pair(currentId, currentPose));
+
+		// add prior links
+		for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
 		{
-			posesOut.insert(std::make_pair(currentId, posesIn.find(currentId)->second));
-
-			// add prior links
-			for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
+			if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
 			{
-				if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
-				{
-					linksOut.insert(*pter);
-				}
+				linksOut.insert(*pter);
 			}
 		}

@@ -240,52 +291,42 @@ void Optimizer::getConnectedGraph(
 			if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
 			{
 				std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId, true, type);
-				if(nextPoses.find(toId) == nextPoses.end())
+				UASSERT(kter!=linksIn.end());
+				if(!uContains(posesOut, toId))
 				{
-					if(!uContains(posesOut, toId))
+					const Transform & poseToIn = posesIn.at(toId);
+					Transform t = kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse();
+					Transform pose;
+					if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0 && (poseToIn.is3DoF() || poseToIn.is4DoF()))
 					{
-						const Transform & poseToIn = posesIn.at(toId);
-						Transform t = kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse();
-						if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0 && (poseToIn.is3DoF() || poseToIn.is4DoF()))
+						if(poseToIn.is3DoF())
 						{
-							if(poseToIn.is3DoF())
-							{
-								posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
-							}
-							else
-							{
-								posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to4DoF()));
-							}
+							pose = (posesOut.at(currentId) * t).to3DoF();
 						}
 						else
 						{
-							posesOut.insert(std::make_pair(toId, posesOut.at(currentId)* t));
+							pose = (posesOut.at(currentId) * t).to4DoF();
 						}
-
-						// add prior links
-						for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
-						{
-							if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
-							{
-								linksOut.insert(*pter);
-							}
-						}
-
-						nextPoses.insert(toId);
+					}
+					else
+					{
+						pose = posesOut.at(currentId)* t;
 					}

-					// only add unique links
-					if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
+					nextPoses.insert(std::make_pair(LinkIdKey(toId, type), pose));
+				}
+
+				// only add unique links
+				if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
+				{
+					if(kter->second.to() < 0)
 					{
-						if(kter->second.to() < 0)
-						{
-							// For landmarks, make sure fromId is the landmark
-							linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse()));
-						}
-						else
-						{
-							linksOut.insert(*kter);
-						}
+						// For landmarks, make sure fromId is the landmark
+						linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse()));
+					}
+					else
+					{
+						linksOut.insert(*kter);
 					}
 				}
 			}
@@ -608,8 +649,8 @@ void Optimizer::computeBACorrespondences(
 					}
 				}

-				if(sFrom.getWords().size() &&
-					sTo.getWords().size() &&
+				if(sFrom.getWordsKpts().size() &&
+					sTo.getWordsKpts().size() &&
 					sFrom.getWords3().size())
 				{
 					if(!rematchFeatures)
@@ -168,6 +168,7 @@ bool Parameters::isFeatureParameter(const std::string & parameter)
 			group.compare("BRISK") == 0 ||
 			group.compare("KAZE") == 0 ||
 			group.compare("SuperPoint") == 0 ||
+			group.compare("SuperPointRpautrat") == 0 ||
 			group.compare("PyDetector") == 0;
 }

@@ -182,6 +183,7 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, boo
 			(stereo && group.compare("Stereo") == 0) ||
 			(icp && group.compare("Icp") == 0) ||
 			(vis && Parameters::isFeatureParameter(iter->first)) ||
+			group.compare("OdomCuVSLAM") == 0 ||
 			group.compare("Reg") == 0 ||
 			group.compare("Optimizer") == 0 ||
 			group.compare("g2o") == 0 ||
@@ -236,6 +238,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters

+		// 0.23.1
+		removedParameters_.insert(std::make_pair("OdomVINS/ConfigPath",    std::make_pair(true, Parameters::kOdomVINSFusionConfigPath())));
+
 		// 0.21.13
 		removedParameters_.insert(std::make_pair("Vis/ForwardEstOnly",    std::make_pair(false, "")));

@@ -658,6 +663,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With SuperPoint Rpautrat:"; 
+#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON) 
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; 
+#else 
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; 
 #endif
 				str = "With Python3:";
 #ifdef RTABMAP_PYTHON
@@ -936,7 +947,7 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With VINS-Fusion:";
-#ifdef RTABMAP_VINS
+#ifdef RTABMAP_VINS_FUSION
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
@@ -1109,8 +1120,8 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 						ignore = true;
 					}
 #endif
-#ifndef RTABMAP_ORBSLAM2
-					if(group.compare("OdomORBSLAM2") == 0)
+#ifndef RTABMAP_ORB_SLAM
+					if(group.compare("OdomORBSLAM") == 0)
 					{
 						ignore = true;
 					}
@@ -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);
@@ -286,6 +292,10 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	{
 		uInsert(_featureParameters, ParametersPair(Parameters::kKpGridCols(), parameters.at(Parameters::kVisGridCols())));
 	}
+	if(uContains(parameters, Parameters::kRtabmapWorkingDirectory()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kRtabmapWorkingDirectory(), parameters.at(Parameters::kRtabmapWorkingDirectory())));
+	}

 	delete _detectorFrom;
 	delete _detectorTo;
@@ -317,6 +327,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
 	UDEBUG("%s=%f", Parameters::kVisPnPMaxVariance().c_str(), _PnPMaxVar);
 	UDEBUG("%s=%f", Parameters::kVisPnPSplitLinearCovComponents().c_str(), _PnPSplitLinearCovarianceComponents);
+	UDEBUG("%s=%f", Parameters::kVisPnPVarianceMedianRatio().c_str(), _PnPVarMedianRatio);
 	UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
 	UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
 	UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
@@ -329,11 +340,12 @@ Transform RegistrationVis::computeTransformationImpl(
 	UDEBUG("Feature Detector = %d", (int)_detectorFrom->getType());
 	UDEBUG("guess=%s", guess.prettyPrint().c_str());

-	UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors,  %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
+	UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors, %d words kpts, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
 			fromSignature.id(),
 			(int)fromSignature.getWords().size(),
 			(int)fromSignature.getWords3().size(),
 			(int)fromSignature.getWordsDescriptors().rows,
+			(int)fromSignature.getWordsKpts().size(),
 			(int)fromSignature.sensorData().keypoints().size(),
 			(int)fromSignature.sensorData().keypoints3D().size(),
 			fromSignature.sensorData().descriptors().rows,
@@ -342,11 +354,12 @@ Transform RegistrationVis::computeTransformationImpl(
 			(int)fromSignature.sensorData().cameraModels().size(),
 			(int)fromSignature.sensorData().stereoCameraModels().size());

-	UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
+	UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d words kpts, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
 			toSignature.id(),
 			(int)toSignature.getWords().size(),
 			(int)toSignature.getWords3().size(),
 			(int)toSignature.getWordsDescriptors().rows,
+			(int)toSignature.getWordsKpts().size(),
 			(int)toSignature.sensorData().keypoints().size(),
 			(int)toSignature.sensorData().keypoints3D().size(),
 			toSignature.sensorData().descriptors().rows,
@@ -375,6 +388,9 @@ Transform RegistrationVis::computeTransformationImpl(
 	{
 		UDEBUG("");
 		// just some checks to make sure that input data are ok
+		UASSERT(fromSignature.getWords().empty() ||
+				fromSignature.getWordsKpts().empty() ||
+				(fromSignature.getWords().size() == fromSignature.getWordsKpts().size()));
 		UASSERT(fromSignature.getWords().empty() ||
 				fromSignature.getWords3().empty() ||
 				(fromSignature.getWords().size() == fromSignature.getWords3().size()));
@@ -382,8 +398,11 @@ Transform RegistrationVis::computeTransformationImpl(
 				(int)fromSignature.getWords().size() == fromSignature.getWordsDescriptors().rows ||
 				fromSignature.sensorData().descriptors().empty() ||
 				fromSignature.getWordsDescriptors().empty() == 0);
-		UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
-				(toSignature.getWords().size() && toSignature.getWords3().empty())||
+		UASSERT(toSignature.getWords().empty() ||
+				toSignature.getWordsKpts().empty() ||
+				(toSignature.getWords().size() == toSignature.getWordsKpts().size()));
+		UASSERT(toSignature.getWords().empty() ||
+				toSignature.getWords3().empty() ||
 				(toSignature.getWords().size() == toSignature.getWords3().size()));
 		UASSERT((int)toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
 				(int)toSignature.getWords().size() == toSignature.getWordsDescriptors().rows ||
@@ -428,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.");
 							}

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

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

@@ -1620,6 +1625,7 @@ Transform RegistrationVis::computeTransformationImpl(
 						cameraTransform,
 						_PnPReprojError,
 						0.99f,
+						_PnPVarMedianRatio,
 						words3A, // for scale estimation
 						&variance,
 						&matchesV);
@@ -1660,14 +1666,10 @@ Transform RegistrationVis::computeTransformationImpl(
 					UINFO(msg.c_str());
 				}
 			}
-			else if(fromSignature.getWords().size() == 0)
+			else 
 			{
-				msg = uFormat("No enough features (%d)", (int)fromSignature.getWords().size());
-				UWARN(msg.c_str());
-			}
-			else
-			{
-				msg = uFormat("No camera model");
+				msg = uFormat("No enough features < %s=%d (from=%d to=%d)", 
+					Parameters::kVisMinInliers().c_str(), _minInliers, (int)fromSignature.getWords().size(), (int)toSignature.getWords().size());
 				UWARN(msg.c_str());
 			}
 		}
@@ -2174,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)",
@@ -2196,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)
@@ -1502,8 +1496,8 @@ bool Rtabmap::process(
 	float angleToClosestNodeInTheGraph = 0;
 	if(_rgbdSlamMode)
 	{
-		double linVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at<double>(0,0), odomCovariance.at<double>(1,1)>=9999?0:odomCovariance.at<double>(1,1), odomCovariance.at<double>(2,2)>=9999?0:odomCovariance.at<double>(2,2));
-		double angVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at<double>(3,3)>=9999?0:odomCovariance.at<double>(3,3), odomCovariance.at<double>(4,4)>=9999?0:odomCovariance.at<double>(4,4), odomCovariance.at<double>(5,5));
+		double linVar = odomCovariance.empty()?0.0f:uMax3(odomCovariance.at<double>(0,0), odomCovariance.at<double>(1,1)>=9999?0:odomCovariance.at<double>(1,1), odomCovariance.at<double>(2,2)>=9999?0:odomCovariance.at<double>(2,2));
+		double angVar = odomCovariance.empty()?0.0f:uMax3(odomCovariance.at<double>(3,3)>=9999?0:odomCovariance.at<double>(3,3), odomCovariance.at<double>(4,4)>=9999?0:odomCovariance.at<double>(4,4), odomCovariance.at<double>(5,5));
 		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), (float)linVar);
 		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), (float)angVar);

@@ -1544,9 +1538,10 @@ bool Rtabmap::process(
 				{
 					float x,y,z, roll,pitch,yaw;
 					t.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-					bool isMoving = fabs(x) > _rgbdLinearUpdate ||
-									fabs(y) > _rgbdLinearUpdate ||
-									fabs(z) > _rgbdLinearUpdate ||
+					bool isMoving = (_rgbdLinearUpdate > 0.0f && (
+										fabs(x) > _rgbdLinearUpdate ||
+										fabs(y) > _rgbdLinearUpdate ||
+										fabs(z) > _rgbdLinearUpdate)) ||
 									(_rgbdAngularUpdate>0.0f && (
 										fabs(roll) > _rgbdAngularUpdate ||
 										fabs(pitch) > _rgbdAngularUpdate ||
@@ -1607,6 +1602,7 @@ bool Rtabmap::process(
 					Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info);
 					if(!t.isNull())
 					{
+						UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
 						UINFO("Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s",
 								oldId,
 								signature->id(),
@@ -1614,7 +1610,6 @@ bool Rtabmap::process(
 								info.covariance.at<double>(5,5),
 								guess.prettyPrint().c_str(),
 								t.prettyPrint().c_str());
-						UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
 						_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.covariance.inv()));

 						if(_optimizeFromGraphEnd)
@@ -1788,7 +1783,7 @@ bool Rtabmap::process(
 			}
 		}
 		_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
-		if(!_memory->isIncremental() && signature->getWeight() >= 0)
+		if(signature->getWeight() >= 0)
 		{
 			UDEBUG("Update odometry localization cache (size=%d/%d)", (int)_odomCachePoses.size(), _maxOdomCacheSize);
 			if(!_odomCachePoses.empty())
@@ -1894,7 +1889,7 @@ bool Rtabmap::process(
 								*iter,
 								transform.prettyPrint().c_str());
 						// Add a loop constraint
-						UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
+						UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
 						if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, getInformation(info.covariance))))
 						{
 							++proximityDetectionsInTimeFound;
@@ -2395,24 +2390,22 @@ bool Rtabmap::process(
 					distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
 				}

-				if(distanceSoFar <= _localRadius)
+				if(_memory->getSignature(_path[i].first) != 0)
 				{
-					if(_memory->getSignature(_path[i].first) != 0)
+					if(immunizedLocations.insert(_path[i].first).second)
 					{
-						if(immunizedLocations.insert(_path[i].first).second)
-						{
-							++immunizedLocally;
-						}
-						UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
-					}
-					else if(retrievalLocalIds.size() < _maxLocalRetrieved)
-					{
-						UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
-						retrievalLocalIds.push_back(_path[i].first);
-						// retrieved locations are automatically immunized
+						++immunizedLocally;
 					}
+					UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
 				}
-				else
+				else if(retrievalLocalIds.size() < _maxLocalRetrieved)
+				{
+					UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
+					retrievalLocalIds.push_back(_path[i].first);
+					// retrieved locations are automatically immunized
+				}
+				
+				if(distanceSoFar > _localRadius)
 				{
 					UDEBUG("Stop on node %d (dist=%fm > %fm)",
 							_path[i].first, distanceSoFar, _localRadius);
@@ -2614,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;
@@ -2667,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
@@ -2679,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)
 				{
@@ -2686,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)
 							{
@@ -2784,7 +2799,7 @@ bool Rtabmap::process(
 											signature->id(),
 											nearestId,
 											transform.prettyPrint().c_str());
-									UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
+									UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);

 									//for statistics
 									loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
@@ -2796,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);
@@ -2995,7 +3011,7 @@ bool Rtabmap::process(
 										}

 										// set Identify covariance for laser scan matching only
-										UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
+										UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
 										_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/_proximityMergedScanCovFactor, scanMatchingIds));
 										loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));

@@ -3063,6 +3079,7 @@ bool Rtabmap::process(
 				loopClosureVisualInliers = info.inliers;
 				loopClosureVisualInliersRatio = info.inliersRatio;
 				loopClosureVisualMatches = info.matches;
+				loopClosureVisualVariance = info.variance;
 				rejectedLoopClosure = transform.isNull();
 				if(rejectedLoopClosure)
 				{
@@ -3083,7 +3100,7 @@ bool Rtabmap::process(
 			if(!rejectedLoopClosure)
 			{
 				// Make the new one the parent of the old one
-				UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
+				UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
 				
 				loopClosureLinearVariance = uMax3(info.covariance.at<double>(0,0), info.covariance.at<double>(1,1)>=9999?0:info.covariance.at<double>(1,1), info.covariance.at<double>(2,2)>=9999?0:info.covariance.at<double>(2,2));
 				loopClosureAngularVariance = uMax3(info.covariance.at<double>(3,3)>=9999?0:info.covariance.at<double>(3,3), info.covariance.at<double>(4,4)>=9999?0:info.covariance.at<double>(4,4), info.covariance.at<double>(5,5));
@@ -3158,17 +3175,7 @@ bool Rtabmap::process(
 			UASSERT(uContains(_optimizedPoses, signature->id()));
 			UASSERT_MSG(uContains(_optimizedPoses, _path[_pathCurrentIndex].first), uFormat("id=%d", _path[_pathCurrentIndex].first).c_str());
 			Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first);
-
-			if(_localRadius == 0.0f || virtualLoop.getNorm() < _localRadius)
-			{
-				_memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // set high variance
-			}
-			else
-			{
-				UERROR("Virtual link larger than local radius (%fm > %fm). Aborting the plan!",
-						virtualLoop.getNorm(), _localRadius);
-				this->clearPath(-1);
-			}
+			_memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // set high variance
 		}
 	}

@@ -3269,6 +3276,7 @@ bool Rtabmap::process(
 				{
 					constraints.insert(std::make_pair(iter->second.from(), iter->second));
 				}
+
 				cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf;
 				for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 				{
@@ -3276,6 +3284,7 @@ bool Rtabmap::process(
 					if(iterPose != _optimizedPoses.end() && poses.find(iterPose->first) == poses.end())
 					{
 						poses.insert(*iterPose);
+
 						// make the poses in the map fixed
 						constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat)));
 						UDEBUG("Constraint %d->%d: %s (type=%s, var=%f)", iterPose->first, iterPose->first, iterPose->second.prettyPrint().c_str(), Link::typeName(Link::kPosePrior).c_str(), 1./_localizationPriorInf);
@@ -3285,11 +3294,14 @@ bool Rtabmap::process(

 				std::map<int, Transform> posesOut;
 				std::multimap<int, Link> edgeConstraintsOut;
+
 				bool priorsIgnored = _graphOptimizer->priorsIgnored();
 				UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false");
 				_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
+
 				// If slam2d: get connected graph while keeping original roll,pitch,z values.
 				_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut);
+				
 				if(ULogger::level() == ULogger::kDebug)
 				{
 					for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
@@ -4055,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());
@@ -4315,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
 		{
@@ -5642,8 +5669,10 @@ int Rtabmap::detectMoreLoopClosures(
 		bool intraSession,
 		bool interSession,
 		const ProgressState * processState,
-		float clusterRadiusMin)
+		float clusterRadiusMin,
+		int toFromMapId)
 {
+	UDEBUG("");
 	UASSERT(iterations>0);

 	if(_graphOptimizer->iterations() <= 0)
@@ -5668,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));
 		}
 	}

@@ -5692,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;
@@ -5744,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))
@@ -5755,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())
 							{
@@ -5764,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;
@@ -5783,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,
@@ -6326,6 +6412,7 @@ bool Rtabmap::addLink(const Link & link)
 		std::map<int, Transform> poses = _odomCachePoses;
 		std::multimap<int, Link> constraints = _odomCacheConstraints;
 		constraints.insert(std::make_pair(link.from(), link));
+		cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf;
 		for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 		{
 			std::map<int, Transform>::iterator iterPose = _optimizedPoses.find(iter->second.to());
@@ -6333,7 +6420,7 @@ bool Rtabmap::addLink(const Link & link)
 			{
 				poses.insert(*iterPose);
 				// make the poses in the map fixed
-				constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, cv::Mat::eye(6,6, CV_64FC1)*999999)));
+				constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat)));
 			}
 		}

@@ -6927,24 +7014,24 @@ void Rtabmap::updateGoalIndex()
 				{
 					distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
 				}
-				if(distanceSoFar <= _localRadius)
+				
+				if(_path[i].first != _path[i-1].first)
 				{
-					if(_path[i].first != _path[i-1].first)
+					const Signature * s = _memory->getSignature(_path[i].first);
+					if(s)
 					{
-						const Signature * s = _memory->getSignature(_path[i].first);
-						if(s)
+						if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
 						{
-							if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
-							{
-								Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
-								_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path
-								UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
-							}
+							Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
+							_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path
+							UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
 						}
 					}
 				}
-				else
+
+				if(distanceSoFar > _localRadius)
 				{
+					UDEBUG("Farthest goal=%d : %f m", _path[i].first, distanceSoFar);
 					break;
 				}
 			}
@@ -7004,11 +7091,8 @@ void Rtabmap::updateGoalIndex()
 					if((goalIndex == _pathCurrentIndex && i == _path.size()-1) ||
 					   _pathUnreachableNodes.find(i) == _pathUnreachableNodes.end())
 					{
-						if(distanceFromCurrentNode <= _localRadius)
-						{
-							goalIndex = i;
-						}
-						else
+						goalIndex = i;
+						if(distanceFromCurrentNode > _localRadius)
 						{
 							break;
 						}
@@ -506,9 +506,10 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 				ignoreFrame = true;
 			}
 		}
+		UASSERT(!odomEvent.info().reg.covariance.empty());
 		if(!lastPose_.isIdentity() &&
-						(odomEvent.pose().isIdentity() ||
-						odomEvent.info().reg.covariance.at<double>(0,0)>=9999))
+			(odomEvent.pose().isIdentity() ||
+			odomEvent.info().reg.covariance.at<double>(0,0)>=9999))
 		{
 			if(odomEvent.pose().isIdentity())
 			{
@@ -960,7 +960,6 @@ void SensorCaptureThread::postUpdate(SensorData * dataPtr, SensorCaptureInfo * i
 			{
 				UASSERT(!data.imu().localTransform().isNull());
 				imu.convertToBaseFrame();
-
 			}
 			_imuFilter->update(
 					imu.angularVelocity()[0],
@@ -548,7 +548,7 @@ void SensorData::setOccupancyGrid(
 			float cellSize,
 			const cv::Point3f & viewPoint)
 {
-	UDEBUG("ground=%d obstacles=%d empty=%d", ground.cols, obstacles.cols, empty.cols);
+	//UDEBUG("ground=%d obstacles=%d empty=%d", ground.cols, obstacles.cols, empty.cols);
 	if((!ground.empty() && (!_groundCellsCompressed.empty() || !_groundCellsRaw.empty())) ||
 	   (!obstacles.empty() && (!_obstacleCellsCompressed.empty() || !_obstacleCellsRaw.empty())) ||
 	   (!empty.empty() && (!_emptyCellsCompressed.empty() || !_emptyCellsRaw.empty())))
@@ -649,7 +649,7 @@ void SensorData::uncompressData(
 		cv::Mat * emptyCellsRaw,
 		cv::Mat * depthConfidenceRaw)
 {
-	UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d,%d)", 
+	/*UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d,%d)", 
 		this->id(),
 		imageRaw?1:0,
 		depthRaw?1:0,
@@ -658,7 +658,7 @@ void SensorData::uncompressData(
 		groundCellsRaw?1:0,
 		obstacleCellsRaw?1:0, 
 		emptyCellsRaw?1:0,
-		depthConfidenceRaw?1:0);
+		depthConfidenceRaw?1:0);*/
 	if(imageRaw == 0 &&
 		depthRaw == 0 &&
 		laserScanRaw == 0 &&
@@ -118,7 +118,7 @@ void Signature::addLinks(const std::map<int, Link> & links)
 }
 void Signature::addLink(const Link & link)
 {
-	UDEBUG("Add link %d to %d (type=%d/%s var=%f,%f)", link.to(), this->id(), (int)link.type(), link.typeName().c_str(), link.transVariance(), link.rotVariance());
+	//UDEBUG("Add link %d to %d (type=%d/%s var=%f,%f)", link.to(), this->id(), (int)link.type(), link.typeName().c_str(), link.transVariance(), link.rotVariance());
 	UASSERT_MSG(link.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
 	UASSERT_MSG((link.to() != this->id()) || link.type()==Link::kPosePrior || link.type()==Link::kGravity, uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
 	UASSERT_MSG(link.to() == this->id() || _links.find(link.to()) == _links.end(), uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
@@ -318,7 +318,7 @@ void Signature::setWords(const std::multimap<int, int> & words,
 	UASSERT_MSG(descriptors.empty() || descriptors.rows == (int)words.size(), uFormat("words=%d, descriptors=%d", (int)words.size(), descriptors.rows).c_str());
 	UASSERT_MSG(points.empty() || points.size() == words.size(),  uFormat("words=%d, points=%d", (int)words.size(), (int)points.size()).c_str());
 	UASSERT_MSG(keypoints.empty() || keypoints.size() == words.size(),  uFormat("words=%d, descriptors=%d", (int)words.size(), (int)keypoints.size()).c_str());
-	UASSERT(words.empty() || !keypoints.empty() || !points.empty() || !descriptors.empty());
+	//UASSERT(words.empty() || !keypoints.empty() || !points.empty() || !descriptors.empty());

 	_invalidWordsCount = 0;
 	for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
@@ -328,7 +328,7 @@ void Signature::setWords(const std::multimap<int, int> & words,
 			++_invalidWordsCount;
 		}
 		// make sure indexes are all valid!
-		UASSERT_MSG(iter->second >=0 && iter->second < (int)words.size(), uFormat("iter->second=%d words.size()=%d", iter->second, (int)words.size()).c_str());
+		UASSERT_MSG(iter->second<0 || iter->second < (int)words.size(), uFormat("iter->second=%d words.size()=%d", iter->second, (int)words.size()).c_str());
 	}

 	_enabled = false;
@@ -113,6 +113,9 @@ std::vector<cv::Point2f> Stereo::computeCorrespondences(
 StereoOpticalFlow::StereoOpticalFlow(const ParametersMap & parameters) :
 		Stereo(parameters),
 		epsilon_(Parameters::defaultStereoEps()),
+		useMinEigenVals_(Parameters::defaultStereoUseMinEigenVals()),
+		minEigThreshold_(Parameters::defaultStereoMinEigThreshold()),
+		errorThreshold_(Parameters::defaultStereoErrorThreshold()),
 		gpu_(Parameters::defaultStereoGpu())
 {
 	this->parseParameters(parameters);
@@ -122,6 +125,9 @@ void StereoOpticalFlow::parseParameters(const ParametersMap & parameters)
 {
 	Stereo::parseParameters(parameters);
 	Parameters::parse(parameters, Parameters::kStereoEps(), epsilon_);
+	Parameters::parse(parameters, Parameters::kStereoUseMinEigenVals(), useMinEigenVals_);
+	Parameters::parse(parameters, Parameters::kStereoMinEigThreshold(), minEigThreshold_);
+	Parameters::parse(parameters, Parameters::kStereoErrorThreshold(), errorThreshold_);
 	Parameters::parse(parameters, Parameters::kStereoGpu(), gpu_);
 #ifndef HAVE_OPENCV_CUDAOPTFLOW
 	if(gpu_)
@@ -171,11 +177,18 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
 				err,
 				this->winSize(),
 				this->maxLevel(),
-				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), epsilon_),
-				cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), this->epsilon()),
+				this->usingMinEigenVals() ? cv::OPTFLOW_LK_GET_MIN_EIGENVALS : 0, this->minEigThreshold());
 		UDEBUG("util2d::calcOpticalFlowPyrLKStereo() end");
 	}
-	updateStatus(leftCorners, rightCorners, status);
+	if(this->usingMinEigenVals())
+	{
+		updateStatus(leftCorners, rightCorners, status);
+	}
+	else
+	{
+		updateStatus(leftCorners, rightCorners, status, err);
+	}
 	return rightCorners;
 }

@@ -227,14 +240,17 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
 void StereoOpticalFlow::updateStatus(
 		const std::vector<cv::Point2f> & leftCorners,
 		const std::vector<cv::Point2f> & rightCorners,
-		std::vector<unsigned char> & status) const
+		std::vector<unsigned char> & status,
+		std::vector<float> err) const
 {
-	UASSERT(leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size());
+	UASSERT(
+		leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size() &&
+		(err.empty() || err.size() == leftCorners.size()));
 	int countFlowRejected = 0;
 	int countDisparityRejected = 0;
 	for(unsigned int i=0; i<status.size(); ++i)
 	{
-		if(status[i]!=0)
+		if(status[i]!=0 && (err.empty() || err[i] < this->errorThreshold()))
 		{
 			float disparity = leftCorners[i].x - rightCorners[i].x;
 			if(disparity <= this->minDisparity() || disparity > this->maxDisparity())
@@ -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;
 }

@@ -51,7 +51,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <fstream>
 #include <string>

-#define KDTREE_SIZE 4
 #define KNN_CHECKS 32

 namespace rtabmap
@@ -69,9 +68,11 @@ VWDictionary::VWDictionary(const ParametersMap & parameters) :
 	_nndrRatio(Parameters::defaultKpNndrRatio()),
 	_newDictionaryPath(Parameters::defaultKpDictionaryPath()),
 	_newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()),
+	_serializeWithChecksum(Parameters::defaultKpSerializeWithChecksum()),
 	_lastWordId(0),
 	useDistanceL1_(false),
 	_flannIndex(new FlannIndex()),
+	_modified(true),
 	_strategy(kNNBruteForce)
 {
 	this->setNNStrategy((NNStrategy)Parameters::defaultKpNNStrategy());
@@ -89,6 +90,7 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 	ParametersMap::const_iterator iter;
 	Parameters::parse(parameters, Parameters::kKpNndrRatio(), _nndrRatio);
 	Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether);
+	Parameters::parse(parameters, Parameters::kKpSerializeWithChecksum(), _serializeWithChecksum);
 	Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann);
 	Parameters::parse(parameters, Parameters::kKpFlannRebalancingFactor(), _rebalancingFactor);
 	bool byteToFloat = _byteToFloat;
@@ -160,7 +162,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 				DBDriver * driver = DBDriver::create();
 				if(driver->openConnection(dictionaryPath, false))
 				{
-					driver->load(this, false);
+					driver->load(*this, false);
 					for(std::map<int, VisualWord*>::iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter)
 					{
 						iter->second->setSaved(true);
@@ -289,6 +291,11 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 	_newDictionaryPath = dictionaryPath;
 }

+bool VWDictionary::isModified() const
+{
+	return _modified;
+}
+
 bool VWDictionary::setNNStrategy(NNStrategy strategy)
 {
 #if CV_MAJOR_VERSION < 3
@@ -484,7 +491,13 @@ void VWDictionary::update()

 	if(_notIndexedWords.size() || _visualWords.size() == 0 || _removedIndexedWords.size())
 	{
-		if(_incrementalFlann &&
+		_modified = true;
+		bool firstUpdate = _removedIndexedWords.empty() && _visualWords.size() == _notIndexedWords.size();
+		UDEBUG("firstUpdate=%s (_removedIndexedWords=%ld, _visualWords=%ld, _notIndexedWords=%ld)", 
+			firstUpdate?"true":"false", _removedIndexedWords.size(), _visualWords.size(), _notIndexedWords.size());
+
+		if(!firstUpdate &&
+			_incrementalFlann &&
 		   _strategy < kNNBruteForce &&
 		   _visualWords.size())
 		{
@@ -501,7 +514,9 @@ void VWDictionary::update()

 			if(_notIndexedWords.size())
 			{
-				ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size());
+				UTimer timer;
+				timer.start();
+				ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size(), _byteToFloat?"true":"false");
 				for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
 				{
 					VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
@@ -528,24 +543,13 @@ void VWDictionary::update()
 					int index = 0;
 					if(!_flannIndex->isBuilt())
 					{
-						UDEBUG("Building FLANN index...");
-						switch(_strategy)
-						{
-						case kNNFlannNaive:
-							_flannIndex->buildLinearIndex(descriptor, useDistanceL1_, _rebalancingFactor);
-							break;
-						case kNNFlannKdTree:
-							UASSERT_MSG(descriptor.type() == CV_32F, "To use KdTree dictionary, float descriptors are required!");
-							_flannIndex->buildKDTreeIndex(descriptor, KDTREE_SIZE, useDistanceL1_, _rebalancingFactor);
-							break;
-						case kNNFlannLSH:
-							UASSERT_MSG(descriptor.type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
-							_flannIndex->buildLSHIndex(descriptor, 12, 20, 2, _rebalancingFactor);
-							break;
-						default:
-							UFATAL("Not supposed to be here!");
-							break;
-						}
+						UDEBUG("Building FLANN index... (strategy=%s, byteToFloat=%s, useDistanceL1=%s, rebalancingFactor=%f)",
+							nnStrategyName(_strategy).c_str(), _byteToFloat?"true":"false", useDistanceL1_?"true":"false", _rebalancingFactor);
+						_flannIndex->buildIndex(
+							_strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR:
+							_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
+							FlannIndex::FLANN_INDEX_KDTREE, // kNNFlannKdTree
+							descriptor, useDistanceL1_, _rebalancingFactor);
 						UDEBUG("Building FLANN index... done!");
 					}
 					else
@@ -561,25 +565,42 @@ void VWDictionary::update()
 					inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), index));
 					UASSERT(inserted.second);
 				}
-				ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done!", (int)_notIndexedWords.size());
+				ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done! (in %f s)", (int)_notIndexedWords.size(), timer.ticks());
 			}
 		}
 		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);
@@ -657,23 +678,13 @@ void VWDictionary::update()
 				ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), dim);
 				ULOGGER_DEBUG("copying data = %f s", timer.ticks());

-				switch(_strategy)
-				{
-				case kNNFlannNaive:
-					_flannIndex->buildLinearIndex(_dataTree, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
-					break;
-				case kNNFlannKdTree:
-					UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
-					_flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
-					break;
-				case kNNFlannLSH:
-					UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
-					_flannIndex->buildLSHIndex(_dataTree, 12, 20, 2, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
-					break;
-				default:
-					break;
-				}
-
+				_flannIndex->buildIndex(
+					_strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR:
+						_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
+							FlannIndex::FLANN_INDEX_KDTREE, // kNNFlannKdTree
+					_dataTree,
+					useDistanceL1_,
+					_incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
 				ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks());
 			}
 		}
@@ -689,6 +700,146 @@ void VWDictionary::update()
 	UDEBUG("");
 }

+std::vector<unsigned char> VWDictionary::serializeIndex() const
+{
+	if(_strategy >= kNNBruteForce) {
+		UINFO("Not flann strategy, ignoring serialization...");
+		return std::vector<unsigned char>();
+	}
+	if(!_flannIndex->isBuilt() || !_removedIndexedWords.empty() || !_notIndexedWords.empty() || _visualWords.empty()) {
+		UWARN("Flann index is not buit, or there are words not indexed, cannot do serialization.");
+		return std::vector<unsigned char>();
+	}
+
+	return _flannIndex->serializeIndex(_serializeWithChecksum);
+}
+
+void VWDictionary::deserializeIndex(const std::vector<unsigned char> & data)
+{
+	deserializeIndex(data.data(), data.size());
+}
+
+void VWDictionary::deserializeIndex(const unsigned char * data, size_t size)
+{
+	if(data== NULL || size == 0)
+	{
+		UWARN("Trying to deserialize empty data, aborting.");
+		return;
+	}
+	UDEBUG("Loading flann index... (data size=%ld bytes)", size);
+	if(_strategy >= kNNBruteForce) {
+		//ignore
+		return;
+	}
+
+	if(_flannIndex->isBuilt()) {
+		UERROR("Flann index is already built, cannot deserialize data!");
+		return;
+	}
+
+	if(_visualWords.empty()) {
+		UERROR("Descriptors should be added before deserializing flann index! See VWDictionary::addWord()");
+		return;
+	}
+
+	if(!(_removedIndexedWords.empty() && _visualWords.size() == _notIndexedWords.size())) {
+		UERROR("State of dictionary not as expected before deserializing. (removed words=%ld, words=%ld, not indexed=%ld)", 
+			_removedIndexedWords.size(), _visualWords.size(), _notIndexedWords.size());
+		return;
+	}
+		
+	std::map<int, int> mapIndexId;
+	std::map<int, int> mapIdIndex;
+	cv::Mat dataTree;
+
+	UTimer timer;
+	timer.start();
+
+	int dim = _visualWords.begin()->second->getDescriptor().cols;
+	int type;
+	if(_visualWords.begin()->second->getDescriptor().type() == CV_8U)
+	{
+		useDistanceL1_ = true;
+		if(_strategy == kNNFlannKdTree)
+		{
+			type = CV_32F;
+			if(!_byteToFloat)
+			{
+				dim *= 8;
+			}
+		}
+		else
+		{
+			type = _visualWords.begin()->second->getDescriptor().type();
+		}
+	}
+	else
+	{
+		type = _visualWords.begin()->second->getDescriptor().type();
+	}
+
+	UASSERT(type == CV_32F || type == CV_8U);
+	UASSERT(dim > 0);
+
+	// Create the data matrix
+	dataTree = cv::Mat(_visualWords.size(), dim, type); // SURF descriptors are CV_32F
+	std::map<int, VisualWord*>::const_iterator iter = _visualWords.begin();
+	for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter)
+	{
+		cv::Mat descriptor;
+		if(iter->second->getDescriptor().type() == CV_8U)
+		{
+			if(_strategy == kNNFlannKdTree)
+			{
+				descriptor = convertBinTo32F(iter->second->getDescriptor(), _byteToFloat);
+			}
+			else
+			{
+				descriptor = iter->second->getDescriptor();
+			}
+		}
+		else
+		{
+			descriptor = iter->second->getDescriptor();
+		}
+
+		UASSERT_MSG(descriptor.type() == type, uFormat("%d vs %d", descriptor.type(), type).c_str());
+		UASSERT_MSG(descriptor.cols == dim, uFormat("%d vs %d", descriptor.cols, dim).c_str());
+
+		descriptor.copyTo(dataTree.row(i));
+		mapIndexId.insert(mapIndexId.end(), std::pair<int, int>(i, iter->second->id()));
+		mapIdIndex.insert(mapIdIndex.end(), std::pair<int, int>(iter->second->id(), i));
+	}
+
+	ULOGGER_DEBUG("mapIndexId.size() = %d, words.size()=%d, dim=%d", mapIndexId.size(), _visualWords.size(), dim);
+	ULOGGER_DEBUG("copying data = %f s", timer.ticks());
+
+	std::string errorMsg;
+	if(_flannIndex->loadIndex(
+		data,
+		size,
+		_strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR:
+			_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
+				FlannIndex::FLANN_INDEX_KDTREE, 
+		dataTree, 
+		useDistanceL1_,
+		_incrementalDictionary && _incrementalFlann ? _rebalancingFactor:1,
+		&errorMsg))
+	{
+		_mapIndexId = mapIndexId;
+		_mapIdIndex = mapIdIndex;
+		_dataTree = dataTree;
+		_notIndexedWords.clear();
+		_modified = false;
+	}
+	else {
+		UWARN("Failed deserializing flann index data (error: %s), the index will be rebuilt on next update.", errorMsg.c_str());
+		_flannIndex->release(); // reset to initial state
+	}
+
+	ULOGGER_DEBUG("Time to load flann index = %f s", timer.ticks());
+}
+
 void VWDictionary::clear(bool printWarningsIfNotEmpty)
 {
 	ULOGGER_DEBUG("");
@@ -718,6 +869,7 @@ void VWDictionary::clear(bool printWarningsIfNotEmpty)
 	_unusedWords.clear();
 	_flannIndex->release();
 	useDistanceL1_ = false;
+	_modified = true;
 }

 int VWDictionary::getNextId()
@@ -725,7 +877,7 @@ int VWDictionary::getNextId()
 	return ++_lastWordId;
 }

-void VWDictionary::addWordRef(int wordId, int signatureId)
+bool VWDictionary::addWordRef(int wordId, int signatureId)
 {
 	VisualWord * vw = 0;
 	vw = uValue(_visualWords, wordId, vw);
@@ -735,10 +887,12 @@ void VWDictionary::addWordRef(int wordId, int signatureId)
 		_totalActiveReferences += 1;

 		_unusedWords.erase(vw->id());
+		return true;
 	}
 	else
 	{
-		UERROR("Not found word %d (dict size=%d)", wordId, (int)_visualWords.size());
+		UWARN("Not found word %d (dict size=%d)", wordId, (int)_visualWords.size());
+		return false;
 	}
 }

@@ -784,14 +938,21 @@ std::list<int> VWDictionary::addNewWords(
 		type = _visualWords.begin()->second->getDescriptor().type();
 		UASSERT(type == CV_32F || type == CV_8U);
 	}
+	static std::string moreInfo = uFormat( 
+		"This could happen if the computer doesn't have access to same "
+		"feature detectors than when the database was created. This could "
+		"also happen if we enabled \"%s\" but the first frame received "
+		"was empty, thus features were re-extracted with a different detector "
+		"than the one used by the odometry.",
+		Parameters::kMemUseOdomFeatures().c_str());
 	if(dim && dim != descriptorsIn.cols)
 	{
-		UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", descriptorsIn.cols, dim);
+		UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary (size=%d). %s", descriptorsIn.cols, dim, moreInfo.c_str());
 		return wordIds;
 	}
 	if(type>=0 && type != descriptorsIn.type())
 	{
-		UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", descriptorsIn.type(), type);
+		UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary (type=%d). %s", descriptorsIn.type(), type, moreInfo.c_str());
 		return wordIds;
 	}

@@ -854,7 +1015,7 @@ std::list<int> VWDictionary::addNewWords(
 	if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k))
 	{
 		//Find nearest neighbors
-		UDEBUG("newPts.total()=%d ", descriptors.rows);
+		UDEBUG("newPts.total()=%d _strategy=%d", descriptors.rows, _strategy);

 		if(_strategy == kNNFlannNaive || _strategy == kNNFlannKdTree || _strategy == kNNFlannLSH)
 		{
@@ -1394,15 +1555,15 @@ void VWDictionary::addWord(VisualWord * vw)
 {
 	if(vw)
 	{
-		_visualWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
-		_notIndexedWords.insert(vw->id());
+		_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord *>(vw->id(), vw));
+		_notIndexedWords.insert(_notIndexedWords.end(), vw->id());
 		if(vw->getReferences().size())
 		{
 			_totalActiveReferences += uSum(uValues(vw->getReferences()));
 		}
 		else
 		{
-			_unusedWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
+			_unusedWords.insert(_unusedWords.end(), std::pair<int, VisualWord *>(vw->id(), vw));
 		}
 		if(_lastWordId < vw->id())
 		{
@@ -57,7 +57,7 @@ void VisualWord::addRef(int signatureId)
 	}
 	else
 	{
-		_references.insert(std::pair<int, int>(signatureId, 1));
+		_references.insert(_references.end(), std::pair<int, int>(signatureId, 1));
 	}
 	++_totalReferences;
 }
@@ -370,9 +370,10 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		matrix[2][0], matrix[2][1], matrix[2][2]);

 	std::vector<float> coeffs = calibHandler.getDistortionCoefficients(cameraId);
-	if(calibHandler.getDistortionModel(cameraId) == dai::CameraModel::Perspective)
-		distCoeffs = (cv::Mat_<double>(1,8) << coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6], coeffs[7]);
-
+	if(calibHandler.getDistortionModel(cameraId) == dai::CameraModel::Perspective) {
+		UASSERT(coeffs.size()>=14);
+		distCoeffs = (cv::Mat_<double>(1,14) << coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6], coeffs[7], coeffs[8], coeffs[9], coeffs[10], coeffs[11], coeffs[12], coeffs[13]);
+	}
 	if(alphaScaling_>-1.0f)
 		newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize_, alphaScaling_);
 	else
@@ -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);
 			}
 		}
 	}
@@ -523,19 +541,19 @@ bool CameraImages::readPoses(
 		UERROR("Cannot read pose file \"%s\".", filePath.c_str());
 		return false;
 	}
-	else if((format != 1 && format != 10 && format != 5 && format != 6 && format != 7 && format != 9) && poses.size() != this->imagesCount())
+	else if((format != 1 && format != 10 && format != 12 && format != 5 && format != 6 && format != 7 && format != 9) && poses.size() != this->imagesCount())
 	{
 		UERROR("The pose count is not the same as the images (%d vs %d)! Please remove "
 				"the pose file path if you don't want to use it (current file path=%s).",
 				(int)poses.size(), this->imagesCount(), filePath.c_str());
 		return false;
 	}
-	else if((format == 1 || format == 10 || format == 5 || format == 6 || format == 7 || format == 9) && (inOutStamps.empty() && stamps.size()!=poses.size()))
+	else if((format == 1 || format == 10 || format == 12 || format == 5 || format == 6 || format == 7 || format == 9) && (inOutStamps.empty() && stamps.size()!=poses.size()))
 	{
 		UERROR("When using RGBD-SLAM, GPS, MALAGA, ST LUCIA and EuRoC MAV formats, images must have timestamps!");
 		return false;
 	}
-	else if(format == 1 || format == 10 || format == 5 || format == 6 || format == 7 || format == 9)
+	else if(format == 1 || format == 10 || format == 12 || format == 5 || format == 6 || format == 7 || format == 9)
 	{
 		UDEBUG("");
 		//Match ground truth values with images
@@ -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();
 					}
@@ -0,0 +1,742 @@
+/*
+Copyright (c) 2010-2025, 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
+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/camera/CameraOrbbecSDK.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UThread.h>
+
+#ifdef RTABMAP_ORBBEC_SDK
+#include <libobsensor/ObSensor.hpp>
+#endif
+
+namespace rtabmap
+{
+#ifdef RTABMAP_ORBBEC_SDK
+Transform obToRtabmap(const OBExtrinsic & t)
+{
+    return Transform(t.rot[0], t.rot[1], t.rot[2], t.trans[0]/1000.0f,
+                     t.rot[3], t.rot[4], t.rot[5], t.trans[1]/1000.0f,
+                     t.rot[6], t.rot[7], t.rot[8], t.trans[2]/1000.0f);
+}
+
+
+cv::Mat obColorFrameToCv(const ob::VideoFrame & videoFrame)
+{
+    cv::Mat rgb;
+    switch(videoFrame.getFormat()) {
+    case OB_FORMAT_MJPG: {
+        cv::Mat rawMat(1, videoFrame.getDataSize(), CV_8UC1, videoFrame.getData());
+        rgb = cv::imdecode(rawMat, 1);
+    } break;
+    case OB_FORMAT_NV21: {
+        cv::Mat rawMat(videoFrame.getHeight() * 3 / 2, videoFrame.getWidth(), CV_8UC1, videoFrame.getData());
+        cv::cvtColor(rawMat, rgb, cv::COLOR_YUV2BGR_NV21);
+    } break;
+    case OB_FORMAT_YUYV:
+    case OB_FORMAT_YUY2: {
+        cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC2, videoFrame.getData());
+        cv::cvtColor(rawMat, rgb, cv::COLOR_YUV2BGR_YUY2);
+    } break;
+    case OB_FORMAT_BGR: {
+        cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC3, videoFrame.getData());
+        cv::cvtColor(rawMat, rgb, cv::COLOR_BGR2RGB);
+    } break;
+    case OB_FORMAT_RGB: {
+        cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC3, videoFrame.getData());
+        cv::cvtColor(rawMat, rgb, cv::COLOR_RGB2BGR);
+    } break;
+    case OB_FORMAT_RGBA: {
+        cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC4, videoFrame.getData());
+        cv::cvtColor(rawMat, rgb, cv::COLOR_RGBA2BGR);
+    } break;
+    case OB_FORMAT_BGRA: {
+        cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC4, videoFrame.getData());
+        cv::cvtColor(rawMat, rgb, cv::COLOR_BGRA2RGB);
+    } break;
+    case OB_FORMAT_UYVY: {
+        cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC2, videoFrame.getData());
+        cv::cvtColor(rawMat, rgb, cv::COLOR_YUV2BGR_UYVY);
+    } break;
+    case OB_FORMAT_I420: {
+        cv::Mat rawMat(videoFrame.getHeight() * 3 / 2, videoFrame.getWidth(), CV_8UC1, videoFrame.getData());
+        cv::cvtColor(rawMat, rgb, cv::COLOR_YUV2BGR_I420);
+    } break;
+    case OB_FORMAT_Y8: {
+        rgb = cv::Mat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC1, videoFrame.getData()).clone();
+    } break;
+    case OB_FORMAT_Y16: {
+        cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_16UC1, videoFrame.getData());
+        rawMat.convertTo(rgb, CV_8UC1, 255.0 / 65535.0);
+    } break;
+    default:
+        break;
+    }
+    return rgb;
+}
+
+cv::Mat obDepthFrameToCv(const ob::DepthFrame & depthFrame)
+{
+    cv::Mat depth;
+    if(depthFrame.getFormat() == OB_FORMAT_Y16 || depthFrame.getFormat() == OB_FORMAT_Z16 || depthFrame.getFormat() == OB_FORMAT_Y12C4) {
+        cv::Mat rawMat = cv::Mat(depthFrame.getHeight(), depthFrame.getWidth(), CV_16UC1, depthFrame.getData());
+        float scale = depthFrame.getValueScale() / 1000.0f;
+        rawMat.convertTo(depth, CV_32F, scale);
+    }
+    return depth;
+}
+
+cv::Mat obIntrinsicToK(const OBCameraIntrinsic & intrinsics)
+{
+    cv::Mat K = cv::Mat::eye(3,3,CV_64FC1);
+    K.at<double>(0,0) = intrinsics.fx;
+    K.at<double>(1,1) = intrinsics.fy;
+    K.at<double>(0,2) = intrinsics.cx;
+    K.at<double>(1,2) = intrinsics.cy;
+    return K;
+}
+cv::Mat obIntrinsicToP(const OBCameraIntrinsic & intrinsics)
+{
+    cv::Mat P = cv::Mat::eye(3,4,CV_64FC1);
+    obIntrinsicToK(intrinsics).copyTo(P.colRange(0,3));
+    return P;
+}
+cv::Mat obDistortionToD(const OBCameraDistortion & distortion)
+{
+    cv::Mat D = cv::Mat(1,8,CV_64FC1);
+    D.at<double>(0,0) = distortion.k1;
+    D.at<double>(0,1) = distortion.k2;
+    D.at<double>(0,2) = distortion.p1;
+    D.at<double>(0,3) = distortion.p2;
+    D.at<double>(0,4) = distortion.k3;
+    D.at<double>(0,5) = distortion.k4;
+    D.at<double>(0,6) = distortion.k5;
+    D.at<double>(0,7) = distortion.k6;
+    if(distortion.k4 == 0 && distortion.k5 == 0 && distortion.k6 == 0)
+    {
+        D = D.colRange(0,5);
+    }
+    return D;
+}
+#endif
+
+bool CameraOrbbecSDK::available()
+{
+#ifdef RTABMAP_ORBBEC_SDK
+    return true;
+#else
+    return false;
+#endif
+}
+
+CameraOrbbecSDK::CameraOrbbecSDK(
+        std::string deviceId,
+        unsigned int colorWidth,
+        unsigned int colorHeight,
+        unsigned int depthWidth,
+        unsigned int depthHeight,
+        float imageRate,
+        const Transform & localTransform) :
+            Camera(imageRate, localTransform)
+#ifdef RTABMAP_ORBBEC_SDK
+            , deviceId_(deviceId),
+            colorWidth_(colorWidth),
+            colorHeight_(colorHeight),
+            depthWidth_(depthWidth),
+            depthHeight_(depthHeight),
+            pipeline_(nullptr),
+            imuPipeline_(nullptr),
+            alignFilter_(nullptr),
+            imuLocalTransformInitialized_(false),
+            lastAccStamp_(0),
+            lastImageStamp_(0),
+            globalTimestampAvailable_(false),
+            rectifyColor_(false),
+            convertDepthToMM_(true),
+            imuPublished_(true)
+#endif
+{
+}
+
+CameraOrbbecSDK::~CameraOrbbecSDK()
+{
+#ifdef RTABMAP_ORBBEC_SDK
+    this->close();
+#endif
+}
+
+void CameraOrbbecSDK::close()
+{
+#ifdef RTABMAP_ORBBEC_SDK
+    if(imuPipeline_) {
+        imuPipeline_->stop();
+        delete imuPipeline_;
+        imuPipeline_=nullptr;
+    }
+    
+    if(pipeline_) {
+        pipeline_->stop();
+        delete pipeline_;
+        pipeline_=nullptr;
+    }
+    delete alignFilter_;
+    alignFilter_ = nullptr;
+    imuLocalTransform_ = Transform();
+    imuLocalTransformInitialized_ = false;
+    lastAccStamp_ = 0;
+    lastImageStamp_ = 0;
+    globalTimestampAvailable_ = false;
+    model_ = CameraModel();
+    imuBuffer_.clear();
+#endif
+}
+
+bool CameraOrbbecSDK::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+#ifdef RTABMAP_ORBBEC_SDK
+    this->close();
+
+    std::shared_ptr<ob::Device> device;
+
+    ob::Context context;
+    auto devices = context.queryDeviceList();
+    UINFO("%d device(s) found", devices->getCount());
+    for(uint32_t i=0; i<devices->getCount(); ++i)
+    {
+        auto currentDevice = devices->getDevice(i);
+        auto info = currentDevice->getDeviceInfo();
+
+        if(deviceId_.find('-') != std::string::npos)
+        {
+            // UID
+            if(deviceId_.compare(info->getUid()) == 0) {
+                device = currentDevice;
+            }
+        }
+        else if(uSplitNumChar(deviceId_).size() > 1)
+        {
+            // Serial
+            if(deviceId_.compare(info->getSerialNumber()) == 0) {
+                device = currentDevice;
+            }
+        }
+        else if((deviceId_.empty() && i==0) ||
+                (!deviceId_.empty() && uStr2Int(deviceId_) == (int)i)) {
+            // Index
+            device = currentDevice;
+        }
+
+        std::string type = "Unknown";
+        switch(info->getDeviceType()) 
+        {
+            case OB_STRUCTURED_LIGHT_MONOCULAR_CAMERA:
+                type = "Structured Light Monocular Camera";
+                break;
+            case OB_STRUCTURED_LIGHT_BINOCULAR_CAMERA:
+                type = "Structured Light Binocular Camera";
+                break;
+            case OB_TOF_CAMERA:
+                type = "TOF Camera";
+                break;
+            default:
+                break;
+        }
+        UINFO("Device %ld:", i);
+        UINFO("   Name: %s", info->getName());
+        UINFO("   Type: %s", type.c_str());
+        UINFO("   Serial: %s", info->getSerialNumber());
+        UINFO("   UID: %s", info->getUid());
+        UINFO("   Chip: %s", info->getAsicName());
+        UINFO("   Hardware version: %s", info->getHardwareVersion());
+        UINFO("   Firmware version: %s", info->getFirmwareVersion());
+    }
+
+    if(device.get() == nullptr) {
+        if(deviceId_.empty()) {
+            UERROR( "Could not find any orbbec compatible devices! Verify that the "
+                    "camera is correctly connected and the udev rules are installed.");
+        }
+        else {
+            UERROR("Could not find an orbbec device with ID \"%s\"! Verify that the "
+                   "camera is correctly connected and the udev rules are installed. "
+                   "Unset the ID to choose the first camera found.");
+        }
+        
+        return false;
+    }
+
+    bool hasGyro = false;
+    bool hasAccel = false;
+    auto sensors = device->getSensorList();
+
+    if(device->isGlobalTimestampSupported())
+    {
+        UINFO("Global (host time sync) timestamp is supported.");
+        device->enableGlobalTimestamp(true);
+        globalTimestampAvailable_ = true;
+    }
+    else
+    {
+        UWARN("Global (host time sync) timestamp is not supported! We will use device timestamp, so the camera frames won't be synchronizable with other sensors.");
+    }
+
+    uint32_t maxColorFps = 0;
+    uint32_t maxDepthFps = 0;
+    for(uint32_t i=0; i<sensors->getCount(); ++i)
+    {
+        if(sensors->getSensorType(i) == OB_SENSOR_GYRO)
+        {
+            hasGyro = true;
+        }
+        if(sensors->getSensorType(i) == OB_SENSOR_ACCEL)
+        {
+            hasAccel = true;
+        }
+        if( sensors->getSensorType(i) == OB_SENSOR_DEPTH ||
+            sensors->getSensorType(i) == OB_SENSOR_COLOR)
+        {
+            auto profiles = sensors->getSensor(i)->getStreamProfileList();
+            UINFO("Supported %s profiles:", sensors->getSensorType(i) == OB_SENSOR_DEPTH?"depth":"color");
+            for(uint32_t j=0; j<profiles->getCount(); ++j)
+            {
+                auto profile = profiles->getProfile(j)->as<ob::VideoStreamProfile>();
+                UINFO("Resolution: %ldx%ld, FPS: %ld, Format: %d", 
+                    profile->getWidth(), profile->getHeight(), profile->getFps(), profile->getFormat(), j==0?" (default)":"");
+                
+                // Get maximum frame rate based on resolution selected
+                if(sensors->getSensorType(i) == OB_SENSOR_DEPTH) {
+                    if( profile->getFps() > maxDepthFps &&
+                        depthWidth_ == profile->getWidth() &&
+                        depthHeight_ == profile->getHeight())
+                    {
+                        maxDepthFps = profile->getFps();
+                    }
+                }
+                else
+                {
+                    if( profile->getFps() > maxColorFps &&
+                        colorWidth_ == profile->getWidth() &&
+                        colorHeight_ == profile->getHeight())
+                    {
+                        maxColorFps = profile->getFps();
+                    }
+                }            
+            }
+        }
+    }
+
+    // Note that for TOF camera, we want maximum frame rate to better 
+    // sync rgb and depth. For stereo cameras, use the specified frame rate.
+    if(this->getImageRate()!=0.0f && device->getDeviceInfo()->getDeviceType() != OB_TOF_CAMERA)
+    {
+        maxColorFps = maxDepthFps = (unsigned int)this->getImageRate();
+        this->setImageRate(0);
+    }
+
+    std::shared_ptr<ob::Config> imuConfig;
+    if(imuPublished_)
+    {
+        if(hasGyro && hasAccel)
+        {
+            imuPipeline_ = new ob::Pipeline(device);
+            imuConfig = std::make_shared<ob::Config>();
+            imuConfig->enableGyroStream();
+            imuConfig->enableAccelStream();
+            try {
+                UINFO("Starting imu pipeline");
+                imuPipeline_->start(imuConfig, [&](std::shared_ptr<ob::FrameSet> frameSet) {
+                    if(frameSet->getCount() != 2)
+                    {
+                        return;
+                    }
+
+                    if(!imuLocalTransformInitialized_)
+                    {
+                        return;
+                    }
+
+                    UASSERT(frameSet->getFrame(OB_FRAME_ACCEL) != nullptr && 
+                            frameSet->getFrame(OB_FRAME_GYRO) != nullptr);
+
+                    auto accel = frameSet->getFrame(OB_FRAME_ACCEL)->as<const ob::AccelFrame>();
+                    auto gyro = frameSet->getFrame(OB_FRAME_GYRO)->as<const ob::GyroFrame>();
+
+                    uint64_t accelStampUs = globalTimestampAvailable_?accel->getGlobalTimeStampUs():accel->getTimeStampUs();
+                    uint64_t gyroStampUs = globalTimestampAvailable_?gyro->getGlobalTimeStampUs():gyro->getTimeStampUs();
+    
+                    if(accelStampUs != gyroStampUs)
+                    {
+                        UWARN("Received accel and gyro frames with different timestamps (%llu vs %llu), skipping.",
+                            accelStampUs, gyroStampUs);
+                        return;
+                    }
+                    
+                    double accStamp = double(accelStampUs)/1e6;
+
+                    if(accelStampUs <= lastAccStamp_) {
+                        return;
+                    }
+
+                    lastAccStamp_ = accelStampUs;
+
+                    auto accelValue = accel->getValue();
+                    auto gyroValue = gyro->getValue();
+                    if(isInterIMUPublishing())
+                    {
+                        IMU imu(cv::Vec3f(gyroValue.x, gyroValue.y, gyroValue.z), cv::Mat::eye(3,3,CV_64FC1),
+                                cv::Vec3f(accelValue.x, accelValue.y, accelValue.z), cv::Mat::eye(3,3,CV_64FC1),
+                                imuLocalTransform_);
+                        this->postInterIMU(imu, accStamp);
+                    }
+                    else
+                    {
+                        UScopeMutex lock(imuMutex_);
+                        imuBuffer_.emplace_hint(imuBuffer_.end(), accStamp, cv::Vec6f(gyroValue.x, gyroValue.y, gyroValue.z, accelValue.x, accelValue.y, accelValue.z));
+                        if(imuBuffer_.size()>1000) {
+                            imuBuffer_.erase(imuBuffer_.begin());
+                        }
+                    }
+                });
+            }
+            catch(const ob::Error & e) {
+                UERROR("Unexpected error when configuring IMU stream: %s", e.what());
+            }
+        }
+        else
+        {
+            UWARN("IMU option is enabled but the camera doesn't have an IMU, ignoring.");
+        }
+    }
+
+    pipeline_ = new ob::Pipeline(device);
+    auto config = std::make_shared<ob::Config>();
+
+    // Set highest frame rate possible to reduce color/depth sync diff
+    config->enableVideoStream(OB_STREAM_COLOR, colorWidth_, colorHeight_, maxColorFps, OB_FORMAT_RGB);
+    config->enableVideoStream(OB_STREAM_DEPTH, depthWidth_, depthHeight_, maxDepthFps, OB_FORMAT_Y16);
+
+    UINFO("Using color profile: %dx%d", colorWidth_, colorHeight_);
+    UINFO("Using depth profile: %dx%d", depthWidth_, depthHeight_);
+
+    config->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_ALL_TYPE_FRAME_REQUIRE);
+
+    config->setAlignMode(ALIGN_DISABLE);
+    config->setDepthScaleRequire(true);
+
+    pipeline_->enableFrameSync();
+
+    try {
+        UINFO("Starting camera pipeline");
+           pipeline_->start(config);
+
+        auto enabledStreams = pipeline_->getConfig()->getEnabledStreamProfileList();
+        if(imuPipeline_ != nullptr) {
+            for(uint32_t i=0; i<enabledStreams->getCount() && !imuLocalTransformInitialized_; ++i)
+            {
+                if(enabledStreams->getProfile(i)->getType() == OB_STREAM_COLOR)
+                {
+                    auto enabledImuStreams = imuPipeline_->getConfig()->getEnabledStreamProfileList();
+                    for(uint32_t j=0; j<enabledImuStreams->getCount(); ++j)
+                    {
+                        if(enabledImuStreams->getProfile(j)->getType() == OB_STREAM_ACCEL)
+                        {
+                            auto extrinsics = enabledStreams->getProfile(i)->as<ob::VideoStreamProfile>()->getExtrinsicTo(enabledImuStreams->getProfile(j)->as<ob::AccelStreamProfile>());
+                            // base -> color -> imu
+                            imuLocalTransform_ = this->getLocalTransform() * obToRtabmap(extrinsics);
+                            UINFO("IMU local transform: %s", imuLocalTransform_.prettyPrint().c_str());
+                            imuLocalTransformInitialized_ = true;
+                            break;
+                        }
+                    }
+                }
+            }
+        }
+
+        std::shared_ptr<ob::StreamProfile> colorProfile;
+        std::shared_ptr<ob::StreamProfile> depthProfile;
+        for(uint32_t i=0; i<enabledStreams->getCount(); ++i)
+        {
+            if(enabledStreams->getProfile(i)->getType() == OB_STREAM_COLOR) {
+                colorProfile = enabledStreams->getProfile(i);
+            }
+            else if(enabledStreams->getProfile(i)->getType() == OB_STREAM_DEPTH) {
+                depthProfile = enabledStreams->getProfile(i);
+            }
+        }
+        bool currentSelectionSupportsHwD2C = false;
+        auto hwD2CSupportedDepthStreamProfiles = pipeline_->getD2CDepthProfileList(colorProfile, ALIGN_D2C_HW_MODE);
+        if(hwD2CSupportedDepthStreamProfiles->count() == 0) {
+            UWARN("Current color profile selected doesn't support any hardware depth to color registration. Software registration is done instead.");
+        }
+        else
+        {
+            auto depthVsp = depthProfile->as<ob::VideoStreamProfile>();
+            auto count    = hwD2CSupportedDepthStreamProfiles->getCount();
+            for(uint32_t i = 0; i < count; i++) {
+                auto vsp = hwD2CSupportedDepthStreamProfiles->getProfile(i)->as<ob::VideoStreamProfile>();
+                UINFO("Supported depth to color format: Resolution: %ldx%ld, FPS: %ld, Format: %d", vsp->getWidth(), vsp->getHeight(), vsp->getFps(), vsp->getFormat(), i==0?" (default)":"");
+                if(vsp->getWidth() == depthVsp->getWidth() && vsp->getHeight() == depthVsp->getHeight() && vsp->getFormat() == depthVsp->getFormat()
+                && vsp->getFps() == depthVsp->getFps()) {
+                    currentSelectionSupportsHwD2C = true;
+                }
+            }
+        }
+        if(!currentSelectionSupportsHwD2C) {
+            UWARN("Hardware depth to color registration cannot be done with the selected color and depth profiles. "
+                  "Software registration is done instead, so more CPU will be needed on the host computer. "
+                  "Set logger level to info to see comptible depth formats for the selected color profile.");
+            alignFilter_ = new ob::Align(OB_STREAM_COLOR);
+            alignFilter_->setMatchTargetResolution(false);
+        }
+        else {
+            UINFO("Enabling hardware depth to color registration!");
+            config->setAlignMode(ALIGN_D2C_HW_MODE);
+            config->setDepthScaleRequire(false);
+            pipeline_->stop();
+            pipeline_->start(config);
+        }
+    }
+    catch(const ob::Error & e)
+    {
+        UERROR("Configuration not supported! Exception: %s", e.what());
+        UERROR("Supported formats:");
+        for(uint32_t i=0; i<sensors->getCount(); ++i)
+        {
+            if( sensors->getSensorType(i) == OB_SENSOR_DEPTH ||
+                sensors->getSensorType(i) == OB_SENSOR_COLOR)
+            {
+                auto profiles = sensors->getSensor(i)->getStreamProfileList();
+                for(uint32_t j=0; j<profiles->getCount(); ++j)
+                {
+                    auto profile = profiles->getProfile(j)->as<ob::VideoStreamProfile>();
+                    UERROR("%sResolution: %ldx%ld, FPS: %ld, Format: %d", 
+                        sensors->getSensorType(i) == OB_SENSOR_DEPTH?"Depth":"Color", 
+                        profile->getWidth(), 
+                        profile->getHeight(), 
+                        profile->getFps(), 
+                        profile->getFormat(), 
+                        j==0?" (default)":"");
+                }
+            }
+        }
+        return false;
+    }
+    
+    return true;
+#else
+    UERROR("CameraOrbbecSDK: RTAB-Map is not built with Orbbec SDK support!");
+    return false;
+#endif
+}
+
+bool CameraOrbbecSDK::isCalibrated() const
+{
+    return true;
+}
+
+std::string CameraOrbbecSDK::getSerial() const
+{
+#ifdef RTABMAP_ORBBEC_SDK
+    if(pipeline_) {
+        return pipeline_->getDevice()->getDeviceInfo()->getSerialNumber();
+    }
+#endif
+    return "";
+}
+
+void CameraOrbbecSDK::enableColorRectification(bool enabled)
+{
+#ifdef RTABMAP_ORBBEC_SDK
+    rectifyColor_ = enabled;
+#endif
+}
+
+void CameraOrbbecSDK::enableImu(bool enabled)
+{
+#ifdef RTABMAP_ORBBEC_SDK
+    imuPublished_ = enabled;
+#endif
+}
+
+void CameraOrbbecSDK::enableDepthMM(bool enabled)
+{
+#ifdef RTABMAP_ORBBEC_SDK
+    convertDepthToMM_ = enabled;
+#endif
+}
+
+SensorData CameraOrbbecSDK::captureImage(SensorCaptureInfo * info)
+{
+    SensorData data;
+
+#ifdef RTABMAP_ORBBEC_SDK
+    if(!pipeline_) {
+        UERROR("Camera is not initialized!");
+        return data;
+    }
+    auto frameset = pipeline_->waitForFrameset();
+    if(frameset == nullptr || frameset->getCount() == 0) {
+        UWARN("No frame received!");
+        return data;
+    }
+    if(frameset->getCount() != 2) {
+        UWARN("Received %s frames, expecting 2!", frameset->getCount());
+        return data;
+    }
+
+    if(alignFilter_ != nullptr) {
+        // Software depth to color registration
+        frameset = alignFilter_->process(frameset)->as<ob::FrameSet>();
+        UASSERT(frameset != nullptr);
+    }
+
+    auto colorFrame = frameset->getFrame(OB_FRAME_COLOR);
+    UASSERT(colorFrame != nullptr);
+    
+    auto depthFrame = frameset->getFrame(OB_FRAME_DEPTH);
+    UASSERT(depthFrame != nullptr);
+
+    auto colorVideoFrame = colorFrame->as<const ob::VideoFrame>();
+    auto depthVideoFrame = depthFrame->as<const ob::DepthFrame>();
+
+    cv::Mat rgb = obColorFrameToCv(*colorVideoFrame);
+    cv::Mat depth = obDepthFrameToCv(*depthVideoFrame);
+
+    if(rgb.empty()) {
+        UERROR("Could not convert the color frame! Type=%d Format=%d", colorFrame->getType(), colorVideoFrame->getFormat());
+    }
+    else if(depth.empty()) {
+        UERROR("Could not convert the depth frame! Type=%d Format=%d", depthFrame->getType(), depthVideoFrame->getFormat());
+    }
+    else if(!rgb.empty() && !depth.empty())
+    {
+        if(!model_.isValidForProjection())
+        {
+            auto streamProfile = colorFrame->getStreamProfile();
+            auto videoStreamProfile = streamProfile->as<ob::VideoStreamProfile>();
+
+            auto intrinsics = videoStreamProfile->getIntrinsic();
+            model_ = CameraModel(
+                getSerial(), 
+                cv::Size(intrinsics.width, intrinsics.height),
+                obIntrinsicToK(intrinsics),
+                obDistortionToD(videoStreamProfile->getDistortion()),
+                cv::Mat::eye(3,3,CV_64FC1),
+                obIntrinsicToP(intrinsics),
+                this->getLocalTransform());
+            if(rectifyColor_ && !model_.initRectificationMap()) {
+                UWARN("Could not initialize rectification map, color images won't be rectified.");
+            }
+        }
+    
+        if(rectifyColor_ && model_.isValidForRectification())
+        {
+            rgb = model_.rectifyImage(rgb);
+        }
+
+        if(convertDepthToMM_)
+        {
+            depth = util2d::cvtDepthFromFloat(depth);
+        }
+
+        uint64_t colorStampUs = globalTimestampAvailable_?colorFrame->getGlobalTimeStampUs():colorFrame->getTimeStampUs();
+        uint64_t depthStampUs = globalTimestampAvailable_?depthFrame->getGlobalTimeStampUs():depthFrame->getTimeStampUs();
+        double colorStamp = double(colorStampUs) / 1e6;
+        double depthStamp = double(depthStampUs) / 1e6;
+        if(fabs(colorStamp - depthStamp) > 0.018) {
+            // The difference seems varying between 0 and 17 ms normally
+            UWARN("Large timestamp difference (%fs) between color (%f) and depth (%f) frames. "
+                "Depth registration would be wrong on fast motion.", 
+                colorStamp - depthStamp, colorStamp, depthStamp);
+        }
+
+        uint64_t stampUs = colorStampUs < depthStampUs ? colorStampUs : depthStampUs;
+
+#ifdef WIN32
+        // On Windows, there is an issue that timestamps are not populated by default without following instructions from:
+        // https://github.com/orbbec/OrbbecSDK_v2/blob/main/scripts/env_setup/obsensor_metadata_win10.md
+        // Detect if the consecutive timestamps are identical, then send error!
+        if (stampUs <= lastImageStamp_)
+        {
+            UERROR("We detected non-consecutive timestamps, make sure you applied the fix from https://github.com/orbbec/OrbbecSDK_v2/blob/main/scripts/env_setup/obsensor_metadata_win10.md .");
+        }
+        lastImageStamp_ = stampUs;
+#endif
+        double stamp = double(stampUs)/1e6;
+
+        data = SensorData(rgb, depth, model_, this->getNextSeqID(), stamp);
+
+        if(imuPublished_ && !imuBuffer_.empty() && !this->isInterIMUPublishing())
+        {
+            cv::Vec6f imuVec;
+            std::map<double, cv::Vec6f>::const_iterator iterA, iterB;
+        
+            imuMutex_.lock();
+            int maximumTries = 10;
+            while(imuBuffer_.rbegin()->first < stamp && maximumTries-- > 0)
+            {
+                imuMutex_.unlock();
+                uSleep(1);
+                imuMutex_.lock();
+            }
+
+            if(imuBuffer_.rbegin()->first < stamp)
+            {
+                UWARN("Could not get IMU data at request image stamp %f after waiting 10 ms, latest imu stamp is %f", stamp, imuBuffer_.rbegin()->first);
+                imuMutex_.unlock();
+            }
+            else
+            {
+                // Interpolate imu data on image stamp
+                iterB = imuBuffer_.lower_bound(stamp);
+                iterA = iterB;
+                if(iterA != imuBuffer_.begin())
+                    iterA = --iterA;
+                if(iterA == iterB || stamp == iterB->first)
+                {
+                    imuVec = iterB->second;
+                }
+                else if(stamp > iterA->first && stamp < iterB->first)
+                {
+                    float t = (stamp-iterA->first) / (iterB->first-iterA->first);
+                    imuVec = iterA->second + t*(iterB->second - iterA->second);
+                }
+                imuBuffer_.erase(imuBuffer_.begin(), iterB);
+
+                imuMutex_.unlock();
+                data.setIMU(IMU(cv::Vec3d(imuVec[0], imuVec[1], imuVec[2]), cv::Mat::eye(3, 3, CV_64FC1), cv::Vec3d(imuVec[3], imuVec[4], imuVec[5]), cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_));
+            }
+        }
+    }
+
+#else
+    UERROR("CameraOrbbecSDK: RTAB-Map is not built with Orbbec SDK support!");
+#endif
+    return data;
+}
+
+} // namespace rtabmap
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UThreadC.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UDirectory.h>
 #include <opencv2/imgproc/types_c.h>

 #ifdef RTABMAP_REALSENSE2
@@ -78,7 +79,8 @@ CameraRealSense2::CameraRealSense2(
 	cameraDepthFps_(30),
 	globalTimeSync_(true),
 	dualMode_(false),
-	closing_(false)
+	closing_(false),
+	playback_(false)
 #endif
 {
 	UDEBUG("");
@@ -130,6 +132,7 @@ void CameraRealSense2::close()
 	}

 	closing_ = false;
+	playback_ = false;
 }

 void CameraRealSense2::imu_callback(rs2::frame frame)
@@ -492,64 +495,76 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 	clockSyncWarningShown_ = false;
 	imuGlobalSyncWarningShown_ = false;

-	rs2::device_list list = ctx_.query_devices();
-	if (0 == list.size())
+	if(uStrContains(deviceId_, ".bag"))
 	{
-		UERROR("No RealSense2 devices were found!");
-		return false;
+		// playback (bag recorded by realsense-viewer)
+		dev_.resize(1);
+		dev_[0] = ctx_.load_device(uReplaceChar(deviceId_, '~', UDirectory::homeDir()));
+		playback_ = true;
+		UINFO("Device ID is a bag (\"%s\"), using playback mode", deviceId_.c_str());
 	}
-
-	bool found=false;
-	try
+	else
 	{
-		for (rs2::device dev : list)
+		rs2::device_list list = ctx_.query_devices();
+		if (0 == list.size())
 		{
-			auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
-			auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
-			auto name = dev.get_info(RS2_CAMERA_INFO_NAME);
+			UERROR("No RealSense2 devices were found!");
+			return false;
+		}

-			uint16_t pid;
-			std::stringstream ss;
-			ss << std::hex << pid_str;
-			ss >> pid;
-			UINFO("Device \"%s\" with serial number %s was found with product ID=%d.", name, sn, (int)pid);
-			if(dualMode_ && pid == 0x0B37)
+		bool found=false;
+		try
+		{
+			for (rs2::device dev : list)
 			{
-				// Dual setup: device[0] = D400, device[1] = T265
-				// T265
-				dev_.resize(2);
-				dev_[1] = dev;
-			}
-			else if (!found && (deviceId_.empty() || deviceId_ == sn || uStrContains(name, uToUpperCase(deviceId_))))
-			{
-				if(dev_.empty())
+				auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
+				auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
+				auto name = dev.get_info(RS2_CAMERA_INFO_NAME);
+
+				uint16_t pid;
+				std::stringstream ss;
+				ss << std::hex << pid_str;
+				ss >> pid;
+				UINFO("Device \"%s\" with serial number %s was found with product ID=%d.", name, sn, (int)pid);
+				if(dualMode_ && pid == 0x0B37)
 				{
-					dev_.resize(1);
+					// Dual setup: device[0] = D400, device[1] = T265
+					// T265
+					dev_.resize(2);
+					dev_[1] = dev;
+				}
+				else if (!found && (deviceId_.empty() || deviceId_ == sn || uStrContains(name, uToUpperCase(deviceId_))))
+				{
+					if(dev_.empty())
+					{
+						dev_.resize(1);
+					}
+					dev_[0] = dev;
+					found=true;
 				}
-				dev_[0] = dev;
-				found=true;
 			}
 		}
-	}
-	catch(const rs2::error & error)
-	{
-		UWARN("%s. Is the camera already used with another app?", error.what());
+		catch(const rs2::error & error)
+		{
+			UWARN("%s. Is the camera already used with another app?", error.what());
+		}
+
+		if (!found)
+		{
+			if(dualMode_ && dev_.size()==2)
+			{
+				UERROR("Dual setup is enabled, but a D400 camera is not detected!");
+				dev_.clear();
+			}
+			else
+			{
+				UERROR("The requested device \"%s\" is NOT found!", deviceId_.c_str());
+			}
+			return false;
+		}
 	}

-	if (!found)
-	{
-		if(dualMode_ && dev_.size()==2)
-		{
-			UERROR("Dual setup is enabled, but a D400 camera is not detected!");
-			dev_.clear();
-		}
-		else
-		{
-			UERROR("The requested device \"%s\" is NOT found!", deviceId_.c_str());
-		}
-		return false;
-	}
-	else if(dualMode_ && dev_.size()!=2)
+	if(dualMode_ && dev_.size()!=2)
 	{
 		UERROR("Dual setup is enabled, but a T265 camera is not detected!");
 		dev_.clear();
@@ -634,7 +649,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			sensors[1] = elem;
 			if(sensors[1].supports(rs2_option::RS2_OPTION_EMITTER_ENABLED))
 			{
-				sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_);
+				if(!sensors[1].is_option_read_only(rs2_option::RS2_OPTION_EMITTER_ENABLED))
+				{
+					sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_);
+				}
+				else if(!emitterEnabled_)
+				{
+					UWARN("rs2_option::RS2_OPTION_EMITTER_ENABLED option is read-only, cannot disable IR emitter.");
+				}
 			}
 		}
 		else if ("Coded-Light Depth Sensor" == module_name)
@@ -712,14 +734,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			for (auto& profile : profiles)
 			{
 				auto video_profile = profile.as<rs2::video_stream_profile>();
-				UINFO("%s %d %d %d %d %s type=%d", rs2_format_to_string(
-						video_profile.format()),
-						video_profile.width(),
-						video_profile.height(),
-						video_profile.fps(),
-						video_profile.stream_index(),
-						video_profile.stream_name().c_str(),
-						video_profile.stream_type());
+				UINFO("%s %d %d %d %d %s type=%d", 
+						rs2_format_to_string(profile.format()),
+						video_profile.get()?video_profile.width():-1,
+						video_profile.get()?video_profile.height():-1,
+						profile.fps(),
+						profile.stream_index(),
+						profile.stream_name().c_str(),
+						profile.stream_type());
 			}
 		}
 		int pi = 0;
@@ -728,10 +750,12 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			auto video_profile = profile.as<rs2::video_stream_profile>();
 			if(!stereo)
 			{
-				if( (video_profile.width()  == cameraWidth_ &&
+				if( (video_profile.get() &&
+					 video_profile.width()  == cameraWidth_ &&
 					 video_profile.height() == cameraHeight_ &&
 					 video_profile.fps()    == cameraFps_) ||
-						(strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 &&
+						((strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 || 
+						  (playback_ && strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "Stereo Module")==0)) &&
 							video_profile.width()  == cameraDepthWidth_ &&
 							video_profile.height() == cameraDepthHeight_ &&
 							video_profile.fps()    == cameraDepthFps_))
@@ -740,7 +764,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st

 					// rgb or ir left
 					if((!ir_ && video_profile.format() == RS2_FORMAT_RGB8 && video_profile.stream_type() == RS2_STREAM_COLOR) ||
-					  (ir_ && video_profile.format() == RS2_FORMAT_Y8 && (video_profile.stream_index() == 1 || isL500)))
+					   (ir_ && video_profile.format() == RS2_FORMAT_Y8 && (video_profile.stream_index() == 1 || isL500)))
 					{
 						if(!profilesPerSensor[i].empty())
 						{
@@ -778,7 +802,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 						}
 					}
 				}
-				else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF)
+				else if(profile.format() == RS2_FORMAT_MOTION_XYZ32F || profile.format() == RS2_FORMAT_6DOF)
 				{
 					//D435i:
 					//MOTION_XYZ32F 0 0 200 (gyro)
@@ -817,6 +841,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			{
 				//T265:
 				if(!dualMode_ &&
+					video_profile.get() &&
 					video_profile.format() == RS2_FORMAT_Y8 &&
 					video_profile.width()  == 848 &&
 					video_profile.height() == 800 &&
@@ -865,7 +890,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 					}
 					added = true;
 				}
-				else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF)
+				else if(profile.format() == RS2_FORMAT_MOTION_XYZ32F || profile.format() == RS2_FORMAT_6DOF)
 				{
 					//MOTION_XYZ32F 0 0 200
 					//MOTION_XYZ32F 0 0 62
@@ -878,20 +903,29 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		}
 		if (!added)
 		{
-			UERROR("Given stream configuration is not supported by the device! "
-					"Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraWidth_, cameraHeight_, cameraFps_);
+			if(strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 || 
+			   (playback_ && strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "Stereo Module")==0))
+			{
+				UERROR("Given stream configuration is not supported by the device! "
+						"Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraDepthWidth_, cameraDepthHeight_, cameraDepthFps_);
+			}
+			else
+			{
+				UERROR("Given stream configuration is not supported by the device! "
+						"Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraWidth_, cameraHeight_, cameraFps_);
+			}
 			UERROR("Available configurations:");
 			for (auto& profile : profiles)
 			{
 				auto video_profile = profile.as<rs2::video_stream_profile>();
-				UERROR("%s %d %d %d %d %s type=%d", rs2_format_to_string(
-						video_profile.format()),
-						video_profile.width(),
-						video_profile.height(),
-						video_profile.fps(),
-						video_profile.stream_index(),
-						video_profile.stream_name().c_str(),
-						video_profile.stream_type());
+				UERROR("%s %d %d %d %d %s type=%d",
+						rs2_format_to_string(profile.format()),
+						video_profile.get()?video_profile.width():-1,
+						video_profile.get()?video_profile.height():-1,
+						profile.fps(),
+						profile.stream_index(),
+						profile.stream_name().c_str(),
+						profile.stream_type());
 			}
 			return false;
 		}
@@ -1075,19 +1109,26 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			 {
 				 auto video_profile = profilesPerSensor[i][j].as<rs2::video_stream_profile>();
 				 UINFO("Opening: %s %d %d %d %d %s type=%d", rs2_format_to_string(
-						 video_profile.format()),
-						 video_profile.width(),
-						 video_profile.height(),
-						 video_profile.fps(),
-						 video_profile.stream_index(),
-						 video_profile.stream_name().c_str(),
-						 video_profile.stream_type());
+						 profilesPerSensor[i][j].format()),
+						 video_profile.get()?video_profile.width():-1,
+						 video_profile.get()?video_profile.height():-1,
+						 profilesPerSensor[i][j].fps(),
+						 profilesPerSensor[i][j].stream_index(),
+						 profilesPerSensor[i][j].stream_name().c_str(),
+						 profilesPerSensor[i][j].stream_type());
 			 }
 			 if(globalTimeSync_ && sensors[i].supports(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED))
 			 {
-				 float value = sensors[i].get_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED);
-				 UINFO("Set RS2_OPTION_GLOBAL_TIME_ENABLED=1 (was %f) for sensor %d", value, (int)i);
-				 sensors[i].set_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED, 1);
+				float value = sensors[i].get_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED);
+				UINFO("Set RS2_OPTION_GLOBAL_TIME_ENABLED=1 (was %f) for sensor %d", value, (int)i);
+				if(!sensors[i].is_option_read_only(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED))
+				{
+					sensors[i].set_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED, 1);
+				}
+				else if(value != 1)
+				{
+					UWARN("rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED option is read-only, cannot enable it.");
+				}
 			 }
 			 sensors[i].open(profilesPerSensor[i]);
 			 if(sensors[i].is<rs2::depth_sensor>())
@@ -1525,11 +1566,21 @@ SensorData CameraRealSense2::captureImage(SensorCaptureInfo * info)
 		else
 		{
 			UERROR("Missing frames (received %d, needed=%d)", (int)frameset.size(), desiredFramesetSize);
+			if(frameset.size()>0)
+			{
+				for (auto it = frameset.begin(); it != frameset.end(); ++it)
+				{
+					UERROR("Received frame only from %s", (*it).get_profile().stream_name().c_str());
+				}
+			}
 		}
 	}
 	catch(const std::exception& ex)
 	{
-		UERROR("An error has occurred during frame callback: %s", ex.what());
+		if(!playback_)
+		{
+			UERROR("An error has occurred during frame callback: %s", ex.what());
+		}
 	}
 #else
 	UERROR("CameraRealSense2: RTAB-Map is not built with RealSense2 support!");
@@ -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;
 }
--- a/Show More
+++ b/Show More