GridMap: fixing eigen error with for downstream consumers

* Parallelize cloud generation in export/view clouds dialog and rtabmap-export

* Simplified NodeExportData by holding Signature directly. Added --threads option (default max cores) for CLI and UI.

* Parallelized texturing phase the same way than cloud generation

* Fixed not cancelable (right away) cloud generation in UI

---------

Co-authored-by: matlabbe <matlabbe@gmail.com>

.appveyor.yml

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

.devcontainer/android/Dockerfile

@@ -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
+
+

.devcontainer/android/devcontainer.json

@@ -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"]
 }
+
+
+  

.devcontainer/android/init.sh

@@ -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'

.devcontainer/focal/Dockerfile

@@ -0,0 +1,19 @@
+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
+RUN echo 'source /opt/ros/$ROS_DISTRO/setup.bash' >> /home/${USERNAME}/.bashrc

.devcontainer/focal/devcontainer.json

@@ -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"]
 }

.devcontainer/jammy/Dockerfile

@@ -0,0 +1,19 @@
+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
+RUN echo 'source /opt/ros/$ROS_DISTRO/setup.bash' >> /home/${USERNAME}/.bashrc

.devcontainer/jammy/devcontainer.json

@@ -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"]
 }

.devcontainer/latest-deps-from-source/Dockerfile

@@ -0,0 +1,105 @@
+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
+RUN echo 'source /opt/ros/${ROS_DISTRO}/setup.bash' >> /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}

.devcontainer/latest-deps-from-source/devcontainer.json

@@ -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"]
+}

.devcontainer/noble-kilted/Dockerfile

@@ -0,0 +1,20 @@
+FROM introlab3it/rtabmap:noble-kilted-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 echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
+RUN echo 'source /opt/ros/kilted/setup.bash' >> /home/${USERNAME}/.bashrc

.devcontainer/noble-kilted/devcontainer.json

@@ -0,0 +1,17 @@
+{
+    "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",
+    "settings": {
+      "terminal.integrated.defaultProfile.linux": "bash"
+    },
+    "remoteUser": "vscode",
+    "runArgs": ["--privileged"]
+}

.devcontainer/noble/Dockerfile

@@ -0,0 +1,23 @@
+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
+RUN echo 'source /opt/ros/$ROS_DISTRO/setup.bash' >> /home/${USERNAME}/.bashrc

.devcontainer/noble/devcontainer.json

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

.devcontainer/resolute/Dockerfile

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

.devcontainer/resolute/devcontainer.json

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

.devcontainer/rolling/Dockerfile

@@ -0,0 +1,80 @@
+
+FROM ubuntu:24.04
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+# Install ROS2
+RUN apt update && \
+    apt install software-properties-common -y && \
+    add-apt-repository universe -y && \
+    apt update && \
+    apt install curl -y && \
+    export ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}') && \
+    curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo ${UBUNTU_CODENAME:-${VERSION_CODENAME}})_all.deb" && \
+    apt install /tmp/ros2-apt-source.deb && \
+    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
+RUN echo 'source /opt/ros/$ROS_DISTRO/setup.bash' >> /home/${USERNAME}/.bashrc

.devcontainer/rolling/devcontainer.json

@@ -0,0 +1,30 @@
+{
+    "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",
+    "hostRequirements": {
+        "gpu": "optional"
+    },
+    "runArgs": ["--privileged",
+                "--network=host",
+                "--gpus=all",
+                //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer
+                "--env=DISPLAY",
+                "--env=QT_X11_NO_MITSHM=1",
+                "--volume=/tmp/.X11-unix:/tmp/.X11-unix"],
+    "containerEnv": {
+        "NVIDIA_VISIBLE_DEVICES": "all"
+    }
+}

.gitattributes

@@ -0,0 +1,53 @@
+# This repository has a historical mix of CRLF and LF files. Letting git convert
+# line endings (core.autocrlf, or a "text=auto" policy) would rewrite whole files
+# on checkout/commit and bury real changes under thousands of ending-only diff
+# lines -- and it would do so at unpredictable times, on whichever file happens
+# to be touched next. Keep every file's bytes exactly as committed.
+#
+# This also keeps LF-only files LF on Windows checkouts, which data/tests/manifest.txt
+# relies on: scripts/fetch_test_data.sh tab-splits it and a trailing CR breaks SHA
+# matching. No per-file exception is needed for that.
+#
+# Note this does not stop an editor or script from rewriting a file's endings; it
+# only stops git from doing it.
+* -text
+
+# -----------------------------------------------------------------------------
+# Migrating to the standard LF policy (a future MR, on its own)
+# -----------------------------------------------------------------------------
+# LF is what git stores natively and what essentially every C++ project on GitHub
+# uses (LLVM, OpenCV, PCL, Qt). The rule above only freezes the current mix; it is
+# a holding position, not the destination. To finish the job:
+#
+#   1. Replace the `* -text` line above with:
+#
+#          * text=auto
+#          # Files that must stay LF even in a Windows working tree:
+#          *.sh    text eol=lf
+#          *.bash  text eol=lf
+#
+#      (`text=auto` = store LF in the repo, check out native line endings.)
+#
+#   2. Rewrite the working tree and stage it, in the same commit as step 1:
+#
+#          git add --renormalize .
+#
+#   3. Verify nothing but line endings changed -- this must print nothing:
+#
+#          git diff --cached --ignore-cr-at-eol
+#
+#   4. Commit alone, with no other change in the MR, then record the SHA so
+#      `git blame` (and GitHub's blame view) skips over it:
+#
+#          echo "<sha of the normalization commit>" >> .git-blame-ignore-revs
+#
+#      Add a follow-up commit for that file; also set it locally with
+#      `git config blame.ignoreRevsFile .git-blame-ignore-revs`.
+#
+# Cost, measured on the tree at the time of writing: 227 CRLF files + 7 with mixed
+# endings, ~138k of ~634k lines rewritten (22%), across 234 files. The commit
+# itself is mechanical and safe -- the real cost is every open branch that touches
+# one of those files, which will conflict over the whole file on rebase. Merge or
+# rebase those with `-X renormalize` (merge) / `--strategy-option renormalize`.
+#
+# Best moment: right after a release, when the branch backlog is smallest.

.github/actions/install-macos-source-deps/action.yml

@@ -0,0 +1,267 @@
+name: 'Install macOS Source Dependencies'
+description: 'Builds (and per-dependency caches) the source-only deps not available from Homebrew (GTSAM, libnabo+libpointmatcher, OrbbecSDK, depthai) and installs them into /usr/local.'
+inputs:
+  os:
+    description: 'Runner os label (e.g. matrix.os), used to namespace the per-dependency caches. Required because intel/arm produce different binaries with identical Homebrew version strings.'
+    required: true
+  build_type:
+    description: 'CMake build type.'
+    required: false
+    default: 'Release'
+runs:
+  using: "composite"
+  steps:
+    - name: Resolve dependency versions
+      id: depver
+      shell: bash
+      # The source deps link against these Homebrew libraries and are compiled
+      # with Xcode's toolchain, so both are folded into the cache keys: a
+      # Homebrew bump (e.g. a new Eigen) or an Xcode update forces a rebuild.
+      run: |
+        BREW=$(brew list --versions eigen boost yaml-cpp | sort)
+        XCODE=$(xcodebuild -version 2>/dev/null || clang --version | head -1)
+        HASH=$(printf '%s\n%s\n' "$BREW" "$XCODE" | shasum | cut -d' ' -f1)
+        echo "hash=$HASH" >> "$GITHUB_OUTPUT"
+
+    # ----------------------------- GTSAM -----------------------------
+    - name: Cache GTSAM
+      id: cache-gtsam
+      uses: actions/cache@v4
+      with:
+        path: ${{ runner.temp }}/deps-stage/gtsam
+        key: gtsam-4.2.1-${{ inputs.os }}-${{ hashFiles('.github/actions/install-macos-source-deps/patches/gtsam-4.2.1-eigen5.patch') }}-${{ steps.depver.outputs.hash }}
+
+    - name: Build GTSAM
+      if: steps.cache-gtsam.outputs.cache-hit != 'true'
+      shell: bash
+      run: |
+        set -e
+        NPROC=$(sysctl -n hw.logicalcpu)
+        PATCHES="${{ github.action_path }}/patches"
+        SRC="${{ runner.temp }}/src-deps/gtsam"
+        STAGE="${{ runner.temp }}/deps-stage/gtsam"
+        rm -rf "$SRC"; mkdir -p "$SRC" "$STAGE"
+        git clone --depth 1 --branch 4.2.1 https://github.com/borglab/gtsam.git "$SRC/gtsam"
+        cd "$SRC/gtsam"
+        git apply "$PATCHES/gtsam-4.2.1-eigen5.patch"
+        cmake -B build \
+          -DCMAKE_BUILD_TYPE=${{ inputs.build_type }} \
+          -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
+          -DGTSAM_WITH_TBB=OFF \
+          -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
+          -DGTSAM_BUILD_TESTS=OFF \
+          -DGTSAM_BUILD_UNSTABLE=OFF \
+          -DGTSAM_USE_SYSTEM_EIGEN=ON \
+          -DCMAKE_POLICY_VERSION_MINIMUM=3.5
+        cmake --build build -j$NPROC
+        DESTDIR="$STAGE" cmake --install build
+
+    # ----------------- libnabo + libpointmatcher --------------------
+    # Built together: libpointmatcher depends on libnabo, so keeping them in one
+    # unit keeps the build order internal (lpm finds libnabo in the unit's stage).
+    - name: Cache pointmatcher
+      id: cache-pointmatcher
+      uses: actions/cache@v4
+      with:
+        path: ${{ runner.temp }}/deps-stage/pointmatcher
+        key: pointmatcher-nabo1.1.2-lpm1.4.4-${{ inputs.os }}-${{ hashFiles('.github/actions/install-macos-source-deps/patches/libnabo-1.1.2.patch', '.github/actions/install-macos-source-deps/patches/libpointmatcher-1.4.4-boost.patch') }}-${{ steps.depver.outputs.hash }}
+
+    - name: Build pointmatcher
+      if: steps.cache-pointmatcher.outputs.cache-hit != 'true'
+      shell: bash
+      run: |
+        set -e
+        NPROC=$(sysctl -n hw.logicalcpu)
+        PATCHES="${{ github.action_path }}/patches"
+        SRC="${{ runner.temp }}/src-deps/pointmatcher"
+        STAGE="${{ runner.temp }}/deps-stage/pointmatcher"
+        rm -rf "$SRC"; mkdir -p "$SRC" "$STAGE"
+        # libnabo (required by libpointmatcher)
+        git clone --depth 1 --branch 1.1.2 https://github.com/ethz-asl/libnabo.git "$SRC/libnabo"
+        cd "$SRC/libnabo"
+        git apply "$PATCHES/libnabo-1.1.2.patch"
+        cmake -B build -DCMAKE_BUILD_TYPE=${{ inputs.build_type }}
+        cmake --build build -j$NPROC
+        DESTDIR="$STAGE" cmake --install build
+        # libpointmatcher (finds libnabo from the unit's staged prefix)
+        git clone --depth 1 --branch 1.4.4 https://github.com/ethz-asl/libpointmatcher.git "$SRC/libpointmatcher"
+        cd "$SRC/libpointmatcher"
+        git apply "$PATCHES/libpointmatcher-1.4.4-boost.patch"
+        cmake -B build -DCMAKE_BUILD_TYPE=${{ inputs.build_type }} -DCMAKE_PREFIX_PATH="$STAGE/usr/local" -DBUILD_EVALUATIONS=OFF -DBUILD_EXAMPLES=OFF
+        cmake --build build -j$NPROC
+        DESTDIR="$STAGE" cmake --install build
+
+    # ----------------------------- OrbbecSDK ------------------------
+    - name: Cache OrbbecSDK
+      id: cache-orbbec
+      uses: actions/cache@v4
+      with:
+        path: ${{ runner.temp }}/deps-stage/orbbec
+        key: orbbec-2.8.7-usrlocal-${{ inputs.os }}-${{ hashFiles('.github/actions/install-macos-source-deps/patches/orbbecsdk-2.8.7-cmake-config-install.patch') }}-${{ steps.depver.outputs.hash }}
+
+    - name: Build OrbbecSDK
+      if: steps.cache-orbbec.outputs.cache-hit != 'true'
+      shell: bash
+      run: |
+        set -e
+        NPROC=$(sysctl -n hw.logicalcpu)
+        PATCHES="${{ github.action_path }}/patches"
+        SRC="${{ runner.temp }}/src-deps/orbbec"
+        STAGE="${{ runner.temp }}/deps-stage/orbbec"
+        rm -rf "$SRC"; mkdir -p "$SRC" "$STAGE"
+        git clone --depth 1 --branch v2.8.7 https://github.com/orbbec/OrbbecSDK_v2.git "$SRC/OrbbecSDK_v2"
+        cd "$SRC/OrbbecSDK_v2"
+        git apply "$PATCHES/orbbecsdk-2.8.7-cmake-config-install.patch"
+        # OrbbecSDK defaults its install prefix to /opt/OrbbecSDK; force /usr/local
+        # so it stages under usr/local like the other deps and is installed/found.
+        cmake -B build -DCMAKE_BUILD_TYPE=${{ inputs.build_type }} -DCMAKE_INSTALL_PREFIX=/usr/local -DOB_BUILD_DOCS=OFF -DOB_BUILD_EXAMPLES=OFF -DOB_BUILD_TOOLS=OFF -DOB_INSTALL_EXAMPLES_SOURCE=OFF
+        cmake --build build -j$NPROC
+        DESTDIR="$STAGE" cmake --install build
+
+    # ----------------------------- depthai --------------------------
+    - name: Cache depthai
+      id: cache-depthai
+      uses: actions/cache@v4
+      with:
+        path: ${{ runner.temp }}/deps-stage/depthai
+        key: depthai-2.32.0-usrlocal-nocv-${{ inputs.os }}-${{ hashFiles('.github/actions/install-macos-source-deps/patches/depthai-2.32.0-hunter-macos.patch') }}-${{ steps.depver.outputs.hash }}
+
+    - name: Build depthai
+      if: steps.cache-depthai.outputs.cache-hit != 'true'
+      shell: bash
+      run: |
+        set -e
+        NPROC=$(sysctl -n hw.logicalcpu)
+        PATCHES="${{ github.action_path }}/patches"
+        SRC="${{ runner.temp }}/src-deps/depthai"
+        STAGE="${{ runner.temp }}/deps-stage/depthai"
+        rm -rf "$SRC"; mkdir -p "$SRC" "$STAGE"
+        # depthai 2.32.0 (Hunter-based; v2 does NOT use vcpkg/CMake presets)
+        git clone --depth 1 --branch v2.32.0 https://github.com/luxonis/depthai-core.git "$SRC/depthai-core"
+        cd "$SRC/depthai-core"
+        git submodule update --init --recursive
+        # Route zlib to Homebrew: Hunter's pinned zlib fork has a classic
+        # TARGET_OS_MAC fdopen block that won't compile against the modern macOS SDK.
+        git apply "$PATCHES/depthai-2.32.0-hunter-macos.patch"
+        # v2 and its old Hunter deps declare cmake_minimum_required < 3.5, which the
+        # runner's CMake 4.x rejects, and CMAKE_POLICY_VERSION_MINIMUM does not
+        # propagate into Hunter's sub-builds. Use a pinned CMake 3.x just for depthai
+        # (Hunter reuses the same cmake binary for its dependency sub-builds).
+        python3 -m venv "$SRC/cmake3-venv"
+        "$SRC/cmake3-venv/bin/pip" install -q "cmake==3.31.6"
+        CMAKE3="$SRC/cmake3-venv/bin/cmake"
+        # DEPTHAI_ENABLE_CURL=OFF: Hunter's CURL drags in the broken zlib and also
+        # fails to configure on this toolchain; rtabmap doesn't need depthai's CURL.
+        # depthai-core defaults its install prefix to <build>/install; force
+        # /usr/local so it stages under usr/local like the other deps.
+        "$CMAKE3" -B build \
+          -DCMAKE_BUILD_TYPE=${{ inputs.build_type }} \
+          -DCMAKE_INSTALL_PREFIX=/usr/local \
+          -DBUILD_SHARED_LIBS=ON \
+          -DDEPTHAI_ENABLE_CURL=OFF \
+          -DDEPTHAI_BUILD_TESTS=OFF \
+          -DDEPTHAI_BUILD_EXAMPLES=OFF \
+          -DDEPTHAI_OPENCV_SUPPORT=OFF \
+          -DCMAKE_PREFIX_PATH="$(brew --prefix zlib)"
+        "$CMAKE3" --build build -j$NPROC
+        DESTDIR="$STAGE" "$CMAKE3" --install build
+
+    # ----------------------------- opengv ---------------------------
+    - name: Cache opengv
+      id: cache-opengv
+      uses: actions/cache@v4
+      with:
+        path: ${{ runner.temp }}/deps-stage/opengv
+        key: opengv-91f4b19c-usrlocal-${{ inputs.os }}-${{ hashFiles('.github/actions/install-macos-source-deps/patches/opengv-91f4b19c-macos.patch') }}-${{ steps.depver.outputs.hash }}
+
+    - name: Build opengv
+      if: steps.cache-opengv.outputs.cache-hit != 'true'
+      shell: bash
+      run: |
+        set -e
+        NPROC=$(sysctl -n hw.logicalcpu)
+        PATCHES="${{ github.action_path }}/patches"
+        SRC="${{ runner.temp }}/src-deps/opengv"
+        STAGE="${{ runner.temp }}/deps-stage/opengv"
+        rm -rf "$SRC"; mkdir -p "$SRC" "$STAGE"
+        # opengv pins a commit (not a tag), so clone then checkout.
+        git clone https://github.com/laurentkneip/opengv.git "$SRC/opengv"
+        cd "$SRC/opengv"
+        git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3
+        # Disable the -march=native/armv7-a/armv8-a flags so it builds portably on
+        # both Intel and Apple Silicon runners.
+        git apply "$PATCHES/opengv-91f4b19c-macos.patch"
+        # CMAKE_POLICY_VERSION_MINIMUM=3.5: opengv declares an ancient
+        # cmake_minimum_required that modern CMake rejects. EIGEN_INCLUDE_DIR points
+        # opengv's bundled FindEigen at Homebrew's eigen (covers /opt/homebrew on arm64).
+        cmake -B build \
+          -DCMAKE_BUILD_TYPE=${{ inputs.build_type }} \
+          -DCMAKE_INSTALL_PREFIX=/usr/local \
+          -DBUILD_TESTS=OFF \
+          -DBUILD_SHARED_LIBS=ON \
+          -DCMAKE_POLICY_VERSION_MINIMUM=3.5 \
+          -DEIGEN_INCLUDE_DIR="$(brew --prefix eigen)/include/eigen3"
+        cmake --build build -j$NPROC
+        DESTDIR="$STAGE" cmake --install build
+
+    # ----------------------------- g2o ------------------------------
+    - name: Cache g2o
+      id: cache-g2o
+      uses: actions/cache@v4
+      with:
+        path: ${{ runner.temp }}/deps-stage/g2o
+        key: g2o-67cbe15c-${{ inputs.os }}-${{ steps.depver.outputs.hash }}
+
+    - name: Build g2o
+      if: steps.cache-g2o.outputs.cache-hit != 'true'
+      shell: bash
+      run: |
+        set -e
+        NPROC=$(sysctl -n hw.logicalcpu)
+        SRC="${{ runner.temp }}/src-deps/g2o"
+        STAGE="${{ runner.temp }}/deps-stage/g2o"
+        rm -rf "$SRC"; mkdir -p "$SRC" "$STAGE"
+        # g2o pins a commit (not a tag), so clone then checkout.
+        git clone https://github.com/RainerKuemmerle/g2o.git "$SRC/g2o"
+        cd "$SRC/g2o"
+        git checkout 67cbe15c998737ac6705d3cd18201a72be0d073d
+        # CMAKE_INSTALL_PREFIX=/usr/local so it stages under usr/local like the
+        # other deps. Homebrew's libomp is keg-only and g2o does not propagate
+        # OpenMP's include/lib dirs, so with Apple clang <omp.h> isn't found and
+        # libomp isn't linked; add its include (-I) and lib (-L -lomp) explicitly.
+        LIBOMP=$(brew --prefix libomp)
+        cmake -B build \
+          -DCMAKE_BUILD_TYPE=${{ inputs.build_type }} \
+          -DCMAKE_INSTALL_PREFIX=/usr/local \
+          -DCMAKE_PREFIX_PATH="$LIBOMP" \
+          -DCMAKE_C_FLAGS="-I$LIBOMP/include" \
+          -DCMAKE_CXX_FLAGS="-I$LIBOMP/include" \
+          -DCMAKE_EXE_LINKER_FLAGS="-L$LIBOMP/lib -lomp" \
+          -DCMAKE_SHARED_LINKER_FLAGS="-L$LIBOMP/lib -lomp" \
+          -DG2O_BUILD_APPS=OFF \
+          -DG2O_BUILD_EXAMPLES=OFF \
+          -DG2O_USE_OPENMP=ON \
+          -DG2O_USE_OPENGL=OFF
+        cmake --build build -j$NPROC
+        DESTDIR="$STAGE" cmake --install build
+
+    # ------------------- install all units into /usr/local ----------
+    - name: Install source dependencies into /usr/local
+      shell: bash
+      # Copy each (cached or freshly built) staged tree into /usr/local so
+      # rtabmap's find_package() and the bundle fixup (which search /usr/local)
+      # pick them up. Runs on both cache hit and miss.
+      run: |
+        set -e
+        STAGE="${{ runner.temp }}/deps-stage"
+        found=0
+        for unit in gtsam pointmatcher orbbec depthai opengv g2o; do
+          if [ -d "$STAGE/$unit/usr/local" ]; then
+            sudo cp -a "$STAGE/$unit/usr/local/." /usr/local/
+            found=1
+          fi
+        done
+        if [ "$found" != "1" ]; then
+          echo "No staged source dependencies found under $STAGE" >&2
+          exit 1
+        fi

.github/actions/install-macos-source-deps/patches/depthai-2.32.0-hunter-macos.patch

@@ -0,0 +1,41 @@
+diff --git a/cmake/depthaiDependencies.cmake b/cmake/depthaiDependencies.cmake
+index 6b0b44f98..672c26402 100644
+--- a/cmake/depthaiDependencies.cmake
++++ b/cmake/depthaiDependencies.cmake
+@@ -17,7 +17,9 @@ else()
+     hunter_add_package(FP16)
+     hunter_add_package(libarchive-luxonis)
+     hunter_add_package(spdlog)
+-    hunter_add_package(ZLIB)
++    # ZLIB: use the system/Homebrew copy instead of Hunter's (the old pinned
++    # zlib fork fails to build against the modern macOS SDK). Found below via
++    # the standard FindZLIB module, which provides the same ZLIB::ZLIB target.
+     if(DEPTHAI_ENABLE_BACKWARD)
+         hunter_add_package(Backward)
+     endif()
+@@ -45,8 +47,23 @@ if(NOT CONFIG_MODE OR (CONFIG_MODE AND NOT DEPTHAI_SHARED_LIBS))
+     # libarchive for firmware packages
+     find_package(archive_static ${_QUIET} CONFIG REQUIRED)
+     find_package(lzma ${_QUIET} CONFIG REQUIRED)
+-    # ZLIB for compressing Apps
+-    find_package(ZLIB CONFIG REQUIRED)
++    # ZLIB for compressing Apps.
++    # Use the system/Homebrew zlib directly instead of Hunter's: Hunter's pinned
++    # zlib fork fails to build against the modern macOS SDK, and Hunter also
++    # intercepts find_package(ZLIB) via its own FindZLIB on the module path, so
++    # we locate it manually and expose the ZLIB::zlib target depthai expects.
++    find_path(SYSTEM_ZLIB_INCLUDE_DIR NAMES zlib.h)
++    find_library(SYSTEM_ZLIB_LIBRARY NAMES z)
++    if(NOT SYSTEM_ZLIB_INCLUDE_DIR OR NOT SYSTEM_ZLIB_LIBRARY)
++        message(FATAL_ERROR "System zlib not found (looked for zlib.h and libz). Install via: brew install zlib")
++    endif()
++    if(NOT TARGET ZLIB::zlib)
++        add_library(ZLIB::zlib UNKNOWN IMPORTED)
++        set_target_properties(ZLIB::zlib PROPERTIES
++            IMPORTED_LOCATION "${SYSTEM_ZLIB_LIBRARY}"
++            INTERFACE_INCLUDE_DIRECTORIES "${SYSTEM_ZLIB_INCLUDE_DIR}"
++        )
++    endif()
+ 
+     # spdlog for library and device logging
+     find_package(spdlog ${_QUIET} CONFIG REQUIRED)

.github/actions/install-macos-source-deps/patches/gtsam-4.2.1-eigen5.patch

@@ -0,0 +1,83 @@
+diff --git a/cmake/HandleEigen.cmake b/cmake/HandleEigen.cmake
+index b3b4f66b6..651cd1009 100644
+--- a/cmake/HandleEigen.cmake
++++ b/cmake/HandleEigen.cmake
+@@ -59,7 +59,12 @@ else()
+ endif()
+
+ # Detect Eigen version:
+-set(EIGEN_VER_H "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}/Eigen/src/Core/util/Macros.h")
++# Since Eigen 5.0.0 the version macros moved from Eigen/src/Core/util/Macros.h
++# to a dedicated Eigen/Version header, so check the new location first.
++set(EIGEN_VER_H "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}/Eigen/Version")
++if (NOT EXISTS ${EIGEN_VER_H})
++    set(EIGEN_VER_H "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}/Eigen/src/Core/util/Macros.h")
++endif()
+ if (EXISTS ${EIGEN_VER_H})
+     file(READ "${EIGEN_VER_H}" STR_EIGEN_VERSION)
+
+diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
+index cfedf6d8c..9c9771407 100644
+--- a/gtsam/base/Matrix.h
++++ b/gtsam/base/Matrix.h
+@@ -54,7 +54,7 @@ using Matrix7##N = Eigen::Matrix<double, 7, N>;  \
+ using Matrix8##N = Eigen::Matrix<double, 8, N>;  \
+ using Matrix9##N = Eigen::Matrix<double, 9, N>;  \
+ static const Eigen::MatrixBase<Matrix##N>::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \
+-static const Eigen::MatrixBase<Matrix##N>::ConstantReturnType Z_##N##x##N = Matrix##N::Zero();
++static const decltype(Matrix##N::Zero()) Z_##N##x##N = Matrix##N::Zero();
+
+ GTSAM_MAKE_MATRIX_DEFS(1)
+ GTSAM_MAKE_MATRIX_DEFS(2)
+diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h
+index f7923ff88..5410393f6 100644
+--- a/gtsam/base/Vector.h
++++ b/gtsam/base/Vector.h
+@@ -27,6 +27,7 @@
+
+ #include <gtsam/global_includes.h>
+ #include <Eigen/Core>
++#include <cassert>
+ #include <iosfwd>
+ #include <list>
+
+@@ -42,14 +43,14 @@ typedef Eigen::Matrix<double, 1, 1> Vector1;
+ typedef Eigen::Vector2d Vector2;
+ typedef Eigen::Vector3d Vector3;
+
+-static const Eigen::MatrixBase<Vector2>::ConstantReturnType Z_2x1 = Vector2::Zero();
+-static const Eigen::MatrixBase<Vector3>::ConstantReturnType Z_3x1 = Vector3::Zero();
++static const decltype(Vector2::Zero()) Z_2x1 = Vector2::Zero();
++static const decltype(Vector3::Zero()) Z_3x1 = Vector3::Zero();
+
+ // Create handy typedefs and constants for vectors with N>3
+ // VectorN and Z_Nx1, for N=1..9
+ #define GTSAM_MAKE_VECTOR_DEFS(N)                \
+   using Vector##N = Eigen::Matrix<double, N, 1>; \
+-  static const Eigen::MatrixBase<Vector##N>::ConstantReturnType Z_##N##x1 = Vector##N::Zero();
++  static const decltype(Vector##N::Zero()) Z_##N##x1 = Vector##N::Zero();
+
+ GTSAM_MAKE_VECTOR_DEFS(4)
+ GTSAM_MAKE_VECTOR_DEFS(5)
+diff --git a/gtsam/linear/iterative.h b/gtsam/linear/iterative.h
+index 22f65b8de..20a0c936e 100644
+--- a/gtsam/linear/iterative.h
++++ b/gtsam/linear/iterative.h
+@@ -61,7 +61,7 @@ namespace gtsam {
+
+     /** Apply operator A'*e */
+     Vector operator^(const Vector& e) const {
+-      return A_ ^ e;
++      return gtsam::operator^(A_, e);
+     }
+
+     /**
+@@ -71,7 +71,7 @@ namespace gtsam {
+
+     /** gradient of objective function 0.5*|Ax-b_|^2 at x = A_'*(Ax-b_) */
+     Vector gradient(const Vector& x) const {
+-      return A() ^ (A() * x - b());
++      return gtsam::operator^(A(), Vector(A() * x - b()));
+     }
+
+     /** Apply operator A */

.github/actions/install-macos-source-deps/patches/libnabo-1.1.2.patch

@@ -0,0 +1,87 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index a5f0c44..69340e4 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -78,18 +78,18 @@ elseif ()
+ 	endif ()
+ endif ()
+
+-# enable C++11 support.
++# enable C++14 support (required by recent Eigen versions).
+ if (CMAKE_VERSION VERSION_LESS "3.1")
+ 	if (MSVC)
+ 		message(FATAL_ERROR "CMake version 3.1 or later is required to compile ${PROJECT_NAME} with Microsoft Visual C++")
+ 	endif ()
+ 	if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+-		set (CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}")
++		set (CMAKE_CXX_FLAGS "-std=c++14 ${CMAKE_CXX_FLAGS}")
+ 	else ()
+-		set (CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
++		set (CMAKE_CXX_FLAGS "-std=c++14 ${CMAKE_CXX_FLAGS}")
+ 	endif ()
+ else ()
+-	set (CMAKE_CXX_STANDARD 11)
++	set (CMAKE_CXX_STANDARD 14)
+ endif ()
+
+ #======================== External Dependencies ===============================
+diff --git a/experimental/kdtree_cpu.cpp b/experimental/kdtree_cpu.cpp
+index 302b672..d63fcd3 100644
+--- a/experimental/kdtree_cpu.cpp
++++ b/experimental/kdtree_cpu.cpp
+@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+ #include "nabo_experimental.h"
+ #include "../nabo/index_heap.h"
++#include <cassert>
+ #include <iostream>
+ #include <stdexcept>
+ #include <limits>
+diff --git a/nabo/kdtree_cpu.cpp b/nabo/kdtree_cpu.cpp
+index cb1f8d1..52a444f 100644
+--- a/nabo/kdtree_cpu.cpp
++++ b/nabo/kdtree_cpu.cpp
+@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ #include <queue>
+ #include <algorithm>
+ #include <utility>
++#include <cassert>
+ #ifdef HAVE_OPENMP
+ #include <omp.h>
+ #endif
+diff --git a/nabo/kdtree_opencl.cpp b/nabo/kdtree_opencl.cpp
+index 5a9fee2..fb1345f 100644
+--- a/nabo/kdtree_opencl.cpp
++++ b/nabo/kdtree_opencl.cpp
+@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+ #include "nabo_private.h"
+ #include "index_heap.h"
++#include <cassert>
+ #include <iostream>
+ #include <sstream>
+ #include <fstream>
+diff --git a/tests/knnshow.cpp b/tests/knnshow.cpp
+index 6f4d3fc..c612378 100644
+--- a/tests/knnshow.cpp
++++ b/tests/knnshow.cpp
+@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+ #include "nabo/nabo.h"
++#include <cassert>
+ #include <iostream>
+ #include <fstream>
+ #include <stdexcept>
+diff --git a/tests/knnvalidate.cpp b/tests/knnvalidate.cpp
+index 2430249..2a7dcc4 100644
+--- a/tests/knnvalidate.cpp
++++ b/tests/knnvalidate.cpp
+@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ #include "nabo/nabo.h"
+ #include "helpers.h"
+ //#include "experimental/nabo_experimental.h"
++#include <cassert>
+ #include <iostream>
+ #include <fstream>
+ #include <stdexcept>

.github/actions/install-macos-source-deps/patches/libpointmatcher-1.4.4-boost.patch

@@ -0,0 +1,32 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index 9dabfd0..7b73418 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -128,9 +128,9 @@ endif()
+ #--------------------
+ # DEPENDENCY:  boost
+ #--------------------
+-find_package(Boost REQUIRED COMPONENTS thread system program_options date_time)
++find_package(Boost REQUIRED COMPONENTS thread program_options date_time)
+ if (Boost_MINOR_VERSION GREATER 47)
+-	find_package(Boost REQUIRED COMPONENTS thread system program_options date_time chrono)
++	find_package(Boost REQUIRED COMPONENTS thread program_options date_time chrono)
+ endif ()
+
+ #--------------------
+diff --git a/libpointmatcherConfig.cmake.in b/libpointmatcherConfig.cmake.in
+index 6de2c85..7c887fd 100644
+--- a/libpointmatcherConfig.cmake.in
++++ b/libpointmatcherConfig.cmake.in
+@@ -7,9 +7,9 @@
+ include(CMakeFindDependencyMacro)
+ find_dependency(libnabo REQUIRED)
+ find_dependency(yaml-cpp REQUIRED)
+-find_package(Boost COMPONENTS thread system program_options date_time REQUIRED)
++find_package(Boost COMPONENTS thread program_options date_time REQUIRED)
+ if (Boost_MINOR_VERSION GREATER 47)
+-  find_package(Boost COMPONENTS thread system program_options date_time chrono REQUIRED)
++  find_package(Boost COMPONENTS thread program_options date_time chrono REQUIRED)
+ endif ()
+ include(${CMAKE_CURRENT_LIST_DIR}/libpointmatcher-config.cmake)
+

.github/actions/install-macos-source-deps/patches/opengv-91f4b19c-macos.patch

@@ -0,0 +1,309 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index 9660f55..0186da4 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -31,12 +31,12 @@ IF(MSVC)
+   add_definitions(-D_USE_MATH_DEFINES)
+ ELSE()
+   IF (CMAKE_SYSTEM_PROCESSOR MATCHES "(arm64)|(ARM64)|(aarch64)|(AARCH64)")
+-    add_definitions (-march=armv8-a)
++    #add_definitions (-march=armv8-a)
+   ELSEIF (CMAKE_SYSTEM_PROCESSOR MATCHES 
+           "(arm)|(ARM)|(armhf)|(ARMHF)|(armel)|(ARMEL)")
+-    add_definitions (-march=armv7-a)
++    #add_definitions (-march=armv7-a)
+   ELSE ()
+-    add_definitions (-march=native) #TODO use correct c++11 def once everybody has moved to gcc 4.7 # for now I even removed std=gnu++0x
++    #add_definitions (-march=native) #TODO use correct c++11 def once everybody has moved to gcc 4.7 # for now I even removed std=gnu++0x
+   ENDIF()
+   add_definitions (
+     -O3
+@@ -181,7 +181,7 @@ add_library( random_generators test/random_generators.cpp test/random_generators
+ set_target_properties( opengv random_generators PROPERTIES
+                     SOVERSION ${PROJECT_VERSION}
+                     VERSION ${PROJECT_VERSION}
+-                    CXX_STANDARD 11
++                    CXX_STANDARD 14
+                     CXX_STANDARD_REQUIRED ON
+                     DEBUG_POSTFIX d )
+ 
+@@ -329,7 +329,7 @@ IF (BUILD_TESTS)
+     test_Sturm
+ 
+     PROPERTIES
+-    CXX_STANDARD 11
++    CXX_STANDARD 14
+     CXX_STANDARD_REQUIRED ON
+     DEBUG_POSTFIX d )
+ 
+diff --git a/modules/FindEigen.cmake b/modules/FindEigen.cmake
+index 126f4a0..39b5524 100644
+--- a/modules/FindEigen.cmake
++++ b/modules/FindEigen.cmake
+@@ -30,7 +30,13 @@ if(NOT Eigen_FIND_VERSION)
+ endif(NOT Eigen_FIND_VERSION)
+ 
+ macro(_eigen3_check_version)
+-  file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
++  # Since Eigen 5.0.0 the version macros moved from Eigen/src/Core/util/Macros.h
++  # to a dedicated Eigen/Version header, so check the new location first.
++  if(EXISTS "${EIGEN_INCLUDE_DIR}/Eigen/Version")
++    file(READ "${EIGEN_INCLUDE_DIR}/Eigen/Version" _eigen3_version_header)
++  else()
++    file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
++  endif()
+ 
+   string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+   set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}")
+diff --git a/src/absolute_pose/CentralAbsoluteAdapter.cpp b/src/absolute_pose/CentralAbsoluteAdapter.cpp
+index 684fa7e..a4ff2bd 100644
+--- a/src/absolute_pose/CentralAbsoluteAdapter.cpp
++++ b/src/absolute_pose/CentralAbsoluteAdapter.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/absolute_pose/CentralAbsoluteAdapter.hpp>
+ 
+ 
+diff --git a/src/absolute_pose/MACentralAbsolute.cpp b/src/absolute_pose/MACentralAbsolute.cpp
+index 6edbabc..1687d45 100644
+--- a/src/absolute_pose/MACentralAbsolute.cpp
++++ b/src/absolute_pose/MACentralAbsolute.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/absolute_pose/MACentralAbsolute.hpp>
+ 
+ 
+diff --git a/src/absolute_pose/MANoncentralAbsolute.cpp b/src/absolute_pose/MANoncentralAbsolute.cpp
+index d9b5b09..a7ae689 100644
+--- a/src/absolute_pose/MANoncentralAbsolute.cpp
++++ b/src/absolute_pose/MANoncentralAbsolute.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/absolute_pose/MANoncentralAbsolute.hpp>
+ 
+ opengv::absolute_pose::MANoncentralAbsolute::MANoncentralAbsolute(
+diff --git a/src/absolute_pose/NoncentralAbsoluteAdapter.cpp b/src/absolute_pose/NoncentralAbsoluteAdapter.cpp
+index 30176aa..6a0e405 100644
+--- a/src/absolute_pose/NoncentralAbsoluteAdapter.cpp
++++ b/src/absolute_pose/NoncentralAbsoluteAdapter.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/absolute_pose/NoncentralAbsoluteAdapter.hpp>
+ 
+ opengv::absolute_pose::NoncentralAbsoluteAdapter::NoncentralAbsoluteAdapter(
+diff --git a/src/absolute_pose/NoncentralAbsoluteMultiAdapter.cpp b/src/absolute_pose/NoncentralAbsoluteMultiAdapter.cpp
+index 88c237a..58a82f4 100644
+--- a/src/absolute_pose/NoncentralAbsoluteMultiAdapter.cpp
++++ b/src/absolute_pose/NoncentralAbsoluteMultiAdapter.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/absolute_pose/NoncentralAbsoluteMultiAdapter.hpp>
+ 
+ opengv::absolute_pose::NoncentralAbsoluteMultiAdapter::NoncentralAbsoluteMultiAdapter(
+diff --git a/src/absolute_pose/methods.cpp b/src/absolute_pose/methods.cpp
+index b1f0889..19c4eaf 100644
+--- a/src/absolute_pose/methods.cpp
++++ b/src/absolute_pose/methods.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/absolute_pose/methods.hpp>
+ #include <opengv/Indices.hpp>
+ 
+diff --git a/src/absolute_pose/modules/main.cpp b/src/absolute_pose/modules/main.cpp
+index ed0c271..99f7f35 100644
+--- a/src/absolute_pose/modules/main.cpp
++++ b/src/absolute_pose/modules/main.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <Eigen/NonLinearOptimization>
+ #include <Eigen/NumericalDiff>
+ 
+diff --git a/src/math/arun.cpp b/src/math/arun.cpp
+index a0d6296..f5c0ac3 100644
+--- a/src/math/arun.cpp
++++ b/src/math/arun.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/math/arun.hpp>
+ 
+ opengv::rotation_t
+diff --git a/src/point_cloud/MAPointCloud.cpp b/src/point_cloud/MAPointCloud.cpp
+index 81fd5dd..cdccae3 100644
+--- a/src/point_cloud/MAPointCloud.cpp
++++ b/src/point_cloud/MAPointCloud.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/point_cloud/MAPointCloud.hpp>
+ 
+ opengv::point_cloud::MAPointCloud::MAPointCloud(
+diff --git a/src/point_cloud/PointCloudAdapter.cpp b/src/point_cloud/PointCloudAdapter.cpp
+index f9faaeb..28fbb7e 100644
+--- a/src/point_cloud/PointCloudAdapter.cpp
++++ b/src/point_cloud/PointCloudAdapter.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/point_cloud/PointCloudAdapter.hpp>
+ 
+ opengv::point_cloud::PointCloudAdapter::PointCloudAdapter(
+diff --git a/src/point_cloud/methods.cpp b/src/point_cloud/methods.cpp
+index 5409eeb..7a26e6a 100644
+--- a/src/point_cloud/methods.cpp
++++ b/src/point_cloud/methods.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/point_cloud/methods.hpp>
+ #include <opengv/Indices.hpp>
+ 
+diff --git a/src/relative_pose/CentralRelativeAdapter.cpp b/src/relative_pose/CentralRelativeAdapter.cpp
+index 38e9a62..5018bd8 100644
+--- a/src/relative_pose/CentralRelativeAdapter.cpp
++++ b/src/relative_pose/CentralRelativeAdapter.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/relative_pose/CentralRelativeAdapter.hpp>
+ 
+ opengv::relative_pose::CentralRelativeAdapter::CentralRelativeAdapter(
+diff --git a/src/relative_pose/CentralRelativeMultiAdapter.cpp b/src/relative_pose/CentralRelativeMultiAdapter.cpp
+index 2ab7476..49a3a07 100644
+--- a/src/relative_pose/CentralRelativeMultiAdapter.cpp
++++ b/src/relative_pose/CentralRelativeMultiAdapter.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/relative_pose/CentralRelativeMultiAdapter.hpp>
+ 
+ opengv::relative_pose::CentralRelativeMultiAdapter::CentralRelativeMultiAdapter(
+diff --git a/src/relative_pose/CentralRelativeWeightingAdapter.cpp b/src/relative_pose/CentralRelativeWeightingAdapter.cpp
+index a6ab478..7684526 100644
+--- a/src/relative_pose/CentralRelativeWeightingAdapter.cpp
++++ b/src/relative_pose/CentralRelativeWeightingAdapter.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/relative_pose/CentralRelativeWeightingAdapter.hpp>
+ 
+ opengv::relative_pose::CentralRelativeWeightingAdapter::CentralRelativeWeightingAdapter(
+diff --git a/src/relative_pose/MACentralRelative.cpp b/src/relative_pose/MACentralRelative.cpp
+index ec2959f..75e76d5 100644
+--- a/src/relative_pose/MACentralRelative.cpp
++++ b/src/relative_pose/MACentralRelative.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/relative_pose/MACentralRelative.hpp>
+ 
+ opengv::relative_pose::MACentralRelative::MACentralRelative(
+diff --git a/src/relative_pose/MANoncentralRelative.cpp b/src/relative_pose/MANoncentralRelative.cpp
+index cea9c14..8566aeb 100644
+--- a/src/relative_pose/MANoncentralRelative.cpp
++++ b/src/relative_pose/MANoncentralRelative.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/relative_pose/MANoncentralRelative.hpp>
+ 
+ opengv::relative_pose::MANoncentralRelative::MANoncentralRelative(
+diff --git a/src/relative_pose/MANoncentralRelativeMulti.cpp b/src/relative_pose/MANoncentralRelativeMulti.cpp
+index 49f8ecf..8b62e07 100644
+--- a/src/relative_pose/MANoncentralRelativeMulti.cpp
++++ b/src/relative_pose/MANoncentralRelativeMulti.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/relative_pose/MANoncentralRelativeMulti.hpp>
+ 
+ opengv::relative_pose::MANoncentralRelativeMulti::MANoncentralRelativeMulti(
+diff --git a/src/relative_pose/NoncentralRelativeAdapter.cpp b/src/relative_pose/NoncentralRelativeAdapter.cpp
+index 552f180..775d520 100644
+--- a/src/relative_pose/NoncentralRelativeAdapter.cpp
++++ b/src/relative_pose/NoncentralRelativeAdapter.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/relative_pose/NoncentralRelativeAdapter.hpp>
+ 
+ opengv::relative_pose::NoncentralRelativeAdapter::NoncentralRelativeAdapter(
+diff --git a/src/relative_pose/NoncentralRelativeMultiAdapter.cpp b/src/relative_pose/NoncentralRelativeMultiAdapter.cpp
+index f41edbe..733023c 100644
+--- a/src/relative_pose/NoncentralRelativeMultiAdapter.cpp
++++ b/src/relative_pose/NoncentralRelativeMultiAdapter.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/relative_pose/NoncentralRelativeMultiAdapter.hpp>
+ 
+ opengv::relative_pose::NoncentralRelativeMultiAdapter::NoncentralRelativeMultiAdapter(
+diff --git a/src/relative_pose/methods.cpp b/src/relative_pose/methods.cpp
+index 0027dae..7678567 100644
+--- a/src/relative_pose/methods.cpp
++++ b/src/relative_pose/methods.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/relative_pose/methods.hpp>
+ #include <opengv/Indices.hpp>
+ 
+diff --git a/src/relative_pose/modules/fivept_nister/modules.cpp b/src/relative_pose/modules/fivept_nister/modules.cpp
+index 4b134c5..258d416 100644
+--- a/src/relative_pose/modules/fivept_nister/modules.cpp
++++ b/src/relative_pose/modules/fivept_nister/modules.cpp
+@@ -29,6 +29,7 @@
+  ******************************************************************************/
+ 
+ 
++#include <cassert>
+ #include <opengv/relative_pose/modules/fivept_nister/modules.hpp>
+ #include <Eigen/NonLinearOptimization>
+ #include <Eigen/NumericalDiff>

.github/actions/install-macos-source-deps/patches/orbbecsdk-2.8.7-cmake-config-install.patch

@@ -0,0 +1,29 @@
+diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
+index 261f2cda..644e068f 100644
+--- a/src/CMakeLists.txt
++++ b/src/CMakeLists.txt
+@@ -92,17 +92,20 @@ if(OB_IS_MAIN_PROJECT)
+     )
+ 
+     install(FILES "${CMAKE_CURRENT_BINARY_DIR}/generated/Export.h" DESTINATION include/libobsensor/h)
+-    install(EXPORT ${OB_SDK_LIB_NAME}Config NAMESPACE ob:: DESTINATION lib)
++    install(EXPORT ${OB_SDK_LIB_NAME}Config NAMESPACE ob:: DESTINATION lib/cmake/${OB_SDK_LIB_NAME})
+ 
+     include(CMakePackageConfigHelpers)
++    # Name the version file <pkg>ConfigVersion.cmake (not <pkg>Version.cmake) and
++    # install it next to the config in lib/cmake/<pkg>, so find_package(OrbbecSDK)
++    # discovers it in config mode and reads its version.
+     write_basic_package_version_file(
+-        "${CMAKE_CURRENT_BINARY_DIR}/${OB_SDK_LIB_NAME}Version.cmake"
++        "${CMAKE_CURRENT_BINARY_DIR}/${OB_SDK_LIB_NAME}ConfigVersion.cmake"
+         VERSION "${PROJECT_VERSION}"
+         COMPATIBILITY SameMajorVersion
+     )
+     install(
+-        FILES "${CMAKE_CURRENT_BINARY_DIR}/${OB_SDK_LIB_NAME}Version.cmake"
+-        DESTINATION lib
++        FILES "${CMAKE_CURRENT_BINARY_DIR}/${OB_SDK_LIB_NAME}ConfigVersion.cmake"
++        DESTINATION lib/cmake/${OB_SDK_LIB_NAME}
+     )
+     if(MSVC)
+         install(FILES $<TARGET_PDB_FILE:${OB_SDK_LIB_NAME}> DESTINATION bin OPTIONAL)

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

@@ -0,0 +1,70 @@
+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_v5
+
+    - 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 7z is built locally with bundle_windows_deps_cuda.bat and uploaded to a
+        # GitHub release. Resolve it by filename via the API (authenticated with the
+        # built-in token) so it works even from a *draft* release and regardless of
+        # the (changing) asset id. Requires the workflow to run in-repo (github.token
+        # can't read introlab drafts from a fork PR).
+        $repo = "introlab/rtabmap"
+        $releaseTag = "0.23.8"
+        $assetName = "vcpkg-export-66c0373d-x64-vs2022-cuda130.7z"
+        $apiHeaders = @{ Authorization = "Bearer ${{ github.token }}"; "User-Agent" = "rtabmap-ci"; Accept = "application/vnd.github+json" }
+        # Select the release by tag_name (populated on drafts too, and unique), then
+        # take the asset from THAT release so concurrent releases can't be confused.
+        $rel = (Invoke-RestMethod -Uri "https://api.github.com/repos/$repo/releases?per_page=100" -Headers $apiHeaders) |
+               Where-Object { $_.tag_name -eq $releaseTag } | Select-Object -First 1
+        if (-not $rel) { throw "Release tagged $releaseTag not found (including drafts)" }
+        $asset = $rel.assets | Where-Object { $_.name -eq $assetName } | Select-Object -First 1
+        if (-not $asset) { throw "Asset $assetName not found in release $releaseTag" }
+        Write-Host "Downloading $assetName (asset id $($asset.id)) ..."
+        $dlHeaders = @{ Authorization = "Bearer ${{ github.token }}"; "User-Agent" = "rtabmap-ci"; Accept = "application/octet-stream" }
+        Invoke-WebRequest -Uri $asset.url -Headers $dlHeaders -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
+        # K4A runtime DLLs (k4a.dll, k4arecord.dll) live in the SDK bin; needed on PATH so fixup_bundle resolves them at package time.
+        echo "$vcpkg_path\installed\x64-windows-release\sdk\windows-desktop\amd64\release\bin" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append
+        echo "${{env.CUDA_PATH}}\bin\x64" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append
+        echo "${{env.CUDA_PATH}}\extras\CUPTI\lib64" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append
+        echo "$vcpkg_path\installed\x64-windows-release\tools\python3\Lib\site-packages\torch\lib" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append
+        echo "$vcpkg_path\installed\x64-windows-release\tools\python3\Lib\site-packages\numpy.libs" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append

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

@@ -0,0 +1,54 @@
+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-v7
+
+    - 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 7z is built locally with bundle_windows_deps.bat and uploaded to a
+        # GitHub release. Resolve it by filename via the API (authenticated with the
+        # built-in token) so it works even from a *draft* release and regardless of
+        # the (changing) asset id. Requires the workflow to run in-repo (github.token
+        # can't read introlab drafts from a fork PR).
+        $repo = "introlab/rtabmap"
+        $releaseTag = "0.23.8"
+        $assetName = "vcpkg-export-66c0373d-x64-vs2022.7z"
+        $apiHeaders = @{ Authorization = "Bearer ${{ github.token }}"; "User-Agent" = "rtabmap-ci"; Accept = "application/vnd.github+json" }
+        # Select the release by tag_name (populated on drafts too, and unique), then
+        # take the asset from THAT release so concurrent releases can't be confused.
+        $rel = (Invoke-RestMethod -Uri "https://api.github.com/repos/$repo/releases?per_page=100" -Headers $apiHeaders) |
+               Where-Object { $_.tag_name -eq $releaseTag } | Select-Object -First 1
+        if (-not $rel) { throw "Release tagged $releaseTag not found (including drafts)" }
+        $asset = $rel.assets | Where-Object { $_.name -eq $assetName } | Select-Object -First 1
+        if (-not $asset) { throw "Asset $assetName not found in release $releaseTag" }
+        Write-Host "Downloading $assetName (asset id $($asset.id)) ..."
+        $dlHeaders = @{ Authorization = "Bearer ${{ github.token }}"; "User-Agent" = "rtabmap-ci"; Accept = "application/octet-stream" }
+        Invoke-WebRequest -Uri $asset.url -Headers $dlHeaders -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
+        # K4A runtime DLLs (k4a.dll, k4arecord.dll) live in the SDK bin; needed on PATH so fixup_bundle resolves them at package time.
+        echo "$vcpkg_path\installed\x64-windows-release\sdk\windows-desktop\amd64\release\bin" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append
+        echo "$vcpkg_path\installed\x64-windows-release\tools\python3\Lib\site-packages\numpy.libs" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append

.github/scripts/ros-deps-env.sh

@@ -0,0 +1,19 @@
+# Puts the graph optimizers taken from the ROS 2 repo (see cmake-linux.yml) on
+# the loader path, for a shell that is about to run something built against
+# them. Sourced, not executed: it exports into the caller.
+#
+#   source .github/scripts/ros-deps-env.sh /opt/ros/humble
+#
+# A missing or empty prefix is a build that took every dependency from the
+# Ubuntu archive: nothing to add.
+#
+# Both directories are needed. GTSAM and g2o install their libraries in the
+# multiarch subdirectory, while the prefix's own lib/ holds the rest, and
+# libgtsam.so carries no RUNPATH -- so the loader finds neither it nor the
+# libmetis-gtsam.so it pulls in without being told where to look.
+ros_prefix="${1:-}"
+if [ -n "$ros_prefix" ] && [ -d "$ros_prefix" ]; then
+  ros_lib="$ros_prefix/lib"
+  export LD_LIBRARY_PATH="${ros_lib}:${ros_lib}/$(gcc -dumpmachine)${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}"
+  echo "LD_LIBRARY_PATH=$LD_LIBRARY_PATH"
+fi

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

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

.github/workflows/cmake-linux.yml

@@ -0,0 +1,214 @@
+name: CMake-Linux
+
+on:
+  push:
+    branches: 
+      - master
+  pull_request:
+    branches:
+      - '**'
+  workflow_dispatch:
+
+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 }}
+    concurrency:
+      group: ${{ github.workflow }}-${{ github.ref }}-${{ matrix.build_name }}
+      cancel-in-progress: true
+    strategy:
+      fail-fast: true
+      matrix:
+        build_name: [ubuntu-22.04, ubuntu-24.04, ubuntu-24.04-with-opengv, ubuntu-26.04]
+        include:
+        # GTSAM is in no Ubuntu release, and libg2o-dev only appears in the
+        # archive from noble on (jammy has no g2o at all).
+        #
+        # 26.04 builds GTSAM itself (gtsam_ref) rather than taking it from the
+        # ROS repo: that runner is on Ubuntu's amd64v3 variant, so rtabmap and
+        # the archive libraries are AVX builds where Eigen aligns to 32, while
+        # the ROS binary is plain x86-64 where it aligns to 16 and uses plain
+        # malloc/free. Allocating on one side and freeing on the other gives
+        # "double free or corruption (out)". Forcing EIGEN_MAX_ALIGN_BYTES=16
+        # on rtabmap would only move the mismatch onto PCL, g2o and Ceres.
+        - build_name: ubuntu-22.04
+          os: ubuntu-22.04
+          extra_deps: "libunwind-dev libceres-dev"
+          ros_deps: "ros-humble-libg2o ros-humble-gtsam"
+          ros_prefix: "/opt/ros/humble"
+          extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON -DWITH_G2O=ON -DWITH_GTSAM=ON"
+        - build_name: ubuntu-24.04
+          os: ubuntu-24.04
+          extra_deps: "libg2o-dev libceres-dev"
+          ros_deps: "ros-jazzy-gtsam"
+          ros_prefix: "/opt/ros/jazzy"
+          extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON -DWITH_G2O=ON -DWITH_GTSAM=ON"
+        - build_name: ubuntu-24.04-with-opengv
+          os: ubuntu-24.04
+          extra_deps: "libg2o-dev libceres-dev"
+          ros_deps: "ros-jazzy-gtsam"
+          ros_prefix: "/opt/ros/jazzy"
+          extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON -DWITH_G2O=ON -DWITH_GTSAM=ON -DBUILD_OPENGV=ON"
+        - build_name: ubuntu-26.04
+          os: ubuntu-26.04
+          # libboost-all-dev for the GTSAM build
+          extra_deps: "libg2o-dev libceres-dev libboost-all-dev"
+          ros_deps: ""
+          ros_prefix: ""
+          # The tag ros-lyrical-gtsam 4.3.0 is built from
+          gtsam_ref: "4.3a0-ros"
+          extra_cmake_def: "-DWITH_CERES=ON -DWITH_PYTHON=ON -DWITH_G2O=ON -DWITH_GTSAM=ON -DBUILD_OPENGV=ON"
+
+    steps:
+    - uses: actions/checkout@v4
+
+    - name: Restore test data
+      id: cache-testdata
+      uses: actions/cache@v4
+      with:
+        # SQLite DBs are binary-portable -> share the cache across
+        # linux / macos / windows. Key omits runner.os on purpose so all
+        # three OSes hit the same entry.
+        path: data/tests/*.db
+        key: testdata-${{ hashFiles('data/tests/manifest.txt') }}
+
+    - name: Fetch test data
+      if: steps.cache-testdata.outputs.cache-hit != 'true'
+      shell: bash
+      run: bash scripts/fetch_test_data.sh
+
+    - 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 curl gnupg lsb-release ${{ matrix.extra_deps }}
+
+    - name: Install Graph Optimizers From The ROS Repo
+      if: matrix.ros_deps != ''
+      env:
+        DEBIAN_FRONTEND: noninteractive
+      run: |
+        sudo curl -fsSL 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 $(lsb_release -cs) main" \
+          | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
+        sudo apt-get update
+        sudo apt-get -y install ${{ matrix.ros_deps }}
+
+    # The compiler's x86-64 baseline, for the cache key: it decides Eigen's
+    # alignment, so a GTSAM built under one must not be restored under the
+    # other. v3 packages still declare Architecture: amd64, so dpkg cannot tell.
+    - name: Toolchain baseline
+      if: matrix.gtsam_ref != ''
+      id: toolchain
+      run: |
+        avx=$(echo | gcc -dM -E -x c++ - | grep -c __AVX__ || true)
+        echo "gcc $(gcc -dumpversion), __AVX__=$avx"
+        echo "id=gcc$(gcc -dumpversion)-avx${avx}" >> "$GITHUB_OUTPUT"
+
+    - name: Restore GTSAM
+      if: matrix.gtsam_ref != ''
+      id: cache-gtsam
+      uses: actions/cache@v4
+      with:
+        path: ${{github.workspace}}/gtsam-install
+        key: gtsam-${{ matrix.gtsam_ref }}-${{ matrix.os }}-${{ steps.toolchain.outputs.id }}
+
+    - name: Build GTSAM
+      if: matrix.gtsam_ref != '' && steps.cache-gtsam.outputs.cache-hit != 'true'
+      run: |
+        git clone --depth 1 --branch ${{ matrix.gtsam_ref }} https://github.com/borglab/gtsam.git ${{runner.temp}}/gtsam-src
+        # Tag-specific fixes, named after the tag.
+        patch_file="${{github.workspace}}/patches/gtsam_$(echo '${{ matrix.gtsam_ref }}' | tr '.' '_').patch"
+        if [ -f "$patch_file" ]; then
+          echo "Applying $patch_file"
+          git -C ${{runner.temp}}/gtsam-src apply "$patch_file"
+        fi
+        cmake -S ${{runner.temp}}/gtsam-src -B ${{runner.temp}}/gtsam-build \
+          -DCMAKE_BUILD_TYPE=Release \
+          -DCMAKE_INSTALL_PREFIX=${{github.workspace}}/gtsam-install \
+          -DGTSAM_USE_SYSTEM_EIGEN=ON \
+          -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
+          -DGTSAM_BUILD_WITH_WERROR=OFF \
+          -DGTSAM_BUILD_TESTS=OFF \
+          -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
+          -DGTSAM_BUILD_UNSTABLE=OFF \
+          -DGTSAM_BUILD_PYTHON=OFF \
+          -DBUILD_SHARED_LIBS=ON
+        cmake --build ${{runner.temp}}/gtsam-build --target install -j $(nproc)
+
+    - name: Set up Python
+      uses: actions/setup-python@v5
+      with:
+        python-version: '3.x'
+
+    - name: Install Python Dependencies
+      run: |
+        python -m pip install --upgrade pip
+        pip install numpy pybind11
+
+    - name: Configure CMake
+      run: |
+        # Both prefixes when a job has both.
+        prefix_path="${{ matrix.ros_prefix }}"
+        if [ -d "${{github.workspace}}/gtsam-install" ]; then
+          prefix_path="${prefix_path:+$prefix_path;}${{github.workspace}}/gtsam-install"
+        fi
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DCMAKE_PREFIX_PATH="$prefix_path" -DPython3_EXECUTABLE=$(which python3) -Dpybind11_DIR=$(python3 -m pybind11 --cmakedir) ${{ matrix.extra_cmake_def }}
+
+    - name: Build
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+    - name: Info
+      working-directory: ${{github.workspace}}/build/bin
+      run: |
+        source ${{github.workspace}}/.github/scripts/ros-deps-env.sh "${{ matrix.ros_prefix }}"
+        source ${{github.workspace}}/.github/scripts/ros-deps-env.sh "${{github.workspace}}/gtsam-install"
+        ./rtabmap-console --version
+
+    - name: Test
+      working-directory: ${{github.workspace}}/build
+      # PYTHONNOUSERSITE=1: prevent the embedded Python interpreter from
+      # loading numpy / other site-packages from ~/.local that were compiled
+      # against a different ABI than the build-time Python (causes numpy
+      # 1.x/2.x mismatch crashes in test_pydetector / test_pydescriptor /
+      # test_pymatcher).
+      env:
+        PYTHONNOUSERSITE: 1
+      run: |
+        source ${{github.workspace}}/.github/scripts/ros-deps-env.sh "${{ matrix.ros_prefix }}"
+        source ${{github.workspace}}/.github/scripts/ros-deps-env.sh "${{github.workspace}}/gtsam-install"
+        # Keep a core file per crashing test, for the backtrace step below: an
+        # abort otherwise leaves only its message, and may not reproduce
+        # elsewhere.
+        ulimit -c unlimited
+        sudo sysctl -w kernel.core_pattern="${{github.workspace}}/build/core.%e.%p" > /dev/null
+        ctest -C ${{env.BUILD_TYPE}} -V -LE performance
+
+    - name: Backtrace of crashed tests
+      if: failure()
+      working-directory: ${{github.workspace}}/build
+      run: |
+        shopt -s nullglob
+        cores=(core.*)
+        if [ ${#cores[@]} -eq 0 ]; then
+          echo "No core file: the failure was an assertion or a wrong value, not a crash."
+          exit 0
+        fi
+        sudo apt-get -y install gdb
+        source ${{github.workspace}}/.github/scripts/ros-deps-env.sh "${{ matrix.ros_prefix }}"
+        source ${{github.workspace}}/.github/scripts/ros-deps-env.sh "${{github.workspace}}/gtsam-install"
+        for core in "${cores[@]}"; do
+          exe="${core#core.}"
+          exe="bin/${exe%.*}"
+          echo "::group::$core ($exe)"
+          gdb -batch -ex "thread apply all bt" "$exe" "$core" || true
+          echo "::endgroup::"
+        done

.github/workflows/cmake-macos.yml

@@ -0,0 +1,160 @@
+name: CMake-MacOS
+
+on:
+  push:
+    branches: 
+      - master
+  pull_request:
+    branches:
+      - '**'
+  workflow_dispatch:
+
+env:
+  BUILD_TYPE: Release
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
+  cancel-in-progress: ${{ github.event_name == 'pull_request' }}
+
+jobs:
+  build:
+    name: ${{ matrix.build_name }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      fail-fast: true
+      matrix:
+        build_name: [macos-sequoia-apple-silicon, macos-tahoe-apple-silicon, macos-tahoe-apple-silicon-cv5]
+        include:
+        # macos-sequoia-intel temporarily disabled. Had some g2o optimizer issues.
+        #- build_name: macos-sequoia-intel
+        #  os: macos-15-intel
+        #  cv: opencv@4
+        - build_name: macos-sequoia-apple-silicon
+          os: macos-15
+          cv: opencv@4
+        # - build_name: macos-tahoe-intel
+        #   os: macos-26-intel
+        #   cv: opencv@4
+        - build_name: macos-tahoe-apple-silicon
+          os: macos-26
+          cv: opencv@4
+        # - build_name: macos-tahoe-intel-cv5
+        #   os: macos-26-intel
+        #   cv: opencv
+        - build_name: macos-tahoe-apple-silicon-cv5
+          os: macos-26
+          cv: opencv
+    steps:
+    - uses: actions/checkout@v4
+
+    - name: Restore test data
+      id: cache-testdata
+      uses: actions/cache@v4
+      with:
+        # SQLite DBs are binary-portable -> share the cache across
+        # linux / macos / windows. Key omits runner.os on purpose so all
+        # three OSes hit the same entry.
+        path: data/tests/*.db
+        key: testdata-${{ hashFiles('data/tests/manifest.txt') }}
+
+    - name: Fetch test data
+      if: steps.cache-testdata.outputs.cache-hit != 'true'
+      shell: bash
+      run: bash scripts/fetch_test_data.sh
+
+    - name: Install Brew Dependencies
+      run: |
+        # Update brew and install from Brewfile if present, or specific packages
+        brew install pcl octomap pdal yaml-cpp librealsense libfreenect libusb zlib libomp suite-sparse ceres-solver ${{ matrix.cv }}
+
+    - name: Install Source Dependencies
+      # Build (and per-dependency cache) the source-only deps not available from
+      # Homebrew, then install them into /usr/local. See the composite action.
+      uses: ./.github/actions/install-macos-source-deps
+      with:
+        os: ${{ matrix.os }}
+        build_type: ${{ env.BUILD_TYPE }}
+
+    - name: Configure CMake
+      run: |
+        # Apple clang has no built-in OpenMP; point find_package(OpenMP) at
+        # Homebrew's keg-only libomp so PCL/g2o/rtabmap enable OpenMP instead of
+        # repeatedly logging "Could NOT find OpenMP".
+        LIBOMP=$(brew --prefix libomp)
+        cmake -B ${{github.workspace}}/build \
+          -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} \
+          -DBUILD_TESTING=ON \
+          -DBUILD_AS_BUNDLE=ON \
+          -DWITH_ORBBEC_SDK=ON \
+          -DWITH_DEPTHAI=ON \
+          -DWITH_CERES=ON \
+          -DOpenMP_C_FLAGS="-Xclang -fopenmp -I$LIBOMP/include" \
+          -DOpenMP_C_LIB_NAMES=omp \
+          -DOpenMP_CXX_FLAGS="-Xclang -fopenmp -I$LIBOMP/include" \
+          -DOpenMP_CXX_LIB_NAMES=omp \
+          -DOpenMP_omp_LIBRARY="$LIBOMP/lib/libomp.dylib"
+
+    - name: Build
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+    - name: Info
+      working-directory: ${{github.workspace}}/build/bin
+      run: |
+        ./rtabmap-console --version
+
+    - name: Build MacOS Package
+      run: |
+        cmake --build ${{ github.workspace }}/build --config ${{ env.BUILD_TYPE }} --target package
+
+    - name: Upload RTABMap Artifacts (DMG)
+      uses: actions/upload-artifact@v4
+      with:
+        name: RTABMap-Binaries-${{ matrix.build_name }}-nonsigned-zip
+        path: |
+          build/RTABMap-*.dmg
+        compression-level: 0
+        if-no-files-found: warn
+        retention-days: ${{ github.event_name == 'pull_request' && 1 || 90 }}
+
+    - name: Test
+      working-directory: ${{github.workspace}}/build
+      # PYTHONNOUSERSITE=1: prevent the embedded Python interpreter from
+      # loading numpy / other site-packages from the user's site that were
+      # compiled against a different ABI than the build-time Python.
+      env:
+        PYTHONNOUSERSITE: 1
+      run: |
+        ctest -C ${{env.BUILD_TYPE}} -V -LE performance
+
+    # A ctest SEGFAULT is reported as just "SEGFAULT" with no backtrace, which
+    # makes a crash inside a long integration test invisible -- the log simply
+    # stops mid-test. macOS's ReportCrash writes a symbolized .ips report for
+    # every crashed process, so surface any that appeared during this job.
+    - name: Dump macOS crash reports on failure
+      if: failure()
+      run: |
+        # ReportCrash can lag the process death by a few seconds.
+        sleep 20
+        found=0
+        for d in "$HOME/Library/Logs/DiagnosticReports" /Library/Logs/DiagnosticReports; do
+          [ -d "$d" ] || continue
+          while IFS= read -r f; do
+            found=1
+            echo "::group::$f"
+            cat "$f"
+            echo "::endgroup::"
+          done < <(find "$d" -type f \( -name '*.ips' -o -name '*.crash' \) -mtime -1 2>/dev/null | sort)
+        done
+        if [ "$found" = 0 ]; then
+          echo "No crash reports found -- the job may have failed without a crash."
+        fi
+
+    - name: Upload ctest log on failure
+      if: failure()
+      uses: actions/upload-artifact@v4
+      with:
+        name: ctest-log-${{ matrix.build_name }}
+        path: build/Testing/Temporary/LastTest.log
+        if-no-files-found: warn
+        retention-days: 7
+

.github/workflows/cmake-ros.yml

@@ -5,55 +5,96 @@ on:
     branches: 
       - master
   pull_request:
-    branches: 
+    branches:
       - '**'
+  workflow_dispatch:
 
 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 }}
-    runs-on: ${{ matrix.os }}
+    name: ${{ matrix.ros_distribution }}
+    runs-on: ubuntu-latest
+    concurrency:
+      group: ${{ github.workflow }}-${{ github.ref }}-${{ matrix.ros_distribution }}
+      cancel-in-progress: true
     strategy:
       fail-fast: false
       matrix:
-        ros_distribution: [ humble, jazzy ]
+        ros_distribution: [ humble, jazzy, kilted, lyrical, rolling]
         include:
         - ros_distribution: 'humble'
-          os: ubuntu-22.04
+          skip_keys: ""
         - ros_distribution: 'jazzy'
-          os: ubuntu-24.04
+          skip_keys: ""
+        - ros_distribution: 'kilted'
+          skip_keys: ""
+        - ros_distribution: 'lyrical'
+          skip_keys: "libpointmatcher"
+        - ros_distribution: 'rolling'
+          skip_keys: "libpointmatcher gtsam"
+          use_ros2_testing: true # Rolling is using ros2-testing (nightly)
+    container:
+      image: osrf/ros:${{ matrix.ros_distribution }}-desktop-full
+    steps:
+    - uses: actions/checkout@v4
+
+    - name: Restore test data
+      id: cache-testdata
+      uses: actions/cache@v4
+      with:
+        path: data/tests/*.db
+        key: testdata-${{ hashFiles('data/tests/manifest.txt') }}
+
+    - name: Fetch test data
+      if: steps.cache-testdata.outputs.cache-hit != 'true'
+      shell: bash
+      run: |
+        # The ROS images are slim; fetch_test_data.sh needs curl, and 7-Zip for
+        # the assets that come as an archive. The package holding it is
+        # p7zip-full up to Ubuntu 24.04 and 7zip on the newer ones.
+        command -v curl >/dev/null || (apt-get update && apt-get install -y --no-install-recommends curl)
+        command -v 7z >/dev/null || command -v 7zz >/dev/null || (apt-get update && \
+          (apt-get install -y --no-install-recommends p7zip-full || \
+           apt-get install -y --no-install-recommends 7zip))
+        bash scripts/fetch_test_data.sh
+
+    # `${{ github.workspace }}` is expanded by the runner on the *host*
+    # (/home/runner/work/...), but this job runs in a container where the runner
+    # bind-mounts that directory somewhere else (/__w/...). Baking the host path
+    # into RTABMAP_TEST_DATA_ROOT points every data-driven test at a directory
+    # that does not exist in the container: the integration tests silently skip
+    # and the ones that imread() committed fixtures abort outright.
+    #
+    # Resolve the path inside the container instead. A `run` step's cwd is the
+    # checkout root -- the same assumption the relative `scripts/...` and
+    # `data/tests/*.db` paths in the steps above already rely on -- so derive it
+    # from $PWD rather than trusting a path computed outside the container. The
+    # existence checks then turn that into a verified precondition: a wrong
+    # mount point or a failed fetch fails here, with the path printed, instead
+    # of degrading into skipped tests.
+    - name: Resolve test data root in the container
+      id: testdata
+      shell: bash
+      run: |
+        root="$PWD/data"
+        test -f "$root/samples/17.jpg" || { echo "::error::test data root $root is missing committed fixtures"; exit 1; }
+        ls "$root"/tests/*.db >/dev/null || { echo "::error::no test databases in $root/tests"; exit 1; }
+        echo "root=$root" >> "$GITHUB_OUTPUT"
 
-    steps:   
     - uses: ros-tooling/setup-ros@v0.7
       with:
         required-ros-distributions: ${{ matrix.ros_distribution }}
-    
-    - uses: actions/checkout@v4
-
-    - name: Install dependencies
-      run: |
-        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
-        rosdep update
-        rosdep install --from-paths ${{github.workspace}} -y
-
-    - name: Configure CMake
-      run: |
-        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
-        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
-
-    - name: Build
-      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
-
-    - name: Info
-      working-directory: ${{github.workspace}}/build/bin
-      run: |
-        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
-        ./rtabmap-console --version
-      
+        use-ros2-testing: ${{ matrix.use_ros2_testing || false }}
+    - uses: ros-tooling/action-ros-ci@v0.4
+      with:
+        package-name: rtabmap
+        target-ros2-distro: ${{ matrix.ros_distribution }}
+        rosdep-skip-keys: "${{ matrix.skip_keys }}"
+        coverage-result: false
+        extra-cmake-args: -DTEST_DATA_ROOT=${{ steps.testdata.outputs.root }}

.github/workflows/cmake-windows.yml

@@ -0,0 +1,171 @@
+name: CMake-Windows
+
+on:
+  push:
+    branches: 
+      - master
+  pull_request:
+    branches:
+      - '**'
+  workflow_dispatch:
+
+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: '-DWITH_TORCH=OFF -DWITH_ZED=OFF'
+        - build_name: windows-2022-cuda
+          os: windows-2022
+          extra_deps: ""
+          extra_cmake_def: '-DWITH_TORCH=ON -DWITH_ZED=ON -DWITH_CUDASIFT=ON'
+
+    steps:
+    - uses: actions/checkout@v4
+
+    - name: Restore test data
+      id: cache-testdata
+      if: matrix.build_name != 'windows-2022-cuda'
+      uses: actions/cache@v4
+      with:
+        # SQLite DBs are binary-portable -> share the cache across
+        # linux / macos / windows. Key omits runner.os on purpose so all
+        # three OSes hit the same entry.
+        path: data/tests/*.db
+        key: testdata-${{ hashFiles('data/tests/manifest.txt') }}
+
+    - name: Fetch test data
+      if: matrix.build_name != 'windows-2022-cuda' && steps.cache-testdata.outputs.cache-hit != 'true'
+      shell: bash
+      run: bash scripts/fetch_test_data.sh
+
+    - 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: |
+        # K4A, K4W2 and ZED are located via these env vars (FindK4A.cmake / FindKinectSDK2.cmake
+        # and the install rules), pointing at the export where the bundle staged their SDK files.
+        $env:K4A_ROOT_DIR = "${{env.VCPKG_EXPORT_PATH}}/installed/x64-windows-release"
+        $env:KINECTSDK20_DIR = "${{env.VCPKG_EXPORT_PATH}}/installed/x64-windows-release"
+        $env:ZED_SDK_ROOT_DIR = "${{env.VCPKG_EXPORT_PATH}}/installed/x64-windows-release"
+        cmake `
+          -B ${{github.workspace}}/build `
+          -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} `
+          -DBUILD_AS_BUNDLE=ON `
+          -DWITH_PYTHON=ON `
+          -DWITH_CERES=ON `
+          -DWITH_ORBBEC_SDK=ON `
+          -DWITH_FREENECT2=ON `
+          -DWITH_K4W2=ON `
+          -DWITH_K4A=ON `
+          -DWITH_DEPTHAI=ON `
+          -DWITH_REALSENSE2=ON `
+          -DWITH_CCCORELIB=ON `
+          ${{ matrix.extra_cmake_def }} `
+          -DBUILD_TESTING=ON `
+          -DVCPKG_MANIFEST_INSTALL=OFF `
+          -DVCPKG_TARGET_TRIPLET=x64-windows-release `
+          -DVCPKG_INSTALLED_DIR="${{env.VCPKG_EXPORT_PATH}}/installed" `
+          -DCMAKE_TOOLCHAIN_FILE=${{env.VCPKG_EXPORT_PATH}}/scripts/buildsystems/vcpkg.cmake `
+          -DTorch_DIR=${{env.VCPKG_EXPORT_PATH}}/installed/x64-windows-release/tools/python3/Lib/site-packages/torch/share/cmake/Torch
+
+    - name: Build
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} --target ALL_BUILD
+
+    - name: Test
+      # Not run on the CUDA build, which is a build+package job only.
+      #
+      # The runner has no NVIDIA GPU or driver (see the driver-DLL note in
+      # app/src/CMakeLists.txt), so no cv::cuda / CUDASIFT path can actually
+      # execute there -- everything falls back to CPU and the run just repeats
+      # the coverage the non-CUDA job already gives, on the same sources. It
+      # used to cost more than it returned: the two cv::cuda-touching tests
+      # (test_util3d_features, test_localgrid) had to be excluded because lazy
+      # CUDA-init stalled them past the 300 s timeout, and once WITH_ZED=ON was
+      # added to this job every remaining corelib test died in the loader with
+      # 0xc0000135 (STATUS_DLL_NOT_FOUND) before reaching main(): rtabmap_core
+      # links sl_zed64.dll, which imports the driver-only nvcuvid.dll /
+      # nvEncodeAPI64.dll that a driver-less runner does not have.
+      #
+      # So this job compiles, links and packages the CUDA artifact; the
+      # non-CUDA job is what verifies behaviour. Same reason the Info step
+      # below is skipped here.
+      if: matrix.build_name != 'windows-2022-cuda'
+      working-directory: ${{github.workspace}}/build
+      # PYTHONHOME points the embedded interpreter at vcpkg's bundled
+      # Python install (where Lib/, DLLs/ live). Without it Python emits
+      # "Could not find platform independent libraries <prefix>" at init
+      # and runs without a stdlib, breaking every numpy import.
+      # PYTHONNOUSERSITE=1: prevent the embedded Python interpreter from
+      # loading numpy / other site-packages from %APPDATA%\Python that
+      # were compiled against a different ABI than the build-time Python
+      # (numpy 1.x/2.x mismatch crashes in test_pydetector /
+      # test_pydescriptor / test_pymatcher).
+      env:
+        PYTHONHOME: ${{env.VCPKG_EXPORT_PATH}}/installed/x64-windows-release/tools/python3
+        PYTHONNOUSERSITE: 1
+      run: |
+        ctest -C ${{env.BUILD_TYPE}} -V --timeout 300 -LE performance
+
+    - name: Info
+      # Skipped for CUDA: the binary links ZED (sl_zed64.dll -> nvcuvid/nvEncodeAPI64),
+      # which need the NVIDIA driver; the GPU-less runner can't load the exe.
+      if: matrix.build_name != 'windows-2022-cuda'
+      working-directory: ${{github.workspace}}/build/bin
+      run: |
+        ./rtabmap-console --version
+
+    - 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: 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 }}

.github/workflows/cmake.yml

@@ -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}}
-      

.github/workflows/coverage.yml

@@ -0,0 +1,198 @@
+name: Coverage
+
+on:
+  push:
+    branches:
+      - master
+  pull_request:
+    branches:
+      - '**'
+  workflow_dispatch:
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
+  cancel-in-progress: ${{ github.event_name == 'pull_request' }}
+
+permissions:
+  contents: read
+  id-token: write
+
+env:
+  BUILD_TYPE: Debug
+
+jobs:
+  coverage:
+    name: Coverage (ubuntu-24.04)
+    runs-on: ubuntu-24.04
+    env:
+      CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }}
+
+    steps:
+      - uses: actions/checkout@v4
+
+      - name: Restore test data
+        id: cache-testdata
+        uses: actions/cache@v4
+        with:
+          # Same key as the other workflows so all of them share one entry.
+          path: data/tests/*.db
+          key: testdata-${{ hashFiles('data/tests/manifest.txt') }}
+
+      - name: Fetch test data
+        if: steps.cache-testdata.outputs.cache-hit != 'true'
+        shell: bash
+        run: bash scripts/fetch_test_data.sh
+
+      - name: Install Linux dependencies
+        run: |
+          DEBIAN_FRONTEND=noninteractive
+          sudo apt-get update
+          sudo apt-get install -y \
+            lcov \
+            libopencv-dev \
+            libpcl-dev \
+            git \
+            cmake \
+            software-properties-common \
+            libyaml-cpp-dev \
+            libg2o-dev \
+            libceres-dev \
+            curl \
+            gnupg \
+            lsb-release
+
+          # GTSAM and libpointmatcher are not in the Ubuntu archive, and
+          # borglab's gtsam PPAs have no noble packages (gtsam-release-4.2 has
+          # no noble suite; gtsam-develop's noble Packages index is empty). The
+          # ROS 2 repo ships both for noble, which is also what the ROS CI jobs
+          # test against, so take them from there.
+          sudo curl -fsSL 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 $(lsb_release -cs) main" \
+            | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
+          sudo apt-get update
+          sudo apt-get install -y \
+            ros-jazzy-gtsam \
+            ros-jazzy-libpointmatcher
+
+      - name: Configure CMake
+        run: |
+          cmake -B ${{ github.workspace }}/build \
+            -DCMAKE_BUILD_TYPE=${{ env.BUILD_TYPE }} \
+            -DENABLE_COVERAGE=ON \
+            -DCMAKE_PREFIX_PATH=/opt/ros/jazzy \
+            -DBUILD_TESTING=ON \
+            -DBUILD_APP=OFF \
+            -DBUILD_TOOLS=OFF \
+            -DBUILD_EXAMPLES=OFF \
+            -DWITH_QT=OFF \
+            -DWITH_PYTHON=OFF \
+            -DWITH_CERES=ON \
+            -DWITH_G2O=ON \
+            -DWITH_GTSAM=ON \
+            -DWITH_MRPT=OFF \
+            -DWITH_CVSBA=OFF \
+            -DWITH_POINTMATCHER=ON \
+            -DWITH_CCCORELIB=OFF \
+            -DWITH_OPEN3D=OFF \
+            -DWITH_LOAM=OFF \
+            -DWITH_FLOAM=OFF \
+            -DWITH_LIOSAM=OFF \
+            -DWITH_FLYCAPTURE2=OFF \
+            -DWITH_ZED=OFF \
+            -DWITH_ZEDOC=OFF \
+            -DWITH_REALSENSE=OFF
+
+      - name: Build
+        run: cmake --build ${{ github.workspace }}/build -j$(nproc)
+
+      - name: Test
+        working-directory: ${{ github.workspace }}/build
+        run: |
+          # The ROS packages are not on the default loader path. Both
+          # directories are needed: GTSAM and libpointmatcher land in
+          # /opt/ros/jazzy/lib, but gtsam's own libmetis-gtsam.so goes to the
+          # multiarch subdirectory, and libgtsam.so carries no RUNPATH, so the
+          # loader can only find it through LD_LIBRARY_PATH.
+          ROS_LIB=/opt/ros/jazzy/lib
+          export LD_LIBRARY_PATH="${{ github.workspace }}/build/bin:${ROS_LIB}:${ROS_LIB}/$(gcc -dumpmachine)${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}"
+          find ${{ github.workspace }}/build -name '*.gcda' -delete
+          # Unit tests only ("long" is the end-to-end replay suite).
+          #
+          # Two reasons. It costs ~90 min in this instrumented -O0 build, 94% of
+          # the job's test time, for ~3 points of coverage. And running it on
+          # some events but not others makes the numbers incomparable: Codecov
+          # measures a pull request against master, so a unit-only PR report
+          # against a full master report reads as a coverage regression on every
+          # PR. Same scope everywhere keeps the comparison meaningful.
+          #
+          # The replays still run (and gate) in the cmake-linux / macos /
+          # windows jobs; they are just not measured here.
+          ctest -V -LE "long|performance"
+
+      - name: Generate LCOV report
+        run: |
+          GCOV_VER="$(gcc -dumpversion | cut -d. -f1)"
+          if command -v "gcov-${GCOV_VER}" >/dev/null 2>&1; then
+            GCOV_TOOL="gcov-${GCOV_VER}"
+          else
+            GCOV_TOOL="gcov"
+          fi
+          LCOV_IGNORE=(--ignore-errors gcov,source,graph,mismatch,unused)
+
+          # The whole of corelib/utilite, test objects included, not just their src/.
+          # An inline function defined in a header is emitted in whichever
+          # translation unit wins comdat folding, which is often a test one, and
+          # its counters then live in that unit's .gcda: capturing only src/
+          # reports such a function as uncovered however often it is called. The
+          # test sources themselves are dropped below, after the counters are read.
+          lcov --gcov-tool "$GCOV_TOOL" "${LCOV_IGNORE[@]}" \
+            --capture \
+            --directory ${{ github.workspace }}/build/corelib \
+            --directory ${{ github.workspace }}/build/utilite \
+            --output-file lcov.info
+
+          lcov "${LCOV_IGNORE[@]}" --extract lcov.info \
+            '${{ github.workspace }}/*' \
+            --output-file lcov.info
+          # The test sources are the instrument, not the subject: a line in a
+          # test counts as uncovered only when the test skipped it (a defensive
+          # cleanup branch, a platform guard), which says nothing about the
+          # library. They are also near-fully covered by construction, so
+          # leaving them in inflates the overall number. Excluded here, before
+          # the upload, so the HTML artifact, the summary below and Codecov all
+          # report the same figure. Kept in sync with coverage-report.sh.
+          lcov "${LCOV_IGNORE[@]}" --remove lcov.info \
+            '*/sqlite3/*' \
+            '*/rtflann/*' \
+            '*/corelib/test/*' \
+            '*/utilite/test/*' \
+            '*/_deps/*' \
+            --output-file lcov.info
+
+          lcov --summary lcov.info
+
+      - name: HTML coverage report
+        run: genhtml --ignore-errors source,mismatch lcov.info --output-directory coverage-html --legend --demangle-cpp
+
+      - name: Upload HTML coverage artifact
+        uses: actions/upload-artifact@v4
+        with:
+          name: coverage-html
+          path: coverage-html
+          retention-days: 14
+
+      - name: Upload to Codecov
+        if: ${{ env.CODECOV_TOKEN != '' }}
+        uses: codecov/codecov-action@v5
+        with:
+          files: lcov.info
+          # Upload ONLY lcov.info. By default the CLI also searches the tree and
+          # runs gcov over every .gcno it finds, uploading those .gcov files
+          # alongside -- which re-introduced corelib/test and utilite/test on
+          # Codecov even though the lcov --remove step had dropped them (the
+          # HTML artifact, built from the same lcov.info, was correctly clean).
+          disable_search: true
+          plugins: noop
+          token: ${{ env.CODECOV_TOKEN }}
+          fail_ci_if_error: false

.github/workflows/docker.yml

@@ -4,6 +4,14 @@ on:
   push:
     branches:
       - 'master'
+  pull_request:
+    branches:
+      - '**'
+  workflow_dispatch:
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
+  cancel-in-progress: ${{ github.event_name == 'pull_request' }}
 
 jobs:
   docker_deps:
@@ -15,14 +23,16 @@ jobs:
     #    $ sudo apt-get upgrade qemu-user-static
     #    $ docker run --rm --privileged multiarch/qemu-user-static --reset -p yes -c yes
     # More info: https://github.com/introlab/rtabmap/issues/1454
-    if: false
+    # Skipped on pull requests; built and pushed only from master (push or
+    # manual dispatch), since it pushes the :*-deps tags to Docker Hub.
+    if: github.ref == 'refs/heads/master'
 
     runs-on: ubuntu-latest
     
     strategy:
       fail-fast: false
       matrix:
-        docker_tag: [focal-deps, jammy-deps, jammy-iron-deps, noble-deps]
+        docker_tag: [focal-deps, jammy-deps, noble-deps, noble-kilted-deps, resolute-deps]
         include:
         - docker_tag: focal-deps
           docker_tags: |
@@ -30,7 +40,6 @@ jobs:
           docker_platforms: |
             linux/amd64
             linux/arm64
-            linux/arm/v7
           docker_path: 'focal/deps'
         - docker_tag: jammy-deps
           docker_tags: |
@@ -39,12 +48,6 @@ jobs:
             linux/amd64
             linux/arm64
           docker_path: 'jammy/deps'
-        - docker_tag: jammy-iron-deps
-          docker_tags: |
-            introlab3it/rtabmap:jammy-iron-deps
-          docker_platforms: |
-            linux/amd64
-          docker_path: 'jammy-iron/deps'
         - docker_tag: noble-deps
           docker_tags: |
             introlab3it/rtabmap:noble-deps
@@ -52,6 +55,20 @@ jobs:
             linux/amd64
             linux/arm64
           docker_path: 'noble/deps'
+        - docker_tag: noble-kilted-deps
+          docker_tags: |
+            introlab3it/rtabmap:noble-kilted-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'noble-kilted/deps'
+        - docker_tag: resolute-deps
+          docker_tags: |
+            introlab3it/rtabmap:resolute-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'resolute/deps'
     
     steps:
       -
@@ -84,13 +101,16 @@ jobs:
           cache-to: type=inline
   
   docker:
-    #needs: docker_deps
+    needs: docker_deps
+    # Run even when docker_deps is skipped (it is, on pull requests).
+    # A manual dispatch is honored only on master, the only ref we push from.
+    if: ${{ !cancelled() && !failure() && (github.event_name != 'workflow_dispatch' || github.ref == 'refs/heads/master') }}
     runs-on: ubuntu-latest
     
     strategy:
       fail-fast: false
       matrix:
-        docker_tag: [bionic, focal, jammy, jammy-iron, noble, android23, android24, android26, android30]
+        docker_tag: [bionic, focal, jammy, noble, noble-kilted, resolute, android23, android24, android26, android30]
         include:
         - docker_tag: bionic
           docker_tags: |
@@ -112,7 +132,6 @@ jobs:
           docker_platforms: |
             linux/amd64
             linux/arm64
-            linux/arm/v7
           docker_path: 'focal'
         - docker_tag: jammy
           docker_tags: |
@@ -124,14 +143,6 @@ jobs:
             linux/amd64
             linux/arm64
           docker_path: 'jammy'
-        - docker_tag: jammy-iron
-          docker_tags: |
-            introlab3it/rtabmap:jammy-iron
-          docker_args: |
-            NOT_USED=0
-          docker_platforms: |
-            linux/amd64
-          docker_path: 'jammy-iron'
         - docker_tag: noble
           docker_tags: |
             introlab3it/rtabmap:noble
@@ -142,6 +153,25 @@ jobs:
             linux/amd64
             linux/arm64
           docker_path: 'noble'
+        - docker_tag: noble-kilted
+          docker_tags: |
+            introlab3it/rtabmap:noble-kilted
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'noble-kilted'
+        - docker_tag: resolute
+          docker_tags: |
+            introlab3it/rtabmap:resolute
+            introlab3it/rtabmap:26.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'resolute'
         - docker_tag: android23
           docker_tags: |
             introlab3it/rtabmap:android23
@@ -150,7 +180,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 +188,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 +196,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 +204,7 @@ jobs:
             API_VERSION=30
           docker_platforms: |
             linux/amd64
-          docker_path: 'bionic/android/rtabmap_apiXX'
+          docker_path: 'noble/android/rtabmap_apiXX'
     
     steps:
       -
@@ -190,6 +220,9 @@ jobs:
         uses: docker/setup-buildx-action@v3
       -
         name: Login to DockerHub
+        # Only needed when pushing; skipped on pull requests (secrets are
+        # unavailable for fork PRs and we don't push there anyway).
+        if: github.event_name != 'pull_request'
         uses: docker/login-action@v3
         with:
           username: ${{ secrets.DOCKERHUB_USERNAME }}
@@ -199,8 +232,8 @@ jobs:
         uses: docker/build-push-action@v6
         with:
           context: .
-          push: true
-          platforms: ${{ matrix.docker_platforms }}
+          push: ${{ github.event_name != 'pull_request' }}
+          platforms: ${{ github.event_name == 'pull_request' && 'linux/amd64' || matrix.docker_platforms }}
           file: ./docker/${{ matrix.docker_path }}/Dockerfile
           build-args: |
             ${{ matrix.docker_args }}

.github/workflows/docs-preview-cleanup.yml

@@ -0,0 +1,54 @@
+name: Documentation preview cleanup
+
+# Removes the per-PR documentation preview published by docs.yml under
+# gh-pages:preview/pr-<number>/ once the pull request is closed (merged or not).
+#
+# Deliberately has no `paths` filter: docs.yml only publishes a preview when
+# documentation-related files changed, but this must fire for every close so a
+# preview can never be orphaned on the branch.
+on:
+  pull_request:
+    types: [closed]
+
+permissions:
+  contents: read
+
+jobs:
+  cleanup:
+    name: Remove PR preview
+    # Forks never get a preview (their token is read-only), so nothing to clean.
+    if: github.event.pull_request.head.repo.full_name == github.repository
+    runs-on: ubuntu-24.04
+
+    # Same group as docs.yml's publish job: both push to gh-pages, and two
+    # concurrent pushes would collide.
+    concurrency:
+      group: gh-pages-publish
+      cancel-in-progress: false
+
+    permissions:
+      contents: write
+
+    steps:
+      - name: Checkout gh-pages
+        uses: actions/checkout@v4
+        with:
+          ref: gh-pages
+          fetch-depth: 1
+
+      - name: Remove preview directory
+        env:
+          PREVIEW_DIR: preview/pr-${{ github.event.number }}
+        run: |
+          set -eux
+          if [[ ! -d "${PREVIEW_DIR}" ]]; then
+            echo "No preview at ${PREVIEW_DIR}, nothing to do."
+            exit 0
+          fi
+          git config user.name 'github-actions[bot]'
+          git config user.email 'github-actions[bot]@users.noreply.github.com'
+          git rm -r --quiet "${PREVIEW_DIR}"
+          # Drop the parent too once the last preview is gone.
+          rmdir preview 2>/dev/null || true
+          git commit -m "Remove preview for PR #${{ github.event.number }}"
+          git push

.github/workflows/docs.yml

@@ -0,0 +1,218 @@
+name: Documentation
+
+on:
+  push:
+    branches:
+      - master
+    tags:
+      - '*'
+    # Publishing commits to the gh-pages branch, so only run when something that
+    # affects the documentation actually changed. Use the manual "Run workflow"
+    # button if a deploy is needed outside of these paths.
+    paths: &docs_paths
+      - '.github/workflows/docs.yml'
+      - 'CMakeLists.txt'
+      - 'Doxyfile.in'
+      - 'docs-report.sh'
+      - 'website/**'
+      - 'doxygen/**'
+      - 'package.xml'
+      - 'corelib/include/**'
+      - 'utilite/include/**'
+  pull_request:
+    branches:
+      - '**'
+    paths: *docs_paths
+  workflow_dispatch:
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
+  cancel-in-progress: ${{ github.event_name == 'pull_request' }}
+
+permissions:
+  contents: read
+
+jobs:
+  build:
+    name: Build site (ubuntu-24.04)
+    runs-on: ubuntu-24.04
+
+    outputs:
+      version: ${{ steps.site.outputs.version }}
+
+    steps:
+      - uses: actions/checkout@v4
+
+      - name: Install dependencies
+        run: |
+          DEBIAN_FRONTEND=noninteractive
+          sudo apt-get update
+          sudo apt-get install -y \
+            doxygen \
+            graphviz \
+            cmake \
+            libopencv-dev \
+            libpcl-dev \
+            libyaml-cpp-dev \
+            git
+
+      # Configures build-docs (CMake generates the export headers that are part
+      # of Doxygen's INPUT), runs Doxygen into build-docs/api/latest/. Same
+      # script used locally, so a local preview matches what gets published.
+      - name: Generate C++ API documentation (Doxygen)
+        run: ./docs-report.sh
+
+      # The theme emits absolute asset links built from site.baseurl, which
+      # GitHub's own Jekyll infers from the repository (/rtabmap). A PR preview
+      # is served from a deeper path, so those links would 404 there. Pin
+      # baseurl to the path this particular build gets published to.
+      - name: Set Jekyll baseurl
+        run: |
+          set -eux
+          if [ "${{ github.event_name }}" = "pull_request" ]; then
+            baseurl="/${{ github.event.repository.name }}/preview/pr-${{ github.event.number }}"
+          else
+            baseurl="/${{ github.event.repository.name }}"
+          fi
+          echo "baseurl: ${baseurl}" >> website/_config.yml
+
+      # Landing page: built here rather than by GitHub's own Jekyll, so that
+      # .nojekyll can be set below and the Doxygen output is served untouched.
+      - name: Build landing page (Jekyll)
+        uses: actions/jekyll-build-pages@v1
+        with:
+          source: ./website
+          destination: ./_site
+
+      - name: Assemble site
+        id: site
+        run: |
+          set -eux
+          # jekyll-build-pages is a container action running as root, so _site
+          # comes back owned by root and the steps below (running as the runner
+          # user) cannot write into it.
+          sudo chown -R "$(id -u):$(id -g)" _site
+
+          # Serve everything verbatim: the site is already built, and Jekyll
+          # would otherwise skip Doxygen files/folders starting with '_'.
+          touch _site/.nojekyll
+
+          mkdir -p _site/api
+          cp -r build-docs/api/latest _site/api/latest
+
+          # On a release tag (plain X.Y.Z, not the ROS-distro variants such as
+          # 0.23.7-jazzy), also publish under the version number. Older versions
+          # already on the branch are preserved by keep_files when publishing.
+          version=""
+          if [[ "${GITHUB_REF_TYPE}" == "tag" && "${GITHUB_REF_NAME}" =~ ^[0-9]+\.[0-9]+\.[0-9]+$ ]]; then
+            version="${GITHUB_REF_NAME}"
+            cp -r build-docs/api/latest "_site/api/${version}"
+          fi
+          echo "version=${version}" >> "$GITHUB_OUTPUT"
+
+          # The dropdown list is maintained by hand in the repository (one entry
+          # per release) and shared by every published version -- see the header
+          # comment in doxygen/versions.js.
+          cp doxygen/versions.js _site/api/versions.js
+
+          # /api/ has no content of its own: send it to the current docs so a
+          # bare .../api/ link lands somewhere useful instead of a 404. The
+          # target is relative, so it works at the site root and under a
+          # preview prefix alike.
+          printf '%s\n' \
+            '<!doctype html>' \
+            '<meta charset="utf-8">' \
+            '<title>RTAB-Map API documentation</title>' \
+            '<meta http-equiv="refresh" content="0; url=latest/">' \
+            '<link rel="canonical" href="latest/">' \
+            '<p>Redirecting to the <a href="latest/">latest API documentation</a>.</p>' \
+            > _site/api/index.html
+
+          test -f _site/index.html
+          test -f _site/api/index.html
+          test -f _site/api/latest/index.html
+
+      - name: Upload site artifact
+        uses: actions/upload-artifact@v4
+        with:
+          name: documentation-site
+          path: _site
+          # include-hidden-files keeps .nojekyll, which the publish job needs.
+          include-hidden-files: true
+          retention-days: 14
+
+  publish:
+    name: Publish to gh-pages
+    needs: build
+    # Forks are skipped for pull requests: their GITHUB_TOKEN is read-only, and
+    # granting write access to a workflow that builds untrusted code would be a
+    # security hole.
+    if: >-
+      github.event_name != 'pull_request' ||
+      github.event.pull_request.head.repo.full_name == github.repository
+    runs-on: ubuntu-24.04
+
+    # Serialize every push to gh-pages, including the preview cleanup workflow,
+    # and never cancel one in flight: two concurrent pushes would collide.
+    concurrency:
+      group: gh-pages-publish
+      cancel-in-progress: false
+
+    permissions:
+      contents: write
+
+    steps:
+      - name: Download site
+        uses: actions/download-artifact@v4
+        with:
+          name: documentation-site
+          path: _site
+
+      # Pull requests go to preview/pr-<number>/, everything else to the site
+      # root. keep_files preserves what this run does not carry: the published
+      # version folders (api/0.23.x/) and the other PRs' previews.
+      - name: Publish
+        uses: peaceiris/actions-gh-pages@v4
+        with:
+          github_token: ${{ secrets.GITHUB_TOKEN }}
+          publish_branch: gh-pages
+          publish_dir: ./_site
+          destination_dir: ${{ github.event_name == 'pull_request' && format('preview/pr-{0}', github.event.number) || '' }}
+          keep_files: true
+          # Do not let the action drop a .nojekyll at the *branch root*: that
+          # disables Jekyll for everything on the branch, including content
+          # published outside this deployment. The site we publish carries its
+          # own .nojekyll inside destination_dir, which is the only place it
+          # should apply.
+          enable_jekyll: true
+          user_name: 'github-actions[bot]'
+          user_email: 'github-actions[bot]@users.noreply.github.com'
+          commit_message: >-
+            ${{ github.event_name == 'pull_request'
+                && format('Preview for PR #{0}', github.event.number)
+                || format('Update documentation ({0})', needs.build.outputs.version || 'latest') }}
+
+      - name: Link the site
+        run: |
+          base="https://${{ github.repository_owner }}.github.io/${{ github.event.repository.name }}"
+          if [[ "${{ github.event_name }}" == "pull_request" ]]; then
+            url="${base}/preview/pr-${{ github.event.number }}"
+            note="Removed automatically when this pull request is closed."
+          else
+            url="${base}"
+            note=""
+          fi
+          {
+            echo "### Documentation"
+            echo ""
+            echo "- Landing page: ${url}/"
+            echo "- C++ API: ${url}/api/latest/"
+            # An `if` rather than `[[ ... ]] && { ... }`: as the last command in
+            # the group, a false test would make the group (and so the whole
+            # step) exit 1. `note` is empty on push, which failed every master
+            # run while pull requests passed.
+            if [[ -n "${note}" ]]; then
+              echo ""
+              echo "${note}"
+            fi
+          } >> "$GITHUB_STEP_SUMMARY"

.github/workflows/ios.yml

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

.gitignore

@@ -12,3 +12,23 @@ compile_flags.txt
 tags
 build_*
 *.bak
+build-*
+
+# Coverage (coverage-report.sh / .github/workflows/coverage.yml)
+coverage-html/
+lcov.info
+*.gcda
+*.gcno
+
+# Doxygen (docs-report.sh / .github/workflows/docs.yml)
+doc/
+# Byte-code of doxygen/generate_parameters_page.py
+__pycache__/
+# Generated from Doxyfile.in by CMake.
+Doxyfile
+
+# Windows bundle: vcpkg tree, build output, and deps built from source (bundle_windows_deps.bat)
+vcpkg/
+vcpkg_installed/
+vcpkg_binaries/
+vcpkg_deps_from_source/

CMakeLists.txt

@@ -1,5 +1,6 @@
 # Top-Level CmakeLists.txt
 cmake_minimum_required(VERSION 3.14)
+
 PROJECT( RTABMap )
 SET(PROJECT_PREFIX rtabmap)
 
@@ -11,6 +12,7 @@ IF(NOT MULTI_ARCH AND NOT DEFINED CMAKE_INSTALL_LIBDIR)
 ENDIF(NOT MULTI_ARCH AND NOT DEFINED CMAKE_INSTALL_LIBDIR)
 
 INCLUDE(GNUInstallDirs)
+INCLUDE(FetchContent)
 
 ####### local cmake modules #######
 SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
@@ -19,10 +21,18 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 21)
-SET(RTABMAP_PATCH_VERSION 13)
+SET(RTABMAP_MINOR_VERSION 23)
+SET(RTABMAP_PATCH_VERSION 11)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
+
+# Make sure we have valid version so that RTABMAP_VERSION_COMPARE logic in Version.h works
+IF(RTABMAP_MINOR_VERSION GREATER 99)
+MESSAGE(FATAL_ERROR "RTABMAP_MINOR_VERSION must be < 100, bump major version and restart minor to 0!")
+ENDIF()
+IF(RTABMAP_PATCH_VERSION GREATER 99)
+MESSAGE(FATAL_ERROR "RTABMAP_PATCH_VERSION must be < 100, bump minor version and restart patch to 0!")
+ENDIF()
   
 SET(PROJECT_VERSION "${RTABMAP_VERSION}")
 
@@ -53,10 +63,6 @@ ELSE()
     option(FLANN_KDTREE_MEM_OPT  "Disable multi-threaded FLANN kd-tree to minimize memory allocations"   ON)
 ENDIF()
 
-IF(FLANN_KDTREE_MEM_OPT)
-    ADD_DEFINITIONS("-DFLANN_KDTREE_MEM_OPT")
-ENDIF(FLANN_KDTREE_MEM_OPT)
-
 IF(WIN32 AND NOT MINGW)
     ADD_DEFINITIONS("-DNOMINMAX")
     ADD_DEFINITIONS("-wd4100 -wd4127 -wd4150 -wd4191 -wd4242 -wd4244 -wd4251 -wd4305 -wd4365 -wd4512 -wd4514 -wd4548 -wd4571 -wd4619 -wd4625 -wd4626 -wd4628 -wd4668 -wd4710 -wd4711 -wd4738 -wd4820 -wd4946 -wd4986")
@@ -75,6 +81,11 @@ endif()
 if(POLICY CMP0072)
     cmake_policy(SET CMP0072 NEW)
 endif()
+# Timestamp the files extracted from a FetchContent URL download at extraction
+# time, so a changed URL actually triggers a rebuild of what depends on them.
+if(POLICY CMP0135)
+    cmake_policy(SET CMP0135 NEW)
+endif()
 
 IF(MINGW)
     # Hide the --enable-auto-import warning
@@ -83,14 +94,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
@@ -165,6 +168,68 @@ ENDIF(APPLE OR WIN32)
 OPTION(BUILD_APP "Build main application" ON)
 OPTION(BUILD_TOOLS "Build tools" ON)
 OPTION(BUILD_EXAMPLES "Build examples" ON)
+# Off by default: it downloads the Doxygen theme at configure time, which an
+# ordinary build has no reason to need. Set by docs-report.sh.
+OPTION(BUILD_DOCUMENTATION "Prepare the API documentation build (downloads the Doxygen theme)" OFF)
+
+# The unit tests are host executables run by ctest, which can't be done from a
+# cross-compiled Android/iOS build.
+if(ANDROID OR IOS OR CMAKE_SYSTEM_NAME STREQUAL "iOS")
+   set(BUILD_TESTING OFF CACHE BOOL "Build the testing tree." FORCE)
+endif()
+
+# Performance tests are benchmarks: they report times, memory and recall
+# instead of asserting on them, as those depend on the machine. They are built
+# by default so they cannot rot, but are labelled "performance" so that a run
+# opts in with `ctest -L performance` and CI opts out with `ctest -LE
+# performance`. They can also be run directly, e.g. bin/test_flann_index_perf.
+OPTION(BUILD_PERF_TESTS "Build the performance tests (see ctest -L performance)" ON)
+
+include(CTest)
+if(BUILD_TESTING)
+   # Add GTest using FetchContent
+   include(FetchContent)
+   FetchContent_Declare(
+       googletest
+       URL https://github.com/google/googletest/archive/refs/tags/v1.16.0.zip
+   )
+   FetchContent_GetProperties(googletest)
+   if(NOT googletest_POPULATED)
+     FetchContent_Populate(googletest)
+     add_subdirectory(${googletest_SOURCE_DIR} ${googletest_BINARY_DIR})
+   endif()
+   include(GoogleTest)
+   set(TEST_DATA_ROOT "${CMAKE_CURRENT_SOURCE_DIR}/data"
+       CACHE PATH "Root directory holding the test fixtures (data/tests/*.db)")
+endif()
+
+OPTION(ENABLE_COVERAGE "Build with gcov/lcov coverage (GCC/Clang, Debug only; see coverage-report.sh)" OFF)
+IF(ENABLE_COVERAGE)
+   IF(NOT CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang")
+      MESSAGE(FATAL_ERROR "ENABLE_COVERAGE requires GCC or Clang with gcov/lcov (current: ${CMAKE_CXX_COMPILER_ID}).")
+   ENDIF()
+   IF(NOT CMAKE_BUILD_TYPE STREQUAL "Debug")
+      MESSAGE(FATAL_ERROR "ENABLE_COVERAGE requires CMAKE_BUILD_TYPE=Debug (current: \"${CMAKE_BUILD_TYPE}\").")
+   ENDIF()
+   MESSAGE(STATUS "Coverage instrumentation enabled")
+   ADD_COMPILE_OPTIONS(--coverage -O0 -g)
+   ADD_LINK_OPTIONS(--coverage)
+ENDIF()
+
+# Check UDEBUG/UINFO/UWARN/UERROR/uFormat format strings against their
+# arguments.
+# -Wno-format-zero-length: UDEBUG("") is used deliberately as a trace point
+# -Wformat-security: non-literal format strings, e.g. UERROR(uFormat(...).c_str())
+OPTION(ENABLE_FORMAT_ERRORS "Treat log/uFormat format-string mismatches as errors (GCC/Clang)" OFF)
+IF(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang")
+   ADD_COMPILE_OPTIONS(-Wformat -Wformat-security -Wno-format-zero-length)
+   IF(ENABLE_FORMAT_ERRORS)
+      MESSAGE(STATUS "Format-string mismatches are errors")
+      ADD_COMPILE_OPTIONS(-Werror=format -Werror=format-security)
+   ENDIF()
+ELSEIF(ENABLE_FORMAT_ERRORS)
+   MESSAGE(WARNING "ENABLE_FORMAT_ERRORS has no effect on ${CMAKE_CXX_COMPILER_ID} (needs GCC/Clang)")
+ENDIF()
 
 ####### DEPENDENCIES #######
 IF(MOBILE_BUILD)
@@ -198,15 +263,17 @@ option(WITH_CCCORELIB     "Include CCCoreLib support"            OFF)
 option(WITH_OPEN3D        "Include Open3D support"               OFF)
 option(WITH_LOAM          "Include LOAM support"                 OFF)
 option(WITH_FLOAM         "Include FLOAM support"                OFF)
+option(WITH_LIOSAM        "Include LIO-SAM support"              OFF)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 option(WITH_ZED           "Include ZED sdk support"              ON)
 option(WITH_ZEDOC         "Include ZED Open Capture support"     ON)
 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_DEPTHAI       "Include depthai-core v2 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 +285,15 @@ 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)
+option(WITH_APRILTAG      "Include AprilTag support"             OFF)
 IF(MOBILE_BUILD)
 option(PCL_OMP            "With PCL OMP implementations"         OFF)
 ELSE()
@@ -234,7 +304,25 @@ option(BUILD_WITH_RPATH_NOT_RUNPATH "Explicitly disable usage of RUNPATH for the
 set(RTABMAP_QT_VERSION AUTO CACHE STRING "Force a specific Qt version.")
 set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5 6)
 
-FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching photo video videoio OPTIONAL_COMPONENTS aruco objdetect xfeatures2d nonfree gpu cudafeatures2d cudaoptflow cudaimgproc)
+# OpenCV components. calib3d was split into "calib" + "geometry" in OpenCV 5.
+# These lists are reused below to generate RTABMapConfig.cmake so downstream
+# find_package(RTABMap) requests the same components this build used.
+SET(RTABMAP_OpenCV_COMPONENTS_5 core imgproc highgui stitching photo video videoio calib geometry)
+SET(RTABMAP_OpenCV_COMPONENTS_4 core imgproc highgui stitching photo video videoio calib3d)
+SET(RTABMAP_OpenCV_OPTIONAL_COMPONENTS_5 objdetect xfeatures2d nonfree gpu cudafeatures2d cudaoptflow cudaimgproc)
+SET(RTABMAP_OpenCV_OPTIONAL_COMPONENTS_4 aruco objdetect xfeatures2d nonfree gpu cudafeatures2d cudaoptflow cudaimgproc)
+
+# Probe OpenCV without a version constraint first, then request the components
+# matching the detected major version. A version-constrained find that fails to
+# match (e.g. asking for 5 when only 4 is present) resets OpenCV_DIR to NOTFOUND,
+# which breaks toolchain builds that rely on a -DOpenCV_DIR hint (e.g. Android,
+# where CMAKE_FIND_ROOT_PATH restricts the search).
+FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core)
+IF(OpenCV_VERSION_MAJOR GREATER 4)
+  FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS ${RTABMAP_OpenCV_COMPONENTS_5} OPTIONAL_COMPONENTS ${RTABMAP_OpenCV_OPTIONAL_COMPONENTS_5})
+ELSE()
+  FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS ${RTABMAP_OpenCV_COMPONENTS_4} OPTIONAL_COMPONENTS ${RTABMAP_OpenCV_OPTIONAL_COMPONENTS_4})
+ENDIF()
 
 IF(WITH_QT)
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
@@ -396,7 +484,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 +506,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 +584,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 +619,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 +647,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 +673,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 +688,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 +700,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,19 +708,25 @@ 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)
+IF(WITH_LIOSAM)
+    find_package(lio_sam QUIET)
+    IF(lio_sam_FOUND)
+       MESSAGE(STATUS "Found lio_sam: ${lio_sam_INCLUDE_DIRS}")
+    ENDIF(lio_sam_FOUND)
+ENDIF(WITH_LIOSAM)
 
 SET(ZED_FOUND FALSE)
 IF(WITH_ZED)
@@ -623,7 +739,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 +792,28 @@ IF(WITH_MYNTEYE)
 ENDIF(WITH_MYNTEYE)
 
 IF(WITH_DEPTHAI)
-    FIND_PACKAGE(depthai 2.24 QUIET)
+    # Force DepthAI major version 2 (>=2.24, <3.0): the v3 API is not yet
+    # supported (see CameraDepthAI).
+    FIND_PACKAGE(depthai 2.24...<3.0)
     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 +825,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 +861,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 +895,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,39 +903,29 @@ 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)
-    # 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)
-    IF(ov_msckf_FOUND AND ov_core_FOUND AND ov_init_FOUND)
-      SET(ov_msckf_INCLUDE_DIRS
-         ${ov_msckf_INCLUDE_DIRS}
-         ${ov_core_INCLUDE_DIRS}
-         ${ov_init_INCLUDE_DIRS})
-      SET(ov_msckf_LIBRARIES
-         ${ov_msckf_LIBRARIES}
-         ${ov_core_LIBRARIES}
-         ${ov_init_LIBRARIES})
-      MESSAGE(STATUS "Found OpenVINS: ${ov_msckf_INCLUDE_DIRS}")
-    ENDIF()
+   FIND_PACKAGE(OpenVINS)
 ENDIF(WITH_OPENVINS)
 
 IF(WITH_FASTCV)
@@ -821,27 +935,97 @@ 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_APRILTAG)
+    FIND_PACKAGE(apriltag QUIET)
+    IF(apriltag_FOUND)
+      get_target_property(APRILTAG_LOCATION apriltag::apriltag LOCATION)
+      get_target_property(APRILTAG_INCLUDES apriltag::apriltag INTERFACE_INCLUDE_DIRECTORIES)
+      FIND_FILE(apriltag_aruco_4x4_50_header NAMES tagAruco4x4_50.h PATH_SUFFIXES aruco PATHS ${APRILTAG_INCLUDES} NO_DEFAULT_PATH)
+      SET(WITH_APRILTAG_ARUCO NO)
+      IF(apriltag_aruco_4x4_50_header)
+         SET(WITH_APRILTAG_ARUCO YES)
+      ENDIF()
+      MESSAGE(STATUS "Found apriltag (with aruco=${WITH_APRILTAG_ARUCO}): ${APRILTAG_LOCATION} ${APRILTAG_INCLUDES}")
+    ENDIF(apriltag_FOUND)
+ENDIF(WITH_APRILTAG)
+
+IF(WITH_OPENGV OR okvis_FOUND)
+   if(NOT BUILD_OPENGV)
+      FIND_PACKAGE(opengv QUIET)
+   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)
+         # OpenGV's CMakeLists.txt declares cmake_minimum_required(VERSION 2.x),
+         # which CMake >= 4.0 (e.g. recent Ubuntu) rejects. Allow it to configure.
+         set(CMAKE_POLICY_VERSION_MINIMUM 3.5)
+         # Eigen should have been already added by PCL, just populate the compatible variables
+         IF(EIGEN_INCLUDE_DIRS)
+            set(EIGEN_INCLUDE_DIRS "${EIGEN_INCLUDE_DIRS}" CACHE PATH "Eigen include dirs" FORCE)
+            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})
 ENDIF()
 
 IF(NOT MSVC)
-    IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1) OR TORCH_FOUND OR MRPT_FOUND)
+    IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1) OR (GTSAM_FOUND AND GTSAM_VERSION VERSION_GREATER_EQUAL "4.3.0") OR TORCH_FOUND OR MRPT_FOUND)
       # Qt6 requires c++17
       include(CheckCXXCompilerFlag)
       CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17)
@@ -929,10 +1113,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 "//")
@@ -964,6 +1152,12 @@ ENDIF(NOT Open3D_FOUND)
 IF(NOT FastCV_FOUND)
    SET(FASTCV "//")
 ENDIF(NOT FastCV_FOUND)
+IF(NOT apriltag_FOUND)
+   SET(APRILTAG "//")
+   SET(APRILTAG_ARUCO "//")
+ELSEIF(NOT WITH_APRILTAG_ARUCO)
+   SET(APRILTAG_ARUCO "//")
+ENDIF()
 IF(NOT opengv_FOUND OR NOT WITH_OPENGV)
    SET(OPENGV "//")
 ENDIF(NOT opengv_FOUND OR NOT WITH_OPENGV)
@@ -982,6 +1176,9 @@ ENDIF(NOT loam_velodyne_FOUND)
 IF(NOT floam_FOUND)
    SET(FLOAM "//")
 ENDIF(NOT floam_FOUND)
+IF(NOT lio_sam_FOUND)
+   SET(LIOSAM "//")
+ENDIF(NOT lio_sam_FOUND)
 IF(NOT Freenect_FOUND)
    SET(FREENECT "//")
 ENDIF()
@@ -1058,6 +1255,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 +1292,14 @@ IF(NOT msckf_vio_FOUND)
    SET(MSCKF_VIO "//")
 ENDIF()
 IF(NOT vins_FOUND)
-   SET(VINS "//")
+   SET(VINSFUSION "//")
 ENDIF()
-IF(NOT ov_msckf_FOUND)
+IF(NOT OpenVINS_FOUND)
    SET(OPENVINS "//")
 ENDIF()
+IF(NOT CUVSLAM_FOUND)
+   SET(CUVSLAM "//")
+ENDIF()
 IF(NOT ORB_SLAM_FOUND)
    SET(ORB_SLAM "//")
 ENDIF()
@@ -1123,6 +1326,15 @@ ENDIF()
 
 CONFIGURE_FILE(Version.h.in ${CMAKE_CURRENT_BINARY_DIR}/corelib/src/include/${PROJECT_PREFIX}/core/Version.h)
 
+# API documentation (Doxygen). Off by default: preparing it downloads the theme,
+# spawns Doxygen to regenerate the HTML header and runs the parameter-page
+# generator, none of which an ordinary build needs. docs-report.sh turns it on;
+# see also .github/workflows/docs.yml.
+IF(BUILD_DOCUMENTATION)
+   INCLUDE(RTABMapDocumentation)
+   RTABMAP_SETUP_DOCUMENTATION()
+ENDIF(BUILD_DOCUMENTATION)
+
 ADD_SUBDIRECTORY( utilite )
 ADD_SUBDIRECTORY( corelib )
 
@@ -1198,6 +1410,18 @@ install(EXPORT rtabmapTargets
 ####
 # Setup RTABMapConfig.cmake
 ####
+IF(OpenCV_VERSION_MAJOR GREATER 4)
+  SET(CONF_OPENCV_COMPONENTS ${RTABMAP_OpenCV_COMPONENTS_5})
+  SET(CONF_OPENCV_OPTIONAL_COMPONENTS ${RTABMAP_OpenCV_OPTIONAL_COMPONENTS_5})
+ELSE()
+  SET(CONF_OPENCV_COMPONENTS ${RTABMAP_OpenCV_COMPONENTS_4})
+  SET(CONF_OPENCV_OPTIONAL_COMPONENTS ${RTABMAP_OpenCV_OPTIONAL_COMPONENTS_4})
+ENDIF()
+STRING(REPLACE ";" " " CONF_OPENCV_COMPONENTS "${CONF_OPENCV_COMPONENTS}")
+STRING(REPLACE ";" " " CONF_OPENCV_OPTIONAL_COMPONENTS "${CONF_OPENCV_OPTIONAL_COMPONENTS}")
+# Pin the OpenCV major version so downstream projects find the same major RTAB-Map was
+# built against
+SET(CONF_OPENCV_VERSION_MAJOR ${OpenCV_VERSION_MAJOR})
 include(CMakePackageConfigHelpers)
 write_basic_package_version_file(
   "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake"
@@ -1240,7 +1464,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}")
@@ -1333,6 +1567,10 @@ MESSAGE(STATUS "  CMAKE_INSTALL_LIBDIR = ${CMAKE_INSTALL_LIBDIR}")
 MESSAGE(STATUS "  BUILD_APP =            ${BUILD_APP}")
 MESSAGE(STATUS "  BUILD_TOOLS =          ${BUILD_TOOLS}")
 MESSAGE(STATUS "  BUILD_EXAMPLES =       ${BUILD_EXAMPLES}")
+MESSAGE(STATUS "  BUILD_TESTING =        ${BUILD_TESTING}")
+MESSAGE(STATUS "  BUILD_PERF_TESTS =     ${BUILD_PERF_TESTS}")
+MESSAGE(STATUS "  ENABLE_COVERAGE =      ${ENABLE_COVERAGE}")
+MESSAGE(STATUS "  ENABLE_FORMAT_ERRORS = ${ENABLE_FORMAT_ERRORS}")
 IF(NOT WIN32)
     # see comment above for the BUILD_SHARED_LIBS option on Windows
     MESSAGE(STATUS "  BUILD_SHARED_LIBS =    ${BUILD_SHARED_LIBS}")
@@ -1354,28 +1592,34 @@ ENDIF(PCL_COMPILE_OPTIONS)
 MESSAGE(STATUS "")
 MESSAGE(STATUS "Optional dependencies ('*' affects some default parameters) :")
 IF(OpenCV_FOUND)
+  IF((OpenCV_VERSION_MAJOR LESS 4 AND OPENCV_ARUCO_FOUND) OR 
+     (OpenCV_VERSION_MAJOR GREATER 4 AND OPENCV_OBJDETECT_FOUND))
+    set(ARUCO_STR "YES")
+  ELSE()
+    set(ARUCO_STR "NO")
+  ENDIF()
   IF(OpenCV_VERSION_MAJOR EQUAL 2)
     IF(OPENCV_NONFREE_FOUND)
-      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = YES (License: Non commercial)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = YES, aruco = ${ARUCO_STR} (License: Non commercial)")
     ELSE()
-      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found, License: BSD)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = NO, aruco = ${ARUCO_STR} (not found, License: BSD)")
     ENDIF()
   ELSE()
     IF(OPENCV_XFEATURES2D_FOUND)
       IF(NONFREE STREQUAL "//")
         IF((OpenCV_VERSION_MAJOR LESS 4) OR ((OpenCV_VERSION_MAJOR EQUAL 4) AND (OpenCV_VERSION_MINOR LESS 5)))
-          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO (License: BSD)")
+          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO, aruco = ${ARUCO_STR} (License: BSD)")
         ELSE()
-          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO (License: Apache 2)")
+          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO, aruco = ${ARUCO_STR} (License: Apache 2)")
         ENDIF()
       ELSE()
-        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = YES (License: Non commercial)")
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = YES, aruco = ${ARUCO_STR} (License: Non commercial)")
       ENDIF()
     ELSE()
       IF((OpenCV_VERSION_MAJOR LESS 4) OR ((OpenCV_VERSION_MAJOR EQUAL 4) AND (OpenCV_VERSION_MINOR LESS 5)))
-        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO (License: BSD)")
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO, aruco = ${ARUCO_STR} (License: BSD)")
       ELSE()
-        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO (License: Apache 2)")
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO, aruco = ${ARUCO_STR} (License: Apache 2)")
       ENDIF()
     ENDIF()
   ENDIF()
@@ -1410,15 +1654,26 @@ MESSAGE(STATUS "  With ORB OcTree           = NO (WITH_ORB_OCTREE=OFF)")
 ENDIF()
 
 IF(TORCH_FOUND)
-MESSAGE(STATUS "  With SupertPoint          = YES (License: GPLv3) libtorch=${Torch_VERSION}")
+MESSAGE(STATUS "  With SuperPoint           = YES (License: GPLv3) libtorch=${Torch_VERSION}")
 ELSEIF(NOT WITH_TORCH)
-MESSAGE(STATUS "  With SupertPoint          = NO (WITH_TORCH=OFF)")
+MESSAGE(STATUS "  With SuperPoint           = NO (WITH_TORCH=OFF)")
 ELSE()
-MESSAGE(STATUS "  With SupertPoint          = 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()
@@ -1439,6 +1694,14 @@ ELSE()
 MESSAGE(STATUS "  With FastCV               = NO (FastCV not found)")
 ENDIF()
 
+IF(apriltag_FOUND)
+MESSAGE(STATUS "  With AprilTag ${apriltag_VERSION}       = YES (aruco=${WITH_APRILTAG_ARUCO}) (License: BSD 2-Clause License)")
+ELSEIF(NOT WITH_APRILTAG)
+MESSAGE(STATUS "  With AprilTag             = NO (WITH_APRILTAG=OFF)")
+ELSE()
+MESSAGE(STATUS "  With AprilTag             = NO (apriltag not found)")
+ENDIF()
+
 IF(PDAL_FOUND)
 MESSAGE(STATUS "  With PDAL ${PDAL_VERSION}           = YES (License: BSD)")
 ELSEIF(NOT WITH_PDAL)
@@ -1548,7 +1811,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 +1834,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 +1930,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)")
@@ -1719,7 +1986,7 @@ MESSAGE(STATUS "  With DepthAI ${depthai_VERSION}       = YES (License: MIT)")
 ELSEIF(NOT WITH_DEPTHAI)
 MESSAGE(STATUS "  With DepthAI              = NO (WITH_DEPTHAI=OFF)")
 ELSE()
-MESSAGE(STATUS "  With DepthAI              = NO (depthai-core not found)")
+MESSAGE(STATUS "  With DepthAI              = NO (depthai-core v2 not found)")
 ENDIF()
 
 IF(xvsdk_FOUND)
@@ -1730,6 +1997,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)
@@ -1747,6 +2022,13 @@ MESSAGE(STATUS "  With floam                = NO (WITH_FLOAM=OFF)")
 ELSE()
 MESSAGE(STATUS "  With floam                = NO (floam not found)")
 ENDIF()
+IF(lio_sam_FOUND)
+MESSAGE(STATUS "  With lio_sam              = YES (License: BSD)")
+ELSEIF(NOT WITH_LIOSAM)
+MESSAGE(STATUS "  With lio_sam              = NO (WITH_LIOSAM=OFF)")
+ELSE()
+MESSAGE(STATUS "  With lio_sam              = NO (lio_sam not found)")
+ENDIF()
 
 IF(libfovis_FOUND)
 MESSAGE(STATUS "  With libfovis             = YES (License: GPLv2)")
@@ -1791,12 +2073,12 @@ 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()
 
-IF(ov_msckf_FOUND)
+IF(OpenVINS_FOUND)
 MESSAGE(STATUS "  With OpenVINS             = YES (License: GPLv3)")
 ELSEIF(NOT WITH_OPENVINS)
 MESSAGE(STATUS "  With OpenVINS             = NO (WITH_OPENVINS=OFF)")
@@ -1814,6 +2096,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 "--------------------------------------------")
 

README.md

@@ -5,9 +5,10 @@ rtabmap
 
 [![Release][release-image]][releases]
 [![Downloads][downloads-image]][downloads]
+[![codecov](https://codecov.io/gh/introlab/rtabmap/graph/badge.svg?token=mPwvfZMOia)](https://codecov.io/gh/introlab/rtabmap)
 [![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
@@ -20,6 +21,7 @@ RTAB-Map library and standalone application.
 
  * For more information (e.g., papers, major updates), visit [RTAB-Map's home page](http://introlab.github.io/rtabmap).
  * For installation instructions and examples, visit [RTAB-Map's wiki](https://github.com/introlab/rtabmap/wiki).
+ * For the C++ API of the library, see the [API documentation](https://introlab.github.io/rtabmap/api/latest/), which also lists all [parameters](https://introlab.github.io/rtabmap/api/latest/parameters.html) and [command-line tools](https://introlab.github.io/rtabmap/api/latest/tools.html).
 
 To use RTAB-Map under ROS, visit the [rtabmap](http://wiki.ros.org/rtabmap) page on the ROS wiki.
 
@@ -35,13 +37,12 @@ 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-macos.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-macos.yml/badge.svg" alt="CMake MaCOS Build Status"/> <br> 
+           <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg" alt="CMake ROS Build Status"/> <br> 
+           <a href="https://github.com/introlab/rtabmap/actions/workflows/docker.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg" alt="Docker Build Status"/>
            </td>
         </tr>
      </tbody>
@@ -59,7 +60,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="5">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 +68,14 @@ 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/Lbin_uR64__rtabmap__ubuntu_resolute_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Lbin_uR64__rtabmap__ubuntu_resolute_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Lyrical</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>

RTABMapConfig.cmake.in

@@ -1,7 +1,7 @@
 include(CMakeFindDependencyMacro)
 
 # Mandatory dependencies
-find_dependency(OpenCV COMPONENTS core calib3d imgproc highgui stitching photo video OPTIONAL_COMPONENTS aruco objdetect xfeatures2d nonfree gpu cudafeatures2d)
+find_dependency(OpenCV @CONF_OPENCV_VERSION_MAJOR@ COMPONENTS @CONF_OPENCV_COMPONENTS@ OPTIONAL_COMPONENTS @CONF_OPENCV_OPTIONAL_COMPONENTS@)
 
 if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/RTABMap_guiTargets.cmake")
     find_dependency(PCL 1.7 COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)

Version.h.in

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

app/android/jni/CameraARCore.cpp

@@ -37,8 +37,8 @@ namespace rtabmap {
 //////////////////////////////
 // CameraARCore
 //////////////////////////////
-CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, bool smoothing, float upstreamRelocalizationAccThr):
-	CameraMobile(smoothing, upstreamRelocalizationAccThr),
+CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, float upstreamRelocalizationAccThr):
+	CameraMobile(upstreamRelocalizationAccThr),
 	env_(env),
 	context_(context),
 	activity_(activity),

app/android/jni/CameraARCore.h

@@ -50,7 +50,7 @@ namespace rtabmap {
 
 class CameraARCore : public CameraMobile {
 public:
-	CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false, float upstreamRelocalizationAccThr = 0.0f);
+	CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, float upstreamRelocalizationAccThr = 0.0f);
 	virtual ~CameraARCore();
 
 	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);

app/android/jni/CameraAREngine.cpp

@@ -40,8 +40,8 @@ namespace rtabmap {
 //////////////////////////////
 // CameraAREngine
 //////////////////////////////
-CameraAREngine::CameraAREngine(void* env, void* context, void* activity, bool smoothing, float upstreamRelocalizationAccThr):
-	CameraMobile(smoothing, upstreamRelocalizationAccThr),
+CameraAREngine::CameraAREngine(void* env, void* context, void* activity, float upstreamRelocalizationAccThr):
+	CameraMobile(upstreamRelocalizationAccThr),
 	env_(env),
 	context_(context),
 	activity_(activity),

app/android/jni/CameraAREngine.h

@@ -46,7 +46,7 @@ namespace rtabmap {
 
 class CameraAREngine : public CameraMobile {
 public:
-	CameraAREngine(void* env, void* context, void* activity, bool smoothing = false, float upstreamRelocalizationAccThr = 0.0f);
+	CameraAREngine(void* env, void* context, void* activity, float upstreamRelocalizationAccThr = 0.0f);
 	virtual ~CameraAREngine();
 
 	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);

app/android/jni/CameraMobile.cpp

@@ -41,9 +41,6 @@ namespace rtabmap {
 //////////////////////////////
 // CameraMobile
 //////////////////////////////
-const float CameraMobile::bilateralFilteringSigmaS = 2.0f;
-const float CameraMobile::bilateralFilteringSigmaR = 0.075f;
-
 const rtabmap::Transform CameraMobile::opticalRotation = Transform(
 		0.0f,  0.0f,  1.0f, 0.0f,
 	   -1.0f,  0.0f,  0.0f, 0.0f,
@@ -53,13 +50,12 @@ const rtabmap::Transform CameraMobile::opticalRotationInv = Transform(
 	    0.0f,  0.0f, -1.0f, 0.0f,
 		1.0f,  0.0f,  0.0f, 0.0f);
 
-CameraMobile::CameraMobile(bool smoothing, float upstreamRelocalizationAccThr) :
+CameraMobile::CameraMobile(float upstreamRelocalizationAccThr) :
 		Camera(10),
 		deviceTColorCamera_(Transform::getIdentity()),
 		textureId_(0),
 		uvs_initialized_(false),
 		stampEpochOffset_(0.0),
-		smoothing_(smoothing),
 		colorCameraToDisplayRotation_(ROTATION_0),
 		originUpdate_(true),
 		upstreamRelocalizationAccThr_(upstreamRelocalizationAccThr),
@@ -394,124 +390,135 @@ SensorData CameraMobile::updateDataOnRender(Transform & pose)
 void CameraMobile::postUpdate()
 {
 	if(data_.isValid())
-	{
-		// adjust origin
-		if(!originOffset_.isNull())
-		{
-			dataPose_ = originOffset_ * dataPose_;
-			viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
-			occlusionModel_.setLocalTransform(originOffset_ * occlusionModel_.localTransform());
-		}
-
-		if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0)
-		{
-			data_.setGPS(lastKnownGPS_);
-		}
-		else if(lastKnownGPS_.stamp()>0.0)
-		{
-			LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp());
-		}
-
-		if(lastEnvSensors_.size())
-		{
-			data_.setEnvSensors(lastEnvSensors_);
-			lastEnvSensors_.clear();
-		}
-
-		if(smoothing_ && !data_.depthRaw().empty())
-		{
-			//UTimer t;
-			data_.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data_.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
-			//LOGD("Bilateral filtering, time=%fs", t.ticks());
-		}
-
-		// Rotate image depending on the camera orientation
-		if(colorCameraToDisplayRotation_ == ROTATION_90)
-		{
-			UDEBUG("ROTATION_90");
-			cv::Mat rgb, depth;
-			cv::Mat rgbt(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
-			cv::flip(data_.imageRaw(),rgb,1);
-			cv::transpose(rgb,rgbt);
-			rgb = rgbt;
-			cv::Mat deptht(data_.depthRaw().cols, data_.depthRaw().rows, data_.depthRaw().type());
-			cv::flip(data_.depthRaw(),depth,1);
-			cv::transpose(depth,deptht);
-			depth = deptht;
-			CameraModel model = data_.cameraModels()[0];
-			cv::Size sizet(model.imageHeight(), model.imageWidth());
-			model = CameraModel(
-					model.fy(),
-					model.fx(),
-					model.cy(),
-					model.cx()>0?model.imageWidth()-model.cx():0,
-					model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
-			model.setImageSize(sizet);
-			data_.setRGBDImage(rgb, depth, model);
-
-			std::vector<cv::KeyPoint> keypoints = data_.keypoints();
-			for(size_t i=0; i<keypoints.size(); ++i)
-			{
-				keypoints[i].pt.x = data_.keypoints()[i].pt.y;
-				keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.x;
-			}
-			data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
-		}
-		else if(colorCameraToDisplayRotation_ == ROTATION_180)
-		{
-			UDEBUG("ROTATION_180");
-			cv::Mat rgb, depth;
-			cv::flip(data_.imageRaw(),rgb,1);
-			cv::flip(rgb,rgb,0);
-			cv::flip(data_.depthOrRightRaw(),depth,1);
-			cv::flip(depth,depth,0);
-			CameraModel model = data_.cameraModels()[0];
-			cv::Size sizet(model.imageWidth(), model.imageHeight());
-			model = CameraModel(
-					model.fx(),
-					model.fy(),
-					model.cx()>0?model.imageWidth()-model.cx():0,
-					model.cy()>0?model.imageHeight()-model.cy():0,
-					model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
-			model.setImageSize(sizet);
-			data_.setRGBDImage(rgb, depth, model);
-
-			std::vector<cv::KeyPoint> keypoints = data_.keypoints();
-			for(size_t i=0; i<keypoints.size(); ++i)
-			{
-				keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.x;
-				keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.y;
-			}
-			data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
-		}
-		else if(colorCameraToDisplayRotation_ == ROTATION_270)
-		{
-			UDEBUG("ROTATION_270");
-			cv::Mat rgb(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
-			cv::transpose(data_.imageRaw(),rgb);
-			cv::flip(rgb,rgb,1);
-			cv::Mat depth(data_.depthOrRightRaw().cols, data_.depthOrRightRaw().rows, data_.depthOrRightRaw().type());
-			cv::transpose(data_.depthOrRightRaw(),depth);
-			cv::flip(depth,depth,1);
-			CameraModel model = data_.cameraModels()[0];
-			cv::Size sizet(model.imageHeight(), model.imageWidth());
-			model = CameraModel(
-					model.fy(),
-					model.fx(),
-					model.cy()>0?model.imageHeight()-model.cy():0,
-					model.cx(),
-					model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
-			model.setImageSize(sizet);
-			data_.setRGBDImage(rgb, depth, model);
-
-			std::vector<cv::KeyPoint> keypoints = data_.keypoints();
-			for(size_t i=0; i<keypoints.size(); ++i)
-			{
-				keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.y;
-				keypoints[i].pt.y = data_.keypoints()[i].pt.x;
-			}
-			data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
-		}
+    {
+        // adjust origin
+        if(!originOffset_.isNull())
+        {
+            dataPose_ = originOffset_ * dataPose_;
+            viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
+            occlusionModel_.setLocalTransform(originOffset_ * occlusionModel_.localTransform());
+        }
+        
+        if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0)
+        {
+            data_.setGPS(lastKnownGPS_);
+        }
+        else if(lastKnownGPS_.stamp()>0.0)
+        {
+            LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp());
+        }
+        
+        if(lastEnvSensors_.size())
+        {
+            data_.setEnvSensors(lastEnvSensors_);
+            lastEnvSensors_.clear();
+        }
+        
+        
+        // Rotate image depending on the camera orientation
+        if(colorCameraToDisplayRotation_ == ROTATION_90)
+        {
+            UDEBUG("ROTATION_90");
+            cv::Mat rgb, depth, confidence;
+            cv::Mat rgbt;
+            cv::flip(data_.imageRaw(),rgb,1);
+            cv::transpose(rgb,rgbt);
+            rgb = rgbt;
+            cv::Mat deptht;
+            cv::flip(data_.depthRaw(),depth,1);
+            cv::transpose(depth,deptht);
+            depth = deptht;
+            if(!data_.depthConfidenceRaw().empty()) {
+                cv::Mat conft;
+                cv::flip(data_.depthConfidenceRaw(),confidence,1);
+                cv::transpose(confidence,conft);
+                confidence = conft;
+            }
+            CameraModel model = data_.cameraModels()[0];
+            cv::Size sizet(model.imageHeight(), model.imageWidth());
+            model = CameraModel(
+                                model.fy(),
+                                model.fx(),
+                                model.cy(),
+                                model.cx()>0?model.imageWidth()-model.cx():0,
+                                model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
+            model.setImageSize(sizet);
+            data_.setRGBDImage(rgb, depth, confidence, model);
+            
+            std::vector<cv::KeyPoint> keypoints = data_.keypoints();
+            for(size_t i=0; i<keypoints.size(); ++i)
+            {
+                keypoints[i].pt.x = data_.keypoints()[i].pt.y;
+                keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.x;
+            }
+            data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
+        }
+        else if(colorCameraToDisplayRotation_ == ROTATION_180)
+        {
+            UDEBUG("ROTATION_180");
+            cv::Mat rgb, depth, confidence;
+            cv::flip(data_.imageRaw(),rgb,1);
+            cv::flip(rgb,rgb,0);
+            cv::flip(data_.depthOrRightRaw(),depth,1);
+            cv::flip(depth,depth,0);
+            if(!data_.depthConfidenceRaw().empty()) {
+                cv::flip(data_.depthConfidenceRaw(),confidence,1);
+                cv::flip(confidence,confidence,0);
+            }
+            CameraModel model = data_.cameraModels()[0];
+            cv::Size sizet(model.imageWidth(), model.imageHeight());
+            model = CameraModel(
+                                model.fx(),
+                                model.fy(),
+                                model.cx()>0?model.imageWidth()-model.cx():0,
+                                model.cy()>0?model.imageHeight()-model.cy():0,
+                                model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
+            model.setImageSize(sizet);
+            data_.setRGBDImage(rgb, depth, confidence, model);
+            
+            std::vector<cv::KeyPoint> keypoints = data_.keypoints();
+            for(size_t i=0; i<keypoints.size(); ++i)
+            {
+                keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.x;
+                keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.y;
+            }
+            data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
+        }
+        else if(colorCameraToDisplayRotation_ == ROTATION_270)
+        {
+            UDEBUG("ROTATION_270");
+            cv::Mat rgb, depth, confidence;
+            cv::transpose(data_.imageRaw(),rgb);
+            cv::flip(rgb,rgb,1);
+            cv::transpose(data_.depthOrRightRaw(),depth);
+            cv::flip(depth,depth,1);
+            if(!data_.depthConfidenceRaw().empty()) {
+                cv::transpose(data_.depthConfidenceRaw(),confidence);
+                cv::flip(confidence,confidence,1);
+            }
+            CameraModel model = data_.cameraModels()[0];
+            cv::Size sizet(model.imageHeight(), model.imageWidth());
+            model = CameraModel(
+                                model.fy(),
+                                model.fx(),
+                                model.cy()>0?model.imageHeight()-model.cy():0,
+                                model.cx(),
+                                model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
+            model.setImageSize(sizet);
+            data_.setRGBDImage(rgb, depth, confidence, model);
+            
+            std::vector<cv::KeyPoint> keypoints = data_.keypoints();
+            for(size_t i=0; i<keypoints.size(); ++i)
+            {
+                keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.y;
+                keypoints[i].pt.y = data_.keypoints()[i].pt.x;
+            }
+            data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
+        }
+        else
+        {
+            UDEBUG("ROTATION_0");
+        }
 	}
 }
 

app/android/jni/CameraMobile.h

@@ -70,9 +70,6 @@ private:
 
 class CameraMobile : public Camera, public UEventsSender {
 public:
-	static const float bilateralFilteringSigmaS;
-	static const float bilateralFilteringSigmaR;
-
 	static const rtabmap::Transform opticalRotation;
 	static const rtabmap::Transform opticalRotationInv;
     
@@ -87,7 +84,7 @@ public:
             int kptsSize = 3);
 
 public:
-	CameraMobile(bool smoothing = false, float upstreamRelocalizationAccThr = 0.0f);
+	CameraMobile(float upstreamRelocalizationAccThr = 0.0f);
 	virtual ~CameraMobile();
 
 	// abstract functions
@@ -110,7 +107,6 @@ public:
 
 	const CameraModel & getCameraModel() const {return model_;}
 	const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
-	void setSmoothing(bool enabled) {smoothing_ = enabled;}
 	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
 	void setGPS(const GPS & gps);
 	void addEnvSensor(int type, float value);
@@ -144,7 +140,6 @@ protected:
 private:
 	bool firstFrame_;
 	double stampEpochOffset_;
-	bool smoothing_;
 	ScreenRotation colorCameraToDisplayRotation_;
 	GPS lastKnownGPS_;
 	EnvSensors lastEnvSensors_;

app/android/jni/CameraTango.cpp

@@ -114,8 +114,7 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
 //////////////////////////////
 // CameraTango
 //////////////////////////////
-CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing) :
-		CameraMobile(smoothing),
+CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan) :
 		tango_config_(0),
 		colorCamera_(colorCamera),
 		decimation_(decimation),
@@ -866,7 +865,7 @@ SensorData CameraTango::updateDataOnRender(Transform & pose)
 	}
 
 	SensorData data;
-	if(tangoDataReady_.acquireTry(1))
+	if(tangoDataReady_.acquireTry(1) == 0)
 	{
 		boost::mutex::scoped_lock  lock(tangoDataMutex_);
 		data = tangoData_;

app/android/jni/CameraTango.h

@@ -45,7 +45,7 @@ namespace rtabmap {
 
 class CameraTango : public CameraMobile {
 public:
-	CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing);
+	CameraTango(bool colorCamera, int decimation, bool publishRawScan);
 	virtual ~CameraTango();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

app/android/jni/RTABMapApp.cpp

@@ -84,6 +84,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 const int g_optMeshId = -100;
 
+const float g_bilateralFilteringSigmaS = 2.0f;
+const float g_bilateralFilteringSigmaR = 0.075f;
+
 #ifdef __ANDROID__
 static JavaVM *jvm;
 static jobject RTABMapActivity = 0;
@@ -201,7 +204,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
         uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), "0"));      // deactivate global retrieval
         uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDMaxLocalRetrieved(), "0"));    // deactivate local retrieval
         uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemMapLabelsAdded(), "false"));    // don't create map labels
-        uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMemoryThr(), "2"));         // keep the WM empty
+        uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMemoryThr(), "1"));         // keep the WM empty
         uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemSTMSize(), "1"));               // STM=1 -->
         uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), "false"));
         uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLinearUpdate(), "0"));
@@ -230,6 +233,7 @@ RTABMapApp::RTABMapApp() :
 		trajectoryMode_(false),
 		rawScanSaved_(false),
 		smoothing_(true),
+		depthBleedingError_(0.0f),
 		depthFromMotion_(false),
 		cameraColor_(true),
 		fullResolution_(false),
@@ -245,7 +249,11 @@ RTABMapApp::RTABMapApp() :
 		maxGainRadius_(0.02f),
 		renderingTextureDecimation_(4),
 		backgroundColor_(0.2f),
-        depthConfidence_(2),
+#ifndef RTABMAP_ARCORE
+        depthConfidence_(100), // iOS
+#else
+		depthConfidence_(0),
+#endif
         upstreamRelocalizationMaxAcc_(0.0f),
 		exportPointCloudFormat_("ply"),
 		dataRecorderMode_(false),
@@ -266,20 +274,20 @@ RTABMapApp::RTABMapApp() :
 		lastPoseEventTime_(0.0),
 		visualizingMesh_(false),
 		exportedMeshUpdated_(false),
+		optTextureMesh_(new pcl::TextureMesh),
+		optRefId_(0),
+		optRefPose_(0),
 		measuresUpdated_(false),
-        targetPoint_(new pcl::PointCloud<pcl::PointXYZRGB>),
-        quadSample_(new pcl::PointCloud<pcl::PointXYZ>),
-        quadSamplePolygons_(2),
-        metricSystem_(true),
+		metricSystem_(true),
         measuringTextSize_(0.05f),
 		snapAxisThr_(0.95),
         measuringMode_(0),
         addMeasureClicked_(false),
         teleportClicked_(false),
         removeMeasureClicked_(false),
-		optTextureMesh_(new pcl::TextureMesh),
-		optRefId_(0),
-		optRefPose_(0),
+		targetPoint_(new pcl::PointCloud<pcl::PointXYZRGB>),
+        quadSample_(new pcl::PointCloud<pcl::PointXYZ>),
+        quadSamplePolygons_(2),
 		mapToOdom_(rtabmap::Transform::getIdentity())
 
 {
@@ -461,7 +469,13 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 				LOGI("Open: Found optimized mesh! Visualizing it.");
 				optTextureMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
                 optMesh_ = rtabmap::Mesh();
-				optTexture_ = textures;
+                if(textures.rows > 4096) // Limitation iOS, just for rendering on device
+                {
+                    cv::resize(textures, optTexture_, cv::Size(4096, 4096), 0.0f, 0.0f, cv::INTER_AREA);
+                }
+                else {
+                    optTexture_ = textures;
+                }
                 if(!optTexture_.empty())
 				{
 					LOGI("Open: Texture mesh: %dx%d.", optTexture_.cols, optTexture_.rows);
@@ -569,8 +583,8 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 						rtabmap::SensorData data = signatures.at(id).sensorData();
                         rawPoses_.insert(std::make_pair(id, signatures.at(id).getPose()));
 
-						cv::Mat tmpA, depth;
-						data.uncompressData(&tmpA, &depth);
+						cv::Mat tmpA, tmpB, tmpC;
+						data.uncompressData(&tmpA, &tmpB, 0, 0, 0, 0, 0, depthConfidence_>0?&tmpC:0);
 
 						if(!(!data.imageRaw().empty() && !data.depthRaw().empty()) && !data.laserScanCompressed().isEmpty())
 						{
@@ -586,8 +600,30 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 							if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
 							{
                                 int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
-                                
-								cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
+
+								if(smoothing_ || depthBleedingError_>0.0f)
+								{
+									cv::Mat depth = data.depthRaw();
+									if(depthBleedingError_ > 0.0f)
+									{
+										rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
+									}
+									if(smoothing_)
+									{
+										depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
+									}
+									data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
+								}
+
+								cloud = rtabmap::util3d::cloudRGBFromSensorData(
+									data, 
+									meshDecimation, 
+									maxCloudDepth_, 
+									minCloudDepth_, 
+									indices.get(), 
+									rtabmap::ParametersMap(), 
+									std::vector<float>(), 
+									depthConfidence_);
 							}
 							else
 							{
@@ -875,11 +911,11 @@ int RTABMapApp::updateMeshDecimation(int width, int height)
         {
             meshDecimation = 5;
         }
-        else if(width % 3 == 0 && width % 3 == 0)
+        else if(width % 3 == 0 && height % 3 == 0)
         {
             meshDecimation = 3;
         }
-        else if(width % 2 == 0 && width % 2 == 0)
+        else if(width % 2 == 0 && height % 2 == 0)
         {
             meshDecimation = 2;
         }
@@ -946,7 +982,7 @@ bool RTABMapApp::startCamera()
 	if(cameraDriver_ == 0) // Tango
 	{
 #ifdef RTABMAP_TANGO
-		camera_ = new rtabmap::CameraTango(cameraColor_, !cameraColor_ || fullResolution_?1:2, rawScanSaved_, smoothing_);
+		camera_ = new rtabmap::CameraTango(cameraColor_, !cameraColor_ || fullResolution_?1:2, rawScanSaved_);
 
 		if (TangoService_setBinder(env, iBinder) != TANGO_SUCCESS) {
 		    UERROR("TangoHandler::ConnectTango, TangoService_setBinder error");
@@ -961,7 +997,7 @@ bool RTABMapApp::startCamera()
 	else if(cameraDriver_ == 1)
 	{
 #ifdef RTABMAP_ARCORE
-		camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, smoothing_, upstreamRelocalizationMaxAcc_);
+		camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, upstreamRelocalizationMaxAcc_);
 #else
 		UERROR("RTAB-Map is not built with ARCore support!");
 #endif
@@ -969,14 +1005,14 @@ bool RTABMapApp::startCamera()
 	else if(cameraDriver_ == 2)
 	{
 #ifdef RTABMAP_ARENGINE
-		camera_ = new rtabmap::CameraAREngine(env, context, activity, smoothing_, upstreamRelocalizationMaxAcc_);
+		camera_ = new rtabmap::CameraAREngine(env, context, activity, upstreamRelocalizationMaxAcc_);
 #else
 		UERROR("RTAB-Map is not built with AREngine support!");
 #endif
 	}
 	else if(cameraDriver_ == 3)
 	{
-		camera_ = new rtabmap::CameraMobile(smoothing_, upstreamRelocalizationMaxAcc_);
+		camera_ = new rtabmap::CameraMobile(upstreamRelocalizationMaxAcc_);
 	}
 
 	if(camera_ == 0)
@@ -1003,7 +1039,11 @@ bool RTABMapApp::startCamera()
 		cameraJustInitialized_ = true;
 		if(useExternalLidar_)
 		{
+#if BOOST_VERSION >= 108700
+            rtabmap::LidarVLP16 * lidar = new rtabmap::LidarVLP16(boost::asio::ip::make_address("192.168.1.201"), 2368, true);
+#else
 			rtabmap::LidarVLP16 * lidar = new rtabmap::LidarVLP16(boost::asio::ip::address_v4::from_string("192.168.1.201"), 2368, true);
+#endif
 			lidar->init();
 			camera_->setImageRate(0); // if lidar, to get close camera synchronization
 			sensorCaptureThread_ = new rtabmap::SensorCaptureThread(lidar, camera_, camera_, rtabmap::Transform::getIdentity());
@@ -1354,8 +1394,8 @@ int RTABMapApp::Render()
 #ifdef DEBUG_RENDERING_PERFORMANCE
 				LOGD("Camera updateOnRender %fs", time.ticks());
 #endif
-                // We detect if we are in measuring mode if rtabmap is not running
-				if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0 && !(rtabmapThread_ == 0 || !rtabmapThread_->isRunning()))
+                // We check if we are in measuring mode: not visualizing mesh or rtabmap is not started (localization mode)
+				if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0 && (!visualizingMesh_ || !(rtabmapThread_ == 0 || !rtabmapThread_->isRunning())))
 				{
 					main_scene_.background_renderer_ = new BackgroundRenderer();
 					main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ <= 2);
@@ -1536,7 +1576,6 @@ int RTABMapApp::Render()
                 int textId = 0;
                 int quadId = 0;
                 int circleId = 0;
-                float sphereRadius = 0.02f;
                 float quadSize=0.05f;
                 float quadAlpha = 0.3f;
 
@@ -2010,8 +2049,8 @@ int RTABMapApp::Render()
 								{
 									rtabmap::SensorData data = bufferedSensorData.at(id);
 
-									cv::Mat tmpA, depth;
-									data.uncompressData(&tmpA, &depth);
+									cv::Mat tmpA, tmpB, tmpC;
+									data.uncompressData(&tmpA, &tmpB, 0, 0, 0, 0, 0, depthConfidence_>0?&tmpC:0);
 									if(!(!data.imageRaw().empty() && !data.depthRaw().empty()) && !data.laserScanCompressed().isEmpty())
 									{
 										rtabmap::LaserScan scan;
@@ -2029,7 +2068,20 @@ int RTABMapApp::Render()
 										if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
 										{
                                             int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
-											cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
+											if(smoothing_ || depthBleedingError_>0.0f)
+											{
+												cv::Mat depth = data.depthRaw();
+												if(depthBleedingError_ > 0.0f)
+												{
+													rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
+												}
+												if(smoothing_)
+												{
+													depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
+												}
+												data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
+											}
+											cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get(), rtabmap::ParametersMap(), std::vector<float>(), depthConfidence_);
 										}
 										else
 										{
@@ -2245,7 +2297,7 @@ int RTABMapApp::Render()
 							if(!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty() && (!useExternalLidar_ || sensorEvent.data().laserScanRaw().isEmpty()))
 							{
                                 int meshDecimation = updateMeshDecimation(sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows);
-								cloud = rtabmap::util3d::cloudRGBFromSensorData(sensorEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
+								cloud = rtabmap::util3d::cloudRGBFromSensorData(sensorEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get(), rtabmap::ParametersMap(), std::vector<float>(), depthConfidence_);
 							}
 							else
 							{
@@ -2948,7 +3000,7 @@ void RTABMapApp::setTrajectoryMode(bool enabled)
 void RTABMapApp::setGraphOptimization(bool enabled)
 {
 	graphOptimization_ = enabled;
-	if((sensorCaptureThread_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0)
+	if((sensorCaptureThread_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature(false)!=0)
 	{
 		std::map<int, rtabmap::Transform> poses;
 		std::multimap<int, rtabmap::Link> links;
@@ -3015,6 +3067,11 @@ void RTABMapApp::setSmoothing(bool enabled)
 	}
 }
 
+void RTABMapApp::setDepthBleedingError(float value)
+{
+	depthBleedingError_ = value;
+}
+
 void RTABMapApp::setDepthFromMotion(bool enabled)
 {
 	if(depthFromMotion_ != enabled)
@@ -3107,10 +3164,10 @@ void RTABMapApp::setBackgroundColor(float gray)
 
 void RTABMapApp::setDepthConfidence(int value)
 {
-    depthConfidence_ = value;
-    if(depthConfidence_>2)
+    depthConfidence_ = value*50; // [0,2] -> [0,100]
+    if(depthConfidence_>100)
     {
-        depthConfidence_ = 2;
+        depthConfidence_ = 100;
     }
 }
 
@@ -3169,7 +3226,7 @@ int RTABMapApp::setMappingParameter(const std::string & key, const std::string &
 	}
 	else
 	{
-		UERROR(uFormat("Key \"%s\" doesn't exist!", compatibleKey.c_str()).c_str());
+		UERROR("Key \"%s\" doesn't exist!", compatibleKey.c_str());
 		return -1;
 	}
 }
@@ -3410,6 +3467,19 @@ bool RTABMapApp::exportMesh(
 							if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1)
 							{
                                 int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
+								if(smoothing_ || depthBleedingError_>0.0f)
+								{
+									cv::Mat depth = data.depthRaw();
+									if(depthBleedingError_ > 0.0f)
+									{
+										rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
+									}
+									if(smoothing_)
+									{
+										depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
+									}
+									data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
+								}
 								cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
 								model = data.cameraModels()[0];
 								depth = data.depthRaw();
@@ -3677,7 +3747,20 @@ bool RTABMapApp::exportMesh(
 							if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1)
 							{
                                 int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
-								cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_);
+								if(smoothing_ || depthBleedingError_>0.0f)
+								{
+									cv::Mat depth = data.depthRaw();
+									if(depthBleedingError_ > 0.0f)
+									{
+										rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
+									}
+									if(smoothing_)
+									{
+										depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
+									}
+									data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
+								}
+								cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, 0, rtabmap::ParametersMap(), std::vector<float>(), depthConfidence_);
 								polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
 							}
 						}
@@ -3935,7 +4018,20 @@ bool RTABMapApp::exportMesh(
 					if(!data.imageRaw().empty() && !data.depthRaw().empty())
 					{
 						// full resolution
-						cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, minCloudDepth_, indices.get());
+						if(smoothing_ || depthBleedingError_>0.0f)
+						{
+							cv::Mat depth = data.depthRaw();
+							if(depthBleedingError_ > 0.0f)
+							{
+								rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
+							}
+							if(smoothing_)
+							{
+								depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
+							}
+							data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
+						}
+						cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, minCloudDepth_, indices.get(), rtabmap::ParametersMap(), std::vector<float>(), depthConfidence_);
 					}
                     else if(!data.laserScanRaw().empty())
                     {
@@ -3965,7 +4061,20 @@ bool RTABMapApp::exportMesh(
 						if(!data.imageRaw().empty() && !data.depthRaw().empty())
 						{
                             int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
-							cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
+							if(smoothing_ || depthBleedingError_>0.0f)
+							{
+								cv::Mat depth = data.depthRaw();
+								if(depthBleedingError_ > 0.0f)
+								{
+									rtabmap::util2d::depthBleedingFiltering(depth, depthBleedingError_);
+								}
+								if(smoothing_)
+								{
+									depth = rtabmap::util2d::fastBilateralFiltering(depth, g_bilateralFilteringSigmaS, g_bilateralFilteringSigmaR);
+								}
+								data.setRGBDImage(data.imageRaw(), depth, data.depthConfidenceRaw(), data.cameraModels());
+							}
+							cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get(), rtabmap::ParametersMap(), std::vector<float>(), depthConfidence_);
 						}
                         else if(!data.laserScanRaw().empty())
                         {
@@ -4117,7 +4226,13 @@ bool RTABMapApp::postExportation(bool visualize)
 				LOGI("postExportation: Found optimized mesh! Visualizing it.");
 				optTextureMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
                 optMesh_ = rtabmap::Mesh();
-                optTexture_ = textures;
+                if(textures.rows > 4096) // Limitation iOS, just for rendering on device
+                {
+                    cv::resize(textures, optTexture_, cv::Size(4096, 4096), 0.0f, 0.0f, cv::INTER_AREA);
+                }
+                else {
+                    optTexture_ = textures;
+                }
 
 				boost::mutex::scoped_lock  lock(renderingMutex_);
 				visualizingMesh_ = true;
@@ -4524,6 +4639,7 @@ void RTABMapApp::postOdometryEvent(
 
 
 				cv::Mat outputDepth;
+				cv::Mat outputDepthConfidence;
 				if(depth && depthHeight>0 && depthWidth>0)
 				{
 #ifndef DISABLE_LOG
@@ -4533,32 +4649,21 @@ void RTABMapApp::postOdometryEvent(
                     {
                         // IOS
                         outputDepth = cv::Mat(depthHeight, depthWidth, CV_32FC1, (void*)depth).clone();
-                        if(conf && confWidth == depthWidth && confHeight == depthHeight && confFormat == 1278226488 && depthConfidence_>0)
+                        if(conf && confWidth == depthWidth && confHeight == depthHeight && confFormat == 1278226488)
                         {
-                            const unsigned char * confPtr = (const unsigned char *)conf;
-                            float * depthPtr = outputDepth.ptr<float>();
-                            int i=0;
-                            for (int y = 0; y < outputDepth.rows; ++y)
-                            {
-                                for (int x = 0; x < outputDepth.cols; ++x)
-                                {
-                                    // https://developer.apple.com/documentation/arkit/arconfidencelevel
-                                    // 0 = low
-                                    // 1 = medium
-                                    // 2 = high
-                                    if(confPtr[y*outputDepth.cols + x] < depthConfidence_)
-                                    {
-                                        depthPtr[y*outputDepth.cols + x] = 0.0f;
-                                        ++i;
-                                    }
-                                }
-                            }
+							// https://developer.apple.com/documentation/arkit/arconfidencelevel
+							// 0 = low
+							// 1 = medium
+							// 2 = high
+							// Re-scale confidence from [0,2] to [0,100]
+							cv::Mat(depthHeight, depthWidth, CV_8UC1, (void*)conf).convertTo(outputDepthConfidence, CV_8UC1, 50, 0);
                         }
                     }
                     else if(depthLen == 2*depthWidth*depthHeight)
                     {
                         // ANDROID
                         outputDepth = cv::Mat(depthHeight, depthWidth, CV_16UC1);
+						outputDepthConfidence = cv::Mat(depthHeight, depthWidth, CV_8UC1);
                         uint16_t *dataShort = (uint16_t *)depth;
                         for (int y = 0; y < outputDepth.rows; ++y)
                         {
@@ -4567,6 +4672,13 @@ void RTABMapApp::postOdometryEvent(
                                 uint16_t depthSample = dataShort[y*outputDepth.cols + x];
                                 uint16_t depthRange = (depthSample & 0x1FFF); // first 3 bits are confidence
                                 outputDepth.at<uint16_t>(y,x) = depthRange;
+								// https://developer.android.com/reference/android/graphics/ImageFormat#DEPTH16
+								// The confidence value is an estimate of correctness for this sample. It 
+								// is encoded in the 3 most significant bits of the sample, with a value of 
+								// 0 representing 100% confidence, a value of 1 representing 0% confidence, a 
+								// value of 2 representing 1/7, a value of 3 representing 2/7, and so on.
+								uint8_t depthConfidence = uint8_t((depthSample >> 13) & 0x7);
+								outputDepthConfidence.at<uint8_t>(y,x) = depthConfidence == 0 ? 100 : (depthConfidence - 1)*100 / 7;
                             }
                         }
                     }
@@ -4626,7 +4738,9 @@ void RTABMapApp::postOdometryEvent(
 								depth_fx, 0, depth_cx,
 								0, depth_fy, depth_cy,
 								0, 0, 1);
-						outputDepth = rtabmap::util2d::registerDepth(outputDepth, depthK, outputDepth.size(), colorK, rgbToDepth);
+						cv::Mat regConfidence;
+						outputDepth = rtabmap::util2d::registerDepth(outputDepth, outputDepthConfidence, depthK, outputDepth.size(), colorK, rgbToDepth, regConfidence);
+						outputDepthConfidence = regConfidence;
 #ifndef DISABLE_LOG
 						UDEBUG("Depth registration time: %fs", time.elapsed());
 #endif
@@ -4666,8 +4780,8 @@ void RTABMapApp::postOdometryEvent(
                         depthModel.setLocalTransform(pose*model.localTransform());
                         camera_->setOcclusionImage(outputDepth, depthModel);
                     }
-                    
-					rtabmap::SensorData data(scan, outputRGB, outputDepth, model, 0, stamp);
+
+					rtabmap::SensorData data(scan, outputRGB, outputDepth, outputDepthConfidence, model, 0, stamp);
 					data.setFeatures(kpts,  kpts3, cv::Mat());
                     glm::mat4 projectionMatrix(0);
                     projectionMatrix[0][0] = p00;

app/android/jni/RTABMapApp.h

@@ -139,6 +139,7 @@ class RTABMapApp : public UEventsHandler {
   void setCameraColor(bool enabled);
   void setFullResolution(bool enabled);
   void setSmoothing(bool enabled);
+  void setDepthBleedingError(float value);
   void setDepthFromMotion(bool enabled);
   void setAppendMode(bool enabled);
   void setUpstreamRelocalizationAccThr(float value);
@@ -235,6 +236,7 @@ class RTABMapApp : public UEventsHandler {
   bool trajectoryMode_;
   bool rawScanSaved_;
   bool smoothing_;
+  float depthBleedingError_;
   bool depthFromMotion_;
   bool cameraColor_;
   bool fullResolution_;
@@ -250,7 +252,7 @@ class RTABMapApp : public UEventsHandler {
   float maxGainRadius_;
   int renderingTextureDecimation_;
   float backgroundColor_;
-  int depthConfidence_;
+  unsigned char depthConfidence_;
   float upstreamRelocalizationMaxAcc_;
   std::string exportPointCloudFormat_;
 

app/android/jni/jni_interface.cpp

@@ -512,6 +512,19 @@ Java_com_introlab_rtabmap_RTABMapLib_setSmoothing(
 	}
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setDepthBleedingError(
+		JNIEnv*, jclass, jlong native_application, float value)
+{
+	if(native_application)
+	{
+		return native(native_application)->setDepthBleedingError(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setDepthFromMotion(
 		JNIEnv*, jclass, jlong native_application, bool enabled)
 {

app/android/res/layout/activity_settings.xml

@@ -114,6 +114,13 @@
 		        android:title="@string/pref_title_smoothing"
 		        android:summary="@string/pref_summary_smoothing"
 		        android:defaultValue="@string/pref_default_smoothing"/>
+			<ListPreference
+		        android:key="@string/pref_key_depth_bleeding_error"
+		        android:title="@string/pref_title_depth_bleeding_error"
+		        android:summary="@string/pref_summary_depth_bleeding_error"
+		        android:entries="@array/pref_depth_bleeding_error_keys"
+		   	    android:entryValues="@array/pref_depth_bleeding_error_values"
+		   		android:defaultValue="@string/pref_default_depth_bleeding_error"/>
 		    <com.introlab.rtabmap.CustomSwitchPreference
 		        android:key="@string/pref_key_fisheye"
 		        android:title="@string/pref_title_fisheye"

app/android/res/values/strings.xml

@@ -76,6 +76,8 @@
     <string name="pref_default_resolution">false</string>
     <string name="pref_key_smoothing">pref_key_smoothing</string>
     <string name="pref_default_smoothing">false</string>
+    <string name="pref_key_depth_bleeding_error">pref_key_depth_bleeding_error</string>
+    <string name="pref_default_depth_bleeding_error">0.0</string>
     <string name="pref_key_fisheye">pref_key_fisheye</string>
     <string name="pref_default_fisheye">false</string>
     
@@ -337,6 +339,8 @@
     <string name="pref_summary_resolution">Save HD images of the color camera if you want very detailed textures. More memory will be required.</string>
     <string name="pref_title_smoothing">Smoothing</string>
     <string name="pref_summary_smoothing">Smooth the point clouds.</string>
+    <string name="pref_title_depth_bleeding_error">Depth Bleeding Filtering Error</string>
+    <string name="pref_summary_depth_bleeding_error">Filter noisy depth points on the edges.</string>
     <string name="pref_title_fisheye">Fish Eye Camera</string>
     <string name="pref_summary_fisheye">Use fish eye camera instead of the color camera. May not work on some devices.</string>
     <string name="pref_title_update_rate">Update Rate</string>
@@ -436,6 +440,26 @@
         <item>"29.41995"</item>
         <item>"19.6133"</item>
     </string-array>
+    <string-array name="pref_depth_bleeding_error_keys">
+        <item>"Disabled"</item>
+        <item>"2.5 cm"</item>
+        <item>"5 cm"</item>
+        <item>"10 cm"</item>
+        <item>"15 cm"</item>
+        <item>"20 cm"</item>
+        <item>"25 cm"</item>
+        <item>"30 cm"</item>
+    </string-array>
+    <string-array name="pref_depth_bleeding_error_values">
+        <item>"0"</item>
+        <item>"0.025"</item>
+        <item>"0.05"</item>
+        <item>"0.10"</item>
+        <item>"0.15"</item>
+        <item>"0.20"</item>
+        <item>"0.25"</item>
+        <item>"0.30"</item>
+    </string-array>
     <string-array name="pref_max_speed_keys">
         <item>"No Limit"</item>
         <item>"High"</item>

app/android/src/com/introlab/rtabmap/RTABMapActivity.java

@@ -1140,6 +1140,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			RTABMapLib.setRawScanSaved(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_raw_scan_saved), Boolean.parseBoolean(getString(R.string.pref_default_raw_scan_saved))));
 			RTABMapLib.setFullResolution(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_resolution), Boolean.parseBoolean(getString(R.string.pref_default_resolution))));
 			RTABMapLib.setSmoothing(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_smoothing), Boolean.parseBoolean(getString(R.string.pref_default_smoothing))));
+			RTABMapLib.setDepthBleedingError(nativeApplication, Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_depth_bleeding_error), getString(R.string.pref_default_depth_bleeding_error))));
 			RTABMapLib.setDepthFromMotion(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_depth_from_motion), Boolean.parseBoolean(getString(R.string.pref_default_depth_from_motion))));
 			RTABMapLib.setCameraColor(nativeApplication, !sharedPref.getBoolean(getString(R.string.pref_key_fisheye), Boolean.parseBoolean(getString(R.string.pref_default_fisheye))));
 			RTABMapLib.setAppendMode(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_append), Boolean.parseBoolean(getString(R.string.pref_default_append))));

app/android/src/com/introlab/rtabmap/RTABMapLib.java

@@ -73,6 +73,7 @@ public class RTABMapLib
     public static native void setRawScanSaved(long nativeApplication, boolean enabled);
     public static native void setFullResolution(long nativeApplication, boolean enabled);
     public static native void setSmoothing(long nativeApplication, boolean enabled);
+    public static native void setDepthBleedingError(long nativeApplication, float value);
     public static native void setDepthFromMotion(long nativeApplication, boolean enabled);
     public static native void setCameraColor(long nativeApplication, boolean enabled);
     public static native void setAppendMode(long nativeApplication, boolean enabled);

app/android/src/com/introlab/rtabmap/SettingsActivity.java

@@ -222,7 +222,8 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((Preference)findPreference(getString(R.string.pref_key_background_color))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_background_color))).getEntry() + ") "+getString(R.string.pref_summary_background_color));
         ((Preference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
           
-        ((Preference)findPreference(getString(R.string.pref_key_arcore_relocalization_acc_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_arcore_relocalization_acc_thr))).getEntry() + ") "+getString(R.string.pref_summary_arcore_relocalization_acc_thr));
+        ((Preference)findPreference(getString(R.string.pref_key_depth_bleeding_error))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_depth_bleeding_error))).getEntry() + ") "+getString(R.string.pref_summary_depth_bleeding_error));
+		((Preference)findPreference(getString(R.string.pref_key_arcore_relocalization_acc_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_arcore_relocalization_acc_thr))).getEntry() + ") "+getString(R.string.pref_summary_arcore_relocalization_acc_thr));
         ((Preference)findPreference(getString(R.string.pref_key_update_rate))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_update_rate))).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
         ((Preference)findPreference(getString(R.string.pref_key_max_speed))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_max_speed))).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
         ((Preference)findPreference(getString(R.string.pref_key_time_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_time_thr))).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
@@ -285,7 +286,8 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	if(key.compareTo(getString(R.string.pref_key_background_color))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_background_color));
         	if(key.compareTo(getString(R.string.pref_key_rendering_texture_decimation))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
         	
-        	if(key.compareTo(getString(R.string.pref_key_arcore_relocalization_acc_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_arcore_relocalization_acc_thr));
+        	if(key.compareTo(getString(R.string.pref_key_depth_bleeding_error))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_depth_bleeding_error));
+			if(key.compareTo(getString(R.string.pref_key_arcore_relocalization_acc_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_arcore_relocalization_acc_thr));
         	if(key.compareTo(getString(R.string.pref_key_update_rate))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
         	if(key.compareTo(getString(R.string.pref_key_max_speed))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
         	if(key.compareTo(getString(R.string.pref_key_time_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_time_thr));

app/ios/RTABMapApp.xcodeproj/project.pbxproj

@@ -8,49 +8,90 @@
 
 /* Begin PBXBuildFile section */
 		4E0D83832621F52C00C879AC /* Settings.bundle in Resources */ = {isa = PBXBuildFile; fileRef = 4E0D83822621F52C00C879AC /* Settings.bundle */; };
+		4E10D89C2DE409870015926D /* librtabmap_core.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB0259D5BE500575D88 /* librtabmap_core.a */; };
+		4E10D89D2DE409870015926D /* librtabmap_utilite.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EAF259D5BE500575D88 /* librtabmap_utilite.a */; };
+		4E10D89E2DE409A60015926D /* libopencv_xfeatures2d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED9259D5BFC00575D88 /* libopencv_xfeatures2d.a */; };
+		4E10D89F2DE409B20015926D /* libopencv_video.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB4259D5BFB00575D88 /* libopencv_video.a */; };
+		4E10D8A02DE409BB0015926D /* libopencv_tracking.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ECC259D5BFC00575D88 /* libopencv_tracking.a */; };
+		4E10D8A12DE409C00015926D /* libopencv_optflow.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC3259D5BFC00575D88 /* libopencv_optflow.a */; };
+		4E10D8A22DE409C70015926D /* libopencv_imgproc.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBE259D5BFC00575D88 /* libopencv_imgproc.a */; };
+		4E10D8A32DE409CA0015926D /* libopencv_features2d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB8259D5BFB00575D88 /* libopencv_features2d.a */; };
+		4E10D8A42DE409CE0015926D /* libopencv_core.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED1259D5BFC00575D88 /* libopencv_core.a */; };
+		4E10D8A52DE409D10015926D /* libopencv_calib3d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED6259D5BFC00575D88 /* libopencv_calib3d.a */; };
+		4E10D8A62DE409D50015926D /* libopencv_aruco.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB5259D5BFB00575D88 /* libopencv_aruco.a */; };
+		4E10D8A72DE409F40015926D /* libopencv_flann.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBC259D5BFB00575D88 /* libopencv_flann.a */; };
+		4E10D8A82DE40A1B0015926D /* libopencv_ximgproc.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EDC259D5BFC00575D88 /* libopencv_ximgproc.a */; };
+		4E10D8A92DE40A3A0015926D /* libopencv_photo.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB6259D5BFB00575D88 /* libopencv_photo.a */; };
+		4E10D8AA2DE40A6C0015926D /* libopencv_objdetect.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBB259D5BFB00575D88 /* libopencv_objdetect.a */; };
+		4E10D8AB2DE40A8C0015926D /* libopencv_stitching.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBD259D5BFB00575D88 /* libopencv_stitching.a */; };
+		4E10D8AC2DE40AA20015926D /* libopencv_imgcodecs.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC4259D5BFC00575D88 /* libopencv_imgcodecs.a */; };
+		4E10D8AD2DE40E990015926D /* libg2o_core.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0D259D5C2200575D88 /* libg2o_core.a */; };
+		4E10D8AE2DE40E990015926D /* libg2o_solver_dense.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F07259D5C2100575D88 /* libg2o_solver_dense.a */; };
+		4E10D8AF2DE40E990015926D /* libg2o_solver_eigen.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F12259D5C2200575D88 /* libg2o_solver_eigen.a */; };
+		4E10D8B02DE40E990015926D /* libg2o_solver_pcg.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0B259D5C2200575D88 /* libg2o_solver_pcg.a */; };
+		4E10D8B12DE40E990015926D /* libg2o_solver_slam2d_linear.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F09259D5C2100575D88 /* libg2o_solver_slam2d_linear.a */; };
+		4E10D8B22DE40E990015926D /* libg2o_solver_structure_only.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F11259D5C2200575D88 /* libg2o_solver_structure_only.a */; };
+		4E10D8B32DE40E990015926D /* libg2o_stuff.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F14259D5C2200575D88 /* libg2o_stuff.a */; };
+		4E10D8B42DE40E990015926D /* libg2o_types_data.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0C259D5C2200575D88 /* libg2o_types_data.a */; };
+		4E10D8B52DE40E990015926D /* libg2o_types_icp.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0F259D5C2200575D88 /* libg2o_types_icp.a */; };
+		4E10D8B62DE40E990015926D /* libg2o_types_sba.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F15259D5C2200575D88 /* libg2o_types_sba.a */; };
+		4E10D8B72DE40E990015926D /* libg2o_types_sclam2d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F1A259D5C2200575D88 /* libg2o_types_sclam2d.a */; };
+		4E10D8B82DE40E990015926D /* libg2o_types_sim3.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F1B259D5C2200575D88 /* libg2o_types_sim3.a */; };
+		4E10D8B92DE40E990015926D /* libg2o_types_slam2d_addons.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F16259D5C2200575D88 /* libg2o_types_slam2d_addons.a */; };
+		4E10D8BA2DE40E990015926D /* libg2o_types_slam2d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0A259D5C2100575D88 /* libg2o_types_slam2d.a */; };
+		4E10D8BB2DE40E990015926D /* libg2o_types_slam3d_addons.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F17259D5C2200575D88 /* libg2o_types_slam3d_addons.a */; };
+		4E10D8BC2DE40E990015926D /* libg2o_types_slam3d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F18259D5C2200575D88 /* libg2o_types_slam3d.a */; };
+		4E10D8BD2DE40EC70015926D /* libpcl_common.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4D259D4DF100575D88 /* libpcl_common.a */; };
+		4E10D8BE2DE40EC70015926D /* libpcl_features.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B43259D4DF100575D88 /* libpcl_features.a */; };
+		4E10D8BF2DE40EC70015926D /* libpcl_filters.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B44259D4DF100575D88 /* libpcl_filters.a */; };
+		4E10D8C02DE40EC70015926D /* libpcl_io_ply.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4E259D4DF100575D88 /* libpcl_io_ply.a */; };
+		4E10D8C12DE40EC70015926D /* libpcl_io.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4C259D4DF100575D88 /* libpcl_io.a */; };
+		4E10D8C22DE40EC70015926D /* libpcl_kdtree.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B46259D4DF100575D88 /* libpcl_kdtree.a */; };
+		4E10D8C32DE40EC70015926D /* libpcl_keypoints.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B49259D4DF100575D88 /* libpcl_keypoints.a */; };
+		4E10D8C42DE40EC70015926D /* libpcl_ml.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4F259D4DF100575D88 /* libpcl_ml.a */; };
+		4E10D8C52DE40EC70015926D /* libpcl_octree.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4B259D4DF100575D88 /* libpcl_octree.a */; };
+		4E10D8C62DE40EC70015926D /* libpcl_registration.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B42259D4DF100575D88 /* libpcl_registration.a */; };
+		4E10D8C72DE40EC70015926D /* libpcl_sample_consensus.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B45259D4DF100575D88 /* libpcl_sample_consensus.a */; };
+		4E10D8C82DE40EC70015926D /* libpcl_search.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B47259D4DF100575D88 /* libpcl_search.a */; };
+		4E10D8C92DE40EC70015926D /* libpcl_segmentation.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B50259D4DF100575D88 /* libpcl_segmentation.a */; };
+		4E10D8CA2DE40EC70015926D /* libpcl_stereo.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B41259D4DF100575D88 /* libpcl_stereo.a */; };
+		4E10D8CB2DE40EC70015926D /* libpcl_surface.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B48259D4DF100575D88 /* libpcl_surface.a */; };
+		4E10D8CC2DE40EE30015926D /* libgtsam_unstable.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0E259D5C2200575D88 /* libgtsam_unstable.a */; };
+		4E10D8CD2DE40EE30015926D /* libgtsam.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F1C259D5C2200575D88 /* libgtsam.a */; };
+		4E10D8CE2DE40EE30015926D /* libmetis-gtsam.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E10D8622DE408EE0015926D /* libmetis-gtsam.a */; };
+		4E10D8CF2DE40EF90015926D /* libboost_serialization.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B30259D4DE900575D88 /* libboost_serialization.a */; };
+		4E10D8D02DE40F060015926D /* libboost_timer.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B34259D4DE900575D88 /* libboost_timer.a */; };
+		4E10D8D12DE40F190015926D /* liblas_c.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E10D8752DE408EF0015926D /* liblas_c.a */; };
+		4E10D8D22DE40F190015926D /* liblas.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFAA9422CAE4E960055DA51 /* liblas.a */; };
+		4E10D8D32DE40F190015926D /* liblaszip.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4ED73AD02D7FECF3007899C2 /* liblaszip.a */; };
+		4E10D8D42DE40F2A0015926D /* libboost_thread.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B33259D4DE900575D88 /* libboost_thread.a */; };
+		4E10D8E12DE4101B0015926D /* liblibjpeg-turbo.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E10D8D52DE40FBE0015926D /* liblibjpeg-turbo.a */; };
+		4E10D8E22DE4101B0015926D /* liblibpng.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E10D8D72DE40FCA0015926D /* liblibpng.a */; };
 		4E1E9CF72D618E69000CB881 /* quad_color.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4E1E9CF62D618079000CB881 /* quad_color.cpp */; };
 		4E1E9CF82D618E69000CB881 /* text_drawable.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4E1E9CF52D618079000CB881 /* text_drawable.cpp */; };
 		4E20B24E266AB94300316EE6 /* Images.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = 4E20B24D266AB94300316EE6 /* Images.xcassets */; };
 		4E20B250266AB95600316EE6 /* Images.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = 4E20B24F266AB95600316EE6 /* Images.xcassets */; };
 		4E2C516725A63119005CEDBD /* DatabaseView.swift in Sources */ = {isa = PBXBuildFile; fileRef = 4E2C516525A63119005CEDBD /* DatabaseView.swift */; };
 		4E2C516825A63119005CEDBD /* VerticalScrollerView.swift in Sources */ = {isa = PBXBuildFile; fileRef = 4E2C516625A63119005CEDBD /* VerticalScrollerView.swift */; };
+		4E3525542DE4C6E500F39D8F /* libg2o_csparse_extension.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F13259D5C2200575D88 /* libg2o_csparse_extension.a */; };
+		4E3525552DE4C6E500F39D8F /* libg2o_solver_csparse.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F10259D5C2200575D88 /* libg2o_solver_csparse.a */; };
+		4E3525572DE4C6E500F39D8F /* libg2o_solver_cholmod.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E3525562DE4C6E500F39D8F /* libg2o_solver_cholmod.a */; };
+		4E3525592DE4C6F600F39D8F /* libcholmod.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E3525582DE4C6F600F39D8F /* libcholmod.a */; };
+		4E35255E2DE4C71000F39D8F /* libcxsparse.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E35255A2DE4C71000F39D8F /* libcxsparse.a */; };
+		4E35255F2DE4C71000F39D8F /* libcolamd.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E35255B2DE4C71000F39D8F /* libcolamd.a */; };
+		4E3525602DE4C71000F39D8F /* libcamd.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E35255C2DE4C71000F39D8F /* libcamd.a */; };
+		4E3525612DE4C71000F39D8F /* libccolamd.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E35255D2DE4C71000F39D8F /* libccolamd.a */; };
+		4E3525642DE4C71C00F39D8F /* libsuitesparseconfig.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E3525622DE4C71C00F39D8F /* libsuitesparseconfig.a */; };
+		4E3525652DE4C71C00F39D8F /* libspqr.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E3525632DE4C71C00F39D8F /* libspqr.a */; };
+		4E3525672DE4C72D00F39D8F /* libamd.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4E3525662DE4C72D00F39D8F /* libamd.a */; };
 		4E8B155426273A580037FC53 /* Zip in Frameworks */ = {isa = PBXBuildFile; productRef = 4E8B155326273A580037FC53 /* Zip */; };
 		4EB1A66625A0D90B0043C7BF /* RTABMap.swift in Sources */ = {isa = PBXBuildFile; fileRef = 4EB1A66525A0D90B0043C7BF /* RTABMap.swift */; };
-		4ED73AD12D7FECF3007899C2 /* liblaszip.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4ED73AD02D7FECF3007899C2 /* liblaszip.a */; };
 		4EE016B3259BE441008CCE65 /* ViewController.swift in Sources */ = {isa = PBXBuildFile; fileRef = 4EE016AF259BE441008CCE65 /* ViewController.swift */; };
 		4EE016B4259BE441008CCE65 /* SceneDelegate.swift in Sources */ = {isa = PBXBuildFile; fileRef = 4EE016B0259BE441008CCE65 /* SceneDelegate.swift */; };
 		4EE016B9259BE449008CCE65 /* AppDelegate.swift in Sources */ = {isa = PBXBuildFile; fileRef = 4EE016B8259BE449008CCE65 /* AppDelegate.swift */; };
 		4EE016C3259BE464008CCE65 /* Main.storyboard in Resources */ = {isa = PBXBuildFile; fileRef = 4EE016BF259BE464008CCE65 /* Main.storyboard */; };
 		4EE016C4259BE464008CCE65 /* LaunchScreen.storyboard in Resources */ = {isa = PBXBuildFile; fileRef = 4EE016C1259BE464008CCE65 /* LaunchScreen.storyboard */; };
 		4EE016C7259BE46F008CCE65 /* Assets.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = 4EE016C6259BE46F008CCE65 /* Assets.xcassets */; };
-		4EFAA9432CAE4E960055DA51 /* liblas.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFAA9422CAE4E960055DA51 /* liblas.a */; };
-		4EFD0B36259D4DE900575D88 /* libboost_filesystem.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B2B259D4DE900575D88 /* libboost_filesystem.a */; };
-		4EFD0B37259D4DE900575D88 /* libboost_program_options.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B2C259D4DE900575D88 /* libboost_program_options.a */; };
-		4EFD0B38259D4DE900575D88 /* libboost_regex.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B2D259D4DE900575D88 /* libboost_regex.a */; };
-		4EFD0B39259D4DE900575D88 /* libboost_iostreams.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B2E259D4DE900575D88 /* libboost_iostreams.a */; };
-		4EFD0B3A259D4DE900575D88 /* libboost_date_time.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B2F259D4DE900575D88 /* libboost_date_time.a */; };
-		4EFD0B3B259D4DE900575D88 /* libboost_serialization.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B30259D4DE900575D88 /* libboost_serialization.a */; };
-		4EFD0B3C259D4DE900575D88 /* libboost_system.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B31259D4DE900575D88 /* libboost_system.a */; };
-		4EFD0B3D259D4DE900575D88 /* libboost_signals.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B32259D4DE900575D88 /* libboost_signals.a */; };
-		4EFD0B3E259D4DE900575D88 /* libboost_thread.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B33259D4DE900575D88 /* libboost_thread.a */; };
-		4EFD0B3F259D4DE900575D88 /* libboost_timer.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B34259D4DE900575D88 /* libboost_timer.a */; };
-		4EFD0B40259D4DE900575D88 /* libboost_chrono.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B35259D4DE900575D88 /* libboost_chrono.a */; };
-		4EFD0B51259D4DF100575D88 /* libpcl_stereo.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B41259D4DF100575D88 /* libpcl_stereo.a */; };
-		4EFD0B52259D4DF100575D88 /* libpcl_registration.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B42259D4DF100575D88 /* libpcl_registration.a */; };
-		4EFD0B53259D4DF100575D88 /* libpcl_features.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B43259D4DF100575D88 /* libpcl_features.a */; };
-		4EFD0B54259D4DF100575D88 /* libpcl_filters.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B44259D4DF100575D88 /* libpcl_filters.a */; };
-		4EFD0B55259D4DF100575D88 /* libpcl_sample_consensus.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B45259D4DF100575D88 /* libpcl_sample_consensus.a */; };
-		4EFD0B56259D4DF100575D88 /* libpcl_kdtree.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B46259D4DF100575D88 /* libpcl_kdtree.a */; };
-		4EFD0B57259D4DF100575D88 /* libpcl_search.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B47259D4DF100575D88 /* libpcl_search.a */; };
-		4EFD0B58259D4DF100575D88 /* libpcl_surface.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B48259D4DF100575D88 /* libpcl_surface.a */; };
-		4EFD0B59259D4DF100575D88 /* libpcl_keypoints.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B49259D4DF100575D88 /* libpcl_keypoints.a */; };
-		4EFD0B5A259D4DF100575D88 /* libpcl_recognition.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4A259D4DF100575D88 /* libpcl_recognition.a */; };
-		4EFD0B5B259D4DF100575D88 /* libpcl_octree.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4B259D4DF100575D88 /* libpcl_octree.a */; };
-		4EFD0B5C259D4DF100575D88 /* libpcl_io.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4C259D4DF100575D88 /* libpcl_io.a */; };
-		4EFD0B5D259D4DF100575D88 /* libpcl_common.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4D259D4DF100575D88 /* libpcl_common.a */; };
-		4EFD0B5E259D4DF100575D88 /* libpcl_io_ply.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4E259D4DF100575D88 /* libpcl_io_ply.a */; };
-		4EFD0B5F259D4DF100575D88 /* libpcl_ml.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B4F259D4DF100575D88 /* libpcl_ml.a */; };
-		4EFD0B60259D4DF100575D88 /* libpcl_segmentation.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B50259D4DF100575D88 /* libpcl_segmentation.a */; };
 		4EFD0B97259D501E00575D88 /* line.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0B63259D501E00575D88 /* line.cpp */; };
 		4EFD0B98259D501E00575D88 /* util.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0B64259D501E00575D88 /* util.cpp */; };
 		4EFD0B99259D501E00575D88 /* obj_loader.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0B65259D501E00575D88 /* obj_loader.cpp */; };
@@ -78,80 +119,8 @@
 		4EFD0BBC259D509300575D88 /* point_cloud_drawable.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0BBA259D509300575D88 /* point_cloud_drawable.cpp */; };
 		4EFD0BC0259D50A800575D88 /* scene.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0BBE259D50A800575D88 /* scene.cpp */; };
 		4EFD0EAB259D581800575D88 /* OpenGLES.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EAA259D581800575D88 /* OpenGLES.framework */; };
-		4EFD0EB1259D5BE500575D88 /* librtabmap_utilite.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EAF259D5BE500575D88 /* librtabmap_utilite.a */; };
-		4EFD0EB2259D5BE500575D88 /* librtabmap_core.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB0259D5BE500575D88 /* librtabmap_core.a */; };
-		4EFD0EDD259D5BFC00575D88 /* libopencv_video.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB4259D5BFB00575D88 /* libopencv_video.a */; };
-		4EFD0EDE259D5BFC00575D88 /* libopencv_aruco.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB5259D5BFB00575D88 /* libopencv_aruco.a */; };
-		4EFD0EDF259D5BFC00575D88 /* libopencv_photo.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB6259D5BFB00575D88 /* libopencv_photo.a */; };
-		4EFD0EE0259D5BFC00575D88 /* libopencv_line_descriptor.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB7259D5BFB00575D88 /* libopencv_line_descriptor.a */; };
-		4EFD0EE1259D5BFC00575D88 /* libopencv_features2d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB8259D5BFB00575D88 /* libopencv_features2d.a */; };
-		4EFD0EE2259D5BFC00575D88 /* libopencv_bgsegm.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EB9259D5BFB00575D88 /* libopencv_bgsegm.a */; };
-		4EFD0EE3259D5BFC00575D88 /* libopencv_dnn_objdetect.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBA259D5BFB00575D88 /* libopencv_dnn_objdetect.a */; };
-		4EFD0EE4259D5BFC00575D88 /* libopencv_objdetect.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBB259D5BFB00575D88 /* libopencv_objdetect.a */; };
-		4EFD0EE5259D5BFC00575D88 /* libopencv_flann.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBC259D5BFB00575D88 /* libopencv_flann.a */; };
-		4EFD0EE6259D5BFC00575D88 /* libopencv_stitching.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBD259D5BFB00575D88 /* libopencv_stitching.a */; };
-		4EFD0EE7259D5BFC00575D88 /* libopencv_imgproc.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBE259D5BFC00575D88 /* libopencv_imgproc.a */; };
-		4EFD0EE8259D5BFC00575D88 /* libopencv_ml.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EBF259D5BFC00575D88 /* libopencv_ml.a */; };
-		4EFD0EE9259D5BFC00575D88 /* libopencv_rgbd.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC0259D5BFC00575D88 /* libopencv_rgbd.a */; };
-		4EFD0EEA259D5BFC00575D88 /* libopencv_xphoto.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC1259D5BFC00575D88 /* libopencv_xphoto.a */; };
-		4EFD0EEB259D5BFC00575D88 /* libopencv_phase_unwrapping.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC2259D5BFC00575D88 /* libopencv_phase_unwrapping.a */; };
-		4EFD0EEC259D5BFC00575D88 /* libopencv_optflow.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC3259D5BFC00575D88 /* libopencv_optflow.a */; };
-		4EFD0EED259D5BFC00575D88 /* libopencv_imgcodecs.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC4259D5BFC00575D88 /* libopencv_imgcodecs.a */; };
-		4EFD0EEE259D5BFC00575D88 /* libopencv_fuzzy.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC5259D5BFC00575D88 /* libopencv_fuzzy.a */; };
-		4EFD0EEF259D5BFC00575D88 /* libopencv_dpm.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC6259D5BFC00575D88 /* libopencv_dpm.a */; };
-		4EFD0EF0259D5BFC00575D88 /* libopencv_plot.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC7259D5BFC00575D88 /* libopencv_plot.a */; };
-		4EFD0EF1259D5BFC00575D88 /* libopencv_dnn.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC8259D5BFC00575D88 /* libopencv_dnn.a */; };
-		4EFD0EF2259D5BFC00575D88 /* libopencv_saliency.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EC9259D5BFC00575D88 /* libopencv_saliency.a */; };
-		4EFD0EF3259D5BFC00575D88 /* libopencv_videoio.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ECA259D5BFC00575D88 /* libopencv_videoio.a */; };
-		4EFD0EF4259D5BFC00575D88 /* libopencv_hfs.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ECB259D5BFC00575D88 /* libopencv_hfs.a */; };
-		4EFD0EF5259D5BFC00575D88 /* libopencv_tracking.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ECC259D5BFC00575D88 /* libopencv_tracking.a */; };
-		4EFD0EF6259D5BFC00575D88 /* libopencv_text.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ECD259D5BFC00575D88 /* libopencv_text.a */; };
-		4EFD0EF7259D5BFC00575D88 /* libopencv_face.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ECE259D5BFC00575D88 /* libopencv_face.a */; };
-		4EFD0EF8259D5BFC00575D88 /* libopencv_bioinspired.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ECF259D5BFC00575D88 /* libopencv_bioinspired.a */; };
-		4EFD0EF9259D5BFC00575D88 /* libopencv_highgui.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED0259D5BFC00575D88 /* libopencv_highgui.a */; };
-		4EFD0EFA259D5BFC00575D88 /* libopencv_core.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED1259D5BFC00575D88 /* libopencv_core.a */; };
-		4EFD0EFB259D5BFC00575D88 /* libopencv_shape.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED2259D5BFC00575D88 /* libopencv_shape.a */; };
-		4EFD0EFC259D5BFC00575D88 /* libopencv_datasets.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED3259D5BFC00575D88 /* libopencv_datasets.a */; };
-		4EFD0EFD259D5BFC00575D88 /* libopencv_img_hash.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED4259D5BFC00575D88 /* libopencv_img_hash.a */; };
-		4EFD0EFE259D5BFC00575D88 /* libopencv_surface_matching.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED5259D5BFC00575D88 /* libopencv_surface_matching.a */; };
-		4EFD0EFF259D5BFC00575D88 /* libopencv_calib3d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED6259D5BFC00575D88 /* libopencv_calib3d.a */; };
-		4EFD0F00259D5BFC00575D88 /* libopencv_structured_light.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED7259D5BFC00575D88 /* libopencv_structured_light.a */; };
-		4EFD0F01259D5BFC00575D88 /* libopencv_reg.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED8259D5BFC00575D88 /* libopencv_reg.a */; };
-		4EFD0F02259D5BFC00575D88 /* libopencv_xfeatures2d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0ED9259D5BFC00575D88 /* libopencv_xfeatures2d.a */; };
-		4EFD0F03259D5BFC00575D88 /* libopencv_videostab.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EDA259D5BFC00575D88 /* libopencv_videostab.a */; };
-		4EFD0F04259D5BFC00575D88 /* libopencv_ccalib.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EDB259D5BFC00575D88 /* libopencv_ccalib.a */; };
-		4EFD0F05259D5BFC00575D88 /* libopencv_ximgproc.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0EDC259D5BFC00575D88 /* libopencv_ximgproc.a */; };
-		4EFD0F1D259D5C2200575D88 /* libg2o_solver_dense.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F07259D5C2100575D88 /* libg2o_solver_dense.a */; };
-		4EFD0F1E259D5C2200575D88 /* libg2o_ext_csparse.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F08259D5C2100575D88 /* libg2o_ext_csparse.a */; };
-		4EFD0F1F259D5C2200575D88 /* libg2o_solver_slam2d_linear.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F09259D5C2100575D88 /* libg2o_solver_slam2d_linear.a */; };
-		4EFD0F20259D5C2200575D88 /* libg2o_types_slam2d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0A259D5C2100575D88 /* libg2o_types_slam2d.a */; };
-		4EFD0F21259D5C2200575D88 /* libg2o_solver_pcg.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0B259D5C2200575D88 /* libg2o_solver_pcg.a */; };
-		4EFD0F22259D5C2200575D88 /* libg2o_types_data.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0C259D5C2200575D88 /* libg2o_types_data.a */; };
-		4EFD0F23259D5C2200575D88 /* libg2o_core.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0D259D5C2200575D88 /* libg2o_core.a */; };
-		4EFD0F24259D5C2200575D88 /* libgtsam_unstable.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0E259D5C2200575D88 /* libgtsam_unstable.a */; };
-		4EFD0F25259D5C2200575D88 /* libg2o_types_icp.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F0F259D5C2200575D88 /* libg2o_types_icp.a */; };
-		4EFD0F26259D5C2200575D88 /* libg2o_solver_csparse.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F10259D5C2200575D88 /* libg2o_solver_csparse.a */; };
-		4EFD0F27259D5C2200575D88 /* libg2o_solver_structure_only.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F11259D5C2200575D88 /* libg2o_solver_structure_only.a */; };
-		4EFD0F28259D5C2200575D88 /* libg2o_solver_eigen.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F12259D5C2200575D88 /* libg2o_solver_eigen.a */; };
-		4EFD0F29259D5C2200575D88 /* libg2o_csparse_extension.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F13259D5C2200575D88 /* libg2o_csparse_extension.a */; };
-		4EFD0F2A259D5C2200575D88 /* libg2o_stuff.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F14259D5C2200575D88 /* libg2o_stuff.a */; };
-		4EFD0F2B259D5C2200575D88 /* libg2o_types_sba.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F15259D5C2200575D88 /* libg2o_types_sba.a */; };
-		4EFD0F2C259D5C2200575D88 /* libg2o_types_slam2d_addons.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F16259D5C2200575D88 /* libg2o_types_slam2d_addons.a */; };
-		4EFD0F2D259D5C2200575D88 /* libg2o_types_slam3d_addons.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F17259D5C2200575D88 /* libg2o_types_slam3d_addons.a */; };
-		4EFD0F2E259D5C2200575D88 /* libg2o_types_slam3d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F18259D5C2200575D88 /* libg2o_types_slam3d.a */; };
-		4EFD0F2F259D5C2200575D88 /* libmetis.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F19259D5C2200575D88 /* libmetis.a */; };
-		4EFD0F30259D5C2200575D88 /* libg2o_types_sclam2d.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F1A259D5C2200575D88 /* libg2o_types_sclam2d.a */; };
-		4EFD0F31259D5C2200575D88 /* libg2o_types_sim3.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F1B259D5C2200575D88 /* libg2o_types_sim3.a */; };
-		4EFD0F32259D5C2200575D88 /* libgtsam.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F1C259D5C2200575D88 /* libgtsam.a */; };
-		4EFD0F34259D5C2900575D88 /* libflann_cpp_s.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F33259D5C2900575D88 /* libflann_cpp_s.a */; };
 		4EFD0F36259D5C7300575D88 /* vtk.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F35259D5C7300575D88 /* vtk.framework */; };
 		4EFD0F3A259D5D1500575D88 /* CameraMobile.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0F38259D5D1500575D88 /* CameraMobile.cpp */; };
-		4EFD0F3D259D5D5C00575D88 /* libsqlite3.tbd in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F3C259D5D5C00575D88 /* libsqlite3.tbd */; };
-		4EFD0F50259D67D900575D88 /* liblibwebp.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F4B259D67D900575D88 /* liblibwebp.a */; };
-		4EFD0F51259D67D900575D88 /* liblibpng.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F4C259D67D900575D88 /* liblibpng.a */; };
-		4EFD0F52259D67D900575D88 /* liblibjpeg-turbo.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F4D259D67D900575D88 /* liblibjpeg-turbo.a */; };
-		4EFD0F53259D67D900575D88 /* libzlib.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F4E259D67D900575D88 /* libzlib.a */; };
-		4EFD0F54259D67D900575D88 /* liblibprotobuf.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0F4F259D67D900575D88 /* liblibprotobuf.a */; };
 		4EFD0F62259E341C00575D88 /* RTABMapApp.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0F61259E341C00575D88 /* RTABMapApp.cpp */; };
 		4EFD0F67259E38DE00575D88 /* NativeWrapper.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0F66259E38DE00575D88 /* NativeWrapper.cpp */; };
 		4EFD0F70259F847300575D88 /* background_renderer.cc in Sources */ = {isa = PBXBuildFile; fileRef = 4EFD0F6E259F847300575D88 /* background_renderer.cc */; };
@@ -160,6 +129,42 @@
 /* Begin PBXFileReference section */
 		44D4681D2D538A4100B094BA /* RTABMapApp.entitlements */ = {isa = PBXFileReference; lastKnownFileType = text.plist.entitlements; name = RTABMapApp.entitlements; path = RTABMapApp/RTABMapApp.entitlements; sourceTree = "<group>"; };
 		4E0D83822621F52C00C879AC /* Settings.bundle */ = {isa = PBXFileReference; lastKnownFileType = "wrapper.plug-in"; path = Settings.bundle; sourceTree = "<group>"; };
+		4E10D8602DE408EE0015926D /* libboost_stacktrace_noop.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_stacktrace_noop.a; path = RTABMapApp/Libraries/lib/libboost_stacktrace_noop.a; sourceTree = "<group>"; };
+		4E10D8612DE408EE0015926D /* libboost_wserialization.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_wserialization.a; path = RTABMapApp/Libraries/lib/libboost_wserialization.a; sourceTree = "<group>"; };
+		4E10D8622DE408EE0015926D /* libmetis-gtsam.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = "libmetis-gtsam.a"; path = "RTABMapApp/Libraries/lib/libmetis-gtsam.a"; sourceTree = "<group>"; };
+		4E10D8632DE408EE0015926D /* libopencv_wechat_qrcode.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libopencv_wechat_qrcode.a; path = RTABMapApp/Libraries/lib/libopencv_wechat_qrcode.a; sourceTree = "<group>"; };
+		4E10D8642DE408EE0015926D /* libboost_container.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_container.a; path = RTABMapApp/Libraries/lib/libboost_container.a; sourceTree = "<group>"; };
+		4E10D8652DE408EE0015926D /* libboost_graph.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_graph.a; path = RTABMapApp/Libraries/lib/libboost_graph.a; sourceTree = "<group>"; };
+		4E10D8662DE408EE0015926D /* libboost_json.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_json.a; path = RTABMapApp/Libraries/lib/libboost_json.a; sourceTree = "<group>"; };
+		4E10D8672DE408EE0015926D /* libboost_stacktrace_basic.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_stacktrace_basic.a; path = RTABMapApp/Libraries/lib/libboost_stacktrace_basic.a; sourceTree = "<group>"; };
+		4E10D8682DE408EE0015926D /* libboost_atomic.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_atomic.a; path = RTABMapApp/Libraries/lib/libboost_atomic.a; sourceTree = "<group>"; };
+		4E10D8692DE408EE0015926D /* libopencv_intensity_transform.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libopencv_intensity_transform.a; path = RTABMapApp/Libraries/lib/libopencv_intensity_transform.a; sourceTree = "<group>"; };
+		4E10D86A2DE408EF0015926D /* libboost_locale.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_locale.a; path = RTABMapApp/Libraries/lib/libboost_locale.a; sourceTree = "<group>"; };
+		4E10D86B2DE408EF0015926D /* liblz4.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = liblz4.a; path = RTABMapApp/Libraries/lib/liblz4.a; sourceTree = "<group>"; };
+		4E10D86C2DE408EF0015926D /* libopencv_dnn_superres.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libopencv_dnn_superres.a; path = RTABMapApp/Libraries/lib/libopencv_dnn_superres.a; sourceTree = "<group>"; };
+		4E10D86D2DE408EF0015926D /* libopencv_rapid.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libopencv_rapid.a; path = RTABMapApp/Libraries/lib/libopencv_rapid.a; sourceTree = "<group>"; };
+		4E10D86E2DE408EF0015926D /* libopencv_quality.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libopencv_quality.a; path = RTABMapApp/Libraries/lib/libopencv_quality.a; sourceTree = "<group>"; };
+		4E10D86F2DE408EF0015926D /* libboost_url.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_url.a; path = RTABMapApp/Libraries/lib/libboost_url.a; sourceTree = "<group>"; };
+		4E10D8702DE408EF0015926D /* libboost_unit_test_framework.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_unit_test_framework.a; path = RTABMapApp/Libraries/lib/libboost_unit_test_framework.a; sourceTree = "<group>"; };
+		4E10D8712DE408EF0015926D /* libboost_random.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_random.a; path = RTABMapApp/Libraries/lib/libboost_random.a; sourceTree = "<group>"; };
+		4E10D8722DE408EF0015926D /* libboost_wave.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_wave.a; path = RTABMapApp/Libraries/lib/libboost_wave.a; sourceTree = "<group>"; };
+		4E10D8732DE408EF0015926D /* libboost_charconv.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_charconv.a; path = RTABMapApp/Libraries/lib/libboost_charconv.a; sourceTree = "<group>"; };
+		4E10D8742DE408EF0015926D /* libboost_type_erasure.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_type_erasure.a; path = RTABMapApp/Libraries/lib/libboost_type_erasure.a; sourceTree = "<group>"; };
+		4E10D8752DE408EF0015926D /* liblas_c.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = liblas_c.a; path = RTABMapApp/Libraries/lib/liblas_c.a; sourceTree = "<group>"; };
+		4E10D8762DE408EF0015926D /* libboost_contract.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_contract.a; path = RTABMapApp/Libraries/lib/libboost_contract.a; sourceTree = "<group>"; };
+		4E10D8772DE408EF0015926D /* libopencv_gapi.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libopencv_gapi.a; path = RTABMapApp/Libraries/lib/libopencv_gapi.a; sourceTree = "<group>"; };
+		4E10D8782DE408EF0015926D /* libopencv_signal.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libopencv_signal.a; path = RTABMapApp/Libraries/lib/libopencv_signal.a; sourceTree = "<group>"; };
+		4E10D8792DE408EF0015926D /* libboost_prg_exec_monitor.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_prg_exec_monitor.a; path = RTABMapApp/Libraries/lib/libboost_prg_exec_monitor.a; sourceTree = "<group>"; };
+		4E10D87A2DE408EF0015926D /* libboost_stacktrace_addr2line.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_stacktrace_addr2line.a; path = RTABMapApp/Libraries/lib/libboost_stacktrace_addr2line.a; sourceTree = "<group>"; };
+		4E10D87B2DE408EF0015926D /* libboost_nowide.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_nowide.a; path = RTABMapApp/Libraries/lib/libboost_nowide.a; sourceTree = "<group>"; };
+		4E10D87C2DE408EF0015926D /* libboost_test_exec_monitor.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_test_exec_monitor.a; path = RTABMapApp/Libraries/lib/libboost_test_exec_monitor.a; sourceTree = "<group>"; };
+		4E10D87D2DE408EF0015926D /* libopencv_mcc.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libopencv_mcc.a; path = RTABMapApp/Libraries/lib/libopencv_mcc.a; sourceTree = "<group>"; };
+		4E10D8D52DE40FBE0015926D /* liblibjpeg-turbo.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = "liblibjpeg-turbo.a"; path = "RTABMapApp/Libraries/lib/opencv4/3rdparty/liblibjpeg-turbo.a"; sourceTree = "<group>"; };
+		4E10D8D72DE40FCA0015926D /* liblibpng.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = liblibpng.a; path = RTABMapApp/Libraries/lib/opencv4/3rdparty/liblibpng.a; sourceTree = "<group>"; };
+		4E10D8D92DE40FF60015926D /* libzlib.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libzlib.a; path = RTABMapApp/Libraries/lib/opencv4/3rdparty/libzlib.a; sourceTree = "<group>"; };
+		4E10D8DA2DE40FF60015926D /* libIlmImf.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libIlmImf.a; path = RTABMapApp/Libraries/lib/opencv4/3rdparty/libIlmImf.a; sourceTree = "<group>"; };
+		4E10D8DB2DE40FF60015926D /* libade.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libade.a; path = RTABMapApp/Libraries/lib/opencv4/3rdparty/libade.a; sourceTree = "<group>"; };
+		4E10D8DC2DE40FF60015926D /* liblibprotobuf.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = liblibprotobuf.a; path = RTABMapApp/Libraries/lib/opencv4/3rdparty/liblibprotobuf.a; sourceTree = "<group>"; };
 		4E1E9CF22D617D08000CB881 /* Measure.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = Measure.h; path = ../android/jni/Measure.h; sourceTree = "<group>"; };
 		4E1E9CF32D618079000CB881 /* quad_color.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = quad_color.h; path = ../android/jni/quad_color.h; sourceTree = "<group>"; };
 		4E1E9CF42D618079000CB881 /* text_drawable.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = text_drawable.h; path = ../android/jni/text_drawable.h; sourceTree = "<group>"; };
@@ -170,6 +175,15 @@
 		4E20B24F266AB95600316EE6 /* Images.xcassets */ = {isa = PBXFileReference; lastKnownFileType = folder.assetcatalog; name = Images.xcassets; path = RTABMapApp/Images.xcassets; sourceTree = "<group>"; };
 		4E2C516525A63119005CEDBD /* DatabaseView.swift */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.swift; name = DatabaseView.swift; path = RTABMapApp/DatabaseView.swift; sourceTree = "<group>"; };
 		4E2C516625A63119005CEDBD /* VerticalScrollerView.swift */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.swift; name = VerticalScrollerView.swift; path = RTABMapApp/VerticalScrollerView.swift; sourceTree = "<group>"; };
+		4E3525562DE4C6E500F39D8F /* libg2o_solver_cholmod.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libg2o_solver_cholmod.a; path = RTABMapApp/Libraries/lib/libg2o_solver_cholmod.a; sourceTree = "<group>"; };
+		4E3525582DE4C6F600F39D8F /* libcholmod.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libcholmod.a; path = RTABMapApp/Libraries/lib/libcholmod.a; sourceTree = "<group>"; };
+		4E35255A2DE4C71000F39D8F /* libcxsparse.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libcxsparse.a; path = RTABMapApp/Libraries/lib/libcxsparse.a; sourceTree = "<group>"; };
+		4E35255B2DE4C71000F39D8F /* libcolamd.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libcolamd.a; path = RTABMapApp/Libraries/lib/libcolamd.a; sourceTree = "<group>"; };
+		4E35255C2DE4C71000F39D8F /* libcamd.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libcamd.a; path = RTABMapApp/Libraries/lib/libcamd.a; sourceTree = "<group>"; };
+		4E35255D2DE4C71000F39D8F /* libccolamd.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libccolamd.a; path = RTABMapApp/Libraries/lib/libccolamd.a; sourceTree = "<group>"; };
+		4E3525622DE4C71C00F39D8F /* libsuitesparseconfig.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libsuitesparseconfig.a; path = RTABMapApp/Libraries/lib/libsuitesparseconfig.a; sourceTree = "<group>"; };
+		4E3525632DE4C71C00F39D8F /* libspqr.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libspqr.a; path = RTABMapApp/Libraries/lib/libspqr.a; sourceTree = "<group>"; };
+		4E3525662DE4C72D00F39D8F /* libamd.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libamd.a; path = RTABMapApp/Libraries/lib/libamd.a; sourceTree = "<group>"; };
 		4E401BE825CC92C100240A56 /* ProgressionStatus.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; name = ProgressionStatus.h; path = ../android/jni/ProgressionStatus.h; sourceTree = "<group>"; };
 		4EB1A66525A0D90B0043C7BF /* RTABMap.swift */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.swift; name = RTABMap.swift; path = RTABMapApp/RTABMap.swift; sourceTree = "<group>"; };
 		4ED73AD02D7FECF3007899C2 /* liblaszip.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = liblaszip.a; path = RTABMapApp/Libraries/lib/liblaszip.a; sourceTree = "<group>"; };
@@ -356,110 +370,79 @@
 			isa = PBXFrameworksBuildPhase;
 			buildActionMask = 2147483647;
 			files = (
-				4EFAA9432CAE4E960055DA51 /* liblas.a in Frameworks */,
-				4ED73AD12D7FECF3007899C2 /* liblaszip.a in Frameworks */,
-				4EFD0F50259D67D900575D88 /* liblibwebp.a in Frameworks */,
-				4EFD0F51259D67D900575D88 /* liblibpng.a in Frameworks */,
-				4EFD0F52259D67D900575D88 /* liblibjpeg-turbo.a in Frameworks */,
-				4EFD0F53259D67D900575D88 /* libzlib.a in Frameworks */,
-				4EFD0F54259D67D900575D88 /* liblibprotobuf.a in Frameworks */,
-				4EFD0F3D259D5D5C00575D88 /* libsqlite3.tbd in Frameworks */,
-				4EFD0F36259D5C7300575D88 /* vtk.framework in Frameworks */,
-				4EFD0F34259D5C2900575D88 /* libflann_cpp_s.a in Frameworks */,
-				4EFD0F1D259D5C2200575D88 /* libg2o_solver_dense.a in Frameworks */,
-				4EFD0F1E259D5C2200575D88 /* libg2o_ext_csparse.a in Frameworks */,
-				4EFD0F1F259D5C2200575D88 /* libg2o_solver_slam2d_linear.a in Frameworks */,
-				4EFD0F20259D5C2200575D88 /* libg2o_types_slam2d.a in Frameworks */,
-				4EFD0F21259D5C2200575D88 /* libg2o_solver_pcg.a in Frameworks */,
-				4EFD0F22259D5C2200575D88 /* libg2o_types_data.a in Frameworks */,
-				4EFD0F23259D5C2200575D88 /* libg2o_core.a in Frameworks */,
-				4EFD0F24259D5C2200575D88 /* libgtsam_unstable.a in Frameworks */,
-				4EFD0F25259D5C2200575D88 /* libg2o_types_icp.a in Frameworks */,
-				4EFD0F26259D5C2200575D88 /* libg2o_solver_csparse.a in Frameworks */,
-				4EFD0F27259D5C2200575D88 /* libg2o_solver_structure_only.a in Frameworks */,
-				4EFD0F28259D5C2200575D88 /* libg2o_solver_eigen.a in Frameworks */,
-				4EFD0F29259D5C2200575D88 /* libg2o_csparse_extension.a in Frameworks */,
-				4EFD0F2A259D5C2200575D88 /* libg2o_stuff.a in Frameworks */,
-				4EFD0F2B259D5C2200575D88 /* libg2o_types_sba.a in Frameworks */,
-				4EFD0F2C259D5C2200575D88 /* libg2o_types_slam2d_addons.a in Frameworks */,
-				4EFD0F2D259D5C2200575D88 /* libg2o_types_slam3d_addons.a in Frameworks */,
-				4EFD0F2E259D5C2200575D88 /* libg2o_types_slam3d.a in Frameworks */,
-				4EFD0F2F259D5C2200575D88 /* libmetis.a in Frameworks */,
-				4EFD0F30259D5C2200575D88 /* libg2o_types_sclam2d.a in Frameworks */,
-				4EFD0F31259D5C2200575D88 /* libg2o_types_sim3.a in Frameworks */,
-				4EFD0F32259D5C2200575D88 /* libgtsam.a in Frameworks */,
 				4E8B155426273A580037FC53 /* Zip in Frameworks */,
-				4EFD0EDD259D5BFC00575D88 /* libopencv_video.a in Frameworks */,
-				4EFD0EDE259D5BFC00575D88 /* libopencv_aruco.a in Frameworks */,
-				4EFD0EDF259D5BFC00575D88 /* libopencv_photo.a in Frameworks */,
-				4EFD0EE0259D5BFC00575D88 /* libopencv_line_descriptor.a in Frameworks */,
-				4EFD0EE1259D5BFC00575D88 /* libopencv_features2d.a in Frameworks */,
-				4EFD0EE2259D5BFC00575D88 /* libopencv_bgsegm.a in Frameworks */,
-				4EFD0EE3259D5BFC00575D88 /* libopencv_dnn_objdetect.a in Frameworks */,
-				4EFD0EE4259D5BFC00575D88 /* libopencv_objdetect.a in Frameworks */,
-				4EFD0EE5259D5BFC00575D88 /* libopencv_flann.a in Frameworks */,
-				4EFD0EE6259D5BFC00575D88 /* libopencv_stitching.a in Frameworks */,
-				4EFD0EE7259D5BFC00575D88 /* libopencv_imgproc.a in Frameworks */,
-				4EFD0EE8259D5BFC00575D88 /* libopencv_ml.a in Frameworks */,
-				4EFD0EE9259D5BFC00575D88 /* libopencv_rgbd.a in Frameworks */,
-				4EFD0EEA259D5BFC00575D88 /* libopencv_xphoto.a in Frameworks */,
-				4EFD0EEB259D5BFC00575D88 /* libopencv_phase_unwrapping.a in Frameworks */,
-				4EFD0EEC259D5BFC00575D88 /* libopencv_optflow.a in Frameworks */,
-				4EFD0EED259D5BFC00575D88 /* libopencv_imgcodecs.a in Frameworks */,
-				4EFD0EEE259D5BFC00575D88 /* libopencv_fuzzy.a in Frameworks */,
-				4EFD0EEF259D5BFC00575D88 /* libopencv_dpm.a in Frameworks */,
-				4EFD0EF0259D5BFC00575D88 /* libopencv_plot.a in Frameworks */,
-				4EFD0EF1259D5BFC00575D88 /* libopencv_dnn.a in Frameworks */,
-				4EFD0EF2259D5BFC00575D88 /* libopencv_saliency.a in Frameworks */,
-				4EFD0EF3259D5BFC00575D88 /* libopencv_videoio.a in Frameworks */,
-				4EFD0EF4259D5BFC00575D88 /* libopencv_hfs.a in Frameworks */,
-				4EFD0EF5259D5BFC00575D88 /* libopencv_tracking.a in Frameworks */,
-				4EFD0EF6259D5BFC00575D88 /* libopencv_text.a in Frameworks */,
-				4EFD0EF7259D5BFC00575D88 /* libopencv_face.a in Frameworks */,
-				4EFD0EF8259D5BFC00575D88 /* libopencv_bioinspired.a in Frameworks */,
-				4EFD0EF9259D5BFC00575D88 /* libopencv_highgui.a in Frameworks */,
-				4EFD0EFA259D5BFC00575D88 /* libopencv_core.a in Frameworks */,
-				4EFD0EFB259D5BFC00575D88 /* libopencv_shape.a in Frameworks */,
-				4EFD0EFC259D5BFC00575D88 /* libopencv_datasets.a in Frameworks */,
-				4EFD0EFD259D5BFC00575D88 /* libopencv_img_hash.a in Frameworks */,
-				4EFD0EFE259D5BFC00575D88 /* libopencv_surface_matching.a in Frameworks */,
-				4EFD0EFF259D5BFC00575D88 /* libopencv_calib3d.a in Frameworks */,
-				4EFD0F00259D5BFC00575D88 /* libopencv_structured_light.a in Frameworks */,
-				4EFD0F01259D5BFC00575D88 /* libopencv_reg.a in Frameworks */,
-				4EFD0F02259D5BFC00575D88 /* libopencv_xfeatures2d.a in Frameworks */,
-				4EFD0F03259D5BFC00575D88 /* libopencv_videostab.a in Frameworks */,
-				4EFD0F04259D5BFC00575D88 /* libopencv_ccalib.a in Frameworks */,
-				4EFD0F05259D5BFC00575D88 /* libopencv_ximgproc.a in Frameworks */,
-				4EFD0EB1259D5BE500575D88 /* librtabmap_utilite.a in Frameworks */,
-				4EFD0EB2259D5BE500575D88 /* librtabmap_core.a in Frameworks */,
+				4EFD0F36259D5C7300575D88 /* vtk.framework in Frameworks */,
 				4EFD0EAB259D581800575D88 /* OpenGLES.framework in Frameworks */,
-				4EFD0B36259D4DE900575D88 /* libboost_filesystem.a in Frameworks */,
-				4EFD0B37259D4DE900575D88 /* libboost_program_options.a in Frameworks */,
-				4EFD0B38259D4DE900575D88 /* libboost_regex.a in Frameworks */,
-				4EFD0B39259D4DE900575D88 /* libboost_iostreams.a in Frameworks */,
-				4EFD0B3A259D4DE900575D88 /* libboost_date_time.a in Frameworks */,
-				4EFD0B3B259D4DE900575D88 /* libboost_serialization.a in Frameworks */,
-				4EFD0B3C259D4DE900575D88 /* libboost_system.a in Frameworks */,
-				4EFD0B3D259D4DE900575D88 /* libboost_signals.a in Frameworks */,
-				4EFD0B3E259D4DE900575D88 /* libboost_thread.a in Frameworks */,
-				4EFD0B3F259D4DE900575D88 /* libboost_timer.a in Frameworks */,
-				4EFD0B40259D4DE900575D88 /* libboost_chrono.a in Frameworks */,
-				4EFD0B51259D4DF100575D88 /* libpcl_stereo.a in Frameworks */,
-				4EFD0B52259D4DF100575D88 /* libpcl_registration.a in Frameworks */,
-				4EFD0B53259D4DF100575D88 /* libpcl_features.a in Frameworks */,
-				4EFD0B54259D4DF100575D88 /* libpcl_filters.a in Frameworks */,
-				4EFD0B55259D4DF100575D88 /* libpcl_sample_consensus.a in Frameworks */,
-				4EFD0B56259D4DF100575D88 /* libpcl_kdtree.a in Frameworks */,
-				4EFD0B57259D4DF100575D88 /* libpcl_search.a in Frameworks */,
-				4EFD0B58259D4DF100575D88 /* libpcl_surface.a in Frameworks */,
-				4EFD0B59259D4DF100575D88 /* libpcl_keypoints.a in Frameworks */,
-				4EFD0B5A259D4DF100575D88 /* libpcl_recognition.a in Frameworks */,
-				4EFD0B5B259D4DF100575D88 /* libpcl_octree.a in Frameworks */,
-				4EFD0B5C259D4DF100575D88 /* libpcl_io.a in Frameworks */,
-				4EFD0B5D259D4DF100575D88 /* libpcl_common.a in Frameworks */,
-				4EFD0B5E259D4DF100575D88 /* libpcl_io_ply.a in Frameworks */,
-				4EFD0B5F259D4DF100575D88 /* libpcl_ml.a in Frameworks */,
-				4EFD0B60259D4DF100575D88 /* libpcl_segmentation.a in Frameworks */,
+				4E10D89C2DE409870015926D /* librtabmap_core.a in Frameworks */,
+				4E10D89D2DE409870015926D /* librtabmap_utilite.a in Frameworks */,
+				4E10D89E2DE409A60015926D /* libopencv_xfeatures2d.a in Frameworks */,
+				4E10D89F2DE409B20015926D /* libopencv_video.a in Frameworks */,
+				4E10D8A02DE409BB0015926D /* libopencv_tracking.a in Frameworks */,
+				4E10D8A12DE409C00015926D /* libopencv_optflow.a in Frameworks */,
+				4E10D8A22DE409C70015926D /* libopencv_imgproc.a in Frameworks */,
+				4E10D8A32DE409CA0015926D /* libopencv_features2d.a in Frameworks */,
+				4E10D8A42DE409CE0015926D /* libopencv_core.a in Frameworks */,
+				4E10D8A52DE409D10015926D /* libopencv_calib3d.a in Frameworks */,
+				4E10D8A62DE409D50015926D /* libopencv_aruco.a in Frameworks */,
+				4E10D8A72DE409F40015926D /* libopencv_flann.a in Frameworks */,
+				4E10D8A82DE40A1B0015926D /* libopencv_ximgproc.a in Frameworks */,
+				4E10D8A92DE40A3A0015926D /* libopencv_photo.a in Frameworks */,
+				4E10D8AA2DE40A6C0015926D /* libopencv_objdetect.a in Frameworks */,
+				4E10D8AB2DE40A8C0015926D /* libopencv_stitching.a in Frameworks */,
+				4E10D8AC2DE40AA20015926D /* libopencv_imgcodecs.a in Frameworks */,
+				4E3525672DE4C72D00F39D8F /* libamd.a in Frameworks */,
+				4E3525642DE4C71C00F39D8F /* libsuitesparseconfig.a in Frameworks */,
+				4E3525652DE4C71C00F39D8F /* libspqr.a in Frameworks */,
+				4E35255E2DE4C71000F39D8F /* libcxsparse.a in Frameworks */,
+				4E35255F2DE4C71000F39D8F /* libcolamd.a in Frameworks */,
+				4E3525602DE4C71000F39D8F /* libcamd.a in Frameworks */,
+				4E3525612DE4C71000F39D8F /* libccolamd.a in Frameworks */,
+				4E3525592DE4C6F600F39D8F /* libcholmod.a in Frameworks */,
+				4E10D8AD2DE40E990015926D /* libg2o_core.a in Frameworks */,
+				4E3525572DE4C6E500F39D8F /* libg2o_solver_cholmod.a in Frameworks */,
+				4E3525542DE4C6E500F39D8F /* libg2o_csparse_extension.a in Frameworks */,
+				4E3525552DE4C6E500F39D8F /* libg2o_solver_csparse.a in Frameworks */,
+				4E10D8AE2DE40E990015926D /* libg2o_solver_dense.a in Frameworks */,
+				4E10D8AF2DE40E990015926D /* libg2o_solver_eigen.a in Frameworks */,
+				4E10D8B02DE40E990015926D /* libg2o_solver_pcg.a in Frameworks */,
+				4E10D8B12DE40E990015926D /* libg2o_solver_slam2d_linear.a in Frameworks */,
+				4E10D8B22DE40E990015926D /* libg2o_solver_structure_only.a in Frameworks */,
+				4E10D8B32DE40E990015926D /* libg2o_stuff.a in Frameworks */,
+				4E10D8B42DE40E990015926D /* libg2o_types_data.a in Frameworks */,
+				4E10D8B52DE40E990015926D /* libg2o_types_icp.a in Frameworks */,
+				4E10D8B62DE40E990015926D /* libg2o_types_sba.a in Frameworks */,
+				4E10D8B72DE40E990015926D /* libg2o_types_sclam2d.a in Frameworks */,
+				4E10D8B82DE40E990015926D /* libg2o_types_sim3.a in Frameworks */,
+				4E10D8B92DE40E990015926D /* libg2o_types_slam2d_addons.a in Frameworks */,
+				4E10D8BA2DE40E990015926D /* libg2o_types_slam2d.a in Frameworks */,
+				4E10D8BB2DE40E990015926D /* libg2o_types_slam3d_addons.a in Frameworks */,
+				4E10D8BC2DE40E990015926D /* libg2o_types_slam3d.a in Frameworks */,
+				4E10D8BD2DE40EC70015926D /* libpcl_common.a in Frameworks */,
+				4E10D8BE2DE40EC70015926D /* libpcl_features.a in Frameworks */,
+				4E10D8BF2DE40EC70015926D /* libpcl_filters.a in Frameworks */,
+				4E10D8C02DE40EC70015926D /* libpcl_io_ply.a in Frameworks */,
+				4E10D8C12DE40EC70015926D /* libpcl_io.a in Frameworks */,
+				4E10D8C22DE40EC70015926D /* libpcl_kdtree.a in Frameworks */,
+				4E10D8C32DE40EC70015926D /* libpcl_keypoints.a in Frameworks */,
+				4E10D8C42DE40EC70015926D /* libpcl_ml.a in Frameworks */,
+				4E10D8C52DE40EC70015926D /* libpcl_octree.a in Frameworks */,
+				4E10D8C62DE40EC70015926D /* libpcl_registration.a in Frameworks */,
+				4E10D8C72DE40EC70015926D /* libpcl_sample_consensus.a in Frameworks */,
+				4E10D8C82DE40EC70015926D /* libpcl_search.a in Frameworks */,
+				4E10D8C92DE40EC70015926D /* libpcl_segmentation.a in Frameworks */,
+				4E10D8CA2DE40EC70015926D /* libpcl_stereo.a in Frameworks */,
+				4E10D8CB2DE40EC70015926D /* libpcl_surface.a in Frameworks */,
+				4E10D8CC2DE40EE30015926D /* libgtsam_unstable.a in Frameworks */,
+				4E10D8CD2DE40EE30015926D /* libgtsam.a in Frameworks */,
+				4E10D8CE2DE40EE30015926D /* libmetis-gtsam.a in Frameworks */,
+				4E10D8CF2DE40EF90015926D /* libboost_serialization.a in Frameworks */,
+				4E10D8D02DE40F060015926D /* libboost_timer.a in Frameworks */,
+				4E10D8D42DE40F2A0015926D /* libboost_thread.a in Frameworks */,
+				4E10D8D12DE40F190015926D /* liblas_c.a in Frameworks */,
+				4E10D8D22DE40F190015926D /* liblas.a in Frameworks */,
+				4E10D8D32DE40F190015926D /* liblaszip.a in Frameworks */,
+				4E10D8E12DE4101B0015926D /* liblibjpeg-turbo.a in Frameworks */,
+				4E10D8E22DE4101B0015926D /* liblibpng.a in Frameworks */,
 			);
 			runOnlyForDeploymentPostprocessing = 0;
 		};
@@ -509,6 +492,51 @@
 		4EFD0B2A259D4DE900575D88 /* Frameworks */ = {
 			isa = PBXGroup;
 			children = (
+				4E3525662DE4C72D00F39D8F /* libamd.a */,
+				4E3525632DE4C71C00F39D8F /* libspqr.a */,
+				4E3525622DE4C71C00F39D8F /* libsuitesparseconfig.a */,
+				4E35255C2DE4C71000F39D8F /* libcamd.a */,
+				4E35255D2DE4C71000F39D8F /* libccolamd.a */,
+				4E35255B2DE4C71000F39D8F /* libcolamd.a */,
+				4E35255A2DE4C71000F39D8F /* libcxsparse.a */,
+				4E3525582DE4C6F600F39D8F /* libcholmod.a */,
+				4E3525562DE4C6E500F39D8F /* libg2o_solver_cholmod.a */,
+				4E10D8DB2DE40FF60015926D /* libade.a */,
+				4E10D8DA2DE40FF60015926D /* libIlmImf.a */,
+				4E10D8DC2DE40FF60015926D /* liblibprotobuf.a */,
+				4E10D8D92DE40FF60015926D /* libzlib.a */,
+				4E10D8D72DE40FCA0015926D /* liblibpng.a */,
+				4E10D8D52DE40FBE0015926D /* liblibjpeg-turbo.a */,
+				4E10D8682DE408EE0015926D /* libboost_atomic.a */,
+				4E10D8732DE408EF0015926D /* libboost_charconv.a */,
+				4E10D8642DE408EE0015926D /* libboost_container.a */,
+				4E10D8762DE408EF0015926D /* libboost_contract.a */,
+				4E10D8652DE408EE0015926D /* libboost_graph.a */,
+				4E10D8662DE408EE0015926D /* libboost_json.a */,
+				4E10D86A2DE408EF0015926D /* libboost_locale.a */,
+				4E10D87B2DE408EF0015926D /* libboost_nowide.a */,
+				4E10D8792DE408EF0015926D /* libboost_prg_exec_monitor.a */,
+				4E10D8712DE408EF0015926D /* libboost_random.a */,
+				4E10D87A2DE408EF0015926D /* libboost_stacktrace_addr2line.a */,
+				4E10D8672DE408EE0015926D /* libboost_stacktrace_basic.a */,
+				4E10D8602DE408EE0015926D /* libboost_stacktrace_noop.a */,
+				4E10D87C2DE408EF0015926D /* libboost_test_exec_monitor.a */,
+				4E10D8742DE408EF0015926D /* libboost_type_erasure.a */,
+				4E10D8702DE408EF0015926D /* libboost_unit_test_framework.a */,
+				4E10D86F2DE408EF0015926D /* libboost_url.a */,
+				4E10D8722DE408EF0015926D /* libboost_wave.a */,
+				4E10D8612DE408EE0015926D /* libboost_wserialization.a */,
+				4E10D8752DE408EF0015926D /* liblas_c.a */,
+				4E10D86B2DE408EF0015926D /* liblz4.a */,
+				4E10D8622DE408EE0015926D /* libmetis-gtsam.a */,
+				4E10D86C2DE408EF0015926D /* libopencv_dnn_superres.a */,
+				4E10D8772DE408EF0015926D /* libopencv_gapi.a */,
+				4E10D8692DE408EE0015926D /* libopencv_intensity_transform.a */,
+				4E10D87D2DE408EF0015926D /* libopencv_mcc.a */,
+				4E10D86E2DE408EF0015926D /* libopencv_quality.a */,
+				4E10D86D2DE408EF0015926D /* libopencv_rapid.a */,
+				4E10D8782DE408EF0015926D /* libopencv_signal.a */,
+				4E10D8632DE408EE0015926D /* libopencv_wechat_qrcode.a */,
 				4ED73AD02D7FECF3007899C2 /* liblaszip.a */,
 				4EFAA9422CAE4E960055DA51 /* liblas.a */,
 				4EFD0F4D259D67D900575D88 /* liblibjpeg-turbo.a */,
@@ -889,7 +917,7 @@
 				ALWAYS_SEARCH_USER_PATHS = NO;
 				CLANG_ANALYZER_NONNULL = YES;
 				CLANG_ANALYZER_NUMBER_OBJECT_CONVERSION = YES_AGGRESSIVE;
-				CLANG_CXX_LANGUAGE_STANDARD = "gnu++14";
+				CLANG_CXX_LANGUAGE_STANDARD = "gnu++17";
 				CLANG_CXX_LIBRARY = "libc++";
 				CLANG_ENABLE_MODULES = YES;
 				CLANG_ENABLE_OBJC_ARC = YES;
@@ -920,7 +948,7 @@
 				DEBUG_INFORMATION_FORMAT = dwarf;
 				ENABLE_STRICT_OBJC_MSGSEND = YES;
 				ENABLE_TESTABILITY = YES;
-				GCC_C_LANGUAGE_STANDARD = gnu11;
+				GCC_C_LANGUAGE_STANDARD = gnu17;
 				GCC_DYNAMIC_NO_PIC = NO;
 				GCC_NO_COMMON_BLOCKS = YES;
 				GCC_OPTIMIZATION_LEVEL = 0;
@@ -950,7 +978,7 @@
 				ALWAYS_SEARCH_USER_PATHS = NO;
 				CLANG_ANALYZER_NONNULL = YES;
 				CLANG_ANALYZER_NUMBER_OBJECT_CONVERSION = YES_AGGRESSIVE;
-				CLANG_CXX_LANGUAGE_STANDARD = "gnu++14";
+				CLANG_CXX_LANGUAGE_STANDARD = "gnu++17";
 				CLANG_CXX_LIBRARY = "libc++";
 				CLANG_ENABLE_MODULES = YES;
 				CLANG_ENABLE_OBJC_ARC = YES;
@@ -981,7 +1009,7 @@
 				DEBUG_INFORMATION_FORMAT = "dwarf-with-dsym";
 				ENABLE_NS_ASSERTIONS = NO;
 				ENABLE_STRICT_OBJC_MSGSEND = YES;
-				GCC_C_LANGUAGE_STANDARD = gnu11;
+				GCC_C_LANGUAGE_STANDARD = gnu17;
 				GCC_NO_COMMON_BLOCKS = YES;
 				GCC_PREPROCESSOR_DEFINITIONS = "";
 				GCC_WARN_64_TO_32_BIT_CONVERSION = YES;
@@ -1035,8 +1063,9 @@
 					"$(inherited)",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
+					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib/opencv4/3rdparty",
 				);
-				MARKETING_VERSION = 0.21.11;
+				MARKETING_VERSION = 0.23.7;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1048,11 +1077,12 @@
 				SYSTEM_HEADER_SEARCH_PATHS = (
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.21\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
+					"\"$(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\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/opencv4\"",
 				);
 				TARGETED_DEVICE_FAMILY = "1,2";
 				VALIDATE_WORKSPACE = NO;
@@ -1093,8 +1123,9 @@
 					"$(inherited)",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
+					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib/opencv4/3rdparty",
 				);
-				MARKETING_VERSION = 0.21.11;
+				MARKETING_VERSION = 0.23.7;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
@@ -1107,11 +1138,12 @@
 				SYSTEM_HEADER_SEARCH_PATHS = (
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.21\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
+					"\"$(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\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/opencv4\"",
 				);
 				TARGETED_DEVICE_FAMILY = "1,2";
 				VALIDATE_WORKSPACE = NO;

app/ios/RTABMapApp/NativeWrapper.cpp

@@ -505,6 +505,13 @@ void setSmoothingNative(const void *object, bool enabled)
     else
         UERROR("object is null!");
 }
+void setDepthBleedingErrorNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setDepthBleedingError(value);
+    else
+        UERROR("object is null!");
+}
 void setAppendModeNative(const void *object, bool enabled)
 {
     if(object)

app/ios/RTABMapApp/NativeWrapper.hpp

@@ -101,6 +101,7 @@ void setGraphVisibleNative(const void *object, bool visible);
 void setGridVisibleNative(const void *object, bool visible);
 void setFullResolutionNative(const void *object, bool enabled);
 void setSmoothingNative(const void *object, bool enabled);
+void setDepthBleedingErrorNative(const void *object, float value);
 void setAppendModeNative(const void *object, bool enabled);
 void setUpstreamRelocalizationAccThrNative(const void *object, float value);
 void setMaxCloudDepthNative(const void *object, float value);

app/ios/RTABMapApp/RTABMap.swift

@@ -453,6 +453,9 @@ class RTABMap {
     func setSmoothing(enabled: Bool) {
         setSmoothingNative(native_rtabmap, enabled)
     }
+    func setDepthBleedingError(value: Float) {
+        setDepthBleedingErrorNative(native_rtabmap, value)
+    }
     func setAppendMode(enabled: Bool) {
         setAppendModeNative(native_rtabmap, enabled)
     }

app/ios/RTABMapApp/ViewController.swift

@@ -162,7 +162,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     func resetNoTouchTimer(_ showHud: Bool = false) {
         if(showHud)
         {
-            print("Show HUD")
             mMenuOpened = false
             mHudVisible = true
             setNeedsStatusBarAppearanceUpdate()
@@ -184,7 +183,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         }
         else if(mState != .STATE_WELCOME && mState != .STATE_CAMERA && presentedViewController as? UIAlertController == nil && !mMenuOpened)
         {
-            print("Hide HUD")
             self.mHudVisible = false
             self.setNeedsStatusBarAppearanceUpdate()
             self.updateState(state: self.mState)
@@ -840,13 +838,11 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             self.isPaused = true
             view?.enableSetNeedsDisplay = true
             self.view.setNeedsDisplay()
-            print("enableSetNeedsDisplay")
         }
         else
         {
             view?.enableSetNeedsDisplay = false
             self.isPaused = false
-            print("disableSetNeedsDisplay")
         }
         
         if !self.isPaused {
@@ -934,7 +930,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         ])
         
         // Measuring menu
-        print("measuringMode = \(measuringMode)")
         let measuringMenu = UIMenu(title: "Measuring...", image: UIImage(systemName: "ruler"), children: [
             UIAction(title: "Plane to Plane Mode", image: measuringMode == 0 ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), handler: { _ in
                 self.measuringMode = 0
@@ -1516,6 +1511,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         rtabmap!.setNodesFiltering(enabled: defaults.bool(forKey: "NodesFiltering"));
         rtabmap!.setFullResolution(enabled: defaults.bool(forKey: "HDMode"));
         rtabmap!.setSmoothing(enabled: defaults.bool(forKey: "Smoothing"));
+        rtabmap!.setDepthBleedingError(value: defaults.float(forKey: "DepthBleedingError"));
         rtabmap!.setAppendMode(enabled: defaults.bool(forKey: "AppendMode"));
         rtabmap!.setUpstreamRelocalizationAccThr(value: defaults.float(forKey: "UpstreamRelocalizationFilteringAccThr"));
         rtabmap!.setExportPointCloudFormat(format: defaults.string(forKey: "ExportPointCloudFormat")!);
@@ -1918,7 +1914,9 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             })
             
         }))
-
+	
+        let previousState = mState
+        
         updateState(state: .STATE_PROCESSING);
         
         present(alertView, animated: true, completion: {
@@ -1989,8 +1987,11 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                             self.updateState(state: .STATE_VISUALIZING)
                             
                             self.rtabmap!.postExportation(visualize: true)
-
-                            self.setGLCamera(type: 2)
+							
+                            if previousState != .STATE_VISUALIZING
+                            {
+                                self.setGLCamera(type: 2)
+                            }
 
                             if self.openedDatabasePath == nil
                             {

app/ios/RTABMapApp/install_deps.sh

@@ -2,7 +2,7 @@
 
 set -euxo pipefail
 
-# Tested on Apple Silicon Mac, with cmake 3.19.2.
+# Tested on Apple Silicon Mac, with cmake 4.0.2, XCode 16.3.
 
 mkdir -p Libraries
 cd Libraries
@@ -28,41 +28,65 @@ sysroot=iphoneos
 # Boost
 if [ ! -e $prefix/include/boost ]
 then
-if [ ! -e boost_1_59_0 ]
+if [ ! -e boost-1.88.0 ]
 then
   echo "wget boost..."
-  curl -L https://downloads.sourceforge.net/project/boost/boost/1.59.0/boost_1_59_0.tar.gz -o boost_1_59_0.tar.gz
-  tar -xzf boost_1_59_0.tar.gz
+  curl -L https://github.com/boostorg/boost/releases/download/boost-1.88.0/boost-1.88.0-cmake.tar.gz -o boost-1.88.0-cmake.tar.gz
+  tar -xzf boost-1.88.0-cmake.tar.gz
 fi
-cd boost_1_59_0
-curl -L https://gist.github.com/matlabbe/0bce8feeb73a499a76afbbcc5c687221/raw/489ff2869eccd6f8d03ffb9090ef839108762741/BoostConfig.cmake.in -o BoostConfig.cmake.in
-curl -L https://gist.github.com/matlabbe/0bce8feeb73a499a76afbbcc5c687221/raw/b07fe7d4e5dfe5f1d110c733e5cf660d79a26378/CMakeLists.txt -o CMakeLists.txt
+cd boost-1.88.0
 mkdir -p build
 cd build
-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 ..
+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 -DBOOST_INCOMPATIBLE_LIBRARIES="process;context;coroutine;fiber;fiber_numa;log_setup;log;cobalt" -DBOOST_IOSTREAMS_ENABLE_ZLIB=OFF -DBOOST_IOSTREAMS_ENABLE_BZIP2=OFF ..
 cmake --build . --config Release
 cmake --build . --config Release --target install
 cd $pwd
-#rm -r boost_1_59_0.tar.gz boost_1_59_0
+#rm -r boost-1.88.0-cmake.tar.gz boost-1.88.0
 fi
 
 # eigen
 if [ ! -e $prefix/include/eigen3 ]
 then
-if [ ! -e eigen-3.3.9 ]
+if [ ! -e eigen-3.4.0 ]
 then
   echo "wget eigen..."
-  curl -L https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.tar.gz -o 3.3.9.tar.gz
-  tar -xzf 3.3.9.tar.gz
+  curl -L https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.tar.gz -o 3.4.0.tar.gz
+  tar -xzf 3.4.0.tar.gz
 fi
-cd eigen-3.3.9
+cd eigen-3.4.0
 mkdir -p build
 cd build
 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 ..
 cmake --build . --config Release
 cmake --build . --config Release --target install
 cd $pwd
-#rm -r 3.3.9.tar.gz eigen-3.3.9
+#rm -r 3.4.0.tar.gz eigen-3.4.0
+fi
+
+# lz4 (required by flann)
+if [ ! -e $prefix/include/lz4.h ]
+then
+if [ ! -e lz4 ]
+then
+  echo "wget lz4..."
+  git clone https://github.com/lz4/lz4.git -b v1.10.0
+fi
+cd lz4
+if [ ! -e LZ4Config.cmake.in ]
+then
+  curl -L https://gist.githubusercontent.com/matlabbe/abd0242305c29495bbba26065269daf2/raw/ad0b1865c02e61449f58358fdc4ddbed3cb5fb87/LZ4Config.cmake.in -o LZ4Config.cmake.in
+fi
+if [ ! -e CMakeLists.txt ]
+then
+  curl -L https://gist.githubusercontent.com/matlabbe/abd0242305c29495bbba26065269daf2/raw/ad0b1865c02e61449f58358fdc4ddbed3cb5fb87/CMakeLists.txt -o CMakeLists.txt
+fi
+mkdir -p build
+cd build
+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 ..
+cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cd $pwd
+#rm -r lz4
 fi
 
 # FLANN
@@ -71,19 +95,17 @@ then
 if [ ! -e flann ]
 then
   echo "wget flann..."
-  git clone https://github.com/flann-lib/flann.git -b 1.8.4
+  git clone https://github.com/flann-lib/flann.git -b 1.9.2
 fi
 cd flann
-if [ ! -e flann_ios.patch ]
+if [ ! -e flann_ios_lz4.patch ]
 then
-  curl -L https://gist.githubusercontent.com/matlabbe/c858ba36fb85d5e44d8667dfb3543e12/raw/8fc40aa9bc3267604869444020476a49f14ab424/flann_ios.patch  -o flann_ios.patch
-  git apply flann_ios.patch
+  curl -L https://gist.githubusercontent.com/matlabbe/c858ba36fb85d5e44d8667dfb3543e12/raw/2586a356dec2b11440ec3c1bb113e709e1266d97/flann_ios_lz4.patch  -o flann_ios_lz4.patch
+  git apply flann_ios_lz4.patch
 fi
 mkdir -p build
 cd build
-# comment "add_subdirectory( test )" in top CMakeLists.txt
-# comment "add_subdirectory( doc )" in top CMakeLists.txt
-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 -DBUILD_PYTHON_BINDINGS=OFF -DBUILD_MATLAB_BINDINGS=OFF -DBUILD_C_BINDINGS=OFF  ..
+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 -DBUILD_PYTHON_BINDINGS=OFF -DBUILD_MATLAB_BINDINGS=OFF -DBUILD_C_BINDINGS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=OFF -DBUILD_DOC=OFF -DUSE_OPENMP=OFF -DLZ4_DIR=$prefix/lib/lz4  ..
 cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cd $pwd
@@ -95,17 +117,14 @@ if [ ! -e $prefix/include/gtsam ]
 then
 if [ ! -e gtsam ]
 then
-  git clone https://bitbucket.org/gtborg/gtsam.git
-  cd gtsam
-  git checkout fbb9d3bdda8b88df51896bc401bfd170573e66f5
-else
-  cd gtsam
+  git clone https://github.com/borglab/gtsam.git -b 4.2
 fi
+cd gtsam
 # patch
-if [ ! -e gtsam_GKlib_ios_fix.patch ]
+if [ ! -e gtsam_4_2_ios.patch ]
 then
-  curl -L https://gist.github.com/matlabbe/76d658dddb841b3355ae3a6e32850cd8/raw/7033cba1c89097b0c830651d7277c04dc92cbdd9/gtsam_GKlib_ios_fix.patch -o gtsam_GKlib_ios_fix.patch
-  git apply gtsam_GKlib_ios_fix.patch
+  curl -L https://gist.githubusercontent.com/matlabbe/76d658dddb841b3355ae3a6e32850cd8/raw/e7355348c2d536ec50f41effa775ed251ae4e045/gtsam_4_2_ios.patch -o gtsam_4_2_ios.patch
+  git apply gtsam_4_2_ios.patch
 fi
 mkdir -p build
 cd build
@@ -116,22 +135,36 @@ cd $pwd
 #rm -rf gtsam
 fi
 
+# suitesparse (dependency of g2o)
+if [ ! -e $prefix/include/suitesparse/SuiteSparse_config.h ]
+then
+if [ ! -e SuiteSparse ]
+then
+  git clone https://github.com/DrTimothyAldenDavis/SuiteSparse.git -b v7.6.1
+fi
+cd SuiteSparse
+mkdir -p build
+cd build
+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 -DSUITESPARSE_USE_OPENMP=OFF -DSUITESPARSE_ENABLE_PROJECTS="cholmod;cxsparse;spqr"  ..
+cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cd $pwd
+fi
+
 # g2o
 if [ ! -e $prefix/include/g2o ]
 then
 if [ ! -e g2o ]
 then
-  git clone https://github.com/RainerKuemmerle/g2o.git
-  cd g2o
-  git checkout a3f7706bdbb849b2808dc3e1b7aee189f63b498e
-else
-  cd g2o
+  git clone https://github.com/RainerKuemmerle/g2o.git -b 20241228_git
 fi
+cd g2o
 # patch
-if [ ! -e g2o_ios_fix.patch ]
+if [ ! -e g2o_20241228_ios.patch ]
 then
-  curl -L https://gist.github.com/matlabbe/b9ccfeae8f0744b275cab23510872680/raw/a58e06accba3976420d4b61341685c123193810e/g2o_ios_fix.patch -o g2o_ios_fix.patch
-  git apply g2o_ios_fix.patch
+ls
+  curl -L https://gist.githubusercontent.com/matlabbe/b9ccfeae8f0744b275cab23510872680/raw/6fe2ffe5ba8fba59171adbd2f38f9c3999c61f75/g2o_20241228_ios.patch -o g2o_20241228_ios.patch
+  git apply g2o_20241228_ios.patch
 fi
 mkdir -p build
 cd build
@@ -142,6 +175,18 @@ cd $pwd
 #rm -rf g2o
 fi
 
+# g2o's installed CMake config hard-codes the absolute build-time prefix of its
+# external dependencies (e.g. suitesparse) in INTERFACE_INCLUDE_DIRECTORIES. That
+# path doesn't exist when the prebuilt Libraries archive is unpacked on another
+# machine (CI), breaking find_package(g2o) with "includes non-existent path".
+# Rewrite those absolute paths to be relocatable (relative to the config file).
+# Run unconditionally (outside the build guard above) so it also fixes the prebuilt
+# archive in CI, where the g2o build step is skipped.
+find "$prefix/lib" -path '*/cmake/g2o/*.cmake' -print0 | while IFS= read -r -d '' f
+do
+  sed -i '' -E 's#[^";]*/Libraries#${CMAKE_CURRENT_LIST_DIR}/../../..#g' "$f"
+done
+
 # VTK
 if [ ! -e $prefix/lib/vtk.framework ]
 then
@@ -149,11 +194,10 @@ if [ ! -e VTK ]
 then
   git clone https://github.com/Kitware/VTK.git
   cd VTK
-  git checkout tags/v8.2.0
+  git checkout tags/v9.5.0.rc1
 else
   cd VTK
 fi
-git cherry-pick bf3ae8072df2393c7270509bae41be0776826346
 mkdir -p build
 cd build
 cmake -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_FRAMEWORK_INSTALL_PREFIX=$prefix/lib -DIOS_DEVICE_ARCHITECTURES="arm64" -DIOS_SIMULATOR_ARCHITECTURES="" -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_IOS_BUILD=ON -DModule_vtkFiltersModeling=ON ..
@@ -163,26 +207,28 @@ cd $pwd
 #rm -rf VTK
 fi
 
+
+
 # PCL
-if [ ! -e $prefix/include/pcl-1.11 ]
+if [ ! -e $prefix/include/pcl-1.15 ]
 then
 if [ ! -e pcl ]
 then
   git clone https://github.com/PointCloudLibrary/pcl.git
   cd pcl
-  git checkout tags/pcl-1.11.1
+  git checkout tags/pcl-1.15.0
 else
   cd pcl
 fi
 # patch
-if [ ! -e pcl_1_11_1_vtk_ios_support.patch ]
+if [ ! -e pcl_1_15_0_ios.patch ]
 then
-  curl -L https://gist.github.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/6869cf26211ab15492599e557b0e729b23b2c119/pcl_1_11_1_vtk_ios_support.patch -o pcl_1_11_1_vtk_ios_support.patch
-  git apply pcl_1_11_1_vtk_ios_support.patch
+  curl -L https://gist.githubusercontent.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/7231688d7fb9e86df72ca7c5f355d6b9727205d5/pcl_1_15_0_ios.patch -o pcl_1_15_0_ios.patch
+  git apply pcl_1_15_0_ios.patch
 fi
 mkdir -p build
 cd build
-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 -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF -DPCL_ENABLE_SSE=OFF -DCMAKE_FIND_ROOT_PATH=$prefix ..
+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 -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_recognition=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_OPENMP=OFF -DWITH_VTK=ON -DPCL_FLANN_REQUIRED_TYPE=STATIC -DPCL_SHARED_LIBS=OFF -DPCL_ENABLE_SSE=OFF -DCMAKE_FIND_ROOT_PATH=$prefix ..
 cmake --build . --config Release
 cmake --build . --config Release --target install
 cd $pwd
@@ -190,32 +236,21 @@ cd $pwd
 fi
 
 # OpenCV
-if [ ! -e $prefix/include/opencv2 ]
+if [ ! -e $prefix/include/opencv4 ]
 then
 if [ ! -e opencv_contrib ]
 then
-  git clone https://github.com/opencv/opencv_contrib.git
-  cd opencv_contrib
-  git checkout tags/3.4.2
+  git clone https://github.com/opencv/opencv_contrib.git -b 4.11.0
 fi
 cd $pwd
 if [ ! -e opencv ]
 then
-  git clone https://github.com/opencv/opencv.git
-  cd opencv
-  git checkout tags/3.4.2
-else
-  cd opencv
-fi
-if [ ! -e opencv_ios.patch ]
-then
-  curl -L https://gist.githubusercontent.com/matlabbe/fdc3ab4854f3a68fbde7277f543b4e5b/raw/f340839c09165056d3845645df24b76507542fd2/opencv_ios.patch -o opencv_ios.patch
-  git apply opencv_ios.patch
+  git clone https://github.com/opencv/opencv.git -b 4.11.0
 fi
+cd opencv
 mkdir -p build
 cd build
-# add "add_definitions(-DPNG_ARM_NEON_OPT=0)" in 3rdparty/libpng/CMakeLists.txt
-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 -DOPENCV_EXTRA_MODULES_PATH=$prefix/opencv_contrib/modules -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_CUDA=OFF -DBUILD_opencv_apps=OFF -DBUILD_opencv_xobjdetect=OFF -DBUILD_opencv_stereo=OFF ..
+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 -DOPENCV_EXTRA_MODULES_PATH=$prefix/opencv_contrib/modules -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_CUDA=OFF -DWITH_WEBP=OFF -DWITH_OPENEXR=OFF -DBUILD_opencv_apps=OFF -DBUILD_opencv_xobjdetect=OFF -DBUILD_opencv_stereo=OFF -DOPENCV_ENABLE_NONFREE=ON ..
 cmake --build . --config Release
 cmake --build . --config Release --target install
 cd $pwd
@@ -228,12 +263,10 @@ if [ ! -e $prefix/include/laszip ]
 then
 if [ ! -e LASzip ]
 then
-  git clone https://github.com/LASzip/LASzip.git
-  cd LASzip
-  git checkout 2.0.1
-else
-  cd LASzip
+  git clone https://github.com/LASzip/LASzip.git -b 2.0.1
 fi
+cd LASzip
+sed -i '' 's/cmake_minimum_required(VERSION 2.6.0)/cmake_minimum_required(VERSION 3.5)/g' CMakeLists.txt
 sed -i '' 's/add_subdirectory(tools)/#add_subdirectory(tools)/g' CMakeLists.txt
 mkdir -p build
 cd build
@@ -251,6 +284,7 @@ then
   git clone https://github.com/libLAS/libLAS.git
 fi
 cd libLAS
+sed -i '' 's/cmake_minimum_required(VERSION 2.8.11)/cmake_minimum_required(VERSION 3.5)/g' CMakeLists.txt
 sed -i '' 's/SHARED/STATIC/g' src/CMakeLists.txt
 mkdir -p build
 cd build
@@ -266,6 +300,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=OFF  -DBUILD_EXAMPLES=OFF -DWITH_LIBLAS=ON ../../../../../..
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=ON  -DBUILD_EXAMPLES=OFF -DWITH_LIBLAS=ON -DWITH_OPENGV=OFF ../../../../../..
 cmake --build . --config Release
 cmake --build . --config Release --target install

app/ios/Settings.bundle/Mapping.plist

@@ -58,7 +58,7 @@
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
 			<key>FooterText</key>
-			<string>Choosing between high and low depth confidence depends on the application. If precise geometry is critical, prioritize high depth confidence even if it means fewer points. If you need a dense point cloud for analysis, even in areas with weak LiDAR signal, a lower depth confidence might be suitable at the cost of highly interpolated points (that could give a wavy look of some surfaces). Important: the points with too low confidence are not saved in the db file, so we cannot recover or filter these points in post-processing.</string>
+			<string>Choosing between high and low depth confidence depends on the application. If precise geometry is critical, prioritize high depth confidence even if it means fewer points. If you need a dense point cloud for analysis, even in areas with weak LiDAR signal, a lower depth confidence might be suitable at the cost of highly interpolated points (that could give a wavy look of some surfaces). Note: it is possible to change confidence level in post-processing.</string>
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
@@ -86,7 +86,7 @@
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
 			<key>FooterText</key>
-			<string>Smooth the point clouds.</string>
+			<string>Smooth the point clouds. Note that this can be changed in post-processing.</string>
 		</dict>
 		<dict>
 			<key>Type</key>
@@ -98,6 +98,44 @@
 			<key>DefaultValue</key>
 			<false/>
 		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Filter interpolated depth pixels between foreground and background objects. Note that values can be changed in post-processing.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Depth Bleeding Filter Error</string>
+			<key>Key</key>
+			<string>DepthBleedingError</string>
+			<key>DefaultValue</key>
+			<string>0</string>
+			<key>Titles</key>
+			<array>
+				<string>Disabled</string>
+				<string>2.5 cm</string>
+				<string>5 cm</string>
+				<string>10 cm</string>
+				<string>15 cm</string>
+				<string>20 cm</string>
+				<string>25 cm</string>
+				<string>30 cm</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<string>0</string>
+				<string>0.025</string>
+				<string>0.05</string>
+				<string>0.10</string>
+				<string>0.15</string>
+				<string>0.20</string>
+				<string>0.25</string>
+				<string>0.30</string>
+			</array>
+		</dict>
 		<dict>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
@@ -590,7 +628,7 @@
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
 			<key>FooterText</key>
-			<string>BRIEF features are fast to compute but are not rotation invariant like FREAK. Warning: Changing feature type will automatically reset the map!</string>
+			<string>BRIEF, FREAK and ORB are binary features, which are fast to compute and use less memory. SURF and SIFT are high dimensional float descriptors (rotation/scale/shear-invariant), so they use more memory and CPU but they are significantly better to detect loop closures in large environments. Warning: Changing feature type will automatically reset the map!</string>
 		</dict>
 		<dict>
 			<key>Type</key>
@@ -603,13 +641,21 @@
 			<string>6</string>
 			<key>Titles</key>
 			<array>
-				<string>BRIEF</string>
-				<string>FREAK</string>
+				<string>SURF</string>
+				<string>SIFT</string>
+				<string>GFTT-FREAK</string>
+				<string>GFTT-BRIEF</string>
+				<string>GFTT-ORB</string>
+				<string>ORB-OCTREE</string>
 			</array>
 			<key>Values</key>
 			<array>
-				<string>6</string>
+				<string>0</string>
+				<string>1</string>
 				<string>5</string>
+				<string>6</string>
+				<string>8</string>
+				<string>10</string>
 			</array>
 		</dict>
 		<dict>

app/ios/Settings.bundle/Root.plist

@@ -494,7 +494,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<string>0.21.11</string>
+			<string>0.22.0</string>
 			<key>Key</key>
 			<string>Version</string>
 			<key>Title</key>

app/src/CMakeLists.txt

@@ -24,15 +24,23 @@ IF(WIN32)
   ELSE( MINGW )
     SET(SRC_FILES ${SRC_FILES} ${PROJECT_NAME}.rc)
   ENDIF( MINGW )
-ENDIF(WIN32)
-
-IF(BUILD_AS_BUNDLE)
-  ADD_DEFINITIONS("-DBUILD_AS_BUNDLE")
+ENDIF(WIN32)
+
+IF(BUILD_AS_BUNDLE)
+  ADD_DEFINITIONS("-DBUILD_AS_BUNDLE")
 ENDIF()
 
 # Add binary
 IF(APPLE AND BUILD_AS_BUNDLE)
   ADD_EXECUTABLE(rtabmap_app MACOSX_BUNDLE ${SRC_FILES})
+  # Custom Info.plist providing NSCameraUsageDescription, required by macOS to
+  # grant the app access to USB/UVC cameras (e.g. Orbbec, RealSense). A bundle
+  # identifier is also set: macOS ties the camera (TCC) permission to it, so it
+  # must be non-empty for the permission to be granted/persisted.
+  SET_TARGET_PROPERTIES(rtabmap_app PROPERTIES
+    MACOSX_BUNDLE_INFO_PLIST ${CMAKE_CURRENT_SOURCE_DIR}/Info.plist.in
+    MACOSX_BUNDLE_GUI_IDENTIFIER "com.introlab.rtabmap"
+    MACOSX_BUNDLE_BUNDLE_NAME "${CMAKE_BUNDLE_NAME}")
 ELSEIF(WIN32 AND BUILD_AS_BUNDLE)
   ADD_EXECUTABLE(rtabmap_app WIN32 ${SRC_FILES})
 ELSE()
@@ -70,13 +78,13 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   SET(thirdparty_dest_dir bin)
  
   IF(APPLE)
-    SET(plugin_dest_dir MacOS/plugins)
-    IF(Qt6_FOUND)
-       SET(plugin_dest_dir PlugIns)
+    SET(plugin_dest_dir MacOS/plugins)
+    IF(Qt6_FOUND)
+       SET(plugin_dest_dir PlugIns)
     ENDIF()
     SET(qtconf_dest_dir Resources)
     SET(thirdparty_dest_dir MacOS)
-    SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/MacOS/${CMAKE_BUNDLE_NAME}")
+    SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/MacOS/${CMAKE_BUNDLE_NAME}")
   ENDIF(APPLE)
 
   IF(OpenNI2_FOUND)
@@ -95,47 +103,225 @@ 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)
-	# Install needed depthengine_2_0.dll
+	# Install needed depthengine_2_0.dll. Its location varies by K4A SDK version:
+	# v1.4.1 ships it under sdk/windows-desktop/amd64/release/bin, or fallback in tools/.
     IF(WIN32)
 		file(TO_CMAKE_PATH "$ENV{K4A_ROOT_DIR}" ENV_K4A_ROOT_DIR)
-		INSTALL(FILES "${ENV_K4A_ROOT_DIR}/tools/depthengine_2_0.dll" 
-          DESTINATION ${thirdparty_dest_dir} 
-          COMPONENT runtime)
+		set(DEPTHENGINE_DLL "")
+		foreach(_depthengine_dir
+			"${ENV_K4A_ROOT_DIR}/sdk/windows-desktop/amd64/release/bin"
+			"${ENV_K4A_ROOT_DIR}/tools")
+			if(EXISTS "${_depthengine_dir}/depthengine_2_0.dll")
+				set(DEPTHENGINE_DLL "${_depthengine_dir}/depthengine_2_0.dll")
+				break()
+			endif()
+		endforeach()
+		IF(DEPTHENGINE_DLL)
+			INSTALL(FILES "${DEPTHENGINE_DLL}"
+              DESTINATION ${thirdparty_dest_dir}
+              COMPONENT runtime)
+		ELSE()
+			MESSAGE(WARNING "depthengine_2_0.dll not found under $ENV{K4A_ROOT_DIR} (sdk/windows-desktop/amd64/release/bin or tools); Kinect for Azure depth may not work at runtime.")
+		ENDIF()
 	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(STATUS "OrbbecSDK_BIN_DIR=${OrbbecSDK_BIN_DIR}")
+        INSTALL(DIRECTORY "${OrbbecSDK_BIN_DIR}/extensions" 
+          DESTINATION ${thirdparty_dest_dir} 
+          COMPONENT runtime 
+          FILES_MATCHING
+          PATTERN "*.lib" EXCLUDE
+          PATTERN "*")
+    ELSEIF(APPLE)
+        # OrbbecSDK loads its extensions (frame processor, filters like
+        # FrameUnpacker) via dlopen relative to libOrbbecSDK. fixup_bundle puts
+        # libOrbbecSDK in Contents/Frameworks, so the extensions must sit next to
+        # it in Frameworks/extensions (they reference @rpath/libOrbbecSDK, which
+        # resolves via the app executable's @executable_path/../Frameworks rpath).
+        # Without this the camera is detected but frame processing fails.
+        get_filename_component(OrbbecSDK_LIB_DIR "${OrbbecSDK_DIR}/../.." ABSOLUTE)
+        INSTALL(DIRECTORY "${OrbbecSDK_LIB_DIR}/extensions"
+          DESTINATION Frameworks
+          COMPONENT runtime)
+        IF(EXISTS "${OrbbecSDK_LIB_DIR}/OrbbecSDKConfig.xml")
+          INSTALL(FILES "${OrbbecSDK_LIB_DIR}/OrbbecSDKConfig.xml"
+            DESTINATION Frameworks
+            COMPONENT runtime)
+        ENDIF()
+    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)
 
-  IF(Qt6_FOUND)
+		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 AND APPLE)
+    # On macOS we do NOT use qt_deploy_runtime_dependencies (macdeployqt):
+    # macdeployqt cannot deploy VTK 9.6+, whose libraries reference each other
+    # via @rpath relative to "@loader_path/../lib". Instead we install the Qt plugins
+    # manually and let fixup_bundle() below deploy all libraries.
+    IF(TARGET Qt6::QCocoaIntegrationPlugin)
+      get_target_property(_qt_plugin_loc Qt6::QCocoaIntegrationPlugin LOCATION)
+      get_filename_component(_qt_plugins_dir "${_qt_plugin_loc}" DIRECTORY) # .../plugins/platforms
+      get_filename_component(_qt_plugins_dir "${_qt_plugins_dir}" DIRECTORY) # .../plugins
+      FOREACH(_qt_plugin_type platforms styles imageformats iconengines)
+        IF(EXISTS "${_qt_plugins_dir}/${_qt_plugin_type}")
+          # Homebrew Qt plugins are symlinks into the Cellar; resolve them to the
+          # real files so they aren't installed as dangling symlinks (which would
+          # break fixup_bundle and codesign).
+          FILE(GLOB _qt_plugin_files "${_qt_plugins_dir}/${_qt_plugin_type}/*${CMAKE_SHARED_LIBRARY_SUFFIX}")
+          SET(_qt_plugin_real "")
+          FOREACH(_qt_plugin_file ${_qt_plugin_files})
+            GET_FILENAME_COMPONENT(_qt_plugin_file "${_qt_plugin_file}" REALPATH)
+            LIST(APPEND _qt_plugin_real "${_qt_plugin_file}")
+          ENDFOREACH()
+          IF(_qt_plugin_real)
+            INSTALL(FILES ${_qt_plugin_real}
+                    DESTINATION ${plugin_dest_dir}/${_qt_plugin_type}
+                    COMPONENT runtime)
+          ENDIF()
+        ENDIF()
+      ENDFOREACH()
+    ELSE()
+      MESSAGE(WARNING "Qt6::QCocoaIntegrationPlugin target not found: Qt plugins may be missing from the bundle")
+    ENDIF()
+    # qt.conf so Qt finds the bundled plugins (path is relative to Contents/)
+    SET(QT_CONF_FILE [Paths]\nPlugins=${plugin_dest_dir})
+    INSTALL(CODE "
+      file(WRITE \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${qtconf_dest_dir}/qt.conf\" \"${QT_CONF_FILE}\")
+      " COMPONENT runtime)
+  ELSEIF(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
 	set(deploy_script "${CMAKE_CURRENT_BINARY_DIR}/deploy_app$<CONFIG>.cmake")
 	file(GENERATE OUTPUT ${deploy_script} CONTENT "
 	# Including the file pointed to by QT_DEPLOY_SUPPORT ensures the generated
 	# deployment script has access to qt_deploy_runtime_dependencies()
-	include(\"${QT_DEPLOY_SUPPORT}\")
+	include(\"${QT_DEPLOY_SUPPORT}\")
 	qt_deploy_runtime_dependencies(
 		EXECUTABLE \"${APPS}\"
 		PLUGINS_DIR ${plugin_dest_dir}
@@ -215,14 +401,17 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
       INSTALL(CODE "
         file(WRITE \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${qtconf_dest_dir}/qt.conf\" \"${QT_CONF_FILE}\")
         " COMPONENT runtime)
-  ENDIF()
+  ENDIF()
 
   # directories to look for dependencies
-  SET(DIRS "${QT_LIBRARY_DIRS}" "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/lib")
+  # CMAKE_LIBRARY_OUTPUT_DIRECTORY is where rtabmap's own libraries (e.g.
+  # librtabmap_core) are built; fixup_bundle needs it to resolve the app's
+  # @rpath references to them (macdeployqt used to handle this via rpath).
+  SET(DIRS "${QT_LIBRARY_DIRS}" "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}" "\$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 +419,65 @@ 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}\")
+
+    # OrbbecSDK loads its filter/frame-processor extensions via dlopen, and they
+    # link @rpath/libOrbbecSDK. Feed them to fixup_bundle so their reference is
+    # rewritten to the embedded libOrbbecSDK
+    file(GLOB ORBBEC_EXT_LIBS
+        \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/Frameworks/extensions/filters/*${CMAKE_SHARED_LIBRARY_SUFFIX}\"
+        \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/Frameworks/extensions/frameprocessor/*${CMAKE_SHARED_LIBRARY_SUFFIX}\")
+    if(ORBBEC_EXT_LIBS)
+        list(APPEND ALL_LIBS \${ORBBEC_EXT_LIBS})
+    endif()
+
+    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)
+    # nvcuvid.dll (NVDEC) and nvEncodeAPI64.dll (NVENC), and the CUDA driver API
+    # nvcuda.dll, ship with the NVIDIA GPU driver (System32), not the CUDA toolkit,
+    # so they are absent on driver-less CI runners and must never be bundled (they
+    # are resolved at runtime from the end user's driver). Mark them 'system' so
+    # fixup_bundle skips them instead of failing to resolve them.
+    function(gp_resolved_file_type_override resolved_file type_var)
+      if(resolved_file MATCHES \"(nvcuvid|nvEncodeAPI64|nvcuda)\")
+        set(\${type_var} \"system\" PARENT_SCOPE)
+      endif()
+    endfunction()
+    fixup_bundle(\"${APPS}\" \"\${ALL_LIBS}\" \"${DIRS}\")
+  " COMPONENT runtime)
+
+  IF(APPLE)
+    # Re-sign ad-hoc AFTER fixup_bundle has finished all its install_name_tool
+    # edits.
+    install(CODE "
+      set(_contents \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}\")
+      get_filename_component(_app \"\${_contents}\" DIRECTORY)
+      file(GLOB_RECURSE _nested \"\${_contents}/*${CMAKE_SHARED_LIBRARY_SUFFIX}\")
+      foreach(_f \${_nested})
+        execute_process(COMMAND codesign --force --sign - --timestamp=none \"\${_f}\")
+      endforeach()
+      file(GLOB _frameworks \"\${_contents}/Frameworks/*.framework\")
+      foreach(_f \${_frameworks})
+        execute_process(COMMAND codesign --force --sign - --timestamp=none \"\${_f}\")
+      endforeach()
+      message(STATUS \"Ad-hoc re-signing bundle: \${_app}\")
+      execute_process(COMMAND codesign --force --sign - --timestamp=none \"\${_app}\")
+    " COMPONENT runtime)
+  ENDIF(APPLE)
 
 ENDIF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   

app/src/Info.plist.in

@@ -0,0 +1,40 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple Computer//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>NSPrincipalClass</key>
+	<string>NSApplication</string>
+	<key>NSHighResolutionCapable</key>
+	<string>True</string>
+	<key>CFBundleDevelopmentRegion</key>
+	<string>English</string>
+	<key>CFBundleExecutable</key>
+	<string>${MACOSX_BUNDLE_EXECUTABLE_NAME}</string>
+	<key>CFBundleGetInfoString</key>
+	<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>
+	<string>${MACOSX_BUNDLE_LONG_VERSION_STRING}</string>
+	<key>CFBundleName</key>
+	<string>${MACOSX_BUNDLE_BUNDLE_NAME}</string>
+	<key>CFBundlePackageType</key>
+	<string>APPL</string>
+	<key>CFBundleShortVersionString</key>
+	<string>${MACOSX_BUNDLE_SHORT_VERSION_STRING}</string>
+	<key>CFBundleSignature</key>
+	<string>????</string>
+	<key>CFBundleVersion</key>
+	<string>${MACOSX_BUNDLE_BUNDLE_VERSION}</string>
+	<key>CSResourcesFileMapped</key>
+	<true/>
+	<key>NSHumanReadableCopyright</key>
+	<string>${MACOSX_BUNDLE_COPYRIGHT}</string>
+	<key>NSCameraUsageDescription</key>
+	<string>RTAB-Map needs access to the camera to capture images from connected RGB-D/stereo cameras (e.g. Orbbec, RealSense).</string>
+</dict>
+</plist>

app/src/main.cpp

@@ -52,6 +52,10 @@ int main(int argc, char* argv[])
 	ULogger::setLevel(ULogger::kWarning);
 
 #ifdef WIN32
+	// STA (single-threaded apartment) is required for the native file dialogs / File Explorer
+	// (see commit d75cc04, "Fixed File Explorer hanging (Qt 5.12)"). Do NOT switch to MTA
+	// (CoInitializeEx COINIT_MULTITHREADED): it deadlocks the
+	// native file dialogs.
 	CoInitialize(nullptr);
 #endif
 
@@ -60,6 +64,12 @@ int main(int argc, char* argv[])
     QSurfaceFormat::setDefaultFormat(QVTKRenderWidget::defaultFormat());
 #endif
 
+	// Recommended by VTK when using QVTKOpenGLNativeWidget (a QOpenGLWidget): let all VTK
+	// render widgets share a single OpenGL context, which is needed for correct rendering
+	// when render widgets live in / move across multiple top-level windows. Must be set
+	// before QApplication is constructed.
+	QApplication::setAttribute(Qt::AA_ShareOpenGLContexts);
+
 	/* Create tasks */
 	QApplication * app = new QApplication(argc, argv);
 	app->setStyleSheet("QMessageBox { messagebox-text-interaction-flags: 5; }"); // selectable message box

archive/2022-IlluminationInvariant/results_common.m

@@ -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

archive/2022-IlluminationInvariant/scripts/export_stats.sh

@@ -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
 

bundle_windows_deps.bat

@@ -0,0 +1,418 @@
+@echo off
+setlocal enabledelayedexpansion
+
+:: --- CONFIGURATION ---
+set "VCPKG_ROOT=%~dp0vcpkg"
+set "EXPORT_DIR=%~dp0vcpkg_binaries"
+:: All deps downloaded/built from source (git clones + the pinned CMake) live here.
+set "SRC_DIR=%~dp0vcpkg_deps_from_source"
+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 ^
+    --x-feature=libfreenect2-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%" --output=%TARGET_NAME% --triplet=%TRIPLET%  || 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
+:: Everything cloned/downloaded below goes into %SRC_DIR% to keep the repo root clean.
+if not exist "%SRC_DIR%" mkdir "%SRC_DIR%"
+cd /d "%SRC_DIR%"
+
+:: Kinect for Windows SDK v2 (k4w2): rtabmap's FindKinectSDK2.cmake expects the
+:: Microsoft SDK layout (KINECTSDK20_DIR\inc and \Lib\x64), but vcpkg's kinectsdk2
+:: port installs vcpkg-style (include\ and lib\). Mirror the bundled headers/lib into
+:: the expected layout so it is found with KINECTSDK20_DIR pointing at the export.
+if "%KINECTSDK20_DIR%"=="" (
+    echo Error: KINECTSDK20_DIR environment variable is not set! Install Kinect for Windows SDK v2: https://www.microsoft.com/en-us/download/details.aspx?id=44561
+    pause
+    exit /b 1
+)
+echo [+] Arranging Kinect SDK v2 layout for FindKinectSDK2...
+set "K4W2_ROOT=%FINAL_EXPORT_PATH%\installed\%TRIPLET%"
+robocopy "%K4W2_ROOT%\include" "%K4W2_ROOT%\inc" Kinect*.h Nui*.h /XO >nul
+robocopy "%K4W2_ROOT%\lib" "%K4W2_ROOT%\Lib\x64" Kinect20*.lib /XO >nul
+robocopy "%KINECTSDK20_DIR%\bin" "%K4W2_ROOT%\bin" Kinect20.dll /XO >nul
+:: Robocopy exit codes under 8 mean successful copies/no changes; 8+ is a failure.
+if !errorlevel! GEQ 8 (
+    echo Error: Robocopy failed with exit code !errorlevel!
+    pause
+    exit /b !errorlevel!
+)
+
+:: orbbec SDK2
+echo [+] Building OrbbecSDK_v2...
+if not exist OrbbecSDK_v2 (
+    echo [+] Downloading and applying patch...
+    git clone https://github.com/orbbec/OrbbecSDK_v2.git
+    cd OrbbecSDK_v2
+    git checkout v2.8.7
+    :: Install the CMake package config under lib/cmake/<pkg> so find_package(OrbbecSDK) works
+    git apply "%~dp0patches\orbbecsdk_2.8.7.patch"
+    cd ..
+)
+cd OrbbecSDK_v2
+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 ^
+  -DOB_BUILD_DOCS=OFF ^
+  -DOB_BUILD_EXAMPLES=OFF ^
+  -DOB_BUILD_TOOLS=OFF ^
+  -DOB_INSTALL_EXAMPLES_SOURCE=OFF || exit /b !errorlevel!
+cmake --build build --config Release --target install || exit /b !errorlevel!
+cd ..
+
+if "%K4A_ROOT_DIR%"=="" (
+    echo Error: K4A_ROOT_DIR environment variable is not set! Install K4A: https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/develop/docs/usage.md
+    pause
+    exit /b 1
+)
+echo Copying Kinect For Azure components...
+robocopy "%K4A_ROOT_DIR%\sdk" "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\sdk" /E /XO
+:: Robocopy exit codes under 8 mean successful copies/no changes. 
+:: 8 or higher means there was a failure.
+if !errorlevel! GEQ 8 (
+    echo Error: Robocopy failed with exit code !errorlevel!
+    pause
+    exit /b !errorlevel!
+)
+
+:: 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 "%~dp0patches\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 "%~dp0patches\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 "%~dp0patches\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 "%~dp0patches\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 ..
+
+:: libfreenect2
+echo [+] Building libfreenect2...
+if not exist libfreenect2 (
+    echo [+] Downloading and applying patch...
+    git clone https://github.com/OpenKinect/libfreenect2.git
+    cd libfreenect2
+    :: Aug 6, 2021
+    git checkout v0.2.1
+    :: Patch makes freenect2Config.cmake relocatable so the prebuilt bundle works
+    :: after being moved/unzipped, drops cudaDeviceProp::clockRate/computeMode
+    :: removed in CUDA 13 from the CUDA build, and renames a CL_ICDL_VERSION local
+    :: that collides with the OpenCL 3.0 macro.
+    git apply "%~dp0patches\libfreenect2_v0.2.1.patch"
+    cd ..
+)
+cd libfreenect2
+:: libfreenect2's FindLibUSB.cmake locates libusb via pkg-config. It declares an
+:: ancient cmake_minimum_required (2.8.12), which makes FindPkgConfig default
+:: PKG_CONFIG_USE_CMAKE_PREFIX_PATH=OFF, so it ignores the vcpkg CMAKE_PREFIX_PATH.
+:: Force it ON so the bundle's lib/pkgconfig (libusb-1.0.pc, libturbojpeg.pc) is
+:: searched, use the vcpkg-bundled pkgconf, and also set PKG_CONFIG_PATH as a belt.
+set "PKG_CONFIG_PATH=%FINAL_EXPORT_PATH%\installed\%TRIPLET%\lib\pkgconfig"
+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"  ^
+  -DPKG_CONFIG_EXECUTABLE="%FINAL_EXPORT_PATH%\installed\%TRIPLET%\tools\pkgconf\pkgconf.exe" ^
+  -DPKG_CONFIG_USE_CMAKE_PREFIX_PATH=ON ^
+  -DCMAKE_BUILD_TYPE=Release ^
+  -DENABLE_CUDA=OFF ^
+  -DENABLE_OPENCL=OFF ^
+  -DBUILD_EXAMPLES=OFF ^
+  -DCMAKE_INSTALL_PREFIX="%FINAL_EXPORT_PATH%\installed\%TRIPLET%" ^
+  -DBUILD_SHARED_LIBS=ON || exit /b !errorlevel!
+cmake --build build --config Release --target install || exit /b !errorlevel!
+cd ..
+
+:: depthai
+echo [+] Building depthai...
+if not exist depthai-core (
+    echo [+] Downloading...
+    git clone https://github.com/luxonis/depthai-core.git
+    cd depthai-core
+    :: depthai 2.32.0 (Hunter-based; v2 does NOT use vcpkg/CMake presets)
+    git checkout v2.32.0
+    git submodule update --init --recursive
+    cd ..
+)
+cd depthai-core
+:: v2 and its old Hunter sub-deps declare cmake_minimum_required < 3.5, which
+:: CMake 4.x rejects, and CMAKE_POLICY_VERSION_MINIMUM does not propagate into
+:: Hunter's sub-builds. Use a pinned CMake 3.x just for depthai (Hunter reuses the
+:: same cmake binary for its dependency sub-builds).
+set "CMAKE3_VER=3.31.12"
+set "CMAKE3_NAME=cmake-%CMAKE3_VER%-windows-x86_64"
+set "CMAKE3_DIR=%SRC_DIR%\%CMAKE3_NAME%"
+set "CMAKE3=%CMAKE3_DIR%\bin\cmake.exe"
+if not exist "%CMAKE3_DIR%" (
+    echo [+] Downloading CMake %CMAKE3_VER% ...
+    curl -L -o "%SRC_DIR%\%CMAKE3_NAME%.zip" "https://github.com/Kitware/CMake/releases/download/v%CMAKE3_VER%/%CMAKE3_NAME%.zip" || exit /b !errorlevel!
+    "%SEVENZIP_EXE%" x "%SRC_DIR%\%CMAKE3_NAME%.zip" -o"%SRC_DIR%" -y || exit /b !errorlevel!
+)
+:: v2 is fully Hunter-managed (no vcpkg toolchain). depthaiDependencies.cmake
+:: unconditionally does find_package(OpenCV 4 QUIET CONFIG) (not gated by
+:: DEPTHAI_OPENCV_SUPPORT); if it finds the bundle's OpenCV, OpenCVModules does
+:: find_dependency(TIFF) which collides with Hunter's config-less FindTIFF. We don't
+:: need depthai's OpenCV at all (CameraDepthAI converts ImgFrame->cv::Mat itself), so
+:: hard-disable the lookup. DEPTHAI_ENABLE_CURL=OFF avoids Hunter's old zlib/curl.
+"%CMAKE3%" -S . -B build -GNinja ^
+  -DCMAKE_INSTALL_PREFIX="%FINAL_EXPORT_PATH%\installed\%TRIPLET%" ^
+  -DCMAKE_BUILD_TYPE=Release ^
+  -DBUILD_SHARED_LIBS=ON ^
+  -DDEPTHAI_OPENCV_SUPPORT=OFF ^
+  -DCMAKE_DISABLE_FIND_PACKAGE_OpenCV=ON ^
+  -DDEPTHAI_ENABLE_CURL=OFF ^
+  -DDEPTHAI_BUILD_EXAMPLES=OFF ^
+  -DDEPTHAI_BUILD_TESTS=OFF || exit /b !errorlevel!
+"%CMAKE3%" --build build --config Release --target install || exit /b !errorlevel!
+cd ..
+
+:: CCCoreLib
+echo [+] Building CCCoreLib...
+if not exist CCCoreLib (
+    echo [+] Downloading...
+    git clone https://github.com/CloudCompare/CCCoreLib.git
+    cd CCCoreLib
+    :: June 20, 2026
+    git checkout 4095bea6552096cb528c3b8b5cb9505df2aa6002
+    git submodule update --init --recursive
+    :: nanoflann is an internal dep (only used in src/Kriging.cpp, no public header),
+    :: but CCCoreLib links it PUBLIC, leaking nanoflann::nanoflann into the exported
+    :: interface and breaking find_package(CCCoreLib). Link it PRIVATE instead.
+    git apply "%~dp0patches\cccorelib_4095bea.patch"
+    cd ..
+)
+cd CCCoreLib
+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 ^
+  -DCCCORELIB_SHARED=ON ^
+  -DCCCORELIB_USE_CGAL=OFF ^
+  -DCCCORELIB_USE_TBB=ON ^
+  -DCCCORELIB_USE_QT_CONCURRENT=OFF || 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
+
+:: The vcpkg export folder should not be in the rtabmap source directory
+:: (otherwise we get some cmake errors about that)
+set VCPKG_UNZIPPED_EXPORT_PATH=%USERPROFILE%\Downloads\vcpkg-export-########-x64-vs2022
+
+set PATH=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\x64-windows-release\bin;%PATH%
+set PATH=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\x64-windows-release\sdk\windows-desktop\amd64\release\bin;%PATH%
+set PATH=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\x64-windows-release\tools\python3\Lib\site-packages\numpy.libs;%PATH%
+set K4A_ROOT_DIR=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\x64-windows-release
+set KINECTSDK20_DIR=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\x64-windows-release
+
+cmake -B build -GNinja ^
+  -DCMAKE_BUILD_TYPE=Release ^
+  -DBUILD_AS_BUNDLE=ON ^
+  -DWITH_PYTHON=ON ^
+  -DWITH_ZED=OFF ^
+  -DWITH_CERES=ON ^
+  -DWITH_ORBBEC_SDK=ON ^
+  -DWITH_FREENECT2=ON ^
+  -DWITH_K4W2=ON ^
+  -DWITH_K4A=ON ^
+  -DWITH_DEPTHAI=ON ^
+  -DWITH_REALSENSE2=ON ^
+  -DWITH_CCCORELIB=ON ^
+  -DWITH_PDAL=OFF ^
+  -DVCPKG_MANIFEST_INSTALL=OFF ^
+  -DVCPKG_TARGET_TRIPLET=x64-windows-release ^
+  -DVCPKG_INSTALLED_DIR="%VCPKG_UNZIPPED_EXPORT_PATH%/installed" ^
+  -DCMAKE_TOOLCHAIN_FILE=%VCPKG_UNZIPPED_EXPORT_PATH%/scripts/buildsystems/vcpkg.cmake ^
+  -DGTSAM_DIR=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\x64-windows-release\CMake 
+
+cmake --build build --config Release --target package
+
+:: To install CPU pytorch inside rtabmap package afterwards.
+:: Note that python.exe is the one in the bin directory of the package, not the system one.
+python.exe -m pip install torch torchvision opencv-python-headless "numpy<2"
+
+:EndComment

bundle_windows_deps_cuda.bat

@@ -0,0 +1,338 @@
+@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"
+:: All deps downloaded/built from source (pytorch, torchvision, opencv, libfreenect2, ...) live here.
+set "SRC_DIR=%~dp0vcpkg_deps_from_source"
+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
+)
+
+:: ZED: copy the main library + the zed-config.cmake so find_package(ZED) resolves
+:: it from the export (the config derives paths from ZED_SDK_ROOT_DIR/CMAKE_PREFIX_PATH).
+if "%ZED_SDK_ROOT_DIR%"=="" (
+    echo Error: ZED_SDK_ROOT_DIR environment variable is not set!
+    pause
+    exit /b 1
+)
+echo Copying ZED main library only...
+:: Only the core runtime goes into the bundle; the neural-depth/TensorRT DLLs are
+:: shipped separately (see the zed-neural-extra archive below).
+robocopy "%ZED_SDK_ROOT_DIR%\bin" "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\bin" sl_zed64.dll zlibwapi.dll /XO
+robocopy "%ZED_SDK_ROOT_DIR%\lib" "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\lib" /E /XO
+robocopy "%ZED_SDK_ROOT_DIR%\include" "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\include" /E /XO
+:: Install ZED's CMake config at the export prefix root so find_package(ZED 2) (config
+:: mode, via the vcpkg toolchain CMAKE_PREFIX_PATH) finds it.
+robocopy "%ZED_SDK_ROOT_DIR%" "%FINAL_EXPORT_PATH%\installed\%TRIPLET%" zed-config.cmake zed-config-version.cmake /XO
+:: Robocopy exit codes under 8 mean successful copies/no changes.
+:: 8 or higher means there was a failure.
+if !errorlevel! GEQ 8 (
+    echo Error: Robocopy failed with exit code !errorlevel!
+    pause
+    exit /b !errorlevel!
+)
+:: Archive the remaining ZED runtime DLLs (neural-depth / TensorRT extras) as a
+:: separate optional download, kept out of the main bundle to keep it small. Built
+:: only once: skipped if the archive already exists. Staged with robocopy /XF
+:: (excludes by name; avoids 7z's "!" exclude switch that delayed expansion mangles).
+set "ZED_EXTRA_7Z=%EXPORT_DIR%\%TARGET_NAME%-zed-neural-extra.7z"
+set "ZED_EXTRA_DIR=%SRC_DIR%\zed-neural-extra"
+if not exist "%ZED_EXTRA_7Z%" (
+    echo [+] Creating ZED neural extra archive...
+    if exist "%ZED_EXTRA_DIR%" rd /s /q "%ZED_EXTRA_DIR%"
+    robocopy "%ZED_SDK_ROOT_DIR%\bin" "%ZED_EXTRA_DIR%" *.dll /XF sl_zed64.dll zlibwapi.dll >nul
+    "%SEVENZIP_EXE%" a -t7z -mx9 "%ZED_EXTRA_7Z%" "%ZED_EXTRA_DIR%\*" || exit /b !errorlevel!
+)
+
+:: 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
+
+:: All deps cloned/built below go into %SRC_DIR% (incl. libfreenect2 built by
+:: bundle_windows_deps.bat) to keep the repo root clean.
+if not exist "%SRC_DIR%" mkdir "%SRC_DIR%"
+cd /d "%SRC_DIR%"
+
+:: 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_contrib
+    :: 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_cudacodec=OFF ^
+  -DBUILD_opencv_python3=OFF ^
+  -DBUILD_opencv_python_bindings_generator=OFF ^
+  -DBUILD_opencv_python_tests=OFF ^
+  -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!
+cd ..
+
+
+:: freenect2 with cuda support
+:: libfreenect2's CUDA processors include <helper_math.h>, which modern CUDA toolkits
+:: no longer ship (it moved to the NVIDIA/cuda-samples repo). Fetch it and point
+:: NVCUDASAMPLES_ROOT at it; libfreenect2 adds %NVCUDASAMPLES_ROOT%\common\inc to nvcc.
+set "CUDA_SAMPLES_DIR=%SRC_DIR%\cuda-samples"
+if not exist "%CUDA_SAMPLES_DIR%\common\inc\helper_math.h" (
+    mkdir "%CUDA_SAMPLES_DIR%\common\inc"
+    curl -L -o "%CUDA_SAMPLES_DIR%\common\inc\helper_math.h" "https://raw.githubusercontent.com/NVIDIA/cuda-samples/v12.5/Common/helper_math.h" || exit /b !errorlevel!
+)
+set "NVCUDASAMPLES_ROOT=%CUDA_SAMPLES_DIR%"
+cd libfreenect2
+cmake -S . -B build_cuda -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"  ^
+  -DPKG_CONFIG_EXECUTABLE="%FINAL_EXPORT_PATH%\installed\%TRIPLET%\tools\pkgconf\pkgconf.exe" ^
+  -DPKG_CONFIG_USE_CMAKE_PREFIX_PATH=ON ^
+  -DCMAKE_INSTALL_PREFIX="%FINAL_EXPORT_PATH%\installed\%TRIPLET%" ^
+  -DCMAKE_BUILD_TYPE=Release ^
+  -DENABLE_CUDA=ON ^
+  -DENABLE_OPENCL=ON ^
+  -DENABLE_OPENGL=ON ^
+  -DBUILD_EXAMPLES=OFF ^
+  -DBUILD_SHARED_LIBS=ON || exit /b !errorlevel!
+cmake --build build_cuda --config Release --target install || exit /b !errorlevel!
+cd ..
+
+:: CudaSift (CUDA SIFT/SURF GPU features, matlabbe fork; needs CUDA)
+echo [+] Building CudaSift...
+if not exist CudaSift (
+    echo [+] Downloading...
+    git clone https://github.com/matlabbe/CudaSift.git
+    cd CudaSift
+    git checkout f764e14ae59ee78ff5b282d38790301d80faadc3
+    cd ..
+)
+cd CudaSift
+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 ^
+  -DVERBOSE=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 || 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
+
+:: The vcpkg export folder should not be in the rtabmap source directory
+:: (otherwise we get some cmake errors about that)
+set VCPKG_UNZIPPED_EXPORT_PATH=%USERPROFILE%\Downloads\vcpkg-export-########-x64-vs2022
+
+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%
+set PATH=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\x64-windows-release\sdk\windows-desktop\amd64\release\bin;%PATH%
+set K4A_ROOT_DIR=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\%TRIPLET%
+set KINECTSDK20_DIR=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\%TRIPLET%
+set ZED_SDK_ROOT_DIR=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\%TRIPLET%
+
+cmake -B build_cuda -GNinja ^
+  -DCMAKE_BUILD_TYPE=Release ^
+  -DBUILD_AS_BUNDLE=ON ^
+  -DWITH_PYTHON=ON ^
+  -DWITH_TORCH=ON ^
+  -DWITH_ZED=ON ^
+  -DWITH_CUDASIFT=ON ^
+  -DWITH_CERES=ON ^
+  -DWITH_ORBBEC_SDK=ON ^
+  -DWITH_FREENECT2=ON ^
+  -DWITH_K4W2=ON ^
+  -DWITH_K4A=ON ^
+  -DWITH_DEPTHAI=ON ^
+  -DWITH_REALSENSE2=ON ^
+  -DWITH_CCCORELIB=ON ^
+  -DWITH_PDAL=OFF ^
+  -DVCPKG_MANIFEST_INSTALL=OFF ^
+  -DVCPKG_TARGET_TRIPLET=%TRIPLET% ^
+  -DVCPKG_INSTALLED_DIR="%VCPKG_UNZIPPED_EXPORT_PATH%/installed" ^
+  -DCMAKE_TOOLCHAIN_FILE=%VCPKG_UNZIPPED_EXPORT_PATH%/scripts/buildsystems/vcpkg.cmake ^
+  -DGTSAM_DIR=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\%TRIPLET%\CMake ^
+  -DTorch_DIR=%VCPKG_UNZIPPED_EXPORT_PATH%\installed\%TRIPLET%\tools\python3\Lib\site-packages\torch\share\cmake\Torch
+
+cmake --build build_cuda --config Release --target package
+
+:: Generate superpoint weights (from share directory of the installed package)
+curl -L -O "https://raw.githubusercontent.com/magicleap/SuperPointPretrainedNetwork/master/demo_superpoint.py"
+curl -L -O "https://github.com/magicleap/SuperPointPretrainedNetwork/raw/refs/heads/master/superpoint_v1.pth"
+..\bin\python.exe rtabmap_trace_superpoint.py
+
+:EndComment

cmake_modules/FindCuVSLAM.cmake

@@ -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)

cmake_modules/FindEigen3.cmake

@@ -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)
@@ -40,7 +40,13 @@ if(NOT Eigen3_FIND_VERSION)
 endif()
 
 macro(_eigen3_check_version)
-  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+  # Since Eigen 5.0.0 the version macros moved from Eigen/src/Core/util/Macros.h
+  # to a dedicated Eigen/Version header, so check the new location first.
+  if(EXISTS "${EIGEN3_INCLUDE_DIR}/Eigen/Version")
+    file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/Version" _eigen3_version_header)
+  else()
+    file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+  endif()
 
   string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
   set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")

cmake_modules/FindG2O.cmake

@@ -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
@@ -97,16 +101,16 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FIL
 	${G2O_TYPES_SBA}
         ${G2O_STUFF_LIBRARY})
   
-  IF(CSPARSE_FOUND)
+  IF(CSPARSE_FOUND AND G2O_SOLVER_CSPARSE AND G2O_SOLVER_CSPARSE_EXTENSION)
      SET(G2O_INCLUDE_DIRS 
         ${G2O_INCLUDE_DIRS} 
         ${CSPARSE_INCLUDE_DIR})
      SET(G2O_LIBRARIES
         ${G2O_LIBRARIES} 
-	${G2O_SOLVER_CSPARSE} 
-	${G2O_SOLVER_CSPARSE_EXTENSION}
-	${CSPARSE_LIBRARY})
-  ENDIF(CSPARSE_FOUND)
+        ${G2O_SOLVER_CSPARSE} 
+        ${G2O_SOLVER_CSPARSE_EXTENSION}
+        ${CSPARSE_LIBRARY})
+  ENDIF(CSPARSE_FOUND AND G2O_SOLVER_CSPARSE AND G2O_SOLVER_CSPARSE_EXTENSION)
 
   IF(G2O_SOLVER_CHOLMOD)
     SET(G2O_INCLUDE_DIRS 
@@ -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")

cmake_modules/FindORB_SLAM.cmake

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

cmake_modules/FindOpenVINS.cmake

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

cmake_modules/MacOSXBundleInfo.plist.in

@@ -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>

cmake_modules/RTABMapDocumentation.cmake

@@ -0,0 +1,112 @@
+# RTABMAP_SETUP_DOCUMENTATION()
+#
+# Prepares everything the API documentation build needs, and writes the Doxyfile:
+#
+#   - fetches Doxygen Awesome, the theme of the generated HTML
+#   - regenerates the Doxygen HTML header and injects our scripts in it
+#   - generates the "Parameter reference" page from Parameters.h
+#   - configures Doxyfile.in into ${CMAKE_BINARY_DIR}/Doxyfile
+#
+# Call it only when the documentation is actually wanted (see BUILD_DOCUMENTATION
+# in the top-level CMakeLists.txt): it needs the network, Doxygen and Python3.
+# Run the result from the source tree, whose layout the Doxyfile paths assume:
+#
+#   doxygen <build dir>/Doxyfile
+#
+# The convenience script docs-report.sh does all of that, and lays the output out
+# the way the published site expects.
+
+MACRO(RTABMAP_SETUP_DOCUMENTATION)
+   # Doxygen Awesome, the theme of the generated HTML. Downloaded here instead of
+   # being vendored, so bumping it is a one-line change; the archive is pinned by
+   # hash. It requires GENERATE_TREEVIEW=YES and HTML_COLORSTYLE=LIGHT (see
+   # Doxyfile.in): the theme brings its own dark mode.
+   INCLUDE(FetchContent)
+   FetchContent_Declare(
+      doxygen-awesome-css
+      URL      https://github.com/jothepro/doxygen-awesome-css/archive/refs/tags/v2.3.4.zip
+      URL_HASH SHA256=cb684b29f6be9e63300de56f26c13a04c298af8937a15ce87390fb5a601022a3
+   )
+   FetchContent_MakeAvailable(doxygen-awesome-css)
+   FetchContent_GetProperties(doxygen-awesome-css SOURCE_DIR AWESOME_CSS_DIR)
+
+   # Layout: the base theme keeps Doxygen's top tab bar. The sidebar-only variant
+   # drops it and puts everything in the left navigation, which needs the wider
+   # tree the theme expects (--side-nav-fixed-width).
+   OPTION(BUILD_DOCUMENTATION_SIDEBAR "Use the sidebar-only layout of the Doxygen theme" OFF)
+   SET(RTABMAP_DOXYGEN_STYLESHEETS "\"${AWESOME_CSS_DIR}/doxygen-awesome.css\"")
+   SET(RTABMAP_DOXYGEN_TREEVIEW_WIDTH 250)
+   IF(BUILD_DOCUMENTATION_SIDEBAR)
+      SET(RTABMAP_DOXYGEN_TREEVIEW_WIDTH 335)
+      STRING(APPEND RTABMAP_DOXYGEN_STYLESHEETS
+         " \\\n                         \"${AWESOME_CSS_DIR}/doxygen-awesome-sidebar-only.css\""
+         " \\\n                         \"${AWESOME_CSS_DIR}/doxygen-awesome-sidebar-only-darkmode-toggle.css\"")
+   ENDIF()
+   # Ours, last so it can override the theme's variables.
+   STRING(APPEND RTABMAP_DOXYGEN_STYLESHEETS
+      " \\\n                         \"${PROJECT_SOURCE_DIR}/doxygen/custom.css\"")
+
+   SET(RTABMAP_DOXYGEN_AWESOME_TOGGLE ${AWESOME_CSS_DIR}/doxygen-awesome-darkmode-toggle.js)
+   # Loaded from the header below; init() runs in <head> so that a reader who
+   # picked dark mode never sees a flash of the light theme.
+   SET(RTABMAP_DOXYGEN_AWESOME_SCRIPTS
+      "<script type=\"text/javascript\" src=\"$relpath^doxygen-awesome-darkmode-toggle.js\"></script>\n<script type=\"text/javascript\">DoxygenAwesomeDarkModeToggle.init();</script>\n")
+
+   # Doxygen HTML header carrying the version-switcher and Doxygen Awesome scripts.
+   # The template is regenerated with `doxygen -w` so it always matches the locally
+   # installed Doxygen (a committed header goes stale and warns on every run); we
+   # only insert <script> tags before </head>. $relpath^ is Doxygen's path from the
+   # current page back to this build's root, which is how the switcher locates
+   # versions.js one level above (the API root) without knowing the site prefix.
+   # The dark-mode toggle must be initialized before the page is rendered, so that
+   # a reader who picked dark does not get a flash of the light theme.
+   FIND_PROGRAM(RTABMAP_DOXYGEN_EXECUTABLE doxygen)
+   SET(RTABMAP_DOXYGEN_HEADER "")
+   IF(RTABMAP_DOXYGEN_EXECUTABLE)
+      SET(RTABMAP_DOXYGEN_HEADER ${PROJECT_BINARY_DIR}/doxygen-header.html)
+      EXECUTE_PROCESS(
+         COMMAND ${RTABMAP_DOXYGEN_EXECUTABLE} -w html
+                 ${RTABMAP_DOXYGEN_HEADER}
+                 ${PROJECT_BINARY_DIR}/doxygen-footer.html
+                 ${PROJECT_BINARY_DIR}/doxygen-stylesheet.css
+         WORKING_DIRECTORY ${PROJECT_BINARY_DIR}
+         OUTPUT_QUIET ERROR_QUIET)
+      IF(EXISTS ${RTABMAP_DOXYGEN_HEADER})
+         FILE(READ ${RTABMAP_DOXYGEN_HEADER} RTABMAP_DOXYGEN_HEADER_CONTENT)
+         STRING(REPLACE "</head>"
+            "<script type=\"text/javascript\">window.RTABMAP_DOC_ROOT=\"$relpath^\";</script>\n<script type=\"text/javascript\" src=\"$relpath^../versions.js\"></script>\n<script type=\"text/javascript\" src=\"$relpath^version-switcher.js\"></script>\n${RTABMAP_DOXYGEN_AWESOME_SCRIPTS}</head>"
+            RTABMAP_DOXYGEN_HEADER_CONTENT "${RTABMAP_DOXYGEN_HEADER_CONTENT}")
+         FILE(WRITE ${RTABMAP_DOXYGEN_HEADER} "${RTABMAP_DOXYGEN_HEADER_CONTENT}")
+      ELSE()
+         SET(RTABMAP_DOXYGEN_HEADER "")
+      ENDIF()
+   ENDIF()
+
+   # "Parameter reference" page: the parameters are declared through the
+   # RTABMAP_PARAM* macros, so Doxygen alone can only show 1900 bare accessors.
+   # The generator reads the declarations and emits one table per group, with the
+   # key, type, default and description together. Optional: without Python the
+   # page is simply left out of INPUT.
+   FIND_PACKAGE(Python3 COMPONENTS Interpreter QUIET)
+   SET(RTABMAP_DOXYGEN_PARAMETERS_PAGE "")
+   IF(Python3_Interpreter_FOUND)
+      EXECUTE_PROCESS(
+         COMMAND ${Python3_EXECUTABLE}
+                 ${PROJECT_SOURCE_DIR}/doxygen/generate_parameters_page.py
+                 --input ${PROJECT_SOURCE_DIR}/corelib/include/${PROJECT_PREFIX}/core/Parameters.h
+                 --output ${PROJECT_BINARY_DIR}/doxygen/parameters.md
+         RESULT_VARIABLE RTABMAP_DOXYGEN_PARAMETERS_RESULT
+         OUTPUT_QUIET ERROR_QUIET)
+      IF(RTABMAP_DOXYGEN_PARAMETERS_RESULT EQUAL 0)
+         SET(RTABMAP_DOXYGEN_PARAMETERS_PAGE ${PROJECT_BINARY_DIR}/doxygen/parameters.md)
+      ELSE()
+         MESSAGE(WARNING "Could not generate the Doxygen parameter reference page.")
+      ENDIF()
+   ENDIF()
+
+   # Doxyfile with PROJECT_NUMBER filled from RTABMAP_VERSION, so the generated
+   # documentation can never drift from the actual version. Its INPUT/OUTPUT paths
+   # are relative to the source tree, so run it from there:
+   #   doxygen <build dir>/Doxyfile
+   CONFIGURE_FILE(${PROJECT_SOURCE_DIR}/Doxyfile.in ${PROJECT_BINARY_DIR}/Doxyfile @ONLY)
+ENDMACRO(RTABMAP_SETUP_DOCUMENTATION)

codecov.yml

@@ -0,0 +1,33 @@
+# Codecov configuration -- https://docs.codecov.com/docs/codecov-yaml
+#
+# Coverage data is produced by .github/workflows/coverage.yml (lcov over a Debug
+# build with ENABLE_COVERAGE=ON) and uploaded by codecov/codecov-action; this
+# file only controls what Codecov reports back on a pull request. Nothing is
+# posted unless the Codecov GitHub App has access to the repository.
+
+# Mark uncovered added lines inline in the "Files changed" tab.
+github_checks:
+  annotations: true
+
+coverage:
+  precision: 2
+  round: down
+  range: "10...90" # red/green scale: 10% is fully red, 90% fully green
+
+  status:
+    # Catch a slow slide down without pinning an absolute number.
+    project:
+      default:
+        target: auto
+        threshold: 1%
+
+    # Coverage of the lines this pull request touches. Advisory: reported, but
+    # does not block the merge -- drop "informational" to make it gate.
+    patch:
+      default:
+        informational: true
+
+comment:
+  layout: "condensed_header, diff, files"
+  behavior: default
+  require_changes: true # stay quiet when coverage doesn't move

corelib/CMakeLists.txt

@@ -1 +1,5 @@
-ADD_SUBDIRECTORY( src )
+ADD_SUBDIRECTORY( src )
+
+if(BUILD_TESTING)
+   ADD_SUBDIRECTORY( test )
+endif()

corelib/include/rtabmap/core/BayesFilter.h

@@ -32,7 +32,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <opencv2/core/core.hpp>
 #include <list>
+#include <map>
 #include <set>
+#include <utility>
+#include <vector>
 #include "rtabmap/utilite/UEventsHandler.h"
 #include "rtabmap/core/Parameters.h"
 
@@ -41,45 +44,185 @@ namespace rtabmap {
 class Memory;
 class Signature;
 
+namespace bayes {
+class PredictionModel;
+class DensePrediction;
+class SparsePrediction;
+}
+
+/**
+ * @class BayesFilter
+ * @brief Recursive Bayesian filter for loop-closure hypothesis estimation in RTAB-Map.
+ *
+ * This class implements the prediction and update steps of a Bayes filter used to estimate
+ * the posterior probability over candidate locations (signatures) in working memory. It is
+ * typically called by Rtabmap after likelihood values have been computed from visual
+ * word comparisons.
+ *
+ * The filter operates in two steps on each iteration:
+ * - **Prediction**: builds a transition matrix from the memory graph and multiplies it
+ *   with the previous posterior to obtain the prior.
+ * - **Update**: multiplies the prior by the observation likelihood and normalizes the result.
+ *
+ * The prediction matrix is built from neighbor relationships in @ref Memory, using a
+ * Gaussian-like model configured through @ref Parameters::kBayesPredictionLC(). A virtual
+ * place (negative signature id, see @ref Memory::kIdVirtual) represents the hypothesis
+ * that the current observation comes from a new location.
+ *
+ * Related parameters (see @ref Parameters):
+ * - @ref Parameters::kBayesPredictionLC() — transition probabilities per graph depth level.
+ * - @ref Parameters::kBayesVirtualPlacePriorThr() — prior for the virtual place.
+ * - @ref Parameters::kBayesFullPredictionUpdate() — regenerate the full prediction matrix each iteration.
+ * - @ref Parameters::kBayesSparsePrediction() — keep the prediction sparse and multiply it sparsely.
+ *
+ * @see Memory::getNeighborsId()
+ * @see Rtabmap
+ */
 class RTABMAP_CORE_EXPORT BayesFilter
 {
 public:
+	/**
+	 * @brief Constructs a Bayes filter with default or custom parameters.
+	 * @param parameters Optional parameter map (Bayes group keys). Defaults are used for missing keys.
+	 */
 	BayesFilter(const ParametersMap & parameters = ParametersMap());
 	virtual ~BayesFilter();
+
+	/**
+	 * @brief Updates internal settings from the parameter map.
+	 * @param parameters Map containing Bayes group keys.
+	 */
 	virtual void parseParameters(const ParametersMap & parameters);
-	const std::map<int, float> & computePosterior(const Memory * memory, const std::map<int, float> & likelihood);
+
+	/**
+	 * @brief Runs one Bayes filter iteration (prediction + update).
+	 *
+	 * Given a likelihood map over signature ids, computes and stores the normalized posterior.
+	 * The prediction matrix is generated or updated from @ref Memory using the ids present
+	 * in @p likelihood.
+	 *
+	 * Read the result with @ref getPosteriorIds() and @ref getPosteriorValues().
+	 *
+	 * @param memory Working memory instance (must not be null).
+	 * @param likelihood Observation likelihood per signature id (must not be empty).
+	 * @return False on error (null memory, empty likelihood, or invalid prediction model),
+	 *         the posterior being left unchanged.
+	 */
+	bool computePosterior(const Memory * memory, const std::map<int, float> & likelihood);
+
+	/**
+	 * @brief Clears posterior, prediction matrix and cached neighbor indices.
+	 */
 	void reset();
 
-	//setters
+	/**
+	 * @brief Sets the loop-closure prediction model from a space-separated string.
+	 *
+	 * Format: `{Vp, Lc, l1, l2, l3, ...}` where:
+	 * - **Vp** — virtual place probability. This is the probability to move to a new place (unvisited location).
+	 * - **Lc** — loop closure (depth 0) probability. This is the probability to stay at the same location.
+	 * - **l1, l2, ...** — probabilities for neighbors at increasing graph depth levels. This is the probability to move to a neighbor at the given depth level.
+	 *
+	 * Each value must be in [0, 1]. At least two values are required. Invalid strings are rejected
+	 * and the previous model is kept.
+	 *
+	 * @param prediction Space-separated list of probabilities (same format as @ref Parameters::kBayesPredictionLC()).
+	 */
 	void setPredictionLC(const std::string & prediction);
 
-	//getters
-	const std::map<int, float> & getPosterior() const {return _posterior;}
-	float getVirtualPlacePrior() const {return _virtualPlacePrior;}
-	const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...}
-	std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...}
+	/**
+	 * @brief The locations the posterior is over, ascending by id.
+	 *
+	 * The virtual place (@ref Memory::kIdVirtual) is the first of them when it is one.
+	 */
+	const std::vector<int> & getPosteriorIds() const {return _posteriorIds;}
 
+	/**
+	 * @brief The probability of each location of @ref getPosteriorIds(), in the same order.
+	 */
+	const std::vector<float> & getPosteriorValues() const {return _posteriorValues;}
+
+	/**
+	 * @brief Returns the virtual place prior threshold.
+	 * @return Value in [0, 1] used when building the virtual place row of the prediction matrix.
+	 */
+	float getVirtualPlacePrior() const;
+
+	/**
+	 * @brief Whether the prediction is being kept in its sparse form rather than as a matrix.
+	 *
+	 * False when @ref Parameters::kBayesSparsePrediction() is disabled, and over a model whose
+	 * values sum to less than 1, which leaves no zero in a column to keep out of the values.
+	 */
+	bool isPredictionSparse() const;
+
+	/**
+	 * @brief Returns the loop-closure prediction model as a vector of values.
+	 * @return Vector in the format `{Vp, Lc, l1, l2, l3, ...}`.
+	 */
+	const std::vector<double> & getPredictionLC() const;
+
+	/**
+	 * @brief Returns the loop-closure prediction model as a space-separated string.
+	 * @return String representation of @ref getPredictionLC().
+	 */
+	std::string getPredictionLCStr() const;
+
+	/**
+	 * @brief Builds or updates the prediction (transition) matrix for the given signature ids.
+	 *
+	 * Rows and columns correspond to @p ids. Neighbor links are queried from @ref Memory to fill
+	 * transition probabilities according to @ref getPredictionLC(). When @p ids match the
+	 * current posterior keys, the cached matrix may be returned without recomputation.
+	 *
+	 * When the prediction is being kept sparse, the matrix is expanded from it rather than
+	 * kept: it costs the memory that keeping the prediction sparse is saving, so ask for it to
+	 * read, dump or compare the prediction, not on every iteration.
+	 *
+	 * @param memory Working memory instance (must not be null).
+	 * @param ids Ordered list of signature ids (often includes @ref Memory::kIdVirtual as first element).
+	 * @return Square CV_32FC1 matrix of size ids.size() × ids.size().
+	 */
 	cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids);
 
+	/**
+	 * @brief Estimates memory usage of this object and its internal containers.
+	 * @return Approximate memory footprint in bytes.
+	 */
 	unsigned long getMemoryUsed() const;
 
 private:
-	cv::Mat updatePrediction(const cv::Mat & oldPrediction,
-			const Memory * memory,
-			const std::vector<int> & oldIds,
-			const std::vector<int> & newIds);
-	void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
-	void normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const;
+	/**
+	 * @brief Realigns the posterior with the ids of the likelihood.
+	 *
+	 * Keeps the probability of the locations that are in both. Called only when the ids differ.
+	 */
+	void updatePosterior(const Memory * memory, const std::map<int, float> & likelihood);
+
+	/**
+	 * @brief Settles whether the prediction is kept sparse, from the parameter and the model.
+	 *
+	 * Called when either of the two changes rather than on every iteration, and releases the
+	 * sparse form when the answer is no.
+	 */
+	void updateKeepSparse();
 
 private:
-	std::map<int, float> _posterior;
-	cv::Mat _prediction;
-	float _virtualPlacePrior;
-	std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
-	bool _fullPredictionUpdate;
-	float _totalPredictionLCValues;
-	float _predictionEpsilon;
-	std::map<int, std::map<int, int> > _neighborsIndex;
+	std::vector<int> _posteriorIds;               ///< The locations the posterior is over, ascending by id.
+	std::vector<float> _posteriorValues;          ///< The probability of each of them, in the same order.
+	std::vector<int> _likelihoodIds;              ///< The ids of the likelihood of an iteration, in its order.
+	std::vector<float> _likelihoodValues;         ///< The likelihood of an iteration, in the same order.
+	std::vector<float> _priorValues;              ///< The prior of an iteration, in the same order.
+
+	bayes::PredictionModel * _model;              ///< The `{Vp, Lc, l1, ...}` model and the column arithmetic of it.
+	bayes::DensePrediction * _dense;              ///< The prediction as a matrix, used when it is not kept sparse.
+	bayes::SparsePrediction * _sparse;            ///< The prediction as its values only, one column at a time.
+	std::map<int, std::map<int, int> > _neighborsIndex; ///< Cached neighbor margins per signature id, for the incremental updates.
+
+	bool _fullPredictionUpdate;                   ///< If true, rebuild the whole prediction each time.
+	bool _sparsePrediction;                       ///< Keep the prediction sparse (Bayes/SparsePrediction).
+	bool _keepSparse;                             ///< Whether it is being kept sparse: the parameter, over a model that leaves nothing sparse to keep.
+	bool _predictionChanged;                      ///< True when the prediction has to be built again.
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Camera.h

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

corelib/include/rtabmap/core/CameraModel.h

@@ -35,21 +35,53 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class CameraModel
+ * @brief Represents a pinhole camera model containing intrinsic and extrinsic 
+ *        parameters, used for projection, rectification, and transformation.
+ *
+ * This class encapsulates camera calibration data, including intrinsic parameters (fx, fy, cx, cy),
+ * distortion coefficients, rectification and projection matrices. It provides utility functions
+ * for image rectification, projection from 2D to 3D, and vice versa.
+ * 
+ * This class supports the 4 to 14 parameters Radial Tangential distortion model (also called Plumb Bob or 
+ * Brown-Conrady model) and 4 parameters Fish Eye model (also known as Equidistant model).
+ * 
+ * @see OpenCV's calib3d module for all supported camera models.
+ */
 class RTABMAP_CORE_EXPORT CameraModel
 {
 public:
 	/**
-	 * Optical rotation used to transform image coordinate frame (x->right, y->down, z->forward)
-	 * to robot coordinate frame (x->forward, y->left, z->up).
+	 * @brief Returns the default optical rotation to convert image coordinates to robot coordinates.
+	 * 
+	 * Image frame: x -> right, y -> down, z -> forward  
+	 * Robot frame: x -> forward, y -> left, z -> up
+	 * 
+	 * @return Transform rotation matrix.
 	 */
 	static Transform opticalRotation() {return Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);}
 
 public:
+
+	/// Default constructor.
 	CameraModel();
-	// K is the camera intrinsic 3x3 CV_64FC1
-	// D is the distortion coefficients 1x5 CV_64FC1
-	// R is the rectification matrix 3x3 CV_64FC1 (computed from stereo or Identity)
-	// P is the projection matrix 3x4 CV_64FC1 (computed from stereo or equal to [K [0 0 1]'])
+	
+	/**
+	 * @brief Constructor using full camera parameters.
+	 * 
+	 * @param name           Camera name or ID.
+	 * @param imageSize      Size of the image (width x height).
+	 * @param K              Intrinsic matrix (3x3).
+	 * @param D              Distortion coefficients, 1xN matrix where N is between 4 and 14 parameters: 
+	 *                       k1,k2,p1,p2[,k3[,k4,k5,k6[,s1,s2,s3,s4[,tx,ty]]]]).
+	 *                       To set Fish Eye / Equidistant model, it is implicitly used if we 
+	 *                       provide 6 values like this: [k1,k2,0,0,k3,k4], where you only need to 
+	 *                       fill "k" parameters.
+	 * @param R              Rectification matrix (3x3).
+	 * @param P              Projection matrix (3x4).
+	 * @param localTransform Local transform to apply to the camera frame.
+	 */
 	CameraModel(
 			const std::string & name,
 			const cv::Size & imageSize,
@@ -59,7 +91,17 @@ public:
 			const cv::Mat & P,
 			const Transform & localTransform = opticalRotation());
 
-	// minimal
+	/**
+	 * @brief Minimal constructor using intrinsic parameters. This assumes the images are already rectified.
+	 *
+	 * @param fx             Focal length x.
+	 * @param fy             Focal length y.
+	 * @param cx             Principal point x.
+	 * @param cy             Principal point y.
+	 * @param localTransform Local transform to apply to the camera frame.
+	 * @param Tx             Baseline * fx (optional). Mainly used in case of stereo pair.
+	 * @param imageSize      Image size (optional).
+	 */
 	CameraModel(
 			double fx,
 			double fy,
@@ -68,7 +110,10 @@ public:
 			const Transform & localTransform = opticalRotation(),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
-	// minimal to be saved
+	
+	/**
+	 * @brief Minimal constructor with name for saving.
+	 */
 	CameraModel(
 			const std::string & name,
 			double fx,
@@ -79,13 +124,51 @@ public:
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 
+	/// Destructor.
 	virtual ~CameraModel() {}
 
+	/**
+	 * @brief Initializes the rectification maps used to undistort and rectify images.
+	 *
+	 * This function prepares the `mapX_` and `mapY_` lookup tables used for image rectification.
+	 * It supports both standard radial-tangential distortion and fisheye/equidistant distortion models.
+	 *
+	 * - If the distortion model is **fisheye** (indicated by `D_.cols == 6`), it uses
+	 *   `cv::fisheye::initUndistortRectifyMap()` to create the rectification maps. This requires OpenCV ≥ 2.4.10.
+	 * - Otherwise, it uses the standard `cv::initUndistortRectifyMap()` for plumb bob or rational polynomial models.
+	 *
+	 * @pre The camera model must be valid for rectification:
+	 *   - `imageSize_` must be non-zero.
+	 *   - `D_` must be a 1-row matrix with an accepted number of columns (4, 5, 6, 8, 12, or 14).
+	 *   - `R_` must be a 3x3 rectification matrix.
+	 *   - `P_` must be a 3x4 projection matrix.
+	 *
+	 * @return `true` if the rectification maps were successfully initialized (`mapX_` and `mapY_` are not empty),
+	 *         `false` otherwise.
+	 *
+	 * @see isRectificationMapInitialized(), rectifyImage(), rectifyDepth()
+	 *
+	 * @warning Requires OpenCV 2.4.10 or newer for fisheye support. If the version is older, fisheye rectification will not work.
+	 */
 	bool initRectificationMap();
+
+	/**
+	 * @brief Checks if the rectification map has been initialized.
+	 * @return True if both mapX_ and mapY_ are initialized.
+	 */
 	bool isRectificationMapInitialized() const {return !mapX_.empty() && !mapY_.empty();}
 
+	/**
+	 * @brief Checks if the model is valid for 2D->3D projection.
+	 */
 	bool isValidForProjection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0;}
+	/**
+	 * @brief Checks if the model is valid for 3D->2D reprojection.
+	 */
 	bool isValidForReprojection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0 && imageWidth()>0 && imageHeight()>0;}
+	/**
+	 * @brief Checks if the model has sufficient data for image rectification.
+	 */
 	bool isValidForRectification() const
 	{
 		return imageSize_.width>0 &&
@@ -96,69 +179,414 @@ public:
 			   !P_.empty();
 	}
 
+	/// Sets the camera name, used to set a camera name when saving to a file.
 	void setName(const std::string & name) {name_=name;}
+	/// Returns the camera name.
 	const std::string & name() const {return name_;}
 
+	/// Returns focal length in x.
 	double fx() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(0,0):P_.at<double>(0,0);}
+	/// Returns focal length in y.
 	double fy() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(1,1):P_.at<double>(1,1);}
+	/// Returns principal point x.
 	double cx() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(0,2):P_.at<double>(0,2);}
+	/// Returns principal point y.
 	double cy() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(1,2):P_.at<double>(1,2);}
+	/// Returns the x translation (usually fx * baseline in case of stereo, otherwise would be 0).
 	double Tx() const {return P_.empty()?0.0:P_.at<double>(0,3);}
 
-	cv::Mat K_raw() const {return K_;} //intrinsic camera matrix (before rectification)
-	cv::Mat D_raw() const {return D_;} //intrinsic distorsion matrix (before rectification)
-	cv::Mat K() const {return !P_.empty()?P_.colRange(0,3):K_;} // if P exists, return rectified version
+	/// Returns the raw intrinsic matrix (before rectification).
+	cv::Mat K_raw() const {return K_;}
+	/// Returns the raw distortion coefficients (before rectification).
+	cv::Mat D_raw() const {return D_;} 
+	/// Returns the rectified camera intrinsic matrix if the projection matrix P exists, otherwise returns the raw intrinsic matrix.
+	cv::Mat K() const {return !P_.empty()?P_.colRange(0,3):K_;}
+	/// Returns the rectified distortion coefficients (1x5 filled with zeros) if the projection matrix P exists, otherwise returns the raw distortion coefficients.
 	cv::Mat D() const {return P_.empty()&&!D_.empty()?D_:cv::Mat::zeros(1,5,CV_64FC1);} // if P exists, return rectified version
-	cv::Mat R() const {return R_;} //rectification matrix
-	cv::Mat P() const {return P_;} //projection matrix
+	/// Returns the rectification matrix.
+	cv::Mat R() const {return R_;}
+	/// Returns the projection matrix.
+	cv::Mat P() const {return P_;}
 
+	/// Sets the local transform of the camera (base frame to optical frame).
 	void setLocalTransform(const Transform & transform) {localTransform_ = transform;}
+	/// Returns the local transform (base frame to optical frame).
 	const Transform & localTransform() const {return localTransform_;}
 
+	/**
+	 * @brief Sets the image size of the camera model and updates the principal point if undefined.
+	 *
+	 * This function updates the internal image size (`imageSize_`) with the provided size.
+	 * If the intrinsic matrices (`K_` or `P_`) are present and the principal point coordinates
+	 * (`cx`, `cy`) are zero, they are set to the image center (`width/2 - 0.5`, `height/2 - 0.5`).
+	 *
+	 * This ensures the camera model remains valid and useful even when the calibration file
+	 * has no principal point set or the image size is updated manually.
+	 *
+	 * @param size The new image size. It must be either both dimensions zero (clearing) or both positive.
+	 *
+	 * @pre `size.width > 0 && size.height > 0` or `size.width == 0 && size.height == 0`
+	 * @post Updates the `imageSize_`, and adjusts `cx` and `cy` in `K_` and `P_` if they were initially zero.
+	 *
+	 * @warning If `K_` or `P_` are not initialized (`empty()`), no updates will be applied to them.
+	 *
+	 * @see imageSize(), imageWidth(), imageHeight()
+	 */
 	void setImageSize(const cv::Size & size);
 	const cv::Size & imageSize() const {return imageSize_;}
 	int imageWidth() const {return imageSize_.width;}
 	int imageHeight() const {return imageSize_.height;}
 
-	double fovX() const; // in radians
-	double fovY() const; // in radians
-	double horizontalFOV() const; // in degrees
-	double verticalFOV() const;   // in degrees
+	/**
+	 * @brief Returns the horizontal field of view (FoV) in radians.
+	 *
+	 * The FoV is computed using the pinhole camera model as:
+	 * \f[
+	 * \text{FoV}_x = 2 \cdot \tan^{-1}\left(\frac{\text{image width}}{2 \cdot f_x}\right)
+	 * \f]
+	 *
+	 * @return Horizontal field of view in radians. Returns 0.0 if image width or focal length is invalid.
+	 */
+	double fovX() const;
+
+	/**
+	 * @brief Returns the vertical field of view (FoV) in radians.
+	 *
+	 * The FoV is computed using the pinhole camera model as:
+	 * \f[
+	 * \text{FoV}_y = 2 \cdot \tan^{-1}\left(\frac{\text{image height}}{2 \cdot f_y}\right)
+	 * \f]
+	 *
+	 * @return Vertical field of view in radians. Returns 0.0 if image height or focal length is invalid.
+	 */
+	double fovY() const;
+
+	/**
+	 * @brief Returns the horizontal field of view in degrees.
+	 *
+	 * Converts the result of `fovX()` from radians to degrees.
+	 *
+	 * @return Horizontal field of view in degrees. Returns 0.0 if the result is invalid.
+	 */
+	double horizontalFOV() const;
+	
+	/**
+	 * @brief Returns the vertical field of view in degrees.
+	 *
+	 * Converts the result of `fovY()` from radians to degrees.
+	 *
+	 * @return Vertical field of view in degrees. Returns 0.0 if the result is invalid.
+	 */
+	double verticalFOV() const;
+
+	/// Checks if the distortion model is fisheye (6 coefficients: k1,k2,0,0,k3,k4).
 	bool isFisheye() const {return D_.cols == 6;}
 
-	bool load(const std::string & filePath);
-	bool load(const std::string & directory, const std::string & cameraName);
+	/**
+	 * @brief Loads the camera model parameters from a YAML calibration file.
+	 *
+	 * This method attempts to read camera intrinsic/extrinsic parameters and image size from a YAML file,
+	 * typically in the ROS calibration format. If the distortion model is "fisheye" or "equidistant", we expect
+	 * 4 coefficients, which are converted to a 6-coefficient format for internal representation.
+	 *
+	 * Fields loaded (if present):  
+	 * - `camera_name`  
+	 * - `image_width` (pixels)
+	 * - `image_height` (pixels)
+	 * - `camera_matrix` (K, 3x3 double matrix)  
+	 * - `distortion_coefficients` (D, 1xN double matrix)  
+	 * - `distortion_model`  (string: name of the model)
+	 * - `rectification_matrix` (R, 3x3 double matrix)  
+	 * - `projection_matrix` (P, 3x4 double matrix)  
+	 * - `local_transform` (camera pose w.r.t robot frame)
+	 *
+	 * On success, the internal matrices and settings of the camera model are updated. If the model
+	 * is valid for rectification, the rectification map is initialized.
+	 *
+	 * @param filePath Absolute or relative path to the YAML file.
+	 * @param initRectificationMaps Set to false to skip building the (potentially large) rectification
+	 *                              maps when rectification won't be used (saves time and memory).
+	 * @return True if the file was successfully loaded and parsed, false otherwise.
+	 *
+	 * @warning Logs warnings if any fields are missing. If file does not exist or parsing fails, returns false.
+	 *
+	 * @see initRectificationMap()
+	 */
+	bool load(const std::string & filePath, bool initRectificationMaps = true);
+
+	/**
+	 * @brief Loads the camera model by constructing a file path from a directory and camera name.
+	 *
+	 * This is a convenience wrapper around `load(filePath)` that constructs the file path as:
+	 * `directory + "/" + cameraName + ".yaml"`.
+	 *
+	 * @param directory Path to the folder containing the camera YAML file.
+	 * @param cameraName Base name of the camera file (without extension).
+	 * @param initRectificationMaps Set to false to skip building the (potentially large) rectification
+	 *                              maps when rectification won't be used (saves time and memory).
+	 * @return True if loading from the constructed path succeeds, false otherwise.
+	 */
+	bool load(const std::string & directory, const std::string & cameraName, bool initRectificationMaps = true);
+
+	/**
+	 * @brief Saves the camera model parameters to a YAML calibration file in ROS format.
+	 *
+	 * The file will include the following fields if they are not empty:
+	 * - `camera_name`
+	 * - `image_width`
+	 * - `image_height`
+	 * - `camera_matrix` (K)
+	 * - `distortion_coefficients` (D)
+	 * - `distortion_model` (auto-detected based on number of distortion coefficients)
+	 * - `rectification_matrix` (R)
+	 * - `projection_matrix` (P)
+	 * - `local_transform` (camera pose w.r.t robot frame)
+	 *
+	 * If the distortion matrix contains 6 coefficients (used for fisheye), it is converted
+	 * to a standard 4-coefficient format for ROS compatibility.
+	 *
+	 * @param directory Path to the folder where the YAML file will be saved.
+	 * @return True if saving was successful, false otherwise.
+	 *
+	 * @note If `name_` is empty, "camera.yaml" is used as the default filename.
+	 * @warning Returns false and logs an error if none of the matrices are set.
+	 */
 	bool save(const std::string & directory) const;
+
+	/**
+	 * @brief Serializes the camera model to a binary format.
+	 *
+	 * The serialization includes the camera intrinsics (`K_`, `D_`), rectification matrix (`R_`), 
+	 * projection matrix (`P_`), image size, and the local transform. The format is compact and suitable 
+	 * for file storage or transmission over a network.
+	 *
+	 * Data layout:
+	 * - Header (11 integers):
+	 *   - [0-2]  RTAB-Map version (major, minor, patch)
+	 *   - [3]    Camera type (0 = mono, 1=stereo)
+	 *   - [4-5]  Image width, height
+	 *   - [6-9]  Element counts for K, D, R, P matrices
+	 *   - [10]   Size of localTransform (0 if null)
+	 * - Data section (in order): raw memory blocks for K, D, R, P (`double` values), followed by `float` values for localTransform
+	 *
+	 * @return A byte vector containing the serialized data. The format is compatible with `deserialize()`.
+	 *
+	 * @note This is a custom binary format, not meant to be human-readable.
+	 * @see deserialize(), StereoCameraModel
+	 */
 	std::vector<unsigned char> serialize() const;
+	/**
+	 * @brief Deserializes a camera model from a byte vector.
+	 *
+	 * This is a convenience wrapper around `deserialize(const unsigned char*, unsigned int)` 
+	 * that takes a `std::vector<unsigned char>` instead of a raw buffer.
+	 *
+	 * @param data Byte vector containing data serialized by `serialize()`.
+	 * @return The number of bytes successfully read and parsed. Returns 0 on failure.
+	 *
+	 * @see serialize(), deserialize(const unsigned char*, unsigned int)
+	 */
 	unsigned int deserialize(const std::vector<unsigned char>& data);
+
+	/**
+	 * @brief Deserializes a camera model from a raw byte buffer.
+	 *
+	 * Reads the camera intrinsics, distortion, rectification, projection matrices, image size, 
+	 * and local transform from a serialized binary format previously created with `serialize()`.
+	 *
+	 * @param data Pointer to the binary data buffer.
+	 * @param dataSize Size of the data buffer in bytes.
+	 * @return The number of bytes successfully read. Returns 0 on error or if the format is invalid.
+	 *
+	 * @warning If the buffer format does not match the expected layout or version, an error is logged 
+	 *          and the camera model remains in a default-initialized state.
+	 *
+	 * @note Assumes little-endian architecture and strict size/type matching. The serialized format 
+	 *       must be created by `CameraModel::serialize()`. Non-mono camera types are not supported. 
+	 *       See `StereoCameraModel` to serialize/deserialize stereo models.
+	 *
+	 * @see serialize()
+	 */
 	unsigned int deserialize(const unsigned char * data, unsigned int dataSize);
 
+	/**
+	 * @brief Returns a new camera model with all intrinsic parameters scaled by a given factor.
+	 *
+	 * This method scales the camera's intrinsic matrix (`K_`) and projection matrix (`P_`), as well as 
+	 * the image size, by the given `scale` factor. The distortion and rectification matrices are left unchanged.
+	 *
+	 * Only valid camera models (i.e., those for which `isValidForProjection()` returns true) are scaled.
+	 * If the model is invalid, a warning is issued and the original model is returned unchanged.
+	 *
+	 * @param scale Scaling factor (> 0). For example, use 0.5 to downscale or 2.0 to upscale.
+	 * @return A scaled copy of the camera model with updated intrinsics and image size.
+	 *
+	 * @warning If the camera model is not valid for projection, the scale operation is ignored.
+	 */
 	CameraModel scaled(double scale) const;
+
+	/**
+	 * @brief Returns a new camera model adjusted for a given region of interest (ROI).
+	 *
+	 * This method shifts the principal point (`cx`, `cy`) in the intrinsic matrix (`K_`) and projection matrix (`P_`)
+	 * by subtracting the ROI’s top-left `(x, y)` offset. The image size is also set to the ROI size.
+	 *
+	 * Only valid camera models (i.e., those for which `isValidForProjection()` returns true) can be adjusted.
+	 * If the model is invalid, a warning is issued and the original model is returned unchanged.
+	 *
+	 * @param roi Region of interest defined as a rectangle (typically a subwindow of the full image).
+	 * @return A new camera model adapted to the ROI with adjusted intrinsics and image size.
+	 *
+	 * @warning If the camera model is not valid for projection, the ROI operation is ignored.
+	 */
 	CameraModel roi(const cv::Rect & roi) const;
 
-	// For depth images, your should use cv::INTER_NEAREST
+	/**
+	 * @brief Rectifies a raw image using the precomputed rectification maps.
+	 *
+	 * This function applies geometric correction (rectification) to an image using the camera model's
+	 * `mapX_` and `mapY_` rectification maps. It is typically used to correct lens distortion in images
+	 * based on the calibration parameters.
+	 *
+	 * @param raw Input raw image (e.g., from camera). Must be a valid `cv::Mat`.
+	 * @param interpolation Interpolation method to use. Typically `cv::INTER_LINEAR` or `cv::INTER_NEAREST`.
+	 *
+	 * @return Rectified image. If the rectification maps are not initialized, the function logs an error
+	 *         and returns a clone of the original image.
+	 *
+	 * @pre `mapX_` and `mapY_` must be initialized using `initRectificationMap()`.
+	 *
+	 * @note Works for color and grayscale images of any valid type.
+	 *
+	 * @see initRectificationMap(), rectifyDepth()
+	 */
 	cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
+
+	/**
+	 * @brief Rectifies a raw depth image using the precomputed rectification maps.
+	 *
+	 * This function applies geometric correction (rectification) to a 16-bit unsigned depth image.
+	 * It performs a pixel-by-pixel bilinear interpolation, only if all neighboring pixels have valid
+	 * (non-zero) depth values, and the variation among them is within 1% of their average.
+	 *
+	 * The method is optimized to avoid introducing noise in regions of high depth variance.
+	 *
+	 * @param raw Input raw depth image (`CV_16UC1`). Must contain 16-bit unsigned depth values.
+	 *
+	 * @return Rectified depth image. If the rectification maps are not initialized or the input
+	 *         image is not of type `CV_16UC1`, the function logs an error and returns a clone of the input.
+	 *
+	 * @pre Input image must be of type `CV_16UC1`. `mapX_` and `mapY_` must be initialized.
+	 *
+	 * @note Inspired by the Kinect2 CPU depth registration implementation from:
+	 *       https://github.com/code-iai/iai_kinect2
+	 *
+	 * @warning Invalid or noisy regions are skipped in interpolation to maintain depth consistency.
+	 *
+	 * @see initRectificationMap(), rectifyImage()
+	 */
 	cv::Mat rectifyDepth(const cv::Mat & raw) const;
 
-	// Project 2D pixel to 3D (in /camera_link frame)
+	/**
+	 * @brief Projects a 2D pixel and depth value into a 3D point in the camera coordinate frame (/camera_link).
+	 *
+	 * This function uses the camera's intrinsic parameters to compute the 3D point corresponding to the given
+	 * 2D image coordinates and depth value.
+	 *
+	 * @param u Horizontal image coordinate (in pixels).
+	 * @param v Vertical image coordinate (in pixels).
+	 * @param depth Depth value at (u, v) in meters.
+	 * @param[out] x Output X coordinate in 3D space.
+	 * @param[out] y Output Y coordinate in 3D space.
+	 * @param[out] z Output Z coordinate in 3D space (equals `depth`).
+	 *
+	 * @note If `depth <= 0`, the output (x, y, z) will be set to `NaN`.
+	 *
+	 * @see reproject()
+	 */
 	void project(float u, float v, float depth, float & x, float & y, float & z) const;
-	// Reproject 3D point (in /camera_link frame) to pixel
+	
+	/**
+	 * @brief Reprojects a 3D point in the camera frame (/camera_link) into 2D image coordinates (floating-point).
+	 *
+	 * This function computes the image plane coordinates for a given 3D point using the camera's
+	 * intrinsic parameters.
+	 *
+	 * @param x X coordinate in camera space.
+	 * @param y Y coordinate in camera space.
+	 * @param z Z coordinate in camera space (must be non-zero).
+	 * @param[out] u Output horizontal image coordinate (float).
+	 * @param[out] v Output vertical image coordinate (float).
+	 *
+	 * @pre `z != 0`
+	 *
+	 * @see project(), reproject(int&, int&)
+	 */
 	void reproject(float x, float y, float z, float & u, float & v) const;
+
+	/**
+	 * @brief Reprojects a 3D point in the camera frame (/camera_link) into 2D image coordinates (rounded to int).
+	 *
+	 * This version of `reproject()` returns integer pixel indices, computed from the 3D position.
+	 *
+	 * @param x X coordinate in camera space.
+	 * @param y Y coordinate in camera space.
+	 * @param z Z coordinate in camera space (must be non-zero).
+	 * @param[out] u Output horizontal image coordinate (integer pixel).
+	 * @param[out] v Output vertical image coordinate (integer pixel).
+	 *
+	 * @pre `z != 0`
+	 *
+	 * @see project(), reproject(float&, float&)
+	 */
 	void reproject(float x, float y, float z, int & u, int & v) const;
+
+	/**
+	 * @brief Checks if a given pixel coordinate lies within the image bounds.
+	 *
+	 * @param u Horizontal image coordinate (in pixels).
+	 * @param v Vertical image coordinate (in pixels).
+	 * @return `true` if the pixel is within the image dimensions, `false` otherwise.
+	 *
+	 * @note Inclusive lower bound, exclusive upper bound: `[0, width)`, `[0, height)`
+	 */
 	bool inFrame(int u, int v) const;
 
 private:
-	std::string name_;
-	cv::Size imageSize_;
-	cv::Mat K_;
-	cv::Mat D_;
-	cv::Mat R_;
-	cv::Mat P_;
-	cv::Mat mapX_;
-	cv::Mat mapY_;
-	Transform localTransform_;
+	std::string name_;         ///< Camera name.
+	cv::Size imageSize_;       ///< Image size.
+	cv::Mat K_;                ///< Intrinsic matrix.
+	cv::Mat D_;                ///< Distortion coefficients.
+	cv::Mat R_;                ///< Rectification matrix.
+	cv::Mat P_;                ///< Projection matrix.
+	cv::Mat mapX_;             ///< Rectification map X.
+	cv::Mat mapY_;             ///< Rectification map Y.
+	Transform localTransform_; ///< Transform from camera to base link.
 };
 
+/**
+ * @brief Stream operator for printing a camera model to an output stream.
+ *
+ * This function outputs the name, image size, and camera matrices (K, D, R, P)
+ * along with the local transformation.
+ *
+ * Example output:
+ * ```
+ * Name: camera1
+ * Size: 640x480
+ * K= [fx,  0, cx;
+ *      0, fy, cy;
+ *      0,  0,  1]
+ * D= [...]
+ * R= [...]
+ * P= [...]
+ * LocalTransform= [...]
+ * ```
+ *
+ * @param os Output stream.
+ * @param model Camera model to print.
+ * @return The modified output stream.
+ */
 RTABMAP_CORE_EXPORT std::ostream& operator<<(std::ostream& os, const CameraModel& model);
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/CameraRGBD.h

@@ -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>

corelib/include/rtabmap/core/Compression.h

@@ -37,29 +37,51 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 
 /**
- * Compress image or data
+ * @class CompressionThread
+ * @brief Background thread to compress or uncompress images and generic matrices.
+ *
+ * In compress mode, pass a source matrix to the constructor with an optional image
+ * format (".png", ".jpg", ".rvl", or empty for zlib data). In uncompress mode, pass
+ * compressed bytes and set @c isImage accordingly. Call @ref UThread::start() then
+ * @ref UThread::join() to obtain the result from @ref getCompressedData() or
+ * @ref getUncompressedData().
  *
  * Example compression:
- *   cv::Mat image;// an image
- *   CompressionThread ct(image);
- *   ct.start();
- *   ct.join();
- *   std::vector<unsigned char> bytes = ct.getCompressedData();
+ * @code
+ * cv::Mat image;
+ * CompressionThread ct(image, ".png");
+ * ct.start();
+ * ct.join();
+ * cv::Mat bytes = ct.getCompressedData();
+ * @endcode
  *
- * Example uncompression
- *   std::vector<unsigned char> bytes;// a compressed image
- *   CompressionThread ct(bytes);
- *   ct.start();
- *   ct.join();
- *   cv::Mat image = ct.getUncompressedData();
+ * Example uncompression:
+ * @code
+ * cv::Mat bytes;
+ * CompressionThread ct(bytes, true);
+ * ct.start();
+ * ct.join();
+ * cv::Mat image = ct.getUncompressedData();
+ * @endcode
  */
 class RTABMAP_CORE_EXPORT CompressionThread : public UThread
 {
 public:
-	// format : ".png" ".jpg" "" (empty is general)
+	/**
+	 * @brief Constructs a thread in compress mode.
+	 * @param mat Source image or data matrix to compress.
+	 * @param format Image format: @c ".png", @c ".jpg", @c ".rvl", or empty for zlib (@ref compressData2).
+	 */
 	CompressionThread(const cv::Mat & mat, const std::string & format = "");
+	/**
+	 * @brief Constructs a thread in uncompress mode.
+	 * @param bytes Compressed bytes (@c CV_8UC1).
+	 * @param isImage If true, decode as image; otherwise decode as zlib data.
+	 */
 	CompressionThread(const cv::Mat & bytes, bool isImage);
+	/** @return Compressed output (@c CV_8UC1), valid after compress mode completes. */
 	const cv::Mat & getCompressedData() const {return compressedData_;}
+	/** @return Uncompressed output, valid after uncompress mode completes. */
 	cv::Mat & getUncompressedData() {return uncompressedData_;}
 protected:
 	virtual void mainLoop();
@@ -71,24 +93,41 @@ private:
 	bool compressMode_;
 };
 
+/** @brief Encodes @p image to a byte buffer (OpenCV @c imencode or RVL for depth). */
 std::vector<unsigned char> RTABMAP_CORE_EXPORT compressImage(const cv::Mat & image, const std::string & format = ".png");
+/** @brief Same as @ref compressImage() but returns a @c CV_8UC1 row matrix. */
 cv::Mat RTABMAP_CORE_EXPORT compressImage2(const cv::Mat & image, const std::string & format = ".png");
 
+/** @brief Decodes compressed image bytes to a @cv::Mat. */
 cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const cv::Mat & bytes);
+/** @brief Decodes compressed image bytes to a @cv::Mat. */
 cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const std::vector<unsigned char> & bytes);
 
+/** @brief Compresses a matrix with zlib; appends rows, cols and type at the end. */
 std::vector<unsigned char> RTABMAP_CORE_EXPORT compressData(const cv::Mat & data);
+/** @brief Same as @ref compressData() but returns a @c CV_8UC1 row matrix. */
 cv::Mat RTABMAP_CORE_EXPORT compressData2(const cv::Mat & data);
 
+/** @brief Restores a matrix compressed with @ref compressData() or @ref compressData2(). */
 cv::Mat RTABMAP_CORE_EXPORT uncompressData(const cv::Mat & bytes);
+/** @brief Restores a matrix compressed with @ref compressData() or @ref compressData2(). */
 cv::Mat RTABMAP_CORE_EXPORT uncompressData(const std::vector<unsigned char> & bytes);
+/** @brief Restores a matrix from a raw compressed buffer. */
 cv::Mat RTABMAP_CORE_EXPORT uncompressData(const unsigned char * bytes, unsigned long size);
 
+/** @brief Compresses a null-terminated string using @ref compressData2(). */
 cv::Mat RTABMAP_CORE_EXPORT compressString(const std::string & str);
+/** @brief Decompresses a string produced by @ref compressString(). */
 std::string RTABMAP_CORE_EXPORT uncompressString(const cv::Mat & bytes);
 
+/**
+ * @brief Detects the compression format of depth image bytes.
+ * @return @c ".rvl" if the buffer has an RVL signature, otherwise @c ".png".
+ */
 std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const cv::Mat & bytes);
+/** @overload */
 std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const std::vector<unsigned char> & bytes);
+/** @overload */
 std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const unsigned char * bytes, size_t size);
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/DBDriver.h

@@ -50,45 +50,102 @@ class Signature;
 class VWDictionary;
 class VisualWord;
 
-// Todo This class needs a refactoring, the _dbSafeAccessMutex problem when the trash is emptying (transaction)
-// "Of course, it has always been the case and probably always will be
-//that you cannot use the same sqlite3 connection in two or more
-//threads at the same time.  You can use different sqlite3 connections
-//at the same time in different threads, or you can move the same
-//sqlite3 connection across threads (subject to the constraints above)
-//but never, never try to use the same connection simultaneously in
-//two or more threads."
-//
+/**
+ * @class DBDriver
+ * @brief Abstract database driver for RTAB-Map maps (signatures, links, words, statistics).
+ *
+ * DBDriver is the persistence layer used by @ref Memory, @ref DBReader and tools that
+ * read or write \c .db files. The default implementation is @ref DBDriverSqlite3, created
+ * by @ref create().
+ *
+ * The driver extends @ref UThreadNode. Pending @ref Signature and @ref VisualWord objects
+ * are queued with @ref asyncSave() and flushed to the database by @ref emptyTrashes()
+ * (also called from the background thread on @ref closeConnection()).
+ *
+ * Public methods are thread-safe where noted (mutex-protected wrappers). Subclasses
+ * implement SQL-specific logic in protected \c *Query() virtual methods.
+ *
+ * @see DBDriverSqlite3
+ * @see Memory
+ */
 class RTABMAP_CORE_EXPORT DBDriver : public UThreadNode
 {
 public:
+	/**
+	 * @brief Factory: returns a SQLite database driver (@ref DBDriverSqlite3).
+	 * @param parameters Optional driver parameters (e.g. @ref Parameters::kDbTargetVersion()).
+	 * @return New driver instance; caller owns the pointer.
+	 */
 	static DBDriver * create(const ParametersMap & parameters = ParametersMap());
 
 public:
 	virtual ~DBDriver();
 
+	/** @brief Parse driver parameters from the map (e.g. target schema version). */
 	virtual void parseParameters(const ParametersMap & parameters);
+	/**
+	 * @brief True when no database file URL was set at open time.
+	 * @note @ref DBDriverSqlite3 overrides this when @ref Parameters::kDbSqlite3InMemory() is enabled.
+	 */
 	virtual bool isInMemory() const {return _url.empty();}
+	/** @return Database file path last passed to @ref openConnection(). */
 	const std::string & getUrl() const {return _url;}
+	/** @return Target schema version for new databases (from parameters). */
 	const std::string & getTargetVersion() const {return _targetVersion;}
 
-	void beginTransaction() const;
-	void commit() const;
-
-	void asyncSave(Signature * s); //ownership transferred
-	void asyncSave(VisualWord * vw); //ownership transferred
+	/**
+	 * @brief Queue a signature for deferred save; ownership is transferred.
+	 *
+	 * @note Only the *compressed* sensor buffers are written
+	 * (SensorData::imageCompressed(), depthOrRightCompressed(),
+	 * laserScanCompressed(), ...). Raw matrices are ignored, so a signature
+	 * carrying only raw data is stored with empty payloads. Memory compresses
+	 * before saving; a caller driving the driver directly should compress first
+	 * with @ref compressImage2() / @ref compressData2(), or pass the compressed
+	 * buffers to SensorData::setRGBDImage() / setLaserScan(), which treat a
+	 * 1-row CV_8UC1 matrix as already compressed.
+	 */
+	void asyncSave(Signature * s);
+	/** @brief Queue a visual word for deferred save; ownership is transferred. */
+	void asyncSave(VisualWord * vw);
+	/**
+	 * @brief Flush queued signatures and visual words to the database.
+	 * @param async If true, signal the background thread instead of flushing synchronously.
+	 */
 	void emptyTrashes(bool async = false);
 	double getEmptyTrashesTime() const {return _emptyTrashesTime;}
 	void setTimestampUpdateEnabled(bool enabled) {_timestampUpdate = enabled;} // used on Update Signature and Word queries
 
-	// Warning: the following functions don't look in the trash, direct database modifications
+	/**
+	 * @brief Export the pose graph to a Graphviz DOT file for visualization.
+	 *
+	 * Reads nodes and links from the database (and optionally from @p otherSignatures
+	 * not yet persisted) and writes @p fileName. Does not modify the database.
+	 * If @p ids is empty, all node ids are included.
+	 */
 	void generateGraph(
 			const std::string & fileName,
 			const std::set<int> & ids = std::set<int>(),
 			const std::map<int, Signature *> & otherSignatures = std::map<int, Signature *>());
+
+	/**
+	 * @name Direct database updates
+	 * @brief Write node or link data immediately (not via the async trash).
+	 * @{
+	 */
+	/** @brief Insert or replace a link in the database. */
 	void addLink(const Link & link);
+	/** @brief Remove a link between two nodes. */
 	void removeLink(int from, int to);
+	/** @brief Update an existing link in the database. */
 	void updateLink(const Link & link);
+	/**
+	 * @brief Update occupancy grid cells for a node.
+	 * @param ground Ground cells (raw @c CV_32FC2/@c CV_32FC3, or compressed @c CV_8UC1 1×N).
+	 * @param obstacles Obstacle cells (same formats as @p ground).
+	 * @param empty Empty cells (same formats as @p ground; ignored on DB schema &lt; 0.16.0).
+	 * Raw mats are compressed internally via @ref SensorData::setOccupancyGrid() before writing.
+	 */
 	void updateOccupancyGrid(
 				int nodeId,
 				const cv::Mat & ground,
@@ -96,22 +153,61 @@ public:
 				const cv::Mat & empty,
 				float cellSize,
 				const cv::Point3f & viewpoint);
+	/** @brief Update camera calibration stored for a node. */
 	void updateCalibration(
 		int nodeId,
 		const std::vector<CameraModel> & models,
 		const std::vector<StereoCameraModel> & stereoModels);
+	/**
+	 * @brief Update the depth image stored for a node.
+	 * @param image Raw depth image, or pre-compressed blob (@c CV_8UC1, single row).
+	 * @param format Compression format when @p image is raw (e.g. @c ".png"); ignored if already compressed.
+	 * Uncompressed images are compressed with @ref compressImage2() before writing.
+	 */
 	void updateDepthImage(int nodeId, const cv::Mat & image, const std::string & format);
+	/**
+	 * @brief Update the laser scan stored for a node.
+	 * @param scan Uncompressed scan, or already compressed (@ref LaserScan::isCompressed()).
+	 * Uncompressed data is compressed with @ref compressData2() before writing.
+	 */
 	void updateLaserScan(int nodeId, const LaserScan & scan);
+	/** @} */
 
 public:
+	/**
+	 * @name Session exports and map artifacts
+	 * @brief Statistics, preview image, optimized poses, 2D map, mesh and FLANN index.
+	 * @{*/
 	void addInfoAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize, const ParametersMap & parameters) const;
+	/**
+	 * @brief Append a @ref Statistics record for a processed node.
+	 * @param statistics Metrics for @ref Statistics::refImageId() (insert skipped if id &le; 0 or data empty).
+	 * @param saveWmState If true and schema &ge; 0.16.2, also store compressed working-memory state.
+	 */
 	void addStatistics(const Statistics & statistics, bool saveWmState) const;
+	/**
+	 * @brief Save the map preview thumbnail in the Admin table.
+	 * @param image Raw image or JPEG-compressed blob (@c CV_8UC1, single row); empty clears it.
+	 * @note Requires database schema &ge; 0.12.0.
+	 */
 	void savePreviewImage(const cv::Mat & image) const;
+	/** @brief Load and uncompress the map preview thumbnail from the Admin table. */
 	cv::Mat loadPreviewImage() const;
+	/** @brief Persist graph-optimized poses and last localization pose. */
 	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	/** @brief Load optimized poses; optional last localization pose output. */
 	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose = 0) const;
+	/** @brief Save the assembled 2D occupancy grid and its origin metadata. */
 	void save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	/** @brief Load the 2D map and fill origin/cell size outputs. */
 	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
+	/**
+	 * @brief Persist the global optimized 3D mesh to the database.
+	 * @param cloud Point cloud (@c CV_32FC1 or @c CV_32FC3).
+	 * @param polygons Optional per-texture polygon index lists (texture → polygon → vertex indices).
+	 * @param texCoords Optional UV coordinates per texture (one per polygon vertex).
+	 * @param textures Optional concatenated square texture images (same size per texture).
+	 */
 	void saveOptimizedMesh(
 			const cv::Mat & cloud,
 			const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons = std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > >(),      // Textures -> polygons -> vertices
@@ -129,14 +225,27 @@ public:
 			std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
 #endif
 			cv::Mat * textures = 0) const;
+	/**
+	 * @brief Persist the visual word dictionary FLANN index (serialized blob).
+	 * @param indexData Serialized index from @ref VWDictionary::serializeIndex(); pass empty to clear.
+	 * @note Requires database schema &ge; 0.23.0. Used when @ref Parameters::kKpFlannIndexSaved() is enabled.
+	 */
+	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);
+	/**
+	 * @name Connection and database introspection
+	 * @brief Mutex-protected wrappers around protected \c *Query() methods.
+	 * @{*/
+	/** @brief Open or create the database at @p url (empty @p url uses an in-memory database). */
+	bool openConnection(const std::string & url, bool overwritten = false, bool readOnly = false);
+	/** @brief Close the connection; optionally flush trashes and save in-memory DB to @p outputUrl. */
 	void closeConnection(bool save = true, const std::string & outputUrl = "");
+	/** @return True if a database connection is active. */
 	bool isConnected() const;
 	unsigned long getMemoryUsed() const; // In bytes
+	/** @return Schema version string stored in the database (e.g. "0.21.0"). */
 	std::string getDatabaseVersion() const;
 	long getNodesMemoryUsed() const;
 	long getLinksMemoryUsed() const;
@@ -158,22 +267,46 @@ public:
 	std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatistics() const;
 	std::map<int, std::vector<int> > getAllStatisticsWmStates() const;
 
+	/** @} */
+
+	/**
+	 * @brief Run a SQL statement that does not return result rows.
+	 *
+	 * Executes @p sql on the open database (e.g. @c INSERT, @c UPDATE, @c DELETE,
+	 * @c CREATE, @c PRAGMA). Thread-safe wrapper around the backend's SQL execution.
+	 *
+	 * For reads, use the dedicated @ref loadSignature(), @ref loadLinks() and related
+	 * query methods instead of raw SQL.
+	 *
+	 * @param sql Complete SQL statement (SQLite syntax for @ref DBDriverSqlite3).
+	 * @note The connection must be open (@ref isConnected()). On failure the driver
+	 *       aborts with an assertion (SQLite backend).
+	 * @warning @p sql is passed verbatim; sanitize any user-controlled values before calling.
+	 */
 	void executeNoResult(const std::string & sql) const;
 
+	/**
+	 * @name Load and query
+	 * @brief Load signatures, words, links and node metadata from the database.
+	 * @{*/
+
 	// 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, bool idsOnly = false) const;
+	void loadLastNodes(std::list<Signature *> & signatures, bool loadWordIdsOnly = false) const; // returned signatures must be freed after usage
+	/** @brief Load one signature by id; caller must delete the returned pointer. */
 	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;
+	/** @brief Load outgoing links from @p signatureId, optionally filtered by @p type. */
 	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;
@@ -187,11 +320,19 @@ public:
 	void getNodesObservingLandmark(int landmarkId, std::map<int, Link> & nodes) const;
 	void getNodeIdByLabel(const std::string & label, int & id) const;
 	void getAllLabels(std::map<int, std::string> & labels) const;
+	/** @} */
 
 protected:
+	/**
+	 * @brief Protected constructor for subclasses.
+	 */
 	DBDriver(const ParametersMap & parameters = ParametersMap());
 
-	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
+	/**
+	 * @name Backend implementation (subclass responsibility)
+	 * @brief Pure virtual SQL/backend hooks invoked by public wrappers above.
+	 * @{*/
+	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 +415,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, bool idsOnly = false) 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 +428,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;
@@ -295,8 +438,14 @@ protected:
 	virtual void getNodesObservingLandmarkQuery(int landmarkId, std::map<int, Link> & nodes) const = 0;
 	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const = 0;
 	virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const = 0;
+	/** @} */
 
 private:
+	/** @brief Begin a database transaction (nested calls are serialized). */
+	void beginTransaction() const;
+	/** @brief Commit the current transaction. */
+	void commit() const;
+
 	//non-abstract methods
 	void saveOrUpdate(const std::vector<Signature *> & signatures);
 	void saveOrUpdate(const std::vector<VisualWord *> & words) const;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -30,28 +30,133 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 #include "rtabmap/core/DBDriver.h"
+#if CV_MAJOR_VERSION < 5
 #include <opencv2/features2d/features2d.hpp>
+#else
+#include <opencv2/features.hpp>
+#endif
 
 typedef struct sqlite3_stmt sqlite3_stmt;
 typedef struct sqlite3 sqlite3;
 
 namespace rtabmap {
 
+/**
+ * @class DBDriverSqlite3
+ * @brief SQLite3 implementation of @ref DBDriver for RTAB-Map map databases.
+ *
+ * This is the default driver returned by @ref DBDriver::create(). It stores signatures,
+ * links, visual words, statistics and sensor payloads in a single \c .db file using
+ * the SQLite C API.
+ *
+ * **Storage modes**
+ * - **File-backed** (default): the database is read/written directly on disk at @ref getUrl().
+ * - **In-memory**: when @ref isInMemory() is true, SQLite uses \c :memory: and the file at
+ *   @ref getUrl() (if any) is loaded at open and optionally saved on @ref closeConnection().
+ *
+ * **SQLite PRAGMA tuning** (applied on connect and when setters are called while connected):
+ * - @ref setCacheSize() — page cache size in pages
+ * - @ref setJournalMode() — rollback journal mode (DELETE … OFF)
+ * - @ref setSynchronous() — fsync policy (OFF / NORMAL / FULL)
+ * - @ref setTempStore() — storage for temporary tables and indices
+ *
+ * Configure via @ref Parameters::kDbSqlite3InMemory(), @ref Parameters::kDbSqlite3CacheSize(),
+ * @ref Parameters::kDbSqlite3JournalMode(), @ref Parameters::kDbSqlite3Synchronous() and
+ * @ref Parameters::kDbSqlite3TempStore(), or call the setters directly.
+ *
+ * @see DBDriver
+ * @see DBDriver::create()
+ */
 class RTABMAP_CORE_EXPORT DBDriverSqlite3: public DBDriver {
 public:
+	/**
+	 * @brief Construct driver with optional SQLite-specific parameters.
+	 * @param parameters Map of parameters (see class description).
+	 */
 	DBDriverSqlite3(const ParametersMap & parameters = ParametersMap());
 	virtual ~DBDriverSqlite3();
 
+	/** @brief Apply SQLite parameters from the map; forwards to @ref DBDriver::parseParameters(). */
 	virtual void parseParameters(const ParametersMap & parameters);
+	/**
+	 * @brief True when the database runs in RAM instead of on disk.
+	 * @return True if @ref getUrl() is empty or @ref setDbInMemory() was used.
+	 */
 	virtual bool isInMemory() const {return getUrl().empty() || _dbInMemory;}
+	/**
+	 * @brief Enable or disable in-memory mode.
+	 * If connected, the connection is closed and reopened with the new mode.
+	 */
 	void setDbInMemory(bool dbInMemory);
+	/**
+	 * @brief Set SQLite rollback journal mode (`PRAGMA journal_mode`).
+	 *
+	 * Controls how SQLite stores the transaction journal used for atomic commit and rollback.
+	 * See https://www.sqlite.org/pragma.html#pragma_journal_mode
+	 *
+	 * @param journalMode Accepted values (invalid values are ignored). RTAB-Map default:
+	 *   @ref Parameters::defaultDbSqlite3JournalMode() = **3 (MEMORY)** (@ref Parameters::kDbSqlite3JournalMode()).
+	 * - **0 — DELETE** (SQLite default): the journal file is deleted at the end of each transaction.
+	 *   Good general-purpose balance of safety and speed.
+	 * - **1 — TRUNCATE**: the journal is truncated to zero length instead of being unlinked; can be
+	 *   faster on some filesystems than DELETE.
+	 * - **2 — PERSIST**: the journal file is not deleted; only its header is zeroed after commit,
+	 *   reducing create/delete overhead at the cost of always keeping a journal file on disk.
+	 * - **3 — MEMORY** (RTAB-Map default): the journal is held in RAM only (not written to disk). Faster, but the
+	 *   database cannot be rolled back after a crash and may corrupt if the process dies mid-write.
+	 * - **4 — OFF**: no rollback journal. Fastest, but a crash or power loss during a write can
+	 *   leave the database inconsistent; transactions cannot be rolled back atomically.
+	 */
 	void setJournalMode(int journalMode);
+
+	/**
+	 * @brief Set the number of database pages kept in SQLite's page cache (`PRAGMA cache_size`).
+	 *
+	 * A larger cache reduces disk I/O when the working set fits in memory. The effective memory
+	 * is approximately `cacheSize * page_size` bytes (page size is usually 4096 bytes unless
+	 * changed with `PRAGMA page_size`). Only positive values are used (page count); see
+	 * https://www.sqlite.org/pragma.html#pragma_cache_size
+	 *
+	 * @param cacheSize Number of pages to cache. RTAB-Map default:
+	 *   @ref Parameters::defaultDbSqlite3CacheSize() = **10000** (@ref Parameters::kDbSqlite3CacheSize()).
+	 */
 	void setCacheSize(unsigned int cacheSize);
+
+	/**
+	 * @brief Set how aggressively SQLite syncs the database file to disk (`PRAGMA synchronous`).
+	 *
+	 * Trade-off between durability after a crash or power loss and write performance.
+	 * See https://www.sqlite.org/pragma.html#pragma_synchronous
+	 *
+	 * @param synchronous Accepted values (invalid values are ignored). RTAB-Map default:
+	 *   @ref Parameters::defaultDbSqlite3Synchronous() = **0 (OFF)** (@ref Parameters::kDbSqlite3Synchronous()).
+	 * - **0 — OFF** (RTAB-Map default): SQLite does not wait for data to reach persistent storage. Fastest; a system
+	 *   crash or power loss during a transaction may corrupt the database.
+	 * - **1 — NORMAL**: syncs at the most critical moments (SQLite default in many builds). A crash
+	 *   may lose the last transaction but the database file structure usually stays valid.
+	 * - **2 — FULL**: syncs after every transaction commit. Slowest; strongest guarantee that a
+	 *   committed transaction survives a power loss (when the OS honors fsync).
+	 */
 	void setSynchronous(int synchronous);
+
+	/**
+	 * @brief Set where SQLite stores temporary tables and indices (`PRAGMA temp_store`).
+	 *
+	 * Affects internal temp storage used for some queries and operations, not RTAB-Map map data.
+	 * See https://www.sqlite.org/pragma.html#pragma_temp_store
+	 *
+	 * @param tempStore Accepted values (invalid values are ignored). RTAB-Map default:
+	 *   @ref Parameters::defaultDbSqlite3TempStore() = **2 (MEMORY)** (@ref Parameters::kDbSqlite3TempStore()).
+	 * - **0 — DEFAULT**: use SQLite's compile-time default (often FILE, i.e. on-disk temp files).
+	 * - **1 — FILE**: store temporary tables and indices in temporary files in the directory
+	 *   given by `PRAGMA temp_store_directory` or the system temp folder.
+	 * - **2 — MEMORY** (RTAB-Map default): store temporary tables and indices in RAM. Can speed up heavy queries
+	 *   but increases memory use; large temp structures may still spill to disk depending on build.
+	 */
 	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 +240,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, bool idsOnly = false) 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 +253,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;
@@ -190,12 +298,14 @@ 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;
 
 protected:
-	sqlite3 * _ppDb;
-	std::string _version;
+	sqlite3 * _ppDb; ///< Open SQLite connection (null when disconnected)
+	std::string _version; ///< Schema version read from the database
 
 private:
 	unsigned long _memoryUsedEstimate;

corelib/include/rtabmap/core/DBReader.h

@@ -59,6 +59,8 @@ public:
 			 int startMapId = 0,
 			 int stopMapId = -1,
 			 bool priorsIgnored = false,
+			 bool imuIgnored = false,
+			 bool intermediateNodesAreNormalNodes = false,
 			 const std::vector<Transform> & cameraLocalTransformOverrides = std::vector<Transform>());
 	DBReader(const std::list<std::string> & databasePaths,
 			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
@@ -74,6 +76,8 @@ public:
 			 int startMapId = 0,
 			 int stopMapId = -1,
 			 bool priorsIgnored = false,
+			 bool imuIgnored = false,
+			 bool intermediateNodesAreNormalNodes = false,
 			 const std::vector<Transform> & cameraLocalTransformOverrides = std::vector<Transform>());
 	virtual ~DBReader();
 
@@ -104,9 +108,11 @@ private:
 	int _stopId;
 	std::vector<unsigned int> _cameraIndices;
 	bool _intermediateNodesIgnored;
+	bool _intermediateNodesAreNormalNodes;
 	bool _landmarksIgnored;
 	bool _featuresIgnored;
 	bool _priorsIgnored;
+	bool _imuIgnored;
 	int _startMapId;
 	int _stopMapId;
 	std::vector<Transform> _cameraLocalTransformOverrides;

corelib/include/rtabmap/core/EnvSensor.h

@@ -28,38 +28,67 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_
 #define CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_
 
+#include <map>
+
 namespace rtabmap {
 
+/**
+ * @class EnvSensor
+ * @brief Single environmental measurement (type, value, timestamp).
+ *
+ * Built-in @ref Type values cover common phone/robot sensors; @ref kCustomSensor1
+ * through @ref kCustomSensor9 are reserved for application-specific channels.
+ * Units depend on the type (see @ref Type).
+ *
+ * Samples are stored per node in @ref SensorData via @ref EnvSensors (one entry
+ * per type). Persisted in the database with node records (@ref DBDriver).
+ *
+ * @see SensorData::setEnvSensors()
+ * @see SensorData::addEnvSensor()
+ * @see SensorData::envSensors()
+ */
 class EnvSensor
 {
 public:
+	/**
+	 * @brief Environmental sensor channel identifier.
+	 *
+	 * Built-in types use fixed units; custom types (≥ @ref kCustomSensor1) have
+	 * application-defined meaning and units.
+	 */
 	enum Type {
-		// built-in types
-		kUndefined = 0,
-		kWifiSignalStrength,      // dBm
-		kAmbientTemperature,      // Celcius
-		kAmbientAirPressure,      // hPa
-		kAmbientLight,            // lx
-		kAmbientRelativeHumidity, // %
+		kUndefined = 0,              ///< Uninitialized / unknown channel.
+		kWifiSignalStrength,         ///< Wi‑Fi signal strength (dBm).
+		kAmbientTemperature,         ///< Ambient temperature (°C).
+		kAmbientAirPressure,         ///< Ambient air pressure (hPa).
+		kAmbientLight,               ///< Ambient illuminance (lx).
+		kAmbientRelativeHumidity,    ///< Relative humidity (%).
 
-		// user types
-		kCustomSensor1 = 100,
-		kCustomSensor2,
-		kCustomSensor3,
-		kCustomSensor4,
-		kCustomSensor5,
-		kCustomSensor6,
-		kCustomSensor7,
-		kCustomSensor8,
-		kCustomSensor9
+		kCustomSensor1 = 100,        ///< User-defined sensor slot 1.
+		kCustomSensor2,             ///< User-defined sensor slot 2.
+		kCustomSensor3,             ///< User-defined sensor slot 3.
+		kCustomSensor4,             ///< User-defined sensor slot 4.
+		kCustomSensor5,             ///< User-defined sensor slot 5.
+		kCustomSensor6,             ///< User-defined sensor slot 6.
+		kCustomSensor7,             ///< User-defined sensor slot 7.
+		kCustomSensor8,             ///< User-defined sensor slot 8.
+		kCustomSensor9              ///< User-defined sensor slot 9.
 	};
 
 public:
+	/** @brief Default constructor: @ref kUndefined type, zero value and stamp. */
 	EnvSensor() :
 		type_(kUndefined),
 		value_(0.0),
 		stamp_(0.0)
 	{}
+
+	/**
+	 * @brief Constructs a reading with the given type, value, and optional stamp.
+	 * @param type Sensor channel (@ref Type).
+	 * @param value Measurement in the units for @p type.
+	 * @param stamp Timestamp in seconds (0 if unknown).
+	 */
 	EnvSensor(const Type & type, const double & value,const double & stamp = 0) :
 		type_(type),
 		value_(value),
@@ -68,8 +97,11 @@ public:
 
 	virtual ~EnvSensor() {}
 
+	/** @return Sensor channel. */
 	const Type & type() const {return type_;}
+	/** @return Measurement value (units depend on @ref type()). */
 	const double & value() const {return value_;}
+	/** @return Timestamp in seconds. */
 	const double & stamp() const {return stamp_;}
 
 private:
@@ -78,6 +110,7 @@ private:
 	double stamp_;
 };
 
+/** @brief Map of environmental readings keyed by @ref EnvSensor::Type (at most one per type). */
 typedef std::map<EnvSensor::Type, EnvSensor> EnvSensors;
 
 }

corelib/include/rtabmap/core/EpipolarGeometry.h

@@ -31,7 +31,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Parameters.h"
 #include "rtabmap/utilite/UStl.h"
 #include <opencv2/core/core.hpp>
+#if CV_MAJOR_VERSION < 5
 #include <opencv2/features2d/features2d.hpp>
+#else
+#include <opencv2/features.hpp>
+#endif
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <list>

corelib/include/rtabmap/core/Features2d.h

@@ -32,7 +32,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/core/core.hpp>
+#if CV_MAJOR_VERSION < 5
 #include <opencv2/features2d/features2d.hpp>
+#else
+#include <opencv2/features.hpp>
+#endif
 #include <list>
 #include <numeric>
 #include "rtabmap/core/Parameters.h"
@@ -70,6 +74,10 @@ class BriefDescriptorExtractor;
 class SIFT;
 #endif
 class SURF;
+#if (CV_MAJOR_VERSION == 5)
+class BRISK;
+class KAZE;
+#endif
 }
 namespace cuda {
 class FastFeatureDetector;
@@ -89,7 +97,13 @@ typedef cv::xfeatures2d::FREAK CV_FREAK;
 typedef cv::xfeatures2d::DAISY CV_DAISY;
 typedef cv::GFTTDetector CV_GFTT;
 typedef cv::xfeatures2d::BriefDescriptorExtractor CV_BRIEF;
+#if (CV_MAJOR_VERSION < 5)
 typedef cv::BRISK CV_BRISK;
+typedef cv::KAZE CV_KAZE;
+#else
+typedef cv::xfeatures2d::BRISK CV_BRISK;
+typedef cv::xfeatures2d::KAZE CV_KAZE;
+#endif
 typedef cv::ORB CV_ORB;
 typedef cv::cuda::SURF_CUDA CV_SURF_GPU;
 typedef cv::cuda::ORB CV_ORB_GPU;
@@ -104,15 +118,33 @@ namespace rtabmap {
 
 class ORBextractor;
 class SPDetector;
+class SPDetectorRpautrat;
 
 class Stereo;
 #if CV_MAJOR_VERSION < 3
 class CV_ORB;
 #endif
 
-// Feature2D
+/**
+ * @class Feature2D
+ * @brief Abstract 2D feature detector and descriptor extractor for visual SLAM.
+ *
+ * Factory @ref create() builds a concrete detector from @ref Type or from
+ * **Kp/DetectorStrategy** in a @ref ParametersMap. Common tuning keys include
+ * **Kp/MaxFeatures**, **Kp/GridRows**, **Kp/GridCols**, **Kp/SSC**, depth filters
+ * (**Kp/MinDepth**, **Kp/MaxDepth**), ROI (**Kp/RoiRatios**), and sub-pixel refinement.
+ *
+ * Pipeline: @ref generateKeypoints() (grid + ROI + optional mask) then
+ * @ref generateDescriptors(). Static helpers filter or cap keypoints before/after
+ * matching. @ref generateKeypoints3D() projects features using stereo or depth when
+ * available in @ref SensorData.
+ *
+ * @see Memory
+ * @see RegistrationVis
+ */
 class RTABMAP_CORE_EXPORT Feature2D {
 public:
+	/** @brief Built-in detector/descriptor strategy (Kp/DetectorStrategy). */
 	enum Type {kFeatureUndef=-1,
 		kFeatureSurf=0,
 		kFeatureSift=1,
@@ -129,8 +161,11 @@ 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
+		kFeatureEnd}; // Sentinel: always keep last. Used to iterate through types.
 
+	/** @return Human-readable name for @p type (e.g. `"ORB"`, `"GFTT+BRIEF"`). */
 	static std::string typeName(Type type)
 	{
 		switch(type){
@@ -164,14 +199,24 @@ public:
 			return "GFTT+Daisy";
 		case kFeatureSurfDaisy:
 			return "SURF+Daisy";
+		case kFeaturePyDetector:
+			return "PyDetector";
+		case kFeatureSuperPointRpautrat:
+			return "SUPERPOINT-RPAUTRAT";
 		default:
 			return "Unknown";
 		}
 	}
 
+	/** @brief Creates a detector from **Kp/DetectorStrategy** in @p parameters. Caller owns the pointer. */
 	static Feature2D * create(const ParametersMap & parameters = ParametersMap());
-	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
+	/** @brief Creates a detector of the given @p type. Caller owns the pointer. */
+	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap());
 
+	/** @brief Returns true if @p type is available (RTAB-Map is built with it). */
+	static bool isAvailable(Feature2D::Type type);
+
+	/** @brief Keeps keypoints whose depth at (u,v) is in (@p minDepth, @p maxDepth). */
 	static void filterKeypointsByDepth(
 				std::vector<cv::KeyPoint> & keypoints,
 				const cv::Mat & depth,
@@ -190,6 +235,7 @@ public:
 			float minDepth,
 			float maxDepth);
 
+	/** @brief Keeps keypoints with stereo disparity ≥ @p minDisparity. */
 	static void filterKeypointsByDisparity(
 				std::vector<cv::KeyPoint> & keypoints,
 				const cv::Mat & disparity,
@@ -200,12 +246,14 @@ public:
 			const cv::Mat & disparity,
 			float minDisparity);
 
+	/** @brief Reduces keypoint count (by response or SSC spatial distribution). */
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
 	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
 	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols, bool ssc = false);
 
+	/** @brief ROI from **Kp/RoiRatios** string (`"left top right bottom"` fractions). */
 	static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
 	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
 
@@ -219,12 +267,15 @@ public:
 public:
 	virtual ~Feature2D();
 
+	/** @brief Detects keypoints in a grayscale @p image (CV_8UC1); optional depth or 8U mask. */
 	std::vector<cv::KeyPoint> generateKeypoints(
 			const cv::Mat & image,
 			const cv::Mat & mask = cv::Mat());
+	/** @brief Computes descriptors for @p keypoints (may shrink the list in some detectors). */
 	cv::Mat generateDescriptors(
 			const cv::Mat & image,
 			std::vector<cv::KeyPoint> & keypoints) const;
+	/** @brief Back-projects keypoints to 3D using depth or stereo in @p data. */
 	std::vector<cv::Point3f> generateKeypoints3D(
 			const SensorData & data,
 			const std::vector<cv::KeyPoint> & keypoints) const;
@@ -233,6 +284,15 @@ public:
 	virtual const ParametersMap & getParameters() const {return parameters_;}
 	virtual Feature2D::Type getType() const = 0;
 
+	/** @brief Returns true when a GPU/CUDA code path **could** be used by
+	 *  this detector on this host: i.e. the build was compiled with the
+	 *  matching GPU support AND a CUDA-capable device is detected at
+	 *  runtime. This is a capability probe -- it does NOT reflect whether
+	 *  the current instance is actually configured to run on GPU (that
+	 *  depends on per-detector parameters like SURF/GpuVersion). Defaults
+	 *  to false; subclasses with a GPU backend override it. */
+	virtual bool isGpuAvailable() const {return false;}
+
 protected:
 	Feature2D(const ParametersMap & parameters = ParametersMap());
 
@@ -256,19 +316,20 @@ private:
 	Stereo * _stereo;
 };
 
-//SURF
+/** @brief SURF detector and descriptor (non-free / xfeatures2d depending on OpenCV build). */
 class RTABMAP_CORE_EXPORT SURF : public Feature2D
 {
 public:
 	SURF(const ParametersMap & parameters = ParametersMap());
 	virtual ~SURF();
 
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const {return kFeatureSurf;}
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual Feature2D::Type getType() const override {return kFeatureSurf;}
+	virtual bool isGpuAvailable() const override;
 
 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;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) override;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const override;
 
 private:
 	double hessianThreshold_;
@@ -283,19 +344,20 @@ private:
 	cv::Ptr<CV_SURF_GPU> _gpuSurf;
 };
 
-//SIFT
+/** @brief SIFT detector and descriptor (optional GPU / CudaSift). */
 class RTABMAP_CORE_EXPORT SIFT : public Feature2D
 {
 public:
 	SIFT(const ParametersMap & parameters = ParametersMap());
 	virtual ~SIFT();
 
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const {return kFeatureSift;}
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual Feature2D::Type getType() const override {return kFeatureSift;}
+	virtual bool isGpuAvailable() const override;
 
 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;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) override;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const override;
 
 private:
 	int nOctaveLayers_;
@@ -305,7 +367,8 @@ private:
 	bool preciseUpscale_;
 	bool rootSIFT_;
 	bool gpu_;
-	float guaussianThreshold_;
+	float gaussianThreshold_;
+	float maxGaussianThreshold_;
 	bool upscale_;
 
 	cv::Ptr<CV_SIFT> sift_;
@@ -316,19 +379,20 @@ private:
 	bool cudaSiftUpscaling_;
 };
 
-//ORB
+/** @brief ORB detector and descriptor (optional GPU). */
 class RTABMAP_CORE_EXPORT ORB : public Feature2D
 {
 public:
 	ORB(const ParametersMap & parameters = ParametersMap());
 	virtual ~ORB();
 
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const {return kFeatureOrb;}
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual Feature2D::Type getType() const override {return kFeatureOrb;}
+	virtual bool isGpuAvailable() const override;
 
 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;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) override;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const override;
 
 private:
 	float scaleFactor_;
@@ -347,19 +411,20 @@ private:
 	cv::Ptr<CV_ORB_GPU> _gpuOrb;
 };
 
-//FAST
+/** @brief FAST corner detector only (no descriptor). */
 class RTABMAP_CORE_EXPORT FAST : public Feature2D
 {
 public:
 	FAST(const ParametersMap & parameters = ParametersMap());
 	virtual ~FAST();
 
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const {return kFeatureUndef;}
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual Feature2D::Type getType() const override {return kFeatureUndef;}
+	virtual bool isGpuAvailable() const override;
 
 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 &, std::vector<cv::KeyPoint> &) const {return cv::Mat();}
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) override;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat &, std::vector<cv::KeyPoint> &) const override {return cv::Mat();}
 
 private:
 	int threshold_;
@@ -383,18 +448,18 @@ private:
 	cv::Ptr<CV_FAST_GPU> _gpuFast;
 };
 
-//FAST_BRIEF
+/** @brief FAST corners + BRIEF descriptors. */
 class RTABMAP_CORE_EXPORT FAST_BRIEF : public FAST
 {
 public:
 	FAST_BRIEF(const ParametersMap & parameters = ParametersMap());
 	virtual ~FAST_BRIEF();
 
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const {return kFeatureFastBrief;}
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual Feature2D::Type getType() const override {return kFeatureFastBrief;}
 
 private:
-	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const override;
 
 private:
 	int bytes_;
@@ -402,18 +467,18 @@ private:
 	cv::Ptr<CV_BRIEF> _brief;
 };
 
-//FAST_FREAK
+/** @brief FAST corners + FREAK descriptors. */
 class RTABMAP_CORE_EXPORT FAST_FREAK : public FAST
 {
 public:
 	FAST_FREAK(const ParametersMap & parameters = ParametersMap());
 	virtual ~FAST_FREAK();
 
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const {return kFeatureFastFreak;}
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual Feature2D::Type getType() const override {return kFeatureFastFreak;}
 
 private:
-	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const override;
 
 private:
 	bool orientationNormalized_;
@@ -424,17 +489,18 @@ private:
 	cv::Ptr<CV_FREAK> _freak;
 };
 
-//GFTT
+/** @brief Good-features-to-track detector (Shi–Tomasi / Harris). */
 class RTABMAP_CORE_EXPORT GFTT : public Feature2D
 {
 public:
 	GFTT(const ParametersMap & parameters = ParametersMap());
 	virtual ~GFTT();
 
-	virtual void parseParameters(const ParametersMap & parameters);
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual bool isGpuAvailable() const override;
 
 private:
-	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) override;
 
 private:
 	double _qualityLevel;
@@ -448,18 +514,18 @@ private:
 	cv::Ptr<CV_GFTT_GPU> _gpuGftt;
 };
 
-//GFTT_BRIEF
+/** @brief GFTT corners + BRIEF descriptors. */
 class RTABMAP_CORE_EXPORT GFTT_BRIEF : public GFTT
 {
 public:
 	GFTT_BRIEF(const ParametersMap & parameters = ParametersMap());
 	virtual ~GFTT_BRIEF();
 
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const {return kFeatureGfttBrief;}
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual Feature2D::Type getType() const override {return kFeatureGfttBrief;}
 
 private:
-	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const override;
 
 private:
 	int bytes_;
@@ -467,18 +533,18 @@ private:
 	cv::Ptr<CV_BRIEF> _brief;
 };
 
-//GFTT_FREAK
+/** @brief GFTT corners + FREAK descriptors. */
 class RTABMAP_CORE_EXPORT GFTT_FREAK : public GFTT
 {
 public:
 	GFTT_FREAK(const ParametersMap & parameters = ParametersMap());
 	virtual ~GFTT_FREAK();
 
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const {return kFeatureGfttFreak;}
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual Feature2D::Type getType() const override {return kFeatureGfttFreak;}
 
 private:
-	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const override;
 
 private:
 	bool orientationNormalized_;
@@ -489,18 +555,18 @@ private:
 	cv::Ptr<CV_FREAK> _freak;
 };
 
-//SURF_FREAK
+/** @brief SURF detector + FREAK descriptors. */
 class RTABMAP_CORE_EXPORT SURF_FREAK : public SURF
 {
 public:
 	SURF_FREAK(const ParametersMap & parameters = ParametersMap());
 	virtual ~SURF_FREAK();
 
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const {return kFeatureSurfFreak;}
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual Feature2D::Type getType() const override {return kFeatureSurfFreak;}
 
 private:
-	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const override;
 
 private:
 	bool orientationNormalized_;
@@ -511,36 +577,36 @@ private:
 	cv::Ptr<CV_FREAK> _freak;
 };
 
-//GFTT_ORB
+/** @brief GFTT corners + ORB descriptors (common default when SURF is unavailable). */
 class RTABMAP_CORE_EXPORT GFTT_ORB : public GFTT
 {
 public:
 	GFTT_ORB(const ParametersMap & parameters = ParametersMap());
 	virtual ~GFTT_ORB();
 
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const {return kFeatureGfttOrb;}
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual Feature2D::Type getType() const override {return kFeatureGfttOrb;}
 
 private:
-	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const override;
 
 private:
 	ORB _orb;
 };
 
-//BRISK
+/** @brief BRISK detector and descriptor. */
 class RTABMAP_CORE_EXPORT BRISK : public Feature2D
 {
 public:
 	BRISK(const ParametersMap & parameters = ParametersMap());
 	virtual ~BRISK();
 
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const {return kFeatureBrisk;}
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual Feature2D::Type getType() const override {return kFeatureBrisk;}
 
 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;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) override;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const override;
 
 private:
 	int thresh_;
@@ -550,19 +616,19 @@ private:
 	cv::Ptr<CV_BRISK> brisk_;
 };
 
-//KAZE
+/** @brief KAZE detector and descriptor (OpenCV 3+). */
 class RTABMAP_CORE_EXPORT KAZE : public Feature2D
 {
 public:
 	KAZE(const ParametersMap & parameters = ParametersMap());
 	virtual ~KAZE();
 
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const { return kFeatureKaze; }
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual Feature2D::Type getType() const override { return kFeatureKaze; }
 
 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;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) override;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const override;
 
 private:
 	bool extended_;
@@ -573,23 +639,23 @@ private:
 	int diffusivity_;
 
 #if CV_MAJOR_VERSION > 2
-	cv::Ptr<cv::KAZE> kaze_;
+	cv::Ptr<CV_KAZE> kaze_;
 #endif
 };
 
-//ORB OCTREE
+/** @brief ORB with octree spatial distribution (RTAB-Map must be built with OCTREE enabled). */
 class RTABMAP_CORE_EXPORT ORBOctree : public Feature2D
 {
 public:
 	ORBOctree(const ParametersMap & parameters = ParametersMap());
 	virtual ~ORBOctree();
 
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const {return kFeatureOrbOctree;}
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual Feature2D::Type getType() const override {return kFeatureOrbOctree;}
 
 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;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) override;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const override;
 
 private:
 	float scaleFactor_;
@@ -603,19 +669,20 @@ private:
 	cv::Mat descriptors_;
 };
 
-//SuperPointTorch
+/** @brief SuperPoint via LibTorch (RTAB-Map must be built with libtorch support). */
 class RTABMAP_CORE_EXPORT SuperPointTorch : public Feature2D
 {
 public:
 	SuperPointTorch(const ParametersMap & parameters = ParametersMap());
 	virtual ~SuperPointTorch();
 
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const { return kFeatureSuperPointTorch; }
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual Feature2D::Type getType() const override { return kFeatureSuperPointTorch; }
+	virtual bool isGpuAvailable() const override;
 
 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;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) override;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const override;
 
 	cv::Ptr<SPDetector> superPoint_;
 
@@ -626,18 +693,44 @@ private:
 	bool cuda_;
 };
 
-//GFTT_DAISY
+/** @brief SuperPoint (rpautrat) via Torch + Python (RTAB-Map must be built with libtorch and Python support). */
+class RTABMAP_CORE_EXPORT SuperPointRpautrat : public Feature2D
+{
+public:
+	SuperPointRpautrat(const ParametersMap & parameters = ParametersMap());
+	virtual ~SuperPointRpautrat();
+
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual Feature2D::Type getType() const override { return kFeatureSuperPointRpautrat; }
+	virtual bool isGpuAvailable() const override;
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) override;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const override;
+
+	cv::Ptr<SPDetectorRpautrat> superPoint_;
+
+	std::string superpointWeightsPath_;
+	std::string superpointModelPath_;
+	std::string outputDir_;
+	float threshold_;
+	bool nms_;
+	int minDistance_;
+	bool cuda_;
+};
+
+/** @brief GFTT corners + DAISY descriptors (OpenCV 3+ xfeatures2d). */
 class RTABMAP_CORE_EXPORT GFTT_DAISY : public GFTT
 {
 public:
 	GFTT_DAISY(const ParametersMap & parameters = ParametersMap());
 	virtual ~GFTT_DAISY();
 
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const {return kFeatureGfttDaisy;}
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual Feature2D::Type getType() const override {return kFeatureGfttDaisy;}
 
 private:
-	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const override;
 
 private:
 	bool orientationNormalized_;
@@ -650,18 +743,18 @@ private:
 #endif
 };
 
-//SURF_DAISY
+/** @brief SURF detector + DAISY descriptors (OpenCV 3+ xfeatures2d). */
 class RTABMAP_CORE_EXPORT SURF_DAISY : public SURF
 {
 public:
 	SURF_DAISY(const ParametersMap & parameters = ParametersMap());
 	virtual ~SURF_DAISY();
 
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const {return kFeatureSurfDaisy;}
+	virtual void parseParameters(const ParametersMap & parameters) override;
+	virtual Feature2D::Type getType() const override {return kFeatureSurfDaisy;}
 
 private:
-	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const override;
 
 private:
 	bool orientationNormalized_;

corelib/include/rtabmap/core/FlannIndex.h

@@ -34,51 +34,174 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+class NanoFlannIndex;
+
+/**
+ * @class FlannIndex
+ * @brief Nearest neighbor index over a set of features
+ *
+ * Wraps the search structures of the vendored rtflann and nanoflann libraries
+ * behind one interface, the structure being chosen with flann_algorithm_t at
+ * build time. Used for the visual word dictionary (VWDictionary) and for the
+ * 2D point searches of visual registration (RegistrationVis).
+ *
+ * The features are not copied: the index refers to the matrices it is given and
+ * keeps them alive, cv::Mat data being reference counted, so they must not be
+ * modified in place while it is in use. Every point it holds is
+ * designated by an index, assigned in the order the points were added and
+ * stable for the lifetime of the index: removePoint() leaves a hole rather
+ * than renumbering the points after it.
+ */
 class RTABMAP_CORE_EXPORT FlannIndex
 {
 public:
+	/**
+	 * @enum flann_algorithm_t
+	 * @brief The index structure built by buildIndex()
+	 *
+	 * The values under 8 are forwarded from rtflann's own enum and have to
+	 * match it (see src/rtflann/defines.h); the nanoflann ones are
+	 * rtabmap-specific and kept outside its range (0-7, 254, 255). A value is
+	 * written in the serialized index header and checked back on load, so none
+	 * of them may be renumbered.
+	 *
+	 * The nanoflann structures take float features only (nanoflann has no
+	 * Hamming metric) and search exactly, ignoring "checks". That makes them
+	 * the fastest ones for 2D and 3D points, and the wrong ones for
+	 * descriptors: an exact search visits more and more of the tree as the
+	 * dimension grows, down to being as slow as an exhaustive search. Prefer
+	 * the approximate rtflann kd-trees for those.
+	 */
+	enum flann_algorithm_t
+	{
+		FLANN_INDEX_LINEAR 			= 0, ///< Exhaustive search
+		FLANN_INDEX_KDTREE 			= 1, ///< 4 randomized kd-trees, searched approximately
+		FLANN_INDEX_KDTREE_SINGLE   = 4, ///< Single kd-tree, searched exactly
+		FLANN_INDEX_LSH 			= 6, ///< Locality-Sensitive Hashing (binary descriptors)
+
+		/// nanoflann kd-tree. With a rebalancing factor of 1 it is built once,
+		/// which is the cheapest to build and to search; over 1 it is the
+		/// weight-balanced tree accepting addPoints()/removePoint(), which
+		/// cannot be serialized while some of its points are removed.
+		NANOFLANN_INDEX_KDTREE_SINGLE = 100,
+	};
+
 	FlannIndex();
 	virtual ~FlannIndex();
 
+	/** @brief Drop the index and everything it holds, back to the state of a new one. */
 	void release();
+
+	/**
+	 * @brief Serialize the index, to be given back to loadIndex()
+	 * @param computeChecksum Add a checksum of the indexed features to the
+	 *        data, which loadIndex() compares against the features it is given
+	 * @return The serialized index, empty when there is nothing to serialize or
+	 *         when the structure in use cannot be
+	 *
+	 * The format depends on the architecture and on the versions of the
+	 * vendored libraries: loadIndex() refuses an index it cannot read, leaving
+	 * it to be rebuilt.
+	 */
+	std::vector<unsigned char> serializeIndex(bool computeChecksum = true) const;
+
+	/** @return Number of indexed features, the removed ones excluded. */
 	size_t indexedFeatures() const;
 
-	// return Bytes
+	/**
+	 * @return Bytes used by the index, the features themselves excluded as
+	 *         they are only referred to.
+	 */
 	size_t memoryUsed() const;
 
-	// Note that useDistanceL1 doesn't have any effect if LSH is used
-	void buildLinearIndex(
+	/**
+	 * @brief Build the index over the given features, releasing any previous one
+	 * @param algorithm The structure to build
+	 * @param features One feature per row, CV_32FC1 or, for the rtflann
+	 *        structures only, CV_8UC1 for binary descriptors (Hamming distance)
+	 * @param useDistanceL1 Search with the L1 distance instead of L2, ignored
+	 *        by LSH and by the binary descriptors
+	 * @param rebalancingFactor Fraction (factor-1)/factor of the index that can
+	 *        be left removed before it is rebuilt, e.g. half of it for 2. Set
+	 *        to 1 to never rebuild it.
+	 */
+	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);
 
+	/**
+	 * @brief Load an index serialized by serializeIndex(), releasing any previous one
+	 * @param indexData The serialized index
+	 * @param algorithm The structure it was built with
+	 * @param features The very same features it was built with, in the same
+	 *        order: the index refers to them by their row
+	 * @param useDistanceL1 The distance it was built with
+	 * @param rebalancingFactor See buildIndex(). The serialized data carries the
+	 *        one the index was built with, which is deprecated and ignored:
+	 *        this one is used instead.
+	 * @param errorMsg Filled with what didn't match when the index is refused
+	 * @return False if the data doesn't correspond to the given features and
+	 *         parameters, in which case the index is left released
+	 */
+	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);
+	/** @brief Load an index from a raw buffer, see the overload above. */
+	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);
+
+	/** @return Whether an index has been built or loaded. */
 	bool isBuilt();
 
+	/** @return Type of the indexed features (CV_32FC1 or CV_8UC1). */
 	int featuresType() const {return featuresType_;}
+	/** @return Dimension of the indexed features. */
 	int featuresDim() const {return featuresDim_;}
 
+	/**
+	 * @brief Add features to the index
+	 * @param features One feature per row, of the type and dimension the index
+	 *        was built with
+	 * @return The index assigned to each of them, empty when the structure
+	 *         doesn't accept points after it is built
+	 */
 	std::vector<unsigned int> addPoints(const cv::Mat & features);
 
+	/**
+	 * @brief Remove an indexed feature, by the index addPoints() gave for it
+	 *
+	 * The feature is only marked as removed: it is skipped by the searches, but
+	 * keeps taking memory until the index is rebuilt (see the rebalancing
+	 * factor of buildIndex()). Not supported by every structure.
+	 */
 	void removePoint(unsigned int index);
 
-	// return squared distances (indices should be casted in size_t)
+	/**
+	 * @brief Search the k nearest neighbors of each query
+	 * @param query One feature per row, of the type and dimension the index was
+	 *        built with
+	 * @param indices Neighbors found, one query per row, CV_32SC1. The
+	 *        neighbors that couldn't be found are set to -1.
+	 * @param dists Their squared distances, CV_32FC1, or CV_32SC1 for the
+	 *        Hamming distances of binary descriptors
+	 * @param knn Number of neighbors to search for
+	 * @param checks Number of leaves an approximate search visits, the exact
+	 *        structures ignoring it
+	 * @param eps Search for eps-approximate neighbors
+	 * @param sorted Give the neighbors back by increasing distance
+	 */
 	void knnSearch(
 			const cv::Mat & query,
 			cv::Mat & indices,
@@ -88,7 +211,21 @@ public:
 			float eps = 0.0,
 			bool sorted = true) const;
 
-	// return squared distances
+	/**
+	 * @brief Search the neighbors of each query within a radius
+	 * @param query One feature per row, of the type and dimension the index was
+	 *        built with
+	 * @param indices Neighbors found, one vector per query
+	 * @param dists Their squared distances, one vector per query
+	 * @param radius Search radius, squared internally: it is a distance, not a
+	 *        squared one
+	 * @param maxNeighbors Maximum number of neighbors per query, the nearest
+	 *        ones being kept. 0 for all of them.
+	 * @param checks Number of leaves an approximate search visits, the exact
+	 *        structures ignoring it
+	 * @param eps Search for eps-approximate neighbors
+	 * @param sorted Give the neighbors back by increasing distance
+	 */
 	void radiusSearch(
 			const cv::Mat & query,
 			std::vector<std::vector<size_t> > & indices,
@@ -100,13 +237,14 @@ public:
 			bool sorted = true) const;
 
 private:
-	void * index_;
+	void * index_;               // rtflann backend
+	NanoFlannIndex * nanoIndex_; // nanoflann backend, only one of the two is set
 	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)

corelib/include/rtabmap/core/GPS.h

@@ -32,9 +32,21 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class GPS
+ * @brief WGS84 GPS fix attached to a sensor sample or graph node.
+ *
+ * Stores a timestamped geographic position with horizontal accuracy and bearing.
+ * Values are persisted in the database as six doubles in the order
+ * @c stamp, @c longitude, @c latitude, @c altitude, @c error, @c bearing.
+ *
+ * @see SensorData::gps()
+ * @see GeodeticCoords
+ */
 class GPS
 {
 public:
+	/** @brief Default-constructs a fix at the origin with zero stamp and error. */
 	GPS():
 		stamp_(0.0),
 		longitude_(0.0),
@@ -43,6 +55,15 @@ public:
 		error_(0.0),
 		bearing_(0.0)
 	{}
+	/**
+	 * @brief Constructs a GPS fix.
+	 * @param stamp Timestamp in seconds.
+	 * @param longitude Longitude in decimal degrees (DD, east positive).
+	 * @param latitude Latitude in decimal degrees (DD, north positive).
+	 * @param altitude Altitude in meters above the WGS84 ellipsoid.
+	 * @param error Horizontal position error radius in meters.
+	 * @param bearing Heading in degrees, 0 = north, increasing clockwise.
+	 */
 	GPS(const double & stamp,
 		const double & longitude,
 		const double & latitude,
@@ -56,13 +77,22 @@ public:
 			error_(error),
 			bearing_(bearing)
 	{}
+	/** @return Timestamp in seconds. */
 	const double & stamp() const {return stamp_;}
+	/** @return Longitude in decimal degrees (DD). */
 	const double & longitude() const {return longitude_;}
+	/** @return Latitude in decimal degrees (DD). */
 	const double & latitude() const {return latitude_;}
+	/** @return Altitude in meters. */
 	const double & altitude() const {return altitude_;}
+	/** @return Horizontal position error in meters. */
 	const double & error() const {return error_;}
+	/** @return Bearing in degrees (north = 0, clockwise). */
 	const double & bearing() const {return bearing_;}
 
+	/**
+	 * @return @ref GeodeticCoords built from @ref latitude(), @ref longitude() and @ref altitude().
+	 */
 	GeodeticCoords toGeodeticCoords() const {return GeodeticCoords(latitude_, longitude_, altitude_);}
 private:
 	double stamp_;     // in sec

corelib/include/rtabmap/core/GeodeticCoords.h

@@ -49,27 +49,61 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class GeodeticCoords
+ * @brief WGS84 geodetic latitude, longitude and altitude with coordinate conversions.
+ *
+ * Conversions follow the WGS84 reference ellipsoid (MRPT-derived implementation).
+ * ENU frames use east = X, north = Y, up = Z relative to a local origin.
+ *
+ * @see GPS::toGeodeticCoords()
+ */
 class RTABMAP_CORE_EXPORT GeodeticCoords
 {
 public:
+	/** @brief Default-constructs coordinates at (0°, 0°, 0 m). */
 	GeodeticCoords();
+	/**
+	 * @brief Constructs geodetic coordinates.
+	 * @param latitude Latitude in decimal degrees (DD, north positive).
+	 * @param longitude Longitude in decimal degrees (DD, east positive).
+	 * @param altitude Altitude in meters above the WGS84 ellipsoid.
+	 */
 	GeodeticCoords(double latitude, double longitude, double altitude);
 
+	/** @return Latitude in decimal degrees. */
 	const double & latitude() const {return latitude_;}
+	/** @return Longitude in decimal degrees. */
 	const double & longitude() const {return longitude_;}
+	/** @return Altitude in meters. */
 	const double & altitude() const {return altitude_;}
 
+	/** @brief Sets latitude in decimal degrees. */
 	void setLatitude(const double & value) {latitude_ = value;}
+	/** @brief Sets longitude in decimal degrees. */
 	void setLongitude(const double & value) {longitude_ = value;}
+	/** @brief Sets altitude in meters. */
 	void setAltitude(const double & value) {altitude_ = value;}
 
+	/** @return ECEF geocentric coordinates (meters) in the WGS84 frame. */
 	cv::Point3d toGeocentric_WGS84() const;
+	/**
+	 * @return ENU offset (meters) from @p origin to this point.
+	 * East = X, north = Y, up = Z.
+	 */
 	cv::Point3d toENU_WGS84(const GeodeticCoords & origin) const; // East=X, North=Y
 
+	/** @brief Sets this point from ECEF geocentric @p geocentric coordinates. */
 	void fromGeocentric_WGS84(const cv::Point3d& geocentric);
+	/** @brief Sets this point from an ENU offset relative to @p origin. */
 	void fromENU_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
 
+	/** @return ECEF geocentric coordinates of ENU point @p enu relative to @p origin. */
 	static cv::Point3d ENU_WGS84ToGeocentric_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
+	/**
+	 * @return ENU offset from geocentric @p origin_geocentric_WGS84 to @p geocentric_WGS84.
+	 * @param origin Geodetic origin used to define the local ENU basis.
+	 */
 	static cv::Point3d Geocentric_WGS84ToENU_WGS84(
 			const cv::Point3d & geocentric_WGS84,
 			const cv::Point3d & origin_geocentric_WGS84,