Updated Multisession3ItMemoryThr flaky test checks

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/focal/Dockerfile

@@ -15,7 +15,5 @@ RUN set -ex && \
     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/Dockerfile

@@ -15,7 +15,5 @@ RUN set -ex && \
     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/latest-deps-from-source/Dockerfile

@@ -34,6 +34,7 @@ RUN set -ex && \
 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}/
 

.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

@@ -19,7 +19,5 @@ RUN set -ex && \
     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

@@ -4,7 +4,7 @@
     },
     "customizations": {
         "vscode": {
-          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "ms-vscode.cpptools-extension-pack"]
         }
       },
     "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
@@ -14,5 +14,17 @@
       "terminal.integrated.defaultProfile.linux": "bash"
     },
     "remoteUser": "vscode",
-    "runArgs": ["--privileged", "--network=host"]
+    "hostRequirements": {
+        "gpu": "optional"
+    },
+    "runArgs": ["--privileged",
+                "--network=host",
+                "--gpus=all",
+                //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer
+                "--env=DISPLAY",
+                "--env=QT_X11_NO_MITSHM=1",
+                "--volume=/tmp/.X11-unix:/tmp/.X11-unix"],
+    "containerEnv": {
+        "NVIDIA_VISIBLE_DEVICES": "all"
+    }
 }

.devcontainer/resolute/Dockerfile

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

.devcontainer/resolute/devcontainer.json

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

.devcontainer/rolling/Dockerfile

@@ -6,11 +6,12 @@ ENV DEBIAN_FRONTEND=noninteractive
 # Install ROS2
 RUN apt update && \
     apt install software-properties-common -y && \
-    add-apt-repository universe && \
+    add-apt-repository universe -y && \
     apt update && \
     apt install curl -y && \
-    curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \
+    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}') && \
-    echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \
+    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
@@ -76,4 +77,4 @@ RUN set -ex && \
 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

@@ -14,5 +14,17 @@
       "terminal.integrated.defaultProfile.linux": "bash"
     },
     "remoteUser": "vscode",
-    "runArgs": ["--privileged", "--network=host"]
+    "hostRequirements": {
+        "gpu": "optional"
+    },
+    "runArgs": ["--privileged",
+                "--network=host",
+                "--gpus=all",
+                //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer
+                "--env=DISPLAY",
+                "--env=QT_X11_NO_MITSHM=1",
+                "--volume=/tmp/.X11-unix:/tmp/.X11-unix"],
+    "containerEnv": {
+        "NVIDIA_VISIBLE_DEVICES": "all"
+    }
 }

.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

@@ -7,68 +7,94 @@ on:
   pull_request:
     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
+    name: ${{ matrix.ros_distribution }}
-    # well on Windows or Mac.  You can convert this to a matrix build if you need
+    runs-on: ubuntu-latest
-    # cross-platform coverage.
+    concurrency:
-    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
+      group: ${{ github.workflow }}-${{ github.ref }}-${{ matrix.ros_distribution }}
-    name: Build on ros ${{ matrix.ros_distribution }} and ${{ matrix.os }}
+      cancel-in-progress: true
-    runs-on: ${{ matrix.os }}
     strategy:
       fail-fast: false
       matrix:
-        ros_distribution: [ humble, jazzy, kilted, rolling ]
+        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'
-          os: ubuntu-24.04
+          skip_keys: ""
+        - ros_distribution: 'lyrical'
+          skip_keys: "libpointmatcher"
         - ros_distribution: 'rolling'
-          os: ubuntu-24.04
+          skip_keys: "libpointmatcher gtsam"
-
+          use_ros2_testing: true # Rolling is using ros2-testing (nightly)
+    container:
+      image: osrf/ros:${{ matrix.ros_distribution }}-desktop-full
     steps:
-    - name: Setup ROS2
+    - uses: actions/checkout@v4
-      # https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html
+
+    - 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: |
-        sudo apt install software-properties-common
+        # The ROS images are slim; fetch_test_data.sh needs curl, and 7-Zip for
-        sudo add-apt-repository universe
+        # the assets that come as an archive. The package holding it is
-        sudo apt update && sudo apt install curl -y
+        # p7zip-full up to Ubuntu 24.04 and 7zip on the newer ones.
-        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}')
+        command -v curl >/dev/null || (apt-get update && apt-get install -y --no-install-recommends curl)
-        curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo $VERSION_CODENAME)_all.deb"
+        command -v 7z >/dev/null || command -v 7zz >/dev/null || (apt-get update && \
-        sudo apt install /tmp/ros2-apt-source.deb
+          (apt-get install -y --no-install-recommends p7zip-full || \
-        sudo apt update
+           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"
 
     - uses: ros-tooling/setup-ros@v0.7
       with:
         required-ros-distributions: ${{ matrix.ros_distribution }}
-    
+        use-ros2-testing: ${{ matrix.use_ros2_testing || false }}
-    - uses: actions/checkout@v4
+    - uses: ros-tooling/action-ros-ci@v0.4
-
+      with:
-    - name: Install dependencies
+        package-name: rtabmap
-      run: |
+        target-ros2-distro: ${{ matrix.ros_distribution }}
-        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
+        rosdep-skip-keys: "${{ matrix.skip_keys }}"
-        rosdep update
+        coverage-result: false
-        rosdep install --from-paths ${{github.workspace}} -y
+        extra-cmake-args: -DTEST_DATA_ROOT=${{ steps.testdata.outputs.root }}
-
-    - 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
-      

.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,56 +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]
-        include:
-        - os: ubuntu-22.04
-          extra_deps: "libunwind-dev libceres-dev"
-          extra_cmake_def: ""
-        - os: ubuntu-24.04
-          extra_deps: "libg2o-dev libceres-dev"
-          extra_cmake_def: "-DWITH_CERES=ON"
-
-    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 ${{ matrix.extra_deps }}
-
-    - uses: actions/checkout@v4
-
-    - name: Configure CMake
-      run: |
-        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{ matrix.extra_cmake_def }}
-
-    - 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, noble-deps, noble-kilted-deps]
+        docker_tag: [focal-deps, jammy-deps, noble-deps, noble-kilted-deps, resolute-deps]
         include:
         - docker_tag: focal-deps
           docker_tags: |
@@ -52,6 +62,13 @@ jobs:
             linux/amd64
             linux/arm64
           docker_path: 'noble-kilted/deps'
+        - docker_tag: resolute-deps
+          docker_tags: |
+            introlab3it/rtabmap:resolute-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'resolute/deps'
     
     steps:
       -
@@ -85,12 +102,15 @@ jobs:
   
   docker:
     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, noble, noble-kilted, android23, android24, android26, android30]
+        docker_tag: [bionic, focal, jammy, noble, noble-kilted, resolute, android23, android24, android26, android30]
         include:
         - docker_tag: bionic
           docker_tags: |
@@ -142,6 +162,16 @@ jobs:
             linux/amd64
             linux/arm64
           docker_path: 'noble-kilted'
+        - docker_tag: resolute
+          docker_tags: |
+            introlab3it/rtabmap:resolute
+            introlab3it/rtabmap:26.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'resolute'
         - docker_tag: android23
           docker_tags: |
             introlab3it/rtabmap:android23
@@ -190,6 +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
+          push: ${{ github.event_name != 'pull_request' }}
-          platforms: ${{ matrix.docker_platforms }}
+          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")
@@ -20,10 +22,18 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 23)
-SET(RTABMAP_PATCH_VERSION 2)
+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}")
 
 SET(PROJECT_VERSION_MAJOR ${RTABMAP_MAJOR_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,6 +263,7 @@ option(WITH_CCCORELIB     "Include CCCoreLib support"            OFF)
 option(WITH_OPEN3D        "Include Open3D support"               OFF)
 option(WITH_LOAM          "Include LOAM support"                 OFF)
 option(WITH_FLOAM         "Include FLOAM support"                OFF)
+option(WITH_LIOSAM        "Include LIO-SAM support"              OFF)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 option(WITH_ZED           "Include ZED sdk support"              ON)
 option(WITH_ZEDOC         "Include ZED Open Capture support"     ON)
@@ -205,7 +271,7 @@ option(WITH_REALSENSE     "Include RealSense support"            ON)
 option(WITH_REALSENSE_SLAM "Include RealSenseSlam 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)
@@ -226,6 +292,8 @@ 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()
@@ -236,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)
@@ -533,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)
@@ -625,6 +721,12 @@ IF(WITH_FLOAM)
        FIND_PACKAGE(Ceres REQUIRED)
     ENDIF(floam_FOUND)
 ENDIF(WITH_FLOAM)
+IF(WITH_LIOSAM)
+    find_package(lio_sam QUIET)
+    IF(lio_sam_FOUND)
+       MESSAGE(STATUS "Found lio_sam: ${lio_sam_INCLUDE_DIRS}")
+    ENDIF(lio_sam_FOUND)
+ENDIF(WITH_LIOSAM)
 
 SET(ZED_FOUND FALSE)
 IF(WITH_ZED)
@@ -637,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)
@@ -689,7 +792,9 @@ IF(WITH_MYNTEYE)
 ENDIF(WITH_MYNTEYE)
 
 IF(WITH_DEPTHAI)
-    FIND_PACKAGE(depthai 2.24)
+    # 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)
@@ -782,8 +887,6 @@ IF(WITH_OKVIS)
        MESSAGE(STATUS "Found okvis: ${OKVIS_INCLUDE_DIRS}")
        find_package(brisk 2 REQUIRED)
        MESSAGE(STATUS "Found brisk: ${BRISK_INCLUDE_DIRS}")
-       find_package(opengv REQUIRED)
-       MESSAGE(STATUS "Found opengv: ${OPENGV_INCLUDE_DIRS}")
        find_package(Ceres 1.9.0 REQUIRED EXACT) # OKVIS requires this specific version
        MESSAGE(STATUS "Found ceres ${Ceres_VERSION}: ${CERES_INCLUDE_DIRS}")
     ENDIF(okvis_FOUND)
@@ -822,22 +925,7 @@ IF(WITH_VINS_FUSION)
 ENDIF(WITH_VINS_FUSION)
 
 IF(WITH_OPENVINS)
-    FIND_PACKAGE(ov_msckf)
+   FIND_PACKAGE(OpenVINS)
-    # On ROS2, the indirect includes and libraries
-    # are not forwarded by ov_msckf target, append them manually
-    FIND_PACKAGE(ov_core)
-    FIND_PACKAGE(ov_init)
-    IF(ov_msckf_FOUND AND ov_core_FOUND AND ov_init_FOUND)
-      SET(ov_msckf_INCLUDE_DIRS
-         ${ov_msckf_INCLUDE_DIRS}
-         ${ov_core_INCLUDE_DIRS}
-         ${ov_init_INCLUDE_DIRS})
-      SET(ov_msckf_LIBRARIES
-         ${ov_msckf_LIBRARIES}
-         ${ov_core_LIBRARIES}
-         ${ov_init_LIBRARIES})
-      MESSAGE(STATUS "Found OpenVINS: ${ov_msckf_INCLUDE_DIRS}")
-    ENDIF()
 ENDIF(WITH_OPENVINS)
 
 IF(WITH_FASTCV)
@@ -847,12 +935,75 @@ IF(WITH_FASTCV)
     ENDIF(FastCV_FOUND)
 ENDIF(WITH_FASTCV)
 
-IF(WITH_OPENGV)
+IF(WITH_APRILTAG)
-    FIND_PACKAGE(opengv QUIET)
+    FIND_PACKAGE(apriltag QUIET)
-    IF(opengv_FOUND)
+    IF(apriltag_FOUND)
-       MESSAGE(STATUS "Found OpenGV: ${opengv_INCLUDE_DIRS}")
+      get_target_property(APRILTAG_LOCATION apriltag::apriltag LOCATION)
-    ENDIF(opengv_FOUND)
+      get_target_property(APRILTAG_INCLUDES apriltag::apriltag INTERFACE_INCLUDE_DIRECTORIES)
-ENDIF(WITH_OPENGV)
+      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)
@@ -862,7 +1013,7 @@ IF(WITH_ORB_SLAM AND NOT G2O_FOUND)
 ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND)
 
 IF(WITH_CUVSLAM)
-    FIND_PACKAGE(CuVSLAM)
+    FIND_PACKAGE(CuVSLAM 14.0.0)
     IF(CUVSLAM_FOUND)
         MESSAGE(STATUS "Found cuVSLAM: ${CUVSLAM_INCLUDE_DIRS}")
     ENDIF()
@@ -874,7 +1025,7 @@ IF(NOT (APPLE OR WIN32) AND BUILD_WITH_RPATH_NOT_RUNPATH)
 ENDIF()
 
 IF(NOT MSVC)
-    IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1) OR TORCH_FOUND OR MRPT_FOUND)
+    IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1) OR (GTSAM_FOUND AND GTSAM_VERSION VERSION_GREATER_EQUAL "4.3.0") OR TORCH_FOUND OR MRPT_FOUND)
       # Qt6 requires c++17
       include(CheckCXXCompilerFlag)
       CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17)
@@ -1001,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)
@@ -1019,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()
@@ -1134,7 +1294,7 @@ ENDIF()
 IF(NOT vins_FOUND)
    SET(VINSFUSION "//")
 ENDIF()
-IF(NOT ov_msckf_FOUND)
+IF(NOT OpenVINS_FOUND)
    SET(OPENVINS "//")
 ENDIF()
 IF(NOT CUVSLAM_FOUND)
@@ -1166,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 )
 
@@ -1241,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"
@@ -1283,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}")
@@ -1376,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}")
@@ -1397,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()
@@ -1453,15 +1654,26 @@ MESSAGE(STATUS "  With ORB OcTree           = NO (WITH_ORB_OCTREE=OFF)")
 ENDIF()
 
 IF(TORCH_FOUND)
-MESSAGE(STATUS "  With SuperPoint          = YES (License: GPLv3) libtorch=${Torch_VERSION}")
+MESSAGE(STATUS "  With SuperPoint           = YES (License: GPLv3) libtorch=${Torch_VERSION}")
 ELSEIF(NOT WITH_TORCH)
-MESSAGE(STATUS "  With SuperPoint          = NO (WITH_TORCH=OFF)")
+MESSAGE(STATUS "  With SuperPoint           = NO (WITH_TORCH=OFF)")
 ELSE()
-MESSAGE(STATUS "  With SuperPoint          = NO (libtorch not found)")
+MESSAGE(STATUS "  With SuperPoint           = NO (libtorch not found)")
 ENDIF()
 
+IF(TORCH_FOUND AND WITH_PYTHON AND Python3_FOUND)
+MESSAGE(STATUS "  With Superpoint Rpautrat  = YES (Liscense: MIT) libtorch=${Torch_VERSION}")
+ELSEIF(NOT WITH_TORCH)
+MESSAGE(STATUS "  With Superpoint Rpautrat  = NO (WITH_TORCH=OFF)")
+ELSEIF(NOT WITH_PYTHON)
+MESSAGE(STATUS "  With Superpoint Rpautrat  = NO (WITH_PYTHON=OFF)")
+ELSE()
+MESSAGE(STATUS "  Wtih Superpoint Rpautrat  = NO (libtorch and/or python3 not found)")
+ENDIF()
+
+
 IF(WITH_PYTHON AND Python3_FOUND)
-MESSAGE(STATUS "  With Python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}            = YES (License: PSF)")
+MESSAGE(STATUS "  With Python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}           = YES (License: PSF)")
 ELSEIF(NOT WITH_PYTHON)
 MESSAGE(STATUS "  With Python3              = NO (WITH_PYTHON=OFF)")
 ELSE()
@@ -1482,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)
@@ -1591,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()
@@ -1706,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)")
@@ -1762,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)
@@ -1798,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)")
@@ -1847,7 +2078,7 @@ ELSE()
 MESSAGE(STATUS "  With VINS-Fusion          = NO (VINS-Fusion not found)")
 ENDIF()
 
-IF(ov_msckf_FOUND)
+IF(OpenVINS_FOUND)
 MESSAGE(STATUS "  With OpenVINS             = YES (License: GPLv3)")
 ELSEIF(NOT WITH_OPENVINS)
 MESSAGE(STATUS "  With OpenVINS             = NO (WITH_OPENVINS=OFF)")

README.md

@@ -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>
-           <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"/>
+           <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> 
-           </td>
+           <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> 
-        </tr>
+           <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> 
-        <tr>
+           <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> 
-           <td>Windows</td>
+           <a href="https://github.com/introlab/rtabmap/actions/workflows/docker.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg" alt="Docker Build Status"/>
-           <td><a href="https://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>
         </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,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define RTABMAP_VERSION_MINOR @PROJECT_VERSION_MINOR@
 #define RTABMAP_VERSION_PATCH @PROJECT_VERSION_PATCH@
 
-#define RTABMAP_VERSION_COMPARE(major, minor, patch)  (major>=@PROJECT_VERSION_MAJOR@ || (major==@PROJECT_VERSION_MAJOR@ && minor>=@PROJECT_VERSION_MINOR@) || (major==@PROJECT_VERSION_MAJOR@ && minor==@PROJECT_VERSION_MINOR@ && patch >=@PROJECT_VERSION_PATCH@))
+#define RTABMAP_VERSION_COMPARE(OP, MAJOR, MINOR, PATCH) (RTABMAP_VERSION_MAJOR*10000+RTABMAP_VERSION_MINOR*100+RTABMAP_VERSION_PATCH OP MAJOR*10000+MINOR*100+PATCH)
 
 @NONFREE@#define RTABMAP_NONFREE
 @TORO@#define RTABMAP_TORO
@@ -63,6 +63,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @CUDASIFT@#define RTABMAP_CUDASIFT
 @LOAM@#define RTABMAP_LOAM
 @FLOAM@#define RTABMAP_FLOAM
+@LIOSAM@#define RTABMAP_LIOSAM
 @DC1394@#define RTABMAP_DC1394
 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
 @ZED@#define RTABMAP_ZED
@@ -92,6 +93,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @TORCH@#define RTABMAP_TORCH
 @PYTHON@#define RTABMAP_PYTHON
 @MADGWICK@#define RTABMAP_MADGWICK
+@APRILTAG@#define RTABMAP_APRILTAG
+@APRILTAG_ARUCO@#define RTABMAP_APRILTAG_WITH_ARUCO
 
 #include <pcl/pcl_config.h>
 

app/android/jni/CameraTango.cpp

@@ -865,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/RTABMapApp.cpp

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

app/ios/RTABMapApp.xcodeproj/project.pbxproj

@@ -1065,7 +1065,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib/opencv4/3rdparty",
 				);
-				MARKETING_VERSION = 0.22.0;
+				MARKETING_VERSION = 0.23.7;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1078,7 +1078,7 @@
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.22\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.23\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",
@@ -1125,7 +1125,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib/opencv4/3rdparty",
 				);
-				MARKETING_VERSION = 0.22.0;
+				MARKETING_VERSION = 0.23.7;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
@@ -1139,7 +1139,7 @@
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.22\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.23\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",

app/ios/RTABMapApp/install_deps.sh

@@ -175,6 +175,18 @@ cd $pwd
 #rm -rf g2o
 fi
 
+# g2o's installed CMake config hard-codes the absolute build-time prefix of its
+# external dependencies (e.g. suitesparse) in INTERFACE_INCLUDE_DIRECTORIES. That
+# path doesn't exist when the prebuilt Libraries archive is unpacked on another
+# machine (CI), breaking find_package(g2o) with "includes non-existent path".
+# Rewrite those absolute paths to be relocatable (relative to the config file).
+# Run unconditionally (outside the build guard above) so it also fixes the prebuilt
+# archive in CI, where the g2o build step is skipped.
+find "$prefix/lib" -path '*/cmake/g2o/*.cmake' -print0 | while IFS= read -r -d '' f
+do
+  sed -i '' -E 's#[^";]*/Libraries#${CMAKE_CURRENT_LIST_DIR}/../../..#g' "$f"
+done
+
 # VTK
 if [ ! -e $prefix/lib/vtk.framework ]
 then
@@ -288,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=ON  -DBUILD_EXAMPLES=OFF -DWITH_LIBLAS=ON ../../../../../..
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=ON  -DBUILD_EXAMPLES=OFF -DWITH_LIBLAS=ON -DWITH_OPENGV=OFF ../../../../../..
 cmake --build . --config Release
 cmake --build . --config Release --target install

app/src/CMakeLists.txt

@@ -33,8 +33,16 @@ 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 ${SRC_FILES})
+  ADD_EXECUTABLE(rtabmap_app WIN32 ${SRC_FILES})
 ELSE()
   ADD_EXECUTABLE(rtabmap_app ${SRC_FILES})
 ENDIF()
@@ -95,18 +103,34 @@ 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" 
+		set(DEPTHENGINE_DLL "")
-          DESTINATION ${thirdparty_dest_dir} 
+		foreach(_depthengine_dir
-          COMPONENT runtime)
+			"${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)
   
@@ -127,35 +151,170 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
         IF(NOT OrbbecSDK_BIN_DIR)
             MESSAGE(FATAL_ERROR "OrbbecSDK.dll not found! Verify your PATH.")
         ENDIF(NOT OrbbecSDK_BIN_DIR)
-		MESSAGE(FATAL "OrbbecSDK_BIN_DIR=${OrbbecSDK_BIN_DIR}")
+        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 "*")
-	ENDIF(WIN32)
+    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
+	# Install needed cudnn dlls
-	# TODO: should be a more general way to include them if version is different
     IF(WIN32 AND CUDA_FOUND)
-		find_file(CUDNN_OPS_DLL NAMES cudnn_ops_infer64_8.dll)
+	    find_path(cuDNN_BIN_DIR NAMES cudnn.dll cudnn64.dll cudnn64_9.dll)
-		find_file(CUDNN_CNN_DLL NAMES cudnn_cnn_infer64_8.dll)
+        IF(NOT cuDNN_BIN_DIR)
-		IF(CUDNN_OPS_DLL AND CUDNN_CNN_DLL)
+            MESSAGE(FATAL_ERROR "cudnn dlls not found! Make sure the dlls are in a directory on your PATH.")
-			MESSAGE(STATUS "Found ${CUDNN_OPS_DLL}")
+        ENDIF(NOT cuDNN_BIN_DIR)
-			MESSAGE(STATUS "Found ${CUDNN_CNN_DLL}")
+		MESSAGE(STATUS "cuDNN_BIN_DIR = ${cuDNN_BIN_DIR}")
-			INSTALL(FILES ${CUDNN_OPS_DLL} ${CUDNN_CNN_DLL}
+		file(GLOB CUDNN_DLLS "${cuDNN_BIN_DIR}/cudnn*.dll")
-			  DESTINATION ${thirdparty_dest_dir} 
+		IF(CUDNN_DLLS)
-			  COMPONENT runtime)
+			MESSAGE(STATUS "Found cuDNN DLLs: ${CUDNN_DLLS}")
-		ELSE()
+			INSTALL(FILES ${CUDNN_DLLS}
-			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.")
+					DESTINATION ${thirdparty_dest_dir} 
+					COMPONENT runtime)
 		ENDIF()
 	ENDIF(WIN32 AND CUDA_FOUND)
   ENDIF(Torch_FOUND)
   
-  IF(Qt6_FOUND)
+  set(python_pyd_dir "")
+  IF(Python3_FOUND)
+	# bundle python3
+    IF(WIN32)
+		set(python_pyd_dir "bin/Lib/site-packages")
+		set(PYTHON_ZIP_NAME "python${Python3_VERSION_MAJOR}${Python3_VERSION_MINOR}.zip")
+		get_filename_component(PYTHON_ROOT "${Python3_EXECUTABLE}" DIRECTORY)
+		file(TO_CMAKE_PATH "${Python3_STDLIB}" SANITIZED_STDLIB)
+
+		MESSAGE(STATUS "Python3_EXECUTABLE=${Python3_EXECUTABLE}")
+		MESSAGE(STATUS "Python3_STDLIB=${SANITIZED_STDLIB}")
+		
+		install(FILES "${Python3_EXECUTABLE}" DESTINATION bin COMPONENT runtime)
+		
+		# when using python-opencv, it expects python3.dll, not python312.dll
+		get_filename_component(VCPKG_TRIPLET_ROOT "${PYTHON_TOOLS_DIR}/../../" ABSOLUTE)
+		set(VCPKG_BIN_DIR "${VCPKG_TRIPLET_ROOT}/bin")
+		find_file(PYTHON3_STABLE_DLL 
+			NAMES python3.dll 
+			PATHS "${VCPKG_BIN_DIR}"
+			NO_DEFAULT_PATH
+		)
+		if(PYTHON3_STABLE_DLL)
+			message(STATUS "Found python3.dll at: ${PYTHON3_STABLE_DLL}")
+			install(FILES "${PYTHON3_STABLE_DLL}" DESTINATION bin COMPONENT runtime)
+		endif()
+
+		# install python Lib in python312.zip (without site-packages, which is installed separatly afterwards)
+		file(TO_CMAKE_PATH "${Python3_STDLIB}" SANITIZED_STDLIB)
+		file(GLOB LIB_CONTENTS RELATIVE "${SANITIZED_STDLIB}" "${SANITIZED_STDLIB}/*")
+		list(REMOVE_ITEM LIB_CONTENTS "site-packages")
+		install(CODE "
+			execute_process(
+				COMMAND \"${CMAKE_COMMAND}\" -E tar cf \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin/${PYTHON_ZIP_NAME}\" --format=zip -- ${LIB_CONTENTS}
+				WORKING_DIRECTORY \"${SANITIZED_STDLIB}\"
+			)
+		" COMPONENT runtime)
+		set(PYTHON_SITE_PACKAGES "${SANITIZED_STDLIB}/site-packages")
+		
+		install(DIRECTORY "${PYTHON_SITE_PACKAGES}/"
+				DESTINATION "${thirdparty_dest_dir}/Lib/site-packages"
+				COMPONENT runtime
+				PATTERN "*.exe" EXCLUDE
+				PATTERN "*.lib" EXCLUDE
+				PATTERN "*.hpp" EXCLUDE
+				PATTERN "*.h" EXCLUDE
+				PATTERN "*/torch/*" EXCLUDE
+		)
+		if(EXISTS "${PYTHON_SITE_PACKAGES}/torch")
+			install(DIRECTORY "${PYTHON_SITE_PACKAGES}/torch"
+					DESTINATION "${thirdparty_dest_dir}/Lib/site-packages/"
+					COMPONENT runtime
+					PATTERN "*.exe" EXCLUDE
+					PATTERN "*.lib" EXCLUDE
+					PATTERN "*.hpp" EXCLUDE
+					PATTERN "*.h" EXCLUDE
+					PATTERN "*.dll" EXCLUDE
+			)
+			file(GLOB_RECURSE PY_DLL_FILES "${PYTHON_SITE_PACKAGES}/torch/*.dll")
+			if(PY_DLL_FILES)
+				install(FILES ${PY_DLL_FILES} DESTINATION bin COMPONENT runtime)
+			endif()
+		endif()
+
+		# install python DDLs
+		file(GLOB_RECURSE PY_DLL_FILES \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${python_pyd_dir}/*.dll\")
+		install(DIRECTORY "${PYTHON_ROOT}/DLLs"
+				DESTINATION "${thirdparty_dest_dir}/"
+				COMPONENT runtime
+				FILES_MATCHING 
+				PATTERN "*.dll"
+				PATTERN "*.pyd"
+		)
+
+		# install our python scripts in share for convenience
+		install(DIRECTORY "${PROJECT_SOURCE_DIR}/corelib/src/python/"
+				DESTINATION share
+				COMPONENT runtime
+				FILES_MATCHING
+				PATTERN "*.py"
+		)
+	ENDIF(WIN32)
+  ENDIF(Python3_FOUND)
+  
+  IF(Qt6_FOUND 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")
@@ -245,7 +404,10 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   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)
@@ -257,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}\")
+    # 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

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

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

cmake_modules/FindEigen3.cmake

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

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

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
+   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})
+      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)
+   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/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 )
+
+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;}
+	 * @brief The locations the posterior is over, ascending by id.
-	float getVirtualPlacePrior() const {return _virtualPlacePrior;}
+	 *
-	const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...}
+	 * The virtual place (@ref Memory::kIdVirtual) is the first of them when it is one.
-	std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...}
+	 */
+	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,
+	 * @brief Realigns the posterior with the ids of the likelihood.
-			const std::vector<int> & oldIds,
+	 *
-			const std::vector<int> & newIds);
+	 * Keeps the probability of the locations that are in both. Called only when the ids differ.
-	void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
+	 */
-	void normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const;
+	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;
+	std::vector<int> _posteriorIds;               ///< The locations the posterior is over, ascending by id.
-	cv::Mat _prediction;
+	std::vector<float> _posteriorValues;          ///< The probability of each of them, in the same order.
-	float _virtualPlacePrior;
+	std::vector<int> _likelihoodIds;              ///< The ids of the likelihood of an iteration, in its order.
-	std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
+	std::vector<float> _likelihoodValues;         ///< The likelihood of an iteration, in the same order.
-	bool _fullPredictionUpdate;
+	std::vector<float> _priorValues;              ///< The prior of an iteration, in the same order.
-	float _totalPredictionLCValues;
+
-	float _predictionEpsilon;
+	bayes::PredictionModel * _model;              ///< The `{Vp, Lc, l1, ...}` model and the column arithmetic of it.
-	std::map<int, std::map<int, int> > _neighborsIndex;
+	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

@@ -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)
+	 * @brief Returns the default optical rotation to convert image coordinates to robot coordinates.
-	 * to robot coordinate frame (x->forward, y->left, z->up).
+	 * 
+	 * 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)
+	 * @brief Constructor using full camera parameters.
-	// P is the projection matrix 3x4 CV_64FC1 (computed from stereo or equal to [K [0 0 1]'])
+	 * 
+	 * @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)
+	/// Returns the raw intrinsic matrix (before rectification).
-	cv::Mat D_raw() const {return D_;} //intrinsic distorsion matrix (before rectification)
+	cv::Mat K_raw() const {return K_;}
-	cv::Mat K() const {return !P_.empty()?P_.colRange(0,3):K_;} // if P exists, return rectified version
+	/// 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
+	/// Returns the rectification matrix.
-	cv::Mat P() const {return P_;} //projection 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
+	 * @brief Returns the horizontal field of view (FoV) in radians.
-	double horizontalFOV() const; // in degrees
+	 *
-	double verticalFOV() const;   // in degrees
+	 * 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_;
+	std::string name_;         ///< Camera name.
-	cv::Size imageSize_;
+	cv::Size imageSize_;       ///< Image size.
-	cv::Mat K_;
+	cv::Mat K_;                ///< Intrinsic matrix.
-	cv::Mat D_;
+	cv::Mat D_;                ///< Distortion coefficients.
-	cv::Mat R_;
+	cv::Mat R_;                ///< Rectification matrix.
-	cv::Mat P_;
+	cv::Mat P_;                ///< Projection matrix.
-	cv::Mat mapX_;
+	cv::Mat mapX_;             ///< Rectification map X.
-	cv::Mat mapY_;
+	cv::Mat mapY_;             ///< Rectification map Y.
-	Transform localTransform_;
+	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/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
+ * @code
- *   CompressionThread ct(image);
+ * cv::Mat image;
- *   ct.start();
+ * CompressionThread ct(image, ".png");
- *   ct.join();
+ * ct.start();
- *   std::vector<unsigned char> bytes = ct.getCompressedData();
+ * ct.join();
+ * cv::Mat bytes = ct.getCompressedData();
+ * @endcode
  *
- * Example uncompression
+ * Example uncompression:
- *   std::vector<unsigned char> bytes;// a compressed image
+ * @code
- *   CompressionThread ct(bytes);
+ * cv::Mat bytes;
- *   ct.start();
+ * CompressionThread ct(bytes, true);
- *   ct.join();
+ * ct.start();
- *   cv::Mat image = ct.getUncompressedData();
+ * 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
+ * @class DBDriver
-//that you cannot use the same sqlite3 connection in two or more
+ * @brief Abstract database driver for RTAB-Map maps (signatures, links, words, statistics).
-//threads at the same time.  You can use different sqlite3 connections
+ *
-//at the same time in different threads, or you can move the same
+ * DBDriver is the persistence layer used by @ref Memory, @ref DBReader and tools that
-//sqlite3 connection across threads (subject to the constraints above)
+ * read or write \c .db files. The default implementation is @ref DBDriverSqlite3, created
-//but never, never try to use the same connection simultaneously in
+ * by @ref create().
-//two or more threads."
+ *
-//
+ * 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;
+	 * @brief Queue a signature for deferred save; ownership is transferred.
-
+	 *
-	void asyncSave(Signature * s); //ownership transferred
+	 * @note Only the *compressed* sensor buffers are written
-	void asyncSave(VisualWord * vw); //ownership transferred
+	 * (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,15 +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
+	/**
-
+	 * @name Connection and database introspection
-	bool openConnection(const std::string & url, bool overwritten = false);
+	 * @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;
@@ -159,11 +267,33 @@ 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 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, 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
@@ -176,6 +306,7 @@ public:
 	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;
@@ -189,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
@@ -279,7 +418,7 @@ protected:
 	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 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;
@@ -299,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
@@ -138,7 +243,7 @@ protected:
 	virtual void saveFlannIndexQuery(const std::vector<unsigned char> & indexData) const;
 
 	// Load objects
-	virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true) 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;
@@ -199,8 +304,8 @@ private:
 	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
 
 protected:
-	sqlite3 * _ppDb;
+	sqlite3 * _ppDb; ///< Open SQLite connection (null when disconnected)
-	std::string _version;
+	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,              ///< Uninitialized / unknown channel.
-		kUndefined = 0,
+		kWifiSignalStrength,         ///< Wi‑Fi signal strength (dBm).
-		kWifiSignalStrength,      // dBm
+		kAmbientTemperature,         ///< Ambient temperature (°C).
-		kAmbientTemperature,      // Celcius
+		kAmbientAirPressure,         ///< Ambient air pressure (hPa).
-		kAmbientAirPressure,      // hPa
+		kAmbientLight,               ///< Ambient illuminance (lx).
-		kAmbientLight,            // lx
+		kAmbientRelativeHumidity,    ///< Relative humidity (%).
-		kAmbientRelativeHumidity, // %
 
-		// user types
+		kCustomSensor1 = 100,        ///< User-defined sensor slot 1.
-		kCustomSensor1 = 100,
+		kCustomSensor2,             ///< User-defined sensor slot 2.
-		kCustomSensor2,
+		kCustomSensor3,             ///< User-defined sensor slot 3.
-		kCustomSensor3,
+		kCustomSensor4,             ///< User-defined sensor slot 4.
-		kCustomSensor4,
+		kCustomSensor5,             ///< User-defined sensor slot 5.
-		kCustomSensor5,
+		kCustomSensor6,             ///< User-defined sensor slot 6.
-		kCustomSensor6,
+		kCustomSensor7,             ///< User-defined sensor slot 7.
-		kCustomSensor7,
+		kCustomSensor8,             ///< User-defined sensor slot 8.
-		kCustomSensor8,
+		kCustomSensor9              ///< User-defined sensor slot 9.
-		kCustomSensor9
 	};
 
 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 void parseParameters(const ParametersMap & parameters) override;
-	virtual Feature2D::Type getType() const {return kFeatureSurf;}
+	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 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;
+	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 void parseParameters(const ParametersMap & parameters) override;
-	virtual Feature2D::Type getType() const {return kFeatureSift;}
+	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 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;
+	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 void parseParameters(const ParametersMap & parameters) override;
-	virtual Feature2D::Type getType() const {return kFeatureOrb;}
+	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 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;
+	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 void parseParameters(const ParametersMap & parameters) override;
-	virtual Feature2D::Type getType() const {return kFeatureUndef;}
+	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 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 {return cv::Mat();}
+	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 void parseParameters(const ParametersMap & parameters) override;
-	virtual Feature2D::Type getType() const {return kFeatureFastBrief;}
+	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 void parseParameters(const ParametersMap & parameters) override;
-	virtual Feature2D::Type getType() const {return kFeatureFastFreak;}
+	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 void parseParameters(const ParametersMap & parameters) override;
-	virtual Feature2D::Type getType() const {return kFeatureGfttBrief;}
+	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 void parseParameters(const ParametersMap & parameters) override;
-	virtual Feature2D::Type getType() const {return kFeatureGfttFreak;}
+	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 void parseParameters(const ParametersMap & parameters) override;
-	virtual Feature2D::Type getType() const {return kFeatureSurfFreak;}
+	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 void parseParameters(const ParametersMap & parameters) override;
-	virtual Feature2D::Type getType() const {return kFeatureGfttOrb;}
+	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 void parseParameters(const ParametersMap & parameters) override;
-	virtual Feature2D::Type getType() const {return kFeatureBrisk;}
+	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 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;
+	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 void parseParameters(const ParametersMap & parameters) override;
-	virtual Feature2D::Type getType() const { return kFeatureKaze; }
+	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 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;
+	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 void parseParameters(const ParametersMap & parameters) override;
-	virtual Feature2D::Type getType() const {return kFeatureOrbOctree;}
+	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 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;
+	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 void parseParameters(const ParametersMap & parameters) override;
-	virtual Feature2D::Type getType() const { return kFeatureSuperPointTorch; }
+	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 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;
+	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 void parseParameters(const ParametersMap & parameters) override;
-	virtual Feature2D::Type getType() const {return kFeatureGfttDaisy;}
+	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 void parseParameters(const ParametersMap & parameters) override;
-	virtual Feature2D::Type getType() const {return kFeatureSurfDaisy;}
+	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,36 +34,117 @@ 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:
-	// A forward of the internal enum, indexes should match. See src/rtflann/defines.h
+	/**
+	 * @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,
+		FLANN_INDEX_LINEAR 			= 0, ///< Exhaustive search
-		FLANN_INDEX_KDTREE 			= 1,
+		FLANN_INDEX_KDTREE 			= 1, ///< 4 randomized kd-trees, searched approximately
-		FLANN_INDEX_KDTREE_SINGLE   = 4,
+		FLANN_INDEX_KDTREE_SINGLE   = 4, ///< Single kd-tree, searched exactly
-		FLANN_INDEX_LSH 			= 6,
+		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
+	/**
+	 * @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);
-	// Return false if the indexData doesn't correspond to expected features used and parameters.
+
+	/**
+	 * @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,
@@ -71,6 +152,7 @@ public:
 		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,
@@ -80,16 +162,46 @@ public:
 		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,
@@ -99,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,
@@ -111,7 +237,8 @@ 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_;

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,

corelib/include/rtabmap/core/GlobalMap.h

@@ -37,14 +37,31 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class GlobalMap
+ * @brief Abstract base for assembling per-node @ref LocalGrid data into a global map.
+ *
+ * Subclasses (@ref OccupancyGrid, @ref OctoMap, @ref GridMap, @ref CloudMap) implement
+ * @ref assemble() to merge new node poses from @ref LocalGridCache into their representation.
+ *
+ * @ref update() decides which poses need assembly (not yet in @ref addedNodes(), present in
+ * cache, id > 0) and may call @ref clear() when @ref fullUpdateNeeded() detects graph
+ * optimization or a disjoint pose set (see @ref Parameters::kGridGlobalUpdateError()).
+ *
+ * Log-odds helpers @ref logodds() and @ref probability() convert between occupancy
+ * probability and the internal log-odds representation used for global hit/miss/clamping
+ * parameters (GridGlobal/Prob*).
+ */
 class RTABMAP_CORE_EXPORT GlobalMap
 {
 public:
+	/** @brief Converts probability in (0, 1) to log-odds. */
 	inline static float logodds(double probability)
 	{
 		return (float) log(probability/(1-probability));
 	}
 
+	/** @brief Converts log-odds back to probability in (0, 1). */
 	inline static double probability(double logodds)
 	{
 		return 1. - ( 1. / (1. + exp(logodds)));
@@ -53,31 +70,66 @@ public:
 public:
 	virtual ~GlobalMap();
 
+	/**
+	 * @brief True if the map should be rebuilt from cache (loop closure or disjoint graph).
+	 *
+	 * Compares @p poses to @ref addedNodes() using @ref getUpdateError(), which is set from
+	 * **Grid/GlobalUpdateError** (`Parameters::kGridGlobalUpdateError()`): if any assembled
+	 * node's pose moved farther than that threshold (meters), or if none of the assembled
+	 * nodes appear in @p poses, returns true and @ref update() will call @ref clear() first.
+	 *
+	 * @param poses Current graph poses (node id → transform).
+	 */
 	bool fullUpdateNeeded(const std::map<int, Transform> & poses) const;
-	bool update(const std::map<int, Transform> & poses); // return true if map has changed
 
+	/**
+	 * @brief Incrementally assemble new nodes from @p poses.
+	 * @param poses Graph poses; ids &gt; 0 with a cached @ref LocalGrid are candidates.
+	 * @return True if @ref assemble() was called (at least one new pose processed).
+	 */
+	bool update(const std::map<int, Transform> & poses);
+
+	/** @brief Clears assembled nodes and grid bounds; does not clear @ref LocalGridCache. */
 	virtual void clear();
 
+	/** @return Grid cell size in meters (Grid/CellSize). */
 	float getCellSize() const {return cellSize_;}
+	/** @return Pose change threshold for full rebuild (Grid/GlobalUpdateError). */
 	float getUpdateError() const {return updateError_;}
+	/** @return Poses of nodes already assembled into the global map. */
 	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 
+	/** @brief 2D grid minimum (x, y) in meters. */
 	void getGridMin(double & x, double & y) const {x=minValues_[0];y=minValues_[1];}
+	/** @brief 2D grid maximum (x, y) in meters. */
 	void getGridMax(double & x, double & y) const {x=maxValues_[0];y=maxValues_[1];}
+	/** @brief 3D grid minimum (x, y, z) in meters. */
 	void getGridMin(double & x, double & y, double & z) const {x=minValues_[0];y=minValues_[1];z=minValues_[2];}
+	/** @brief 3D grid maximum (x, y, z) in meters. */
 	void getGridMax(double & x, double & y, double & z) const {x=maxValues_[0];y=maxValues_[1];z=maxValues_[2];}
 
+	/** @brief Approximate memory used by assembled-node bookkeeping (bytes). */
 	virtual unsigned long getMemoryUsed() const;
 
 protected:
+	/**
+	 * @brief Constructs the base map; subclasses call from their constructor.
+	 * @param cache Non-null cache of per-node local grids.
+	 * @param parameters Optional Grid/ and GridGlobal/ parameter overrides.
+	 */
 	GlobalMap(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap());
 
+	/** @brief Subclass hook: merge @p newPoses into the global map; call @ref addAssembledNode(). */
 	virtual void assemble(const std::list<std::pair<int, Transform> > & newPoses) = 0;
 
+	/** @return Cached local grids keyed by node id. */
 	const std::map<int, LocalGrid> & cache() const {return cache_->localGrids();}
 
+	/** @return Poses of nodes already assembled (same as @ref addedNodes()). */
 	const std::map<int, Transform> & assembledNodes() const {return addedNodes_;}
+	/** @return True if @p id is already in the assembled set. */
 	bool isNodeAssembled(int id) {return addedNodes_.find(id) != addedNodes_.end();}
+	/** @brief Records a successfully assembled node (ids &lt;= 0 are ignored). */
 	void addAssembledNode(int id, const Transform & pose);
 
 protected:

corelib/include/rtabmap/core/Graph.h

@@ -40,39 +40,106 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 class Memory;
 
+/**
+ * @namespace rtabmap::graph
+ * @brief Pose-graph I/O, trajectory metrics, link utilities, and path planning.
+ *
+ * Functions operate on maps of signature ids to @ref Transform poses and
+ * @ref Link constraints (typically stored as `std::multimap<int, Link>` keyed by
+ * the source node id).
+ *
+ * Main groups:
+ * - **I/O:** @ref exportPoses(), @ref importPoses(), @ref exportGPS()
+ * - **Evaluation:** @ref calcKittiSequenceErrors(), @ref calcRelativeErrors(),
+ *   @ref calcRMSE(), @ref computeMaxGraphErrors()
+ * - **Links:** @ref findLink(), @ref findLinks(), @ref filterLinks(),
+ *   @ref filterDuplicateLinks()
+ * - **Spatial queries:** @ref findNearestNode(), @ref findNearestNodes(),
+ *   @ref frustumPosesFiltering(), @ref radiusPosesFiltering()
+ * - **Planning:** @ref computePath(), @ref computePathLength(), @ref getPaths()
+ */
 namespace graph {
 
-////////////////////////////////////////////
+/**
-// Graph utilities
+ * @brief Writes poses (and optional constraints) to disk.
-////////////////////////////////////////////
+ * @param filePath Output path; extension may be appended from @p format.
-
+ * @param format Export format:
+ *   - `0` Raw text (`.txt`): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
+ *   - `1` RGBD-SLAM format, in motion capture frame like the ground truth of RGB-D SLAM Dataset (requires @p stamps) : stamp x y z qx qy qz qw
+ *   - `10` Like `1` without coordinate-frame change (i.e., in base frame) : stamp x y z qx qy qz qw
+ *   - `11` Like `10` with landmark ids after positive ids : stamp x y z qx qy qz qw id
+ *   - `2` KITTI odometry format : r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
+ *   - `3` TORO graph (requires @p constraints; uses @p parameters)
+ *   - `4` g2o (requires @p constraints; uses @p parameters)
+ * @param poses Node id → pose.
+ * @param constraints Required for formats `3` and `4`.
+ * @param stamps Required for formats `1`, `10`, and `11` (same size as @p poses).
+ * @param parameters Optional optimizer parameters for formats `3` and `4`.
+ * @return False on I/O or validation error.
+ */
 bool RTABMAP_CORE_EXPORT exportPoses(
 		const std::string & filePath,
-		int format, // 0=Raw (*.txt), 1=RGBD-SLAM motion capture (*.txt) (10=without change of coordinate frame, 11=10+ID), 2=KITTI (*.txt), 3=TORO (*.graph), 4=g2o (*.g2o)
+		int format,
 		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & constraints = std::multimap<int, Link>(), // required for formats 3 and 4
+		const std::multimap<int, Link> & constraints = std::multimap<int, Link>(),
-		const std::map<int, double> & stamps = std::map<int, double>(),  // required for format 1
+		const std::map<int, double> & stamps = std::map<int, double>(),
-		const ParametersMap & parameters = ParametersMap()); // optional for formats 3 and 4
+		const ParametersMap & parameters = ParametersMap());
 
+/**
+ * @brief Loads poses (and optional constraints) from disk.
+ * @param filePath Input path.
+ * @param format Import format:
+ *   - `0` Raw text: 3×4 matrix per line (`Transform::fromString()`)
+ *   - `1` RGBD-SLAM motion capture: stamp x y z qw qx qy qz (applies optical-frame conversion)
+ *   - `2` KITTI odometry: 3×4 matrix per line (applies optical-frame conversion)
+ *   - `3` TORO graph (fills @p constraints)
+ *   - `4` g2o (not supported yet)
+ *   - `5` NewCollege: stamp x y (2D; first pose is origin)
+ *   - `6` Malaga Urban GPS: 25-field `*_GPS.txt` line (local X/Y/Z)
+ *   - `7` St Lucia INS: 12-field log (GPS → local ENU + roll/pitch/yaw)
+ *   - `8` Karlsruhe: timestamp lat lon alt x y z roll pitch yaw (first pose is origin)
+ *   - `9` EuRoC MAV: stamp x y z qw qx qy qz vx vy vz vr vp vy ax ay az (17 CSV fields)
+ *   - `10` RGBD-SLAM like `1` without coordinate-frame change
+ *   - `11` RGBD-SLAM like `10` with node id as 9th field: stamp x y z qw qx qy qz id
+ *   - `12` RGBD Bonn dynamic dataset format (stamp + pose; Bonn-specific frame conversion)
+ * @param poses Output node id → pose.
+ * @param constraints Optional output links (format `3` only).
+ * @param stamps Optional output timestamps (formats `1`, `5`–`9`, `10`–`12` when present in file).
+ * @return False on I/O or parse error.
+ */
 bool RTABMAP_CORE_EXPORT importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV, 12=rgbd_bonn
+		int format,
 		std::map<int, Transform> & poses,
-		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
+		std::multimap<int, Link> * constraints = 0,
-		std::map<int, double> * stamps = 0); // optional for format 1 and 9
+		std::map<int, double> * stamps = 0);
 
+/**
+ * @brief Exports GPS samples to a PLY point cloud.
+ * @param filePath Output `.ply` path.
+ * @param gpsValues Node id → @ref GPS fix.
+ * @param rgba Point color (default opaque white).
+ */
 bool RTABMAP_CORE_EXPORT exportGPS(
 		const std::string & filePath,
 		const std::map<int, GPS> & gpsValues,
 		unsigned int rgba = 0xFFFFFFFF);
 
 /**
- * Compute translation and rotation errors for KITTI datasets.
+ * @brief KITTI odometry benchmark error over fixed trajectory segments.
- * See http://www.cvlibs.net/datasets/kitti/eval_odometry.php.
+ *
- * @param poses_gt, Ground Truth poses
+ * For each start pose (every 10 frames) and segment length in
- * @param poses_result, Estimated poses
+ * {100, 200, …, 800} m along @p poses_gt, compares the relative transform
- * @param t_err, Output translation error (%)
+ * GT vs estimate and accumulates normalized errors. The returned values are
- * @param r_err, Output rotation error (deg/m)
+ * the mean over all valid segments.
+ *
+ * @param poses_gt Ground-truth poses in temporal order (one per frame).
+ * @param poses_result Estimated poses (same length and ordering as @p poses_gt).
+ * @param t_err Output mean translation error (%): segment translation error (m)
+ *              divided by segment length, averaged, then × 100.
+ * @param r_err Output mean rotation error (deg/m): segment rotation error (rad)
+ *              divided by segment length, averaged, then converted to deg/m.
+ * @see http://www.cvlibs.net/datasets/kitti/eval_odometry.php
  */
 void RTABMAP_CORE_EXPORT calcKittiSequenceErrors(
 		const std::vector<Transform> &poses_gt,
@@ -81,11 +148,21 @@ void RTABMAP_CORE_EXPORT calcKittiSequenceErrors(
 		float & r_err);
 
 /**
- * Compute average of translation and rotation errors between each poses.
+ * @brief Mean frame-to-frame relative pose error (RPE-style, one step).
- * @param poses_gt, Ground Truth poses
+ *
- * @param poses_result, Estimated poses
+ * For each consecutive pair `(i, i+1)`, builds the relative motion in ground
- * @param t_err, Output translation error (m)
+ * truth and in the estimate, then measures how much they differ:
- * @param r_err, Output rotation error (deg)
+ * - translation: Euclidean distance between the two relative transforms (m)
+ * - rotation: angle between the two relative transforms (rad → deg)
+ *
+ * Returns the arithmetic mean over all `N-1` pairs (`N` = trajectory length).
+ * Unlike @ref calcKittiSequenceErrors(), there is no fixed segment length and
+ * no path-length normalization.
+ *
+ * @param poses_gt Ground-truth poses in temporal order (one per frame).
+ * @param poses_result Estimated poses (same length and ordering as @p poses_gt).
+ * @param t_err Output mean translation error over consecutive pairs (m).
+ * @param r_err Output mean rotation error over consecutive pairs (deg).
  */
 void RTABMAP_CORE_EXPORT calcRelativeErrors (
 		const std::vector<Transform> &poses_gt,
@@ -94,12 +171,39 @@ void RTABMAP_CORE_EXPORT calcRelativeErrors (
 		float & r_err);
 
 /**
- * Compute root-mean-square error (RMSE) like the TUM RGBD
+ * @brief Absolute trajectory error (ATE) with Sim(3)-style alignment (TUM RGB-D tool).
- * dataset's evaluation tool (absolute trajectory error).
+ *
- * See https://vision.in.tum.de/data/datasets/rgbd-dataset
+ * Only poses whose id exists in both @p groundTruth and @p poses are compared.
- * @param groundTruth, Ground Truth poses
+ * An alignment transform @c t is estimated so that per-pose error is measured after
- * @param poses, Estimated poses
+ * bringing the estimate into the reference frame:
- * @return Gt to Map transform
+ * - If more than five poses match: @c t from SVD on position correspondences
+ *   (estimate positions → ground-truth positions; z ignored when @p align2D is true).
+ * - Otherwise: @c t = groundTruth[firstId] * poses[firstId]⁻¹ using the first matched id.
+ *
+ * For each matched pose, after `aligned = t * poses[id]`:
+ * - **Translational error:** Euclidean distance between `aligned` and `groundTruth[id]` (m).
+ * - **Rotational error:** Angle between the poses' +X axes (deg).
+ *
+ * The eight `@p translational_*` and `@p rotational_*` outputs are statistics over those
+ * per-pose errors (all matched poses). They are set to `0` when no id matches.
+ *
+ * @param groundTruth Reference trajectory (node id → pose).
+ * @param poses Estimated trajectory; ids not in @p groundTruth are skipped.
+ * @param translational_rmse Root mean square of translational errors (m).
+ * @param translational_mean Arithmetic mean of translational errors (m).
+ * @param translational_median Middle sample in matched-pose iteration order (m).
+ * @param translational_std Standard deviation of translational errors (m).
+ * @param translational_min Minimum translational error (m).
+ * @param translational_max Maximum translational error (m).
+ * @param rotational_rmse Root mean square of rotational errors (deg).
+ * @param rotational_mean Arithmetic mean of rotational errors (deg).
+ * @param rotational_median Middle sample in matched-pose iteration order (deg).
+ * @param rotational_std Standard deviation of rotational errors (deg).
+ * @param rotational_min Minimum rotational error (deg).
+ * @param rotational_max Maximum rotational error (deg).
+ * @param align2D If true, alignment uses x/y only (z set to 0 for correspondence); 3D if false.
+ * @return Alignment transform @c t applied as `t * poses[id]` before error computation.
+ * @see https://vision.in.tum.de/data/datasets/rgbd-dataset
  */
 Transform RTABMAP_CORE_EXPORT calcRMSE(
 		const std::map<int, Transform> &groundTruth,
@@ -118,91 +222,209 @@ Transform RTABMAP_CORE_EXPORT calcRMSE(
 		float & rotational_max,
 		bool align2D = false);
 
-void RTABMAP_CORE_EXPORT computeMaxGraphErrors(
+/**
+ * @brief Largest pose-graph constraint violations after optimization.
+ *
+ * For each non-self-referenced link (`from != to`), compares the relative pose implied by @p poses to the
+ * link measurement and tracks the worst linear/angular error and error/std ratios.
+ */
+struct MaxGraphErrors
+{
+	float linear=-1.0f;         ///< Absolute linear error (m) of the worst link.
+	float angular=-1.0f;        ///< Absolute angular error (rad) of the worst link.
+	float linearRatio=-1.0f;    ///< linear / sqrt(trans variance) of the worst link.
+	float angularRatio=-1.0f;   ///< angular / sqrt(rot variance) of the worst link.
+	Link linearLink;            ///< Link with largest @ref linearRatio.
+	Link angularLink;           ///< Link with largest @ref angularRatio.
+};
+
+/**
+ * @brief Finds the worst pose-graph constraint residuals after optimization.
+ *
+ * Iterates over @p links and, for each non-self-referenced edge (`from != to`):
+ * 1. Looks up `T_from` and `T_to` in @p poses (returns default @ref MaxGraphErrors if
+ *    any endpoint pose is missing, null, or not invertible).
+ * 2. Builds the relative pose implied by the optimized poses:
+ *    - Normal link: `t = T_from⁻¹ · T_to`
+ *    - Landmark (`from < 0`): `t = T_to⁻¹ · T_from`, link measurement inverted
+ * 3. Compares `t` to the link transform:
+ *    - **Linear error:** max |Δx|, |Δy|, and |Δz| (z ignored when @p for3DoF is true).
+ *    - **Angular error:** full 3D angle between `t` and the link, or yaw-only if @p for3DoF;
+ *      skipped for @ref Link::kLandmark when the information matrix does not constrain yaw.
+ * 4. Normalizes by link uncertainty: `error / sqrt(variance)` using the link information
+ *    matrix (largest diagonal variance for translation/rotation).
+ *
+ * The returned @ref MaxGraphErrors holds the link with the highest linear and angular
+ * *ratios* (not necessarily the largest absolute error).
+ *
+ * @param poses Optimized node poses (must contain every `from` and `to` id used).
+ * @param links Graph constraints (typically `std::multimap<int, Link>` keyed by `from`).
+ * @param for3DoF If true, linear error uses x/y only and angular error compares yaw only.
+ * @return @ref MaxGraphErrors; fields stay `-1` when no valid link was checked or on early abort.
+ */
+MaxGraphErrors RTABMAP_CORE_EXPORT computeMaxGraphErrors(
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
-		float & maxLinearErrorRatio,
-		float & maxAngularErrorRatio,
-		float & maxLinearError,
-		float & maxAngularError,
-		const Link ** maxLinearErrorLink = 0,
-		const Link ** maxAngularErrorLink = 0,
 		bool for3DoF = false);
 
+/**
+ * @brief Maximum information-matrix diagonal over odometry neighbor links.
+ *
+ * Scans @p links of type @ref Link::kNeighbor or @ref Link::kNeighborMerged and,
+ * for each dof (x, y, z, roll, pitch, yaw), keeps the largest diagonal entry of
+ * the 6×6 information matrix.
+ *
+ * @param links Graph constraints (multimap keyed by source id).
+ * @return Six maximum information values, or an empty vector if no neighbor links exist.
+ */
 std::vector<double> RTABMAP_CORE_EXPORT getMaxOdomInf(const std::multimap<int, Link> & links);
 
+/**
+ * @brief Finds the first link from @p from to @p to in a multimap keyed by source id.
+ *
+ * Iterates all entries with key @p from and matches the destination (and optionally
+ * @p type). When @p checkBothWays is true, also searches key @p to for a link back
+ * to @p from.
+ *
+ * @param links Link multimap (`key` = source node id).
+ * @param from Source node id.
+ * @param to Destination node id.
+ * @param checkBothWays If true, also match `to → from`.
+ * @param type Required link type, or @ref Link::kUndef to accept any type.
+ * @return Iterator to the link, or `links.end()` if not found.
+ */
 std::multimap<int, Link>::iterator RTABMAP_CORE_EXPORT findLink(
 		std::multimap<int, Link> & links,
 		int from,
 		int to,
 		bool checkBothWays = true,
 		Link::Type type = Link::kUndef);
+/** @overload
+ *  `std::multimap<int, std::pair<int, Link::Type>>`. */
 std::multimap<int, std::pair<int, Link::Type> >::iterator RTABMAP_CORE_EXPORT findLink(
 		std::multimap<int, std::pair<int, Link::Type> > & links,
 		int from,
 		int to,
 		bool checkBothWays = true,
 		Link::Type type = Link::kUndef);
+/** @overload
+ *  `std::multimap<int, int>`. */
 std::multimap<int, int>::iterator RTABMAP_CORE_EXPORT findLink(
 		std::multimap<int, int> & links,
 		int from,
 		int to,
 		bool checkBothWays = true);
+/** @overload
+ *  Const `std::multimap<int, Link>`. */
 std::multimap<int, Link>::const_iterator RTABMAP_CORE_EXPORT findLink(
 		const std::multimap<int, Link> & links,
 		int from,
 		int to,
 		bool checkBothWays = true,
 		Link::Type type = Link::kUndef);
+/** @overload
+ *  Const `std::multimap<int, std::pair<int, Link::Type>>`. */
 std::multimap<int, std::pair<int, Link::Type> >::const_iterator RTABMAP_CORE_EXPORT findLink(
 		const std::multimap<int, std::pair<int, Link::Type> > & links,
 		int from,
 		int to,
 		bool checkBothWays = true,
 		Link::Type type = Link::kUndef);
+/** @overload
+ *  Const `std::multimap<int, int>`. */
 std::multimap<int, int>::const_iterator RTABMAP_CORE_EXPORT findLink(
 		const std::multimap<int, int> & links,
 		int from,
 		int to,
 		bool checkBothWays = true);
+
+/**
+ * @brief Lists all links incident on node @p from.
+ *
+ * Outgoing links (`link.from() == from`) are returned as stored; for incoming links
+ * (`link.to() == from`), the inverse link is returned so the pose of @p from is always
+ * the source frame.
+ *
+ * @param links Graph constraints.
+ * @param from Node id to query.
+ * @return Incident links (may be empty).
+ */
 std::list<Link> RTABMAP_CORE_EXPORT findLinks(
 		const std::multimap<int, Link> & links,
 		int from);
 
+/**
+ * @brief Removes duplicate undirected links.
+ *
+ * Keeps the first occurrence of each `(from, to)` or `(to, from)` pair with the same
+ * @ref Link::Type (see @ref findLink() with @p checkBothWays).
+ *
+ * @param links Input link multimap.
+ * @return Copy without duplicates.
+ */
 std::multimap<int, Link> RTABMAP_CORE_EXPORT filterDuplicateLinks(
 		const std::multimap<int, Link> & links);
+
 /**
- * Return links not of type "filteredType". If inverted=true, return links of type "filteredType".
+ * @brief Filters links by type or self-reference.
+ *
+ * - If @p filteredType is @ref Link::kSelfRefLink "kSelfRefLink": exclude self-references (`from == to`),
+ *   or include only them when @p inverted is true.
+ * - Otherwise: exclude links of @p filteredType, or keep only that type when @p inverted is true.
+ *
+ * @param links Input links.
+ * @param filteredType Type to filter, or @ref Link::kSelfRefLink for self-reference filtering.
+ * @param inverted If true, keep the filtered set instead of removing it.
+ * @return Filtered link container (same structure as input).
  */
 std::multimap<int, Link> RTABMAP_CORE_EXPORT filterLinks(
 		const std::multimap<int, Link> & links,
 		Link::Type filteredType,
 		bool inverted = false);
-/**
+/** @overload
- * Return links not of type "filteredType". If inverted=true, return links of type "filteredType".
+ *  For `std::map<int, Link>`. */
- */
 std::map<int, Link> RTABMAP_CORE_EXPORT filterLinks(
 		const std::map<int, Link> & links,
 		Link::Type filteredType,
 		bool inverted = false);
 
-//Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
+/**
+ * @brief Keeps poses inside (or outside) a camera frustum.
+ *
+ * Transforms each pose position into the frustum defined by @p cameraPose using
+ * @ref util3d::frustumFiltering() (this assumes the cameraPose includes the optical rotation of the camera (X right, Y down, Z forward).
+ *
+ * @param poses Input poses (null poses are skipped) in base frame (X forward, Y left, Z up), 
+ * @param cameraPose Frustum origin and orientation including the optical rotation of the camera (X right, Y down, Z forward).
+ * @param horizontalFOV Horizontal field of view (deg); see @ref CameraModel::horizontalFOV().
+ * @param verticalFOV Vertical field of view (deg); see @ref CameraModel::verticalFOV().
+ * @param nearClipPlaneDistance Near clipping distance (m).
+ * @param farClipPlaneDistance Far clipping distance (m).
+ * @param negative If false, keep poses inside the frustum; if true, keep poses outside.
+ * @return Subset of @p poses passing the filter.
+ */
 std::map<int, Transform> RTABMAP_CORE_EXPORT frustumPosesFiltering(
 		const std::map<int, Transform> & poses,
 		const Transform & cameraPose,
-		float horizontalFOV = 45.0f, // in degrees, xfov = atan((image_width/2)/fx)*2
+		float horizontalFOV = 45.0f,
-		float verticalFOV = 45.0f,   // in degrees, yfov = atan((image_height/2)/fy)*2
+		float verticalFOV = 45.0f,
 		float nearClipPlaneDistance = 0.1f,
 		float farClipPlaneDistance = 100.0f,
 		bool negative = false);
 
 /**
- * Get only the the most recent or older poses in the defined radius.
+ * @brief Subsamples poses that are spatially (and optionally angularly) redundant.
- * @param poses The poses
+ *
- * @param radius Radius (m) of the search for near neighbors
+ * For each pose not yet processed, finds all poses within @p radius (KD-tree). When
- * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle)
+ * @p angle &gt; 0, only poses whose +X axis differs by at most @p angle (rad) are grouped.
- * @param keepLatest keep the latest node if true, otherwise the oldest node is kept
+ * From each group, keeps one pose: the latest in map order if @p keepLatest, otherwise
- * @return A map containing only most recent or older poses in the the defined radius
+ * the earliest. The first and last poses of the input map are always kept.
+ *
+ * @param poses Input trajectory (map iteration order defines “latest/oldest”).
+ * @param radius Clustering radius (m); if `≤ 0` or fewer than three poses, returns @p poses unchanged.
+ * @param angle Max heading difference within a cluster (rad); `0` ignores orientation.
+ * @param keepLatest If true, keep the latest pose per cluster; otherwise the earliest.
+ * @return Subsampled poses.
  */
 std::map<int, Transform> RTABMAP_CORE_EXPORT radiusPosesFiltering(
 		const std::map<int, Transform> & poses,
@@ -211,31 +433,55 @@ std::map<int, Transform> RTABMAP_CORE_EXPORT radiusPosesFiltering(
 		bool keepLatest = true);
 
 /**
- * Get all neighbor nodes in a fixed radius around each pose.
+ * @brief Radius-neighbor clustering of poses.
- * @param poses The poses
+ *
- * @param radius Radius (m) of the search for near neighbors
+ * For each pose, inserts `(queryId, neighborId)` into the output for every other pose
- * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle)
+ * within @p radius (and within @p angle of the query heading when @p angle &gt; 0).
- * @return A map between each pose id and its neighbors found in the radius
+ *
+ * @param poses Input poses.
+ * @param radius Search radius (m); no pairs if `≤ 0` or fewer than two poses.
+ * @param angle Max heading difference (rad); `0` ignores orientation.
+ * @return Multimap of pose id → neighbor id (both ids from @p poses).
  */
 std::multimap<int, int> RTABMAP_CORE_EXPORT radiusPosesClustering(
 		const std::map<int, Transform> & poses,
 		float radius,
 		float angle);
 
+/**
+ * @brief Reduces a pose graph into hyper-nodes and hyper-links.
+ *
+ * **Hyper-nodes:** clusters poses connected by non-neighbor loop-closure links.
+ * Clustering starts from the largest id downward; each cluster is keyed by its parent
+ * (hyper-node) id.
+ *
+ * **Hyper-links:** for each @ref Link::kNeighbor or @ref Link::kNeighborMerged link between
+ * different clusters, builds one merged @ref Link along the shortest path through
+ * intra-cluster closure links (Dijkstra with unit cost).
+ *
+ * @param poses Input optimized poses.
+ * @param links Input constraints (should be unique per directed edge for closure links).
+ * @param hyperNodes Output `hyperNodeId → childPoseId` membership.
+ * @param hyperLinks Output links between hyper-nodes (one per hyper-edge, most recent kept).
+ */
 void reduceGraph(
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
-		std::multimap<int, int> & hyperNodes, //<parent ID, child ID>
+		std::multimap<int, int> & hyperNodes,
 		std::multimap<int, Link> & hyperLinks);
 
 /**
- * Perform A* path planning in the graph.
+ * @brief A* shortest path on a pose graph with Euclidean edge costs.
- * @param poses The graph's poses
+ *
- * @param links The graph's links (from node id -> to node id)
+ * Edge cost between adjacent nodes is the Euclidean distance between their poses in
- * @param from initial node
+ * @p poses. Uses `costSoFar + distToEnd` where `distToEnd` is the distance to the goal pose.
- * @param to final node
+ *
- * @param updateNewCosts Keep up-to-date costs while traversing the graph.
+ * @param poses Node id → pose (must contain every node reached by @p links).
- * @return the path ids from id "from" to id "to" including initial and final nodes.
+ * @param links Directed edges (`from` → `to`) keyed by source id.
+ * @param from Start node id.
+ * @param to Goal node id.
+ * @param updateNewCosts If true, use a multimap queue that can decrease keys when a shorter path is found.
+ * @return Ordered path from @p from to @p to (inclusive) with poses; empty if unreachable.
  */
 std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
 			const std::map<int, rtabmap::Transform> & poses,
@@ -245,14 +491,17 @@ std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
 			bool updateNewCosts = false);
 
 /**
- * Perform Dijkstra path planning in the graph.
+ * @brief Dijkstra shortest path on link constraints.
- * @param poses The graph's poses
+ *
- * @param links The graph's links (from node id -> to node id)
+ * Explores outgoing links keyed by `link.from()`. Edge cost is `1` when
- * @param from initial node
+ * @p useSameCostForAllLinks is true, otherwise the translation norm of the link transform.
- * @param to final node
+ *
- * @param updateNewCosts Keep up-to-date costs while traversing the graph.
+ * @param links Constraints keyed by source node id.
- * @param useSameCostForAllLinks Ignore distance between nodes
+ * @param from Start node id.
- * @return the path ids from id "from" to id "to" including initial and final nodes.
+ * @param to Goal node id.
+ * @param updateNewCosts If true, allow cost improvements on open nodes.
+ * @param useSameCostForAllLinks If true, unit edge cost; else use `link.transform().getNorm()`.
+ * @return Node ids from @p from to @p to (inclusive); empty if unreachable.
  */
 std::list<int> RTABMAP_CORE_EXPORT computePath(
 			const std::multimap<int, Link> & links,
@@ -262,13 +511,36 @@ std::list<int> RTABMAP_CORE_EXPORT computePath(
 			bool useSameCostForAllLinks = false);
 
 /**
- * Perform Dijkstra path planning in the graph.
+ * @brief Dijkstra path through the live @ref Memory pose graph.
- * @param fromId initial node
+ *
- * @param toId final node
+ * Loads links from @ref Memory (optionally from the database), chains transforms along
- * @param memory The graph's memory
+ * the chosen path, and returns the accumulated poses. Self-referenced links are skipped.
- * @param lookInDatabase check links in database
+ *
- * @param updateNewCosts Keep up-to-date costs while traversing the graph.
+ * By default (`linearVelocity` and `angularVelocity` ≤ 0), edge cost is translation
- * @return the path ids from id "fromId" to id "toId" including initial and final nodes (Identity pose for the first node).
+ * distance (m) only. When set &gt; 0, costs are expressed in seconds of motion:
+ * - @p linearVelocity adds `linkTranslation / linearVelocity` (time to drive the edge at
+ *   that speed). Used alone it scales every edge by the same factor, so the **shortest path
+ *   is unchanged**; set it to your robot’s typical forward speed (e.g. `0.5` m/s) when you
+ *   also use @p angularVelocity so translation and rotation costs are comparable.
+ * - @p angularVelocity adds `headingMismatch / angularVelocity`, where heading mismatch is
+ *   the angle between the displacement to the next node and that node’s forward (+X) axis.
+ *   This is what changes which path is chosen: a chain followed **mostly forward** (small
+ *   mismatch) can beat a shorter route through loop closures that require large reorientations
+ *   (e.g. `angularVelocity = 1.0` rad/s with `linearVelocity = 0.5` m/s).
+ *   With @p angularVelocity &gt; 0 and @p linearVelocity ≤ 0, translation is ignored and the
+ *   path minimizes heading mismatch only (forward-following paths, regardless of distance).
+ *   This can help loop-closure detection when the map was built with a forward-facing camera:
+ *   the path stays aligned with how places were observed while driving forward.
+ *
+ * @param fromId Start signature id (`≥ 0`).
+ * @param toId Goal signature id (`≠ 0`).
+ * @param memory Graph memory (must not be null).
+ * @param lookInDatabase If true, load links from the database when not already in RAM.
+ * @param updateNewCosts If true, allow cost improvements on open nodes.
+ * @param linearVelocity If &gt; 0, add `translationNorm / linearVelocity` to edge cost (m/s).
+ * @param angularVelocity If &gt; 0, add rotation time from motion direction change (rad/s).
+ * @param ignoreDirectLinks If true, skip the direct edge between @p fromId and @p toId.
+ * @return Path as `(nodeId, pose)` pairs; first pose is identity at @p fromId. Empty if unreachable.
  */
 std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
 		int fromId,
@@ -276,15 +548,19 @@ std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
 		const Memory * memory,
 		bool lookInDatabase = true,
 		bool updateNewCosts = false,
-		float linearVelocity = 0.0f,   // m/sec
+		float linearVelocity = 0.0f,
-		float angularVelocity = 0.0f); // rad/sec
+		float angularVelocity = 0.0f,
+		bool ignoreDirectLinks = false);
 
 /**
- * Find the nearest node of the target pose
+ * @brief Id of the nearest pose to @p targetPose.
- * @param nodes the nodes to search for
+ *
- * @param targetPose the target pose to search around
+ * Wrapper around @ref findNearestNodes() with `radius=0`, `k=1` (1-NN in 3D).
- * @param distance squared distance of the nearest node found (optional)
+ *
- * @return the node id.
+ * @param poses Nodes to search.
+ * @param targetPose Query position (x, y, z only; orientation is not used).
+ * @param distance If not null, set to the squared Euclidean distance of the match.
+ * @return Closest node id, or `0` if @p poses is empty.
  */
 int RTABMAP_CORE_EXPORT findNearestNode(
 		const std::map<int, rtabmap::Transform> & poses,
@@ -292,12 +568,18 @@ int RTABMAP_CORE_EXPORT findNearestNode(
 		float * distance = 0);
 
 /**
- * Find the nearest nodes of the query pose or node
+ * @brief Spatial neighbors of a node (KD-tree radius or k-NN search).
- * @param nodeId the query id
+ *
- * @param nodes the nodes to search for
+ * @p nodeId is removed from the search set. Requires `radius &gt; 0` or `k &gt; 0`.
- * @param radius radius to search for (m), if 0, k should be > 0.
+ * When `radius &gt; 0`, returns all poses within @p radius (up to @p k if `k &gt; 0`).
- * @param k max nearest neighbors (0=all inside the radius)
+ * When `radius == 0`, returns the @p k nearest neighbors.
- * @return the nodes with squared distance to query node.
+ *
+ * @param nodeId Query node (must exist in @p poses); excluded from results.
+ * @param poses Candidate poses.
+ * @param radius Search radius (m).
+ * @param angle Max +X axis angle difference (rad); `0` ignores heading.
+ * @param k Max neighbors (`0` = all within radius).
+ * @return Neighbor id → squared Euclidean distance.
  */
 std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(
 		int nodeId,
@@ -305,18 +587,33 @@ std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(
 		float radius,
 		float angle = 0.0f,
 		int k=0);
+/**
+ * @brief Spatial neighbors of a pose (KD-tree radius or k-NN search).
+ * @param targetPose Query pose (position used; orientation used when @p angle &gt; 0).
+ * @param poses Candidate poses (not modified).
+ * @param radius Search radius (m).
+ * @param angle Max +X axis angle difference (rad); `0` ignores heading.
+ * @param k Max neighbors (`0` = all within radius).
+ * @return Neighbor id → squared Euclidean distance.
+ */
 std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(
 		const Transform & targetPose,
 		const std::map<int, Transform> & poses,
 		float radius,
 		float angle = 0.0f,
 		int k=0);
+/**
+ * @brief Like @ref findNearestNodes(int,const std::map<int,Transform>&,float,float,int)
+ * but returns full @ref Transform values.
+ */
 std::map<int, Transform> RTABMAP_CORE_EXPORT findNearestPoses(
 		int nodeId,
 		const std::map<int, Transform> & poses,
 		float radius,
 		float angle = 0.0f,
 		int k=0);
+/** @overload
+ *  Query by @ref Transform instead of node id. */
 std::map<int, Transform> RTABMAP_CORE_EXPORT findNearestPoses(
 		const Transform & targetPose,
 		const std::map<int, Transform> & poses,
@@ -324,30 +621,63 @@ std::map<int, Transform> RTABMAP_CORE_EXPORT findNearestPoses(
 		float angle = 0.0f,
 		int k=0);
 
-// Use new findNearestNodes() interface with radius=0, angle=0.
+/** @deprecated Use @ref findNearestNodes(const Transform&,const std::map<int,Transform>&,float,float,int) with `radius=0`, `k` set. */
 RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k);
-// Renamed to findNearestNodes()
+/** @deprecated Use @ref findNearestNodes(int,const std::map<int,Transform>&,float,float,int). */
 RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius);
-// Renamed to findNearestNodes()
+/** @deprecated Use @ref findNearestNodes(const Transform&,const std::map<int,Transform>&,float,float,int). */
 RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius);
-// Renamed to findNearestNodes()
+/** @deprecated Use @ref findNearestPoses(int,const std::map<int,Transform>&,float,float,int). */
 RTABMAP_DEPRECATED std::map<int, Transform> RTABMAP_CORE_EXPORT getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f);
-// Renamed to findNearestNodes()
+/** @deprecated Use @ref findNearestPoses(const Transform&,const std::map<int,Transform>&,float,float,int). */
 RTABMAP_DEPRECATED std::map<int, Transform> RTABMAP_CORE_EXPORT getPosesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f);
 
+/**
+ * @brief Path length along an ordered list of poses.
+ *
+ * Sums `path[i].second.getDistance(path[i+1].second)` for consecutive entries.
+ *
+ * @param path Ordered `(nodeId, pose)` pairs.
+ * @return Total length (m), or `0` if fewer than two poses.
+ */
 float RTABMAP_CORE_EXPORT computePathLength(
-		const std::vector<std::pair<int, Transform> > & path,
+		const std::vector<std::pair<int, Transform> > & path);
-		unsigned int fromIndex = 0,
-		unsigned int toIndex = 0);
 
-// assuming they are all linked in map order
+/**
+ * @brief Path length in map iteration order.
+ *
+ * Sums distances between consecutive poses in ascending map key order (does not verify
+ * that entries form a connected path in the graph).
+ *
+ * @param path Poses keyed by node id (sorted by key).
+ * @return Total length (m), or `0` if fewer than two poses.
+ */
 float RTABMAP_CORE_EXPORT computePathLength(
 		const std::map<int, Transform> & path);
 
+/**
+ * @brief Splits poses into chains connected only by neighbor links.
+ *
+ * Repeatedly builds a path starting from the lowest remaining id: adds the next pose
+ * in map order only if a @ref Link::kNeighbor or @ref Link::kNeighborMerged link exists
+ * from the previous pose to it. Stops at the first gap, pushes the chain, and continues
+ * until @p poses is empty.
+ *
+ * @param poses Input poses (cleared as segments are extracted).
+ * @param links Graph constraints keyed by source id.
+ * @return List of pose maps, each a contiguous neighbor chain.
+ */
 std::list<std::map<int, Transform> > RTABMAP_CORE_EXPORT getPaths(
 		std::map<int, Transform> poses,
 		const std::multimap<int, Link> & links);
 
+/**
+ * @brief Axis-aligned bounding box of pose positions.
+ *
+ * @param poses Input poses (no effect if empty).
+ * @param min Output minimum (x, y, z) in meters.
+ * @param max Output maximum (x, y, z) in meters.
+ */
 void RTABMAP_CORE_EXPORT computeMinMax(const std::map<int, Transform> & poses,
 		cv::Vec3f & min,
 		cv::Vec3f & max);

corelib/include/rtabmap/core/IMU.h

@@ -1,25 +1,74 @@
 /*
- * IMU.h
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
- *
+All rights reserved.
- *  Created on: 2018-03-05
+
- *      Author: mathieu
+Redistribution and use in source and binary forms, with or without
- */
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
 
 #ifndef IMU_H_
 #define IMU_H_
 
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 #include <opencv2/core/core.hpp>
 #include <rtabmap/utilite/UEvent.h>
 #include <rtabmap/core/Transform.h>
 
 namespace rtabmap {
 
-
+/**
-// Correspondence class to sensor_msgs/IMU
+ * @class IMU
-class IMU
+ * @brief Inertial measurement sample (ROS @c sensor_msgs/Imu-like fields).
+ *
+ * Holds orientation (quaternion), angular velocity, linear acceleration, optional
+ * 3×3 row-major covariance matrices (double), and an optional @ref Transform
+ * expressing the IMU frame relative to the robot base.
+ *
+ * @ref empty() is true when @ref localTransform() is null (default constructor).
+ * A sample constructed with @ref Transform::getIdentity() is not empty.
+ *
+ * @ref convertToBaseFrame() rotates linear/angular velocity (and orientation when
+ * quaternion x/y/z are non-zero) into the base frame, then clears rotation in
+ * @ref localTransform() while keeping translation.
+ *
+ * @see SensorData::imu()
+ * @see IMUEvent
+ */
+class RTABMAP_CORE_EXPORT IMU
 {
 public:
+	/** @brief Default-constructs an empty sample (null @ref localTransform()). */
 	IMU() {}
+
+	/**
+	 * @brief Constructs a sample with orientation and motion data.
+	 * @param orientation Unit quaternion (qx, qy, qz, qw).
+	 * @param orientationCovariance 3×3 row-major covariance about x, y, z (empty if unused).
+	 * @param angularVelocity Rad/s about x, y, z.
+	 * @param angularVelocityCovariance 3×3 row-major covariance (empty if unused).
+	 * @param linearAcceleration m/s² about x, y, z.
+	 * @param linearAccelerationCovariance 3×3 row-major covariance (empty if unused).
+	 * @param localTransform IMU frame in base coordinates (default identity).
+	 */
 	IMU(const cv::Vec4d & orientation, // qx qy qz qw
 		const cv::Mat & orientationCovariance,
 		const cv::Vec3d & angularVelocity,
@@ -36,6 +85,15 @@ public:
 			localTransform_(localTransform)
 	{
 	}
+
+	/**
+	 * @brief Constructs a sample without orientation (e.g. no magnetometer / no attitude).
+	 * @param angularVelocity Rad/s about x, y, z.
+	 * @param angularVelocityCovariance 3×3 row-major covariance (empty if unused).
+	 * @param linearAcceleration m/s² about x, y, z.
+	 * @param linearAccelerationCovariance 3×3 row-major covariance (empty if unused).
+	 * @param localTransform IMU frame in base coordinates (default identity).
+	 */
 	IMU(const cv::Vec3d & angularVelocity,
 		const cv::Mat & angularVelocityCovariance,
 		const cv::Vec3d & linearAcceleration,
@@ -49,21 +107,37 @@ public:
 	{
 	}
 
-	// qx qy qz qw
+	/** @return Orientation quaternion (qx, qy, qz, qw). */
 	const cv::Vec4d & orientation() const {return orientation_;}
-	const cv::Mat & orientationCovariance() const {return orientationCovariance_;} // 3x3 double Row major about x, y, z axes, empty if orientation is not set
+	/** @return 3×3 orientation covariance (row-major, empty if orientation unset). */
+	const cv::Mat & orientationCovariance() const {return orientationCovariance_;}
 
+	/** @return Angular velocity (rad/s). */
 	const cv::Vec3d & angularVelocity() const {return angularVelocity_;}
-	const cv::Mat & angularVelocityCovariance() const {return angularVelocityCovariance_;} // 3x3 double Row major about x, y, z axes, empty if angularVelocity is not set
+	/** @return 3×3 angular velocity covariance (row-major, empty if unused). */
+	const cv::Mat & angularVelocityCovariance() const {return angularVelocityCovariance_;}
 
-	const cv::Vec3d linearAcceleration() const {return linearAcceleration_;}
+	/** @return Linear acceleration (m/s²). */
-	const cv::Mat & linearAccelerationCovariance() const {return linearAccelerationCovariance_;} // 3x3 double Row major x, y z, empty if linearAcceleration is not set
+	const cv::Vec3d & linearAcceleration() const {return linearAcceleration_;}
+	/** @return 3×3 linear acceleration covariance (row-major, empty if unused). */
+	const cv::Mat & linearAccelerationCovariance() const {return linearAccelerationCovariance_;}
 
+	/** @return Transform from IMU frame to base frame. */
 	const Transform & localTransform() const {return localTransform_;}
 
-	// apply local transform rotation to data, and set Identity rotation for local transform
+	/**
+	 * @brief Rotate motion (and optionally orientation) into the base frame.
+	 *
+	 * Applies @ref localTransform() rotation to vectors and covariances, then sets
+	 * rotational part of @ref localTransform() to identity (translation unchanged).
+	 * No-op if @ref localTransform() is null or rotation is identity.
+	 * Orientation is updated only when quaternion qx, qy, qz, qw are not all zero.
+	 */
 	void convertToBaseFrame();
 
+	/**
+	 * @brief True when @ref localTransform() is null (placeholder / unset sample).
+	 */
 	bool empty() const
 	{
 		return localTransform_.isNull();
@@ -71,30 +145,43 @@ public:
 
 private:
 	cv::Vec4d orientation_;
-	cv::Mat orientationCovariance_; // 3x3 double Row major about x, y, z axes, empty if orientation is not set
+	cv::Mat orientationCovariance_;
 
 	cv::Vec3d angularVelocity_;
-	cv::Mat angularVelocityCovariance_; // 3x3 double Row major about x, y, z axes, empty if angularVelocity is not set
+	cv::Mat angularVelocityCovariance_;
 
 	cv::Vec3d linearAcceleration_;
-	cv::Mat linearAccelerationCovariance_; // 3x3 double Row major x, y z, empty if linearAcceleration is not set
+	cv::Mat linearAccelerationCovariance_;
 
 	Transform localTransform_;
 };
 
+/**
+ * @class IMUEvent
+ * @brief @ref UEvent carrying an @ref IMU sample and timestamp.
+ */
 class IMUEvent : public UEvent
 {
 public:
+	/** @brief Default-constructs an event with zero stamp. */
 	IMUEvent() :
 		stamp_(0.0)
 	{}
+	/**
+	 * @brief Constructs an event with IMU data and stamp.
+	 * @param data IMU sample.
+	 * @param stamp Timestamp in seconds.
+	 */
 	IMUEvent(const IMU & data, double stamp) :
 		data_(data),
 		stamp_(stamp)
 	{
 	}
+	/** @return Event type name for the utilite event system. */
 	virtual std::string getClassName() const {return "IMUEvent";}
+	/** @return IMU payload. */
 	const IMU & getData() const {return data_;}
+	/** @return Timestamp in seconds. */
 	double getStamp() const {return stamp_;}
 
 private:
@@ -104,5 +191,4 @@ private:
 
 }
 
-
 #endif /* IMU_H_ */

corelib/include/rtabmap/core/IMUFilter.h

@@ -34,47 +34,110 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class IMUFilter
+ * @brief Fuses gyroscope and accelerometer samples into an orientation quaternion.
+ *
+ * Implementations integrate angular rates and correct drift using the measured
+ * gravity vector. The public @ref update() API takes timestamps and computes the
+ * time step between consecutive calls.
+ *
+ * Factory methods @ref create() return heap-allocated instances; the caller owns
+ * the pointer (e.g. @ref SensorCaptureThread, @ref IMUThread, @ref Camera).
+ *
+ * Output orientation from @ref getOrientation() is a unit quaternion
+ * `(qx, qy, qz, qw)` in the same convention as @ref IMU (body frame).
+ *
+ * Filter tuning parameters are read from @ref ParametersMap (ImuFilter/... keys);
+ * see @ref kComplementaryFilter and @ref kMadgwick (the latter only when RTAB-Map
+ * is built with Madgwick support).
+ *
+ * @see IMU
+ * @see SensorCaptureThread::enableIMUFiltering()
+ */
 class RTABMAP_CORE_EXPORT IMUFilter
 {
 public:
+	/**
+	 * @brief Orientation fusion algorithm.
+	 */
 	enum Type {
-		kMadgwick=0,
+		kMadgwick = 0,              /**< Madgwick AHRS (attitude and heading reference system). RTAB-Map must be built with Madgwick support. */
-		kComplementaryFilter=1};
+		kComplementaryFilter = 1    /**< Complementary filter (always available). */
-public:
+	};
+
+	/**
+	 * @brief Creates a filter using type parsed from @p parameters.
+	 * @param parameters Optional ImuFilter/... tuning parameters.
+	 * @return New filter instance (caller owns the pointer).
+	 */
 	static IMUFilter * create(const ParametersMap & parameters = ParametersMap());
+
+	/**
+	 * @brief Creates a filter of the given @p type.
+	 * @param type Fusion algorithm; falls back to @ref kComplementaryFilter if
+	 *             @ref kMadgwick is requested but not compiled in.
+	 * @param parameters Optional ImuFilter/... tuning parameters.
+	 * @return New filter instance (caller owns the pointer).
+	 */
 	static IMUFilter * create(IMUFilter::Type type, const ParametersMap & parameters = ParametersMap());
 
-public:
+	virtual ~IMUFilter() {}
-	virtual void parseParameters(const ParametersMap & parameters) {}
-	virtual ~IMUFilter(){}
 
+	/**
+	 * @brief Re-reads filter parameters from @p parameters.
+	 * @param parameters ImuFilter/... keys (implementation-specific).
+	 */
+	virtual void parseParameters(const ParametersMap & parameters) {}
+
+	/**
+	 * @brief Integrates one IMU sample and updates the internal orientation estimate.
+	 * @param gx Gyroscope x angular rate (rad/s).
+	 * @param gy Gyroscope y angular rate (rad/s).
+	 * @param gz Gyroscope z angular rate (rad/s).
+	 * @param ax Accelerometer x (m/s²; magnitude ~9.81 when stationary).
+	 * @param ay Accelerometer y (m/s²).
+	 * @param az Accelerometer z (m/s²).
+	 * @param stamp Sample time (seconds); used with the previous stamp to compute `dt`.
+	 */
 	void update(
 			double gx, double gy, double gz,
 			double ax, double ay, double az,
 			double stamp);
 
+	/** @return Active fusion algorithm type. */
 	virtual IMUFilter::Type type() const = 0;
+
+	/**
+	 * @brief Current orientation estimate.
+	 * @param qx Quaternion x
+	 * @param qy Quaternion y
+	 * @param qz Quaternion z
+	 * @param qw Quaternion w (scalar)
+	 */
 	virtual void getOrientation(double & qx, double & qy, double & qz, double & qw) const = 0;
+
+	/**
+	 * @brief Resets internal state to the given orientation.
+	 * @param qx Quaternion x (default 0)
+	 * @param qy Quaternion y (default 0)
+	 * @param qz Quaternion z (default 0)
+	 * @param qw Quaternion w (default 1, identity)
+	 */
 	virtual void reset(double qx = 0.0, double qy = 0.0, double qz = 0.0, double qw = 1.0) = 0;
 
 protected:
 	IMUFilter(const ParametersMap & parameters = ParametersMap()) : previousStamp_(0) {}
 
 private:
-	// Update from accelerometer and gyroscope data.
-	// [gx, gy, gz]: Angular veloctiy, in rad / s.
-	// [ax, ay, az]: Normalized gravity vector.
-	// dt: time delta, in seconds.
 	virtual void updateImpl(
 			double gx, double gy, double gz,
 			double ax, double ay, double az,
 			double dt) = 0;
 
-private:
 	double previousStamp_;
 };
 
-}
+} // namespace rtabmap
-
 
 #endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_ */

corelib/include/rtabmap/core/IMUThread.h

@@ -37,26 +37,63 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <fstream>
 
-namespace rtabmap
+namespace rtabmap {
-{
 
 class IMUFilter;
 
 /**
- * Class IMUThread
+ * @class IMUThread
+ * @brief Background thread that replays IMU measurements from a CSV file.
  *
+ * Reads gyroscope and accelerometer samples from a comma-separated file (EuRoC-style
+ * or epoch timestamps), optionally fuses them with @ref IMUFilter, and posts
+ * @ref IMUEvent on each step.
+ *
+ * CSV format:
+ * - First line: header (skipped).
+ * - Following lines: `stamp,gx,gy,gz,ax,ay,az` (stamp in seconds with a decimal
+ *   point, or EuRoC nanoseconds without one).
+ *
+ * Playback rate is limited by @ref setRate() when `rate > 0`; otherwise samples are
+ * read as fast as possible (or spaced using inter-sample timestamps after the first
+ * pair). When the file ends, the thread posts an empty @ref IMUEvent and stops.
+ *
+ * @see IMUEvent
+ * @see IMUFilter
+ * @see SensorCaptureThread::enableIMUFiltering()
  */
 class RTABMAP_CORE_EXPORT IMUThread :
 	public UThread,
 	public UEventsSender
 {
 public:
+	/**
+	 * @brief Constructs the replay thread.
+	 * @param rate Target playback rate in Hz (`0` = no rate cap until timestamps apply).
+	 * @param localTransform IMU frame relative to the robot base (stored in each @ref IMU).
+	 */
 	IMUThread(int rate, const Transform & localTransform);
 	virtual ~IMUThread();
 
+	/**
+	 * @brief Opens and validates an IMU CSV file.
+	 * @param path Path to the CSV file.
+	 * @return False if the file is missing or contains no data rows.
+	 */
 	bool init(const std::string & path);
+
+	/** @brief Sets the target playback rate in Hz. */
 	void setRate(int rate);
-	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
+
+	/**
+	 * @brief Enables orientation fusion on replayed samples.
+	 * @param filteringStrategy @ref IMUFilter::Type index (`0` = Madgwick, `1` = complementary).
+	 * @param parameters Optional ImuFilter/... tuning parameters.
+	 * @param baseFrameConversion If true, rotate IMU vectors into the base frame before filtering.
+	 */
+	void enableIMUFiltering(int filteringStrategy = 1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
+
+	/** @brief Disables fusion and deletes the internal @ref IMUFilter. */
 	void disableIMUFiltering();
 
 private:

corelib/include/rtabmap/core/LASWriter.h

@@ -28,16 +28,57 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_
 #define CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_
 
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
+#include <string>
+#include <vector>
 
 namespace rtabmap {
 
-int saveLASFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud,          const std::vector<int> & cameraIds = std::vector<int>());
+/**
-int saveLASFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud,       const std::vector<int> & cameraIds = std::vector<int>(), const std::vector<float> & intensities = std::vector<float>());
+ * @brief Writes a PCL point cloud to a LAS/LAZ file (requires RTAB-Map built with libLAS).
-int saveLASFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud,         const std::vector<int> & cameraIds = std::vector<int>());
+ *
+ * Output uses 1 mm XYZ scale (`0.001`). The file extension (`.las` or `.laz`) selects
+ * uncompressed or compressed output when libLAS LAZ support is available.
+ *
+ * @param filePath Output path (`.las` or `.laz`).
+ * @param cloud Input point cloud.
+ * @param cameraIds Optional per-point camera/signature ids (stored as point source ID);
+ *                  must be empty or the same length as @p cloud.
+ * @return `0` on success, `1` on error (e.g. LAZ not supported).
+ */
+int RTABMAP_CORE_EXPORT saveLASFile(
+		const std::string & filePath,
+		const pcl::PointCloud<pcl::PointXYZ> & cloud,
+		const std::vector<int> & cameraIds = std::vector<int>());
 
-}
+/**
+ * @brief Writes a colored point cloud to LAS/LAZ, with optional intensity and camera ids.
+ * @param filePath Output path (`.las` or `.laz`).
+ * @param cloud RGB point cloud (8-bit channels mapped to 16-bit LAS color).
+ * @param cameraIds Optional per-point ids (same length as @p cloud or empty).
+ * @param intensities Optional per-point intensities (same length as @p cloud or empty).
+ * @return `0` on success, `1` on error.
+ */
+int RTABMAP_CORE_EXPORT saveLASFile(
+		const std::string & filePath,
+		const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
+		const std::vector<int> & cameraIds = std::vector<int>(),
+		const std::vector<float> & intensities = std::vector<float>());
 
+/**
+ * @brief Writes a point cloud with intensity channel to LAS/LAZ.
+ * @param filePath Output path (`.las` or `.laz`).
+ * @param cloud XYZI point cloud (coordinates and intensity written).
+ * @param cameraIds Optional per-point ids (same length as @p cloud or empty).
+ * @return `0` on success, `1` on error.
+ */
+int RTABMAP_CORE_EXPORT saveLASFile(
+		const std::string & filePath,
+		const pcl::PointCloud<pcl::PointXYZI> & cloud,
+		const std::vector<int> & cameraIds = std::vector<int>());
+
+} // namespace rtabmap
 
 #endif /* CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_ */

corelib/include/rtabmap/core/Landmark.h

@@ -36,65 +36,99 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-class Landmark
+/**
+ * @class Landmark
+ * @brief Optimized pose of a visual landmark (e.g. ArUco/AprilTag) in the map.
+ *
+ * Stores a positive landmark @ref id(), optional physical @ref size() (m), world
+ * @ref pose(), and a 6×6 pose covariance matrix (x, y, z, roll, pitch, yaw).
+ *
+ * Used in @ref Landmarks maps on @ref SensorData and in memory during SLAM.
+ * Graph constraints to landmarks use @ref Link::kLandmark with a **negative**
+ * landmark id on the link (`to` is typically `-id()`).
+ *
+ * Covariance diagonal entries must be finite and strictly positive. Use a large
+ * value (e.g. `9999`) on angular axes when orientation uncertainty is unknown.
+ *
+ * @see SensorData::setLandmarks()
+ * @see Signature::addLandmark()
+ */
+class RTABMAP_CORE_EXPORT Landmark
 {
 public:
+	/** @brief Default-constructs an invalid landmark (`id == 0`). */
 	Landmark() :
 		id_(0),
-        size_(0.0f)
+		size_(0.0f)
 	{}
-    Landmark(const int & id, const float & size, const Transform & pose, const cv::Mat & covariance) :
+
-        id_(id),
+	/**
-        size_(size),
+	 * @brief Constructs a landmark with size, pose, and covariance.
-        pose_(pose),
+	 * @param id Positive landmark identifier.
-        covariance_(covariance)
+	 * @param size Physical size of the marker (m); `0` if unknown.
-    {
+	 * @param pose Landmark pose in the base frame (typically the robot frame).
-        UASSERT(id_>0);
+	 * @param covariance 6×6 `CV_64FC1` covariance (diagonal entries must be > 0).
-        UASSERT(!pose_.isNull());
+	 */
-        UASSERT(covariance_.cols == 6 && covariance_.rows == 6 && covariance_.type() == CV_64FC1);
+	Landmark(const int & id, const float & size, const Transform & pose, const cv::Mat & covariance) :
-        UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(0,0)).c_str());
+		id_(id),
-        UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(1,1)).c_str());
+		size_(size),
-        UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(2,2)).c_str());
+		pose_(pose),
-        UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(3,3)).c_str());
+		covariance_(covariance)
-        UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(4,4)).c_str());
+	{
-        UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(5,5)).c_str());
+		UASSERT(id_>0);
-    }
+		UASSERT(!pose_.isNull());
-    // Use constructor with size=0 instead.
+		UASSERT(covariance_.cols == 6 && covariance_.rows == 6 && covariance_.type() == CV_64FC1);
-    RTABMAP_DEPRECATED Landmark(const int & id, const Transform & pose, const cv::Mat & covariance);
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(0,0)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(1,1)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(2,2)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(3,3)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(4,4)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(5,5)).c_str());
+	}
+
+	/**
+	 * @brief Deprecated; use the constructor with explicit @p size (`0` if unknown).
+	 */
+	RTABMAP_DEPRECATED Landmark(const int & id, const Transform & pose, const cv::Mat & covariance);
 
 	virtual ~Landmark() {}
 
+	/** @return Positive landmark id (map key in @ref Landmarks). */
 	const int & id() const {return id_;}
-    const float & size() const {return size_;}
+	/** @return Marker size in metres (`0` if unknown). */
+	const float & size() const {return size_;}
+	/** @return Landmark pose in the map frame. */
 	const Transform & pose() const {return pose_;}
+	/** @return 6×6 pose covariance (`CV_64FC1`). */
 	const cv::Mat & covariance() const {return covariance_;}
 
 private:
 	int id_;
-    float size_;
+	float size_;
 	Transform pose_;
 	cv::Mat covariance_;
 };
 
+/** @brief Map of landmark id → @ref Landmark (typically positive keys). */
 typedef std::map<int, Landmark> Landmarks;
 
 inline Landmark::Landmark(const int & id, const Transform & pose, const cv::Mat & covariance) :
-    id_(id),
+	id_(id),
-    size_(0.0f),
+	size_(0.0f),
-    pose_(pose),
+	pose_(pose),
-    covariance_(covariance)
+	covariance_(covariance)
 {
-    UASSERT(id_>0);
+	UASSERT(id_>0);
-    UASSERT(!pose_.isNull());
+	UASSERT(!pose_.isNull());
-    UASSERT(covariance_.cols == 6 && covariance_.rows == 6 && covariance_.type() == CV_64FC1);
+	UASSERT(covariance_.cols == 6 && covariance_.rows == 6 && covariance_.type() == CV_64FC1);
-    UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(0,0)).c_str());
+	UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(0,0)).c_str());
-    UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(1,1)).c_str());
+	UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(1,1)).c_str());
-    UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(2,2)).c_str());
+	UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(2,2)).c_str());
-    UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(3,3)).c_str());
+	UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(3,3)).c_str());
-    UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(4,4)).c_str());
+	UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(4,4)).c_str());
-    UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(5,5)).c_str());
+	UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(5,5)).c_str());
 }
 
-}
+} // namespace rtabmap
 
 #endif /* CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_ */

corelib/include/rtabmap/core/LaserScan.h

@@ -34,22 +34,42 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class LaserScan
+ * @brief Represents 2D or 3D laser scan data with support for multiple point data formats.
+ *
+ * The LaserScan class stores structured laser scan data used in SLAM, mapping, and perception.
+ * It supports various formats including point coordinates, intensity, normals, RGB colors,
+ * and timestamps. Utility methods are provided for format checking, cloning, and combining scans.
+ */
 class RTABMAP_CORE_EXPORT LaserScan
 {
 public:
-	enum Format{kUnknown=0,
+	/**
-		kXY=1,
+	 * @brief Enumeration of possible formats for laser scan data.
-		kXYI=2,
+	 *
-		kXYNormal=3,
+	 * These values represent different combinations of point attributes
-		kXYINormal=4,
+	 * that can be stored in a laser scan. The format determines how each
-		kXYZ=5,
+	 * point in the scan is structured.
-		kXYZI=6,
+	 */
-		kXYZRGB=7,
+	enum Format{
-		kXYZNormal=8,
+		kUnknown=0,			/**< Unknown format. */
-		kXYZINormal=9,
+		kXY=1,				/**< 2D points with X and Y coordinates. */
-		kXYZRGBNormal=10,
+		kXYI=2,				/**< 2D points with X, Y and intensity. */
-		kXYZIT=11};
+		kXYNormal=3,		/**< 2D points with X, Y and normal vectors. */
+		kXYINormal=4,		/**< 2D points with X, Y, intensity and normal vectors. */
+		kXYZ=5,				/**< 3D points with X, Y and Z coordinates. */
+		kXYZI=6,			/**< 3D points with X, Y, Z and intensity. */
+		kXYZRGB=7,			/**< 3D points with X, Y, Z and RGB color. */
+		kXYZNormal=8,		/**< 3D points with X, Y, Z and normal vectors. */
+		kXYZINormal=9,		/**< 3D points with X, Y, Z, intensity and normal vectors. */
+		kXYZRGBNormal=10,	/**< 3D points with X, Y, Z, RGB color and normal vectors. */
+		kXYZIT=11,			/**< 3D points with X, Y, Z, intensity and time. */
+		kXYZIRT=12			/**< 3D points with X, Y, Z, intensity, ring and time. */
+	};
 
+	/// @name Static Utility Functions
+	/// @{
 	static std::string formatName(const Format & format);
 	static int channels(const Format & format);
 	static bool isScan2d(const Format & format);
@@ -57,11 +77,22 @@ public:
 	static bool isScanHasRGB(const Format & format);
 	static bool isScanHasIntensity(const Format & format);
 	static bool isScanHasTime(const Format & format);
+	static bool isScanHasRing(const Format & format);
+	static float packRGB(unsigned char r, unsigned char g, unsigned char b);
+	static void unpackRGB(float rgb, unsigned char & r, unsigned char & g, unsigned char & b);
+
+	/**
+	 * @brief Converts legacy scan format to a LaserScan object.
+	 */
 	static LaserScan backwardCompatibility(
 			const cv::Mat & oldScanFormat,
 			int maxPoints = 0,
 			int maxRange = 0,
 			const Transform & localTransform = Transform::getIdentity());
+	
+	/**
+	 * @brief Converts legacy scan format with additional metadata to a LaserScan.
+	 */
 	static LaserScan backwardCompatibility(
 			const cv::Mat & oldScanFormat,
 			float minRange,
@@ -70,14 +101,19 @@ public:
 			float angleMax,
 			float angleInc,
 			const Transform & localTransform = Transform::getIdentity());
+	/// @}
 
 public:
+
+	/// @name Constructors
+	/// @{
 	LaserScan();
 	LaserScan(const LaserScan & data,
 			int maxPoints,
 			float maxRange,
 			const Transform & localTransform = Transform::getIdentity());
-	// Use version without \"format\" argument.
+	
+	/// @deprecated Use constructor without `format` argument.
 	RTABMAP_DEPRECATED LaserScan(const LaserScan & data,
 			int maxPoints,
 			float maxRange,
@@ -88,7 +124,8 @@ public:
 			float maxRange,
 			Format format,
 			const Transform & localTransform = Transform::getIdentity());
-	// Use version without \"format\" argument.
+	
+	/// @deprecated Use constructor without `format` argument.
 	RTABMAP_DEPRECATED LaserScan(const LaserScan & data,
 			Format format,
 			float minRange,
@@ -112,7 +149,10 @@ public:
 			float angleMax,
 			float angleIncrement,
 			const Transform & localTransform = Transform::getIdentity());
+	/// @}
 
+	/// @name Accessors
+	/// @{
 	const cv::Mat & data() const {return data_;}
 	Format format() const {return format_;}
 	std::string formatName() const {return formatName(format_);}
@@ -125,38 +165,58 @@ public:
 	float angleIncrement() const {return angleIncrement_;}
 	void setLocalTransform(const Transform & t) {localTransform_ = t;}
 	Transform localTransform() const {return localTransform_;}
+	/// @}
 
+	/// @name Status and Format Checks
+	/// @{
 	bool empty() const {return data_.empty();}
 	bool isEmpty() const {return data_.empty();}
-	int size() const {return data_.total();}
+	int size() const {return static_cast<int>(data_.total());}
 	int dataType() const {return data_.type();}
 	bool is2d() const {return isScan2d(format_);}
 	bool hasNormals() const {return isScanHasNormals(format_);}
 	bool hasRGB() const {return isScanHasRGB(format_);}
 	bool hasIntensity() const {return isScanHasIntensity(format_);}
 	bool hasTime() const {return isScanHasTime(format_);}
+	bool hasRing() const {return isScanHasRing(format_);}
+	/** @brief True if the scan is stored compressed (single-row @c CV_8UC1 blob) instead of raw points. */
 	bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
 	bool isOrganized() const {return data_.rows > 1;}
 	LaserScan clone() const;
 	LaserScan densify() const;
+	/// @}
 
+	/// @name Operations
+	/// @{
 	int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
 	int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
 	int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
-	int getTimeOffset() const {return hasTime()?4:-1;}
+	int getRingOffset() const {return format_==kXYZIRT?4:-1;}
+	int getTimeOffset() const {return format_==kXYZIT?4:(format_==kXYZIRT?5:-1);}
 
+	/**
+	 * @brief Access a specific field value of a point.
+	 * @param pointIndex Index of the point.
+	 * @param channelOffset Channel offset to access (e.g., 0=X, 1=Y, 2=Z if 2D, for other fields, use corresponding getter functions).
+	 * @return Reference to the field value.
+	 * @see getIntensityOffset() getRGBOffset() getNormalsOffset() getTimeOffset()
+	 */
 	float & field(unsigned int pointIndex, unsigned int channelOffset);
 
+	/**
+	 * @brief Clear the scan data.
+	 */
 	void clear() {data_ = cv::Mat();}
 
 	/**
-	 * Concatenate scan's data, localTransform is ignored.
+	 * @brief Concatenate scan's data (localTransform is ignored).
 	 */
 	LaserScan & operator+=(const LaserScan &);
 	/**
-	 * Concatenate scan's data, localTransform is ignored.
+	 * @brief Concatenate scan's data (localTransform is ignored).
 	 */
 	LaserScan operator+(const LaserScan &);
+	/// @}
 
 private:
 	void init(const cv::Mat & data,
@@ -170,17 +230,17 @@ private:
 			const Transform & localTransform = Transform::getIdentity());
 
 private:
-	cv::Mat data_;
+	cv::Mat data_;				///< The scan data matrix.
-	Format format_;
+	Format format_;				///< The scan data format.
-	int maxPoints_;
+	int maxPoints_;				///< Maximum number of points allowed.
-	float rangeMin_;
+	float rangeMin_;			///< Minimum valid range.
-	float rangeMax_;
+	float rangeMax_;			///< Maximum valid range.
-	float angleMin_;
+	float angleMin_;			///< Minimum angle (for 2D scans).
-	float angleMax_;
+	float angleMax_;			///< Maximum angle (for 2D scans).
-	float angleIncrement_;
+	float angleIncrement_;		///< Angular increment (for 2D scans).
-	Transform localTransform_;
+	Transform localTransform_;	///< Transform from base frame to scan frame.
 };
 
-}
+} // namespace rtabmap
 
 #endif /* CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_ */

corelib/include/rtabmap/core/Link.h

@@ -35,58 +35,127 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class Link
+ * @brief Directed constraint between two nodes in RTAB-Map's pose graph.
+ *
+ * A link connects signature @ref from() to signature @ref to() with a relative
+ * @ref Transform and an information matrix (inverse covariance) used by graph
+ * optimization. Links are stored on @ref Signature objects and persisted in the
+ * database; they define odometry chains, loop closures, landmarks, and priors.
+ *
+ * The transform is expressed from the @p from node frame to the @p to node frame
+ * (i.e. pose of @p to relative to @p from), unless @ref type() indicates a
+ * special semantics (e.g. @ref kPosePrior, @ref kLandmark).
+ *
+ * @see Signature
+ * @see Memory::addLink()
+ */
 class RTABMAP_CORE_EXPORT Link
 {
 public:
+	/**
+	 * @brief Link category and filter sentinels.
+	 *
+	 * Values @ref kSelfRefLink, @ref kAllWithLandmarks, @ref kAllWithoutLandmarks and
+	 * @ref kUndef are also used as query filters when retrieving links from memory or
+	 * the database (they are not stored as link types on signatures).
+	 */
 	enum Type {
-		kNeighbor,
+		kNeighbor,           /**< Sequential odometry link between consecutive nodes. */
-		kGlobalClosure,
+		kGlobalClosure,      /**< Global loop closure added by global loop closure detection (i.e., using bags-of-words to find loop closures with other nodes in WM without using current estimated pose). */
-		kLocalSpaceClosure,
+		kLocalSpaceClosure,  /**< Local loop closure added by proximity detection by space (i.e. using current estimated pose to find loop closures with nearby nodes in WM). */
-		kLocalTimeClosure,
+		kLocalTimeClosure,   /**< Local loop closure added by proximity detection by time (i.e. between nodes in STM). */
-		kUserClosure,
+		kUserClosure,        /**< User-defined loop closure constraint. */
-		kVirtualClosure,
+		kVirtualClosure,     /**< Virtual link added to keep the path linked to local map. */
-		kNeighborMerged,
+		kNeighborMerged,     /**< Merged neighbor link after graph reduction. */
-		kPosePrior, // Absolute pose in /world frame, From == To
+		kPosePrior,          /**< Absolute pose prior in the world frame (@p from == @p to). */
-		kLandmark,  // Transform /base_link -­­> /landmark, "From" is node observing the landmark "To" (landmark is negative id)
+		kLandmark,           /**< Observation of a landmark: @p from is the observer node, @p to is a negative landmark id. */
-		kGravity,  // Orientation of the base frame accordingly to gravity (From == To)
+		kGravity,            /**< Gravity direction constraint on the base frame (@p from == @p to). */
-		kEnd,
+		kEnd,                /**< Sentinel: number of link types that can be stored. */
-		kSelfRefLink = 97, // Include kPosePrior and kGravity (all links where From=To)
+		kSelfRefLink = 97,   /**< Filter: links where @p from == @p to (e.g. @ref kPosePrior, @ref kGravity). */
-		kAllWithLandmarks = 98,
+		kAllWithLandmarks = 98,    /**< Filter: all link types including @ref kLandmark. */
-		kAllWithoutLandmarks = 99,
+		kAllWithoutLandmarks = 99, /**< Filter: all link types except @ref kLandmark. */
-		kUndef = 99};
+		kUndef = 99                /**< Undefined type or invalid link. */
+	};
+
+	/** @return Human-readable name for @p type (e.g. "Neighbor", "GlobalClosure"). */
 	static std::string typeName(Type type);
 
+	/** @brief Default constructor; creates an invalid link (@ref kUndef). */
 	Link();
+	/**
+	 * @brief Constructs a link between two nodes.
+	 * @param from Source signature id.
+	 * @param to Target signature id (negative for landmarks when @p type is @ref kLandmark).
+	 * @param type Link category.
+	 * @param transform Relative transform from @p from to @p to.
+	 * @param infMatrix 6x6 information matrix (inverse covariance)
+	 * @param userData Optional payload; compressed automatically if not already @c CV_8UC1.
+	 */
 	Link(int from,
 			int to,
 			Type type,
 			const Transform & transform,
-			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1), // information matrix: inverse of covariance matrix
+			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1),
 			const cv::Mat & userData = cv::Mat());
 
+	/** @return True if ids, transform, and type are valid for use in the graph. */
 	bool isValid() const {return from_ != 0 && to_ != 0 && !transform_.isNull() && type_!=kUndef;}
 
+	/** @return Source signature id. */
 	int from() const {return from_;}
+	/** @return Target signature id (negative for a landmark). */
 	int to() const {return to_;}
+	/** @return Relative transform, from the @ref from() frame to the @ref to() frame. */
 	const Transform & transform() const {return transform_;}
+	/** @return Link category (see @ref Type). */
 	Type type() const {return type_;}
 	std::string typeName() const {return typeName(type_);}
 	const cv::Mat & infMatrix() const {return infMatrix_;}
+
+	/**
+	 * @brief Rotation variance derived from the information matrix diagonal (roll, pitch, yaw).
+	 * @param minimum If true, returns the largest diagonal entry (most uncertain axis);
+	 *                if false, returns the smallest non-zero entry.
+	 */
 	double rotVariance(bool minimum = true) const;
+	/**
+	 * @brief Translation variance derived from the information matrix diagonal (x, y, z).
+	 * @param minimum If true, returns the largest diagonal entry; if false, the smallest non-zero entry.
+	 */
 	double transVariance(bool minimum = true) const;
 
 	void setFrom(int from) {from_ = from;}
 	void setTo(int to) {to_ = to;}
 	void setTransform(const Transform & transform) {transform_ = transform;}
 	void setType(Type type) {type_ = type;}
+	/** @brief Sets the 6x6 information matrix (@c CV_64FC1); diagonal entries must be positive and finite. */
     void setInfMatrix(const cv::Mat & infMatrix);
 
+	/** @return Uncompressed user data, empty unless it was set raw or @ref uncompressUserData() was called. */
 	const cv::Mat & userDataRaw() const {return _userDataRaw;}
+	/** @return User data as stored/persisted, in compressed form. */
 	const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
+	/** @brief Decompresses user data into @ref userDataRaw() if compressed data is stored. */
 	void uncompressUserData();
+	/** @return Uncompressed user data without modifying internal storage. */
 	cv::Mat uncompressUserDataConst() const;
 
+	/**
+	 * @brief Chains this link (from → to) with @p link (to → link.to).
+	 * @param link Second link; must satisfy @c this->to() == link.from().
+	 * @param outputType Type of the merged link.
+	 * @return Single link from @ref from() to @p link.to() with transform
+	 *         \(T_{ac} = T_{ab} T_{bc}\) (or null if either input transform is null).
+	 *         Information matrix handling depends on @p outputType:
+	 *         - @ref kNeighborMerged "kNeighborMerged": \(\Omega_{ac} = (\Omega_{ab}^{-1} + \Omega_{bc}^{-1})^{-1}\)
+	 *           (covariances add when both legs are diagonal and independent).
+	 *         - Other types: keeps the full information matrix of @p link unless
+	 *           \(\Omega_{ab}(0,0) < \Omega_{bc}(0,0)\) (i.e. the smaller x information entry).
+	 */
 	Link merge(const Link & link, Type outputType) const;
+	/** @return Link with swapped endpoints and inverted transform. */
 	Link inverse() const;
 
 private:

corelib/include/rtabmap/core/LocalGrid.h

@@ -35,30 +35,72 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class LocalGrid
+ * @brief Local occupancy grid cells for one map node (ground, obstacles, empty).
+ *
+ * Each layer is stored as a 1×N `cv::Mat` of cell coordinates in the robot/base
+ * frame. Layouts match @ref LaserScan grid storage (@ref LaserScan::Format):
+ * - `CV_32FC2` (2 ch): `kXY` — (x, y)
+ * - `CV_32FC3` (3 ch): `kXYZ` or `kXYI` — (x, y, z) or (x, y, intensity)
+ * - `CV_32FC4` (4 ch): `kXYZI` or `kXYZRGB` — (x, y, z, intensity) or (x, y, z, RGB)
+ * - `CV_32FC5` (5 ch): `kXYNormal` or `kXYZIT` — 2D (x, y, nx, ny, nz) or 3D (x, y, z, intensity, time)
+ * - `CV_32FC6` (6 ch): `kXYINormal`, `kXYZNormal` or `kXYZIRT`
+ * - `CV_32FC7` (7 ch): `kXYZINormal` or `kXYZRGBNormal`
+ *
+ * @ref is3D() is true when every non-empty layer is `CV_32FC3`, `CV_32FC4` or `CV_32FC6`
+ * (5-channel layers are treated as 2D by @ref OccupancyGrid). Extra fields are not used for pose/projection.
+ * @ref cellSize is the grid resolution in meters (must be > 0).
+ * @ref viewPoint is the sensor/view origin used when the grid was built.
+ *
+ * @see LaserScan
+ * @see LocalGridMaker
+ * @see OccupancyGrid
+ * @see LocalGridCache
+ */
 class RTABMAP_CORE_EXPORT LocalGrid
 {
 public:
+	/**
+	 * @brief Builds a local grid from cell matrices.
+	 * @param ground Ground cell coordinates (may be empty).
+	 * @param obstacles Obstacle cell coordinates (may be empty).
+	 * @param empty Empty/free cell coordinates (may be empty).
+	 * @param cellSize Grid cell size in meters (must be > 0).
+	 * @param viewPoint View/sensor origin in the grid frame.
+	 */
 	LocalGrid(const cv::Mat & ground,
 		 const cv::Mat & obstacles,
 		 const cv::Mat & empty,
 		 float cellSize,
 		 const cv::Point3f & viewPoint = cv::Point3f(0,0,0));
 	virtual ~LocalGrid() {}
+
+	/** @return True if every non-empty layer is `CV_32FC3`, `CV_32FC4` or `CV_32FC6` (see class doc). */
 	bool is3D() const;
-public:
+
-	cv::Mat groundCells;
+	cv::Mat groundCells;   ///< Ground cells (1×N, `CV_32FC2`–`CV_32FC7`, see class doc).
-	cv::Mat obstacleCells;
+	cv::Mat obstacleCells; ///< Obstacle cells (same layout as @ref groundCells).
-	cv::Mat emptyCells;
+	cv::Mat emptyCells;    ///< Empty/free cells (same layout as @ref groundCells).
-	float cellSize;
+	float cellSize;        ///< Cell size in meters.
-	cv::Point3f viewPoint;
+	cv::Point3f viewPoint; ///< View point used to build the grid.
 };
 
+/**
+ * @class LocalGridCache
+ * @brief Cache of @ref LocalGrid entries keyed by map node id.
+ *
+ * Used by @ref GlobalMap and its subclasses (e.g. @ref OccupancyGrid) to hold per-node
+ * local grids before assembly. Node id `0` is stored as `-1` (temporary grid). Negative ids
+ * passed to @ref add() are rejected.
+ */
 class RTABMAP_CORE_EXPORT LocalGridCache
 {
 public:
 	LocalGridCache() {}
 	virtual ~LocalGridCache() {}
 
+	/** @brief Inserts or replaces the grid for @p nodeId (from separate cell mats). */
 	void add(int nodeId,
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
@@ -66,11 +108,22 @@ public:
 			float cellSize,
 			const cv::Point3f & viewPoint = cv::Point3f(0,0,0));
 
+	/** @brief Inserts or replaces the grid for @p nodeId. */
 	void add(int nodeId, const LocalGrid & localGrid);
 
+	/**
+	 * @brief Copies a grid to @p anotherCache if present here and absent there.
+	 * @return True if a grid was shared.
+	 */
 	bool shareTo(int nodeId, LocalGridCache & anotherCache) const;
 
+	/** @brief Approximate memory used by cached grids (bytes). */
 	unsigned long getMemoryUsed() const;
+
+	/**
+	 * @brief Removes cached grids.
+	 * @param temporaryOnly If true, removes only entries with negative ids (e.g. `-1`).
+	 */
 	void clear(bool temporaryOnly = false);
 
 	size_t size() const {return localGrids_.size();}

corelib/include/rtabmap/core/LocalGridMaker.h

@@ -39,17 +39,61 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class LocalGridMaker
+ * @brief Builds per-node local occupancy grids from laser scans or depth clouds.
+ *
+ * Configured via `Grid/` parameters (@ref parseParameters()). Used by @ref Memory
+ * to populate @ref LocalGrid cells (ground / obstacles / empty) stored on
+ * @ref SensorData and cached in @ref LocalGridCache for global maps
+ * (@ref OccupancyGrid, @ref OctoMap, @ref GridMap).
+ *
+ * **Sensor source** (`Grid/Sensor`, @ref isGridFromDepth()):
+ * - `0` — laser scan only
+ * - `1` — depth image(s) only (default)
+ * - `2` — laser scan and depth
+ *
+ * **2D laser path:** projects a 2D @ref LaserScan with @ref util3d::occupancy2DFromLaserScan().
+ *
+ * **3D path:** segments the scan/cloud into ground and obstacles (@ref segmentCloud()),
+ * then outputs @ref LocalGrid cell matrices (2D projection or 3D cells per `Grid/3D`).
+ *
+ * @see LocalGrid
+ * @see Memory
+ */
 class RTABMAP_CORE_EXPORT LocalGridMaker
 {
 public:
+	/** @brief Constructs with @ref parseParameters() on @p parameters (or defaults). */
 	LocalGridMaker(const ParametersMap & parameters = ParametersMap());
 	virtual ~LocalGridMaker();
+
+	/** @brief Updates grid settings from `Grid/` entries in @p parameters. */
 	virtual void parseParameters(const ParametersMap & parameters);
 
+	/** @return Current `Grid/CellSize` (m). */
 	float getCellSize() const {return cellSize_;}
+	/** @return True if occupancy is built from depth (`Grid/Sensor` is 1 or 2). */
 	bool isGridFromDepth() const {return occupancySensor_;}
+	/** @return True if `Grid/MapFrameProjection` is enabled. */
 	bool isMapFrameProjection() const {return projMapFrame_;}
 
+	/**
+	 * @brief Segments a point cloud into ground and obstacle indices.
+	 *
+	 * Applies optional voxel filtering (`Grid/PreVoxelFiltering`, leaf size `Grid/CellSize`),
+	 * footprint crop, height filtering, then either normal-based ground segmentation
+	 * (`Grid/NormalsSegmentation`) or Z passthrough.
+	 *
+	 * @param cloud Input cloud (sensor frame, transformed internally using @p pose). Must be non-null; an empty cloud is allowed and returns empty outputs.
+	 * @param indices Subset of @p cloud to process (must be non-null; empty = all points when the cloud is dense).
+	 * @param pose Node pose (used for map-frame projection and footprint).
+	 * @param viewPoint Sensor origin for segmentation / ray tracing.
+	 * @param groundIndices Output indices of ground points.
+	 * @param obstaclesIndices Output indices of obstacle points.
+	 * @param flatObstacles Optional output for flat obstacle clusters.
+	 * @return Segmented cloud (voxel-downsampled when `Grid/PreVoxelFiltering` is true, using `Grid/CellSize`).
+	 */
 	template<typename PointT>
 	typename pcl::PointCloud<PointT>::Ptr segmentCloud(
 			const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -60,6 +104,22 @@ public:
 			pcl::IndicesPtr & obstaclesIndices,     // output cloud indices
 			pcl::IndicesPtr * flatObstacles = 0) const; // output cloud indices
 
+	/**
+	 * @brief Creates a local grid from a @ref Signature's laser scan or depth data.
+	 * @param node Signature with sensor data (pose used for 3D scans).
+	 * @param groundCells Output ground cells (@ref LocalGrid format).
+	 * @param obstacleCells Output obstacle cells.
+	 * @param emptyCells Output empty/free cells. Behavior depends on the sensor path:
+	 *                   - **2D laser, laser-only mode** (`Grid/Sensor`=0 and a 2D @ref LaserScan):
+	 *                     uses @ref util3d::occupancy2DFromLaserScan() / @ref util3d::create2DMap().
+	 *                     Free space along each hit beam is always ray-traced (sensor → obstacle).
+	 *                     `Grid/Scan2dUnknownSpaceFilled` is separate: when true, it additionally
+	 *                     sweeps unknown angular gaps between the first and last hit (sparse FOV /
+	 *                     “holes” in coverage) out to `Grid/RangeMax` or scan max range.
+	 *                   - **Other cases** (3D scan, depth, `Grid/Sensor`=2, etc.): see
+	 *                     the @ref LaserScan overload of @ref createLocalMap().
+	 * @param viewPoint Output view point used for the grid.
+	 */
 	void createLocalMap(
 			const Signature & node,
 			cv::Mat & groundCells,
@@ -67,6 +127,19 @@ public:
 			cv::Mat & emptyCells,
 			cv::Point3f & viewPoint);
 
+	/**
+	 * @brief Creates a local grid from a 3D (or organized) @ref LaserScan.
+	 * @param cloud Input scan in sensor frame.
+	 * @param pose Node pose in map/odom frame.
+	 * @param groundCells Output ground cells.
+	 * @param obstacleCells Output obstacle cells.
+	 * @param emptyCells Output empty/free cells. Filled when `Grid/RayTracing` is true:
+	 *                   3D via OctoMap if `Grid/3D` and OctoMap support are enabled
+	 *                   (`Grid/RangeMax`, `Grid/CellSize`); otherwise 2D ray fill via
+	 *                   `occupancy2DFromLaserScan` when `Grid/3D` is false (`Grid/RangeMax`,
+	 *                   `Grid/CellSize`).
+	 * @param viewPointInOut View point (may be rotated if @ref isMapFrameProjection()).
+	 */
 	void createLocalMap(
 			const LaserScan & cloud,
 			const Transform & pose,
@@ -75,6 +148,26 @@ public:
 			cv::Mat & emptyCells,
 			cv::Point3f & viewPointInOut) const;
 
+private:
+	/**
+	 * @brief Radius filters the points closer than @p maxRange, keeping the farther
+	 *        points as-is (in case we want to ray trace empty space beyond max range).
+	 * @param cloud Input cloud.
+	 * @param indices Subset of @p cloud to filter. Asserted to be non-null and non-empty
+	 *                (@ref util3d::radiusFiltering() would filter the whole cloud otherwise).
+	 * @param maxRange Range (m) above which points are not filtered. 0 means no limit.
+	 * @param radiusSearch Radius (m) used for the noise filtering.
+	 * @param minNeighborsInRadius Minimum neighbors required inside @p radiusSearch.
+	 * @return Indices of the points that passed the filtering, plus those beyond @p maxRange.
+	 */
+	template<typename PointT>
+	static pcl::IndicesPtr noiseFilteringWithMaxRange(
+			const typename pcl::PointCloud<PointT>::Ptr & cloud,
+			const pcl::IndicesPtr & indices,
+			float maxRange,
+			float radiusSearch,
+			int minNeighborsInRadius);
+
 protected:
 	ParametersMap parameters_;
 
@@ -112,4 +205,4 @@ protected:
 
 #include <rtabmap/core/impl/LocalMapMaker.hpp>
 
-#endif /* SRC_MAP_H_ */
+#endif /* SRC_LOCAL_MAP_H_ */

corelib/include/rtabmap/core/MarkerDetector.h

@@ -32,14 +32,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/CameraModel.h>
 #include <opencv2/opencv_modules.hpp>
 
-#ifdef HAVE_OPENCV_ARUCO
+#if (CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)) && defined(HAVE_OPENCV_OBJDETECT)
+#include <opencv2/objdetect.hpp>
+#elif defined(HAVE_OPENCV_ARUCO)
 #include <opencv2/aruco.hpp>
 #endif
 
 namespace rtabmap {
 
-typedef std::map<int, Transform> MapIdPose;
-
 class MarkerInfo {
 public:
     MarkerInfo(int id, float length, Transform pose) :
@@ -58,19 +58,17 @@ private:
 
 class RTABMAP_CORE_EXPORT MarkerDetector {
 
+public:
+  enum Strategy {
+    kStrategyOpencv,
+    kStrategyApriltag
+  };
+    
 public:
 	MarkerDetector(const ParametersMap & parameters = ParametersMap());
 	virtual ~MarkerDetector();
 	void parseParameters(const ParametersMap & parameters);
 
-	// Use the other detect(), in which the returned map contains the length of each marker detected.
-    RTABMAP_DEPRECATED
-    MapIdPose detect(const cv::Mat & image,
-			const CameraModel & model,
-			const cv::Mat & depth = cv::Mat(),
-			float * estimatedMarkerLength = 0,
-			cv::Mat * imageWithDetections = 0);
-
     std::map<int, MarkerInfo> detect(const cv::Mat & image,
 		    const std::vector<CameraModel> & models,
 		    const cv::Mat & depth = cv::Mat(),
@@ -84,15 +82,22 @@ public:
 		    cv::Mat * imageWithDetections = 0);
 
 private:
-#ifdef HAVE_OPENCV_ARUCO
+  Strategy strategy_;
-	cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
+  float markerLength_;
-	float markerLength_;
+  std::map<int, float> markerLengths_;
 	float maxDepthError_;
 	float maxRange_;
 	float minRange_;
 	int dictionaryId_;
+#if ((CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)) && defined(HAVE_OPENCV_OBJDETECT)) || defined(HAVE_OPENCV_ARUCO)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)
+  cv::Ptr<cv::aruco::ArucoDetector> arucoDetector_;
+#endif
+  cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
 	cv::Ptr<cv::aruco::Dictionary> dictionary_;
 #endif
+  void * apriltagLibDetector_;
+  void * apriltagLibFamily_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Memory.h

@@ -41,7 +41,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <set>
 #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/pcl_config.h>
 
 namespace rtabmap {
@@ -61,47 +65,213 @@ class LocalGridMaker;
 class MarkerDetector;
 class GlobalDescriptorExtractor;
 
+/**
+ * @class Memory
+ * @brief Three-tiered memory management (STM, WM, LTM) for RTAB-Map.
+ *
+ * Memory is the core map data structure used by @ref Rtabmap. It stores observations
+ * as @ref Signature nodes connected by @ref Link edges and orchestrates their lifecycle
+ * across three tiers:
+ *
+ * - **Short-Term Memory (STM)**: most recently added signatures, kept at a fixed size
+ *   (see @ref Parameters::kMemSTMSize()). Used to delay recently observed places before
+ *   they become candidates for loop closure.
+ * - **Working Memory (WM)**: signatures available for loop-closure likelihood
+ *   computation in the current iteration. Older signatures are transferred from WM
+ *   to LTM by @ref forget() to bound iteration time.
+ * - **Long-Term Memory (LTM)**: persisted in the database via @ref DBDriver. Signatures
+ *   can be brought back to WM with @ref reactivateSignatures() when their neighbors still 
+ *   in WM are good loop-closure candidates.
+ *
+ * The class also owns a visual word dictionary (@ref VWDictionary), feature extractor
+ * (@ref Feature2D) and registration pipelines (@ref Registration, @ref RegistrationVis,
+ * @ref RegistrationIcp) used to compute relative transforms between signatures.
+ *
+ * Typical iteration: @ref update() adds a new @ref SensorData as a @ref Signature in
+ * STM; @ref computeLikelihood() scores it against WM; the @ref Rtabmap caller decides
+ * on loop closures with @ref BayesFilter; @ref cleanup() drops bad signatures;
+ * @ref forget() transfers oldest WM signatures to LTM and @ref reactivateSignatures()
+ * pulls relevant ones back.
+ *
+ * @see Signature
+ * @see DBDriver
+ * @see VWDictionary
+ * @see Rtabmap
+ */
 class RTABMAP_CORE_EXPORT Memory
 {
 public:
+	/** @brief First valid signature id assigned to a new signature (positive integer). */
 	static const int kIdStart;
+	/** @brief Reserved id for the "virtual place" used by the Bayes filter (negative). */
 	static const int kIdVirtual;
+	/** @brief Sentinel value indicating an invalid signature id (zero). */
 	static const int kIdInvalid;
 
 public:
+	/**
+	 * @brief Constructs a Memory instance with the given parameters.
+	 *
+	 * The database is not opened here; call @ref init() to open or create a database
+	 * and load persisted state. @p parameters may include any key from @ref Parameters
+	 * (memory, keypoint, registration, etc.); missing keys fall back to defaults.
+	 */
 	Memory(const ParametersMap & parameters = ParametersMap());
 	virtual ~Memory();
 
+	/**
+	 * @brief Re-parses parameters and propagates them to owned sub-objects.
+	 *
+	 * Forwards the relevant subset to @ref VWDictionary, @ref Feature2D, the registration
+	 * pipelines and the database driver. Safe to call at runtime to change settings.
+	 */
 	virtual void parseParameters(const ParametersMap & parameters);
+	/** @return The most recent parameter map applied via the constructor or @ref parseParameters(). */
 	virtual const ParametersMap & getParameters() const {return parameters_;}
+	/**
+	 * @brief Adds a sensor observation to the map (overload without odometry pose).
+	 *
+	 * Equivalent to calling the full @ref update() with an identity pose and empty covariance.
+	 * The new signature is added to STM; oldest STM entries are promoted to WM as needed.
+	 *
+	 * @param data Sensor data (images, scan, user data, odometry features) for this frame.
+	 * @param stats Optional output statistics receiver for timing/diagnostic values.
+	 * @return True if a signature was successfully created and added, false otherwise.
+	 */
 	bool update(const SensorData & data,
 			Statistics * stats = 0);
+	/**
+	 * @brief Adds a sensor observation with odometry pose and velocity to the map.
+	 *
+	 * Creates a new @ref Signature, extracts visual words, links it to the previous
+	 * STM signature with a neighbor link, runs rehearsal against STM and promotes
+	 * the oldest STM signature to WM if STM is full.
+	 *
+	 * @param data Sensor data (images, scan, user data, odometry features).
+	 * @param pose Odometry pose at this frame (null if odometry not used).
+	 * @param covariance 6x6 odometry covariance (or empty if null odometry is provided).
+	 * @param velocity Optional 6-vector (vx, vy, vz, vroll, vpitch, vyaw).
+	 * @param stats Optional output statistics receiver.
+	 * @return True on success, false if signature creation failed.
+	 */
 	bool update(const SensorData & data,
 			const Transform & pose,
 			const cv::Mat & covariance,
 			const std::vector<float> & velocity = std::vector<float>(), // vx,vy,vz,vroll,vpitch,vyaw
 			Statistics * stats = 0);
+	/**
+	 * @brief Opens or creates a database and loads existing state into WM.
+	 *
+	 * @param dbUrl Path to the database file (empty for in-memory).
+	 * @param dbOverwritten If true, deletes the existing file before opening.
+	 * @param parameters Optional parameter override applied before loading.
+	 * @param postInitClosingEvents If true, posts @ref RtabmapEventInit events for
+	 *        progress reporting (used by GUI).
+	 * @return True on success, false if the database could not be opened.
+	 */
 	bool init(const std::string & dbUrl,
 			bool dbOverwritten = false,
 			const ParametersMap & parameters = ParametersMap(),
 			bool postInitClosingEvents = false);
+	/**
+	 * @brief Flushes pending data and closes the database connection.
+	 *
+	 * @param databaseSaved If true, persists STM/WM signatures and statistics before closing.
+	 *        If false, in-memory state is discarded.
+	 * @param postInitClosingEvents If true, posts progress events while closing.
+	 * @param ouputDatabasePath If non-empty, the database is copied to this path on close. If a
+	 *        database on disk was initially created/loaded on a different path, it will be updated with latest
+	 *        changes and renamed to the output path.
+	 */
 	void close(bool databaseSaved = true, bool postInitClosingEvents = false, const std::string & ouputDatabasePath = "");
+	/**
+	 * @brief Computes loop-closure likelihood of @p signature against a set of WM ids.
+	 *
+	 * Compares visual words (tf-idf if enabled) between @p signature and each id in
+	 * @p ids and returns a normalized likelihood per id.
+	 *
+	 * @param signature Query signature (typically the last added one).
+	 * @param ids Candidate signature ids in working memory.
+	 * @return Map from id to likelihood score.
+	 */
 	std::map<int, float> computeLikelihood(const Signature * signature,
 			const std::list<int> & ids);
+	/**
+	 * @brief Starts a new map id, breaking session continuity (e.g. after localization loss).
+	 *
+	 * @param reducedIds If non-null, populated with id remappings produced by graph reduction
+	 *        triggered by the new map.
+	 * @return The new map id (auto-incremented).
+	 */
 	int incrementMapId(std::map<int, int> * reducedIds = 0);
+	/**
+	 * @brief Refreshes the age of @p signatureId in working memory, marking it as recent.
+	 *
+	 * Used to keep loop-closure hypotheses active so they are not transferred to LTM
+	 * during the next @ref forget() call.
+	 */
 	void updateAge(int signatureId);
 
+	/**
+	 * @brief Transfers oldest signatures from WM to LTM to respect memory and/or time limits.
+	 *
+	 * Two regimes are used depending on the visual word dictionary state:
+	 * - **Word-count regime**: active only when mapping mode is on, the @ref VWDictionary
+	 *   is in incremental mode, contains at least one word, and is *not* using incremental
+	 *   FLANN. In this regime, signatures are transferred until the number of visual words
+	 *   removed from the dictionary catches up with the number of new words indexed since
+	 *   the previous iteration.
+	 * - **Signature-count regime**: used in every other case (localization mode, dictionary
+	 *   not incremental, dictionary still empty -- e.g. lidar-only mapping or no feature
+	 *   extraction -- or incremental FLANN, where the word count is no longer the bottleneck).
+	 *   In this regime, at least one more signature than the count added/retrieved in the
+	 *   previous iteration is transferred, regardless of words.
+	 *
+	 * In both regimes, candidate selection (see @c getRemovableSignatures()) honors
+	 * @p ignoredIds, skips intermediate nodes, and excludes WM nodes linked to STM (to
+	 * preserve rehearsal). Intermediate (weight==-1) nodes linked to a transferred
+	 * signature are dragged out with it.
+	 *
+	 * @param ignoredIds Signatures that must not be transferred (e.g. STM, retrieved ids, on the planned path).
+	 * @return Ids of signatures moved to LTM, in transfer order.
+	 */
 	std::list<int> forget(const std::set<int> & ignoredIds = std::set<int>());
+	/**
+	 * @brief Reloads signatures from LTM into WM.
+	 *
+	 * @param ids Candidate ids; those already in WM/STM are ignored.
+	 * @param maxLoaded Hard cap on number of ids actually loaded (0 = unlimited).
+	 * @param timeDbAccess Output: time spent in the database driver (seconds).
+	 * @return Ids effectively brought back to WM.
+	 */
 	std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
 
+	/**
+	 * @brief Drops the last signature if flagged as bad, or any signature in localization mode.
+	 * @return Id of the removed signature, or 0 if none was removed.
+	 */
 	int cleanup();
+	/** @brief Persists @p statistics to the database; @p saveWMState records the WM id list. */
 	void saveStatistics(const Statistics & statistics, bool saveWMState);
+	/** @brief Stores a preview image (typically a thumbnail of the last frame) in the database. */
 	void savePreviewImage(const cv::Mat & image) const;
+	/** @brief Loads the preview image previously written by @ref savePreviewImage(). */
 	cv::Mat loadPreviewImage() const;
+	/** @brief Persists an optimized pose graph and the last localization pose for next session. */
 	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	/** @brief Loads optimized poses previously written by @ref saveOptimizedPoses(). */
 	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose) const;
+	/** @brief Persists a 2D occupancy grid (origin @p xMin, @p yMin and resolution @p cellSize). */
 	void save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	/** @brief Loads the 2D occupancy grid previously written by @ref save2DMap(). */
 	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
+	/**
+	 * @brief Persists an optimized textured/colored mesh to the database.
+	 * @param cloud Point cloud (XYZRGB) of vertices.
+	 * @param polygons Per-texture list of polygons; each polygon is a list of vertex indices.
+	 * @param texCoords Per-texture list of UV coords matching @p polygons.
+	 * @param textures Concatenated texture images (square, equal-sized).
+	 */
 	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
@@ -111,6 +281,7 @@ public:
 			const std::vector<std::vector<Eigen::Vector2f> > & texCoords = std::vector<std::vector<Eigen::Vector2f> >(), // Textures -> uv coords for each vertex of the polygons
 #endif
 			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size)
+	/** @brief Loads the optimized mesh previously written by @ref saveOptimizedMesh(). */
 	cv::Mat loadOptimizedMesh(
 			std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons = 0,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
@@ -119,12 +290,39 @@ public:
 			std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
 #endif
 			cv::Mat * textures = 0) const;
+	/** @brief Forces the database driver to flush any queued signature/word saves. */
 	void emptyTrash();
+	/** @brief Blocks until the asynchronous database write thread has finished pending work. */
 	void joinTrashThread();
+	/**
+	 * @brief Adds a graph link between two signatures.
+	 * @param link Link to add (type, transform, covariance).
+	 * @param addInDatabase If true, the link is also added when one of the ids is only in LTM.
+	 * @return True if the link was added, false on conflict or missing nodes.
+	 */
 	bool addLink(const Link & link, bool addInDatabase = false);
+	/** @brief Replaces an existing link with @p link (same endpoints and type). */
 	void updateLink(const Link & link, bool updateInDatabase = false);
+	/** @brief Removes every virtual link in WM. */
 	void removeAllVirtualLinks();
+	/** @brief Removes virtual links attached to @p signatureId. */
 	void removeVirtualLinks(int signatureId);
+	/**
+	 * @brief Breadth-first walk of the pose graph from @p signatureId.
+	 *
+	 * Visits neighbor and (optionally) loop-closure neighbors up to @p maxGraphDepth.
+	 *
+	 * @param signatureId Starting node.
+	 * @param maxGraphDepth Maximum graph distance (0 = infinite graph depth).
+	 * @param maxCheckedInDatabase Cap on LTM look-ups (-1 = unlimited, 0 = WM only).
+	 * @param incrementMarginOnLoop If true, loop-closure links count toward depth.
+	 * @param ignoreLoopIds If true, loop-closure neighbors are not traversed.
+	 * @param ignoreIntermediateNodes If true, weight==-1 intermediate nodes are skipped.
+	 * @param ignoreLocalSpaceLoopIds If true, only global loop closures are traversed.
+	 * @param nodesSet If non-empty, traversal is constrained to these ids.
+	 * @param dbAccessTime Output: time spent in database access (seconds).
+	 * @return Map from visited node id to graph depth, including @p signatureId (with graph depth of 0)
+	 */
 	std::map<int, int> getNeighborsId(
 			int signatureId,
 			int maxGraphDepth,
@@ -135,57 +333,206 @@ public:
 			bool ignoreLocalSpaceLoopIds = false,
 			const std::set<int> & nodesSet = std::set<int>(),
 			double * dbAccessTime = 0) const;
+	/**
+	 * @brief Returns neighbor ids within a Euclidean radius using optimized poses.
+	 * @param signatureId Query node id.
+	 * @param radius Maximum distance from @p signatureId (meters).
+	 * @param optimizedPoses Pose graph after optimization (used for distances).
+	 * @param maxGraphDepth Maximum graph distance (in terms of nodes) to bound the search.
+	 * @return Map from node id to squared distance from @p signatureId.
+	 */
 	std::map<int, float> getNeighborsIdRadius(
 			int signatureId,
 			float radius,
 			const std::map<int, Transform> & optimizedPoses,
 			int maxGraphDepth) const;
-	void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
+	/** @brief Marks @p locationId as intermediate (weight = -1); excludes it from loop closure. */
+	void convertToIntermediate(int locationId);
+	/**
+	 * @brief Removes @p locationId from WM/STM and the database.
+	 * @param locationId Id of the signature to delete.
+	 * @param deletedWords Optional output: words whose reference count dropped to zero.
+	 * @param keepLinkedInDb If true, the location keeps its links, weight and label and
+	 *                       stays part of the graph in the database. If false (default),
+	 *                       it is unlinked first -- links removed on both sides, weight
+	 *                       invalidated, label cleared -- so at best it is kept as
+	 *                       history only and no longer appears in
+	 *                       @ref getAllSignatureIds(). An unlinked location is written
+	 *                       to the database only if it was already saved there or if
+	 *                       @ref Parameters::kMemNotLinkedNodesKept() is true (default);
+	 *                       otherwise it is discarded outright.
+	 */
+	void deleteLocation(int locationId, std::list<int> * deletedWords = 0, bool keepLinkedInDb = false);
+	/** @brief Forces @p locationId to be flushed to the database. */
 	void saveLocationData(int locationId);
+	/** @brief Removes any link between @p idA and @p idB (both directions). */
 	void removeLink(int idA, int idB);
-	void removeRawData(int id, bool image = true, bool scan = true, bool userData = true);
+	/** @brief Strips raw images, scan, user data and/or occupancy grid from @p id to save memory (RAM). This doesn't clear any compressed data.*/
+	void removeRawData(int id, bool image = true, bool scan = true, bool userData = true, bool occupancyGrid = true);
+	/**
+	 * @brief Merges @p id with a close neighbor (graph reduction).
+	 * @param id Node to reduce.
+	 * @param maxDistance Maximum distance to a neighbor to allow the merge (meters).
+	 * @param keepLinkedInDb Same meaning as in @ref deleteLocation(): if true, the merged
+	 *                       node keeps its links, weight and label and stays part of the
+	 *                       graph in the database. If false (default), it is unlinked
+	 *                       first -- links removed on both sides, weight invalidated,
+	 *                       label cleared -- so at best it is kept as history only and no
+	 *                       longer appears in @ref getAllSignatureIds(). An unlinked node
+	 *                       is written to the database only if it was already saved there
+	 *                       or if @ref Parameters::kMemNotLinkedNodesKept() is true
+	 *                       (default); otherwise it is discarded outright.
+	 * @param direction Restrict merge target: 0=any, 1=previous neighbor, 2=next neighbor.
+	 * @return Id of the node @p id was merged into, or 0 if no reduction was performed.
+	 */
+	int reduceNode(int id, float maxDistance = 0.0f, bool keepLinkedInDb = false, int direction = 0);
+	/**
+	 * @brief Enables a dummy visual word dictionary (no descriptors kept, word ids only).
+	 * @note Must be called before @ref init(); ignored (with an error logged) once the
+	 *       database driver is set.
+	 * @note Silently disabled by @ref init() when the database holds no dictionary,
+	 *       since there would be nothing to skip loading, and the memory would
+	 *       otherwise be left unable to add new nodes.
+	 */
+	void setDummyDictionary(bool enabled);
 
-	//getters
+	/** @return Working memory as { signature id, age } (does not include STM). */
 	const std::map<int, double> & getWorkingMem() const {return _workingMem;}
+	/**
+	 * @brief Returns the number of signatures in working memory, excluding the virtual place.
+	 * @param ignoreIntermediateNodes If true, intermediate nodes (weight = -1) are not counted.
+	 */
+	size_t getWorkingMemSize(bool ignoreIntermediateNodes = false) const;
+	/** @return Number of intermediate nodes (weight = -1) currently in working memory. */
+	int getWorkingMemIntermediateNodesCount() const {return _workingMemIntermediateNodesCount;}
+	/** @return Number of intermediate nodes (weight = -1) currently in short-term memory. */
+	int getStMemIntermediateNodesCount() const {return _stMemIntermediateNodesCount;}
+	/** @return Set of signature ids currently in short-term memory. */
 	const std::set<int> & getStMem() const {return _stMem;}
+	/** @return Configured maximum STM size (@ref Parameters::kMemSTMSize()). */
 	int getMaxStMemSize() const {return _maxStMemSize;}
+	/** @brief Returns neighbor (sequential) links of @p signatureId; @p lookInDatabase also checks LTM. */
 	std::multimap<int, Link> getNeighborLinks(int signatureId,
 			bool lookInDatabase = false) const;
+	/** @brief Returns loop-closure links of @p signatureId; @p lookInDatabase also checks LTM. */
 	std::multimap<int, Link> getLoopClosureLinks(int signatureId,
 			bool lookInDatabase = false) const;
-	std::multimap<int, Link> getLinks(int signatureId, // can be also used to get links from landmarks
+	/**
+	 * @brief Returns all links of @p signatureId (neighbor, loop, prior, gravity, ...).
+	 * @param signatureId Source node id (can also be a landmark id).
+	 * @param lookInDatabase Also query LTM for links.
+	 * @param withLandmarks Include landmark links in the result.
+	 */
+	std::multimap<int, Link> getLinks(int signatureId,
 			bool lookInDatabase = false,
 			bool withLandmarks = false) const;
+	/** @brief Returns links of every signature; @p ignoreNullLinks drops empty placeholders. */
 	std::multimap<int, Link> getAllLinks(bool lookInDatabase, bool ignoreNullLinks = true, bool withLandmarks = false) const;
+	/** @return True if raw binary data (images, scans) is kept in memory after compression. */
 	bool isBinDataKept() const {return _binDataKept;}
+	/** @return Similarity threshold used by rehearsal (@ref Parameters::kMemRehearsalSimilarity()). */
 	float getSimilarityThreshold() const {return _similarityThreshold;}
+	/** @return Map from signature id to weight (rehearsal accumulation count) for WM and STM. */
 	std::map<int, int> getWeights() const;
+	/** @return Id of the most recently added signature, or 0 if none. */
 	int getLastSignatureId() const;
-	const Signature * getLastWorkingSignature() const;
+	/**
+	 * @brief Returns the most recent WM signature.
+	 * @param ignoreIntermediateNodes If true, skips weight==-1 placeholder nodes.
+	 */
+	const Signature * getLastWorkingSignature(bool ignoreIntermediateNodes) const;
+	/** @brief Returns all nodes observing landmark @p landmarkId, mapped to the observation link. */
 	std::map<int, Link> getNodesObservingLandmark(int landmarkId, bool lookInDatabase) const;
+	/** @return Signature id labeled @p label, or 0 if not found. */
 	int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const;
+	/**
+	 * @brief Assigns or removes a label on @p id.
+	 * @param id Signature id; pass 0 to remove an existing label by name.
+	 * @param label Label text; empty to remove.
+	 * @return True if the label was applied.
+	 */
 	bool labelSignature(int id, const std::string & label);
+	/** @return Map from signature id to non-empty label (including STM+WM+LTM). */
 	const std::map<int, std::string> & getAllLabels() const {return _labels;}
+	/** @return Reverse landmark index: { landmark id (negative), nodes observing it }. */
 	const std::map<int, std::set<int> > & getLandmarksIndex() const {return _landmarksIndex;}
+	/** @return True if every persisted node (in LTM) is currently loaded in WM/STM. */
 	bool allNodesInWM() const {return _allNodesInWM;}
 
 	/**
-	 * Set user data. Detect automatically if raw or compressed. If raw, the data is
+	 * @brief Attaches user data to signature @p id, compressing it on the fly if needed.
-	 * compressed too. A matrix of type CV_8UC1 with 1 row is considered as compressed.
+	 *
-	 * If you have one dimension unsigned 8 bits raw data, make sure to transpose it
+	 * The format is detected automatically: a single-row @c CV_8UC1 matrix is treated
-	 * (to have multiple rows instead of multiple columns) in order to be detected as
+	 * as already-compressed data and stored as-is; anything else is considered raw and
-	 * not compressed.
+	 * compressed before being stored.
+	 *
+	 * @note If you pass one-dimensional unsigned 8-bit raw data, transpose it so it has
+	 *       multiple rows (not multiple columns), otherwise it will be misdetected as
+	 *       already compressed.
+	 *
+	 * @param id Target signature id (must be in WM/STM or LTM).
+	 * @param data Raw or pre-compressed user data.
+	 * @return True if the data was attached, false if @p id was not found.
 	 */
 	bool setUserData(int id, const cv::Mat & data);
-	int getDatabaseMemoryUsed() const; // in bytes
+	/** @return On-disk database size in bytes. */
+	int getDatabaseMemoryUsed() const;
+	/** @return Schema version of the open database (e.g. "0.20.0"). */
 	std::string getDatabaseVersion() const;
+	/** @return File path of the open database (empty if in-memory). */
 	std::string getDatabaseUrl() const;
+	/** @return Last @ref emptyTrash() flush time in seconds. */
 	double getDbSavingTime() const;
+	/** @return Map id of signature @p id; @p lookInDatabase also queries LTM. */
 	int getMapId(int id, bool lookInDatabase = false) const;
+	/** @return Odometry pose stored with @p signatureId (null if unknown). */
 	Transform getOdomPose(int signatureId, bool lookInDatabase = false) const;
+	/** @return Ground-truth pose stored with @p signatureId (null if unknown). */
 	Transform getGroundTruthPose(int signatureId, bool lookInDatabase = false) const;
-	const std::map<int, Transform> & getGroundTruths() const {return _groundTruths;} // only those in working+STM memory
+	/** @return Ground-truth poses for nodes currently in WM/STM. */
+	const std::map<int, Transform> & getGroundTruths() const {return _groundTruths;}
+	/**
+	 * @brief Returns a GPS fix for @p id, falling back to the nearest GPS-tagged neighbor.
+	 *
+	 * Two cases:
+	 * - If @p id has a GPS fix attached, @p gps is set to that fix and @p offsetENU
+	 *   is left at identity.
+	 * - Otherwise, the graph is searched (via @ref getNeighborsId() with depth
+	 *   @p maxGraphDepth, ignoring loop closures) for the closest neighbor that has a
+	 *   GPS fix. When one is found, @p gps is set to that neighbor's fix and
+	 *   @p offsetENU is the rigid transform from that neighbor's pose to @p id,
+	 *   expressed in ENU coordinates (derived from the neighbor's heading/bearing).
+	 *   Applying @p offsetENU on top of the GPS-derived pose of the neighbor yields
+	 *   the ENU pose of @p id.
+	 *
+	 * If no GPS fix is found on @p id or any reachable neighbor, @p gps is returned
+	 * empty (@c gps.stamp()==0) and @p offsetENU is identity.
+	 *
+	 * @param id Query signature id.
+	 * @param gps Output GPS fix (empty if none found).
+	 * @param offsetENU Output ENU-frame offset from the GPS-tagged node to @p id
+	 *        (identity when @p id itself carries the GPS fix or when none is found).
+	 * @param lookInDatabase If true, also fetch missing nodes from LTM during the search.
+	 * @param maxGraphDepth Maximum graph depth used to look for a GPS-tagged neighbor
+	 *        when @p id has none (0 = no depth limit, i.e. search the whole reachable graph).
+	 */
 	void getGPS(int id, GPS & gps, Transform & offsetENU, bool lookInDatabase, int maxGraphDepth = 0) const;
+	/**
+	 * @brief Reads metadata of @p signatureId (no images/scan/words).
+	 *
+	 * @param signatureId Node id.
+	 * @param odomPose Output odometry pose (null if not set).
+	 * @param mapId Output map id.
+	 * @param weight Output rehearsal weight.
+	 * @param label Output label (empty if none).
+	 * @param stamp Output timestamp (seconds, epoch).
+	 * @param groundTruth Output ground-truth pose (null if not set).
+	 * @param velocity Output 6-vector velocity (empty if not set).
+	 * @param gps Output GPS fix (invalid if not set).
+	 * @param sensors Output environmental sensor readings.
+	 * @param lookInDatabase Also query LTM.
+	 * @return True if @p signatureId was found.
+	 */
 	bool getNodeInfo(int signatureId,
 			Transform & odomPose,
 			int & mapId,
@@ -197,39 +544,183 @@ public:
 			GPS & gps,
 			EnvSensors & sensors,
 			bool lookInDatabase = false) const;
+	/** @return Compressed image blob for @p signatureId (empty if not stored). */
 	cv::Mat getImageCompressed(int signatureId) const;
+	/**
+	 * @brief Loads sensor data of @p locationId from WM or LTM.
+	 * @param images Include compressed RGB/depth images.
+	 * @param scan Include laser scan blob.
+	 * @param userData Include user data blob.
+	 * @param occupancyGrid Include occupancy grid cells.
+	 */
 	SensorData getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const;
+	/** @brief Loads the visual words, 3D points and global descriptors stored with @p nodeId. */
 	void getNodeWordsAndGlobalDescriptors(int nodeId,
 			std::multimap<int, int> & words,
 			std::vector<cv::KeyPoint> & wordsKpts,
 			std::vector<cv::Point3f> & words3,
 			cv::Mat & wordsDescriptors,
 			std::vector<GlobalDescriptor> & globalDescriptors) const;
+	/** @brief Loads mono and/or stereo camera calibration stored with @p nodeId. */
 	void getNodeCalibration(int nodeId,
 			std::vector<CameraModel> & models,
 			std::vector<StereoCameraModel> & stereoModels) const;
+	/**
+	 * @brief Returns all signature ids in WM, STM and LTM.
+	 * @param ignoreChildren If true, nodes not linked to graph anymore are excluded
+	 */
 	std::set<int> getAllSignatureIds(bool ignoreChildren = true) const;
+	/**
+	 * @brief Reports whether the in-memory map has been modified since the database was last
+	 *        loaded or reset. @ref close() uses this flag to decide whether the database
+	 *        needs to be rewritten.
+	 *
+	 * The flag is cleared to @c false on construction, by @ref close() and when the memory is cleared,
+	 * and is raised to @c true on any of the following events:
+	 *
+	 * - **@ref update() in mapping mode** (@ref Parameters::kMemIncrementalMemory() == @c true,
+	 *   see @ref isIncremental()): every successful call sets the flag, since a new
+	 *   @ref Signature is added to the graph and the visual word dictionary may grow.
+	 * - **@ref update() in localization mode** (@ref isIncremental() == @c false): the flag
+	 *   is set only when @ref Parameters::kMemLocalizationDataSaved() is enabled
+	 *   (see @ref isLocalizationDataSaved()), i.e. when the new node must be persisted
+	 *   back to the database. Pure localization (the default,
+	 *   @ref Parameters::kMemLocalizationDataSaved() == @c false) leaves the flag at
+	 *   @c false even after many @ref update() calls, because nothing needs to be saved.
+	 * - **@ref reduceNode()**: merging a node into a neighbor mutates the graph and
+	 *   marks the memory as changed (and also raises the link-changed flag).
+	 * - **@ref init() dictionary repair**: when @ref init() rebuilds the visual word
+	 *   dictionary because words are missing from the database, the flag is forced to
+	 *   @c true so the regenerated dictionary is saved back on @ref close(), even if no
+	 *   new data was processed.
+	 *
+	 * Note that link-only modifications (e.g. @ref addLink(), @ref updateLink(),
+	 * @ref removeLink()) update an independent @c _linksChanged flag, not this one.
+	 *
+	 * @return True if the memory has changed and would need to be persisted.
+	 */
 	bool memoryChanged() const {return _memoryChanged;}
+	/**
+	 * @return True if the memory grows on @ref update() (mapping mode), false in localization mode.
+	 * @see Parameters::kMemIncrementalMemory()
+	 */
 	bool isIncremental() const {return _incrementalMemory;}
+	/**
+	 * @return True in localization mode when database writes are disabled
+	 *         (i.e. @ref isIncremental() is false and @ref Parameters::kMemLocalizationReadOnly() is enabled).
+	 * @see Parameters::kMemIncrementalMemory()
+	 * @see Parameters::kMemLocalizationReadOnly()
+	 */
+	bool isReadOnly() const {return !_incrementalMemory && _localizationReadOnly;}
+	/**
+	 * @return True if data added during localization is persisted to the database.
+	 * @see Parameters::kMemLocalizationDataSaved()
+	 */
 	bool isLocalizationDataSaved() const {return _localizationDataSaved;}
+	/** @return Signature with @p id in WM/STM, or null if not loaded. */
 	const Signature * getSignature(int id) const;
+	/** @return True if @p signatureId is in short-term memory. */
 	bool isInSTM(int signatureId) const {return _stMem.find(signatureId) != _stMem.end();}
+	/** @return True if @p signatureId is in working memory. */
 	bool isInWM(int signatureId) const {return _workingMem.find(signatureId) != _workingMem.end();}
+	/** @return True if @p signatureId is not in STM/WM, so when it is in LTM or non-existing. */
 	bool isInLTM(int signatureId) const {return !this->isInSTM(signatureId) && !this->isInWM(signatureId);}
+	/** @return True if signature ids are auto-generated, false if taken from sensor data id. */
 	bool isIDsGenerated() const {return _generateIds;}
+	/** @return Id of the last accepted global loop-closure node, or 0 if none. */
 	int getLastGlobalLoopClosureId() const {return _lastGlobalLoopClosureId;}
+	/** @return Feature extractor used to compute visual words. */
 	const Feature2D * getFeature2D() const {return _feature2D;}
+	/** @return True if graph reduction is enabled (@ref Parameters::kMemReduceGraph()). */
 	bool isGraphReduced() const {return _reduceGraph;}
+	/**
+	 * @return Running per-axis maximum of the diagonal of every neighbor (odometry) link's
+	 *         information matrix observed so far, as a 6-vector
+	 *         (x, y, z, roll, pitch, yaw). Empty until at least one 6x6 neighbor link
+	 *         information matrix has been seen.
+	 *
+	 * This is a runtime statistic, not a configurable parameter: it is updated on
+	 * @ref init() (over all loaded neighbor links) and on every @ref update() that
+	 * adds a new neighbor link.
+	 *
+	 * It is consumed by @ref Rtabmap::getInformation() when
+	 * @ref Parameters::kRGBDLoopCovLimited() is enabled, to clip loop-closure
+	 * information matrices so a loop never claims higher confidence than odometry
+	 * itself ever provided.
+	 *
+	 * @see Parameters::kRGBDLoopCovLimited()
+	 */
 	const std::vector<double> & getOdomMaxInf() const {return _odomMaxInf;}
+	/**
+	 * @return True if the odometry pose orientation is used (instead of the IMU
+	 *         orientation) as the source of each new node's gravity link.
+	 *
+	 * When enabled, every new node gets a self-loop @ref Link::kGravity holding the
+	 * rotation of the odometry pose passed to @ref update(). This assumes odometry is
+	 * already gravity-aligned (e.g. a VIO front-end). When disabled, the gravity link
+	 * is built from the IMU orientation in @ref SensorData::imu() if available.
+	 *
+	 * Gravity links are consumed by graph optimization only when
+	 * @ref Parameters::kOptimizerGravitySigma() is non-zero.
+	 *
+	 * @see Parameters::kMemUseOdomGravity()
+	 * @see Parameters::kOptimizerGravitySigma()
+	 */
 	bool isOdomGravityUsed() const {return _useOdometryGravity;}
 
+	/** @brief Writes a human-readable dump of WM/STM, links and weights to @p fileNameTree. */
 	void dumpMemoryTree(const char * fileNameTree) const;
+	/** @brief Dumps every internal map (signatures, words, dictionary) to text files in @p directory. */
 	virtual void dumpMemory(std::string directory) const;
+	/** @brief Dumps signatures' word ids (and 3D positions if @p words3D) to @p fileNameSign. */
 	virtual void dumpSignatures(const char * fileNameSign, bool words3D) const;
+	/** @brief Dumps the visual word dictionary: references to @p fileNameRef, descriptors to @p fileNameDesc. */
 	void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
+	/** @return Approximate RAM usage of the in-memory state, in bytes. */
 	unsigned long getMemoryUsed() const; //Bytes
 
+	/**
+	 * @brief Writes a Graphviz DOT file of the pose graph.
+	 * @param fileName Output path.
+	 * @param ids If non-empty, restrict the graph to these node ids.
+	 */
 	void generateGraph(const std::string & fileName, const std::set<int> & ids = std::set<int>());
+	/**
+	 * @brief Removes spurious obstacle points from each node's local grid using a reference 2D map.
+	 *
+	 * For every node in @p poses, the node's local **obstacle** grid is loaded, each
+	 * obstacle point is projected with @p poses into the reference @p map, and the
+	 * point is kept only if either:
+	 * - the reference @p map cell at its projection is not free space
+	 *   (i.e. the cell is an obstacle or unknown, value != 0), or
+	 * - the reference @p map contains an obstacle cell (value == 100) within
+	 *   @p cropRadius cells of the projection.
+	 *
+	 * Points that fall on a free cell and have no obstacle neighbor within
+	 * @p cropRadius are dropped. The filtered obstacle grid replaces the node's grid
+	 * in WM/STM and (if the node is already persisted) in the database via
+	 * @ref DBDriver::updateOccupancyGrid().
+	 *
+	 * **Ground and empty cells are not touched**: they are read and written back as-is.
+	 *
+	 * When @p filterScans is true, the same projection/filtering rule is also applied
+	 * to each node's raw laser scan, and the rewritten scan is saved back to the
+	 * database. This is useful to remove dynamic objects from the stored scans before
+	 * re-meshing or re-exporting.
+	 *
+	 * @param poses Optimized poses used to project the local grids/scans into @p map.
+	 * @param map Reference 2D occupancy grid (cell values: 0 free, 100 occupied,
+	 *        anything else unknown).
+	 * @param xMin Reference map origin x in world coordinates (meters).
+	 * @param yMin Reference map origin y in world coordinates (meters).
+	 * @param cellSize Reference map resolution (meters/cell); must match the nodes' grid cell size.
+	 * @param cropRadius Search radius (in cells) around each projected point used to
+	 *        accept points near an obstacle in @p map.
+	 * @param filterScans If true, also filter and rewrite the raw laser scan attached
+	 *        to each node, using the same rule as for obstacle cells.
+	 * @return Number of (node, grid or scan) modifications performed, or -1 on error
+	 *         (no database loaded, empty @p poses or empty @p map).
+	 */
 	int cleanupLocalGrids(
 			const std::map<int, Transform> & poses,
 			const cv::Mat & map,
@@ -239,10 +730,20 @@ public:
 			int cropRadius = 1,
 			bool filterScans = false);
 
-	//keypoint stuff
+	/** @return Visual word dictionary used for tf-idf likelihood and feature matching. */
 	const VWDictionary * getVWDictionary() const;
 
-	// RGB-D stuff
+	/**
+	 * @brief Extracts a sub-graph (poses + links) for a set of node ids.
+	 *
+	 * Used by graph optimization callers to retrieve constraints for a region of interest.
+	 *
+	 * @param ids Ids to include.
+	 * @param poses Output: odometry poses for @p ids.
+	 * @param links Output: links between the nodes (and to landmarks if @p landmarksAdded).
+	 * @param lookInDatabase If true, fetch missing data from LTM.
+	 * @param landmarksAdded If true, also include landmark constraints.
+	 */
 	void getMetricConstraints(
 			const std::set<int> & ids,
 			std::map<int, Transform> & poses,
@@ -250,9 +751,31 @@ public:
 			bool lookInDatabase = false,
 			bool landmarksAdded = false);
 
+	/**
+	 * @brief Computes the relative transform from @p fromS to @p toS using the registration pipeline.
+	 * @param fromS Source signature (will be modified to cache extracted data).
+	 * @param toS Target signature.
+	 * @param guess Initial transform estimate (null if unknown).
+	 * @param info Optional output with inlier counts, variance and diagnostics.
+	 * @param useKnownCorrespondencesIfPossible If true, reuses existing word-id correspondences.
+	 * @return The estimated transform, or a null @ref Transform on failure.
+	 */
 	Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false) const;
+	/** @brief Convenience overload: loads signatures by id and forwards to the @ref Signature variant. */
 	Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false);
+	/**
+	 * @brief Refines a transform using ICP alignment of laser scans only.
+	 * @return The refined transform, or a null @ref Transform on failure.
+	 */
 	Transform computeIcpTransform(const Signature & fromS, const Signature & toS, Transform guess, RegistrationInfo * info = 0) const;
+	/**
+	 * @brief ICP registration of one node against an assembled cloud from multiple neighbors.
+	 * @param newId New (query) node id.
+	 * @param oldId Reference node id.
+	 * @param poses Neighbor poses used to assemble the reference cloud.
+	 * @param info Optional output with inlier counts and diagnostics.
+	 * @return The estimated transform, or a null @ref Transform on failure.
+	 */
 	Transform computeIcpTransformMulti(
 			int newId,
 			int oldId,
@@ -273,9 +796,9 @@ private:
 	std::list<Signature *> getRemovableSignatures(int count,
 			const std::set<int> & ignoredIds = std::set<int>());
 	int getNextId();
-	void initCountId();
 	void rehearsal(Signature * signature, Statistics * stats = 0);
 	bool rehearsalMerge(int oldId, int newId);
+	bool canBeReduced(const Link & link, float maxDistance, int direction);
 
 	const std::map<int, Signature*> & getSignatures() const {return _signatures;}
 
@@ -292,6 +815,7 @@ private:
 	int getNi(int signatureId) const;
 
 protected:
+	/** @brief Database driver owning the persistent storage (created by @ref init()). */
 	DBDriver * _dbDriver;
 
 private:
@@ -307,6 +831,7 @@ private:
 	std::string _rgbCompressionFormat;
 	std::string _depthCompressionFormat;
 	bool _incrementalMemory;
+	bool _localizationReadOnly;
 	bool _localizationDataSaved;
 	bool _flannIndexSaved;
 	bool _reduceGraph;
@@ -355,6 +880,8 @@ private:
 	bool _memoryChanged; // False by default, become true only when Memory::update() is called.
 	bool _linksChanged; // False by default, become true when links are modified.
 	int _signaturesAdded;
+	int _workingMemIntermediateNodesCount; // number of nodes with weight==-1 currently in _workingMem
+	int _stMemIntermediateNodesCount; // number of nodes with weight==-1 currently in _stMem
 	bool _allNodesInWM;
 	bool _receivingOdometryFeatures;
 	GPS _gpsOrigin;
@@ -386,6 +913,8 @@ private:
 	MarkerDetector * _markerDetector;
 
 	GlobalDescriptorExtractor * _globalDescriptorExtractor;
+
+	bool _dummyDictionary;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Odometry.h

@@ -39,54 +39,115 @@ namespace rtabmap {
 class OdometryInfo;
 class ParticleFilter;
 
+/**
+ * @class Odometry
+ * @brief Abstract base class for visual, lidar and visual-inertial odometry backends.
+ *
+ * Odometry estimates the incremental motion between consecutive @ref SensorData frames.
+ * Concrete implementations override @c computeTransform(); the public @ref process()
+ * pipeline handles IMU caching, optional motion guesses, filtering (Kalman or particle),
+ * image decimation, deskewing and pose integration.
+ *
+ * Use @ref create() to instantiate a backend from @ref Parameters::kOdomStrategy().
+ *
+ * @see OdometryThread
+ * @see OdometryInfo
+ */
 class RTABMAP_CORE_EXPORT Odometry
 {
 public:
+	/** @brief Odometry backend selected by @ref Parameters::kOdomStrategy(). */
 	enum Type {
-		kTypeUndef = -1,
+		kTypeUndef = -1,    /**< Undefined / invalid type. */
-		kTypeF2M = 0,
+		kTypeF2M = 0,       /**< Frame-to-map (default). */
-		kTypeF2F = 1,
+		kTypeF2F = 1,       /**< Frame-to-frame. */
-		kTypeFovis = 2,
+		kTypeFovis = 2,     /**< FOVIS stereo visual odometry. */
-		kTypeViso2 = 3,
+		kTypeViso2 = 3,     /**< libviso2. */
-		kTypeDVO = 4,
+		kTypeDVO = 4,       /**< Dense visual odometry. */
-		kTypeORBSLAM = 5,
+		kTypeORBSLAM = 5,   /**< ORB-SLAM 2/3. */
-		kTypeOkvis = 6,
+		kTypeOkvis = 6,     /**< OKVIS. */
-		kTypeLOAM = 7,
+		kTypeLOAM = 7,      /**< LOAM lidar odometry. */
-		kTypeMSCKF = 8,
+		kTypeMSCKF = 8,     /**< MSCKF visual-inertial. */
-		kTypeVINSFusion = 9,
+		kTypeVINSFusion = 9,/**< VINS-Fusion. */
-		kTypeOpenVINS = 10,
+		kTypeOpenVINS = 10, /**< OpenVINS. */
-		kTypeFLOAM = 11,
+		kTypeFLOAM = 11,    /**< FLOAM lidar odometry. */
-		kTypeOpen3D = 12,
+		kTypeOpen3D = 12,   /**< Open3D RGB-D odometry. */
-		kTypeCuVSLAM = 13
+		kTypeCuVSLAM = 13,  /**< cuVSLAM. */
+		kTypeLIOSAM = 14    /**< LIO-SAM. */
 	};
 
-public:
+	/**
+	 * @brief Creates an odometry instance from @ref Parameters::kOdomStrategy() in @p parameters.
+	 * @param parameters RTAB-Map parameters (odometry strategy and related options).
+	 * @return New odometry object (caller owns the pointer). Falls back to @ref kTypeF2M if the type is unknown.
+	 */
 	static Odometry * create(const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @brief Creates an odometry instance of a given @p type.
+	 * @param type In/out odometry type; updated to @ref kTypeF2M if @p type is unknown.
+	 * @param parameters RTAB-Map parameters passed to the concrete backend.
+	 */
 	static Odometry * create(Type & type, const ParametersMap & parameters = ParametersMap());
 
-public:
 	virtual ~Odometry();
+	/**
+	 * @brief Processes a sensor frame and updates the integrated pose.
+	 * @param data Input sensor data (must have @c id() >= 0). May be modified in place (decompression, deskewing).
+	 * @param info Optional output statistics and debug data.
+	 * @return Updated integrated pose (@ref getPose()) after the frame is processed,
+	 *         or a null transform if odometry is lost. The incremental transform is
+	 *         available in @c OdometryInfo::transform when @p info is provided.
+	 */
 	Transform process(SensorData & data, OdometryInfo * info = 0);
+	/**
+	 * @brief Processes a sensor frame with an external motion guess.
+	 * @param data Input sensor data.
+	 * @param guess Optional prior on the incremental transform (used by the backend when supported).
+	 * @param info Optional output statistics and debug data.
+	 */
 	Transform process(SensorData & data, const Transform & guess, OdometryInfo * info = 0);
+	/**
+	 * @brief Resets internal state and sets the initial pose.
+	 * @param initialPose Starting pose (must not be null). Z/roll/pitch may be cleared if @ref Parameters::kRegForce3DoF() is enabled.
+	 */
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	/** @return Concrete odometry backend type. */
 	virtual Odometry::Type getType() = 0;
+	/** @return True if the backend can process unrectified camera images. */
 	virtual bool canProcessRawImages() const {return false;}
+	/** @return True if the backend processes IMU asynchronously outside @ref process(). */
 	virtual bool canProcessAsyncIMU() const {return false;}
 
-	//getters
+	/** @return Current integrated odometry pose. */
 	const Transform & getPose() const {return _pose;}
+	/** @return True if @ref OdometryInfo debug/statistics fields are filled in @ref process(). */
 	bool isInfoDataFilled() const {return _fillInfoData;}
-	// Use getVelocityGuess() instead.
+	/** @deprecated Use @ref getVelocityGuess() instead. */
 	RTABMAP_DEPRECATED const Transform & previousVelocityTransform() const;
+	/** @return Last estimated velocity used for motion guessing (may be null). */
 	const Transform & getVelocityGuess() const {return velocityGuess_;}
+	/** @return Timestamp of the previously processed frame. */
 	double previousStamp() const {return previousStamp_;}
+	/** @return Number of frames processed since the last @ref reset(). */
 	unsigned int framesProcessed() const {return framesProcessed_;}
+	/** @return True if input images are already rectified (see @ref Parameters::kRtabmapImagesAlreadyRectified()). */
 	bool imagesAlreadyRectified() const {return _imagesAlreadyRectified;}
 
 protected:
+	/** @return IMU orientations cached from recent frames (stamp → transform). */
 	const std::map<double, Transform> & imus() const {return imus_;}
 
+	/** @brief Constructs the base odometry state from RTAB-Map parameters. */
+	Odometry(const rtabmap::ParametersMap & parameters);
+
 private:
+	/**
+	 * @brief Computes the incremental transform for one frame (implemented by subclasses).
+	 * @param data Sensor data for this frame (may already be decimated or deskewed).
+	 * @param guess Motion prior from the base class or the caller.
+	 * @param info Optional debug/statistics output.
+	 * @return Incremental transform, or null if tracking failed.
+	 */
 	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
 
 	void initKalmanFilter(const Transform & initialPose = Transform::getIdentity(), float vx=0.0f, float vy=0.0f, float vz=0.0f, float vroll=0.0f, float vpitch=0.0f, float vyaw=0.0f);
@@ -128,9 +189,6 @@ private:
 	std::vector<StereoCameraModel> stereoModels_;
 	std::vector<CameraModel> models_;
 	std::map<double, Transform> imus_;
-
-protected:
-	Odometry(const rtabmap::ParametersMap & parameters);
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/OdometryEvent.h

@@ -36,13 +36,40 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class OdometryEvent
+ * @brief One processed frame with the pose the odometry integrated for it.
+ *
+ * Posted by @ref OdometryThread for every frame it processes, and consumed by
+ * @ref RtabmapThread, which passes the data and the pose to
+ * @ref Rtabmap::process(). A null @ref pose() means **odometry is lost** on this
+ * frame; RtabmapThread reads that as a reset and starts a new map.
+ *
+ * The covariance is always 6x6 @c CV_64FC1 with positive finite diagonal terms
+ * (identity when the front-end did not provide one, i.e. "unknown but valid").
+ * A value ≥ 9999 on the first diagonal term is the convention for a lost estimate.
+ *
+ * @see OdometryThread
+ * @see OdometryInfo
+ * @see RtabmapThread
+ */
 class OdometryEvent : public UEvent
 {
 public:
+	/** @brief Creates an empty event, with an identity covariance. */
 	OdometryEvent()
 	{
 		_info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
 	}
+	/**
+	 * @brief Constructor.
+	 * @param data The frame that was processed.
+	 * @param pose Integrated odometry pose, null if odometry is lost.
+	 * @param info Everything else the iteration produced.
+	 *
+	 * An empty covariance in @p info is replaced by identity; otherwise it must be
+	 * 6x6 @c CV_64FC1 with finite, strictly positive diagonal terms.
+	 */
 	OdometryEvent(
 		const SensorData & data,
 		const Transform & pose,
@@ -66,10 +93,19 @@ public:
 	virtual ~OdometryEvent() {}
 	virtual std::string getClassName() const {return "OdometryEvent";}
 
+	/** @return The processed frame, modifiable (e.g. to attach user data). */
 	SensorData & data() {return _data;}
+	/** @return The processed frame. */
 	const SensorData & data() const {return _data;}
+	/** @return Integrated odometry pose, null if odometry was lost on this frame. */
 	const Transform & pose() const {return _pose;}
+	/** @return 6x6 covariance of the motion estimate (@c CV_64FC1). */
 	const cv::Mat & covariance() const {return _info.reg.covariance;}
+	/**
+	 * @brief Linear and angular velocity, from the motion and the frame interval.
+	 * @return (vx, vy, vz, vroll, vpitch, vyaw) in m/s and rad/s, or an empty
+	 *         vector when the interval is unknown.
+	 */
 	std::vector<float> velocity() const {
 		if(_info.interval>0.0)
 		{
@@ -86,6 +122,7 @@ public:
 		}
 		return std::vector<float>();
 	}
+	/** @return Quality indicators, timings and intermediate data of the iteration. */
 	const OdometryInfo & info() const {return _info;}
 
 private:
@@ -94,12 +131,24 @@ private:
 	OdometryInfo _info;
 };
 
+/**
+ * @class OdometryResetEvent
+ * @brief Asks @ref OdometryThread to restart the odometry from a given pose.
+ *
+ * The buffered frames and IMU samples are dropped, and the integration starts
+ * over from @ref getPose(). Handled even before the thread is started.
+ *
+ * @see OdometryThread
+ * @see Odometry::reset()
+ */
 class OdometryResetEvent : public UEvent
 {
 public:
+	/** @param pose Pose to restart from (identity by default). */
 	OdometryResetEvent(const Transform & pose = Transform::getIdentity()){_pose = pose;}
 	virtual ~OdometryResetEvent() {}
 	virtual std::string getClassName() const {return "OdometryResetEvent";}
+	/** @return The pose the odometry should restart from. */
 	const Transform & getPose() const {return _pose;}
 private:
 	Transform _pose;

corelib/include/rtabmap/core/OdometryInfo.h

@@ -34,58 +34,86 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RegistrationInfo.h"
 #include "rtabmap/core/CameraModel.h"
 #include "rtabmap/core/LaserScan.h"
+#if CV_MAJOR_VERSION < 5
 #include <opencv2/features2d/features2d.hpp>
+#else
+#include <opencv2/features.hpp>
+#endif
 
 namespace rtabmap {
 
+/**
+ * @class OdometryInfo
+ * @brief What one @ref Odometry iteration produced, beyond the pose.
+ *
+ * Filled by @ref Odometry::process() when a pointer is passed to it, and carried
+ * to the rest of the application by @ref OdometryEvent. It holds the incremental
+ * motion, the quality indicators used to decide whether the estimate can be
+ * trusted, the timings, and the intermediate data a viewer needs to draw what
+ * the front-end sees (local map, matched features).
+ *
+ * Which fields are filled depends on the front-end: the feature-related ones
+ * come from @ref OdometryF2M and @ref OdometryF2F, and an ICP-only or external
+ * strategy leaves them empty.
+ *
+ * @see Odometry
+ * @see OdometryEvent
+ * @see RegistrationInfo
+ */
 class RTABMAP_CORE_EXPORT OdometryInfo
 {
 public:
 	OdometryInfo();
+	/** @brief A copy without the heavy members (features, local map, scan). */
 	OdometryInfo copyWithoutData() const;
+	/**
+	 * @brief Formats the content as the `Odometry/...` statistics.
+	 * @param pose Current pose, added to the output when not null.
+	 * @return Statistic name (with its unit) to value, as published in @ref Statistics.
+	 */
 	std::map<std::string, float> statistics(const Transform & pose = Transform());
 
-	bool lost;
+	bool lost;               ///< True when the motion could not be estimated on this frame (@ref transform is then null).
-	RegistrationInfo reg;
+	RegistrationInfo reg;    ///< Registration result: matches, inliers, covariance, ICP indicators and timings.
-	int features;
+	int features;            ///< Number of features extracted in the current frame.
-	int localMapSize;
+	int localMapSize;        ///< Number of 3D points in the local feature map (F2M).
-	int localScanMapSize;
+	int localScanMapSize;    ///< Number of points in the local scan map (F2M).
-	int localKeyFrames;
+	int localKeyFrames;      ///< Number of key frames forming the local map (F2M).
-	int localBundleOutliers;
+	int localBundleOutliers; ///< Features rejected by the last local bundle adjustment.
-	int localBundleConstraints;
+	int localBundleConstraints; ///< Feature observations kept by the last local bundle adjustment.
-	float localBundleTime;
+	float localBundleTime;   ///< Time spent in the local bundle adjustment (s).
-	std::map<int, Transform> localBundlePoses;
+	std::map<int, Transform> localBundlePoses; ///< Key frame poses optimized by the local bundle adjustment.
-	std::map<int, std::vector<CameraModel> > localBundleModels;
+	std::map<int, std::vector<CameraModel> > localBundleModels; ///< Camera models of @ref localBundlePoses.
-	float localBundleAvgInlierDistance;
+	float localBundleAvgInlierDistance;    ///< Average distance of the bundle adjustment inliers (m).
-	int localBundleMaxKeyFramesForInlier;
+	int localBundleMaxKeyFramesForInlier;  ///< Highest number of key frames observing a same inlier.
-	std::vector<int> localBundleOutliersPerCam;
+	std::vector<int> localBundleOutliersPerCam; ///< Outliers of the last local bundle adjustment, per camera.
-	bool keyFrameAdded;
+	bool keyFrameAdded;      ///< True if this frame became a key frame of the local map.
-	float timeDeskewing;
+	float timeDeskewing;     ///< Time spent deskewing the laser scan (s).
-	float timeEstimation;
+	float timeEstimation;    ///< Time spent estimating the motion (s).
-	float timeParticleFiltering;
+	float timeParticleFiltering; ///< Time spent in the particle filter (s), when enabled.
-	double stamp;
+	double stamp;            ///< Stamp of the processed frame.
-	double interval;
+	double interval;         ///< Time since the previous processed frame (s); the divisor for velocities.
-	Transform transform;
+	Transform transform;     ///< Motion since the previous frame, null when @ref lost.
-	Transform transformFiltered;
+	Transform transformFiltered; ///< @ref transform after Kalman or particle filtering, when enabled.
-	Transform transformGroundTruth;
+	Transform transformGroundTruth; ///< Ground truth motion since the previous frame, when the data provides it.
-	Transform guessVelocity; // deprecated, will be removed. Use guess and interval instead.
+	Transform guessVelocity; ///< @deprecated Use @ref guess and @ref interval instead.
-	Transform guess;
+	Transform guess;         ///< Motion guess given to the front-end (from the velocity model, an external pose or an IMU).
-	float distanceTravelled;
+	float distanceTravelled; ///< Distance travelled since the odometry was last reset (m).
-	int memoryUsage; //MB
+	int memoryUsage;         ///< Process memory used (MB), only with @ref Parameters::kRtabmapPublishRAMUsage().
-	double gravityRollError;
+	double gravityRollError; ///< Absolute roll difference between the estimated pose and the IMU gravity (rad).
-	double gravityPitchError;
+	double gravityPitchError;///< Absolute pitch difference between the estimated pose and the IMU gravity (rad).
 
-	int type;
+	int type;                ///< Odometry strategy that produced this, see @ref Parameters::kOdomStrategy().
 
 	// F2M
-	std::multimap<int, cv::KeyPoint> words;
+	std::multimap<int, cv::KeyPoint> words; ///< Keypoints of the current frame, by word id (F2M).
-	std::map<int, cv::Point3f> localMap;
+	std::map<int, cv::Point3f> localMap;    ///< Local feature map in the odometry frame, by word id (F2M).
-	LaserScan localScanMap;
+	LaserScan localScanMap;                 ///< Local scan map in the odometry frame (F2M).
 
 	// F2F
-	std::vector<cv::Point2f> refCorners;
+	std::vector<cv::Point2f> refCorners; ///< Corners of the reference frame (F2F).
-	std::vector<cv::Point2f> newCorners;
+	std::vector<cv::Point2f> newCorners; ///< Where those corners were found in the current frame (F2F).
-	std::vector<int> cornerInliers;
+	std::vector<int> cornerInliers;      ///< Indices in @ref refCorners / @ref newCorners kept as inliers (F2F).
 };
 
 }

corelib/include/rtabmap/core/OdometryThread.h

@@ -39,13 +39,67 @@ namespace rtabmap {
 
 class Odometry;
 
+/**
+ * @class OdometryThread
+ * @brief Runs an @ref Odometry front-end in its own thread, driven by events.
+ *
+ * The thread owns the @ref Odometry object and calls @ref Odometry::process() on
+ * the frames it receives, so that a slow odometry update does not block the
+ * sensor thread. It sits in the middle of the event-based pipeline:
+ * @ref SensorCaptureThread → OdometryThread → @ref RtabmapThread.
+ *
+ * **Input events** (handled in @ref handleEvent(), i.e. in the caller's thread):
+ * - @ref SensorEvent — a frame to process. It is rejected with an error if
+ *   it carries neither a laser scan nor an image with its calibration (an
+ *   @ref OdometryMono front-end accepts RGB alone).
+ * - @ref IMUEvent — an IMU sample, kept in a separate buffer.
+ * - @ref OdometryResetEvent — resets the odometry to the pose it carries
+ *   (identity if null) and drops everything buffered. Unlike the others, it is
+ *   handled even before the thread is started.
+ *
+ * **Output event**: one @ref OdometryEvent per processed frame, carrying the
+ * data, the integrated pose and the @ref OdometryInfo. A null pose means
+ * odometry is lost; that is what @ref RtabmapThread reads to start a new map.
+ *
+ * **Buffering.** The frame buffer holds @p dataBufferMaxSize frames and drops
+ * the oldest when full, so with the default size of 1 the odometry always works
+ * on the freshest frame rather than falling behind. IMU samples are buffered
+ * apart and fed to the odometry up to the stamp of the frame about to be
+ * processed, so that tightly-coupled back-ends see them in order. A frame whose
+ * stamp falls outside the buffered IMU window is skipped with a warning: with an
+ * asynchronous IMU, it must be published faster (less delay) than the camera or lidar.
+ *
+ * When the incoming @ref SensorEvent already carries an odometry pose (a robot
+ * publishing its own odometry), the motion between two consecutive such poses is
+ * passed to @ref Odometry::process() as a guess.
+ *
+ * @see Odometry
+ * @see RtabmapThread
+ * @see SensorCaptureThread
+ */
 class RTABMAP_CORE_EXPORT OdometryThread : public UThread, public UEventsHandler {
 public:
-	// take ownership of Odometry
+	/**
+	 * @brief Constructor.
+	 * @param odometry The odometry to run; must not be null. The thread takes
+	 *                 ownership and deletes it in the destructor.
+	 * @param dataBufferMaxSize Maximum number of frames waiting to be processed
+	 *                          (0 = unlimited). Beyond that the oldest frame is
+	 *                          dropped, keeping the odometry on recent data.
+	 */
 	OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize = 1);
 	virtual ~OdometryThread();
 
 protected:
+	/**
+	 * @brief Receives the events listed in the class description.
+	 *
+	 * Runs in the posting thread: frames and IMU samples are only buffered here,
+	 * the odometry itself runs in the thread's main loop. Data events are ignored
+	 * until the thread is started, an @ref OdometryResetEvent is not.
+	 *
+	 * @return Always false, so the event keeps being dispatched to other handlers.
+	 */
 	virtual bool handleEvent(UEvent * event);
 
 private:

corelib/include/rtabmap/core/Optimizer.h

@@ -32,12 +32,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <map>
 #include <list>
+#include <set>
 #include <rtabmap/core/Link.h>
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Signature.h>
 
 namespace rtabmap {
 
+/**
+ * @class FeatureBA
+ * @brief A single bundle adjustment feature observation: one keypoint seen in one frame.
+ *
+ * Used as the per-frame value in the @c wordReferences map (`<wordId, <frameId, FeatureBA>>`)
+ * passed to @ref Optimizer::optimizeBA(). @ref depth is in meters when available (RGB-D / stereo
+ * with disparity) or 0 if unknown (monocular). @ref descriptor and @ref cameraIndex are optional;
+ * @ref cameraIndex selects which @ref CameraModel of a multi-camera rig the keypoint belongs to.
+ */
 class FeatureBA
 {
 public:
@@ -49,31 +59,74 @@ public:
 	{
 		//UDEBUG("kpt=(%f,%f) depth=%f, camIndex=%d", kpt.pt.x, kpt.pt.y, depth, cameraIndex);
 	}
-	cv::KeyPoint kpt;
+	cv::KeyPoint kpt;     ///< 2D image keypoint.
-	float depth;
+	float depth;          ///< Depth at @ref kpt in meters, or 0 if unknown (monocular).
-	cv::Mat descriptor;
+	cv::Mat descriptor;   ///< Optional descriptor for the keypoint (used when re-matching is enabled).
-	int cameraIndex;
+	int cameraIndex;      ///< Index into the frame's camera model list for multi-camera rigs.
 };
 
-////////////////////////////////////////////
+typedef std::map<int, std::set<int> > BAOutliers; // <word ID, rejected pose IDs>, matching wordReferences
-// Graph optimizers
+
-////////////////////////////////////////////
+/**
+ * @class Optimizer
+ * @brief Abstract base for pose-graph and bundle-adjustment optimizers.
+ *
+ * Optimizer is a factory + interface in front of several third-party back-ends
+ * (@ref kTypeTORO "TORO", @ref kTypeG2O "g2o", @ref kTypeGTSAM "GTSAM",
+ * @ref kTypeCeres "Ceres", @ref kTypeCVSBA "cvsba"). Use @ref create() to instantiate one
+ * based on the @c Optimizer/Strategy parameter; use @ref isAvailable() to check whether a
+ * given back-end was compiled in.
+ *
+ * Two families of methods are exposed:
+ * - **Pose-graph optimization** (@ref optimize / @ref optimizeIncremental) — refines poses given
+ *   relative-pose constraints. Subclasses override @ref optimize() with covariance output.
+ * - **Bundle adjustment** (@ref optimizeBA) — jointly refines poses and 3D points using
+ *   reprojection error. Subclasses override the lowest-level overload; the others are
+ *   convenience wrappers that fill in models/correspondences from @ref Signature data.
+ *
+ * Common knobs (iterations, robust kernels, 2D-vs-3D, etc.) are configured through
+ * @ref parseParameters() or per-attribute setters and apply to whichever back-end is selected.
+ */
 class RTABMAP_CORE_EXPORT Optimizer
 {
 public:
+	/** @brief Graph-optimizer back-end identifier. */
 	enum Type {
-		kTypeUndef = -1,
+		kTypeUndef = -1, ///< Unspecified / invalid.
-		kTypeTORO = 0,
+		kTypeTORO = 0,   ///< TORO (tree-based relaxation).
-		kTypeG2O = 1,
+		kTypeG2O = 1,    ///< g2o (general graph optimization, supports BA).
-		kTypeGTSAM = 2,
+		kTypeGTSAM = 2,  ///< GTSAM (factor graphs, iSAM2-style incremental).
-		kTypeCeres = 3,
+		kTypeCeres = 3,  ///< Ceres Solver (nonlinear least squares, supports BA).
-		kTypeCVSBA = 4
+		kTypeCVSBA = 4   ///< cvsba (sparse bundle adjustment only).
 	};
+
+	/**
+	 * @brief Returns whether @p type was compiled in (its third-party dependency was found).
+	 *
+	 * @ref kTypeUndef is treated as unavailable. @ref create() falls back through this check
+	 * to pick the first available back-end when the requested one is missing.
+	 */
 	static bool isAvailable(Optimizer::Type type);
+
+	/**
+	 * @brief Factory: build an optimizer from a @ref ParametersMap.
+	 *
+	 * Reads @c Optimizer/Strategy from @p parameters; falls back to the default strategy if
+	 * the chosen back-end isn't compiled in. Caller owns the returned pointer.
+	 */
 	static Optimizer * create(const ParametersMap & parameters);
+
+	/** @brief Factory: build an optimizer of a specific @p type. Caller owns the result. */
 	static Optimizer * create(Optimizer::Type type, const ParametersMap & parameters = ParametersMap());
 
-	// Get connected poses and constraints from a set of links
+	/**
+	 * @brief Extracts the connected component reachable from @p fromId.
+	 *
+	 * Walks @p linksIn breadth-first starting at @p fromId and copies every visited pose
+	 * (from @p posesIn) and every traversed link into @p posesOut / @p linksOut. Use this to
+	 * isolate the subgraph that actually affects @p fromId before calling @ref optimize(),
+	 * since the back-ends require a single connected component.
+	 */
 	void getConnectedGraph(
 			int fromId,
 			const std::map<int, Transform> & posesIn,
@@ -84,19 +137,23 @@ public:
 public:
 	virtual ~Optimizer() {}
 
+	/** @brief Returns the concrete back-end identifier (one of @ref Type). */
 	virtual Type type() const = 0;
 
-	// getters
+	/// @name Getters for the settings shared across back-ends.
-	int iterations() const {return iterations_;}
+	/// @{
-	bool isSlam2d() const {return slam2d_;}
+	int iterations() const {return iterations_;}             ///< Max solver iterations.
-	bool isCovarianceIgnored() const {return covarianceIgnored_;}
+	bool isSlam2d() const {return slam2d_;}                  ///< True if optimizing in SE(2) instead of SE(3).
-	double epsilon() const {return epsilon_;}
+	bool isCovarianceIgnored() const {return covarianceIgnored_;} ///< If true, all edges share an identity information matrix.
-	bool isRobust() const {return robust_;}
+	double epsilon() const {return epsilon_;}                ///< Convergence threshold on cost decrease.
-	bool priorsIgnored() const {return priorsIgnored_;}
+	bool isRobust() const {return robust_;}                  ///< If true, use a robust kernel / switchable factors against bad loop closures.
-	bool landmarksIgnored() const {return landmarksIgnored_;}
+	bool priorsIgnored() const {return priorsIgnored_;}      ///< If true, unary priors on poses are dropped.
-	float gravitySigma() const {return gravitySigma_;}
+	bool landmarksIgnored() const {return landmarksIgnored_;}///< If true, landmark/marker observations are dropped.
+	float gravitySigma() const {return gravitySigma_;}       ///< Std-dev (rad) of the gravity prior on roll/pitch; 0 disables it.
+	/// @}
 
-	// setters
+	/// @name Setters mirroring the corresponding getters.
+	/// @{
 	void setIterations(int iterations) {iterations_ = iterations;}
 	void setSlam2d(bool enabled) {slam2d_ = enabled;}
 	void setCovarianceIgnored(bool enabled) {covarianceIgnored_ = enabled;}
@@ -105,9 +162,33 @@ public:
 	void setPriorsIgnored(bool enabled) {priorsIgnored_ = enabled;}
 	void setLandmarksIgnored(bool enabled) {landmarksIgnored_ = enabled;}
 	void setGravitySigma(float value) {gravitySigma_ = value;}
+	/// @}
 
+	/**
+	 * @brief Reads shared knobs from @p parameters and applies them to this instance.
+	 *
+	 * Subclasses override to additionally read back-end-specific keys (e.g. @c g2o/Solver,
+	 * @c GTSAM/Optimizer); they should call this base implementation first.
+	 */
 	virtual void parseParameters(const ParametersMap & parameters);
 
+	/**
+	 * @brief Pose-graph optimization that grows the graph one node at a time.
+	 *
+	 * Inserts poses in @p poses iteration order, propagating odometry edges (@c Neighbor /
+	 * @c NeighborMerged) directly and triggering a call to @ref optimize() whenever a loop
+	 * closure is added. A final full @ref optimize() pass anchors at @p rootId. Useful for
+	 * warm-starting heavily-deformed initial guesses where a single-shot @ref optimize()
+	 * can diverge.
+	 *
+	 * @param rootId  Pose whose absolute transform is held fixed.
+	 * @param poses   Initial poses keyed by id.
+	 * @param constraints  Relative-pose constraints (see @ref Link::Type).
+	 * @param intermediateGraphes  Optional: appended per outer iteration for debug/visualization.
+	 * @param finalError  Optional: written with the solver's final cost.
+	 * @param iterationsDone  Optional: written with the solver's actual iteration count.
+	 * @return Refined poses (same key set as @p poses), or empty on failure.
+	 */
 	std::map<int, Transform> optimizeIncremental(
 			int rootId,
 			const std::map<int, Transform> & poses,
@@ -116,6 +197,12 @@ public:
 			double * finalError = 0,
 			int * iterationsDone = 0);
 
+	/**
+	 * @brief Pose-graph optimization (single shot).
+	 *
+	 * Convenience overload that discards the output covariance. See the covariance-returning
+	 * overload below for parameter docs.
+	 */
 	std::map<int, Transform> optimize(
 				int rootId,
 				const std::map<int, Transform> & poses,
@@ -124,7 +211,22 @@ public:
 				double * finalError = 0,
 				int * iterationsDone = 0);
 
-	// inherited classes should implement one of these methods
+	/**
+	 * @brief Pose-graph optimization with marginal covariance of @p rootId.
+	 *
+	 * This is the primary back-end entry point — concrete subclasses override it. The base
+	 * implementation just emits an error.
+	 *
+	 * @param rootId  Pose to hold fixed during optimization.
+	 * @param poses   Initial pose estimates.
+	 * @param constraints  Relative-pose constraints between pose ids (and optionally to landmarks).
+	 * @param outputCovariance  Output: 6x6 covariance of the last optimized pose w.r.t. @p rootId
+	 *                          (3x3 for 2D mode). Filled only if the back-end supports it.
+	 * @param intermediateGraphes  Optional: appended at each iteration for debug/visualization.
+	 * @param finalError  Optional: written with the solver's final cost.
+	 * @param iterationsDone  Optional: written with the solver's actual iteration count.
+	 * @return Refined poses, or empty on failure.
+	 */
 	virtual std::map<int, Transform> optimize(
 				int rootId,
 				const std::map<int, Transform> & poses,
@@ -133,25 +235,57 @@ public:
 				std::list<std::map<int, Transform> > * intermediateGraphes = 0,
 				double * finalError = 0,
 				int * iterationsDone = 0);
+
+	/**
+	 * @brief Bundle adjustment: jointly refine poses and 3D points (back-end-level entry point).
+	 *
+	 * Concrete subclasses (g2o, Ceres, cvsba) override this; the base implementation errors out.
+	 * The other @ref optimizeBA() overloads ultimately funnel here.
+	 *
+	 * @param rootId  Pose to hold fixed. If negative, ALL poses other than the (positive) lowest id
+	 *                are held fixed.
+	 * @param poses   Initial pose estimates keyed by frame id.
+	 * @param links   Edges used to define the BA problem topology.
+	 * @param models  Camera model(s) per frame; for stereo, @c Tx must be set on the model
+	 *                (= -baseline*fx). Multi-camera rigs have multiple entries per frame.
+	 * @param points3DMap  In/out: world 3D points keyed by word id; refined on return.
+	 * @param wordReferences  Observations: `<wordId, <frameId, FeatureBA>>`. See @ref FeatureBA.
+	 * @param outliers  Optional output: word ids dropped by the back-end's outlier rejection.
+	 * @return Refined poses, or empty on failure.
+	 */
 	virtual std::map<int, Transform> optimizeBA(
-			int rootId, // if negative, all other poses are fixed
+			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
-			const std::map<int, std::vector<CameraModel> > & models, // in case of stereo, Tx should be set
+			const std::map<int, std::vector<CameraModel> > & models,
 			std::map<int, cv::Point3f> & points3DMap,
-			const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
+			const std::map<int, std::map<int, FeatureBA> > & wordReferences,
-			std::set<int> * outliers = 0);
+			BAOutliers * outliers = 0);
 
+	/**
+	 * @brief BA wrapper that derives camera models and correspondences from signatures.
+	 *
+	 * Builds @c models per frame from each @ref Signature's @ref SensorData (mono or stereo,
+	 * with stereo-baseline encoded in Tx), calls @ref computeBACorrespondences() to populate
+	 * @p points3DMap / @p wordReferences from the signatures' words, then delegates to the
+	 * back-end overload above.
+	 *
+	 * @param rematchFeatures  If true, re-match descriptors across linked frames before
+	 *                         building correspondences (more accurate, more expensive).
+	 * @param registrationParameters  Forwarded to @ref RegistrationVis when re-matching.
+	 */
 	std::map<int, Transform> optimizeBA(
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
 			const std::map<int, Signature> & signatures,
 			std::map<int, cv::Point3f> & points3DMap,
-			std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
+			std::map<int, std::map<int, FeatureBA> > & wordReferences,
 			bool rematchFeatures = false,
 			const ParametersMap & registrationParameters = ParametersMap());
 
+	/** @brief BA convenience wrapper: like the overload above but ignores the
+	 *  refined 3D points and observation map. */
 	std::map<int, Transform> optimizeBA(
 			int rootId,
 			const std::map<int, Transform> & poses,
@@ -160,19 +294,41 @@ public:
 			bool rematchFeatures = false,
 			const ParametersMap & registrationParameters = ParametersMap());
 
+	/**
+	 * @brief Refine a single two-frame link via BA.
+	 *
+	 * Sets the @c from frame at identity and the @c to frame at @c link.transform(), then runs
+	 * BA over the supplied 3D points and observations. Returns the refined relative transform,
+	 * or @c link.transform() unchanged on failure.
+	 */
 	Transform optimizeBA(
 			const Link & link,
 			const CameraModel & model,
 			std::map<int, cv::Point3f> & points3DMap,
 			const std::map<int, std::map<int, FeatureBA> > & wordReferences,
-			std::set<int> * outliers = 0);
+			BAOutliers * outliers = 0);
 
+	/**
+	 * @brief Build BA correspondences (3D points + per-frame observations) from signatures.
+	 *
+	 * For each link, matches words between the two signatures with @ref RegistrationVis,
+	 * triangulates / lifts depth into world coordinates using the frames' initial poses, and
+	 * populates @p points3DMap (one entry per word id) and @p wordReferences (one entry per
+	 * (word, frame) observation).
+	 *
+	 * @param rematchFeatures  If true, descriptors are re-matched between frames instead of
+	 *                         relying on pre-existing word ids — more robust to feature drift.
+	 * @param useLinkTransformAsGuess  If true, the link's transform seeds the PnP guess
+	 *                                 instead of estimating it from scratch.
+	 * @param registrationParameters  Forwarded to @ref RegistrationVis (estimation type,
+	 *                                inlier counts, NNDR, etc.).
+	 */
 	void computeBACorrespondences(
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
 			const std::map<int, Signature> & signatures,
 			std::map<int, cv::Point3f> & points3DMap,
-			std::map<int, std::map<int, FeatureBA > > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
+			std::map<int, std::map<int, FeatureBA > > & wordReferences,
 			bool rematchFeatures = false,
 			bool useLinkTransformAsGuess = false,
 			ParametersMap registrationParameters = ParametersMap());

corelib/include/rtabmap/core/Parameters.h

@@ -40,7 +40,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
+/** @brief Parameter keys mapped to their values, as used by every configurable class (see @ref Parameters). */
 typedef std::map<std::string, std::string> ParametersMap; // Key, value
+/** @brief A single parameter key/value pair, the entry type of @ref ParametersMap. */
 typedef std::pair<std::string, std::string> ParametersPair;
 
 /**
@@ -63,8 +65,11 @@ typedef std::pair<std::string, std::string> ParametersPair;
  */
 #define RTABMAP_PARAM(PREFIX, NAME, TYPE, DEFAULT_VALUE, DESCRIPTION) \
     public: \
+        /** @brief Key of parameter PREFIX##/##NAME: DESCRIPTION Default value: DEFAULT_VALUE (TYPE). */ \
         static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
+        /** @brief Default value of parameter PREFIX##/##NAME: DEFAULT_VALUE. */ \
         static TYPE default##PREFIX##NAME() {return (TYPE)DEFAULT_VALUE;} \
+        /** @brief Type of parameter PREFIX##/##NAME, as a string: TYPE. */ \
         static std::string type##PREFIX##NAME() {return std::string(#TYPE);} \
     private: \
         class Dummy##PREFIX##NAME { \
@@ -97,8 +102,11 @@ typedef std::pair<std::string, std::string> ParametersPair;
  */
 #define RTABMAP_PARAM_STR(PREFIX, NAME, DEFAULT_VALUE, DESCRIPTION) \
     public: \
+        /** @brief Key of parameter PREFIX##/##NAME: DESCRIPTION Default value: DEFAULT_VALUE (string). */ \
         static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
+        /** @brief Default value of parameter PREFIX##/##NAME: DEFAULT_VALUE. */ \
         static std::string default##PREFIX##NAME() {return DEFAULT_VALUE;} \
+        /** @brief Type of parameter PREFIX##/##NAME, as a string: string. */ \
         static std::string type##PREFIX##NAME() {return std::string("string");} \
     private: \
         class Dummy##PREFIX##NAME { \
@@ -130,8 +138,11 @@ typedef std::pair<std::string, std::string> ParametersPair;
  */
 #define RTABMAP_PARAM_COND(PREFIX, NAME, TYPE, COND, DEFAULT_VALUE1, DEFAULT_VALUE2, DESCRIPTION) \
     public: \
+        /** @brief Key of parameter PREFIX##/##NAME: DESCRIPTION Default value: DEFAULT_VALUE1 if COND, DEFAULT_VALUE2 otherwise (TYPE). */ \
         static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
+        /** @brief Default value of parameter PREFIX##/##NAME: DEFAULT_VALUE1 if COND, DEFAULT_VALUE2 otherwise. */ \
         static TYPE default##PREFIX##NAME() {return COND?DEFAULT_VALUE1:DEFAULT_VALUE2;} \
+        /** @brief Type of parameter PREFIX##/##NAME, as a string: TYPE. */ \
         static std::string type##PREFIX##NAME() {return std::string(#TYPE);} \
     private: \
         class Dummy##PREFIX##NAME { \
@@ -165,7 +176,8 @@ typedef std::pair<std::string, std::string> ParametersPair;
  *         std::string strValue = Util::value(Parameters::getDefaultParameters(), theKey); // strValue = "640"
  * @endcode
  * @see getDefaultParameters()
- * TODO Add a detailed example with simple classes
+ * @see The @ref parameters "Parameter reference" page, which lists every parameter
+ *      with its type, default value and description.
  */
 class RTABMAP_CORE_EXPORT Parameters
 {
@@ -213,17 +225,18 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM_STR(Mem, DepthCompressionFormat,   ".rvl",        "Depth image compression format for 16UC1 depth type. It should be \".png\" or \".rvl\". If depth type is 32FC1, \".png\" is used.");
     RTABMAP_PARAM(Mem, STMSize,                   unsigned int, 10, "Short-term memory size.");
     RTABMAP_PARAM(Mem, IncrementalMemory,           bool, true,     "SLAM mode, otherwise it is Localization mode.");
+    RTABMAP_PARAM(Mem, LocalizationReadOnly,        bool, false,    uFormat("In localization mode, open the database in read-only mode (ignored if %s=true). Currrenty incompatible with memory management (%s and %s cannot be used) and if there are disjoint sessions in working memory. Last localization pose won't be saved back in the database at the end of the session, so the robot will always restart to original last localization pose, unless %s is used or an external initial pose is provided on initialization.", kMemIncrementalMemory().c_str(), kRtabmapLoopThr().c_str(), kRtabmapMemoryThr().c_str(), kRGBDStartAtOrigin().c_str()).c_str());
     RTABMAP_PARAM(Mem, LocalizationDataSaved,       bool, false,    uFormat("Save localization data during localization session (when %s=false). When enabled, the database will then also grow in localization mode. This mode would be used only for debugging purpose.", kMemIncrementalMemory().c_str()).c_str());
     RTABMAP_PARAM(Mem, ReduceGraph,                 bool, false,    uFormat("Reduce graph. Merge nodes when loop closures are added (ignoring those with user data). Note that this approach assumes that 100%% of the loop closures accepted are good, so it is highly recommended to enable \"%s\" at the same time.", kRGBDOptimizeMaxError().c_str()));
     RTABMAP_PARAM(Mem, RecentWmRatio,               float, 0.2,     "Ratio of locations after the last loop closure in WM that cannot be transferred.");
     RTABMAP_PARAM(Mem, TransferSortingByWeightId,   bool, false,    "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");
-    RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne,  bool, false,    "On merge, update to new id. When false, no copy.");
+    RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne,  bool, false,    uFormat("On merge, update to new id. When false, no copy. Keep this disable if %s=true.", kRtabmapCreateIntermediateNodes().c_str()));
     RTABMAP_PARAM(Mem, RehearsalWeightIgnoredWhileMoving, bool, false, "When the robot is moving, weights are not updated on rehearsal.");
     RTABMAP_PARAM(Mem, GenerateIds,                 bool, true,     "True=Generate location IDs, False=use input image IDs.");
     RTABMAP_PARAM(Mem, BadSignaturesIgnored,        bool, false,    "Bad signatures are ignored.");
     RTABMAP_PARAM(Mem, InitWMWithAllNodes,          bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
     RTABMAP_PARAM(Mem, DepthAsMask,                 bool, true,     "Use depth image as mask when extracting features for vocabulary.");
-    RTABMAP_PARAM(Mem, DepthMaskFloorThr,           float, 0.0,     uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kMemDepthAsMask().c_str()));
+    RTABMAP_PARAM(Mem, DepthMaskFloorThr,           float, 0.0,     uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled. Ignored if %s is false.", kMemDepthAsMask().c_str()));
     RTABMAP_PARAM(Mem, StereoFromMotion,            bool, false,    uFormat("Triangulate features without depth using stereo from motion (odometry). It would be ignored if %s is true and the feature detector used supports masking.", kMemDepthAsMask().c_str()));
     RTABMAP_PARAM(Mem, ImagePreDecimation,          unsigned int, 1, uFormat("Decimation of the RGB image before visual feature detection. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.",kMemDepthAsMask().c_str()));
     RTABMAP_PARAM(Mem, ImagePostDecimation,         unsigned int, 1, uFormat("Decimation of the RGB image before saving it to database. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. Decimation is done from the original image. If set to same value than %s, data already decimated is saved (no need to re-decimate the image).", kMemImagePreDecimation().c_str()));
@@ -239,29 +252,29 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Mem, RotateImagesUpsideUp,        bool, false,    "Rotate images so that upside is up if they are not already. This can be useful in case the robots don't have all same camera orientation but are using the same map, so that not rotation-invariant visual features can still be used across the fleet.");
 
     // KeypointMemory (Keypoint-based)
-    RTABMAP_PARAM(Kp, NNStrategy,               int, 1,       "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
+    RTABMAP_PARAM(Kp, NNStrategy,               int, 1,       "FLANN Linear=0, FLANN KdTree=1, FLANN LSH=2, Brute Force=3, Brute Force GPU=4, FLANN KdTree Single=5, NanoFLANN KdTree=6");
     RTABMAP_PARAM(Kp, IncrementalDictionary,    bool, true,   "");
-    RTABMAP_PARAM(Kp, IncrementalFlann,         bool, true,   uFormat("When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary increases of the factor \"%s\" in size).", kKpFlannRebalancingFactor().c_str()));
+    RTABMAP_PARAM(Kp, IncrementalFlann,         bool, true,   uFormat("When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is only rebuilt when too many of its features have been removed, see \"%s\").", kKpFlannRebalancingFactor().c_str()));
-    RTABMAP_PARAM(Kp, FlannRebalancingFactor,   float, 2.0,   uFormat("Factor used when rebuilding the incremental FLANN index (see \"%s\"). Set <=1 to disable.", kKpIncrementalFlann().c_str()));
+    RTABMAP_PARAM(Kp, FlannRebalancingFactor,   float, 2.0,   uFormat("Rebuild the incremental FLANN index (see \"%s\") once the ratio (factor-1)/factor of its features has been removed, e.g. half of them for a factor of 2. Rebuilding frees the memory of the removed features and speeds up the searches. Features are mostly removed when memory management is enabled (\"%s\" or \"%s\"). Set to 1 to never rebuild, which also uses less memory as the features don't have to be referenced one by one.", kKpIncrementalFlann().c_str(), kRtabmapTimeThr().c_str(), kRtabmapMemoryThr().c_str()));
     RTABMAP_PARAM(Kp, ByteToFloat,              bool, false,  uFormat("For %s=1, binary descriptors are converted to float by converting each byte to float instead of converting each bit to float. When converting bytes instead of bits, less memory is used and search is faster at the cost of slightly less accurate matching.", kKpNNStrategy().c_str()));
     RTABMAP_PARAM(Kp, MaxDepth,                 float, 0,     "Filter extracted keypoints by depth (0=inf).");
     RTABMAP_PARAM(Kp, MinDepth,                 float, 0,     "Filter extracted keypoints by depth.");
     RTABMAP_PARAM(Kp, MaxFeatures,              int, 500,     "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
     RTABMAP_PARAM(Kp, SSC,                      bool, false,  "If true, SSC (Suppression via Square Covering) is applied to limit keypoints.");
-    RTABMAP_PARAM(Kp, BadSignRatio,             float, 0.5,   "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
+    RTABMAP_PARAM(Kp, BadSignRatio,             float, 0.5,   uFormat("Bad signature ratio. If %s=0, the ratio is computed from the average number of words per signature (less than Ratio x AverageWordsPerImage = bad).", kKpMaxFeatures().c_str()));
     RTABMAP_PARAM(Kp, NndrRatio,                float, 0.8,   "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
 #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
     // OpenCV>2 without xFeatures2D module doesn't have BRIEF
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 8,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 8,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector 16=SuperPoint-Rpautrat");
 #else
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector 16=SuperPoint-Rpautrat");
 #endif
     RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed,      bool, true,   "Use of the td-idf strategy to compute the likelihood.");
     RTABMAP_PARAM(Kp, Parallelized,             bool, true,   "If the dictionary update and signature creation were parallelized.");
     RTABMAP_PARAM_STR(Kp, RoiRatios,       "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
     RTABMAP_PARAM_STR(Kp, DictionaryPath,       "",           "Path of the pre-computed dictionary");
     RTABMAP_PARAM(Kp, NewWordsComparedTogether, bool, true,   "When adding new words to dictionary, they are compared also with each other (to detect same words in the same signature).");
-    RTABMAP_PARAM(Kp, FlannIndexSaved,          bool, false,  uFormat("Save FLANN index during localization session (when %s=false). The FLANN index will be saved to database after the first time localization mode is used, then on next sessions, the index is reloaded from the database instead of being rebuilt again. This can save significant loading time when the visual word dictionary is big (>1M words). Note that if the dictionary is modified (parameters or data), the index will be rebuilt and saved again on the next session.", kMemIncrementalMemory().c_str()).c_str());
+    RTABMAP_PARAM(Kp, FlannIndexSaved,          bool, false,  uFormat("Save FLANN index during localization session (when %s=false). The FLANN index will be saved to database after the first time localization mode is used, then on next sessions, the index is reloaded from the database instead of being rebuilt again. This can save significant loading time when the visual word dictionary is big (>1M words). Note that if the dictionary is modified (parameters or data), the index will be rebuilt and saved again on the next session. Ignored on initialization if %s is enabled.", kMemIncrementalMemory().c_str(), kMemInitWMWithAllNodes().c_str()).c_str());
     RTABMAP_PARAM(Kp, SerializeWithChecksum,    bool, true,   "On serialization of the FLANN index, compute checksum of the data used by the FLANN index. This adds a slight overhead on serialization/deserialization to make sure that the dictionary data correspond to same data used when the index was built.");
     RTABMAP_PARAM(Kp, SubPixWinSize,            int, 3,       "See cv::cornerSubPix().");
     RTABMAP_PARAM(Kp, SubPixIterations,         int, 0,       "See cv::cornerSubPix(). 0 disables sub pixel refining.");
@@ -271,10 +284,26 @@ class RTABMAP_CORE_EXPORT Parameters
 
     //Database
     RTABMAP_PARAM(DbSqlite3, InMemory,     bool, false,      "Using database in the memory instead of a file on the hard disk.");
-    RTABMAP_PARAM(DbSqlite3, CacheSize, unsigned int, 10000, "Sqlite cache size (default is 2000).");
+    RTABMAP_PARAM(DbSqlite3, CacheSize, unsigned int, 10000,
-    RTABMAP_PARAM(DbSqlite3, JournalMode,  int, 3,           "0=DELETE, 1=TRUNCATE, 2=PERSIST, 3=MEMORY, 4=OFF (see sqlite3 doc : \"PRAGMA journal_mode\")");
+            "PRAGMA cache_size: number of database pages kept in SQLite's page cache (approx. cacheSize * page_size bytes, often ~4 KiB per page). "
-    RTABMAP_PARAM(DbSqlite3, Synchronous,  int, 0,           "0=OFF, 1=NORMAL, 2=FULL (see sqlite3 doc : \"PRAGMA synchronous\")");
+            "Larger values reduce disk I/O when the working set fits in RAM. SQLite built-in default is typically 2000 pages.");
-    RTABMAP_PARAM(DbSqlite3, TempStore,    int, 2,           "0=DEFAULT, 1=FILE, 2=MEMORY (see sqlite3 doc : \"PRAGMA temp_store\")");
+    RTABMAP_PARAM(DbSqlite3, JournalMode,  int, 3,
+            "PRAGMA journal_mode: rollback journal storage. See sqlite.org/pragma.html#pragma_journal_mode for more details. "
+            "0=DELETE (SQLite default): journal file deleted after each commit. "
+            "1=TRUNCATE: journal truncated to zero length. "
+            "2=PERSIST: journal file kept, header zeroed after commit. "
+            "3=MEMORY: journal in RAM only; faster, weaker crash safety. "
+            "4=OFF: no journal; fastest, risk of corruption on crash.");
+    RTABMAP_PARAM(DbSqlite3, Synchronous,  int, 0,
+            "PRAGMA synchronous: how aggressively SQLite syncs the database to disk. See sqlite.org/pragma.html#pragma_synchronous for more details. "
+            "0=OFF: no wait for persistent storage; fastest, corruption possible on power loss. "
+            "1=NORMAL: sync at critical moments (common SQLite default with WAL). "
+            "2=FULL (SQLite safest default): sync after every commit; slowest.");
+    RTABMAP_PARAM(DbSqlite3, TempStore,    int, 2,
+            "PRAGMA temp_store: where SQLite stores temporary tables and indices. See sqlite.org/pragma.html#pragma_temp_store for more details. "
+            "0=DEFAULT: SQLite compile-time default (often on-disk temp files). "
+            "1=FILE: temporary files in the system temp directory. "
+            "2=MEMORY: temporary data in RAM when possible.");
     RTABMAP_PARAM_STR(Db, TargetVersion,   "",               "Target database version for backward compatibility purpose. Only Major and minor versions are used and should be set (e.g., 0.19 vs 0.20 or 1.0 vs 2.0). Patch version is ignored (e.g., 0.20.1 and 0.20.3 will generate a 0.20 database).");
 
     // Keypoints descriptors/detectors
@@ -293,7 +322,8 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(SIFT, PreciseUpscale,    bool, false,  "Whether to enable precise upscaling in the scale pyramid (OpenCV >= 4.8).");
     RTABMAP_PARAM(SIFT, RootSIFT,          bool, false,  "Apply RootSIFT normalization of the descriptors.");
     RTABMAP_PARAM(SIFT, Gpu,               bool, false,  "CudaSift: Use GPU version of SIFT. This option is enabled only if RTAB-Map is built with CudaSift dependency and GPUs are detected.");
-    RTABMAP_PARAM(SIFT, GaussianThreshold, float, 2.0,   "CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features are produced by the detector.");
+    RTABMAP_PARAM(SIFT, GaussianThreshold, float, 2.0,   "CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features with low response/hessian are produced by the detector.");
+    RTABMAP_PARAM(SIFT, MaxGaussianThreshold, float, 0.0,   uFormat("CudaSift: Maximum threshold on difference of Gaussians for feature pruning (ignored if smaller or equal than %s). The lower the threshold, the less features with high response/hessian are produced by the detector.", kSIFTGaussianThreshold().c_str()));
     RTABMAP_PARAM(SIFT, Upscale,           bool, false,  "CudaSift: Whether to enable upscaling.");
 
     RTABMAP_PARAM(BRIEF, Bytes,            int, 32,      "Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");
@@ -346,13 +376,21 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(SuperPoint, NMSRadius,     int,  4,      uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSuperPointNMS().c_str()));
     RTABMAP_PARAM(SuperPoint, Cuda,          bool, true,   "Use Cuda device for Torch, otherwise CPU device is used by default.");
 
+    RTABMAP_PARAM_STR(SuperPointRpautrat, WeightsPath, "",           "[Required] SuperPoint weights file (*.pth).");
+    RTABMAP_PARAM_STR(SuperPointRpautrat, ModelPath, "",           "[Required] SuperPoint python model file (superpoint_pytorch.py).");
+    RTABMAP_PARAM(SuperPointRpautrat, Threshold,     float, 0.005, "Detector response threshold to accept keypoint.");
+    RTABMAP_PARAM(SuperPointRpautrat, NMS,           bool,  true,  "If true, non-maximum suppression is applied to detected keypoints.");
+    RTABMAP_PARAM(SuperPointRpautrat, NMSRadius,     int,  4,      uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSuperPointRpautratNMS().c_str()));
+    RTABMAP_PARAM(SuperPointRpautrat, Cuda,          bool, true,   "Use Cuda device for Torch, otherwise CPU device is used by default.");
+
     RTABMAP_PARAM_STR(PyDetector, Path,       "",           "Path to python script file (see available ones in rtabmap/corelib/src/python/*). See the header to see where the script should be copied.");
 	RTABMAP_PARAM(PyDetector, Cuda,           bool, true,   "Use cuda.");
 
     // BayesFilter
-    RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9,  "Virtual place prior");
+    RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9,  "Virtual place prior. Considering that we are at a new place, this is the prior probability to move again to a new place (unvisited location). The prior probability to move to a previously visited location is 1 - VirtualPlacePriorThr (split equally against all previously visited locations).");
-    RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23", "Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}.");
+    RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23", "Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}. Considering we are at a previously visited location, the first value is the probability to move to a new place (unvisited location), the second value is the probability to stay at the same location, the third value is the probability to move to a neighbor or loop closure at the first depth level, the fourth value is the probability to move to a neighbor or loop closure at the second depth level, etc. If the sum of the values is not 1, the difference is normalized against all remaining visited locations. Normally, the sum of these values should be 1.");
     RTABMAP_PARAM(Bayes, FullPredictionUpdate, bool, false, "Regenerate all the prediction matrix on each iteration (otherwise only removed/added ids are updated).");
+    RTABMAP_PARAM(Bayes, SparsePrediction, bool, true, uFormat("Use a sparse representation of the prediction instead of a dense matrix, which significantly reduces memory usage and processing time on large maps. Ignored when the values of %s sum to less than 1, as the prediction is then not sparse.", kBayesPredictionLC().c_str()).c_str());
 
     // Verify hypotheses
     RTABMAP_PARAM(VhEp, Enabled, bool, false,       uFormat("Verify visual loop closure hypothesis by computing a fundamental matrix. This is done prior to transformation computation when %s is enabled.", kRGBDEnabled().c_str()));
@@ -362,14 +400,15 @@ class RTABMAP_CORE_EXPORT Parameters
 
     // RGB-D SLAM
     RTABMAP_PARAM(RGBD, Enabled,                  bool, true,  "Activate metric SLAM. If set to false, classic RTAB-Map loop closure detection is done using only images and without any metric information.");
-    RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.1,  "Minimum linear displacement (m) to update the map. Rehearsal is done prior to this, so weights are still updated.");
+    RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.1,  uFormat("Minimum linear displacement (m) to update the map. Rehearsal is done prior to this, so weights are still updated. To update the map when not moving, both %s and %s should be set to 0.", Parameters::kRGBDLinearUpdate().c_str(), Parameters::kRGBDAngularUpdate().c_str()));
-    RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated.");
+    RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  uFormat("Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated. To update the map when not moving, both %s and %s should be set to 0.", Parameters::kRGBDLinearUpdate().c_str(), Parameters::kRGBDAngularUpdate().c_str()));
     RTABMAP_PARAM(RGBD, LinearSpeedUpdate,        float, 0.0,  "Maximum linear speed (m/s) to update the map (0 means not limit).");
     RTABMAP_PARAM(RGBD, AngularSpeedUpdate,       float, 0.0,  "Maximum angular speed (rad/s) to update the map (0 means not limit).");
     RTABMAP_PARAM(RGBD, AggressiveLoopThr,        float, 0.05, uFormat("Loop closure threshold used (overriding %s) when a new mapping session is not yet linked to a map of the highest loop closure hypothesis. In localization mode, this threshold is used when there are no loop closure constraints with any map in the cache (%s). In all cases, the goal is to aggressively loop on a previous map in the database. Only used when %s is enabled. Set 1 to disable.", kRtabmapLoopThr().c_str(), kRGBDMaxOdomCacheSize().c_str(), kRGBDEnabled().c_str()));
     RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
     RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
     RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 3.0,   uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. If used with \"%s\", the disabled loop closure links will be removed.", kOptimizerRobust().c_str()));
+    RTABMAP_PARAM(RGBD, OptimizeMaxErrorRepairRadius, float, 0.0,   uFormat("If two consecutive loop closures are rejected by %s on the same old loop closure link, we will remove that old link, and other old links under that radius if necessary, until optimization is accepted. When optimization is accepted, the old loop closure links are removed from the graph. This feature is useful to reject bad loop closures that were accepted previously. Set to 0 to disable this feature.", kRGBDOptimizeMaxError().c_str()));
     RTABMAP_PARAM(RGBD, MaxLoopClosureDistance,   float, 0.0,   "Reject loop closures/localizations if the distance from the map is over this distance (0=disabled).");
     RTABMAP_PARAM(RGBD, ForceOdom3DoF,            bool, true,  uFormat("Force odometry pose to be 3DoF if %s=true.", kRegForce3DoF().c_str()));
     RTABMAP_PARAM(RGBD, StartAtOrigin,            bool, false, uFormat("If true, rtabmap will assume the robot is starting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode (%s=false).", kMemIncrementalMemory().c_str()));
@@ -446,9 +485,11 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(g2o, Solver,            int, 0,          "0=csparse 1=pcg 2=cholmod 3=Eigen");
 #endif
     RTABMAP_PARAM(g2o, Optimizer,         int, 0,          "0=Levenberg 1=GaussNewton");
-    RTABMAP_PARAM(g2o, PixelVariance,     double, 1.0,     "Pixel variance used for bundle adjustment.");
+
-    RTABMAP_PARAM(g2o, RobustKernelDelta, double, 8,       "Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Observations with chi2 over this threshold will be ignored in the second optimization pass.");
+    RTABMAP_PARAM(Optimizer, Baseline,           double, 0.075,   "When doing bundle adjustment with RGB-D data (mono camera + depth), set a fake baseline (m) so the BA backend treats depth as stereo disparity. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Set to 0 to keep the problem mono (depth observations are ignored). For real stereo data the baseline in the calibration (Tx) is used directly.");
-    RTABMAP_PARAM(g2o, Baseline,          double, 0.075,   "When doing bundle adjustment with RGB-D data, we can set a fake baseline (m) to do stereo bundle adjustment (if 0, mono bundle adjustment is done). For stereo data, the baseline in the calibration is used directly.");
+    RTABMAP_PARAM(Optimizer, PixelVariance,     double, 1.0,     "Pixel variance used on the u/v axes of every bundle adjustment reprojection edge. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Should approximate the squared 1-sigma keypoint localization error in pixels. Set higher (e.g. 4-9) if features are noisy (low texture, motion blur, low light, or large detector scale). Set lower (e.g. 0.01-0.1) if features are sub-pixel refined (Lucas-Kanade tracking, parabolic peak interpolation). Intuition: the lower the pixel variance, the more the optimizer trusts the keypoint positions.");
+    RTABMAP_PARAM(Optimizer, DisparityVariance, double, 1.0,     "Disparity variance used on the disparity axis (u - u_right) of stereo / RGB-D bundle adjustment edges. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Defaults to the same value as PixelVariance for backward compatibility. Set higher (e.g. 2-4) if your depth source is noisier than your feature detector's u/v precision (typical for stereo block matchers / SGM at long range). Set lower (e.g. 0.01-0.1) if your depth source is more accurate than the u/v detector (typical for ToF / LiDAR-fused depth where range is measured directly rather than triangulated). Intuition: the lower the disparity variance, the more the optimizer trusts the depth measurements. Geometric note: wider baseline and/or higher image resolution improve a block matcher's effective disparity precision (larger disparity magnitudes and finer sub-pixel refinement), so wide-baseline high-resolution stereo pairs can usually afford a lower disparity variance (e.g. 0.1-0.5); narrow-baseline low-resolution pairs should keep it higher (e.g. 1-4).");
+    RTABMAP_PARAM(Optimizer, RobustKernelDelta, double, 8,       "Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Applies to all BA-capable backends (g2o, GTSAM, Ceres). Observations with chi2 over this threshold will be ignored in the second optimization pass.");
 
     RTABMAP_PARAM(GTSAM, Optimizer,       int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");
     RTABMAP_PARAM(GTSAM, Incremental,     bool, false,     uFormat("Do graph optimization incrementally (iSAM2) to increase optimization speed on loop closures. Note that only GaussNewton and Dogleg optimization algorithms are supported (%s) in this mode.", kGTSAMOptimizer().c_str()));
@@ -456,7 +497,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(GTSAM, IncRelinearizeSkip,       int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.");
 
     // Odometry
-    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D 13=cuVSLAM");
+    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D 13=cuVSLAM 14=LIO-SAM");
     RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,       "Automatically reset odometry after X consecutive images where odometry cannot be computed (a value of 0 disables auto-reset). When a reset occurs, odometry resumes from the last successfully computed pose with large covariance to trigger a new map. If external odometry is used, it will also be reset based on the motion estimated relative to the last computed pose but no large covariance will be received, so that a new map won't be triggered.");
     RTABMAP_PARAM(Odom, Holonomic,              bool, true,   "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
     RTABMAP_PARAM(Odom, FillInfoData,           bool, true,   "Fill info with data (inliers/outliers features).");
@@ -489,9 +530,9 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(OdomF2M, ScanRange,           float, 0,     "[Geometry] Distance Range used to filter points of local map (when > 0). 0 means local map is updated using time and not range.");
     RTABMAP_PARAM(OdomF2M, ValidDepthRatio,     float, 0.75,  "If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map.");
 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
-    RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 1, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
+    RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 1, uFormat("Local bundle adjustment. Value matches the %s parameter: 0=disabled (TORO is not BA-capable), 1=g2o, 2=GTSAM, 3=Ceres, 4=cvsba.", kOptimizerStrategy().c_str()));
 #else
-    RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
+    RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 0, uFormat("Local bundle adjustment. Value matches the %s parameter: 0=disabled (TORO is not BA-capable), 1=g2o, 2=GTSAM, 3=Ceres, 4=cvsba.", kOptimizerStrategy().c_str()));
 #endif
     RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 10, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
     RTABMAP_PARAM(OdomF2M, BundleAdjustmentMinMotion, float, 0.0, "To create a new keyframe with bundle adjustment, a minimum motion (in pixels) can be required. The motion is computed by the average distance between inliers of the previous keyframe and new frame.");
@@ -551,7 +592,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(OdomViso2, BucketWidth,               double, 50,  "Width of bucket.");
     RTABMAP_PARAM(OdomViso2, BucketHeight,              double, 50,  "Height of bucket.");
 
-    // Odometry ORB_SLAM2
+    // Odometry ORB_SLAM
     RTABMAP_PARAM_STR(OdomORBSLAM, VocPath,             "",       "Path to ORB vocabulary (*.txt).");
     RTABMAP_PARAM(OdomORBSLAM, Bf,              double, 0.076,    "Fake IR projector baseline (m) used only when stereo is not used.");
     RTABMAP_PARAM(OdomORBSLAM, ThDepth,         double, 40.0,     "Close/Far threshold. Baseline times.");
@@ -610,71 +651,90 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM_STR(OdomVINSFusion, ConfigPath,     "",  "Path of VINS-Fusion config file.");
 
     // Odometry OpenVINS
-    RTABMAP_PARAM(OdomOpenVINS, UseStereo,                 bool,   true,   "If we have more than 1 camera, if we should try to track stereo constraints between pairs");
+    RTABMAP_PARAM_STR(OdomOpenVINS, ConfigPath,            "",   "Path of OpenVINS config file (*.yaml). Same format used than OpenVINS library. Note that any parameter from that config file will overwrite the same parameter in OdomOpenVINS group.");
-    RTABMAP_PARAM(OdomOpenVINS, UseKLT,                    bool,   true,   "If true we will use KLT, otherwise use a ORB descriptor + robust matching");
+    RTABMAP_PARAM(OdomOpenVINS, UseStereo,                 bool,   true,   "If we have more than 1 camera, if we should try to track stereo constraints between pairs.");
-    RTABMAP_PARAM(OdomOpenVINS, NumPts,                    int,    200,    "Number of points (per camera) we will extract and try to track");
+    RTABMAP_PARAM(OdomOpenVINS, UseKLT,                    bool,   true,   "If true we will use KLT, otherwise use a ORB descriptor + robust matching.");
-    RTABMAP_PARAM(OdomOpenVINS, MinPxDist,                 int,    15,     "Eistance between features (features near each other provide less information)");
+    RTABMAP_PARAM(OdomOpenVINS, NumPts,                    int,    200,    "Number of points (per camera) we will extract and try to track.");
-    RTABMAP_PARAM(OdomOpenVINS, FiTriangulate1d,           bool,   false,  "If we should perform 1d triangulation instead of 3d");
+    RTABMAP_PARAM(OdomOpenVINS, MinPxDist,                 int,    15,     "Eistance between features (features near each other provide less information).");
-    RTABMAP_PARAM(OdomOpenVINS, FiRefineFeatures,          bool,   true,   "If we should perform Levenberg-Marquardt refinement");
+    RTABMAP_PARAM(OdomOpenVINS, FiTriangulate1d,           bool,   false,  "If we should perform 1d triangulation instead of 3d.");
-    RTABMAP_PARAM(OdomOpenVINS, FiMaxRuns,                 int,    5,      "Max runs for Levenberg-Marquardt");
+    RTABMAP_PARAM(OdomOpenVINS, FiRefineFeatures,          bool,   true,   "If we should perform Levenberg-Marquardt refinement.");
-    RTABMAP_PARAM(OdomOpenVINS, FiMaxBaseline,             double, 40,     "Max baseline ratio to accept triangulated features");
+    RTABMAP_PARAM(OdomOpenVINS, FiMaxRuns,                 int,    5,      "Max runs for Levenberg-Marquardt.");
-    RTABMAP_PARAM(OdomOpenVINS, FiMaxCondNumber,           double, 10000,  "Max condition number of linear triangulation matrix accept triangulated features");
+    RTABMAP_PARAM(OdomOpenVINS, FiMaxBaseline,             double, 40,     "Max baseline ratio to accept triangulated features.");
+    RTABMAP_PARAM(OdomOpenVINS, FiMaxCondNumber,           double, 10000,  "Max condition number of linear triangulation matrix accept triangulated features.");
 
-    RTABMAP_PARAM(OdomOpenVINS, UseFEJ,                    bool,   true,   "If first-estimate Jacobians should be used (enable for good consistency)");
+    RTABMAP_PARAM(OdomOpenVINS, UseFEJ,                    bool,   true,   "If first-estimate Jacobians should be used (enable for good consistency).");
-    RTABMAP_PARAM(OdomOpenVINS, Integration,               int,    1,      "0=discrete, 1=rk4, 2=analytical (if rk4 or analytical used then analytical covariance propagation is used)");
+    RTABMAP_PARAM(OdomOpenVINS, Integration,               int,    1,      "0=discrete, 1=rk4, 2=analytical (if rk4 or analytical used then analytical covariance propagation is used).");
-    RTABMAP_PARAM(OdomOpenVINS, CalibCamExtrinsics,        bool,   false,  "Bool to determine whether or not to calibrate imu-to-camera pose");
+    RTABMAP_PARAM(OdomOpenVINS, CalibCamExtrinsics,        bool,   false,  "Bool to determine whether or not to calibrate imu-to-camera pose.");
-    RTABMAP_PARAM(OdomOpenVINS, CalibCamIntrinsics,        bool,   false,  "Bool to determine whether or not to calibrate camera intrinsics");
+    RTABMAP_PARAM(OdomOpenVINS, CalibCamIntrinsics,        bool,   false,  "Bool to determine whether or not to calibrate camera intrinsics.");
-    RTABMAP_PARAM(OdomOpenVINS, CalibCamTimeoffset,        bool,   false,  "Bool to determine whether or not to calibrate camera to IMU time offset");
+    RTABMAP_PARAM(OdomOpenVINS, CalibCamTimeoffset,        bool,   false,  "Bool to determine whether or not to calibrate camera to IMU time offset.");
-    RTABMAP_PARAM(OdomOpenVINS, CalibIMUIntrinsics,        bool,   false,  "Bool to determine whether or not to calibrate the IMU intrinsics");
+    RTABMAP_PARAM(OdomOpenVINS, CalibIMUIntrinsics,        bool,   false,  "Bool to determine whether or not to calibrate the IMU intrinsics.");
-    RTABMAP_PARAM(OdomOpenVINS, CalibIMUGSensitivity,      bool,   false,  "Bool to determine whether or not to calibrate the Gravity sensitivity");
+    RTABMAP_PARAM(OdomOpenVINS, CalibIMUGSensitivity,      bool,   false,  "Bool to determine whether or not to calibrate the Gravity sensitivity.");
-    RTABMAP_PARAM(OdomOpenVINS, MaxClones,                 int,    11,     "Max clone size of sliding window");
+    RTABMAP_PARAM(OdomOpenVINS, MaxClones,                 int,    11,     "Max clone size of sliding window.");
-    RTABMAP_PARAM(OdomOpenVINS, MaxSLAM,                   int,    50,     "Max number of estimated SLAM features");
+    RTABMAP_PARAM(OdomOpenVINS, MaxSLAM,                   int,    50,     "Max number of estimated SLAM features.");
-    RTABMAP_PARAM(OdomOpenVINS, MaxSLAMInUpdate,           int,    25,     "Max number of SLAM features we allow to be included in a single EKF update.");
+    RTABMAP_PARAM(OdomOpenVINS, MaxSLAMInUpdate,           int,    25,     "Max number of SLAM features we allow to be included in a single EKF update..");
-    RTABMAP_PARAM(OdomOpenVINS, MaxMSCKFInUpdate,          int,    50,     "Max number of MSCKF features we will use at a given image timestep.");
+    RTABMAP_PARAM(OdomOpenVINS, MaxMSCKFInUpdate,          int,    50,     "Max number of MSCKF features we will use at a given image timestep..");
-    RTABMAP_PARAM(OdomOpenVINS, FeatRepMSCKF,              int,    0,      "What representation our features are in (msckf features)");
+    RTABMAP_PARAM(OdomOpenVINS, FeatRepMSCKF,              int,    0,      "What representation our features are in (msckf features).");
-    RTABMAP_PARAM(OdomOpenVINS, FeatRepSLAM,               int,    4,      "What representation our features are in (slam features)");
+    RTABMAP_PARAM(OdomOpenVINS, FeatRepSLAM,               int,    4,      "What representation our features are in (slam features).");
-    RTABMAP_PARAM(OdomOpenVINS, DtSLAMDelay,               double, 0.0,    "Delay, in seconds, that we should wait from init before we start estimating SLAM features");
+    RTABMAP_PARAM(OdomOpenVINS, DtSLAMDelay,               double, 0.0,    "Delay, in seconds, that we should wait from init before we start estimating SLAM features.");
-    RTABMAP_PARAM(OdomOpenVINS, GravityMag,                double, 9.81,   "Gravity magnitude in the global frame (i.e. should be 9.81 typically)");
+    RTABMAP_PARAM(OdomOpenVINS, GravityMag,                double, 9.81,   "Gravity magnitude in the global frame (i.e. should be 9.81 typically).");
-    RTABMAP_PARAM_STR(OdomOpenVINS, LeftMaskPath,                  "",     "Mask for left image");
+    RTABMAP_PARAM_STR(OdomOpenVINS, LeftMaskPath,                  "",     "Mask for left image.");
-    RTABMAP_PARAM_STR(OdomOpenVINS, RightMaskPath,                 "",     "Mask for right image");
+    RTABMAP_PARAM_STR(OdomOpenVINS, RightMaskPath,                 "",     "Mask for right image.");
 
-    RTABMAP_PARAM(OdomOpenVINS, InitWindowTime,            double, 2.0,    "Amount of time we will initialize over (seconds)");
+    RTABMAP_PARAM(OdomOpenVINS, InitWindowTime,            double, 2.0,    "Amount of time we will initialize over (seconds).");
-    RTABMAP_PARAM(OdomOpenVINS, InitIMUThresh,             double, 1.0,    "Variance threshold on our acceleration to be classified as moving");
+    RTABMAP_PARAM(OdomOpenVINS, InitIMUThresh,             double, 1.0,    "Variance threshold on our acceleration to be classified as moving.");
-    RTABMAP_PARAM(OdomOpenVINS, InitMaxDisparity,          double, 10.0,   "Max disparity to consider the platform stationary (dependent on resolution)");
+    RTABMAP_PARAM(OdomOpenVINS, InitMaxDisparity,          double, 10.0,   "Max disparity to consider the platform stationary (dependent on resolution).");
-    RTABMAP_PARAM(OdomOpenVINS, InitMaxFeatures,           int,    50,     "How many features to track during initialization (saves on computation)");
+    RTABMAP_PARAM(OdomOpenVINS, InitMaxFeatures,           int,    50,     "How many features to track during initialization (saves on computation).");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynUse,                bool,   false,  "If dynamic initialization should be used");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynUse,                bool,   false,  "If dynamic initialization should be used.");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEOptCalib,        bool,   false,  "If we should optimize calibration during intialization (not recommended)");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEOptCalib,        bool,   false,  "If we should optimize calibration during intialization (not recommended).");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxIter,         int,    50,     "How many iterations the MLE refinement should use (zero to skip the MLE)");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxIter,         int,    50,     "How many iterations the MLE refinement should use (zero to skip the MLE).");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxTime,         double, 0.05,   "How many seconds the MLE should be completed in");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxTime,         double, 0.05,   "How many seconds the MLE should be completed in.");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxThreads,      int,    6,      "How many threads the MLE should use");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxThreads,      int,    6,      "How many threads the MLE should use.");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynNumPose,            int,    6,      "Number of poses to use within our window time (evenly spaced)");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynNumPose,            int,    6,      "Number of poses to use within our window time (evenly spaced).");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMinDeg,             double, 10.0,   "Orientation change needed to try to init");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMinDeg,             double, 10.0,   "Orientation change needed to try to init.");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationOri,       double, 10.0,   "What to inflate the recovered q_GtoI covariance by");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationOri,       double, 10.0,   "What to inflate the recovered q_GtoI covariance by.");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationVel,       double, 100.0,  "What to inflate the recovered v_IinG covariance by");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationVel,       double, 100.0,  "What to inflate the recovered v_IinG covariance by.");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBg,        double, 10.0,   "What to inflate the recovered bias_g covariance by");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBg,        double, 10.0,   "What to inflate the recovered bias_g covariance by.");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBa,        double, 100.0,  "What to inflate the recovered bias_a covariance by");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBa,        double, 100.0,  "What to inflate the recovered bias_a covariance by.");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMinRecCond,         double, 1e-15,  "Reciprocal condition number thresh for info inversion");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMinRecCond,         double, 1e-15,  "Reciprocal condition number thresh for info inversion.");
 
-    RTABMAP_PARAM(OdomOpenVINS, TryZUPT,                   bool,   true,   "If we should try to use zero velocity update");
+    RTABMAP_PARAM(OdomOpenVINS, TryZUPT,                   bool,   true,   "If we should try to use zero velocity update.");
-    RTABMAP_PARAM(OdomOpenVINS, ZUPTChi2Multiplier,        double, 0.0,    "Chi2 multiplier for zero velocity");
+    RTABMAP_PARAM(OdomOpenVINS, ZUPTChi2Multiplier,        double, 0.0,    "Chi2 multiplier for zero velocity.");
-    RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxVelodicy,           double, 0.1,    "Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt)");
+    RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxVelodicy,           double, 0.1,    "Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt).");
-    RTABMAP_PARAM(OdomOpenVINS, ZUPTNoiseMultiplier,       double, 10.0,   "Multiplier of our zupt measurement IMU noise matrix (default should be 1.0)");
+    RTABMAP_PARAM(OdomOpenVINS, ZUPTNoiseMultiplier,       double, 10.0,   "Multiplier of our zupt measurement IMU noise matrix (default should be 1.0).");
-    RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxDisparity,          double, 0.5,    "Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt)");
+    RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxDisparity,          double, 0.5,    "Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt).");
-    RTABMAP_PARAM(OdomOpenVINS, ZUPTOnlyAtBeginning,       bool,   false,  "If we should only use the zupt at the very beginning static initialization phase");
+    RTABMAP_PARAM(OdomOpenVINS, ZUPTOnlyAtBeginning,       bool,   false,  "If we should only use the zupt at the very beginning static initialization phase.");
 
-    RTABMAP_PARAM(OdomOpenVINS, AccelerometerNoiseDensity, double, 0.01,   "[m/s^2/sqrt(Hz)] (accel \"white noise\")");
+    RTABMAP_PARAM(OdomOpenVINS, AccelerometerNoiseDensity, double, 0.01,   "[m/s^2/sqrt(Hz)] (accel \"white noise\").");
-    RTABMAP_PARAM(OdomOpenVINS, AccelerometerRandomWalk,   double, 0.001,  "[m/s^3/sqrt(Hz)] (accel bias diffusion)");
+    RTABMAP_PARAM(OdomOpenVINS, AccelerometerRandomWalk,   double, 0.001,  "[m/s^3/sqrt(Hz)] (accel bias diffusion).");
-    RTABMAP_PARAM(OdomOpenVINS, GyroscopeNoiseDensity,     double, 0.001,  "[rad/s/sqrt(Hz)] (gyro \"white noise\")");
+    RTABMAP_PARAM(OdomOpenVINS, GyroscopeNoiseDensity,     double, 0.001,  "[rad/s/sqrt(Hz)] (gyro \"white noise\").");
-    RTABMAP_PARAM(OdomOpenVINS, GyroscopeRandomWalk,       double, 0.0001, "[rad/s^2/sqrt(Hz)] (gyro bias diffusion)");
+    RTABMAP_PARAM(OdomOpenVINS, GyroscopeRandomWalk,       double, 0.0001, "[rad/s^2/sqrt(Hz)] (gyro bias diffusion).");
-    RTABMAP_PARAM(OdomOpenVINS, UpMSCKFSigmaPx,            double, 1.0,    "Pixel noise for MSCKF features");
+    RTABMAP_PARAM(OdomOpenVINS, UpMSCKFSigmaPx,            double, 1.0,    "Pixel noise for MSCKF features.");
-    RTABMAP_PARAM(OdomOpenVINS, UpMSCKFChi2Multiplier,     double, 1.0,    "Chi2 multiplier for MSCKF features");
+    RTABMAP_PARAM(OdomOpenVINS, UpMSCKFChi2Multiplier,     double, 1.0,    "Chi2 multiplier for MSCKF features.");
-    RTABMAP_PARAM(OdomOpenVINS, UpSLAMSigmaPx,             double, 1.0,    "Pixel noise for SLAM features");
+    RTABMAP_PARAM(OdomOpenVINS, UpSLAMSigmaPx,             double, 1.0,    "Pixel noise for SLAM features.");
-    RTABMAP_PARAM(OdomOpenVINS, UpSLAMChi2Multiplier,      double, 1.0,    "Chi2 multiplier for SLAM features");
+    RTABMAP_PARAM(OdomOpenVINS, UpSLAMChi2Multiplier,      double, 1.0,    "Chi2 multiplier for SLAM features.");
 
     // Odometry Open3D
     RTABMAP_PARAM(OdomOpen3D, MaxDepth,         float, 3.0,  "Maximum depth.");
     RTABMAP_PARAM(OdomOpen3D, Method,           int, 0,  "Registration method: 0=PointToPlane, 1=Intensity, 2=Hybrid.");
 
+    // Odometry cuVSLAM
+    RTABMAP_PARAM(OdomCuVSLAM, MulticamMode,        int, 0,  "cuVSLAM multicam_mode setting: 0=moderate, 1=performance, 2=precision.");
+
+    // Odometry LIO-SAM
+    RTABMAP_PARAM_STR(OdomLIOSAM, ConfigPath,  "", "Path to LIO-SAM params.yaml config file. When set, sensor/IMU/feature parameters are loaded from the file and the individual parameters below are ignored.");
+    RTABMAP_PARAM(OdomLIOSAM, Sensor,       int,   0,     "LiDAR sensor: 0=Velodyne, 1=Ouster, 2=Livox");
+    RTABMAP_PARAM(OdomLIOSAM, NScan,        int,   16,    "Number of LiDAR channels (16, 32, 64, 128).");
+    RTABMAP_PARAM(OdomLIOSAM, HorizonScan,  int,   1800,  "Horizontal resolution (Velodyne:1800, Ouster:512/1024/2048).");
+    RTABMAP_PARAM(OdomLIOSAM, ImuAccNoise,  float, 0.01,  "IMU accelerometer white noise.");
+    RTABMAP_PARAM(OdomLIOSAM, ImuGyrNoise,  float, 0.001, "IMU gyroscope white noise.");
+    RTABMAP_PARAM(OdomLIOSAM, ImuAccBiasN,  float, 0.0002,"IMU accelerometer bias noise.");
+    RTABMAP_PARAM(OdomLIOSAM, ImuGyrBiasN,  float, 0.00003,"IMU gyroscope bias noise.");
+    RTABMAP_PARAM(OdomLIOSAM, ImuGravity,   float, 9.80511,"Gravity magnitude.");
+    RTABMAP_PARAM(OdomLIOSAM, EdgeThreshold,float, 1.0,   "Edge feature curvature threshold.");
+    RTABMAP_PARAM(OdomLIOSAM, SurfThreshold,float, 0.1,   "Surface feature curvature threshold.");
+    RTABMAP_PARAM(OdomLIOSAM, LinVar,       float, 0.01,  "Linear output variance.");
+    RTABMAP_PARAM(OdomLIOSAM, AngVar,       float, 0.01,  "Angular output variance.");
+
     // Common registration parameters
     RTABMAP_PARAM(Reg, RepeatOnce,               bool, true,    "Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration (like on loop closure). It can be useful if the registration approach used can use a guess to get better matches.");
     RTABMAP_PARAM(Reg, Strategy,                 int, 0,        "0=Vis, 1=Icp, 2=VisIcp");
@@ -704,16 +764,16 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Vis, Iterations,               int,    300,   "Maximum iterations to compute the transform.");
 #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
     // OpenCV>2 without xFeatures2D module doesn't have BRIEF
-    RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
+    RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector 16=SuperPoint-Rpautrat");
 #else
-    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
+    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector 16=SuperPoint-Rpautrat");
 #endif
     RTABMAP_PARAM(Vis, MaxFeatures,               int,   1000,  "0 no limits.");
     RTABMAP_PARAM(Vis, SSC,                       bool,  false, "If true, SSC (Suppression via Square Covering) is applied to limit keypoints.");
     RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,     "Max depth of the features (0 means no limit).");
     RTABMAP_PARAM(Vis, MinDepth,                  float, 0,     "Min depth of the features (0 means no limit).");
     RTABMAP_PARAM(Vis, DepthAsMask,               bool,  true,  "Use depth image as mask when extracting features.");
-    RTABMAP_PARAM(Vis, DepthMaskFloorThr,         float, 0.0,    uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kVisDepthAsMask().c_str()));
+    RTABMAP_PARAM(Vis, DepthMaskFloorThr,         float, 0.0,    uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled. Ignored if %s is false.", kVisDepthAsMask().c_str()));
     RTABMAP_PARAM_STR(Vis, RoiRatios,        "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
     RTABMAP_PARAM(Vis, SubPixWinSize,             int,   3,     "See cv::cornerSubPix().");
     RTABMAP_PARAM(Vis, SubPixIterations,          int,   0,     "See cv::cornerSubPix(). 0 disables sub pixel refining.");
@@ -721,7 +781,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Vis, GridRows,                  int,   1,     uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
     RTABMAP_PARAM(Vis, GridCols,                  int,   1,     uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
     RTABMAP_PARAM(Vis, CorType,                   int,   0,     "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
-    RTABMAP_PARAM(Vis, CorNNType,                 int,   1,     uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4, BruteForceCrossCheck=5, SuperGlue=6, GMS=7. Used for features matching approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorNNType,                 int,   1,     uFormat("[%s=0] FLANN Linear=0, FLANN KdTree=1, FLANN LSH=2, Brute Force=3, Brute Force GPU=4, Brute Force Cross Check=5, SuperGlue=6, GMS=7, FLANN KdTree Single=8, NanoFLANN KdTree=9. Used for features matching approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorNNDR,                   float, 0.8,   uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorGuessWinSize,           int,   40,    uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool,  false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str()));
@@ -729,11 +789,14 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Vis, CorFlowIterations,         int,   30,    uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowEps,                float, 0.01,  uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowMaxLevel,           int,   3,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
-    RTABMAP_PARAM(Vis, CorFlowGpu,                bool,  false, uFormat("[%s=1] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA).", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowUseMinEigenVals,    bool,  true,  uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach. Use minimum eigen values as an error measure, otherwise L1 distance between patches is used as an error measure.", kVisCorType().c_str()));
-#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
+    RTABMAP_PARAM(Vis, CorFlowMinEigThreshold,    float, 1e-4,  uFormat("[%s=true] If the minimum eigenvalue of a feature's spatial gradient matrix is less than this threshold, then the feature is filtered out.", kVisCorFlowUseMinEigenVals().c_str()));
-    RTABMAP_PARAM(Vis, BundleAdjustment,          int,   1,     "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
+    RTABMAP_PARAM(Vis, CorFlowErrorThreshold,     float, 20,    uFormat("[%s=false] Filter out features with error greater than this threshold.", kVisCorFlowUseMinEigenVals().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowGpu,                bool,  false, uFormat("[%s=1] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA). Note that %s is not used in the GPU implementation.", kVisCorType().c_str(), kVisCorFlowUseMinEigenVals().c_str()));
+    #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
+    RTABMAP_PARAM(Vis, BundleAdjustment,          int,   1,     uFormat("Optimization with bundle adjustment. Value matches the %s parameter: 0=disabled (TORO is not BA-capable), 1=g2o, 2=GTSAM, 3=Ceres, 4=cvsba.", kOptimizerStrategy().c_str()));
 #else
-    RTABMAP_PARAM(Vis, BundleAdjustment,          int,   0,     "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
+    RTABMAP_PARAM(Vis, BundleAdjustment,          int,   0,     uFormat("Optimization with bundle adjustment. Value matches the %s parameter: 0=disabled (TORO is not BA-capable), 1=g2o, 2=GTSAM, 3=Ceres, 4=cvsba.", kOptimizerStrategy().c_str()));
 #endif
 
     // Features matching approaches
@@ -783,8 +846,9 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Icp, PointToPlaneRadius,          float, 0.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
     RTABMAP_PARAM(Icp, PointToPlaneGroundNormalsUp, float, 0.0,   "Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans.");
     RTABMAP_PARAM(Icp, PointToPlaneMinComplexity,   float, 0.02,  uFormat("Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when %s=true.", kIcpPointToPlaneLowComplexityStrategy().c_str(), kIcpPointToPlane().c_str()));
-    RTABMAP_PARAM(Icp, PointToPlaneLowComplexityStrategy, int, 1, uFormat("If structural complexity is below %s: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept \"as is\" the transform computed by PointToPoint.", kIcpPointToPlaneMinComplexity().c_str()));
+    RTABMAP_PARAM(Icp, PointToPlaneComplexityCentered, bool, false, uFormat("If false (default), the complexity metric uses the uncentered second-moment matrix (1/N) * sum(n_i * n_i^T), whose smallest eigenvalue directly measures how well the surface normals span R^N. If true, uses centered PCA (cv::PCA covariance) for backwards compatibility -- but the centered metric is known to mis-classify perpendicular-surface scenes as degenerate when normals are consistently viewpoint-flipped (only N distinct directions in N-D collapse to rank N-1 after centering). For true degeneracies (parallel surfaces, e.g. corridors) the two metrics agree because the normal mean is zero. The %s threshold of 0.02 works under either setting.", kIcpPointToPlaneMinComplexity().c_str()));
-    RTABMAP_PARAM(Icp, OutlierRatio,                float, 0.85,   uFormat("Outlier ratio used with %s>0. For libpointmatcher, this parameter set TrimmedDistOutlierFilter/ratio for convenience when configuration file is not set. For CCCoreLib, this parameter set the \"finalOverlapRatio\". The value should be between 0 and 1.", kIcpStrategy().c_str()));
+    RTABMAP_PARAM(Icp, PointToPlaneLowComplexityStrategy, int, 1, uFormat("If structural complexity is below %s: set to 0 so that the transform is automatically rejected, set to 1 (default, legacy) to recompute the transform with PointToPoint and limit its correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to recompute the transform with PointToPoint and accept it \"as is\", set to 3 to keep the PointToPlane transform and apply the same axis-constrained projection as strategy 1.", kIcpPointToPlaneMinComplexity().c_str()));
+    RTABMAP_PARAM(Icp, OutlierRatio,                float, 0.85,   uFormat("Outlier ratio. For libpointmatcher (%s=1), sets TrimmedDistOutlierFilter/ratio for convenience when configuration file is not set. For CCCoreLib (%s=2), sets \"finalOverlapRatio\". For PCL (%s=0), if 0<value<1, installs a RANSAC correspondence rejector with inlier threshold = value * %s. The value should be between 0 and 1.", kIcpStrategy().c_str(), kIcpStrategy().c_str(), kIcpStrategy().c_str(), kIcpMaxCorrespondenceDistance().c_str()));
     RTABMAP_PARAM_STR(Icp, DebugExportFormat,       "",           "Export scans used for ICP in the specified format (a warning on terminal will be shown with the file paths used). Supported formats are \"pcd\", \"ply\" or \"vtk\". If logger level is debug, from and to scans will stamped, so previous files won't be overwritten.");
 
     // libpointmatcher
@@ -807,7 +871,10 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Stereo, OpticalFlow,           bool, true,    "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
     RTABMAP_PARAM(Stereo, SSD,                   bool, true,    uFormat("[%s=false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.", kStereoOpticalFlow().c_str()));
     RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  uFormat("[%s=true] Epsilon stop criterion.", kStereoOpticalFlow().c_str()));
-    RTABMAP_PARAM(Stereo, Gpu,                   bool, false,   uFormat("[%s=true] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA).", kStereoOpticalFlow().c_str()));
+    RTABMAP_PARAM(Stereo, UseMinEigenVals,       bool, true,    uFormat("[%s=true] Use minimum eigen values as an error measure, otherwise L1 distance between patches is used as an error measure.", kStereoOpticalFlow().c_str()));
+    RTABMAP_PARAM(Stereo, MinEigThreshold,       double, 1e-4,  uFormat("[%s=true] If the minimum eigenvalue of a feature's spatial gradient matrix is less than this threshold, then the feature is filtered out.", kStereoUseMinEigenVals().c_str()));
+    RTABMAP_PARAM(Stereo, ErrorThreshold,        double, 50,    uFormat("[%s=false] Filter out features with error greater than this threshold.", kStereoUseMinEigenVals().c_str()));
+    RTABMAP_PARAM(Stereo, Gpu,                   bool, false,   uFormat("[%s=true] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA). Note that %s is not used in the GPU implementation.", kStereoOpticalFlow().c_str(), kStereoUseMinEigenVals().c_str()));
 
     RTABMAP_PARAM(Stereo, DenseStrategy,         int, 0,  "0=cv::StereoBM, 1=cv::StereoSGBM");
 
@@ -883,19 +950,29 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(GridGlobal, ProbClampingMax,      float,  0.971,   "Probability clamping maximum (value between 0 and 1).");
     RTABMAP_PARAM(GridGlobal, FloodFillDepth,       unsigned int, 0, "Flood fill filter (0=disabled), used to remove empty cells outside the map. The flood fill is done at the specified depth (between 1 and 16) of the OctoMap.");
 
-    RTABMAP_PARAM(Marker, Dictionary,             int,   0,     "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20");
+    RTABMAP_PARAM(Marker, Strategy,               int,   0,     "Marker detection implementation: 0=OpenCV, 1=AprilTag");
-    RTABMAP_PARAM(Marker, Length,                 float, 0,     "The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization).");
+    RTABMAP_PARAM(Marker, Dictionary,             int,   0,     "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20, DICT_ARUCO_MIP_36H12=21");
+    RTABMAP_PARAM(Marker, Length,                 float, 0,     "The length (m) of the markers' side. Value <=0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization). If 0, the length is estimated only on the first marker detected, then re-used for all next detections (i.e., this assumes that markers have all the same length). With <0, the length is estimated once for each unique marker, then re-used for next detections with the same marker ID.");
+    RTABMAP_PARAM_STR(Marker, Lengths,            "",           uFormat("List of markers to detect. Format is the marker's ID followed by its length (in meters), multiple markers are separated by a vertical line (\"id1 length|id2 length\"). We can also define a range of markers with \"id1:id2 length\" (id2 included). If empty, all markers of the chosen dictionary can be detected and their length is set/estimated based on %s. For example, to detect markers 12 and 14 with lengths of 8 and 15 cm respectively, and all markers between 30 and 40 with a length of 10 cm, set \"12 0.08|14 0.15|30:40 0.1\".", kMarkerLength().c_str()).c_str());
     RTABMAP_PARAM(Marker, MaxDepthError,          float, 0.01,  uFormat("Maximum depth error between all corners of a marker when estimating the marker length (when %s is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length.", kMarkerLength().c_str()));
     RTABMAP_PARAM(Marker, VarianceLinear,         float, 0.001, uFormat("Linear variance to set on marker detections. If %s is enabled and %s=2 (GTSAM): it is the variance of the range factor, with 9999 to disable range factor and to do only bearing.", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str()));
     RTABMAP_PARAM(Marker, VarianceAngular,        float, 0.01,  uFormat("Angular variance to set on marker detections. If %s is enabled, it is ignored with %s=1 (g2o) and it corresponds to bearing variance with %s=2 (GTSAM).", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str(), kOptimizerStrategy().c_str()));
     RTABMAP_PARAM(Marker, VarianceOrientationIgnored, bool, false, uFormat("When this setting is false, the landmark's orientation is optimized during graph optimization. When this setting is true, only the position of the landmark is optimized. This can be useful when the landmark's orientation estimation is not reliable. Note that for %s=1 (g2o), only %s needs be set if we ignore orientation. For %s=2 (GTSAM), instead of optimizing the landmark's position directly, a bearing/range factor is used, with %s as the variance of the range factor (with 9999 to optimize the position with only a bearing factor) and %s as the variance of the bearing factor (pitch/yaw).", kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kMarkerVarianceAngular().c_str()));
-    RTABMAP_PARAM(Marker, CornerRefinementMethod, int,   0,     "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
     RTABMAP_PARAM(Marker, MaxRange,               float, 0.0,   "Maximum range in which markers will be detected. <=0 for unlimited range.");
     RTABMAP_PARAM(Marker, MinRange,               float, 0.0,   "Miniminum range in which markers will be detected. <=0 for unlimited range.");
-    RTABMAP_PARAM_STR(Marker, Priors,             "",           "World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), markers are separated by vertical line (\"id1 x y z roll pitch yaw|id2 x y z roll pitch yaw\"). Example:  \"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57\" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).");
+    RTABMAP_PARAM_STR(Marker, Priors,             "",           "World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), multiple markers are separated by vertical line (\"id1 x y z roll pitch yaw|id2 x y z roll pitch yaw\"). Example:  \"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57\" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).");
     RTABMAP_PARAM(Marker, PriorsVarianceLinear,   float, 0.001, "Linear variance to set on marker priors.");
     RTABMAP_PARAM(Marker, PriorsVarianceAngular,  float, 0.001, "Angular variance to set on marker priors.");
 
+    RTABMAP_PARAM(MarkerAprilTag, NThreads,         int,     1,     "How many threads should be used?");
+    RTABMAP_PARAM(MarkerAprilTag, QuadDecimate,     float,   1.0,   "Detection of quads can be done on a lower-resolution image, improving speed at a cost of pose accuracy and a slight decrease in detection rate. Decoding the binary payload is still done at full resolution.");
+    RTABMAP_PARAM(MarkerAprilTag, QuadSigma,        float,   0.0,   "What Gaussian blur should be applied to the segmented image (used for quad detection?)  Parameter is the standard deviation in pixels.  Very noisy images benefit from non-zero values (e.g. 0.8).");
+    RTABMAP_PARAM(MarkerAprilTag, RefineEdges,      bool,    true,   uFormat("When true, the edges of the each quad are adjusted to \"snap to\" strong gradients nearby. This is useful when decimation is employed, as it can increase the quality of the initial quad estimate substantially. Generally recommended to be on (true). Very computationally inexpensive. Option is ignored if %s = 1.", kMarkerAprilTagQuadDecimate().c_str()));
+    RTABMAP_PARAM(MarkerAprilTag, DecodeSharpening, double,  0.25,  "How much sharpening should be done to decoded images? This can help decode small tags but may or may not help in odd lighting conditions or low light conditions.");
+    RTABMAP_PARAM(MarkerAprilTag, Debug,            bool,    false, uFormat("When true, write a variety of debugging images to the working directory where the app started (not %s) at various stages through the detection process. (Somewhat slow).", kRtabmapWorkingDirectory().c_str()));
+
+    RTABMAP_PARAM(MarkerOpenCV, CornerRefinementMethod, int,   0,     "Corner refinement method for OpenCV strategy (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
+
     RTABMAP_PARAM(ImuFilter, MadgwickGain,                  double, 0.1,  "Gain of the filter. Higher values lead to faster convergence but more noise. Lower values lead to slower convergence but smoother signal, belongs in [0, 1].");
     RTABMAP_PARAM(ImuFilter, MadgwickZeta,                  double, 0.0,  "Gyro drift gain (approx. rad/s), belongs in [-1, 1].");
 

corelib/include/rtabmap/core/ParticleFilter.h

@@ -69,7 +69,7 @@ std::vector<double> resample(const std::vector<double> & p, // particles
 	std::vector<double> np; //new particles
 	if(p.size() != w.size() || p.size() == 0)
 	{
-		UERROR("particles (%d) and weights (%d) are not the same size", p.size(), w.size());
+		UERROR("particles (%d) and weights (%d) are not the same size", (int)p.size(), (int)w.size());
 		return np;
 	}
 

corelib/include/rtabmap/core/PythonInterface.h

@@ -21,16 +21,24 @@ class gil_scoped_release;
 namespace rtabmap {
 
 /**
- * Create a single PythonInterface on main thread at
+ * Process-wide singleton owning the embedded Python interpreter.
- * global scope before any Python classes.
+ * Call PythonInterface::instance() from the main thread (typically near
+ * the top of main()) before any Python-using class is constructed.
  */
 class RTABMAP_CORE_EXPORT PythonInterface
 {
 public:
-	PythonInterface();
+	// Pass a caller tag (e.g. class name) so the main-thread assertion
-	virtual ~PythonInterface();
+	// can report who triggered the first construction.
+	static PythonInterface & instance(const std::string & caller = "");
+
+	PythonInterface(const PythonInterface &) = delete;
+	PythonInterface & operator=(const PythonInterface &) = delete;
 
 private:
+	explicit PythonInterface(const std::string & caller);
+	~PythonInterface();
+
 	pybind11::scoped_interpreter* guard_;
 	pybind11::gil_scoped_release* release_;
 };

corelib/include/rtabmap/core/Registration.h

@@ -36,52 +36,90 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class Registration
+ * @brief Abstract base for registering two observations (visual, ICP, or both).
+ *
+ * Factory @ref create() builds @ref RegistrationVis, @ref RegistrationIcp, or
+ * the two chained (@ref kTypeVisIcp) from **Reg/Strategy** in a @ref ParametersMap.
+ *
+ * @ref computeTransformation() wraps signatures and delegates to
+ * @ref computeTransformationMod(), which calls @ref computeTransformationImpl()
+ * on the concrete strategy, optionally chains a child registration (e.g. ICP
+ * after visual), applies **Reg/Force3DoF**, and may repeat once with the first
+ * result as guess when **Reg/RepeatOnce** is enabled.
+ *
+ * Requirement queries (@ref isImageRequired(), @ref isScanRequired(), etc.) OR-combine
+ * with the child when the parent returns false. Minimum correspondence thresholds
+ * take the maximum between parent and child.
+ *
+ * @see RegistrationVis
+ * @see RegistrationIcp
+ * @see RegistrationInfo
+ */
 class RTABMAP_CORE_EXPORT Registration
 {
 public:
+	/** @brief Registration strategy selected by **Reg/Strategy**. */
 	enum Type {
-		kTypeUndef = -1,
+		kTypeUndef = -1, /**< Undefined / invalid strategy. */
-		kTypeVis = 0,
+		kTypeVis = 0,    /**< Visual registration only (@ref RegistrationVis). */
-		kTypeIcp = 1,
+		kTypeIcp = 1,    /**< Geometric registration only (@ref RegistrationIcp). */
-		kTypeVisIcp = 2
+		kTypeVisIcp = 2  /**< Visual registration refined by ICP (@ref RegistrationVis chained with @ref RegistrationIcp). */
 	};
+	/** @brief Minimum diagonal value for linear covariance (m²). */
 	static double COVARIANCE_LINEAR_EPSILON;
+	/** @brief Minimum diagonal value for angular covariance (rad²). */
 	static double COVARIANCE_ANGULAR_EPSILON;
 
 public:
+	/** @brief Creates a registration from **Reg/Strategy** in @p parameters. Caller owns the pointer. */
 	static Registration * create(const ParametersMap & parameters);
+	/** @brief Creates a registration of @p type (may be adjusted, e.g. to @ref kTypeVis). Caller owns the pointer. */
 	static Registration * create(Type & type, const ParametersMap & parameters = ParametersMap());
 
 public:
 	virtual ~Registration();
+	/** @brief Parses **Reg/RepeatOnce**, **Reg/Force3DoF** and forwards to the child registration if set. */
 	virtual void parseParameters(const ParametersMap & parameters);
 
+	/** @brief True if images are needed (parent or child). */
 	bool isImageRequired() const;
+	/** @brief True if laser scans are needed (parent or child). */
 	bool isScanRequired() const;
+	/** @brief True if user data is needed (parent or child). */
 	bool isUserDataRequired() const;
 
+	/** @brief True if the strategy can refine an initial guess (parent or child). */
 	bool canUseGuess() const;
 
+	/** @brief Minimum visual inliers required (max of parent and child thresholds). */
 	int getMinVisualCorrespondences() const;
+	/** @brief Minimum geometry inlier ratio required (max of parent and child thresholds). */
 	float getMinGeometryCorrespondencesRatio() const;
 
+	/** @return Value of **Reg/RepeatOnce**. */
 	bool repeatOnce() const {return repeatOnce_;}
+	/** @return Value of **Reg/Force3DoF**. */
 	bool force3DoF() const {return force3DoF_;}
 
-	// take ownership!
+	/** @brief Replaces the current child registration; takes ownership of @p child. */
 	void setChildRegistration(Registration * child);
 
+	/** @brief Registers @p from to @p to using immutable signatures (copied internally). */
 	Transform computeTransformation(
 			const Signature & from,
 			const Signature & to,
 			Transform guess = Transform::getIdentity(),
 			RegistrationInfo * info = 0) const;
+	/** @brief Registers two @ref SensorData observations (wrapped as signatures). */
 	Transform computeTransformation(
 			const SensorData & from,
 			const SensorData & to,
 			Transform guess = Transform::getIdentity(),
 			RegistrationInfo * info = 0) const;
 
+	/** @brief Registers @p from to @p to; signatures may be modified by the implementation. */
 	Transform computeTransformationMod(
 			Signature & from,
 			Signature & to,
@@ -89,11 +127,14 @@ public:
 			RegistrationInfo * info = 0) const;
 
 protected:
-	// take ownership of child
+	/** @brief @p child is owned and deleted in the destructor. */
 	Registration(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
 
-	// It is safe to modify the signatures in the implementation, if so, the
+	/**
-	// child registration will use these modifications.
+	 * @brief Strategy-specific registration.
+	 *
+	 * May modify @p from and @p to; a child registration reuses those changes.
+	 */
 	virtual Transform computeTransformationImpl(
 			Signature & from,
 			Signature & to,

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -39,8 +39,33 @@ namespace rtabmap {
 class RTABMAP_CORE_EXPORT RegistrationIcp : public Registration
 {
 public:
+	enum IcpStrategy {
+		kIcpUndef = -1,        /**< Undefined / invalid type. */
+		kIcpPCL = 0,           /**< Point Cloud Library. */
+		kIcpPointMatcher = 1,  /**< libpointmatcher. */
+		kIcpCCCoreLib = 2,     /**< CCCoreLib (Cloud Compare). */
+		kIcpEnd = 3            /**< Sentinel: always keep last. Used to iterate through strategies. */
+	};
+
+public:
+	/**
+	 * @brief Returns true if @p strategy is built into this rtabmap binary.
+	 *
+	 * @ref kIcpPCL is always available; @ref kIcpPointMatcher requires the
+	 * RTABMAP_POINTMATCHER build flag; @ref kIcpCCCoreLib requires
+	 * RTABMAP_CCCORELIB.
+	 */
+	static bool available(IcpStrategy strategy);
+
+	/**
+	 * @brief Human-readable name for @p strategy ("PCL", "libpointmatcher",
+	 * "CCCoreLib", or "Unknown").
+	 */
+	static const char * strategyName(IcpStrategy strategy);
+
 	// take ownership of child
 	RegistrationIcp(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
+	RegistrationIcp(IcpStrategy strategy, const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
 	virtual ~RegistrationIcp();
 
 	virtual void parseParameters(const ParametersMap & parameters);
@@ -56,7 +81,7 @@ protected:
 	virtual float getMinGeometryCorrespondencesRatioImpl() const {return _correspondenceRatio;}
 
 private:
-	int _strategy;
+	IcpStrategy _strategy;
 	float _maxTranslation;
 	float _maxRotation;
 	float _voxelSize;
@@ -75,6 +100,7 @@ private:
 	float _pointToPlaneRadius;
 	float _pointToPlaneGroundNormalsUp;
 	float _pointToPlaneMinComplexity;
+	bool _pointToPlaneComplexityCentered;
 	int _pointToPlaneLowComplexityStrategy;
 	std::string _libpointmatcherConfig;
 	int _libpointmatcherKnn;

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -31,6 +31,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class RegistrationInfo
+ * @brief Statistics and diagnostics returned by @ref Registration.
+ *
+ * Filled by @ref RegistrationVis and/or @ref RegistrationIcp during
+ * @ref Registration::computeTransformation(). The 6×6 @ref covariance matrix
+ * is clamped to @ref Registration::COVARIANCE_LINEAR_EPSILON and
+ * @ref Registration::COVARIANCE_ANGULAR_EPSILON on the diagonal when empty or
+ * too small.
+ *
+ * Visual fields are set by @ref RegistrationVis; ICP fields by @ref RegistrationIcp.
+ * In a combined Vis+ICP pipeline, both sets may be populated in the same object.
+ */
 class RegistrationInfo
 {
 public:
@@ -41,17 +54,20 @@ public:
 		inliersMeanDistance(0.0f),
 		inliersDistribution(0.0f),
 		matches(0),
+		variance(0.0f),
 		icpInliersRatio(0),
 		icpTranslation(0.0f),
 		icpRotation(0.0f),
 		icpStructuralComplexity(0.0f),
 		icpStructuralDistribution(0.0f),
 		icpCorrespondences(0),
-		icpRMS(0)
+		icpRMS(0),
+		icpIterations(-1)
 
 	{
 	}
 
+	/** @brief Copies scalar metrics and @ref covariance; omits correspondence ID vectors. */
 	RegistrationInfo copyWithoutData() const
 	{
 		RegistrationInfo output;
@@ -64,6 +80,7 @@ public:
 		output.inliersDistribution = inliersDistribution;
 		output.matches = matches;
 		output.matchesPerCam = matchesPerCam;
+		output.variance = variance;
 		output.icpInliersRatio = icpInliersRatio;
 		output.icpTranslation = icpTranslation;
 		output.icpRotation = icpRotation;
@@ -71,33 +88,36 @@ public:
 		output.icpStructuralDistribution = icpStructuralDistribution;
 		output.icpCorrespondences = icpCorrespondences;
         output.icpRMS = icpRMS;
+		output.icpIterations = icpIterations;
 		return output;
 	}
 
-	cv::Mat covariance;
+	cv::Mat covariance;           /**< 6×6 registration uncertainty (CV_64FC1). */
-	std::string rejectedMsg;
+	std::string rejectedMsg;      /**< Reason the registration was rejected, if any. */
-	double totalTime;
+	double totalTime;             /**< Total registration time (seconds). */
 
 	// RegistrationVis
-	int inliers;
+	int inliers;                  /**< Number of visual inliers. */
-	float inliersRatio;
+	float inliersRatio;           /**< Ratio of inliers to matches. */
-	float inliersMeanDistance;
+	float inliersMeanDistance;    /**< Mean reprojection or descriptor distance of inliers. */
-	float inliersDistribution;
+	float inliersDistribution;  /**< Spatial spread of inliers in the image. */
-	std::vector<int> inliersIDs;
+	std::vector<int> inliersIDs; /**< Indices of inlier keypoints. */
-	int matches;
+	int matches;                  /**< Total visual matches before outlier rejection. */
-	std::vector<int> matchesIDs;
+	float variance;               /**< Estimated variance of the visual constraint. */
-	std::vector<int> projectedIDs; // "From" IDs
+	std::vector<int> matchesIDs; /**< Indices of all matches. */
-	std::vector<int> inliersPerCam;
+	std::vector<int> projectedIDs; /**< Source ("from") feature IDs used in projection. */
-	std::vector<int> matchesPerCam;
+	std::vector<int> inliersPerCam; /**< Inlier count per camera (multi-camera). */
+	std::vector<int> matchesPerCam; /**< Match count per camera (multi-camera). */
 
 	// RegistrationIcp
-	float icpInliersRatio;
+	float icpInliersRatio;        /**< Ratio of ICP inlier correspondences. */
-	float icpTranslation;
+	float icpTranslation;         /**< Translation component of the ICP correction (m). */
-	float icpRotation;
+	float icpRotation;            /**< Rotation component of the ICP correction (rad). */
-	float icpStructuralComplexity;
+	float icpStructuralComplexity; /**< Structural complexity of the scan overlap. */
-	float icpStructuralDistribution;
+	float icpStructuralDistribution; /**< Distribution of structural features in the overlap. */
-	int   icpCorrespondences;
+	int   icpCorrespondences;     /**< Number of ICP point correspondences. */
-	float icpRMS;
+	float icpRMS;                 /**< Root-mean-square error of ICP correspondences (m). */
+	int   icpIterations;          /**< Number of ICP iterations actually performed (-1 if unavailable from backend). */
 };
 
 }

corelib/include/rtabmap/core/RegistrationVis.h

@@ -41,23 +41,53 @@ class Feature2D;
 class PyMatcher;
 #endif
 
-// Visual registration
+/**
+ * @class RegistrationVis
+ * @brief Visual registration between two signatures using features and geometry.
+ *
+ * Extracts and matches features (@ref Feature2D), then estimates motion with
+ * **Vis/EstimationType** (3D→3D, 3D→2D PnP, or 2D→2D epipolar). Supports RGB-D
+ * (@ref CameraModel + depth), stereo (@ref StereoCameraModel), optical flow, GMS,
+ * and optional Python matchers when built with Python support.
+ *
+ * Can chain a child registration (e.g. @ref RegistrationIcp in
+ * @ref Registration::kTypeVisIcp). Feature-related Vis/ parameters are mapped
+ * to Kp/ for @ref Feature2D::create().
+ *
+ * @see Registration
+ * @see RegistrationInfo
+ */
 class RTABMAP_CORE_EXPORT RegistrationVis : public Registration
 {
 public:
-	// take ownership of child
+	/** @brief @p child is owned and deleted in the destructor. */
 	RegistrationVis(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
 	virtual ~RegistrationVis();
 
+	/** @brief Parses Vis/ and feature parameters; recreates feature detectors. */
 	virtual void parseParameters(const ParametersMap & parameters);
 
+	/** @return **Vis/InlierDistance** (m). */
 	float getInlierDistance() const {return _inlierDistance;}
+	/** @return **Vis/Iterations** (RANSAC iterations). */
 	int getIterations() const {return _iterations;}
+	/** @return **Vis/MinInliers**. */
 	int getMinInliers() const {return _minInliers;}
+	/** @return **Vis/CorNNType** nearest-neighbor strategy. */
 	int getNNType() const {return _nnType;}
+	/** @return Name of the **Vis/CorNNType** nearest-neighbor strategy in use. */
+	std::string getNNTypeName() const {return getNNTypeName(_nnType);}
+
+	/**
+	 * @brief Name of a Vis/CorNNType value
+	 */
+	static std::string getNNTypeName(int nnType);
+	/** @return **Vis/CorNNDR** ratio test threshold. */
 	float getNNDR() const {return _nndr;}
+	/** @return **Vis/EstimationType** (0: 3D→3D, 1: PnP, 2: epipolar). */
 	int getEstimationType() const {return _estimationType;}
 
+	/** @return Feature detector used for the “from” signature. */
 	const Feature2D * getDetector() const {return _detectorFrom;}
 
 protected:
@@ -91,6 +121,9 @@ private:
 	float _flowEps;
 	int _flowMaxLevel;
 	bool _flowGpu;
+	bool _flowUseMinEigenVals;
+	float _flowMinEigThreshold;
+	float _flowErrorThreshold;
 	float _nndr;
 	int _nnType;
 	bool _gmsWithRotation;

corelib/include/rtabmap/core/Rtabmap.h

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Statistics.h"
 #include "rtabmap/core/Link.h"
 #include "rtabmap/core/ProgressState.h"
+#include "rtabmap/core/Graph.h"
 
 #include <opencv2/core/core.hpp>
 #include <list>
@@ -49,24 +50,175 @@ class Memory;
 class BayesFilter;
 class Signature;
 class Optimizer;
-class PythonInterface;
 
+/**
+ * @class Rtabmap
+ * @brief Top-level RTAB-Map SLAM pipeline (mapping, localization and loop closure).
+ *
+ * Rtabmap orchestrates the full SLAM iteration. Each new sensor observation passed to
+ * @ref process() goes through the steps described below.
+ *
+ *
+ * @par 1. Memory update
+ *
+ * Done via @ref Memory::update(): a new @ref Signature is added to STM, the oldest
+ * STM entry is promoted to WM if STM is full, and rehearsal compares the new
+ * signature to the previous STM signature.
+ *
+ * When the robot barely moved since the previous frame (odometry displacement below
+ * @ref Parameters::kRGBDLinearUpdate() and @ref Parameters::kRGBDAngularUpdate()), the
+ * iteration is flagged as a "small displacement":
+ *   - If a loop closure or localization was already accepted on a recent iteration,
+ *     appearance-based global loop-closure detection and proximity detection by space
+ *     are both skipped to avoid wasting work while the robot is stationary at an
+ *     already-known location (only retrieval runs).
+ *   - Otherwise (no recent loop closure / localization), both still run normally so
+ *     a first-time loop closure can still be detected from a standstill.
+ *
+ * In either case, at the end of the iteration, if no loop closure, proximity
+ * detection or landmark observation latched onto the new node, it is deleted from
+ * @ref Memory so the map does not grow while the robot is idle.
+ *
+ * Rehearsal still runs first, so visually similar consecutive idle frames may also be
+ * merged into the previous STM signature (its weight is incremented and the new
+ * signature is discarded) when the similarity exceeds
+ * @ref Parameters::kMemRehearsalSimilarity().
+ *
+ *
+ * @par 2. Loop-closure hypothesis
+ *
+ * Scored via @ref Memory::computeLikelihood() and the recursive @ref BayesFilter
+ * (prior + observation update).
+ *
+ *
+ * @par 3. Hypothesis selection
+ *
+ * The highest posterior is compared against the loop-closure threshold
+ * (@ref Parameters::kRtabmapLoopThr()); if accepted, the loop-closure link is added
+ * and the pose graph is re-optimized by @ref Optimizer.
+ *
+ * In **RGB-D mode**, two extra checks must pass before the link is committed:
+ *   - a valid geometric transform must be computed between the two candidate nodes by
+ *     the registration pipeline (visual + optional ICP, see
+ *     @ref Memory::computeTransform());
+ *   - the resulting transform must not be rejected by the graph-optimization
+ *     consistency check (see @ref Parameters::kRGBDOptimizeMaxError()).
+ *
+ * The optimization used by the consistency check depends on the operating mode:
+ *   - In **mapping mode**, the local map is re-optimized
+ *     around the current signature including the new link, then
+ *     @ref graph::computeMaxGraphErrors() measures the worst per-link residual / its
+ *     standard deviation. If the ratio exceeds @ref Parameters::kRGBDOptimizeMaxError(),
+ *     the loop closure(s) added this iteration are removed from @ref Memory.
+ *   - In **localization mode**, optimization is run on a sub-graph composed of the
+ *     odometry cache (@ref Parameters::kRGBDMaxOdomCacheSize()), the newly added
+ *     localization link and pose priors fixing the map nodes (with the variance set
+ *     from @ref Parameters::kRGBDLocalizationPriorError()). The same error-ratio check is
+ *     applied; on failure the localization is rejected for this iteration but the
+ *     persisted map and its links are left untouched.
+ *
+ * In both modes, if the same link is rejected twice in a row, a graph repair may
+ * also be attempted (within @ref Parameters::kRGBDOptimizeMaxErrorRepairRadius()) to
+ * drop the offending link instead of the new candidate.
+ *
+ * If either RGB-D check fails, the candidate is discarded and no link is added.
+ *
+ *
+ * @par 4. Retrieval
+ *
+ * Once a loop-closure hypothesis is selected, neighbors of the matched node are
+ * brought back from LTM into WM via @ref Memory::reactivateSignatures(), so the next
+ * iteration can compare against them too. Up to @ref Parameters::kRtabmapMaxRetrieved()
+ * nodes are pulled per iteration; nodes around the current path or local pose may
+ * also be retrieved (capped by @ref Parameters::kRGBDMaxLocalRetrieved()).
+ *
+ * Retrieval (and the related node immunization) is only active when memory management
+ * is enabled, i.e. when @ref Parameters::kRtabmapTimeThr() or
+ * @ref Parameters::kRtabmapMemoryThr() is non-zero.
+ *
+ *
+ * @par 5. Proximity detection (RGB-D mode)
+ *
+ * Visual and scan-based local matches to nearby nodes, used in addition to the
+ * appearance-based loop closure.
+ *
+ * Candidate proximity links go through the same two RGB-D gates as loop closures
+ * above: a valid geometric transform must be computed by the registration pipeline,
+ * and the transform must not be rejected by the graph-optimization consistency check.
+ *
+ *
+ * @par 6. Transfer (WM to LTM)
+ *
+ * At the end of the iteration, if the iteration exceeded the configured time budget
+ * (@ref Parameters::kRtabmapTimeThr()) or WM exceeded its size budget
+ * (@ref Parameters::kRtabmapMemoryThr()), @ref Memory::forget() moves the oldest
+ * low-frequency signatures from WM to LTM (immunized nodes -- retrieved neighbors,
+ * the last localization node, etc. -- are kept in WM).
+ *
+ * Transfer is skipped when both thresholds are 0 (memory management disabled).
+ *
+ *
+ * @par 7. Map / localization output
+ *
+ * Optimized poses, current map correction and statistics are made available to
+ * callers via the getters below.
+ *
+ *
+ * @par Operating modes
+ *
+ * Selected by @ref Parameters::kMemIncrementalMemory() (see @ref Memory::isIncremental()):
+ *   - **Mapping**: STM and WM grow; loop closures update the optimized graph.
+ *   - **Localization**: STM/WM are frozen; the current node is matched against the
+ *     persisted map and only @ref getLastLocalizationPose() is updated.
+ *
+ *
+ * @par Path planning
+ *
+ * Rtabmap also exposes basic graph-based path planning in RGB-D mode
+ * (@ref computePath(), @ref getPath(), @ref getPathStatus()), used by the GUI to
+ * navigate between mapped locations.
+ *
+ * When memory management is enabled (see step 4 Retrieval and step 6 Transfer), the
+ * retrieval step also pulls nodes along the currently planned path back from LTM into
+ * WM (capped by @ref Parameters::kRGBDMaxLocalRetrieved()) so the robot is able to
+ * re-localize against upcoming waypoints as it follows the path, even when those
+ * nodes had been transferred out of WM earlier.
+ *
+ *
+ * @see Memory
+ * @see BayesFilter
+ * @see Optimizer
+ * @see Parameters
+ */
 class RTABMAP_CORE_EXPORT Rtabmap
 {
 public:
-	enum VhStrategy {kVhNone, kVhEpipolar, kVhUndef};
+	/** @brief Loop-closure verification strategy. */
+	enum VhStrategy {
+		kVhNone,     ///< No verification: the highest hypothesis above threshold is accepted.
+		kVhEpipolar, ///< Epipolar geometry verification (mostly historical, RGB-only mode).
+		kVhUndef     ///< Sentinel -- undefined.
+	};
 
 public:
 	Rtabmap();
 	virtual ~Rtabmap();
 
 	/**
-	 * @brief Main loop of rtabmap.
+	 * @brief Main RTAB-Map iteration: ingests one sensor frame and updates the map.
-	 * @param data Sensor data to process.
+	 *
-	 * @param odomPose Odometry pose, should be non-null for RGB-D SLAM mode.
+	 * Adds @p data to @ref Memory, runs the Bayes filter on the current likelihood,
-	 * @param covariance Odometry covariance.
+	 * selects a loop-closure hypothesis if any, performs proximity detection,
-	 * @param externalStats External statistics to be saved in the database for convenience
+	 * re-optimizes the graph as needed, and refreshes @ref getStatistics() and
-	 * @return true if data has been added to map.
+	 * @ref getLastLocalizationPose().
+	 *
+	 * @param data Sensor data for this frame (images, scan, user data, ...).
+	 * @param odomPose Odometry pose; must be non-null in RGB-D SLAM mode.
+	 *        Pass a null @ref Transform to fall back to appearance-only mode.
+	 * @param odomCovariance 6x6 odometry covariance (default: identity).
+	 * @param odomVelocity Optional 6-vector (vx, vy, vz, vroll, vpitch, vyaw).
+	 * @param externalStats Extra named statistics to record in the database for this iteration.
+	 * @return True if @p data was added to the map (i.e. the memory update succeeded).
 	 */
 	bool process(
 			const SensorData & data,
@@ -74,7 +226,13 @@ public:
 			const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1),
 			const std::vector<float> & odomVelocity = std::vector<float>(),
 			const std::map<std::string, float> & externalStats = std::map<std::string, float>());
-	// for convenience
+	/**
+	 * @brief Convenience overload: builds a diagonal covariance from scalar variances.
+	 *
+	 * The 6x6 odometry covariance is constructed as
+	 * @c diag(odomLinearVariance, odomLinearVariance, odomLinearVariance,
+	 * odomAngularVariance, odomAngularVariance, odomAngularVariance).
+	 */
 	bool process(
 			const SensorData & data,
 			Transform odomPose,
@@ -82,113 +240,366 @@ public:
 			float odomAngularVariance,
 			const std::vector<float> & odomVelocity = std::vector<float>(),
 			const std::map<std::string, float> & externalStats = std::map<std::string, float>());
-	// for convenience, loop closure detection only
+	/**
+	 * @brief Appearance-only convenience overload (loop-closure detection without odometry).
+	 *
+	 * Equivalent to processing @p image alone, with no odometry pose. Useful for offline
+	 * loop-closure benchmarking on image sequences.
+	 *
+	 * @param image RGB or grayscale frame.
+	 * @param id Optional frame id (0 = auto-generated).
+	 * @param externalStats Extra named statistics to record in the database for this iteration.
+	 */
 	bool process(
 			const cv::Mat & image,
 			int id=0, const std::map<std::string, float> & externalStats = std::map<std::string, float>());
 
 	/**
-	 * Initialize Rtabmap with parameters and a database
+	 * @brief Initializes Rtabmap with parameters and a database.
+	 *
 	 * @param parameters Parameters overriding default parameters and database parameters
-	 *                   (@see loadDatabaseParameters)
+	 *                   (see @p loadDatabaseParameters).
-	 * @param databasePath The database input/output path. If not set, an
+	 * @param databasePath Database input/output path. If empty, an in-memory database is
-	 *                     empty database is used in RAM. If set and the file doesn't exist,
+	 *                     used. If set and the file does not exist, it is created empty;
-	 *                     it will be created empty. If the database exists, nodes and
+	 *                     if it exists, nodes and the visual word vocabulary are loaded
-	 *                     vocabulary will be loaded in working memory.
+	 *                     into working memory.
-	 * @param loadDatabaseParameters If an existing database is used (@see databasePath),
+	 * @param loadDatabaseParameters If true and an existing database is opened, the
-	 *                               the parameters inside are loaded and set to current
+	 *                               parameters stored inside the database are loaded and
-	 *                               Rtabmap instance.
+	 *                               applied to this Rtabmap instance (then overridden by
+	 *                               @p parameters).
 	 */
 	void init(const ParametersMap & parameters, const std::string & databasePath = "", bool loadDatabaseParameters = false);
 	/**
-	 * Initialize Rtabmap with parameters from a configuration file and a database
+	 * @brief Initializes Rtabmap from a configuration file and a database.
-	 * @param configFile Configuration file (*.ini) overriding default parameters and database parameters
+	 *
-	 *                   (@see loadDatabaseParameters)
+	 * @param configFile Configuration file (*.ini) overriding default parameters and
-	 * @param databasePath The database input/output path. If not set, an
+	 *                   database parameters (see @p loadDatabaseParameters).
-	 *                     empty database is used in RAM. If set and the file doesn't exist,
+	 * @param databasePath Database input/output path; same semantics as the other @ref init().
-	 *                     it will be created empty. If the database exists, nodes and
+	 * @param loadDatabaseParameters If true and an existing database is opened, the
-	 *                     vocabulary will be loaded in working memory.
+	 *                               parameters stored inside the database are loaded and
-	 * @param loadDatabaseParameters If an existing database is used (@see databasePath),
+	 *                               applied first, then overridden by values from @p configFile.
-	 *                               the parameters inside are loaded and set to current
-	 *                               Rtabmap instance.
 	 */
 	void init(const std::string & configFile = "", const std::string & databasePath = "", bool loadDatabaseParameters = false);
 
 	/**
-	 * Close rtabmap. This will delete rtabmap object if set.
+	 * @brief Closes Rtabmap and releases the underlying @ref Memory.
-	 * @param databaseSaved true=database saved, false=database discarded.
+	 *
-	 * @param databasePath output database file name, ignored if
+	 * @param databaseSaved If true, the in-memory state is flushed to the database;
-	 *                     Db/Sqlite3InMemory=false (opened database is
+	 *                      if false, in-memory changes are discarded.
-	 *                     then overwritten).
+	 * @param ouputDatabasePath If non-empty, the database is copied to this path on
+	 *                      close. If a database on disk was initially created/loaded on
+	 *                      a different path, it will be updated with the latest changes
+	 *                      and renamed to the output path.
 	 */
 	void close(bool databaseSaved = true, const std::string & ouputDatabasePath = "");
 
+	/** @return Working directory used for dumps, log files and temporary outputs. */
 	const std::string & getWorkingDir() const {return _wDir;}
+	/** @return True if RGB-D SLAM mode is enabled (@ref Parameters::kRGBDEnabled()). */
 	bool isRGBDMode() const { return _rgbdSlamMode; }
+	/** @return Id of the loop-closure hypothesis accepted at the last @ref process() iteration, or 0 if none. */
 	int getLoopClosureId() const {return _loopClosureHypothesis.first;}
+	/** @return Posterior probability of the accepted loop-closure hypothesis, or 0 if none. */
 	float getLoopClosureValue() const {return _loopClosureHypothesis.second;}
+	/** @return Id of the highest-posterior hypothesis at the last iteration (whether or not it was accepted). */
 	int getHighestHypothesisId() const {return _highestHypothesis.first;}
+	/** @return Posterior of the highest-posterior hypothesis at the last iteration. */
 	float getHighestHypothesisValue() const {return _highestHypothesis.second;}
+	/** @return Id of the last non-intermediate signature added to the map (0 if none). */
 	int getLastLocationId() const;
-	std::list<int> getWM() const; // working memory
+	/** @return Working memory ids ordered as in @ref Memory::getWorkingMem(). */
-	std::set<int> getSTM() const; // short-term memory
+	std::list<int> getWM() const;
-	int getWMSize() const; // working memory size
+	/** @return Short-term memory ids. */
-	int getSTMSize() const; // short-term memory size
+	std::set<int> getSTM() const;
+	/** @return Working memory size (number of WM signatures). */
+	int getWMSize() const;
+	/** @return Short-term memory size (number of STM signatures). */
+	int getSTMSize() const;
+	/** @return Per-signature weights (rehearsal counts) for WM and STM. */
 	std::map<int, int> getWeights() const;
+	/** @return Total number of signatures across WM, STM and LTM. */
 	int getTotalMemSize() const;
+	/** @return Wall-clock duration of the last @ref process() call, in seconds. */
 	double getLastProcessTime() const {return _lastProcessTime;};
+	/** @return True if @p locationId is currently in short-term memory. */
 	bool isInSTM(int locationId) const;
+	/** @return True if signature ids are auto-generated (vs. taken from @ref SensorData::id()). */
 	bool isIDsGenerated() const;
+	/** @return Statistics produced by the last @ref process() iteration. */
 	const Statistics & getStatistics() const;
+	/** @return Optimized poses of the current local map (last graph optimization result). */
 	const std::map<int, Transform> & getLocalOptimizedPoses() const {return _optimizedPoses;}
+	/** @return Constraints (links) of the current local map. */
 	const std::multimap<int, Link> & getLocalConstraints() const {return _constraints;}
+	/**
+	 * @return Optimized pose of @p locationId in the current local map (identity if not present).
+	 */
 	Transform getPose(int locationId) const;
+	/**
+	 * @return Transform mapping odometry frame to the optimized map frame.
+	 *
+	 * This is the correction applied to incoming odometry poses so they align with the
+	 * latest graph optimization output. Updated whenever a loop closure or proximity
+	 * detection re-optimizes the graph.
+	 *
+	 * In ROS terms, this corresponds to the standard @c /map -> @c /odom TF transform
+	 * published by SLAM systems: composing it with the live odometry pose
+	 * (@c /odom -> @c /base_link) yields the robot pose in the map frame.
+	 */
 	Transform getMapCorrection() const {return _mapCorrection;}
+	/** @return Owned @ref Memory (may be null before @ref init()). */
 	const Memory * getMemory() const {return _memory;}
+	/** @return Radius (meters) under which the current path goal is considered reached. */
 	float getGoalReachedRadius() const {return _goalReachedRadius;}
+	/** @return Local radius (meters) used by proximity detection and path planning queries. */
 	float getLocalRadius() const {return _localRadius;}
+	/**
+	 * @return Last localized pose in the map frame.
+	 *
+	 * In **localization mode**, this is the corrected odometry pose of the last
+	 * processed frame. In **mapping mode**, this is the last pose returned by
+	 * @ref getLocalOptimizedPoses().
+	 */
 	const Transform & getLastLocalizationPose() const {return _lastLocalizationPose;}
 
-	float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
+	/**
-	void setTimeThreshold(float maxTimeAllowed); // in ms
+	 * @return Maximum allowed processing time per @ref process() call, in milliseconds.
-	int getMemoryThreshold() const {return _maxMemoryAllowed;} // in nodes
+	 * @see Parameters::kRtabmapTimeThr()
-	void setMemoryThreshold(int maxMemoryAllowed); // in nodes
+	 */
+	float getTimeThreshold() const {return _maxTimeAllowed;}
+	/**
+	 * @brief Sets the per-iteration time budget (ms).
+	 *
+	 * Drives how aggressively WM is transferred to LTM to keep iterations under the
+	 * threshold. 0 disables the time bound.
+	 *
+	 * @note This setting and @ref setMemoryThreshold() are the only two switches that
+	 *       enable RTAB-Map's memory management (WM-to-LTM transfer, retrieval and node
+	 *       immunization). When both are 0, memory management is disabled and all
+	 *       signatures stay in working memory.
+	 *
+	 * @see Parameters::kRtabmapTimeThr()
+	 */
+	void setTimeThreshold(float maxTimeAllowed);
+	/**
+	 * @return Maximum allowed WM size (number of signatures).
+	 * @see Parameters::kRtabmapMemoryThr()
+	 */
+	int getMemoryThreshold() const {return _maxMemoryAllowed;}
+	/**
+	 * @brief Sets the maximum number of signatures kept in WM (0 = unbounded).
+	 *
+	 * @note This setting and @ref setTimeThreshold() are the only two switches that
+	 *       enable RTAB-Map's memory management (WM-to-LTM transfer, retrieval and node
+	 *       immunization). When both are 0, memory management is disabled and all
+	 *       signatures stay in working memory.
+	 *
+	 * @see Parameters::kRtabmapMemoryThr()
+	 */
+	void setMemoryThreshold(int maxMemoryAllowed);
 
+	/**
+	 * @brief Sets the localization prior pose used to seed the next @ref process() call
+	 *        (localization mode only).
+	 *
+	 * Tells RTAB-Map where the robot is currently located in the map frame, so that the
+	 * very next call to @ref process() can align incoming odometry with the persisted
+	 * map without waiting for a loop closure. Typical use cases: restoring localization
+	 * after a session restart, applying an external pose estimate (e.g. from a GPS or
+	 * a known starting point), or recovering from "kidnapped robot" situations.
+	 *
+	 * This call only stages state; it does **not** itself produce a non-identity
+	 * @ref getMapCorrection(). The alignment between the odometry frame and the map
+	 * frame is performed on the next @ref process() call, which consumes
+	 * @p initialPose together with the incoming odometry pose. Two branches are taken
+	 * depending on @ref Parameters::kRGBDOptimizeFromGraphEnd():
+	 * - **false (default)**: @ref getMapCorrection() is set so that the live odometry
+	 *   pose is shifted to land on @p initialPose in the map frame (the optimized map
+	 *   is left untouched).
+	 * - **true**: every optimized node pose is rigidly transformed so that the map
+	 *   itself moves to align with @p initialPose (the map correction stays close to
+	 *   identity).
+	 *
+	 * The transform applied is restricted by SLAM dimensionality: 3-DoF (x, y, yaw)
+	 * for 2D SLAM, 4-DoF (x, y, z, yaw) when gravity is available
+	 * (IMU orientation or @ref Memory::isOdomGravityUsed()) and
+	 * @ref Parameters::kOptimizerGravitySigma() is non-zero, full 6-DoF otherwise.
+	 *
+	 * Side effects on the staged state:
+	 * - @ref getLastLocalizationPose() is replaced by @p initialPose; the localization
+	 *   covariance, the last localization node id and the odometry cache used for
+	 *   loop-closure rejection are all cleared.
+	 * - @ref getMapCorrection() is reset to identity and any backup is cleared.
+	 * - If the current map has not been optimized yet (no entries in
+	 *   @ref getLocalOptimizedPoses()) and a last working signature exists, the map
+	 *   is optimized around that signature so the next @ref process() has something
+	 *   to localize against.
+	 *
+	 * After the next @ref process() consumes the prior, the nearest optimized node to
+	 * @p initialPose is recorded as the last localization node.
+	 *
+	 * @param initialPose Robot pose in the map frame.
+	 *
+	 * @note No-op (with warning) in mapping mode.
+	 *
+	 * @see Parameters::kMemIncrementalMemory()
+	 * @see Parameters::kRGBDOptimizeFromGraphEnd()
+	 */
 	void setInitialPose(const Transform & initialPose);
+	/**
+	 * @brief Starts a new map session (next @ref process() will create a fresh map id).
+	 *
+	 * In **mapping mode**, this increments the map id, clears the local optimized graph
+	 * and resets the Bayes filter.
+	 * In **localization mode**, it resets the map correction, the localization node and
+	 * the odometry cache; if @ref Parameters::kRGBDStartAtOrigin() is enabled, the
+	 * last localization pose is reset to identity.
+	 *
+	 * @return The new map id (mapping mode), or -1 (localization mode).
+	 */
 	int triggerNewMap();
+	/**
+	 * @brief Assigns or clears a label on signature @p id.
+	 * @return True if the label was applied.
+	 */
 	bool labelLocation(int id, const std::string & label);
 	/**
-	 * Set user data. Detect automatically if raw or compressed. If raw, the data is
+	 * @brief Attaches user data to signature @p id, compressing it on the fly if needed.
-	 * compressed too. A matrix of type CV_8UC1 with 1 row is considered as compressed.
+	 *
-	 * If you have one dimension unsigned 8 bits raw data, make sure to transpose it
+	 * The format is detected automatically: a single-row @c CV_8UC1 matrix is treated
-	 * (to have multiple rows instead of multiple columns) in order to be detected as
+	 * as already-compressed data and stored as-is; anything else is considered raw and
-	 * not compressed.
+	 * compressed before being stored.
+	 *
+	 * @note If you pass one-dimensional unsigned 8-bit raw data, transpose it so it has
+	 *       multiple rows (not multiple columns), otherwise it will be misdetected as
+	 *       already compressed.
+	 *
+	 * @param id Target signature id (must be in WM/STM or LTM).
+	 * @param data Raw or pre-compressed user data.
+	 * @return True if the data was attached.
 	 */
 	bool setUserData(int id, const cv::Mat & data);
+	/**
+	 * @brief Writes a Graphviz DOT file of the pose graph.
+	 * @param path Output file path.
+	 * @param id If non-zero, root the graph at @p id; otherwise use the last signature.
+	 * @param margin Maximum graph depth around @p id to include.
+	 */
 	void generateDOTGraph(const std::string & path, int id=0, int margin=5);
+	/**
+	 * @brief Exports the current pose graph to a text file.
+	 *
+	 * Forwards to @ref graph::exportPoses() after collecting either the optimized or
+	 * the raw odometry poses (and the matching constraints when needed).
+	 *
+	 * @param path Output file path.
+	 * @param optimized If true, export optimized poses; otherwise raw odometry poses.
+	 * @param global If true, include nodes from LTM as well; otherwise only WM/STM.
+	 * @param format Output format code; see @ref graph::exportPoses() for the full
+	 *        list of supported values (raw, RGBD-SLAM/TUM, KITTI, TORO, g2o, ...).
+	 *
+	 * @see graph::exportPoses()
+	 */
 	void exportPoses(
 			const std::string & path,
 			bool optimized,
 			bool global,
-			int format // 0=raw, 1=rgbd-slam format, 2=KITTI format, 3=TORO, 4=g2o
+			int format
 	);
+	/**
+	 * @brief Clears all in-memory state and resets the database.
+	 *
+	 * In incremental mode, also clears the persisted map. In read-only memory mode,
+	 * resets the in-memory state but leaves the database untouched.
+	 */
 	void resetMemory();
+	/** @brief Dumps the Bayes-filter prediction matrix to a file in the working directory. */
 	void dumpPrediction() const;
+	/** @brief Dumps the @ref Memory state (signatures, words, dictionary) to the working directory. */
 	void dumpData() const;
+	/**
+	 * @brief Re-parses parameters and propagates them to owned sub-objects
+	 *        (@ref Memory, @ref BayesFilter, @ref Optimizer, ...).
+	 */
 	void parseParameters(const ParametersMap & parameters);
+	/** @return Current effective parameter map. */
 	const ParametersMap & getParameters() const {return _parameters;}
+	/**
+	 * @brief Sets the working directory used for dumps, logs and temporary files.
+	 *
+	 * Can also be configured through @ref Parameters::kRtabmapWorkingDirectory() in the
+	 * parameter map passed to @ref init() or @ref parseParameters().
+	 *
+	 * @see Parameters::kRtabmapWorkingDirectory()
+	 */
 	void setWorkingDirectory(std::string path);
+	/**
+	 * @brief Removes the loop-closure link added at the last @ref process() iteration.
+	 *
+	 * Looks at the last non-intermediate signature in STM and erases any
+	 * @ref Link::kGlobalClosure, @ref Link::kLocalSpaceClosure, @ref Link::kLocalTimeClosure
+	 * or @ref Link::kUserClosure attached to it. The current optimized map is updated
+	 * accordingly.
+	 */
 	void rejectLastLoopClosure();
+	/**
+	 * @brief Deletes the most recent (non-intermediate) location from the map.
+	 *
+	 * Used by tools to undo the very last @ref process() iteration. In mapping mode,
+	 * the optimized graph is recomputed without the deleted node.
+	 *
+	 * @note Locations whose neighbors include intermediate nodes are not supported.
+	 */
 	void deleteLastLocation();
+	/**
+	 * @brief Replaces the current optimized poses and constraints with externally
+	 *        provided ones.
+	 *
+	 * Useful when graph optimization is performed outside of Rtabmap.
+	 *
+	 * @warning No consistency check is performed against the current @ref Memory state:
+	 *          @p poses and @p constraints overwrite the internal containers verbatim.
+	 *          The caller is responsible for ensuring that every id in @p poses (and
+	 *          every endpoint of every link in @p constraints) belongs to a signature
+	 *          currently in STM or WM (see @ref Memory::isInSTM() / @ref Memory::isInWM()).
+	 *          Passing poses for ids that are no longer loaded will leave dangling
+	 *          entries that may confuse subsequent @ref process() calls.
+	 */
 	void setOptimizedPoses(const std::map<int, Transform> & poses, const std::multimap<int, Link> & constraints);
+	/**
+	 * @brief Returns a copy of signature @p id with optional payloads attached.
+	 *
+	 * Loads from WM/STM if present, otherwise from LTM. Selectively populates the
+	 * returned @ref Signature with images, scan, user data, occupancy grid, visual
+	 * words and global descriptors.
+	 */
 	Signature getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid, bool withWords, bool withGlobalDescriptors) const;
-	// Use getGraph() instead with withImages=true, withScan=true, withUserData=true and withGrid=true.
+	/**
+	 * @brief Deprecated: use @ref getGraph() instead with @c withImages=true,
+	 *        @c withScan=true, @c withUserData=true and @c withGrid=true.
+	 */
 	RTABMAP_DEPRECATED
 		void get3DMap(std::map<int, Signature> & signatures,
 				std::map<int, Transform> & poses,
 				std::multimap<int, Link> & constraints,
 				bool optimized,
 				bool global) const;
+	/**
+	 * @brief Extracts a full snapshot of the current pose graph.
+	 *
+	 * @param poses Output: pose for every selected node.
+	 * @param constraints Output: links between selected nodes.
+	 * @param optimized If true, return optimized poses; otherwise raw odometry poses.
+	 * @param global If true, include nodes from LTM as well; otherwise only WM/STM.
+	 * @param signatures Optional output: a copy of each node's @ref Signature (with the
+	 *        payloads requested by the @p with* flags).
+	 * @param withImages Attach compressed RGB/depth images to @p signatures.
+	 * @param withScan Attach laser scan blob.
+	 * @param withUserData Attach user data blob.
+	 * @param withGrid Attach occupancy grid cells.
+	 * @param withWords Attach visual words (id, keypoints, 3D points, descriptors).
+	 * @param withGlobalDescriptors Attach global descriptors.
+	 */
 	void getGraph(std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
 			bool optimized,
@@ -200,8 +611,55 @@ public:
 			bool withGrid = false,
 			bool withWords = true,
 			bool withGlobalDescriptors = true) const;
-	std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius, int k=0, std::map<int, float> * distsSqr=0); // If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks.
+	/**
-	std::map<int, Transform> getNodesInRadius(int nodeId, float radius, int k=0, std::map<int, float> * distsSqr=0); // If nodeId==0, return poses around latest node. If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
+	 * @brief Returns optimized poses within a metric radius of @p pose.
+	 *
+	 * @param pose Query pose in the map frame.
+	 * @param radius Search radius in meters (0 falls back to @ref Parameters::kRGBDLocalRadius()).
+	 * @param k If non-zero, also cap the result to the @p k nearest neighbors.
+	 * @param distsSqr Optional output: per-id squared distance to @p pose.
+	 * @return Nodes (and possibly landmarks) within the radius, mapped to their pose.
+	 *         Landmarks have a negative id.
+	 */
+	std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius, int k=0, std::map<int, float> * distsSqr=0);
+	/**
+	 * @brief Returns optimized poses within a metric radius of node @p nodeId.
+	 *
+	 * @param nodeId Query node id. Pass 0 to query around the latest node. A negative
+	 *        id requests neighbors of the corresponding landmark.
+	 * @param radius Search radius in meters (0 falls back to @ref Parameters::kRGBDLocalRadius()).
+	 * @param k If non-zero, cap the result to the @p k nearest neighbors.
+	 * @param distsSqr Optional output: per-id squared distance to @p nodeId.
+	 * @return Nodes (and possibly landmarks) within the radius.
+	 */
+	std::map<int, Transform> getNodesInRadius(int nodeId, float radius, int k=0, std::map<int, float> * distsSqr=0);
+	/**
+	 * @brief Post-processing: searches for additional loop closures over the existing graph.
+	 *
+	 * Clusters nearby optimized poses and runs registration between candidates that are
+	 * not yet linked. New links are added to @ref Memory and the graph is re-optimized.
+	 *
+	 * @note The registration approach used here is the one configured in @ref Memory via
+	 *       @ref Parameters::kRegStrategy() (0=Vis, 1=Icp, 2=VisIcp), so the quality and
+	 *       sensor requirements of this pass mirror the live loop-closure pipeline.
+	 *
+	 * @note Candidate cluster pairs whose ids differ by less than
+	 *       @ref Parameters::kMemSTMSize(), or that are already reachable from each
+	 *       other within that many graph hops, are filtered out. This prevents trivial
+	 *       "loop closures" between temporally or topologically adjacent nodes.
+	 *
+	 * @param clusterRadiusMax Maximum metric distance (m) between two candidate nodes.
+	 * @param clusterAngle Maximum angular distance (rad) between two candidate nodes.
+	 * @param iterations Number of refinement passes.
+	 * @param intraSession Include loop closures within the same map session.
+	 * @param interSession Include loop closures between different map sessions.
+	 * @param state Optional progress sink; cancellation requests are honored.
+	 * @param clusterRadiusMin Minimum metric distance (m); pairs closer than this are
+	 *        considered already linked through neighbor links.
+	 * @param toFromMapId If >=0, restrict candidate pairs to nodes belonging to this map id.
+	 * @return Number of loop closures added, or -1 on error
+	 *         (e.g. not in RGB-D mode, no optimizer iterations).
+	 */
 	int detectMoreLoopClosures(
 			float clusterRadiusMax = 0.5f,
 			float clusterAngle = M_PI/6.0f,
@@ -209,12 +667,33 @@ public:
 			bool intraSession = true,
 			bool interSession = true,
 			const ProgressState * state = 0,
-			float clusterRadiusMin = 0.0f);
+			float clusterRadiusMin = 0.0f,
+			int toFromMapId = -1);
+	/**
+	 * @brief Runs a global bundle adjustment over the optimized graph.
+	 *
+	 * @param optimizerType Backend optimizer (e.g. 1=g2o); availability depends on
+	 *        what RTAB-Map was built with.
+	 * @param rematchFeatures If true, re-match visual features between connected nodes
+	 *        before BA (otherwise reuse existing word-id correspondences).
+	 * @param iterations Solver iterations (0 falls back to @ref Parameters::kOptimizerIterations()).
+	 * @param pixelVariance Pixel reprojection variance used by the cost (0 falls back
+	 *        to @ref Parameters::kOptimizerPixelVariance()).
+	 * @return True if BA was run and improved poses were stored.
+	 */
 	bool globalBundleAdjustment(
 			int optimizerType = 1 /*g2o*/,
 			bool rematchFeatures = true,
 			int iterations = 0,
 			float pixelVariance = 0.0f);
+	/**
+	 * @brief Filters spurious obstacles from every node's local grid using a reference 2D map.
+	 *
+	 * Thin wrapper around @ref Memory::cleanupLocalGrids(); see that method for the
+	 * exact filtering rule and the meaning of @p cropRadius and @p filterScans.
+	 *
+	 * @return Number of (node, grid or scan) modifications, or -1 on error.
+	 */
 	int cleanupLocalGrids(
 			const std::map<int, Transform> & mapPoses,
 			const cv::Mat & map,
@@ -223,28 +702,212 @@ public:
 			float cellSize,
 			int cropRadius = 1,
 			bool filterScans = false);
+	/**
+	 * @brief Re-runs registration on every link of the current graph and updates the
+	 *        ones that converge.
+	 *
+	 * Useful after parameter changes to refresh stored transforms.
+	 *
+	 * @note The registration approach is the one configured in @ref Memory via
+	 *       @ref Parameters::kRegStrategy() (0=Vis, 1=Icp, 2=VisIcp). For each link,
+	 *       the link's existing relative transform (the constraint produced by the
+	 *       current optimized local graph) is passed as the initial guess to
+	 *       @ref Memory::computeTransform(), so links already close to convergence
+	 *       are refined locally rather than re-estimated from scratch.
+	 *
+	 * @return Number of links refined, or -1 if not in RGB-D mode.
+	 */
 	int refineLinks();
+	/**
+	 * @brief Adds an external link to the map.
+	 *
+	 * The link's "from" and "to" endpoints must exist in memory (incremental mode) or
+	 * in the optimized poses (localization mode). RGB-D mode only.
+	 *
+	 * @return True if the link was added.
+	 */
 	bool addLink(const Link & link);
+	/**
+	 * @brief Converts an odometry covariance into an information matrix, clipping by
+	 *        @ref Memory::getOdomMaxInf() when @ref Parameters::kRGBDLoopCovLimited()
+	 *        is enabled.
+	 */
 	cv::Mat getInformation(const cv::Mat & covariance) const;
+	/**
+	 * @brief Marks node ids whose data should be re-emitted on the next @ref process().
+	 *
+	 * The requested signatures are attached to the @ref Statistics object produced by
+	 * the next @ref process() call (via the same mechanism as the regular "last signature
+	 * data"), so consumers reading @ref getStatistics() pick them up alongside the
+	 * normal output. Up to @ref Parameters::kRtabmapMaxRepublished() ids are emitted
+	 * per iteration; any leftover ids stay queued for subsequent iterations until they
+	 * are republished or fall out of the current graph.
+	 *
+	 * Pass an empty vector to clear the request set. Requires
+	 * @ref Parameters::kRtabmapMaxRepublished() > 0 and
+	 * @ref Parameters::kRtabmapPublishLastSignature() = true.
+	 */
 	void addNodesToRepublish(const std::vector<int> & ids);
+	/**
+	 * @brief Loads the visual word dictionary as ids only, without descriptors.
+	 *
+	 * With a dummy dictionary, @ref init() populates @ref VWDictionary with placeholder
+	 * @ref VisualWord objects carrying an empty descriptor, and the dictionary update
+	 * (FLANN index construction) is skipped. This makes opening a large database much
+	 * faster and lighter in RAM when the word descriptors are not needed, e.g. to inspect
+	 * or post-process an existing map rather than to localize in it.
+	 *
+	 * The dummy dictionary is silently disabled if the database has no words, or if the
+	 * dictionary has to be rebuilt from the nodes because it was not saved properly.
+	 *
+	 * @param enabled True to load ids only, false to load the full dictionary (default).
+	 *
+	 * @note Must be called before @ref init(); an error is logged and the call ignored
+	 *       once the memory exists.
+	 * @warning Incompatible with mapping: adding new nodes asserts in
+	 *          @c Memory::createSignature(). Loop closure detection also cannot match
+	 *          new observations against a descriptor-less dictionary.
+	 *
+	 * @see Memory::setDummyDictionary()
+	 */
+	void setDummyDictionary(bool enabled = true);
 
-	int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
+	/** @return Current path status: -1 = failed, 0 = idle / executing, 1 = success. */
-	void clearPath(int status); // -1=failed 0=idle/executing 1=success
+	int getPathStatus() const {return _pathStatus;}
+	/**
+	 * @brief Clears the current path and sets its terminal status.
+	 * @param status -1 = failed, 0 = idle / executing, 1 = success.
+	 */
+	void clearPath(int status);
+	/**
+	 * @brief Plans a path from the current location to node @p targetNode.
+	 *
+	 * RGB-D mode only (requires @ref Parameters::kRGBDEnabled() = true).
+	 *
+	 * @param targetNode Destination node id (positive) or landmark id (negative).
+	 * @param global If true, also search nodes in LTM; otherwise only the current
+	 *        optimized map.
+	 * @return True if a path was computed; the result is available via @ref getPath().
+	 *
+	 * @see Parameters::kRGBDEnabled()
+	 */
 	bool computePath(int targetNode, bool global);
-	bool computePath(const Transform & targetPose, float tolerance = -1.0f); // only in current optimized map, tolerance (m) < 0 means RGBD/LocalRadius, 0 means infinite
+	/**
+	 * @brief Plans a path in the current optimized map toward a metric goal pose.
+	 *
+	 * @param targetPose Goal pose in the map frame.
+	 * @param tolerance Goal-acceptance tolerance (meters). A negative value falls back
+	 *        to @ref Parameters::kRGBDLocalRadius(); 0 means infinite tolerance.
+	 * @return True if a path was computed.
+	 */
+	bool computePath(const Transform & targetPose, float tolerance = -1.0f);
+	/** @return The currently planned path as a sequence of (node id, pose) waypoints. */
 	const std::vector<std::pair<int, Transform> > & getPath() const {return _path;}
+	/** @return Upcoming waypoints (from the current path index onward). */
 	std::vector<std::pair<int, Transform> > getPathNextPoses() const;
+	/** @return Upcoming node ids (from the current path index onward). */
 	std::vector<int> getPathNextNodes() const;
+	/** @return Id of the current intermediate path goal (the node currently being chased). */
 	int getPathCurrentGoalId() const;
+	/** @return Index of the current waypoint in @ref getPath(). */
 	unsigned int getPathCurrentIndex() const {return _pathCurrentIndex;}
+	/** @return Index of the current intermediate goal in @ref getPath(). */
 	unsigned int getPathCurrentGoalIndex() const {return _pathGoalIndex;}
+	/** @return Transform from the final waypoint pose to the requested goal pose. */
 	const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
 
+	/**
+	 * @brief Returns optimized poses of WM nodes located in front of @p fromId.
+	 *
+	 * Candidates are first gathered around @p fromId, then STM nodes are excluded, the
+	 * survivors are cropped to a forward-facing box of width @p radius (1 m behind,
+	 * @p radius ahead, +/-@p radius laterally), and a KdTree radius search keeps the
+	 * @p maxNearestNeighbors closest poses in that box.
+	 *
+	 * @note Mapping vs. localization mode differs only in how the initial candidate
+	 *       set is built:
+	 *       - In **mapping mode** (incremental), candidates are produced by a
+	 *         graph-radius walk from @p fromId: nodes reachable within @p maxDiffID
+	 *         graph hops AND within @p radius meters in the optimized poses.
+	 *       - In **localization mode**, the graph-hop restriction is ignored: every
+	 *         optimized pose within @p radius meters of @p fromId is considered.
+	 *       The forward-box crop and KdTree radius search that follow are identical
+	 *       in both modes.
+	 *
+	 * @param fromId Reference node (must be in @ref Memory and @ref getLocalOptimizedPoses()).
+	 * @param maxNearestNeighbors Cap on the number of nodes returned.
+	 * @param radius Maximum metric distance from @p fromId (meters).
+	 * @param maxDiffID Maximum graph depth from @p fromId in mapping mode (0 = unlimited).
+	 *        Ignored in localization mode.
+	 */
 	std::map<int, Transform> getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const;
+	/**
+	 * @brief Segments a set of optimized poses into paths connected by neighbor links.
+	 *
+	 * Designed to be called on the result of a radius search around @p target (a set
+	 * of @p poses already constrained to be metrically close to the goal). Within that
+	 * radius, the method partitions the @p poses into one or more "paths" where each
+	 * path is a connected component reachable from its starting node using **only
+	 * neighbor (sequential) links** -- loop-closure links, landmark links and
+	 * intermediate nodes are not used to traverse between members. Paths are produced
+	 * one at a time, each starting from the still-unclaimed pose nearest to @p target;
+	 * a candidate is added to the current path only if it has at least one neighbor
+	 * link to a node already in the path.
+	 *
+	 * Used internally by proximity detection by space (see
+	 * @ref Parameters::kRGBDProximityBySpace()) in two independent stages, each
+	 * iterating over the segmented paths:
+	 * - **One-to-one** (visual registration): runs registration between the current
+	 *   node and at most one node per neighbor-connected path, avoiding redundant
+	 *   attempts against nearby members of the same local trajectory.
+	 * - **One-to-many** (scan matching, enabled when
+	 *   @ref Parameters::kRGBDProximityPathMaxNeighbors() > 0): on each path,
+	 *   neighboring nodes are assembled around the nearest pose on the path (up to
+	 *   the configured count, walked forward and backward) and their laser scans are
+	 *   merged for an ICP registration against the current scan. The
+	 *   neighbor-link-only structure of each path is what makes this assembly
+	 *   geometrically consistent.
+	 *
+	 * @param poses Candidate nodes with their optimized poses (typically pre-filtered
+	 *        to a radius around @p target).
+	 * @param target Reference pose used to order paths: each path's starting node is
+	 *        the still-unclaimed pose closest to @p target.
+	 * @param maxGraphDepth Maximum graph depth traversed from the starting node when
+	 *        gathering candidates for a path (0 = unlimited).
+	 * @return Map from the starting node id of each path to its (node id -> pose) chain.
+	 */
 	std::map<int, std::map<int, Transform> > getPaths(const std::map<int, Transform> & poses, const Transform & target, int maxGraphDepth = 0) const;
+	/**
+	 * @brief Applies the standard RTAB-Map likelihood adjustment.
+	 *
+	 * Normalizes raw likelihoods using mean and standard deviation across non-null
+	 * values. Real-place entries with @c value <= @c mean + @c stdDev are clamped to
+	 * @c 1.0; only entries above that threshold are scaled. The virtual place (the
+	 * first key in @p likelihood, representing the "new place" hypothesis) is then
+	 * set so that its likelihood reflects how peaked the real distribution is.
+	 *
+	 * The exact formulas are selected by @ref Parameters::kRtabmapVirtualPlaceLikelihoodRatio()
+	 * (default 0, Angeli PhD formulation):
+	 *
+	 * - **Ratio = 0** (mean / std-dev formulation):
+	 *   - Real place above threshold: @c (value - (stdDev - epsilon)) / mean
+	 *   - Virtual place: @c mean / stdDev + 1 (when @c stdDev is non-trivial and a
+	 *     maximum exists; otherwise 2).
+	 *   The virtual place "wins" when the real-place distribution is flat
+	 *   (small @c stdDev relative to @c mean).
+	 *
+	 * - **Ratio != 0** (z-score formulation):
+	 *   - Real place above threshold: @c (value - mean) / stdDev (i.e. the z-score).
+	 *   - Virtual place: @c stdDev / (max - mean) + 1 (when @c max > @c mean;
+	 *     otherwise 2). The virtual place "wins" when no real candidate stands out
+	 *     far above the mean.
+	 *
+	 * In both formulations a low virtual-place likelihood favors a real-place loop
+	 * closure on the next Bayes update; a high one favors the "new place" hypothesis.
+	 *
+	 * @see Parameters::kRtabmapVirtualPlaceLikelihoodRatio()
+	 */
 	void adjustLikelihood(std::map<int, float> & likelihood) const;
-	std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
-											const std::map<int, float> & likelihood) const;
 
 private:
 	void optimizeCurrentMap(int id,
@@ -263,6 +926,13 @@ private:
 			std::multimap<int, Link> * constraints = 0,
 			double * error = 0,
 			int * iterationsDone = 0) const;
+	std::list<std::pair<int, int> > repairGraph(
+		graph::MaxGraphErrors & maxGraphErrors,
+		std::map<int, Transform> & poses,
+		std::multimap<int, Link> & constraints,
+		double & optimizationError,
+		int & optimizationIterations,
+		cv::Mat & optimizationCovariance);
 	void updateGoalIndex();
 	bool computePath(int targetNode, std::map<int, Transform> nodes, const std::multimap<int, rtabmap::Link> & constraints);
 
@@ -280,8 +950,8 @@ private:
 	bool _publishRAMUsage;
 	bool _computeRMSE;
 	bool _saveWMState;
-	float _maxTimeAllowed; // in ms
+	float _maxTimeAllowed;          ///< Per-iteration time budget (ms).
-	unsigned int _maxMemoryAllowed; // signatures count in WM
+	unsigned int _maxMemoryAllowed; ///< Maximum number of signatures kept in WM.
 	float _loopThr;
 	float _loopRatio;
 	float _aggressiveLoopThr;
@@ -319,9 +989,10 @@ private:
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
 	float _optimizationMaxError;
+	float _optimizationMaxErrorRepairRadius;
 	bool _startNewMapOnLoopClosure;
 	bool _startNewMapOnGoodSignature;
-	float _goalReachedRadius; // meters
+	float _goalReachedRadius;       ///< Path-goal acceptance radius (meters).
 	bool _goalsSavedInUserData;
 	int _pathStuckIterations;
 	float _pathLinearVelocity;
@@ -367,17 +1038,18 @@ private:
 	std::map<int, Transform> _optimizedPoses;
 	std::multimap<int, Link> _constraints;
 	Transform _mapCorrection;
-	Transform _mapCorrectionBackup; // used in localization mode when odom is lost
+	Transform _mapCorrectionBackup;     ///< Used in localization mode when odometry is lost.
-	Transform _lastLocalizationPose; // Corrected odometry pose. In mapping mode, this corresponds to last pose return by getLocalOptimizedPoses().
+	Transform _lastLocalizationPose;    ///< Corrected odometry pose; in mapping mode, last pose of getLocalOptimizedPoses().
-	int _lastLocalizationNodeId; // for localization mode
+	int _lastLocalizationNodeId;        ///< Last localization node id (localization mode).
 	cv::Mat _localizationCovariance;
 	std::map<int, std::pair<cv::Point3d, Transform> > _gpsGeocentricCache;
 	bool _currentSessionHasGPS;
 	LaserScan _globalScanMap;
 	std::map<int, Transform> _globalScanMapPoses;
-	std::map<int, Transform> _odomCachePoses;       // used in localization mode to reject loop closures
+	std::map<int, Transform> _odomCachePoses;       ///< Odometry cache used to reject loop closures (localization mode).
-	std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
+	std::multimap<int, Link> _odomCacheConstraints; ///< Odometry cache constraints (localization mode).
 	std::map<int, Transform> _markerPriors;
+	std::pair<int, int> _lastRejectedLoopClosureIds;
 
 	std::set<int> _nodesToRepublish;
 
@@ -391,10 +1063,7 @@ private:
 	int _pathStuckCount;
 	float _pathStuckDistance;
 
-#ifdef RTABMAP_PYTHON
+	bool _dummyDictionary;
-	PythonInterface * _python;
-#endif
-
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -54,30 +54,41 @@ private:
 	Statistics _stats;
 };
 
+/**
+ * @class RtabmapEventCmd
+ * @brief Command sent to @ref RtabmapThread, with its parameters.
+ *
+ * Posting one to @ref UEventsManager is how the map is controlled from another
+ * thread: initializing or closing it, resetting the memory, requesting the graph,
+ * setting a goal, labelling a node... The arguments listed for each @ref Cmd are
+ * passed as @p value1 / @p value2 to the constructor, and parameter maps through
+ * @p parameters.
+ */
 class RtabmapEventCmd : public UEvent
 {
 public:
 	enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Cmd enum ?!
+	/** @brief What the receiving @ref RtabmapThread should do. */
 	enum Cmd {
-			kCmdUndef,
+			kCmdUndef,            /**< No command. */
-			kCmdInit,             // params: [string] database path + ParametersMap
+			kCmdInit,             /**< Initialize the map. Params: [string] database path + ParametersMap. */
-			kCmdResetMemory,
+			kCmdResetMemory,      /**< Clear the map and start over from an empty memory. */
-			kCmdClose,            // params: [bool] database saved (default true), [string] output database path (empty=use same database to save, only work when Db/Sqlite3InMemory=true)
+			kCmdClose,            /**< Close the map. Params: [bool] database saved (default true), [string] output database path (empty=use same database to save, only work when Db/Sqlite3InMemory=true). */
-			kCmdUpdateParams,     // params: ParametersMap
+			kCmdUpdateParams,     /**< Apply new parameters. Params: ParametersMap. */
-			kCmdDumpMemory,
+			kCmdDumpMemory,       /**< Dump the memory content to text files in the working directory. */
-			kCmdDumpPrediction,
+			kCmdDumpPrediction,   /**< Dump the Bayes filter prediction matrix to a text file. */
-			kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin
+			kCmdGenerateDOTGraph, /**< Write the graph as a DOT file. Params: [bool] global, [string] path, if global=false: [int] id, [int] margin. */
-			kCmdExportPoses,      // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO, 4=g2o)
+			kCmdExportPoses,      /**< Export the poses to a file. Params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO, 4=g2o). */
-			kCmdCleanDataBuffer,
+			kCmdCleanDataBuffer,  /**< Drop the frames waiting in the thread's buffer. */
-			kCmdPublish3DMap,     // params: [bool] global, [bool] optimized, [bool] graphOnly
+			kCmdPublish3DMap,     /**< Ask for the map, answered with a @ref RtabmapEvent3DMap. Params: [bool] global, [bool] optimized, [bool] graphOnly. */
-			kCmdRepublishData,    // params: [vector<int>] ids
+			kCmdRepublishData,    /**< Republish the data of some nodes. Params: [vector&lt;int&gt;] ids. */
-			kCmdTriggerNewMap,
+			kCmdTriggerNewMap,    /**< Start a new map session (see @ref Rtabmap::triggerNewMap()). */
-			kCmdPause,
+			kCmdPause,            /**< Stop consuming incoming data. */
-			kCmdResume,
+			kCmdResume,           /**< Resume after a @ref kCmdPause. */
-			kCmdGoal,             // params: [string] label or [int] location ID
+			kCmdGoal,             /**< Plan a path to a node. Params: [string] label or [int] location ID. */
-			kCmdCancelGoal,
+			kCmdCancelGoal,       /**< Abandon the current goal. */
-			kCmdLabel,            // params: [string] label, [int] location ID
+			kCmdLabel,            /**< Label a node. Params: [string] label, [int] location ID. */
-			kCmdRemoveLabel       // params: [string] label
+			kCmdRemoveLabel       /**< Remove a label. Params: [string] label. */
 	};
 public:
 	RtabmapEventCmd(Cmd cmd, const ParametersMap & parameters = ParametersMap()) :

corelib/include/rtabmap/core/RtabmapThread.h

@@ -48,40 +48,138 @@ namespace rtabmap {
 
 class Rtabmap;
 
+/**
+ * @class RtabmapThread
+ * @brief Runs a @ref Rtabmap instance in its own thread, driven by events.
+ *
+ * The thread owns the @ref Rtabmap object and calls @ref Rtabmap::process() on the
+ * data it receives, so that mapping never blocks the sensor or odometry threads.
+ * It is the last stage of the event-based pipeline:
+ * @ref SensorCaptureThread → @ref OdometryThread → RtabmapThread.
+ *
+ * **Input events** (handled in @ref handleEvent(), i.e. in the caller's thread):
+ * - @ref OdometryEvent and @ref SensorEvent -- the frame and its odometry pose are
+ *   buffered for the main loop. A null pose means odometry is lost and the frame
+ *   is dropped (unless the memory is in localization mode).
+ * - @ref UserDataEvent -- user data attached to the next processed frame.
+ * - @ref RtabmapEventCmd -- a command (see @ref RtabmapEventCmd::Cmd), such as
+ *   initializing or closing the map, triggering a new map, pausing, setting a
+ *   goal, labelling a node or requesting the map.
+ * - @ref ParamEvent -- applies new parameters, equivalent to a
+ *   @ref RtabmapEventCmd::kCmdUpdateParams command.
+ *
+ * **Output events**, posted from the thread:
+ * - @ref RtabmapEvent with the @ref Statistics of an iteration that added a node.
+ * - @ref RtabmapEvent3DMap in answer to a map request.
+ * - @ref RtabmapGlobalPathEvent and @ref RtabmapGoalStatusEvent for path planning.
+ * - @ref RtabmapLabelErrorEvent when a label could not be set.
+ *
+ * **Input regulation**, on top of what @ref Rtabmap does:
+ * - Frames are queued in a buffer of @ref Parameters::kRtabmapImageBufferSize()
+ *   elements; when it is full the oldest frame is dropped, so a slow map update
+ *   never delays the live pipeline.
+ * - Frames arriving faster than @ref Parameters::kRtabmapDetectionRate() are
+ *   skipped, which is the rate limiting that @ref Rtabmap::process() itself does
+ *   not do. With @ref Parameters::kRtabmapCreateIntermediateNodes() enabled they
+ *   are kept instead, and added as intermediate nodes (odometry-only, not used
+ *   for loop closure detection).
+ * - An identity odometry pose, or a covariance ≥ 9999, is read as an odometry
+ *   reset and triggers a new map id on the next processed frame. The largest
+ *   covariance seen between two processed frames is the one passed on, so the
+ *   link weight does not depend on the odometry frame rate.
+ *
+ * The object registers itself as a @ref UEventsHandler, so posting an event to
+ * @ref UEventsManager is enough to feed it:
+ * @code
+ * rtabmap::RtabmapThread rtabmapThread(new rtabmap::Rtabmap()); // takes ownership
+ * rtabmapThread.start();
+ * UEventsManager::addHandler(&rtabmapThread);
+ * UEventsManager::createPipe(&odomThread, &rtabmapThread, "OdometryEvent");
+ * @endcode
+ *
+ * @see Rtabmap
+ * @see OdometryThread
+ * @see SensorCaptureThread
+ */
 class RTABMAP_CORE_EXPORT RtabmapThread :
 	public UThreadNode,
 	public UEventsHandler
 {
 public:
+	/** @brief What the main loop does on its next wake-up. */
 	enum State {
-		kStateDetecting,
+		kStateDetecting,      /**< Process the next buffered frame. */
-		kStateProcessCommand
+		kStateProcessCommand  /**< Execute the next queued @ref RtabmapEventCmd. */
 	};
 
 public:
-	// take ownership
+	/**
+	 * @brief Constructor.
+	 * @param rtabmap The map to run; must not be null. The thread takes ownership
+	 *                and deletes it in @ref close(), which the destructor calls.
+	 *
+	 * The buffer size, detection rate and intermediate node settings are taken
+	 * from the default parameters here, then updated by every
+	 * @ref RtabmapEventCmd::kCmdInit and @ref RtabmapEventCmd::kCmdUpdateParams
+	 * command.
+	 */
 	RtabmapThread(Rtabmap * rtabmap);
 	virtual ~RtabmapThread();
 
+	/** @brief Drops the buffered frames, the pending user data and the odometry state. */
 	void clearBufferedData();
+	/**
+	 * @brief Sets the maximum rate at which frames are processed (Hz, 0 = unlimited).
+	 *
+	 * Same setting as @ref Parameters::kRtabmapDetectionRate(). Frames arriving
+	 * faster are dropped, or added as intermediate nodes if
+	 * @ref createIntermediateNodes() is enabled.
+	 */
 	void setDetectorRate(float rate);
+	/**
+	 * @brief Sets how many frames may wait in the buffer (0 = unlimited).
+	 *
+	 * Same setting as @ref Parameters::kRtabmapImageBufferSize(). Once full, the
+	 * oldest frame is dropped to make room for the new one.
+	 */
 	void setDataBufferSize(unsigned int bufferSize);
+	/**
+	 * @brief Keeps the frames skipped by @ref setDetectorRate() as intermediate nodes.
+	 *
+	 * Same setting as @ref Parameters::kRtabmapCreateIntermediateNodes(). Those
+	 * nodes carry the odometry link but take no part in loop closure detection.
+	 */
 	void createIntermediateNodes(bool enabled);
 
+	/** @return Maximum processing rate in Hz (0 = unlimited). */
 	float getDetectorRate() const {return _rate;}
+	/** @return Maximum number of buffered frames (0 = unlimited). */
 	unsigned int getDataBufferSize() const {return _dataBufferMaxSize;}
+	/** @return True if skipped frames are added as intermediate nodes. */
 	bool getCreateIntermediateNodes() const {return _createIntermediateNodes;}
 
 	/**
-	 * Close rtabmap. This will delete rtabmap object if set.
+	 * @brief Joins the thread and closes rtabmap. This will delete rtabmap object if set.
 	 * @param databaseSaved true=database saved, false=database discarded.
 	 * @param databasePath output database file name, ignored if
 	 *                     Db/Sqlite3InMemory=false (opened database is
 	 *                     then overwritten).
+	 *
+	 * Called by the destructor with @p databaseSaved true. The object cannot be
+	 * used afterwards: the @ref Rtabmap instance it owned is gone.
 	 */
 	void close(bool databaseSaved, const std::string & databasePath = "");
 
 protected:
+	/**
+	 * @brief Receives the events listed in the class description.
+	 *
+	 * Runs in the posting thread: data events are only queued here, the work
+	 * happens in the thread's main loop. Events are ignored until the thread is
+	 * started.
+	 *
+	 * @return Always false, so the event keeps being dispatched to other handlers.
+	 */
 	virtual bool handleEvent(UEvent * anEvent);
 
 private:
@@ -106,7 +204,6 @@ private:
 	unsigned int _dataBufferMaxSize;
 	float _rate;
 	bool _createIntermediateNodes;
-	UTimer * _frameRateTimer;
 	double _previousStamp;
 
 	Rtabmap * _rtabmap;

corelib/include/rtabmap/core/SensorCapture.h

@@ -43,50 +43,231 @@ namespace rtabmap
 {
 
 /**
- * Class Camera
+ * @class SensorCapture
+ * @brief Abstract base class for sensor data capture (cameras, lidars, etc.)
  * 
+ * SensorCapture provides a unified interface for capturing sensor data from various
+ * sensor types including cameras (RGB-D, stereo, mono) and lidars. It handles frame
+ * rate control, local transform management, and provides a common API for sensor
+ * initialization and data capture.
+ * 
+ * The class implements the Template Method pattern:
+ * - `takeData()` handles frame rate control and timing, then calls the pure virtual
+ *   `captureData()` method implemented by derived classes
+ * - Derived classes must implement `captureData()` to perform the actual sensor capture
+ * 
+ * Key features:
+ * - **Frame rate control**: Automatic throttling to maintain target frame rate
+ * - **Local transform**: Transform from robot base frame to sensor frame
+ * - **Capture timing**: Tracks capture time and provides it via SensorCaptureInfo
+ * - **Sequence IDs**: Automatic sequence number generation for captured data
+ * 
+ * Derived classes include:
+ * - **Camera**: Base class for all camera types (RGB-D, stereo, mono, file readers, etc.)
+ * - **Lidar**: Base class for lidar sensors
+ * 
+ * @note This is an abstract class. Use concrete implementations like CameraRGBD,
+ *       CameraStereo, LidarVLP16, etc., or create custom derived classes.
+ * 
+ * @see Camera
+ * @see Lidar
+ * @see SensorData
+ * @see SensorCaptureInfo
+ * @see SensorCaptureThread
  */
 class RTABMAP_CORE_EXPORT SensorCapture
 {
 public:
+	/**
+	 * @brief Virtual destructor
+	 */
 	virtual ~SensorCapture();
+	
+	/**
+	 * @brief Captures sensor data with frame rate control
+	 * 
+	 * This method handles frame rate throttling and timing, then calls the
+	 * pure virtual `captureData()` method to perform the actual capture.
+	 * 
+	 * The method:
+	 * - Enforces the target frame rate by sleeping if necessary
+	 * - Measures capture time and stores it in SensorCaptureInfo
+	 * - Assigns sequence IDs to captured data
+	 * - Warns if the target frame rate cannot be reached
+	 * 
+	 * @param info Optional pointer to SensorCaptureInfo to fill with capture metadata
+	 *             (ID, timestamp, capture time). If null, no info is filled.
+	 * @return SensorData containing the captured sensor data
+	 * 
+	 * @note If frame rate is 0, data is captured as fast as possible without throttling.
+	 * @note The returned SensorData should have rectified images if calibration was loaded.
+	 * 
+	 * @see captureData()
+	 */
 	SensorData takeData(SensorCaptureInfo * info = 0);
 
+	/**
+	 * @brief Initializes the sensor
+	 * 
+	 * Pure virtual method that must be implemented by derived classes to initialize
+	 * the sensor hardware or data source. This typically involves:
+	 * - Opening device connections or file streams
+	 * - Loading camera calibration parameters
+	 * - Configuring sensor settings
+	 * 
+	 * @param calibrationFolder Directory path where calibration files are located
+	 *                          (default: current directory ".")
+	 * @param cameraName Base name of the camera for loading calibration files
+	 *                   (default: empty string)
+	 * @return True if initialization was successful, false otherwise
+	 * 
+	 * @note Must be called before calling takeData() or captureData()
+	 */
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
+	
+	/**
+	 * @brief Returns the sensor's serial number or unique identifier
+	 * 
+	 * Pure virtual method that must be implemented by derived classes to return
+	 * a unique identifier for the sensor (e.g., device serial number, file path,
+	 * or other identifier).
+	 * 
+	 * @return String identifier for the sensor
+	 */
 	virtual std::string getSerial() const = 0;
+	
+	/**
+	 * @brief Checks if the sensor provides odometry poses
+	 * 
+	 * Some sensors (e.g., visual-inertial cameras) can provide pose estimates
+	 * directly. This method indicates whether the sensor supports pose queries.
+	 * 
+	 * @return True if the sensor provides odometry poses, false otherwise
+	 * 
+	 * @note Default implementation returns false. Derived classes should override
+	 *       if they support pose estimation.
+	 * 
+	 * @see getPose()
+	 */
 	virtual bool odomProvided() const { return false; }
+	
+	/**
+	 * @brief Gets the sensor's pose estimate at a specific timestamp
+	 * 
+	 * Queries the sensor for its pose estimate at the given timestamp. This is
+	 * typically used for sensors that provide visual-inertial odometry or other
+	 * pose estimation capabilities.
+	 * 
+	 * @param stamp Timestamp in seconds for which to query the pose
+	 * @param[out] pose Output transform representing the sensor pose
+	 * @param[out] covariance Output covariance matrix (6x6) representing twist uncertainty
+	 * @param maxWaitTime Maximum time in seconds to wait for pose data (default: 0.06)
+	 * @return True if pose was successfully retrieved, false otherwise
+	 * 
+	 * @note Default implementation returns false. Derived classes should override
+	 *       if they support pose estimation.
+	 * 
+	 * @see odomProvided()
+	 */
 	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06) { return false; }
 
-	//getters
+	// Getters
+	
+	/**
+	 * @brief Returns the target frame rate
+	 * @return Frame rate in Hz (0 = unlimited, capture as fast as possible)
+	 */
 	float getFrameRate() const {return _frameRate;}
+	
+	/**
+	 * @brief Returns the local transform from base frame to sensor frame
+	 * @return Const reference to the local transform
+	 */
 	const Transform & getLocalTransform() const {return _localTransform;}
 
-	//setters
+	// Setters
+	
+	/**
+	 * @brief Sets the target frame rate
+	 * 
+	 * Controls how often `takeData()` will capture data. The method will throttle
+	 * captures to maintain the target rate.
+	 * 
+	 * @param frameRate Target frame rate in Hz (0 = unlimited, capture as fast as possible)
+	 * 
+	 * @note Setting frame rate to 0 disables throttling and captures as fast as possible.
+	 */
 	void setFrameRate(float frameRate) {_frameRate = frameRate;}
+	
+	/**
+	 * @brief Sets the local transform from base frame to sensor frame
+	 * 
+	 * The local transform represents the pose of the sensor relative to the robot's
+	 * base frame. This is used to transform sensor data into the robot's coordinate system.
+	 * 
+	 * @param localTransform Transform from base frame to sensor frame
+	 */
 	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
 
+	/**
+	 * @brief Resets the frame rate timer
+	 * 
+	 * Resets the internal timer used for frame rate control. This is useful when
+	 * starting a new capture session or after a pause.
+	 */
 	void resetTimer();
+	
 protected:
 	/**
-	 * Constructor
+	 * @brief Protected constructor
 	 * 
-	 * @param frameRate the frame rate (Hz), 0 for fast as the sensor can
+	 * Creates a SensorCapture instance with the specified frame rate and local transform.
-	 * @param localTransform the transform from base frame to sensor frame
+	 * This constructor is protected because SensorCapture is an abstract base class
+	 * and should not be instantiated directly.
+	 * 
+	 * @param frameRate Target frame rate in Hz (0 = unlimited, capture as fast as possible)
+	 * @param localTransform Transform from base frame to sensor frame (default: identity)
 	 */
 	SensorCapture(float frameRate = 0, const Transform & localTransform = Transform::getIdentity());
 
 	/**
-	 * returned rgb and depth images should be already rectified if calibration was loaded
+	 * @brief Pure virtual method to capture sensor data
+	 * 
+	 * This method must be implemented by derived classes to perform the actual
+	 * sensor data capture. It is called by `takeData()` after frame rate throttling.
+	 * 
+	 * The returned SensorData should:
+	 * - Have rectified images if calibration was loaded during `init()`
+	 * - Include proper timestamps
+	 * - Contain valid sensor data (images, depth, laser scans, etc.)
+	 * 
+	 * @param info Optional pointer to SensorCaptureInfo to fill with capture metadata.
+	 *             The base class will fill ID, timestamp, and capture time, but derived
+	 *             classes can add additional information.
+	 * @return SensorData containing the captured sensor data
+	 * 
+	 * @note If capture fails, return an empty SensorData (id=0, stamp=0.0).
+	 * @note RGB and depth images should be already rectified if calibration was loaded.
+	 * 
+	 * @see takeData()
 	 */
 	virtual SensorData captureData(SensorCaptureInfo * info = 0) = 0;
 
+	/**
+	 * @brief Gets the next sequence ID
+	 * 
+	 * Returns and increments the internal sequence counter. This is used to assign
+	 * unique sequence numbers to captured data.
+	 * 
+	 * @return Next sequence ID (starts at 1, increments with each call)
+	 */
 	int getNextSeqID() {return ++_seq;}
 
 private:
-	float _frameRate;
+	float _frameRate; ///< Target frame rate in Hz (0 = unlimited)
-	Transform _localTransform;
+	Transform _localTransform; ///< Transform from base frame to sensor frame
-	UTimer * _frameRateTimer;
+	UTimer * _frameRateTimer; ///< Timer for frame rate control
-	int _seq;
+	int _seq; ///< Sequence counter for captured data
 };
 
 

corelib/include/rtabmap/core/SensorCaptureInfo.h

@@ -33,10 +33,50 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
+/**
+ * @class SensorCaptureInfo
+ * @brief Metadata structure for sensor data capture and processing
+ * 
+ * SensorCaptureInfo contains metadata about sensor data capture, including timing
+ * information for various processing steps, camera identification, and odometry
+ * information. This information is typically attached to SensorEvent objects to
+ * provide context about how the sensor data was captured and processed.
+ * 
+ * The class tracks timing for various processing stages:
+ * - **Capture**: Time to capture data from the sensor
+ * - **Processing**: Time for various image processing steps (deskewing, disparity,
+ *   mirroring, exposure compensation, decimation, histogram equalization, etc.)
+ * - **Depth processing**: Time for depth-related operations (scan from depth,
+ *   undistort depth, bilateral filtering)
+ * - **Total**: Total processing time
+ * 
+ * Odometry information includes:
+ * - **Pose**: The odometry pose at the time of capture
+ * - **Covariance**: Uncertainty of the odometry twist (6x6 matrix)
+ * - **Velocity**: 6DOF velocity vector [vx, vy, vz, vroll, vpitch, vyaw]
+ * 
+ * @note All timing values are in seconds (float).
+ * @note The odometry covariance matrix is initialized to identity by default.
+ * 
+ * @see SensorEvent
+ * @see SensorData
+ */
 class SensorCaptureInfo
 {
 
 public:
+	/**
+	 * @brief Default constructor
+	 * 
+	 * Initializes all fields to default values:
+	 * - cameraName: empty string
+	 * - id: 0
+	 * - stamp: 0.0
+	 * - All timing fields: 0.0f
+	 * - odomPose: null transform
+	 * - odomCovariance: 6x6 identity matrix
+	 * - odomVelocity: empty vector
+	 */
 	SensorCaptureInfo() :
 		cameraName(""),
 		id(0),
@@ -55,28 +95,41 @@ public:
 		odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
 	{
 	}
+	
+	/**
+	 * @brief Virtual destructor
+	 */
 	virtual ~SensorCaptureInfo() {}
 
-	std::string cameraName;
+	std::string cameraName; ///< Camera/sensor name identifier
-	int id;
+	int id; ///< Capture ID (typically matches SensorData ID)
-	double stamp;
+	double stamp; ///< Timestamp in seconds (typically matches SensorData stamp)
-	float timeCapture;
+	
-	float timeDeskewing;
+	// Timing information (all in seconds)
-	float timeDisparity;
+	float timeCapture; ///< Time to capture data from the sensor (seconds)
-	float timeMirroring;
+	float timeDeskewing; ///< Time for laser scan deskewing (seconds)
-	float timeStereoExposureCompensation;
+	float timeDisparity; ///< Time to compute stereo disparity (seconds)
-	float timeImageDecimation;
+	float timeMirroring; ///< Time for image mirroring/flipping (seconds)
-	float timeHistogramEqualization;
+	float timeStereoExposureCompensation; ///< Time for stereo exposure compensation (seconds)
-	float timeScanFromDepth;
+	float timeImageDecimation; ///< Time for image decimation/downsampling (seconds)
-	float timeUndistortDepth;
+	float timeHistogramEqualization; ///< Time for histogram equalization (seconds)
-	float timeBilateralFiltering;
+	float timeScanFromDepth; ///< Time to convert depth image to laser scan (seconds)
-	float timeTotal;
+	float timeUndistortDepth; ///< Time to undistort depth image (seconds)
-	Transform odomPose;
+	float timeBilateralFiltering; ///< Time for bilateral filtering of depth (seconds)
-	cv::Mat odomCovariance;
+	float timeTotal; ///< Total processing time (seconds)
-	std::vector<float> odomVelocity;
+	
+	// Odometry information
+	Transform odomPose; ///< Odometry pose at the time of capture (in odometry coordinate frame)
+	cv::Mat odomCovariance; ///< Odometry twist covariance matrix (6x6, CV_64FC1). Default: identity matrix
+	std::vector<float> odomVelocity; ///< 6DOF odometry velocity [vx, vy, vz, vroll, vpitch, vyaw] (m/s, rad/s)
 };
 
-//backward compatibility
+/**
+ * @deprecated Use SensorCaptureInfo instead
+ * @brief Backward compatibility typedef
+ * 
+ * CameraInfo is deprecated. Use SensorCaptureInfo instead.
+ */
 RTABMAP_DEPRECATED typedef SensorCaptureInfo CameraInfo;
 
 } // namespace rtabmap