Merge branch 'ros2' of github.com:introlab/rtabmap_ros into humble-devel

.github/lcovrc

@@ -0,0 +1,45 @@
+# lcov configuration for .github/workflows/coverage.yml.
+#
+# A copy of colcon-lcov-result 0.5.0's default (colcon_lcov_result/verb/configuration/lcovrc),
+# with branch coverage turned off below. The whole file is needed: --lcov-config-file
+# replaces the default rather than adding to it.
+#
+# Why no branch coverage: gcc emits a hidden "it threw" branch on nearly every line that
+# calls a function, and no test takes it. Each such line -- a declaration like
+# 'rtabmap::Transform t;' included -- then counts as a partial, and Codecov reports a
+# partial line as not covered. lcov's geninfo_no_exception_branch would drop those
+# branches, but not with the lcov 1.15 of Ubuntu 22.04 and gcc 11: its JSON reader
+# ignores the flag, and its text reader cannot read gcc 11's .gcno files. So only line
+# coverage is reported: a line is covered when a test ran it.
+
+geninfo_auto_base=1
+
+# Specify size of tabs
+genhtml_num_spaces = 2
+
+# Include color legend in HTML output if non-zero
+genhtml_legend = 1
+
+# Include function coverage data display
+genhtml_function_coverage = 1
+
+# Include branch coverage data display
+genhtml_branch_coverage = 0
+
+# Specify whether to capture coverage data for external source
+# files
+geninfo_external = 0
+
+# Less verbose output
+lcov_quiet = 1
+
+# Specify if function coverage data should be collected and
+# processed.
+lcov_function_coverage = 1
+
+# Specify if branch coverage data should be collected and
+# processed.
+lcov_branch_coverage = 0
+
+## Follow symlinks
+lcov_follow = 1

.github/workflows/coverage.yml

@@ -0,0 +1,187 @@
+name: Coverage
+
+on:
+  push:
+    branches: [ ros2 ]
+  pull_request:
+    branches: [ ros2 ]
+  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
+
+jobs:
+  coverage:
+    name: Coverage (humble)
+    runs-on: ubuntu-latest
+    container:
+      # RTAB-Map master, already built and installed on top of ROS Humble
+      # (jammy = 22.04) -- the same base the repo's own docker/humble image
+      # uses. Saves building the library here, and pins the coverage run to
+      # RTAB-Map master rather than whatever the released binaries carry.
+      image: introlab3it/rtabmap:jammy
+    env:
+      CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }}
+
+    steps:
+      - uses: actions/checkout@v4
+
+      # The container runs as root, so no sudo (it is not installed).
+      #
+      # colcon lcov-result is not a built-in verb -- it comes from the
+      # colcon-lcov-result package, which action-ros-ci calls but does not
+      # install. ros2.yml gets it for free because it runs ros-tooling/setup-ros
+      # first; this job calls action-ros-ci directly, so install it here.
+      # Without it action-ros-ci logs "invalid choice: 'lcov-result'", ignores
+      # the failure, and the job goes green having measured nothing.
+      # Versions pinned to the ones setup-ros uses.
+      - run: |
+          export DEBIAN_FRONTEND=noninteractive
+          apt-get update
+          # lcov for genhtml (the browsable artifact below); colcon-lcov-result
+          # shells out to it too.
+          apt-get install -y lcov python3-pip
+          pip3 install -U \
+            colcon-lcov-result==0.5.0 \
+            colcon-coveragepy-result==0.0.8
+          colcon lcov-result --help > /dev/null
+          # The image ships rosdep but has never initialized it -- the repo's
+          # own docker/humble Dockerfile runs `rosdep init` for the same reason.
+          # action-ros-ci only runs `rosdep update`, which fails with "no
+          # sources directory exists" until this has happened once.
+          rosdep init || true
+
+      # Only the packages that have tests. The others are still built when a
+      # tested package depends on them (--packages-up-to), just not measured.
+      - uses: ros-tooling/[email protected]
+        with:
+          package-name: rtabmap_conversions rtabmap_util rtabmap_sync rtabmap_odom rtabmap_slam rtabmap_python
+          target-ros2-distro: humble
+          # RTAB-Map is installed in the image, not as an apt package, so rosdep
+          # cannot resolve the key and must not try.
+          rosdep-skip-keys: rtabmap
+          # The coverage-gcc mixin only adds --coverage; it sets no build type,
+          # and neither action-ros-ci nor this repo's CMakeLists do. Without
+          # this the build type is empty, which happens to mean -O0 but is not
+          # guaranteed to stay that way -- and at -O2 inlining and dead-code
+          # elimination make gcov's line attribution unreliable.
+          extra-cmake-args: -DCMAKE_BUILD_TYPE=Debug
+          colcon-defaults: |
+            {
+              "build": {
+                "mixin": ["coverage-gcc"]
+              },
+              "test": {
+                "pytest-with-coverage": true
+              }
+            }
+          # Pinned so a change in the mixin repository cannot break this job.
+          colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/b8436aa16c0bdbc01081b12caa253cbf16e0fb82/index.yaml
+          # Run the coverage passes ourselves, in the step below. The action's
+          # own baseline pass is a bare `colcon lcov-result --initial` with no
+          # package selection, so it walks every package in the workspace --
+          # including the ones --packages-up-to never built -- and fails with
+          # "cannot read .../build/rtabmap_viz".
+          coverage-result: false
+
+      # Both passes restricted to the packages that were actually built.
+      #
+      # lcov captures each package's whole build directory, so more than this
+      # repo's sources land in it. Filtered:
+      #   */test/*  the test sources -- the instrument, not the subject. A line
+      #             there is uncovered only when the test skipped it, which says
+      #             nothing about the code under test, and they are near-fully
+      #             covered by construction.
+      #   /usr/*, /opt/*  everything outside the workspace. RTAB-Map's own .cpp
+      #             files are never captured (the library is installed in the
+      #             image, not built here), but its inline and template code is
+      #             emitted into the objects of the packages that include it,
+      #             as are PCL, Eigen and the standard library.
+      #   *CompilerId*, */CMakeFiles/*  CMake's compiler-probe translation
+      #             units. colcon-lcov-result tries to delete their .gcno files
+      #             but misses these, and they are recorded against a path that
+      #             does not exist -- which kills the genhtml pass colcon runs
+      #             at the end ("cannot read .../CMakeCCompilerId.c", exit 2).
+      #             Filters run before that pass, so dropping them here is
+      #             enough.
+      # Quoting them matters: action-ros-ci injects filters unquoted, so the
+      # shell can glob them away before colcon ever sees them.
+      - name: Coverage report
+        working-directory: ros_ws
+        run: |
+          # `.` not `source`: steps in this container run under sh (dash), where
+          # `source` does not exist. GitHub picks sh whenever it cannot find
+          # bash in the image's PATH, and says so in the log ("shell: sh -e").
+          . /opt/ros/humble/setup.sh
+          # C++ packages only -- rtabmap_python emits no .gcno for lcov to read,
+          # and is measured by the coveragepy step below instead.
+          PKGS="rtabmap_conversions rtabmap_util rtabmap_sync rtabmap_odom rtabmap_slam"
+          # Baseline from the .gcno files. Without it a source file that no test
+          # ever loaded is missing from the report altogether rather than
+          # counted as 0%, which quietly inflates the result.
+          # Line coverage only, no branch coverage: see the comment in .github/lcovrc.
+          LCOVRC=src/rtabmap_ros/.github/lcovrc
+          colcon lcov-result --initial --packages-select $PKGS --lcov-config-file $LCOVRC
+          # The verb ends by running genhtml and returns *its* exit code, so a
+          # cosmetic HTML hiccup fails the whole job even though the report was
+          # written. The deliverable is total_coverage.info; the HTML is a
+          # convenience. Tolerate the former, then gate on the latter.
+          colcon lcov-result --packages-select $PKGS --lcov-config-file $LCOVRC --verbose \
+            --filter '*/test/*' '/usr/*' '/opt/*' \
+              '*CompilerId*' '*/CMakeFiles/*' || true
+          test -s lcov/total_coverage.info
+
+      # Python coverage is a separate mechanism: the coverage-gcc mixin only adds
+      # --coverage to the compiler, which does nothing for an ament_python package.
+      # colcon test --pytest-with-coverage (set above) writes a Cobertura report into
+      # the package's own build directory instead.
+      #
+      # Its paths are relative to the package rather than the repository, so
+      # cv_compression.py arrives as "rtabmap_python/cv_compression.py" -- one level
+      # short of where it really lives. Rewrite them here rather than leave Codecov to
+      # guess, which it does by suffix and can get wrong.
+      - name: Python coverage report
+        working-directory: ros_ws
+        run: |
+          python3 - <<'EOF'
+          import pathlib
+          import xml.etree.ElementTree as ET
+          p = pathlib.Path('build/rtabmap_python/coverage.xml')
+          if not p.is_file():
+              print('::warning::no python coverage produced for rtabmap_python')
+              raise SystemExit(0)
+          tree = ET.parse(p)
+          root = tree.getroot()
+          for source in root.iter('source'):
+              source.text = '.'
+          for cls in root.iter('class'):
+              cls.set('filename', 'rtabmap_python/' + cls.get('filename'))
+          tree.write(p, xml_declaration=True, encoding='utf-8')
+          print('rewrote', p, 'to repository-relative paths')
+          EOF
+
+      # colcon lcov-result runs genhtml itself, into the same lcov/ directory.
+      - name: Upload HTML coverage artifact
+        uses: actions/upload-artifact@v4
+        with:
+          name: coverage-html
+          path: ros_ws/lcov
+          retention-days: 14
+
+      - name: Upload to Codecov
+        if: ${{ env.CODECOV_TOKEN != '' }}
+        uses: codecov/codecov-action@v5
+        with:
+          files: ros_ws/lcov/total_coverage.info,ros_ws/build/rtabmap_python/coverage.xml
+          # Upload ONLY the aggregated lcov file. By default the CLI also walks
+          # the tree and runs gcov over every .gcno it finds, which re-adds the
+          # test sources the --filter above just dropped.
+          disable_search: true
+          plugins: noop
+          token: ${{ env.CODECOV_TOKEN }}
+          fail_ci_if_error: false

.github/workflows/docker-ros2.yml

@@ -1,73 +1,62 @@
-name: docker
+name: docker-ros2
+
+# ROS2 images: humble, jazzy, kilted and lyrical.
+#
+# The images built from this tree (docker/*/latest) compile the workspace, which
+# is far too slow to emulate, so every arch of those is built natively: amd64 on
+# an x86 runner, arm64 on a GitHub arm64 runner. Because a single Docker Hub tag
+# cannot hold two independently pushed architectures, each build pushes an
+# arch-suffixed tag (e.g. :humble-latest-amd64 / :humble-latest-arm64) and a
+# final job joins them into the real multi-arch tag (:humble-latest) with
+# `imagetools create`.
+#
+# The runner image only hosts the build; it does not have to match the Ubuntu
+# release inside the image, so ubuntu-26.04{,-arm} is used for all of them
+# (ubuntu-22.04{,-arm} and ubuntu-24.04{,-arm} also exist, if ever needed).
 
 on:
   push:
     branches: [ ros2 ]
   pull_request:
     branches: [ ros2 ]
+  workflow_dispatch:
 
 concurrency:
   group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
   cancel-in-progress: ${{ github.event_name == 'pull_request' }}
 
 jobs:
+  # Images built from this tree (docker/*/latest), the ones a change here can break.
   docker:
-    runs-on: ubuntu-latest
-    
+    # A manual dispatch is honored only on ros2, the only ref we push from.
+    if: ${{ github.event_name != 'workflow_dispatch' || github.ref == 'refs/heads/ros2' }}
+    runs-on: ${{ matrix.runner }}
+
     strategy:
       fail-fast: false
       matrix:
-        docker_tag: [humble, humble-latest, jazzy, jazzy-latest, kilted, kilted-latest, lyrical-latest]
+        docker_tag: [humble-latest, jazzy-latest, kilted-latest, lyrical-latest]
+        arch: [amd64, arm64]
         include:
-        - docker_tag: humble
-          docker_path: 'humble'
-          docker_platforms: |
-            linux/amd64
         - docker_tag: humble-latest
           docker_path: 'humble/latest'
-          docker_platforms: |
-            linux/amd64
-            linux/arm64
-        - docker_tag: jazzy
-          docker_path: 'jazzy'
-          docker_platforms: |
-            linux/amd64
-            linux/arm64
         - docker_tag: jazzy-latest
           docker_path: 'jazzy/latest'
-          docker_platforms: |
-            linux/amd64
-            linux/arm64
-        - docker_tag: kilted
-          docker_path: 'kilted'
-          docker_platforms: |
-            linux/amd64
-            linux/arm64
         - docker_tag: kilted-latest
           docker_path: 'kilted/latest'
-          docker_platforms: |
-            linux/amd64
-        #Disabled till rtabmap_ros is released on lyrical
-        #- docker_tag: lyrical
-        #  docker_path: 'lyrical'
-        #  docker_platforms: |
-        #    linux/amd64
-        #    linux/arm64
         - docker_tag: lyrical-latest
           docker_path: 'lyrical/latest'
-          docker_platforms: |
-            linux/amd64
-            linux/arm64
-    
+        - arch: amd64
+          runner: ubuntu-26.04
+          docker_platform: linux/amd64
+        - arch: arm64
+          runner: ubuntu-26.04-arm
+          docker_platform: linux/arm64
+
     steps:
       -
         name: Checkout
         uses: actions/checkout@v4
-      -
-        name: Set up QEMU
-        uses: docker/setup-qemu-action@v3
-        with:
-          platforms: all
       -
         name: Set up Docker Buildx
         uses: docker/setup-buildx-action@v3
@@ -86,9 +75,105 @@ jobs:
         with:
           context: .
           push: ${{ github.event_name != 'pull_request' }}
-          platforms: ${{ github.event_name == 'pull_request' && 'linux/amd64' || matrix.docker_platforms }}
+          platforms: ${{ matrix.docker_platform }}
+          # Run the test suites inside the image being built. Nothing of them is kept, and
+          # the build fails if one does, so no image is published from a tree that fails.
+          build-args: |
+            RUN_TESTS=1
+          file: ./docker/${{ matrix.docker_path }}/Dockerfile
+          tags: introlab3it/rtabmap_ros:${{ matrix.docker_tag }}-${{ matrix.arch }}
+          no-cache: true
+          cache-to: type=inline
+
+  docker_manifest:
+    needs: docker
+    # Nothing to join on pull requests, where the per-arch tags are never pushed.
+    if: ${{ !cancelled() && !failure() && github.event_name != 'pull_request' && (github.event_name != 'workflow_dispatch' || github.ref == 'refs/heads/ros2') }}
+    runs-on: ubuntu-26.04
+
+    strategy:
+      fail-fast: false
+      matrix:
+        docker_tag: [humble-latest, jazzy-latest, kilted-latest, lyrical-latest]
+
+    steps:
+      -
+        name: Login to DockerHub
+        uses: docker/login-action@v3
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      -
+        name: Create multi-arch manifest
+        run: |
+          docker buildx imagetools create \
+            -t introlab3it/rtabmap_ros:${{ matrix.docker_tag }} \
+            introlab3it/rtabmap_ros:${{ matrix.docker_tag }}-amd64 \
+            introlab3it/rtabmap_ros:${{ matrix.docker_tag }}-arm64
+
+  # Images that install a released rtabmap_ros from apt (docker/<distro>): they hold
+  # nothing from the tree under review, so building them on a pull request would only
+  # report that the release still installs. Left to the pushes that publish them.
+  #
+  # This one is left on a single QEMU-emulated job, for simplicity: it only
+  # apt-installs a released rtabmap_ros, so nothing is compiled under emulation,
+  # and one build pushes the multi-arch tag straight away -- no per-arch tags and
+  # no manifest job to join them.
+  docker-released:
+    if: ${{ github.event_name != 'pull_request' && (github.event_name != 'workflow_dispatch' || github.ref == 'refs/heads/ros2') }}
+    runs-on: ubuntu-latest
+
+    strategy:
+      fail-fast: false
+      matrix:
+        docker_tag: [humble, jazzy, kilted, lyrical]
+        include:
+        - docker_tag: humble
+          docker_path: 'humble'
+          docker_platforms: |
+            linux/amd64
+        - docker_tag: jazzy
+          docker_path: 'jazzy'
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+        - docker_tag: kilted
+          docker_path: 'kilted'
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+        - docker_tag: lyrical
+          docker_path: 'lyrical'
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+
+    steps:
+      -
+        name: Checkout
+        uses: actions/checkout@v4
+      -
+        name: Set up QEMU
+        uses: docker/setup-qemu-action@v3
+        with:
+          platforms: all
+      -
+        name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v3
+      -
+        name: Login to DockerHub
+        uses: docker/login-action@v3
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      -
+        name: Build and push
+        uses: docker/build-push-action@v6
+        with:
+          context: .
+          push: true
+          platforms: ${{ matrix.docker_platforms }}
           file: ./docker/${{ matrix.docker_path }}/Dockerfile
           tags: introlab3it/rtabmap_ros:${{ matrix.docker_tag }}
           no-cache: true
           cache-to: type=inline
-

.github/workflows/docker.yml

@@ -4,9 +4,13 @@ on:
   push:
     branches:
       - 'master'
+  workflow_dispatch:
 
 jobs:
   docker:
+    # Built and pushed only from master (push or manual dispatch), since it
+    # pushes the introlab3it/rtabmap_ros tags to Docker Hub.
+    if: github.ref == 'refs/heads/master'
     runs-on: ubuntu-latest
     
     strategy:

.github/workflows/ros2.yml

@@ -6,33 +6,41 @@ on:
     branches: [ humble-devel ]
   pull_request:
     branches: [ humble-devel ]
+  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' }}
+  cancel-in-progress: true
 
 jobs:
   build:
-    name: Build ros2 ${{ matrix.ros_distro }}
+    name: Build ros2 ${{ matrix.ros_distro }}${{ matrix.ros2_testing && ' (ros-testing)' || '' }}
     runs-on: ubuntu-latest
     strategy:
       matrix:
         ros_distro: [humble]
+        ros2_testing: [false, true] # build with main and ros-testing apt repositories
         include:
         - ros_distro: humble
-          skip_keys: ''
-          packages: 'rtabmap_ros'
+          skip_keys: '' # skip keys shoudl be empty when release on ROSDISTRO_devel branch, comment these keys in the appriopriate package.
+          # Listed packages are built with their dependencies, but only listed packages are tested.
+          packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_slam rtabmap_sync rtabmap_util rtabmap_python'
       fail-fast: false
     container:
       image: osrf/ros:${{ matrix.ros_distro }}-desktop-full
     steps:   
     - uses: actions/checkout@v4
+    - name: Remove ros2-apt-source (conflicts with ros2-testing-apt-source)
+      if: ${{ matrix.ros2_testing }}
+      run: |
+        apt-get remove -y ros2-apt-source
     - uses: ros-tooling/[email protected]
       with:
         required-ros-distributions: ${{ matrix.ros_distro }}
+        use-ros2-testing: ${{ matrix.ros2_testing }}
     - run: |
         DEBIAN_FRONTEND=noninteractive
         sudo apt update

.gitignore

@@ -1,3 +1,12 @@
 .pydevproject
 .settings
+.vscode
 __pycache__
+# rosdoc2 build artifacts. docs_build/ holds a copy of each package manifest, so
+# colcon would otherwise see two packages of every name and fail with "Duplicate
+# package names not supported" -- hence the COLCON_IGNORE, which is committed so
+# nobody has to know that. rosdoc2 leaves an existing marker in place.
+docs_build/*
+!docs_build/COLCON_IGNORE
+cross_reference
+doc_output

README.md

@@ -1,89 +1,86 @@
 rtabmap_ros
 ===========
 
-RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Humble minimum required**: currently most nodes are ported to ROS2. The interface is the same than on ROS1 (parameters and topic names should still match ROS1 documentation on [rtabmap_ros](http://wiki.ros.org/rtabmap_ros)). 
+ROS 2 wrapper for [RTAB-Map](https://github.com/introlab/rtabmap), a graph-based SLAM library with appearance-based loop closure detection. It builds and maintains a 3D map from RGB-D, stereo or lidar data, closes loops on revisited places and exports the result as an occupancy grid, a point cloud or an OctoMap.
+
+**ROS 2 Humble minimum required.** The interface matches ROS 1: parameters and topic names still follow the [ROS 1 documentation](http://wiki.ros.org/rtabmap_ros) for anything not yet covered by the package pages below.
 
 #### CI Latest
 
-  <table>
-    <tbody>
-        <tr>
-           <td>ROS 1</td>
-           <td><a href="https://github.com/introlab/rtabmap_ros/actions/workflows/ros1.yml"><img src="https://github.com/introlab/rtabmap_ros/actions/workflows/ros1.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/rtabmap_ros/actions/workflows/docker.yml"><img src="https://github.com/introlab/rtabmap_ros/actions/workflows/docker.yml/badge.svg" alt="Build Status"/>
-           </td>
-        </tr>
-        <tr>
-           <td>ROS 2</td>
-           <td><a href="https://github.com/introlab/rtabmap_ros/actions/workflows/ros2.yml"><img src="https://github.com/introlab/rtabmap_ros/actions/workflows/ros2.yml/badge.svg" alt="Build Status"/>
-           </td>
-        </tr>
-     </tbody>
-  </table>
- 
- #### ROS Binaries
- 
- <table>
-    <tbody>
-        <tr>
-            <td rowspan="1">ROS 1</td>
-            <td>Noetic</td>
-            <td><a href="http://build.ros.org/job/Nbin_ufv8_uFv8__rtabmap_ros__ubuntu_focal_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Nbin_ufv8_uFv8__rtabmap_ros__ubuntu_focal_arm64__binary" alt="Build Status"/></td>
-        </tr>
-        <tr>
-            <td rowspan="4">ROS 2</td>
-            <td>Humble</td>
-            <td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
-        </tr>
-        <tr>
-            <td>Jazzy</td>
-            <td><a href="http://build.ros2.org/job/Jbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
-        </tr>
-        <tr>
-            <td>Rolling</td>
-            <td><a href="http://build.ros2.org/job/Rbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
-        </tr>
-        <tr>
-           <td>Docker</td>
-           <td>
-             <a href="https://hub.docker.com/r/introlab3it/rtabmap_ros">rtabmap_ros</a>
-           </td>
-           <td><img src="https://img.shields.io/docker/pulls/introlab3it/rtabmap_ros.svg?label=pulls" alt="Docker Pulls"/></td>
-        </tr>
-    </tbody>
-</table>
+| | Build | Docker |
+|---|---|---|
+| ROS 1 | [![ROS 1](https://github.com/introlab/rtabmap_ros/actions/workflows/ros1.yml/badge.svg)](https://github.com/introlab/rtabmap_ros/actions/workflows/ros1.yml) | [![Docker](https://github.com/introlab/rtabmap_ros/actions/workflows/docker.yml/badge.svg)](https://github.com/introlab/rtabmap_ros/actions/workflows/docker.yml) |
+| ROS 2 | [![ROS 2](https://github.com/introlab/rtabmap_ros/actions/workflows/ros2.yml/badge.svg)](https://github.com/introlab/rtabmap_ros/actions/workflows/ros2.yml) | [![Docker ROS 2](https://github.com/introlab/rtabmap_ros/actions/workflows/docker-ros2.yml/badge.svg)](https://github.com/introlab/rtabmap_ros/actions/workflows/docker-ros2.yml) |
 
-# Usage
+#### ROS Binaries
 
-* For sensor integration examples (stereo and RGB-D cameras, 3D LiDAR), see [rtabmap_examples](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) sub-folder.
+| | Distro | Ubuntu | Released | In apt | Build |
+|---|---|---|---|---|---|
+| ROS 1 | Noetic (EOL) | 20.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fnoetic%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/noetic/distribution.yaml) | [![apt](https://img.shields.io/ros/v/noetic/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#noetic) |  |
+| ROS 2 | Humble | 22.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fhumble%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/humble/distribution.yaml) | [![apt](https://img.shields.io/ros/v/humble/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#humble) | [![build](http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary)](http://build.ros2.org/job/Hbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary/) |
+| ROS 2 | Iron (EOL) | 22.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Firon%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/iron/distribution.yaml) | [![apt](https://img.shields.io/ros/v/iron/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#iron) |  |
+| ROS 2 | Jazzy | 24.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fjazzy%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/jazzy/distribution.yaml) | [![apt](https://img.shields.io/ros/v/jazzy/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#jazzy) | [![build](http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary)](http://build.ros2.org/job/Jbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary/) |
+| ROS 2 | Kilted | 24.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fkilted%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/kilted/distribution.yaml) | [![apt](https://img.shields.io/ros/v/kilted/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#kilted) | [![build](http://build.ros2.org/buildStatus/icon?job=Kbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary)](http://build.ros2.org/job/Kbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary/) |
+| ROS 2 | Lyrical | 26.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Flyrical%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/lyrical/distribution.yaml) | [![apt](https://img.shields.io/ros/v/lyrical/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#lyrical) | [![build](http://build.ros2.org/buildStatus/icon?job=Lbin_uR64__rtabmap_ros__ubuntu_resolute_amd64__binary)](http://build.ros2.org/job/Lbin_uR64__rtabmap_ros__ubuntu_resolute_amd64__binary/) |
+| ROS 2 | Rolling | 26.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Frolling%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/rolling/distribution.yaml) | [![apt](https://img.shields.io/ros/v/rolling/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#rolling) |  |
+| Docker | [rtabmap_ros](https://hub.docker.com/r/introlab3it/rtabmap_ros) | | | ![Docker Pulls](https://img.shields.io/docker/pulls/introlab3it/rtabmap_ros.svg?label=pulls) | |
 
-* For robot integration examples (turtlebot3 and turtlebot4, nav2 integration), see [rtabmap_demos](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_demos) sub-folder.
+*Released* is the version bloomed into [rosdistro](https://github.com/ros/rosdistro); *In apt* is what `apt install` actually gives you today. They differ while a release is waiting on a buildfarm sync.
 
-## Logging
-To make RTAB-Map's logs appear ordered with RCLCPP's logs, set the following environment variables in your `.bashrc` (see official "[About Logging](https://docs.ros.org/en/humble/Concepts/Intermediate/About-Logging.html)" documentation for more info):
-```bash
-export RCUTILS_LOGGING_USE_STDOUT=1
-export RCUTILS_LOGGING_BUFFERED_STREAM=1
-# Optional, but if you like colored logs:
-export RCUTILS_COLORIZED_OUTPUT=1
-```
+# Packages
 
-## Recommended DDS
-If RTAB-Map's GUI or topic frequency feel laggy (even if processing time looks fast enough), it may be caused by the DDS. I recommend to use [Cyclone DDS](https://docs.ros.org/en/foxy/Installation/DDS-Implementations/Working-with-Eclipse-CycloneDDS.html), you can try it by adding this before launching any nodes/launch files (or add to your `.bashrc`):
-```bash
-export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
-# Cyclone prefers multicast by default, if your router got too much spammed, 
-# disable multicast with (https://github.com/ros2/rmw_cyclonedds/issues/489):
-export CYCLONEDDS_URI="<Disc><DefaultMulticastAddress>0.0.0.0</></>"
-```
+The stack is split into small packages so a pipeline only pulls in what it uses. Package names link to their documentation where it exists; the rest are being written and will be linked as they land.
 
-# Installation 
+### SLAM
+
+| Package | Description |
+|---|---|
+| [`rtabmap_slam`](rtabmap_slam/README.md) | The `rtabmap` node itself: appearance-based loop closure detection, graph optimization, memory management and map assembly. |
+| [`rtabmap_odom`](rtabmap_odom/README.md) | Odometry nodes — `rgbd_odometry`, `stereo_odometry` and `icp_odometry`. Any external odometry can be used instead. |
+| [`rtabmap_sync`](rtabmap_sync/README.md) | Synchronizes camera and lidar topics into a single message so they reach the SLAM node together — `rgbd_sync`, `stereo_sync`, `rgbdx_sync`. |
+
+### Sensor processing
+
+| Package | Description |
+|---|---|
+| [`rtabmap_util`](rtabmap_util/README.md) | Utility nodes around the pipeline: format conversions, point cloud filtering and assembly, obstacle detection, map assembly, database replay. Most are useful on their own. |
+| `rtabmap_costmap_plugins` | A variant of nav2's voxel layer that follows the robot along z, keeping the voxel grid centered on the base frame. For robots that change altitude, e.g. drones. |
+
+### Interfaces and libraries
+
+| Package | Description |
+|---|---|
+| `rtabmap_msgs` | Message, service and action definitions used across the stack. |
+| [`rtabmap_conversions`](rtabmap_conversions/README.md) | C++ library converting between RTAB-Map library types and ROS 2 messages. |
+| [`rtabmap_python`](rtabmap_python/README.md) | Python helpers for RTAB-Map's own binary formats, currently the compressed matrices carried in `rtabmap_msgs` fields and database blobs. |
+
+### Visualization
+
+| Package | Description |
+|---|---|
+| `rtabmap_viz` | RTAB-Map's own GUI as a ROS 2 node: live graph, loop closures, feature matches and the parameter panel. |
+| `rtabmap_rviz_plugins` | RViz displays for the map graph, the assembled cloud and the SLAM info. |
+
+### Launch files
+
+| Package | Description |
+|---|---|
+| `rtabmap_launch` | `rtabmap.launch.py`, the one-line way to bring up the whole stack. |
+| [`rtabmap_examples`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) | Sensor integration examples: stereo and RGB-D cameras, 3D lidar. |
+| [`rtabmap_demos`](rtabmap_demos/README.md) | Full robot demos: turtlebot3 and turtlebot4, nav2 integration, multi-session mapping. |
+
+# Installation
+
+These instructions are for ROS 2. For ROS 1, follow the [installation instructions](https://github.com/introlab/rtabmap_ros/tree/master#installation) on the [`master`](https://github.com/introlab/rtabmap_ros/tree/master) branch, which also carries the latest version for Noetic.
 
 ### Binaries
+
 ```bash
 sudo apt install ros-$ROS_DISTRO-rtabmap-ros
 ```
 
 ### From Source
+
 * Make sure to uninstall any rtabmap binaries:
     ```
     sudo apt remove ros-$ROS_DISTRO-rtabmap*
@@ -103,3 +100,60 @@ sudo apt install ros-$ROS_DISTRO-rtabmap-ros
     colcon build --symlink-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DRTABMAP_SYNC_USER_DATA=ON -DCMAKE_BUILD_TYPE=Release
     ```
 
+### Testing
+
+```bash
+cd ~/ros2_ws
+colcon build --base-paths src/rtabmap_ros
+colcon test --base-paths src/rtabmap_ros
+colcon test-result --verbose
+```
+
+# Usage
+
+* For sensor integration examples (stereo and RGB-D cameras, 3D LiDAR), see [rtabmap_examples](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) sub-folder.
+
+* For robot integration examples (turtlebot3 and turtlebot4, nav2 integration), see [rtabmap_demos](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_demos) sub-folder.
+
+## Logging
+To make RTAB-Map's logs appear ordered with RCLCPP's logs, set the following environment variables in your `.bashrc` (see official "[About Logging](https://docs.ros.org/en/jazzy/Concepts/Intermediate/About-Logging.html)" documentation for more info):
+```bash
+export RCUTILS_LOGGING_USE_STDOUT=1
+export RCUTILS_LOGGING_BUFFERED_STREAM=1
+# Optional, but if you like colored logs:
+export RCUTILS_COLORIZED_OUTPUT=1
+```
+
+## Recommended DDS
+If RTAB-Map's GUI or topic frequency feel laggy (even if processing time looks fast enough), it may be caused by the DDS. I recommend to use [Cyclone DDS](https://docs.ros.org/en/jazzy/Installation/RMW-Implementations/DDS-Implementations/Working-with-Eclipse-CycloneDDS.html), you can try it by adding this before launching any nodes/launch files (or add to your `.bashrc`):
+```bash
+export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+# Cyclone prefers multicast by default, if your router got too much spammed, 
+# disable multicast with (https://github.com/ros2/rmw_cyclonedds/issues/489):
+export CYCLONEDDS_URI="<Disc><DefaultMulticastAddress>0.0.0.0</></>"
+```
+
+# Documentation
+
+* **Package documentation** — the tables above, and the [API reference on docs.ros.org](https://docs.ros.org/en/jazzy/p/rtabmap_ros/).
+* **Examples** — [rtabmap_examples](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) for sensors, [rtabmap_demos](rtabmap_demos/README.md) for full robots.
+* **Parameters** — every `Rtabmap/*`, `Grid/*`, `Odom/*` and other core parameter is listed in the [RTAB-Map parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html).
+* **Library API** — [RTAB-Map's own API documentation](https://introlab.github.io/rtabmap/api/latest/).
+* **Papers and videos** — [introlab.github.io/rtabmap](https://introlab.github.io/rtabmap/).
+* **Old tutorials** — the [ROS 1 wiki](http://wiki.ros.org/rtabmap_ros/Tutorials), for anything not covered above; parameters and topic names are unchanged.
+
+## Building the documentation
+
+Each package's API reference is generated with [rosdoc2](https://github.com/ros-infrastructure/rosdoc2) from the Doxygen comments in its public headers, and published to docs.ros.org. rosdoc2 documents one package per invocation, so building the whole stack is a loop over them — run it from the repository root:
+
+```bash
+for pkg in rtabmap_*/; do
+  rosdoc2 build --package-path "$pkg" --output-directory doc_output || break
+done
+```
+
+Each package lands in `doc_output/<package>/index.html`.
+
+# License
+
+BSD-3-Clause, see [LICENSE](LICENSE). RTAB-Map itself may be built with components under other licenses; see the [rtabmap](https://github.com/introlab/rtabmap) repository.

codecov.yml

@@ -0,0 +1,91 @@
+# Codecov configuration -- https://docs.codecov.com/docs/codecov-yaml
+#
+# Coverage data is produced by .github/workflows/coverage.yml (colcon's
+# coverage-gcc mixin over the packages that have tests, aggregated by
+# colcon-lcov-result) 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.
+
+# action-ros-ci checks the repository out into a colcon workspace, so every
+# path in the lcov report is prefixed. Strip it, otherwise Codecov cannot match
+# a file to the one in the diff and reports no coverage at all.
+fixes:
+  - "ros_ws/src/rtabmap_ros/::"
+
+# 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
+
+# Per-package breakdown, computed from the same single upload -- no extra job
+# and no separate flag upload per package. Each component gets its own line in
+# the pull request comment and its own status check.
+component_management:
+  default_rules:
+    statuses:
+      - type: project
+        target: auto
+        threshold: 1%
+  individual_components:
+    - component_id: rtabmap_conversions
+      name: rtabmap_conversions
+      paths:
+        - rtabmap_conversions/**
+    - component_id: rtabmap_util
+      name: rtabmap_util
+      paths:
+        - rtabmap_util/**
+    - component_id: rtabmap_sync
+      name: rtabmap_sync
+      paths:
+        - rtabmap_sync/**
+    - component_id: rtabmap_odom
+      name: rtabmap_odom
+      paths:
+        - rtabmap_odom/**
+    - component_id: rtabmap_slam
+      name: rtabmap_slam
+      paths:
+        - rtabmap_slam/**
+    - component_id: rtabmap_python
+      name: rtabmap_python
+      paths:
+        - rtabmap_python/**
+
+comment:
+  layout: "condensed_header, diff, components, files"
+  behavior: default
+  require_changes: true # stay quiet when coverage doesn't move
+
+# Only rtabmap_conversions, rtabmap_util, rtabmap_sync, rtabmap_odom, rtabmap_slam and
+# rtabmap_python have tests today, so
+# everything else would report as 0% and drag the total down to a number that
+# says nothing. As a package gains tests, drop its line here and add it to
+# individual_components above.
+ignore:
+  - "rtabmap_costmap_plugins/**"
+  - "rtabmap_demos/**"
+  - "rtabmap_examples/**"
+  - "rtabmap_launch/**"
+  - "rtabmap_msgs/**"
+  - "rtabmap_rviz_plugins/**"
+  - "rtabmap_viz/**"
+  - "**/test/**"
+  - "**/setup.py" # packaging scaffolding, not code under test

docker/humble/latest/Dockerfile

@@ -4,14 +4,26 @@ RUN mkdir -p ros2_ws/src
 
 COPY . ros2_ws/src/rtabmap_ros
 
+# No rosdep "-r": a bad mirror must fail here.
 RUN source /ros_entrypoint.sh && \
     cd ros2_ws && \
-    export MAKEFLAGS="-j2" && \
+    echo 'Acquire::Retries "5";' > /etc/apt/apt.conf.d/80-retries && \
     rosdep init && \
     rosdep update && \
     apt-get update && \
-    rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap" && \
-    apt-get clean && rm -rf /var/lib/apt/lists/ && \
+    rosdep install --from-paths src --ignore-src -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap" && \
+    bash src/rtabmap_ros/docker/verify_deps.sh && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG RUN_TESTS=0
+
+RUN source /ros_entrypoint.sh && \
+    cd ros2_ws && \
+    export MAKEFLAGS="-j2" && \
     colcon build --executor sequential --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    if [ "$RUN_TESTS" = "1" ]; then \
+        colcon test --executor sequential --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install && \
+        colcon test-result --verbose; \
+    fi && \
     cd && \
     rm -rf ros2_ws

docker/iron/latest/Dockerfile

@@ -6,15 +6,21 @@ RUN source /ros_entrypoint.sh && \
 
 COPY . ros2_ws/src/rtabmap_ros
 
+# No rosdep "-r": a bad mirror must fail here.
 RUN source /ros_entrypoint.sh && \
     cd ros2_ws && \
-    export MAKEFLAGS="-j1" && \
+    echo 'Acquire::Retries "5";' > /etc/apt/apt.conf.d/80-retries && \
     rosdep init && \
     rosdep update && \
     apt-get update && \
-    rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool && \
+    rosdep install --from-paths src --ignore-src -y -t build_export -t test -t build -t buildtool_export -t buildtool && \
     apt remove ros-$ROS_DISTRO-rtabmap* -y && \
-    apt-get clean && rm -rf /var/lib/apt/lists/ && \
+    bash src/rtabmap_ros/docker/verify_deps.sh && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+RUN source /ros_entrypoint.sh && \
+    cd ros2_ws && \
+    export MAKEFLAGS="-j1" && \
     colcon build --event-handlers console_direct+ --install-base /opt/ros/iron --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
     cd && \
     rm -rf ros2_ws

docker/jazzy/latest/Dockerfile

@@ -6,14 +6,26 @@ RUN source /ros_entrypoint.sh && \
 
 COPY . ros2_ws/src/rtabmap_ros
 
+# No rosdep "-r": a bad mirror must fail here.
 RUN source /ros_entrypoint.sh && \
     cd ros2_ws && \
-    export MAKEFLAGS="-j2" && \
+    echo 'Acquire::Retries "5";' > /etc/apt/apt.conf.d/80-retries && \
     rosdep init && \
     rosdep update && \
     apt-get update && \
-    rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap grid_map_ros" && \
-    apt-get clean && rm -rf /var/lib/apt/lists/ && \
-    colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    rosdep install --from-paths src --ignore-src -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap grid_map_ros" && \
+    bash src/rtabmap_ros/docker/verify_deps.sh && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG RUN_TESTS=0
+
+RUN source /ros_entrypoint.sh && \
+    cd ros2_ws && \
+    export MAKEFLAGS="-j2" && \
+    colcon build --executor sequential --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    if [ "$RUN_TESTS" = "1" ]; then \
+        colcon test --executor sequential --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install && \
+        colcon test-result --verbose; \
+    fi && \
     cd && \
     rm -rf ros2_ws

docker/kilted/latest/Dockerfile

@@ -6,14 +6,26 @@ RUN source /ros_entrypoint.sh && \
 
 COPY . ros2_ws/src/rtabmap_ros
 
+# No rosdep "-r": a bad mirror must fail here.
 RUN source /ros_entrypoint.sh && \
     cd ros2_ws && \
-    export MAKEFLAGS="-j2" && \
+    echo 'Acquire::Retries "5";' > /etc/apt/apt.conf.d/80-retries && \
     rosdep init && \
     rosdep update && \
     apt-get update && \
-    rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros" && \
-    apt-get clean && rm -rf /var/lib/apt/lists/ && \
-    colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    rosdep install --from-paths src --ignore-src -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros" && \
+    bash src/rtabmap_ros/docker/verify_deps.sh && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG RUN_TESTS=0
+
+RUN source /ros_entrypoint.sh && \
+    cd ros2_ws && \
+    export MAKEFLAGS="-j2" && \
+    colcon build --executor sequential --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    if [ "$RUN_TESTS" = "1" ]; then \
+        colcon test --executor sequential --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install && \
+        colcon test-result --verbose; \
+    fi && \
     cd && \
     rm -rf ros2_ws

docker/lyrical/latest/Dockerfile

@@ -6,14 +6,26 @@ RUN source /ros_entrypoint.sh && \
 
 COPY . ros2_ws/src/rtabmap_ros
 
+# No rosdep "-r": a bad mirror must fail here.
 RUN source /ros_entrypoint.sh && \
     cd ros2_ws && \
-    export MAKEFLAGS="-j2" && \
+    echo 'Acquire::Retries "5";' > /etc/apt/apt.conf.d/80-retries && \
     rosdep init && \
     rosdep update && \
     apt-get update && \
-    rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros nav2_costmap_2d" && \
-    apt-get clean && rm -rf /var/lib/apt/lists/ && \
-    colcon build --packages-skip rtabmap_costmap_plugins rtabmap_ros --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    rosdep install --from-paths src --ignore-src -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros" && \
+    bash src/rtabmap_ros/docker/verify_deps.sh && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG RUN_TESTS=0
+
+RUN source /ros_entrypoint.sh && \
+    cd ros2_ws && \
+    export MAKEFLAGS="-j2" && \
+    colcon build --executor sequential --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    if [ "$RUN_TESTS" = "1" ]; then \
+        colcon test --executor sequential --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install && \
+        colcon test-result --verbose; \
+    fi && \
     cd && \
     rm -rf ros2_ws

docker/verify_deps.sh

@@ -0,0 +1,76 @@
+#!/usr/bin/env bash
+#
+# Sanity-check the build sysroot right after "rosdep install".
+#
+# apt/dpkg can be left in a half-applied state, most often on the QEMU-emulated
+# arm64 CI leg (ports.ubuntu.com is a single, frequently desynced mirror). CMake
+# does not notice, because both PCL and VTK look their files up in ways that
+# degrade silently:
+#
+#   * PCLConfig.cmake resolves each component with
+#     find_library(... HINTS ${PCL_LIBRARY_DIRS} NO_DEFAULT_PATH), so a missing
+#     libpcl_*.so only yields "Could NOT find PCL_COMMON (missing:
+#     PCL_COMMON_LIBRARY)" on stderr and configuring still succeeds.
+#
+#   * VTK-targets.cmake creates every VTK::* imported target, then loads their
+#     IMPORTED_LOCATION from the per-configuration files it picks up with
+#     file(GLOB VTK-targets-*.cmake). An empty glob is not an error, so the
+#     targets survive with no location at all and the build only dies at the
+#     generate step with "IMPORTED_LOCATION not set for imported target
+#     VTK::CommonCore configuration Release".
+#
+# Both surface hours into the build, in whichever package first links those
+# targets (rtabmap_odom, via pcl_ros). Fail here instead, where the cause is
+# still readable.
+
+set -euo pipefail
+shopt -s nullglob
+
+status=0
+
+fail() {
+  echo "verify_deps: $*" >&2
+  status=1
+}
+
+# PCLConfig.cmake and the libpcl_*.so development symlinks both ship in
+# libpcl-dev, so finding the config without them means the package is not
+# fully installed.
+for config in /usr/lib/*/cmake/pcl/PCLConfig.cmake /usr/lib/cmake/pcl/PCLConfig.cmake; do
+  # nullglob only drops patterns, not wildcard-free words.
+  [ -e "${config}" ] || continue
+  libdir=${config%/cmake/pcl/PCLConfig.cmake}
+  for component in common io kdtree search surface filters registration \
+                   sample_consensus segmentation visualization; do
+    if [ ! -e "${libdir}/libpcl_${component}.so" ]; then
+      fail "${libdir}/libpcl_${component}.so is missing while ${config} is installed (libpcl-dev is incomplete)"
+    fi
+  done
+done
+
+for targets in /usr/lib/*/cmake/vtk-*/VTK-targets.cmake /usr/lib/cmake/vtk-*/VTK-targets.cmake; do
+  [ -e "${targets}" ] || continue
+  if ! compgen -G "${targets%.cmake}-*.cmake" > /dev/null; then
+    fail "no VTK-targets-<config>.cmake next to ${targets}, every VTK::* target would have no IMPORTED_LOCATION (libvtk-dev is incomplete)"
+  fi
+done
+
+# libssl-dev ships the libssl.so and libcrypto.so development symlinks beside the
+# headers FindOpenSSL reads its version from. Every find_package(rclcpp) reaches
+# find_package(OpenSSL REQUIRED) through fastrtps-config.cmake, so headers without
+# the symlinks fail the first package that configures rclcpp, several packages in.
+if [ -e /usr/include/openssl/opensslv.h ]; then
+  for lib in ssl crypto; do
+    if ! compgen -G "/usr/lib/*-linux-gnu/lib${lib}.so" > /dev/null \
+       && [ ! -e "/usr/lib/lib${lib}.so" ]; then
+      fail "no lib${lib}.so under /usr/lib while /usr/include/openssl is installed (libssl-dev is incomplete)"
+    fi
+  done
+fi
+
+if [ "${status}" -ne 0 ]; then
+  echo "verify_deps: dependency installation left an inconsistent sysroot, aborting before the build" >&2
+  exit 1
+fi
+
+echo "verify_deps: PCL, VTK and OpenSSL sysroot look consistent"

rtabmap_conversions/CMakeLists.txt

@@ -30,7 +30,7 @@ find_package(tf2_eigen REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 
-find_package(RTABMap 0.23.5 REQUIRED)
+find_package(RTABMap 0.23.13 REQUIRED)
 
 # libraries
 SET(Libraries
@@ -122,5 +122,22 @@ install(DIRECTORY include/
    FILES_MATCHING PATTERN "*.h"
 )
 
+#############
+## Testing ##
+#############
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ament_add_gtest(test_msg_conversion test/test_msg_conversion.cpp)
+  if(TARGET test_msg_conversion)
+    target_link_libraries(test_msg_conversion rtabmap_conversions)
+    if("$ENV{ROS_DISTRO}" STRLESS "lyrical")
+      ament_target_dependencies(test_msg_conversion ${AmentLibraries})
+    else()
+      target_link_libraries(test_msg_conversion ${Libraries} ${PublicLibraries})
+    endif()
+  endif()
+endif()
+
 ament_package()
 

rtabmap_conversions/README.md

@@ -0,0 +1,68 @@
+# rtabmap_conversions
+
+Conversions between [RTAB-Map](https://github.com/introlab/rtabmap) library types and ROS 2 messages.
+
+This package is a library only — it contains no nodes, no launch files and no parameters. Every other `rtabmap_ros` package that touches a message goes through it: [`rtabmap_slam`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_slam), [`rtabmap_odom`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_odom), [`rtabmap_sync`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_sync), [`rtabmap_util`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_util), [`rtabmap_viz`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_viz) and [`rtabmap_rviz_plugins`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_rviz_plugins).
+
+You only need it directly if you are writing your own node against RTAB-Map's C++ API and want to publish or subscribe to `rtabmap_msgs`.
+
+## Contents
+
+- [Usage](#usage)
+- [What it covers](#what-it-covers)
+- [Conventions worth knowing](#conventions-worth-knowing)
+- [License](#license)
+
+## Usage
+
+Add the dependency to your `package.xml` and `CMakeLists.txt`:
+
+```xml
+<depend>rtabmap_conversions</depend>
+```
+
+```cmake
+find_package(rtabmap_conversions REQUIRED)
+target_link_libraries(my_node rtabmap_conversions::rtabmap_conversions)
+```
+
+Everything lives in a single header and the `rtabmap_conversions` namespace:
+
+```cpp
+#include <rtabmap_conversions/MsgConversion.h>
+
+// A pose message to an rtabmap::Transform and back.
+rtabmap::Transform pose = rtabmap_conversions::transformFromPoseMsg(msg.pose);
+
+geometry_msgs::msg::Pose out;
+rtabmap_conversions::transformToPoseMsg(pose, out);
+```
+
+The naming is uniform: `xxxFromROS()` converts a message into an RTAB-Map type, `xxxToROS()` goes the other way. `ToROS()` functions write through a reference parameter so the message can be reused; `FromROS()` functions return by value.
+
+## What it covers
+
+| Group | Functions |
+|---|---|
+| Transforms | `transformFromTF`, `transformToTF`, `transformFromGeometryMsg`, `transformToGeometryMsg`, `transformFromPoseMsg`, `transformToPoseMsg` |
+| TF lookups | `getTransform`, `getMovingTransform` |
+| Camera models | `cameraModelFromROS`, `cameraModelToROS`, `stereoCameraModelFromROS` |
+| Images | `toCvCopy`, `toCvShare`, `rgbdImageFromROS`, `rgbdImageToROS`, `convertRGBDMsgs`, `convertStereoMsg` |
+| Laser scans | `convertScanMsg`, `convertScan3dMsg`, `deskew`, `transformPointCloud`, `sizeOfPointField` |
+| Features | `keypointFromROS`, `point2fFromROS`, `point3fFromROS`, `globalDescriptorFromROS` (+ vector and `ToROS` variants) |
+| Graph | `mapDataFromROS`, `mapGraphFromROS`, `nodeFromROS`, `linkFromROS`, `sensorDataFromROS` (+ `ToROS` variants) |
+| Misc | `infoFromROS`, `odomInfoFromROS`, `odomInfoToStatistics`, `imuFromROS`, `userDataFromROS`, `envSensorFromROS`, `landmarksFromROS`, `timestampFromROS`, `timestampToROS` |
+
+Full signatures and per-function notes are in the [API documentation](https://docs.ros.org/en/jazzy/p/rtabmap_conversions/) and in [`MsgConversion.h`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h).
+
+## Conventions worth knowing
+
+These cut across the whole API and are not obvious from the signatures. Per-function caveats — object lifetimes, which fields a given `ToROS()` fills — are documented on the functions themselves.
+
+**Null transforms.** RTAB-Map distinguishes a *null* transform (unknown) from identity. Over the wire this is encoded as an all-zero quaternion, so `transformFromGeometryMsg()` and `transformFromPoseMsg()` return a null `rtabmap::Transform` for one. Always check `isNull()` before using a result. `tf2::Transform` cannot represent this — it stores rotation as a basis matrix — so `transformToTF()` returns a `bool` instead.
+
+**`CameraInfo` matrices are fixed-size arrays.** `k`, `r` and `p` are `std::array`, so they are never "empty" — an unset matrix is all zeros. `cameraModelFromROS()` treats a zero `k[0]`/`p[0]` (the focal length) as absent.
+
+## License
+
+BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license).

rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h

@@ -71,76 +71,374 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define RCLCPP_QOS(queueSize, qos) rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos)
 #endif
 
+/**
+ * @namespace rtabmap_conversions
+ * @brief Conversions between RTAB-Map library types and ROS 2 messages.
+ *
+ * Naming is uniform throughout: `xxxFromROS()` converts a message into an RTAB-Map
+ * type and returns it by value, `xxxToROS()` writes an RTAB-Map type into a message
+ * passed by reference so the message can be reused.
+ *
+ * @note RTAB-Map distinguishes a *null* transform (unknown) from an identity one. On
+ *       the wire a null transform is encoded as an all-zero quaternion, so results of
+ *       the `transformFromXxx()` functions should be checked with
+ *       rtabmap::Transform::isNull() before use.
+ */
 namespace rtabmap_conversions {
 
-void transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTransform);
+//============================================================================
+// Transforms
+// Conversions between rtabmap::Transform and the tf2 / geometry_msgs representations.
+//============================================================================
+
+/**
+ * @brief Convert a rtabmap::Transform into a tf2::Transform.
+ * @param[in]  transform   the transform to convert
+ * @param[out] tfTransform the converted transform, or filled with NaN if @p transform is null
+ * @return false if @p transform is null, true otherwise
+ *
+ * @note tf2::Transform stores its rotation as a basis matrix and so cannot represent
+ *       the all-zero quaternion used elsewhere to mean "null". The null case is
+ *       reported through the return value instead, and the output is poisoned with
+ *       NaN so that ignoring that return value fails loudly rather than silently
+ *       proceeding with a plausible-looking identity.
+ * @see transformFromTF()
+ */
+bool transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTransform);
+
+/**
+ * @brief Convert a tf2::Transform into a rtabmap::Transform.
+ * @param transform the transform to convert
+ * @return the converted transform, or a null transform if @p transform contains NaN
+ *         (which is how transformToTF() reports a null transform)
+ * @see transformToTF()
+ */
 rtabmap::Transform transformFromTF(const tf2::Transform & transform);
 
+/**
+ * @brief Convert a rtabmap::Transform into a geometry_msgs Transform.
+ *
+ * The quaternion is normalized. A null @p transform is encoded as an all-zero
+ * quaternion, which transformFromGeometryMsg() decodes back to null.
+ *
+ * @param[in]  transform the transform to convert
+ * @param[out] msg       the converted message
+ */
 void transformToGeometryMsg(const rtabmap::Transform & transform, geometry_msgs::msg::Transform & msg);
+
+/**
+ * @brief Convert a geometry_msgs Transform into a rtabmap::Transform.
+ * @param msg the message to convert
+ * @return the converted transform, or a null transform if the quaternion is all zeros
+ */
 rtabmap::Transform transformFromGeometryMsg(const geometry_msgs::msg::Transform & msg);
 
+/**
+ * @brief Convert a rtabmap::Transform into a geometry_msgs Pose.
+ * @param[in]  transform the transform to convert
+ * @param[out] msg       the converted message; a null @p transform gives an all-zero orientation
+ */
 void transformToPoseMsg(const rtabmap::Transform & transform, geometry_msgs::msg::Pose & msg);
+
+/**
+ * @brief Convert a geometry_msgs Pose into a rtabmap::Transform.
+ * @param msg                    the message to convert
+ * @param ignoreRotationIfNotSet if true, an all-zero orientation yields a
+ *                               translation-only transform instead of a null one
+ * @return the converted transform, or a null transform if the orientation is all zeros
+ *         and @p ignoreRotationIfNotSet is false
+ *
+ * @warning geometry_msgs::msg::Quaternion defaults to `w = 1`, not all zeros, so a
+ *          default-constructed Pose is a valid identity rotation rather than "unset".
+ */
 rtabmap::Transform transformFromPoseMsg(const geometry_msgs::msg::Pose & msg, bool ignoreRotationIfNotSet = false);
 
+
+//============================================================================
+// Images
+// Extracting OpenCV images from RGBDImage messages, and building them back.
+//============================================================================
+
+/**
+ * @brief Extract the RGB and depth images of an RGBDImage message, copying the pixels.
+ *
+ * Handles both the raw (`rgb`, `depth`) and compressed (`rgb_compressed`,
+ * `depth_compressed`) fields. Both output pointers are always valid; they hold an
+ * empty image when the corresponding field is not set.
+ *
+ * @param[in]  image the message to read
+ * @param[out] rgb   the RGB image
+ * @param[out] depth the depth image
+ * @see toCvShare() to avoid the copy
+ */
 void toCvCopy(const rtabmap_msgs::msg::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth);
+
+/**
+ * @brief Extract the RGB and depth images of an RGBDImage message without copying.
+ *
+ * The returned images alias the message's buffers, so @p image must outlive them. Both
+ * output pointers are always valid; they hold an empty image when the corresponding
+ * field is not set.
+ *
+ * @param[in]  image the message to read; its shared pointer keeps the buffers alive
+ * @param[out] rgb   the RGB image
+ * @param[out] depth the depth image
+ */
 void toCvShare(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
+
+/**
+ * @brief Extract the RGB and depth images of an RGBDImage message without copying.
+ *
+ * Both output pointers are always valid; they hold an empty image when the corresponding
+ * field is not set.
+ *
+ * @param[in]  image         the message to read
+ * @param[in]  trackedObject object whose lifetime keeps the message buffers alive
+ * @param[out] rgb           the RGB image
+ * @param[out] depth         the depth image
+ */
 void toCvShare(const rtabmap_msgs::msg::RGBDImage & image, const std::shared_ptr<void const>& trackedObject, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
+
+/**
+ * @brief Fill an RGBDImage message from a SensorData.
+ *
+ * Supports a single RGB-D camera or a single stereo pair; multi-camera data cannot be
+ * represented by this message and is rejected with an error.
+ *
+ * @param[in]  data          the sensor data to convert
+ * @param[out] msg           the converted message, stamped with @p data's stamp
+ * @param[in]  sensorFrameId frame id stamped on the message and its sub-messages
+ *
+ * @note rtabmap::SensorData holds its stamp as a double, so the stamp written here is
+ *       only accurate to a few hundred nanoseconds at current epoch times and will not
+ *       compare equal to the ROS stamp the data originally came from. Callers that need
+ *       the exact original stamp assign `msg.header` after this call.
+ * @note Unlike infoToROS(), an already-stamped `msg.header` is overwritten rather than
+ *       kept: the same header is applied to every sub-message here, so preserving only
+ *       the top-level one would leave the message internally inconsistent.
+ */
 void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::RGBDImage & msg, const std::string & sensorFrameId);
+
+/**
+ * @brief Build a SensorData from an RGBDImage message.
+ *
+ * The stamp is taken from the top-level `image->header`, and the camera's local
+ * transform is not carried by the message (callers resolve it from TF).
+ *
+ * The depth image is optional: a message carrying only the color image and its camera
+ * info gives a SensorData with no depth, which is valid.
+ *
+ * @param image the message to convert
+ * @return the converted sensor data, empty (SensorData::isValid() false) if the message
+ *         carries no color image or an unsupported encoding
+ *
+ * @warning The returned SensorData does **not** copy the pixels: it points into the
+ *          message's own buffers. @p image must therefore outlive it and must not be
+ *          modified meanwhile. Deep-copy the images before letting the SensorData
+ *          escape a subscription callback, because the queue recycles the message as
+ *          soon as the callback returns.
+ */
 rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & image);
 
-// copy data
+
+//============================================================================
+// Compressed data
+//============================================================================
+
+/**
+ * @brief Copy an already-compressed cv::Mat into a byte vector.
+ * @param[in]  compressed a 1xN CV_8UC1 matrix of compressed bytes, or an empty matrix
+ * @param[out] bytes      the bytes; cleared when @p compressed is empty
+ */
 void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes);
+
+/**
+ * @brief Wrap a byte vector as a 1xN CV_8UC1 cv::Mat of compressed data.
+ * @param bytes the bytes to wrap
+ * @param copy  if false, the returned matrix aliases @p bytes, which must then outlive it
+ * @return the matrix, empty when @p bytes is empty
+ */
 cv::Mat compressedMatFromBytes(const std::vector<unsigned char> & bytes, bool copy = true);
 
+
+//============================================================================
+// Statistics
+//============================================================================
+
+/**
+ * @brief Read an Info message into RTAB-Map statistics.
+ * @param[in]  info the message to convert
+ * @param[out] stat the statistics, marked as extended
+ * @note The stamp comes from `info.header`, which infoToROS() does not set.
+ */
 void infoFromROS(const rtabmap_msgs::msg::Info & info, rtabmap::Statistics & stat);
+
+/**
+ * @brief Fill an Info message from RTAB-Map statistics.
+ * @param[in]  stats the statistics to convert
+ * @param[out] info  the converted message
+ * @note If the caller left `info.header.stamp` unset it is filled from @p stats, so that
+ *       infoFromROS() recovers a stamp. An already-stamped header is never overwritten:
+ *       rtabmap::Statistics holds its stamp as a double, so the value derived from it is
+ *       only accurate to a few hundred nanoseconds at current epoch times and will not
+ *       compare equal to the ROS stamp the data came from. Callers wanting the exact
+ *       input stamp — or a publication time unrelated to the data — stamp the header
+ *       themselves before or after this call.
+ * @warning The frame id is never set: rtabmap::Statistics does not carry one, so the
+ *          caller must always fill `info.header.frame_id` itself.
+ */
 void infoToROS(const rtabmap::Statistics & stats, rtabmap_msgs::msg::Info & info);
 
+
+//============================================================================
+// Features and landmarks
+// Keypoints, 2D/3D points, descriptors and environmental sensors.
+//============================================================================
+
+/** @brief Convert a Link message into a rtabmap::Link, including its 6x6 information matrix. */
 rtabmap::Link linkFromROS(const rtabmap_msgs::msg::Link & msg);
+/** @brief Fill a Link message from a rtabmap::Link. */
 void linkToROS(const rtabmap::Link & link, rtabmap_msgs::msg::Link & msg);
 
+/** @brief Convert a KeyPoint message into a cv::KeyPoint. */
 cv::KeyPoint keypointFromROS(const rtabmap_msgs::msg::KeyPoint & msg);
+/** @brief Fill a KeyPoint message from a cv::KeyPoint. */
 void keypointToROS(const cv::KeyPoint & kpt, rtabmap_msgs::msg::KeyPoint & msg);
 
+/** @brief Convert keypoint messages into a new vector of cv::KeyPoint. */
 std::vector<cv::KeyPoint> keypointsFromROS(const std::vector<rtabmap_msgs::msg::KeyPoint> & msg);
+
+/**
+ * @brief Append keypoint messages to an existing vector.
+ * @param[in]     msg    the messages to convert
+ * @param[in,out] kpts   vector the keypoints are appended to; existing content is kept
+ * @param[in]     xShift offset added to the x coordinate of every appended keypoint,
+ *                       used when several camera images are laid out side by side
+ */
 void keypointsFromROS(const std::vector<rtabmap_msgs::msg::KeyPoint> & msg, std::vector<cv::KeyPoint> & kpts, int xShift=0);
+
+/** @brief Fill keypoint messages from a vector of cv::KeyPoint. */
 void keypointsToROS(const std::vector<cv::KeyPoint> & kpts, std::vector<rtabmap_msgs::msg::KeyPoint> & msg);
 
+/** @brief Convert a GlobalDescriptor message, decompressing its data and info matrices. */
 rtabmap::GlobalDescriptor globalDescriptorFromROS(const rtabmap_msgs::msg::GlobalDescriptor & msg);
+/** @brief Fill a GlobalDescriptor message, compressing its data and info matrices. */
 void globalDescriptorToROS(const rtabmap::GlobalDescriptor & desc, rtabmap_msgs::msg::GlobalDescriptor & msg);
 
+/** @brief Convert global descriptor messages into RTAB-Map descriptors. */
 std::vector<rtabmap::GlobalDescriptor> globalDescriptorsFromROS(const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & msg);
+/** @brief Fill global descriptor messages; @p msg is cleared first. */
 void globalDescriptorsToROS(const std::vector<rtabmap::GlobalDescriptor> & desc, std::vector<rtabmap_msgs::msg::GlobalDescriptor> & msg);
 
+/** @brief Convert an EnvSensor message into a rtabmap::EnvSensor. */
 rtabmap::EnvSensor envSensorFromROS(const rtabmap_msgs::msg::EnvSensor & msg);
+/** @brief Fill an EnvSensor message from a rtabmap::EnvSensor. */
 void envSensorToROS(const rtabmap::EnvSensor & sensor, rtabmap_msgs::msg::EnvSensor & msg);
+/** @brief Convert EnvSensor messages into a map keyed by sensor type. */
 rtabmap::EnvSensors envSensorsFromROS(const std::vector<rtabmap_msgs::msg::EnvSensor> & msg);
+/** @brief Fill EnvSensor messages from a map of sensors; @p msg is cleared first. */
 void envSensorsToROS(const rtabmap::EnvSensors & sensors, std::vector<rtabmap_msgs::msg::EnvSensor> & msg);
 
+/** @brief Convert a Point2f message into a cv::Point2f. */
 cv::Point2f point2fFromROS(const rtabmap_msgs::msg::Point2f & msg);
+/** @brief Fill a Point2f message from a cv::Point2f. */
 void point2fToROS(const cv::Point2f & kpt, rtabmap_msgs::msg::Point2f & msg);
 
+/** @brief Convert Point2f messages into a vector of cv::Point2f. */
 std::vector<cv::Point2f> points2fFromROS(const std::vector<rtabmap_msgs::msg::Point2f> & msg);
+/** @brief Fill Point2f messages from a vector of cv::Point2f. */
 void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_msgs::msg::Point2f> & msg);
 
+/** @brief Convert a Point3f message into a cv::Point3f. */
 cv::Point3f point3fFromROS(const rtabmap_msgs::msg::Point3f & msg);
+/** @brief Fill a Point3f message from a cv::Point3f. */
 void point3fToROS(const cv::Point3f & kpt, rtabmap_msgs::msg::Point3f & msg);
 
+/**
+ * @brief Convert Point3f messages into a vector of cv::Point3f.
+ * @param msg       the messages to convert
+ * @param transform applied to every point; ignored when null or identity
+ * @return the converted points
+ */
 std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_msgs::msg::Point3f> & msg, const rtabmap::Transform & transform = rtabmap::Transform());
+
+/**
+ * @brief Append Point3f messages to an existing vector.
+ * @param[in]     msg       the messages to convert
+ * @param[in,out] points3   vector the points are appended to; existing content is kept
+ * @param[in]     transform applied to every appended point; ignored when null or identity
+ */
 void points3fFromROS(const std::vector<rtabmap_msgs::msg::Point3f> & msg, std::vector<cv::Point3f> & points3, const rtabmap::Transform & transform = rtabmap::Transform());
+
+/**
+ * @brief Fill Point3f messages from a vector of cv::Point3f.
+ * @param[in]  kpts      the points to convert
+ * @param[out] msg       the converted messages
+ * @param[in]  transform applied to every point; ignored when null or identity
+ */
 void points3fToROS(const std::vector<cv::Point3f> & kpts, std::vector<rtabmap_msgs::msg::Point3f> & msg, const rtabmap::Transform & transform = rtabmap::Transform());
 
+
+//============================================================================
+// Camera models
+//============================================================================
+
+/**
+ * @brief Convert a CameraInfo message into a rtabmap::CameraModel.
+ *
+ * Fisheye/equidistant distortion (4 coefficients) is repacked into RTAB-Map's 1x6
+ * layout. A projection matrix means the model describes an already-rectified image.
+ *
+ * @param camInfo        the message to convert
+ * @param localTransform transform from the base frame to the optical frame
+ * @return the converted model
+ *
+ * @note `k`, `r` and `p` are fixed-size arrays and so are never empty. An unset matrix
+ *       is all zeros, which is detected through the focal length (`k[0]` / `p[0]`).
+ */
 rtabmap::CameraModel cameraModelFromROS(
 		const sensor_msgs::msg::CameraInfo & camInfo,
 		const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity());
+
+/**
+ * @brief Fill a CameraInfo message from a rtabmap::CameraModel.
+ *
+ * A model carrying a projection matrix describes a rectified image, so zero distortion
+ * is reported for it. Without one, `P` is synthesized as `[K | 0]` and the raw
+ * distortion coefficients are emitted (`equidistant` for a 1x6 fisheye matrix,
+ * `rational_polynomial` above 5 coefficients, `plumb_bob` otherwise).
+ *
+ * @param[in]  model   the model to convert
+ * @param[out] camInfo the converted message; the header is not set
+ */
 void cameraModelToROS(
 		const rtabmap::CameraModel & model,
 		sensor_msgs::msg::CameraInfo & camInfo);
 
+/**
+ * @brief Build a stereo model from a pair of CameraInfo messages.
+ * @param leftCamInfo     left camera info
+ * @param rightCamInfo    right camera info; the baseline is read from its `P(0,3)`
+ * @param localTransform  transform from the base frame to the left optical frame
+ * @param stereoTransform explicit left-to-right transform, when not encoded in `P`
+ * @return the converted model
+ */
 rtabmap::StereoCameraModel stereoCameraModelFromROS(
 		const sensor_msgs::msg::CameraInfo & leftCamInfo,
 		const sensor_msgs::msg::CameraInfo & rightCamInfo,
 		const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity(),
 		const rtabmap::Transform & stereoTransform = rtabmap::Transform());
+
+/**
+ * @brief Build a stereo model, resolving the local transform from TF.
+ * @param leftCamInfo      left camera info
+ * @param rightCamInfo     right camera info
+ * @param frameId          base frame the model's local transform is expressed in
+ * @param tfBuffer         must contain @p frameId -> the left camera info's frame at
+ *                         its stamp
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @return the converted model, invalid if the transform could not be resolved
+ */
 rtabmap::StereoCameraModel stereoCameraModelFromROS(
 		const sensor_msgs::msg::CameraInfo & leftCamInfo,
 		const sensor_msgs::msg::CameraInfo & rightCamInfo,
@@ -148,12 +446,34 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS(
 		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform);
 
+
+//============================================================================
+// Map graph
+// Poses, links, nodes and sensor data — the map serialization path.
+//============================================================================
+
+/**
+ * @brief Read a MapData message into poses, links and signatures.
+ * @param[in]  msg        the message to convert
+ * @param[out] poses      optimized poses by node id
+ * @param[out] links      constraints, keyed by their originating node id
+ * @param[out] signatures node data by node id
+ * @param[out] mapToOdom  transform from the map frame to the odometry frame
+ */
 void mapDataFromROS(
 		const rtabmap_msgs::msg::MapData & msg,
 		std::map<int, rtabmap::Transform> & poses,
 		std::multimap<int, rtabmap::Link> & links,
 		std::map<int, rtabmap::Signature> & signatures,
 		rtabmap::Transform & mapToOdom);
+/**
+ * @brief Fill a MapData message from poses, links and signatures.
+ * @param[in]  poses      optimized poses by node id
+ * @param[in]  links      constraints
+ * @param[in]  signatures node data by node id
+ * @param[in]  mapToOdom  transform from the map frame to the odometry frame
+ * @param[out] msg        the converted message; the header is not set
+ */
 void mapDataToROS(
 		const std::map<int, rtabmap::Transform> & poses,
 		const std::multimap<int, rtabmap::Link> & links,
@@ -161,40 +481,159 @@ void mapDataToROS(
 		const rtabmap::Transform & mapToOdom,
 		rtabmap_msgs::msg::MapData & msg);
 
+/**
+ * @brief Read a MapGraph message into poses and links.
+ * @param[in]  msg       the message to convert
+ * @param[out] poses     optimized poses by node id
+ * @param[out] links     constraints, keyed by their originating node id
+ * @param[out] mapToOdom transform from the map frame to the odometry frame
+ */
 void mapGraphFromROS(
 		const rtabmap_msgs::msg::MapGraph & msg,
 		std::map<int, rtabmap::Transform> & poses,
 		std::multimap<int, rtabmap::Link> & links,
 		rtabmap::Transform & mapToOdom);
+/**
+ * @brief Fill a MapGraph message from poses and links.
+ * @param[in]  poses     optimized poses by node id
+ * @param[in]  links     constraints
+ * @param[in]  mapToOdom transform from the map frame to the odometry frame
+ * @param[out] msg       the converted message; the header is not set
+ */
 void mapGraphToROS(
 		const std::map<int, rtabmap::Transform> & poses,
 		const std::multimap<int, rtabmap::Link> & links,
 		const rtabmap::Transform & mapToOdom,
 		rtabmap_msgs::msg::MapGraph & msg);
 
+/**
+ * @brief Convert a SensorData message into a rtabmap::SensorData.
+ * @param msg the message to convert
+ * @return the converted sensor data
+ * @note `ground_truth_pose` is not read here; nodeFromROS() owns that field.
+ */
 rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg);
+
+/**
+ * @brief Fill a SensorData message from a rtabmap::SensorData.
+ * @param[in]  signature   the sensor data to convert
+ * @param[out] msg         the converted message
+ * @param[in]  frameId     frame id stamped on the message
+ * @param[in]  copyRawData also serialize the uncompressed images and laser scan, which
+ *                         is significantly larger on the wire
+ */
 void sensorDataToROS(const rtabmap::SensorData & signature, rtabmap_msgs::msg::SensorData & msg, const std::string & frameId = "base_link", bool copyRawData = false);
 
+/**
+ * @brief Convert a Node message into a rtabmap::Signature, with its data and visual words.
+ * @param msg the message to convert
+ * @return the converted signature
+ */
 rtabmap::Signature nodeFromROS(const rtabmap_msgs::msg::Node & msg);
+
+/**
+ * @brief Fill a Node message from a rtabmap::Signature.
+ * @param[in]  signature the signature to convert
+ * @param[out] msg       the converted message
+ */
 void nodeToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg);
 
-// DEPRECATED
+/** @deprecated Use nodeFromROS() instead. */
 rtabmap::Signature nodeDataFromROS(const rtabmap_msgs::msg::Node & msg);
+/** @deprecated Use nodeToROS() instead. */
 void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg);
 
+/** @brief Convert only the node's metadata (id, map id, weight, stamp, label, pose). */
 rtabmap::Signature nodeInfoFromROS(const rtabmap_msgs::msg::Node & msg);
+/** @brief Fill only the node's metadata (id, map id, weight, stamp, label, pose). */
 void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg);
 
+
+//============================================================================
+// Odometry
+//============================================================================
+
+/**
+ * @brief Format odometry info as the `Odometry/...` statistics published with the map.
+ * @param info the odometry info to summarize
+ * @return statistic name (with its unit) to value
+ * @note The covariance-derived entries are omitted when `info.reg.covariance` is not a
+ *       6x6 CV_64FC1 matrix, which is the case for a default-constructed OdometryInfo.
+ */
 std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo & info);
+
+/**
+ * @brief Convert an OdomInfo message into a rtabmap::OdometryInfo.
+ * @param msg        the message to convert
+ * @param ignoreData skip the heavy members (words, local map, correspondences)
+ * @return the converted odometry info
+ */
 rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_msgs::msg::OdomInfo & msg, bool ignoreData = false);
+
+/**
+ * @brief Fill an OdomInfo message from a rtabmap::OdometryInfo.
+ * @param[in]  info       the odometry info to convert
+ * @param[out] msg        the converted message
+ * @param[in]  ignoreData skip the heavy members (words, local map, correspondences)
+ */
 void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_msgs::msg::OdomInfo & msg, bool ignoreData = false);
 
+
+//============================================================================
+// User data, IMU and landmarks
+//============================================================================
+
+/**
+ * @brief Extract the payload of a UserData message.
+ * @param dataMsg the message to read
+ * @return the payload; still compressed when the message was written with compression,
+ *         in which case the caller applies rtabmap::uncompressData()
+ */
 cv::Mat userDataFromROS(const rtabmap_msgs::msg::UserData & dataMsg);
+
+/**
+ * @brief Fill a UserData message.
+ * @param[in]  data     the payload
+ * @param[out] dataMsg  the converted message
+ * @param[in]  compress compress the payload, which is then carried as a 1xN byte blob
+ */
 void userDataToROS(const cv::Mat & data, rtabmap_msgs::msg::UserData & dataMsg, bool compress);
 
+/**
+ * @brief Convert an Imu message into a rtabmap::IMU.
+ * @param msg            the message to convert
+ * @param localTransform transform from the base frame to the IMU frame
+ * @return the converted IMU sample, with its three covariance matrices
+ */
 rtabmap::IMU imuFromROS(const sensor_msgs::msg::Imu & msg, const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity());
+
+/**
+ * @brief Fill an Imu message from a rtabmap::IMU.
+ * @param[in]  imu the IMU sample to convert
+ * @param[out] msg the converted message; the header is not set
+ */
 void imuToROS(const rtabmap::IMU & imu, sensor_msgs::msg::Imu & msg);
 
+/**
+ * @brief Convert tag/landmark detections into RTAB-Map landmarks, expressed in @p frameId.
+ *
+ * Each detection is transformed from its own frame into @p frameId, then corrected for
+ * the odometry motion between @p odomStamp and the detection's stamp.
+ *
+ * @param tags               detections by landmark id, each paired with its tag size;
+ *                           ids must be > 0, others are dropped with an error
+ * @param frameId            base frame the landmarks are expressed in
+ * @param odomFrameId        fixed frame used for the odometry correction; when empty
+ *                           no correction is applied
+ * @param odomStamp          stamp the landmarks should be synchronized to
+ * @param tfBuffer           must contain @p frameId -> each detection's frame at that
+ *                           detection's stamp, and, when @p odomFrameId is set,
+ *                           @p odomFrameId -> @p frameId covering both stamps
+ * @param waitForTransform   seconds to wait for TF, 0 to not wait
+ * @param defaultLinVariance linear variance used when a detection carries no covariance
+ * @param defaultAngVariance angular variance used when a detection carries no covariance
+ * @return the landmarks, keyed by id
+ */
 rtabmap::Landmarks landmarksFromROS(
 		const std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > & tags,
 		const std::string & frameId,
@@ -205,10 +644,43 @@ rtabmap::Landmarks landmarksFromROS(
 		double defaultLinVariance,
 		double defaultAngVariance);
 
-inline double timestampFromROS(const rclcpp::Time & stamp) {return stamp.seconds();}
-inline rclcpp::Time timestampToROS(const double & t) {int32_t sec= (int32_t)floor(t); return rclcpp::Time(sec, (uint32_t)std::round((t-sec) * 1e9));}
 
-// common stuff
+//============================================================================
+// Timestamps
+//============================================================================
+
+/**
+ * @brief Convert a ROS time into seconds.
+ * @note A double holds about 15-16 significant digits, so at current epoch times
+ *       (~1.7e9 s) it resolves to roughly 400 ns. Converting back with timestampToROS()
+ *       therefore does not reproduce the original stamp exactly, and the rounding can
+ *       carry into the seconds field. Compare converted stamps with a tolerance, and
+ *       keep the original rclcpp::Time whenever exactness matters.
+ */
+inline double timestampFromROS(const rclcpp::Time & stamp) {return stamp.seconds();}
+/**
+ * @brief Convert seconds into a ROS time.
+ * @note The result uses RCL_ROS_TIME, matching how message header stamps convert. The
+ *       rclcpp::Time(sec, nsec) constructor defaults to RCL_SYSTEM_TIME instead, and
+ *       comparing times of different clock types throws.
+ */
+inline rclcpp::Time timestampToROS(const double & t) {int32_t sec= (int32_t)floor(t); return rclcpp::Time(sec, (uint32_t)std::round((t-sec) * 1e9), RCL_ROS_TIME);}
+
+
+//============================================================================
+// TF lookups
+//============================================================================
+
+/**
+ * @brief Look a static relationship between two frames up in TF.
+ * @param fromFrameId      the reference frame
+ * @param toFrameId        the target frame
+ * @param stamp            time of the lookup
+ * @param tfBuffer         buffer to query
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @return the transform, or a null transform if the lookup failed (which is logged
+ *         rather than thrown)
+ */
 rtabmap::Transform getTransform(
 		const std::string & fromFrameId,
 		const std::string & toFrameId,
@@ -216,9 +688,19 @@ rtabmap::Transform getTransform(
 		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform);
 
-
-// get moving transform accordingly to a fixed frame. For example get
-// transform of /base_link between two stamps accordingly to /odom frame.
+/**
+ * @brief Measure how a frame moved between two stamps, relative to a fixed frame.
+ *
+ * For example, the motion of `base_link` between two stamps as seen from `odom`.
+ *
+ * @param movingFrame      the frame whose motion is measured
+ * @param fixedFrame       the frame the motion is measured against
+ * @param stampFrom        start of the interval
+ * @param stampTo          end of the interval
+ * @param tfBuffer         buffer to query
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @return the motion, or a null transform if the lookup failed
+ */
 rtabmap::Transform getMovingTransform(
 		const std::string & movingFrame,
 		const std::string & fixedFrame,
@@ -227,6 +709,54 @@ rtabmap::Transform getMovingTransform(
 		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform);
 
+
+//============================================================================
+// Sensor message conversion
+// Assembling RGB-D, stereo and laser scan messages into RTAB-Map inputs.
+//============================================================================
+
+/**
+ * @brief Assemble one or more RGB-D (or RGB + right) camera streams into RTAB-Map inputs.
+ *
+ * With several cameras the images are concatenated horizontally into a single wide
+ * image and one model is produced per camera. Whether the second image is treated as a
+ * depth map or as the right image of a stereo pair is inferred from its encoding, and
+ * for `mono16` from whether the camera infos carry a baseline in `P(0,3)`.
+ *
+ * @param imageMsgs            RGB (or left) images, one per camera; may be empty
+ * @param depthMsgs            depth (or right) images, one per camera; may be empty
+ * @param cameraInfoMsgs       camera infos, one per camera; must not be empty
+ * @param depthCameraInfoMsgs  camera infos of the depth/right cameras; may be empty
+ * @param frameId              base frame the local transforms are expressed in
+ * @param odomFrameId          fixed frame the robot motion is measured against, used to
+ *                             re-express each camera pose relative to the base frame at
+ *                             @p odomStamp; empty to skip that correction entirely
+ * @param odomStamp            stamp the data is synchronized to
+ * @param[out] rgb             the assembled RGB (or left) image
+ * @param[out] depth           the assembled depth (or right) image
+ * @param[out] cameraModels    one model per camera, when the input is RGB-D
+ * @param[out] stereoCameraModels one model per camera, when the input is stereo
+ * @param tfBuffer             must contain @p frameId -> each camera's optical frame at
+ *                             that camera's stamp, and, when @p odomFrameId is set,
+ *                             @p odomFrameId -> @p frameId covering both @p odomStamp
+ *                             and the camera stamps
+ * @param waitForTransform     seconds to wait for TF, 0 to not wait
+ * @param alreadRectifiedImages whether the images are already rectified
+ * @param localKeyPointsMsgs   optional per-camera keypoints to merge
+ * @param localPoints3dMsgs    optional per-camera 3D points to merge
+ * @param localDescriptorsMsgs optional per-camera descriptors to merge
+ * @param[out] localKeyPoints  merged keypoints, shifted to the concatenated image
+ * @param[out] localPoints3d   merged 3D points
+ * @param[out] localDescriptors merged descriptors
+ * @return false on an unsupported encoding or a missing camera local transform
+ *
+ * @note The odometry correction is applied per camera, using each camera's own stamp,
+ *       and only when it differs from @p odomStamp. If that lookup fails the function
+ *       warns and carries on with an uncorrected pose — unlike a missing camera local
+ *       transform, which is fatal and returns false.
+ * @note A camera's RGB and depth stamps are assumed to be equal. Should they differ,
+ *       the depth stamp is the one used, since the geometry is what gets synchronized.
+ */
 bool convertRGBDMsgs(
 		const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
 		const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
@@ -249,6 +779,34 @@ bool convertRGBDMsgs(
 		std::vector<cv::Point3f> * localPoints3d = 0,
 		cv::Mat * localDescriptors = 0);
 
+/**
+ * @brief Convert a stereo pair into RTAB-Map inputs.
+ *
+ * The left image keeps its color; the right image is always reduced to mono.
+ *
+ * @param leftImageMsg     left image
+ * @param rightImageMsg    right image
+ * @param leftCamInfoMsg   left camera info
+ * @param rightCamInfoMsg  right camera info; the baseline is read from its `P(0,3)`
+ * @param frameId          base frame the local transform is expressed in
+ * @param odomFrameId      fixed frame the robot motion is measured against, used to
+ *                         re-express the camera pose relative to the base frame at
+ *                         @p odomStamp; empty to skip that correction entirely
+ * @param odomStamp        stamp the data is synchronized to
+ * @param[out] left        the left image
+ * @param[out] right       the right image, as mono
+ * @param[out] stereoModel the stereo model
+ * @param tfBuffer         must contain @p frameId -> the left image's frame at the left
+ *                         image stamp, and, when @p odomFrameId is set,
+ *                         @p odomFrameId -> @p frameId covering both stamps
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @param alreadyRectified whether the images are already rectified
+ * @return false on an unsupported encoding or a missing local transform
+ *
+ * @note The odometry correction is applied only when the left image stamp differs from
+ *       @p odomStamp. A failed correction lookup warns and leaves the pose uncorrected;
+ *       a missing local transform is fatal and returns false.
+ */
 bool convertStereoMsg(
 		const cv_bridge::CvImageConstPtr& leftImageMsg,
 		const cv_bridge::CvImageConstPtr& rightImageMsg,
@@ -264,6 +822,36 @@ bool convertStereoMsg(
 		double waitForTransform,
 		bool alreadyRectified);
 
+/**
+ * @brief Convert a 2D LaserScan into a rtabmap::LaserScan.
+ * @param scan2dMsg        the scan to convert
+ * @param frameId          base frame the scan's local transform is expressed in
+ * @param odomFrameId      fixed frame the robot motion is measured against, used to
+ *                         re-express the scan pose relative to the base frame at
+ *                         @p odomStamp; empty to skip that correction entirely
+ * @param odomStamp        stamp the scan is synchronized to
+ * @param[out] scan        the converted scan
+ * @param tfBuffer         must contain @p frameId -> the laser frame at the scan stamp,
+ *                         and, to deskew, the laser frame relative to @p odomFrameId
+ *                         across the whole sweep, since the points are projected through
+ *                         it
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @param outputInFrameId  express the points in @p frameId rather than the laser frame
+ * @return false if the scan is malformed (zero angle increment, inverted range or angle
+ *         bounds) or if a required transform is missing
+ *
+ * @note Unlike convertScan3dMsg(), this deskews the scan itself: the points are
+ *       projected with laser_geometry, which transforms each ray at its own time using
+ *       @p scan2dMsg.time_increment. That only corrects for motion if the projection
+ *       target is a fixed frame, i.e. if @p odomFrameId is set — with it empty the
+ *       target is @p frameId, which does not move relative to itself. This is also why
+ *       the laser frame must be known across the whole sweep, which the function checks
+ *       up front. When it is not known relative to @p odomFrameId -- odometry not
+ *       published on TF -- the scan is converted as with @p odomFrameId empty, neither
+ *       deskewed nor synchronized, with a warning shown once, rather than refused.
+ * @note The odometry correction is applied only when the scan stamp differs from
+ *       @p odomStamp; a failed correction lookup warns and leaves the pose uncorrected.
+ */
 bool convertScanMsg(
 		const sensor_msgs::msg::LaserScan & scan2dMsg,
 		const std::string & frameId,
@@ -274,6 +862,32 @@ bool convertScanMsg(
 		double waitForTransform,
 		bool outputInFrameId = false);
 
+/**
+ * @brief Convert a PointCloud2 into a rtabmap::LaserScan.
+ * @param scan3dMsg        the cloud to convert
+ * @param frameId          base frame the scan's local transform is expressed in
+ * @param odomFrameId      fixed frame the robot motion is measured against, used to
+ *                         re-express the scan pose relative to the base frame at
+ *                         @p odomStamp; empty to skip that correction entirely
+ * @param odomStamp        stamp the scan is synchronized to
+ * @param[out] scan        the converted scan
+ * @param tfBuffer         must contain @p frameId -> the cloud's frame at the cloud
+ *                         stamp, and, when @p odomFrameId is set, @p odomFrameId ->
+ *                         @p frameId covering both stamps
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @param maxPoints        downsample to at most this many points, 0 for no limit
+ * @param maxRange         drop points beyond this range, 0 for no limit
+ * @param is2D             treat the cloud as planar
+ * @return false if the local transform could not be resolved
+ *
+ * @note The cloud is assumed to be already deskewed. A single rigid transform is applied
+ *       to the whole cloud, so any motion during the sweep is preserved as-is; call
+ *       deskew() on the message first if the sensor was moving. This is unlike
+ *       convertScanMsg(), which deskews 2D scans itself through laser_geometry.
+ * @note The odometry correction is applied only when the cloud stamp differs from
+ *       @p odomStamp; a failed correction lookup warns and leaves the pose uncorrected.
+ * @see deskew()
+ */
 bool convertScan3dMsg(
 		const sensor_msgs::msg::PointCloud2 & scan3dMsg,
 		const std::string & frameId,
@@ -286,6 +900,29 @@ bool convertScan3dMsg(
 		float maxRange = 0.0f,
 		bool is2D = false);
 
+
+//============================================================================
+// Point cloud utilities
+//============================================================================
+
+/**
+ * @brief Deskew a point cloud using TF.
+ *
+ * Corrects each point for the sensor motion during the sweep, using the per-point time
+ * channel (`t`, `time`, `stamps` or `timestamp`). See the other overload for how that
+ * channel is interpreted.
+ *
+ * @param input            the cloud to deskew
+ * @param[out] output      the deskewed cloud, expressed in the frame at input's header stamp
+ * @param fixedFrameId     frame the sensor motion is measured against
+ * @param tfBuffer         must contain the cloud's own frame relative to
+ *                         @p fixedFrameId across the whole sweep
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @param slerp            interpolate between the sweep's two end poses instead of
+ *                         looking TF up for every point; one query instead of N, at the
+ *                         cost of linearizing the motion across the sweep
+ * @return false if the cloud has no usable time channel or a lookup failed
+ */
 bool deskew(
 		const sensor_msgs::msg::PointCloud2 & input,
 		sensor_msgs::msg::PointCloud2 & output,
@@ -294,21 +931,49 @@ bool deskew(
 		double waitForTransform,
 		bool slerp = false);
 
+/**
+ * @brief Deskew a point cloud using a constant velocity model.
+ *
+ * The per-point time channel may be named `t`, `time`, `stamps` or `timestamp`. Its
+ * datatype decides how it is read: `UINT32` (nanoseconds) and `FLOAT32` (seconds) are
+ * *offsets from the message header stamp*, while `FLOAT64` carries *absolute* stamps,
+ * with milliseconds/microseconds/nanoseconds detected automatically by magnitude.
+ *
+ * On success the channel is zeroed to mark the cloud as deskewed, so calling this again
+ * on the same cloud is a no-op that returns true rather than an error.
+ *
+ * @param input        cloud with a per-point time channel
+ * @param[out] output  deskewed cloud, expressed in the frame at input's header stamp
+ * @param velocity     twist of the sensor frame (m/s and rad/s)
+ * @return false if the cloud has no usable time channel or @p velocity is null
+ */
 bool deskew(
 		const sensor_msgs::msg::PointCloud2 & input,
 		sensor_msgs::msg::PointCloud2 & output,
-		double previousStamp,
 		const rtabmap::Transform & velocity);
 
-// Missing function in ros2 (from old pcl_ros)
+/**
+ * @brief Apply a rigid transform to the XYZ fields of a point cloud.
+ *
+ * Missing function in ROS 2, taken from the old pcl_ros.
+ *
+ * @param transform the transform to apply
+ * @param in        the cloud to transform
+ * @param[out] out  the transformed cloud; all other fields are copied unchanged
+ */
 void transformPointCloud (
 		const Eigen::Matrix4f &transform,
 		const sensor_msgs::msg::PointCloud2 &in,
         sensor_msgs::msg::PointCloud2 &out);
 
-/** Return the size of a datatype (which is an enum of sensor_msgs::PointField::) in bytes
- * @param datatype one of the enums of sensor_msgs::PointField::
- * Note: Missing function in ros2 (from old pcl_ros)
+/**
+ * @brief Return the size in bytes of a PointField datatype.
+ *
+ * Missing function in ROS 2, taken from the old pcl_ros.
+ *
+ * @param datatype one of the sensor_msgs::msg::PointField enums
+ * @return the size in bytes
+ * @throws std::runtime_error if @p datatype is not a known PointField type
  */
 inline int sizeOfPointField(int datatype)
 {
@@ -330,6 +995,13 @@ inline int sizeOfPointField(int datatype)
   return -1;
 }
 
+/**
+ * @brief Find the entry of a map whose key is closest to @p key.
+ * @param buffer the map to search; must not be empty
+ * @param key    the key to look for
+ * @return iterator to the closest entry, clamped to the first or last one when @p key
+ *         falls outside the map's range
+ */
 template <typename K, typename V>
 typename std::map<K, V>::const_iterator getClosestIterator(
 	const std::map<K, V> & buffer,
@@ -365,6 +1037,7 @@ typename std::map<K, V>::const_iterator getClosestIterator(
 	return iterB;
 }
 
+
 }
 
 #endif /* MSGCONVERSION_H_ */

rtabmap_conversions/include/rtabmap_conversions/PointCloudConversion.h

@@ -0,0 +1,75 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_CONVERSIONS_POINTCLOUDCONVERSION_H_
+#define RTABMAP_CONVERSIONS_POINTCLOUDCONVERSION_H_
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+/**
+ * @file
+ * @brief pcl::toROSMsg and pcl::fromROSMsg, minus their empty-cloud crash.
+ *
+ * Both take the address of the first point, and of the first byte of the output, before
+ * checking that there is one (see pcl/conversions.h): for an empty cloud that indexes
+ * past the end of an empty vector. Nothing notices while the standard library does not
+ * check, which is why it went unseen for years -- Ubuntu enables those checks from
+ * resolute on, and then the process aborts outright.
+ *
+ * An empty cloud is ordinary here rather than exceptional: a scan whose points were all
+ * filtered out, a frame with no obstacles in it, an occupancy grid with nothing new. Each
+ * of those still has to be published, so the conversions are used through this.
+ */
+
+namespace rtabmap_conversions {
+
+/**
+ * @brief @p cloud as a PointCloud2 message.
+ *
+ * An empty cloud is converted as a single point and emptied afterwards, so the message
+ * still carries the field layout the installed PCL would have given it.
+ */
+template<typename PointT>
+void toPointCloud2Msg(
+		const pcl::PointCloud<PointT> & cloud, sensor_msgs::msg::PointCloud2 & msg)
+{
+	if(!cloud.empty())
+	{
+		pcl::toROSMsg(cloud, msg);
+		return;
+	}
+
+	pcl::PointCloud<PointT> onePoint;
+	onePoint.header = cloud.header;
+	onePoint.is_dense = cloud.is_dense;
+	onePoint.push_back(PointT());
+	pcl::toROSMsg(onePoint, msg);
+	msg.width = 0;
+	msg.height = 1;
+	msg.row_step = 0;
+	msg.data.clear();
+}
+
+/// @brief @p msg as a point cloud, an empty message included.
+template<typename PointT>
+void fromPointCloud2Msg(
+		const sensor_msgs::msg::PointCloud2 & msg, pcl::PointCloud<PointT> & cloud)
+{
+	if(msg.data.empty())
+	{
+		cloud.clear();
+		cloud.is_dense = msg.is_dense;
+		pcl_conversions::toPCL(msg.header, cloud.header);
+		return;
+	}
+	pcl::fromROSMsg(msg, cloud);
+}
+
+}  // namespace rtabmap_conversions
+
+#endif /* RTABMAP_CONVERSIONS_POINTCLOUDCONVERSION_H_ */

rtabmap_conversions/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_conversions</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's conversions package. This package can be used to convert rtabmap_msgs's msgs into RTAB-Map's library objects.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -30,7 +30,10 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
+
   <export>
 	<build_type>ament_cmake</build_type>
+	<rosdoc2>rosdoc2.yaml</rosdoc2>
   </export>
 </package>

rtabmap_conversions/rosdoc2.yaml

@@ -0,0 +1,35 @@
+## Configuration for rosdoc2, the documentation generator used by docs.ros.org.
+## Regenerate the annotated default with:
+##   rosdoc2 default_config --package-path rtabmap_conversions
+## Build the docs locally with:
+##   rosdoc2 build --package-path rtabmap_conversions --output-directory doc_output
+
+## This 'attic section' self-documents this file's type and version.
+type: 'rosdoc2 config'
+version: 1
+
+---
+
+settings:
+    ## Generate the standard index page from package.xml (description, maintainer,
+    ## license, links) and a table of contents for the builders below.
+    generate_package_index: true
+
+    ## This is an ament_cmake package, so doxygen runs on the public headers by
+    ## default and there are no Python modules to document.
+    always_run_doxygen: false
+    always_run_sphinx_apidoc: false
+
+builders:
+    ## Doxygen parses the public C++ API out of include/.
+    - doxygen: {
+        name: 'rtabmap_conversions Public C/C++ API',
+        output_dir: 'generated/doxygen'
+    }
+    ## Sphinx renders the landing page and pulls the Doxygen XML in through
+    ## breathe/exhale so the API is browsable alongside the narrative docs.
+    - sphinx: {
+        name: 'rtabmap_conversions',
+        doxygen_xml_directory: 'generated/doxygen/xml',
+        output_dir: ''
+    }

rtabmap_conversions/src/MsgConversion.cpp

@@ -25,8 +25,12 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <rtabmap_conversions/PointCloudConversion.h>
 #include "rtabmap_conversions/MsgConversion.h"
 
+#include <cmath>
+#include <limits>
+
 #include <opencv2/highgui/highgui.hpp>
 #include <zlib.h>
 #include "rclcpp/rclcpp.hpp"
@@ -60,21 +64,46 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap_conversions {
 
-void transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTransform)
+bool transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTransform)
 {
-	if(!transform.isNull())
+	if(transform.isNull())
 	{
-		geometry_msgs::msg::TransformStamped gm = tf2::eigenToTransform(transform.toEigen3d());
-		//tf2::fromMsg(gm, tfTransform);
-	}
-	else
-	{
-		tfTransform = tf2::Transform(tf2::Quaternion(0,0,0,0));
+		// tf2::Transform cannot represent a null transform: it stores its rotation as a
+		// basis matrix, so there is no equivalent of the all-zero quaternion used by the
+		// geometry_msgs conversions. Fill it with NaN so that a caller ignoring the
+		// return value corrupts its results loudly instead of silently carrying on with
+		// an identity that looks legitimate.
+		const tf2Scalar nan = std::numeric_limits<tf2Scalar>::quiet_NaN();
+		tfTransform = tf2::Transform(
+				tf2::Matrix3x3(nan, nan, nan, nan, nan, nan, nan, nan, nan),
+				tf2::Vector3(nan, nan, nan));
+		return false;
 	}
+
+	geometry_msgs::msg::Transform msg;
+	transformToGeometryMsg(transform, msg);
+	tf2::fromMsg(msg, tfTransform);
+	return true;
 }
 
 rtabmap::Transform transformFromTF(const tf2::Transform & transform)
 {
+	// transformToTF() poisons its output with NaN for a null transform, as tf2::Transform
+	// has no null representation of its own. Map that back to a null transform here so the
+	// two functions round-trip, and so a NaN coming from anywhere else does not silently
+	// propagate into the rest of the pipeline.
+	const tf2::Vector3 & origin = transform.getOrigin();
+	const tf2::Matrix3x3 & basis = transform.getBasis();
+	bool nan = std::isnan(origin.x()) || std::isnan(origin.y()) || std::isnan(origin.z());
+	for(int i=0; !nan && i<3; ++i)
+	{
+		nan = std::isnan(basis[i].x()) || std::isnan(basis[i].y()) || std::isnan(basis[i].z());
+	}
+	if(nan)
+	{
+		return rtabmap::Transform();
+	}
+
 	Eigen::Isometry3d eigenTf;
 	geometry_msgs::msg::Transform gm = tf2::toMsg(transform);
 	eigenTf = tf2::transformToEigen(gm);
@@ -227,6 +256,11 @@ void toCvShare(const rtabmap_msgs::msg::RGBDImage & image, const std::shared_ptr
 				depth = ptr;
 			}
 		}
+		else
+		{
+			// empty
+			depth = std::make_shared<cv_bridge::CvImage>();
+		}
 	}
 	catch(cv::Exception& e) {
 		UFATAL("Fatal error while converting rgbd image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what());
@@ -244,6 +278,7 @@ void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::RGBDIma
 		UERROR("Cannot convert multi-camera data to rgbd image");
 		return;
 	}
+	msg.header = header;
 	if(data.cameraModels().size() == 1)
 	{
 		//rgb+depth
@@ -403,7 +438,11 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSh
 		int depthWidth = depthMsg->image.cols;
 		int depthHeight = depthMsg->image.rows;
 
+		// The depth image is optional: a message can legitimately carry only the color
+		// image and its camera info. Compare the resolutions only when there is a depth
+		// image, otherwise the ratios divide by zero.
 		UASSERT_MSG(
+			depthMsg->image.empty() ||
 			imageWidth/depthWidth == imageHeight/depthHeight,
 			uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
 
@@ -419,7 +458,8 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSh
 			 imageMsg->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
 			 imageMsg->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
 			 imageMsg->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
-			!(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+			!(depthMsg->image.empty() ||
+			 depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
 			 depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
 			 depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
 		{
@@ -558,6 +598,13 @@ void infoFromROS(const rtabmap_msgs::msg::Info & info, rtabmap::Statistics & sta
 
 void infoToROS(const rtabmap::Statistics & stats, rtabmap_msgs::msg::Info & info)
 {
+	// Fall back to the statistics' own stamp when the caller left the header unstamped.
+	// Callers that already stamped it keep their value, which may be a publication time
+	// unrelated to the data, or the exact input stamp rather than this double-derived one.
+	if(info.header.stamp.sec == 0 && info.header.stamp.nanosec == 0)
+	{
+		info.header.stamp = timestampToROS(stats.stamp());
+	}
 	info.ref_id = stats.refImageId();
 	info.loop_closure_id = stats.loopClosureId();
 	info.proximity_detection_id = stats.proximityDetectionId();
@@ -829,9 +876,12 @@ rtabmap::CameraModel cameraModelFromROS(
 		const sensor_msgs::msg::CameraInfo & camInfo,
 		const rtabmap::Transform & localTransform)
 {
+	// Note: k, r and p are fixed-size arrays in the ROS message, so they are never
+	// empty and their size is always right. An unset matrix is signalled by all-zero
+	// content instead: k[0] and p[0] hold the focal length, which is always non-zero
+	// for a valid calibration, and an unset rectification matrix is all zeros.
 	cv:: Mat K;
-	UASSERT(camInfo.k.empty() || camInfo.k.size() == 9);
-	if(!camInfo.k.empty())
+	if(camInfo.k[0] != 0.0)
 	{
 		K = cv::Mat(3, 3, CV_64FC1);
 		memcpy(K.data, camInfo.k.data(), 9*sizeof(double));
@@ -858,17 +908,22 @@ rtabmap::CameraModel cameraModelFromROS(
 		}
 	}
 
+	// R is a rotation matrix, so any of its elements can legitimately be zero: only
+	// an entirely zero matrix means "not set".
 	cv:: Mat R;
-	UASSERT(camInfo.r.empty() || camInfo.r.size() == 9);
-	if(!camInfo.r.empty())
+	bool rIsSet = false;
+	for(size_t i=0; !rIsSet && i<camInfo.r.size(); ++i)
+	{
+		rIsSet = camInfo.r[i] != 0.0;
+	}
+	if(rIsSet)
 	{
 		R = cv::Mat(3, 3, CV_64FC1);
 		memcpy(R.data, camInfo.r.data(), 9*sizeof(double));
 	}
 
 	cv:: Mat P;
-	UASSERT(camInfo.p.empty() || camInfo.p.size() == 12);
-	if(!camInfo.p.empty())
+	if(camInfo.p[0] != 0.0)
 	{
 		P = cv::Mat(3, 4, CV_64FC1);
 		memcpy(P.data, camInfo.p.data(), 12*sizeof(double));
@@ -942,8 +997,9 @@ void cameraModelToROS(
 	{
 		memset(camInfo.p.data(), 0.0, 12*sizeof(double));
 		if(!model.K_raw().empty()) {
+			// P = [K | 0]: copying K already sets the homogeneous P(2,2)=1, and the
+			// fourth column (the Tx/Ty/Tz translation) stays zero for a single camera.
 			model.K_raw().copyTo(cv::Mat(3,4,CV_64FC1, camInfo.p.data()).colRange(0,3));
-			camInfo.p.back() = 1.0;
 		}
 	}
 	else
@@ -1249,7 +1305,8 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg)
 		pcl::PCLPointCloud2 cloud;
 		pcl_conversions::toPCL(msg.laser_scan, cloud);
 		s.setLaserScan(rtabmap::LaserScan(
-			rtabmap::util3d::laserScanFromPointCloud(cloud),
+			rtabmap::util3d::laserScanFromPointCloud(cloud, true,
+					rtabmap::LaserScan::isScan2d((rtabmap::LaserScan::Format)msg.laser_scan_format)),
 			msg.laser_scan_max_pts,
 			msg.laser_scan_max_range,
 			transformFromGeometryMsg(msg.laser_scan_local_transform)),
@@ -1352,10 +1409,13 @@ void sensorDataToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::Sensor
 	{
 		pcl::PCLPointCloud2::Ptr cloud = rtabmap::util3d::laserScanToPointCloud2(data.laserScanRaw());
 		pcl_conversions::moveFromPCL(*cloud, msg.laser_scan);
-		msg.laser_scan_max_pts = data.laserScanCompressed().maxPoints();
-		msg.laser_scan_max_range = data.laserScanCompressed().rangeMax();
-		msg.laser_scan_format = data.laserScanCompressed().format();
-		transformToGeometryMsg(data.laserScanCompressed().localTransform(), msg.laser_scan_local_transform);
+		// Describe the scan we just serialized: reading these from laserScanCompressed()
+		// zeroes them whenever only the raw scan is set, and sensorDataFromROS() then
+		// fails its format assertion.
+		msg.laser_scan_max_pts = data.laserScanRaw().maxPoints();
+		msg.laser_scan_max_range = data.laserScanRaw().rangeMax();
+		msg.laser_scan_format = data.laserScanRaw().format();
+		transformToGeometryMsg(data.laserScanRaw().localTransform(), msg.laser_scan_local_transform);
 	}
 	if(!data.laserScanCompressed().empty())
 	{
@@ -1624,10 +1684,17 @@ std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo &
 	stats.insert(std::make_pair("Odometry/ICPStructuralComplexity/", info.reg.icpStructuralComplexity));
 	stats.insert(std::make_pair("Odometry/ICPStructuralDistribution/", info.reg.icpStructuralDistribution));
 	stats.insert(std::make_pair("Odometry/ICPCorrespondences/", info.reg.icpCorrespondences));
-	stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)info.reg.covariance.at<double>(0,0))));
-	stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)info.reg.covariance.at<double>(5,5))));
-	stats.insert(std::make_pair("Odometry/VarianceLin/", (float)info.reg.covariance.at<double>(0,0)));
-	stats.insert(std::make_pair("Odometry/VarianceAng/", (float)info.reg.covariance.at<double>(5,5)));
+	// RegistrationInfo leaves covariance empty by default, so only read it when the
+	// expected 6x6 matrix is actually there.
+	if(info.reg.covariance.type() == CV_64FC1 &&
+	   info.reg.covariance.rows == 6 &&
+	   info.reg.covariance.cols == 6)
+	{
+		stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)info.reg.covariance.at<double>(0,0))));
+		stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)info.reg.covariance.at<double>(5,5))));
+		stats.insert(std::make_pair("Odometry/VarianceLin/", (float)info.reg.covariance.at<double>(0,0)));
+		stats.insert(std::make_pair("Odometry/VarianceAng/", (float)info.reg.covariance.at<double>(5,5)));
+	}
 	stats.insert(std::make_pair("Odometry/TimeEstimation/ms", info.timeEstimation*1000.0f));
 	stats.insert(std::make_pair("Odometry/TimeFiltering/ms", info.timeParticleFiltering*1000.0f));
 	stats.insert(std::make_pair("Odometry/LocalMapSize/", info.localMapSize));
@@ -2648,13 +2715,41 @@ bool convertScanMsg(
 	}
 
 	// make sure the frame of the laser is updated during the whole scan time
-	rtabmap::Transform tmpT = getMovingTransform(
-			scan2dMsg.header.frame_id,
-			odomFrameId.empty()?frameId:odomFrameId,
-			rclcpp::Time(scan2dMsg.header.stamp.sec, scan2dMsg.header.stamp.nanosec),
-			rclcpp::Time(scan2dMsg.header.stamp.sec, scan2dMsg.header.stamp.nanosec) + rclcpp::Duration::from_seconds(scan2dMsg.ranges.size()*scan2dMsg.time_increment),
-			tfBuffer,
-			waitForTransform);
+	const rclcpp::Time scanStart(scan2dMsg.header.stamp);
+	const rclcpp::Time scanEnd = scanStart + rclcpp::Duration::from_seconds((scan2dMsg.ranges.empty()?0:scan2dMsg.ranges.size()-1)*scan2dMsg.time_increment);
+	std::string fixedFrameId = odomFrameId.empty()?frameId:odomFrameId;
+	rtabmap::Transform tmpT;
+	if(fixedFrameId == frameId || tfBuffer._frameExists(fixedFrameId)) // don't wait for a frame never published
+	{
+		tmpT = getMovingTransform(
+				scan2dMsg.header.frame_id,
+				fixedFrameId,
+				scanStart,
+				scanEnd,
+				tfBuffer,
+				waitForTransform);
+	}
+	if(tmpT.isNull() && fixedFrameId != frameId)
+	{
+		// Odometry not in TF: use the scan as it is rather than dropping it.
+		static bool warned = false;
+		if(!warned)
+		{
+			UWARN("Could not get laser frame \"%s\" relative to odometry frame \"%s\" over the scan "
+				  "(%fs to %fs). Laser scans are used without deskewing nor synchronization with "
+				  "odometry. Publish odometry on TF to have them deskewed. This message is only shown once.",
+				  scan2dMsg.header.frame_id.c_str(), odomFrameId.c_str(), scanStart.seconds(), scanEnd.seconds());
+			warned = true;
+		}
+		fixedFrameId = frameId;
+		tmpT = getMovingTransform(
+				scan2dMsg.header.frame_id,
+				fixedFrameId,
+				scanStart,
+				scanEnd,
+				tfBuffer,
+				waitForTransform);
+	}
 	if(tmpT.isNull())
 	{
 		return false;
@@ -2674,12 +2769,12 @@ bool convertScanMsg(
 	//transform in frameId_ frame
 	sensor_msgs::msg::PointCloud2 scanOut;
 	laser_geometry::LaserProjection projection;
-	projection.transformLaserScanToPointCloud(odomFrameId.empty()?frameId:odomFrameId, scan2dMsg, scanOut, tfBuffer);
+	projection.transformLaserScanToPointCloud(fixedFrameId, scan2dMsg, scanOut, tfBuffer);
 
 	//transform back in laser frame
 	rtabmap::Transform laserToOdom = getTransform(
 			scan2dMsg.header.frame_id,
-			odomFrameId.empty()?frameId:odomFrameId,
+			fixedFrameId,
 			scan2dMsg.header.stamp,
 			tfBuffer,
 			waitForTransform);
@@ -2689,7 +2784,7 @@ bool convertScanMsg(
 	}
 
 	// sync with odometry stamp
-	if(!odomFrameId.empty() && odomStamp != scan2dMsg.header.stamp)
+	if(fixedFrameId != frameId && odomStamp != scan2dMsg.header.stamp)
 	{
 		rtabmap::Transform sensorT = getMovingTransform(
 				frameId,
@@ -2743,7 +2838,7 @@ bool convertScanMsg(
 	if(hasIntensity)
 	{
 		pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
-		pcl::fromROSMsg(scanOut, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScan);
 		pclScan->is_dense = true;
 		data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom).data(); // put back in laser frame
 		format = rtabmap::LaserScan::kXYI;
@@ -2751,7 +2846,7 @@ bool convertScanMsg(
 	else
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
-		pcl::fromROSMsg(scanOut, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScan);
 		pclScan->is_dense = true;
 		data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom).data(); // put back in laser frame
 		format = rtabmap::LaserScan::kXY;
@@ -2832,8 +2927,7 @@ bool deskew_impl(
 		tf2_ros::Buffer * tfBuffer,
 		double waitForTransform,
 		bool slerp,
-		const rtabmap::Transform & velocity,
-		double previousStamp)
+		const rtabmap::Transform & velocity)
 {
 	if(tfBuffer != 0)
 	{
@@ -2857,12 +2951,6 @@ bool deskew_impl(
 			return false;
 		}
 
-		if(previousStamp <= 0.0)
-		{
-			UERROR("previousStamp should be >0 when constant velocity model is used!");
-			return false;
-		}
-
 		if(velocity.isNull())
 		{
 			UERROR("velocity should be valid when constant velocity model is used!");
@@ -3133,8 +3221,23 @@ bool deskew_impl(
 	}
 	else if(lastStamp == firstStamp)
 	{
-		UERROR("First and last stamps in the scan are the same (%f) (header=%f)!", timestampFromROS(lastStamp), timestampFromROS(input.header.stamp));
-		return false;
+		// There is no time spread across the scan, so there is nothing to correct. This
+		// happens when the driver doesn't fill the per-point time channel, and also when
+		// the cloud has already been deskewed: deskewing zeroes that channel to mark it.
+		// Pass the cloud through unchanged so that deskewing twice is a no-op rather than
+		// a failure that makes the caller drop the frame.
+		static bool warned = false;
+		if(!warned)
+		{
+			UWARN("First and last stamps in the scan are the same (%f) (header=%f), the "
+				  "cloud is returned unchanged. Either the time channel is not filled by "
+				  "the driver, or the cloud has already been deskewed. This warning is "
+				  "only shown once.",
+				  timestampFromROS(lastStamp), timestampFromROS(input.header.stamp));
+			warned = true;
+		}
+		output = input;
+		return true;
 	}
 	std::string errorMsg;
 	if(tfBuffer != 0 &&
@@ -3184,23 +3287,19 @@ bool deskew_impl(
 			float vx,vy,vz, vroll,vpitch,vyaw;
 			velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
 
-			// We need three poses:
-			//  1- The pose of base frame in odom frame at first stamp
-			//  2- The pose of base frame in odom frame at msg stamp
-			//  3- The pose of base frame in odom frame at last stamp
-			UASSERT(timestampFromROS(firstStamp) >= previousStamp);
-			UASSERT(timestampFromROS(lastStamp) > previousStamp);
-			double dt1 = timestampFromROS(firstStamp) - previousStamp;
-			double dt2 = timestampFromROS(input.header.stamp) - previousStamp;
-			double dt3 = timestampFromROS(lastStamp) - previousStamp;
-
-			rtabmap::Transform p1(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
-			rtabmap::Transform p2(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2);
-			rtabmap::Transform p3(vx*dt3, vy*dt3, vz*dt3, vroll*dt3, vpitch*dt3, vyaw*dt3);
+			// Integrate the velocity directly from the stamp of the msg, which is the
+			// frame the deskewed cloud is expressed in. Going through a third, earlier
+			// reference pose and composing it away would give the same answer for a pure
+			// translation, but not for a rotation: Transform() scales roll/pitch/yaw
+			// linearly instead of using the twist exponential, so the composition only
+			// cancels in the small-angle limit. Keeping dt bounded by the scan duration
+			// is where that approximation is at its best.
+			double dt1 = timestampFromROS(firstStamp) - timestampFromROS(input.header.stamp);
+			double dt3 = timestampFromROS(lastStamp) - timestampFromROS(input.header.stamp);
 
 			// First and last poses are relative to stamp of the msg
-			firstPose = p2.inverse() * p1;
-			lastPose = p2.inverse() * p3;
+			firstPose = rtabmap::Transform(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
+			lastPose = rtabmap::Transform(vx*dt3, vy*dt3, vz*dt3, vroll*dt3, vpitch*dt3, vyaw*dt3);
 		}
 
 		if(firstPose.isNull())
@@ -3227,6 +3326,7 @@ bool deskew_impl(
 
 	output = input;
 	rclcpp::Time stamp;
+	bool clampWarned = false;   // reported once per cloud, see the clamp below
 	UTimer processingTime;
 	if(timeOnColumns)
 	{
@@ -3272,7 +3372,27 @@ bool deskew_impl(
 			rtabmap::Transform transform;
 			if(slerp)
 			{
-				transform = firstPose.interpolate((stamp-firstStamp).seconds() / scanTime, lastPose);
+				// The ordering check only compares the first and last samples, so a stamp
+				// outside [firstStamp, lastStamp] can slip through. Clamp it: extrapolating
+				// would throw the point far beyond the sweep.
+				double ratio = (stamp-firstStamp).seconds() / scanTime;
+				if(ratio < 0.0 || ratio > 1.0)
+				{
+					// Warned once per cloud rather than once per process: the timestamp
+					// channel is corrupted, which is a serious upstream problem worth
+					// reporting on every affected scan, but not once per point.
+					if(!clampWarned)
+					{
+						UWARN("A point has a stamp (%f) outside the first (%f) and last (%f) "
+							  "stamps of the scan, its correction is clamped to the closest end "
+							  "of the sweep. The timestamp channel of the input cloud is likely "
+							  "corrupted. Only the first such point of this cloud is reported.",
+							  timestampFromROS(stamp), timestampFromROS(firstStamp), timestampFromROS(lastStamp));
+						clampWarned = true;
+					}
+					ratio = ratio<0.0?0.0:1.0;
+				}
+				transform = firstPose.interpolate(float(ratio), lastPose);
 			}
 			else
 			{
@@ -3366,7 +3486,27 @@ bool deskew_impl(
 			rtabmap::Transform transform;
 			if(slerp)
 			{
-				transform = firstPose.interpolate((stamp-firstStamp).seconds() / scanTime, lastPose);
+				// The ordering check only compares the first and last samples, so a stamp
+				// outside [firstStamp, lastStamp] can slip through. Clamp it: extrapolating
+				// would throw the point far beyond the sweep.
+				double ratio = (stamp-firstStamp).seconds() / scanTime;
+				if(ratio < 0.0 || ratio > 1.0)
+				{
+					// Warned once per cloud rather than once per process: the timestamp
+					// channel is corrupted, which is a serious upstream problem worth
+					// reporting on every affected scan, but not once per point.
+					if(!clampWarned)
+					{
+						UWARN("A point has a stamp (%f) outside the first (%f) and last (%f) "
+							  "stamps of the scan, its correction is clamped to the closest end "
+							  "of the sweep. The timestamp channel of the input cloud is likely "
+							  "corrupted. Only the first such point of this cloud is reported.",
+							  timestampFromROS(stamp), timestampFromROS(firstStamp), timestampFromROS(lastStamp));
+						clampWarned = true;
+					}
+					ratio = ratio<0.0?0.0:1.0;
+				}
+				transform = firstPose.interpolate(float(ratio), lastPose);
 			}
 			else
 			{
@@ -3428,16 +3568,15 @@ bool deskew(
 		double waitForTransform,
 		bool slerp)
 {
-	return deskew_impl(input, output, fixedFrameId, &tfBuffer, waitForTransform, slerp, rtabmap::Transform(), 0);
+	return deskew_impl(input, output, fixedFrameId, &tfBuffer, waitForTransform, slerp, rtabmap::Transform());
 }
 
 bool deskew(
 		const sensor_msgs::msg::PointCloud2 & input,
 		sensor_msgs::msg::PointCloud2 & output,
-		double previousStamp,
 		const rtabmap::Transform & velocity)
 {
-	return deskew_impl(input, output, "", 0, 0, true, velocity, previousStamp);
+	return deskew_impl(input, output, "", 0, 0, true, velocity);
 }
 
 
@@ -3493,8 +3632,15 @@ transformPointCloud (
     Eigen::Vector4f pt_out;
 
     bool max_range_point = false;
-    int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset;
-    float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset]));
+    // Only touch in.fields[dist_idx] when the "distance" field actually exists:
+    // indexing with -1 is out of bounds and aborts on a hardened libstdc++.
+    int distance_ptr_offset = 0;
+    float* distance_ptr = NULL;
+    if (dist_idx >= 0)
+    {
+      distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset;
+      distance_ptr = (float*)(&in.data[distance_ptr_offset]);
+    }
     if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2]))
     {
       if (distance_ptr==NULL || !std::isfinite(*distance_ptr))  // Invalid point

rtabmap_costmap_plugins/CMakeLists.txt

@@ -45,6 +45,7 @@ IF("$ENV{ROS_DISTRO}" STRLESS "lyrical")
   IF("$ENV{ROS_DISTRO}" STRLESS "jazzy")
     target_compile_definitions(rtabmap_costmap_plugins PRIVATE -DPRE_ROS_JAZZY)
   ENDIF()
+  target_compile_definitions(rtabmap_costmap_plugins PRIVATE -DPRE_ROS_LYRICAL)
   ament_target_dependencies(rtabmap_costmap_plugins ${AmentLibraries})
 ELSE()
   target_link_libraries(rtabmap_costmap_plugins PRIVATE ${Libraries})

rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp

@@ -284,6 +284,15 @@ protected:
   rcl_interfaces::msg::SetParametersResult
   dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
 
+  /**
+   * @brief Declares one of this layer's parameters and returns its value
+   * @param node the node the layer runs in
+   * @param name the parameter's name, without the layer prefix
+   * @param defaultValue the value to use when nothing else sets it
+   */
+  template<typename T, typename NodeT>
+  T declareOrGetParameter(NodeT & node, const std::string & name, const T & defaultValue);
+
   // Dynamic parameters handler
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
 };

rtabmap_costmap_plugins/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_costmap_plugins</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's costmap plugins.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

rtabmap_costmap_plugins/src/voxel_layer.cpp

@@ -58,42 +58,76 @@ using rcl_interfaces::msg::ParameterType;
 namespace rtabmap_costmap_plugins
 {
 
+namespace
+{
+
+/// nav2's Observation::cloud_ used to be a raw pointer and is now the cloud itself, so
+/// it is reached through this rather than dereferenced directly.
+inline const sensor_msgs::msg::PointCloud2 & cloudOf(
+  const sensor_msgs::msg::PointCloud2 & cloud)
+{
+  return cloud;
+}
+
+inline const sensor_msgs::msg::PointCloud2 & cloudOf(
+  const sensor_msgs::msg::PointCloud2 * cloud)
+{
+  return *cloud;
+}
+
+/// nav2 hands out observations by value up to kilted and by shared pointer after it.
+inline const nav2_costmap_2d::Observation & obsOf(
+  const nav2_costmap_2d::Observation & observation)
+{
+  return observation;
+}
+
+inline const nav2_costmap_2d::Observation & obsOf(
+  const std::shared_ptr<const nav2_costmap_2d::Observation> & observation)
+{
+  return *observation;
+}
+
+}  // namespace
+
+/// nav2 declared a layer's parameters through Layer::declareParameter up to kilted and
+/// through the node itself after it.
+template<typename T, typename NodeT>
+T VoxelLayer::declareOrGetParameter(
+  NodeT & node, const std::string & name, const T & defaultValue)
+{
+#ifdef PRE_ROS_LYRICAL
+  declareParameter(name, rclcpp::ParameterValue(defaultValue));
+  T value = defaultValue;
+  node->get_parameter(name_ + "." + name, value);
+  return value;
+#else
+  return node->declare_or_get_parameter(name_ + "." + name, defaultValue);
+#endif
+}
+
 void VoxelLayer::onInitialize()
 {
   nav2_costmap_2d::ObstacleLayer::onInitialize();
 
-  declareParameter("enabled", rclcpp::ParameterValue(true));
-  declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true));
-  declareParameter("min_obstacle_height", rclcpp::ParameterValue(0.0));
-  declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0));
-  declareParameter("z_voxels", rclcpp::ParameterValue(10));
-  declareParameter("origin_z", rclcpp::ParameterValue(0.0));
-  declareParameter("z_resolution", rclcpp::ParameterValue(0.2));
-  declareParameter("unknown_threshold", rclcpp::ParameterValue(15));
-  declareParameter("mark_threshold", rclcpp::ParameterValue(0));
-  declareParameter("combination_method", rclcpp::ParameterValue(1));
-  declareParameter("publish_voxel_map", rclcpp::ParameterValue(false));
-  declareParameter("robot_base_frame", rclcpp::ParameterValue("base_link"));
-
   auto node = node_.lock();
   if (!node) {
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  node->get_parameter(name_ + "." + "enabled", enabled_);
-  node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
-  node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_);
-  node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_);
-  node->get_parameter(name_ + "." + "z_voxels", size_z_);
-  node->get_parameter(name_ + "." + "origin_z", origin_z_);
-  node->get_parameter(name_ + "." + "z_resolution", z_resolution_);
-  node->get_parameter(name_ + "." + "unknown_threshold", unknown_threshold_);
-  node->get_parameter(name_ + "." + "mark_threshold", mark_threshold_);
-  node->get_parameter(name_ + "." + "publish_voxel_map", publish_voxel_);
-  node->get_parameter(name_ + "." + "robot_base_frame", robot_base_frame_);
+  enabled_ = declareOrGetParameter(node, "enabled", true);
+  footprint_clearing_enabled_ = declareOrGetParameter(node, "footprint_clearing_enabled", true);
+  min_obstacle_height_ = declareOrGetParameter(node, "min_obstacle_height", 0.0);
+  max_obstacle_height_ = declareOrGetParameter(node, "max_obstacle_height", 2.0);
+  size_z_ = declareOrGetParameter(node, "z_voxels", 10);
+  origin_z_ = declareOrGetParameter(node, "origin_z", 0.0);
+  z_resolution_ = declareOrGetParameter(node, "z_resolution", 0.2);
+  unknown_threshold_ = declareOrGetParameter(node, "unknown_threshold", 15);
+  mark_threshold_ = declareOrGetParameter(node, "mark_threshold", 0);
+  publish_voxel_ = declareOrGetParameter(node, "publish_voxel_map", false);
+  robot_base_frame_ = declareOrGetParameter(node, "robot_base_frame", std::string("base_link"));
 
-  int combination_method_param{};
-  node->get_parameter(name_ + "." + "combination_method", combination_method_param);
+  const int combination_method_param = declareOrGetParameter(node, "combination_method", 1);
 #ifdef PRE_ROS_JAZZY
   combination_method_ = combination_method_param;
 #else
@@ -169,7 +203,11 @@ void VoxelLayer::updateBounds(
   useExtraBounds(min_x, min_y, max_x, max_y);
 
   bool current = true;
+#ifdef PRE_ROS_LYRICAL
   std::vector<nav2_costmap_2d::Observation> observations, clearing_observations;
+#else
+  std::vector<nav2_costmap_2d::Observation::ConstSharedPtr> observations, clearing_observations;
+#endif
 
   // get the marking observations
   current = getMarkingObservations(observations) && current;
@@ -182,16 +220,15 @@ void VoxelLayer::updateBounds(
 
   // raytrace freespace
   for (unsigned int i = 0; i < clearing_observations.size(); ++i) {
-    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
+    raytraceFreespace(obsOf(clearing_observations[i]), min_x, min_y, max_x, max_y);
   }
 
   // place the new obstacles into a priority queue... each with a priority of zero to begin with
-  for (std::vector<nav2_costmap_2d::Observation>::const_iterator it = observations.begin(); it != observations.end();
-    ++it)
+  for (auto it = observations.begin(); it != observations.end(); ++it)
   {
-    const nav2_costmap_2d::Observation & obs = *it;
+    const nav2_costmap_2d::Observation & obs = obsOf(*it);
 
-    const sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_);
+    const sensor_msgs::msg::PointCloud2 & cloud = cloudOf(obs.cloud_);
 
     double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_;
     double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_;
@@ -277,7 +314,9 @@ void VoxelLayer::raytraceFreespace(
 {
   auto clearing_endpoints_ = std::make_unique<sensor_msgs::msg::PointCloud2>();
 
-  if (clearing_observation.cloud_->height == 0 || clearing_observation.cloud_->width == 0) {
+  const sensor_msgs::msg::PointCloud2 & clearing_cloud = cloudOf(clearing_observation.cloud_);
+
+  if (clearing_cloud.height == 0 || clearing_cloud.width == 0) {
     return;
   }
 
@@ -311,8 +350,8 @@ void VoxelLayer::raytraceFreespace(
   }
 
   clearing_endpoints_->data.clear();
-  clearing_endpoints_->width = clearing_observation.cloud_->width;
-  clearing_endpoints_->height = clearing_observation.cloud_->height;
+  clearing_endpoints_->width = clearing_cloud.width;
+  clearing_endpoints_->height = clearing_cloud.height;
   clearing_endpoints_->is_dense = true;
   clearing_endpoints_->is_bigendian = false;
 
@@ -331,9 +370,9 @@ void VoxelLayer::raytraceFreespace(
   double map_end_y = origin_y_ + getSizeInMetersY();
   double map_end_z = origin_z_ + getSizeInMetersZ();
 
-  sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
-  sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
-  sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(clearing_cloud, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(clearing_cloud, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(clearing_cloud, "z");
 
   for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
     double wpx = *iter_x;
@@ -431,7 +470,7 @@ void VoxelLayer::raytraceFreespace(
 
   if (publish_clearing_points) {
     clearing_endpoints_->header.frame_id = global_frame_;
-    clearing_endpoints_->header.stamp = clearing_observation.cloud_->header.stamp;
+    clearing_endpoints_->header.stamp = clearing_cloud.header.stamp;
 
     clearing_endpoints_pub_->publish(std::move(clearing_endpoints_));
   }

rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py

@@ -27,13 +27,17 @@ def launch_setup(context, *args, **kwargs):
     localization = localization == 'True' or localization == 'true'
     icp_odometry = LaunchConfiguration('icp_odometry').perform(context)
     icp_odometry = icp_odometry == 'True' or icp_odometry == 'true'
+    deskewing = LaunchConfiguration('deskewing').perform(context)
+    deskewing = deskewing == 'True' or deskewing == 'true'
     
     parameters={
           'frame_id':'base_footprint',
           'use_sim_time':use_sim_time,
           'subscribe_depth':False,
           'subscribe_rgb':False,
-          'subscribe_scan':True,
+          'subscribe_scan':not deskewing,
+          'subscribe_scan_cloud':deskewing,
+          'scan_cloud_is_2d': True,
           'approx_sync':True,
           'use_action_for_goal':True,
           'Reg/Strategy':'1',
@@ -50,13 +54,24 @@ def launch_setup(context, *args, **kwargs):
         arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
                
     remappings=[
-          ('scan', '/scan')]
+          ('scan', '/scan' if not deskewing else 'scan_not_used'),
+          ('scan_cloud', '/scan/deskewed' if deskewing else 'scan_cloud_not_used' )]
     if icp_odometry:
         remappings.append(('odom', 'icp_odom'))
     
     return [
         # Nodes to launch
-        
+
+        # Lidar deskewing (optional, useful with real lidar)
+        Node(
+            condition=IfCondition(LaunchConfiguration('deskewing')),
+            package='rtabmap_util', executable='lidar_deskewing', output='screen',
+            parameters=[{'wait_for_transform': 0.1,
+                         'fixed_frame_id': 'odom',
+                         'slerp': False,
+                         'use_sim_time':use_sim_time}],
+            remappings=[('input_scan', '/scan')]),
+
         # ICP odometry (optional)
         Node(
             condition=IfCondition(LaunchConfiguration('icp_odometry')),
@@ -97,5 +112,9 @@ def generate_launch_description():
             'icp_odometry', default_value='false',
             description='Launch ICP odometry on top of wheel odometry.'),
 
+        DeclareLaunchArgument(
+            'deskewing', default_value='false',
+            description='Do lidar scan deskewing based on wheel odometry.'),
+
         OpaqueFunction(function=launch_setup)
     ])

rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py

@@ -182,6 +182,12 @@ def generate_launch_description():
             'icp_odometry', default_value='false',
             description='Launch ICP odometry on top of wheel odometry.'),
         
+        DeclareLaunchArgument(
+            'deskewing', default_value='false',
+            description='Do lidar scan deskewing based on wheel odometry. In simulation it '
+                        'doesn\'t matter because laser scans are not skewed, but the option '
+                        'is there for the demo.'),
+
         DeclareLaunchArgument(
             'x_pose', default_value='-2.0',
             description='Initial position of the robot in the simulator.'),

rtabmap_demos/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_demos</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's demo launch files.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

rtabmap_examples/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_examples</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's example launch files.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

rtabmap_launch/launch/rtabmap.launch.py

@@ -508,7 +508,7 @@ def generate_launch_description():
         DeclareLaunchArgument('odom_tf_angular_variance',   default_value='0.01',    description='If TF is used to get odometry, this is the default angular variance'),
         DeclareLaunchArgument('odom_tf_linear_variance',    default_value='0.001',   description='If TF is used to get odometry, this is the default linear variance'),
         DeclareLaunchArgument('odom_args',                  default_value='',      description='More arguments for odometry (overwrite same parameters in rtabmap_args).'),
-        DeclareLaunchArgument('odom_sensor_sync',           default_value='false', description=''),
+        DeclareLaunchArgument('odom_sensor_sync',           default_value='true', description='Correct each sensor\'s position for the motion between its stamp and the odometry\'s, using TF.'),
         DeclareLaunchArgument('odom_guess_frame_id',        default_value='',      description=''),
         DeclareLaunchArgument('odom_guess_min_translation', default_value='0.0',   description=''),
         DeclareLaunchArgument('odom_guess_min_rotation',    default_value='0.0',   description=''),

rtabmap_launch/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_launch</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's main launch files.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

rtabmap_msgs/CMakeLists.txt

@@ -11,11 +11,12 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64")
-  # issues #1285 #1288
+  # issues #1285 #1288 (best-effort probes: not REQUIRED, a phantom miss under
+  # emulated arm64 should not fail the build)
   find_library(
     rcutils_LIB NAMES rcutils
     PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib"
-    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH
   )
 endif()
 

rtabmap_msgs/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_msgs</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's msgs package.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

rtabmap_odom/CMakeLists.txt

@@ -157,4 +157,52 @@ install(DIRECTORY include/
    FILES_MATCHING PATTERN "*.h"
 )
 
+#############
+## Testing ##
+#############
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(OpenCV REQUIRED COMPONENTS core imgcodecs)
+  # Recorded sensor input is replayed by the tests themselves, not by "ros2 bag play".
+  find_package(rosbag2_cpp REQUIRED)
+
+  # Real frames the visual odometry tests register against, read from the source tree:
+  # these binaries are never installed, and the fixtures are not either. See
+  # test/data/README.md for where they come from.
+  set(rtabmap_odom_test_data_root "${CMAKE_CURRENT_SOURCE_DIR}/test/data")
+
+  # Each node gets its own test binary: a crash or a stuck executor in one node cannot
+  # take the others down, and every binary starts with a clean DDS graph.
+  #
+  # Each binary also gets its own DDS domain. colcon tests packages in parallel and ctest
+  # can run these binaries in parallel, while these suites share topic names -- odom,
+  # rgbd_image, scan_cloud -- with rtabmap_sync's and rtabmap_util's. On a shared domain
+  # they discover each other's publishers and assertions then see traffic the test never
+  # sent. rtabmap_util numbers from 30 and rtabmap_sync from 50; keep the ranges apart.
+  set(rtabmap_odom_test_domain_id 70)
+  macro(rtabmap_odom_add_node_test test_name)
+    ament_add_gtest(${test_name} test/${test_name}.cpp
+      ENV ROS_DOMAIN_ID=${rtabmap_odom_test_domain_id}
+      TIMEOUT 300)
+    math(EXPR rtabmap_odom_test_domain_id "${rtabmap_odom_test_domain_id} + 1")
+    if(TARGET ${test_name})
+      target_include_directories(${test_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test)
+      target_compile_definitions(${test_name} PRIVATE
+        RTABMAP_ODOM_TEST_DATA_ROOT="${rtabmap_odom_test_data_root}")
+      target_link_libraries(${test_name} rtabmap_odom_plugins rtabmap_odom
+        opencv_core opencv_imgcodecs rosbag2_cpp::rosbag2_cpp rtabmap::core)
+      if("$ENV{ROS_DISTRO}" STRLESS "lyrical")
+        ament_target_dependencies(${test_name} ${AmentLibraries})
+      else()
+        target_link_libraries(${test_name} ${Libraries} ${PublicLibraries})
+      endif()
+    endif()
+  endmacro()
+
+  rtabmap_odom_add_node_test(test_odometry_ros)
+  rtabmap_odom_add_node_test(test_rgbd_odometry)
+  rtabmap_odom_add_node_test(test_stereo_odometry)
+  rtabmap_odom_add_node_test(test_icp_odometry)
+endif()
+
 ament_package()

rtabmap_odom/README.md

@@ -0,0 +1,273 @@
+# rtabmap_odom
+
+Odometry for [RTAB-Map](https://github.com/introlab/rtabmap): where the robot is relative to a local fixed frame, estimated from its own sensors — a pose that moves continuously and never jumps, but drifts over time.
+
+SLAM needs a pose for every measurement it maps. These nodes produce one by registering each new frame against the last — visually from an RGB-D or stereo camera, or geometrically from a lidar — and integrating the result into a [`nav_msgs/msg/Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) and a TF.
+
+## Contents
+
+- [Nodes](#nodes)
+  - [Choosing a sensor modality for the environment](#choosing-a-sensor-modality-for-the-environment)
+- [Library](#library)
+- [Conventions](#conventions)
+  - [Frames and TF](#frames-and-tf)
+  - [RTAB-Map's own parameters](#rtab-maps-own-parameters)
+  - [Feeding in an external guess](#feeding-in-an-external-guess)
+  - [IMU](#imu)
+  - [Update rates and dropped frames](#update-rates-and-dropped-frames)
+  - [Lost frames, resets and new maps](#lost-frames-resets-and-new-maps)
+- [Services](#services)
+- [Published topics](#published-topics)
+  - [Outputting filtered scans and features](#outputting-filtered-scans-and-features)
+- [Diagnostics](#diagnostics)
+- [License](#license)
+
+## Nodes
+
+| Node | Description |
+|---|---|
+| [rgbd_odometry](doc/rgbd_odometry.md) | Visual odometry from a color image and a depth image registered to it. |
+| [stereo_odometry](doc/stereo_odometry.md) | Visual odometry from a stereo pair. |
+| [icp_odometry](doc/icp_odometry.md) | Geometric odometry from a 2D or 3D lidar. |
+
+Every node is a [composable node](https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Composition.html) as well as a standalone executable.
+
+### Choosing a sensor modality for the environment
+
+Which to use is a question about the **environment**, not about which sensor is better. A camera tracks visual texture; a lidar tracks geometry. Each fails where its own cue is missing, and the two failures do not overlap much.
+
+| Environment | Use | Why |
+|---|---|---|
+| Visually textured and well lit — offices, cluttered rooms, daylight outdoors | **Camera** | Plenty of features to match, and appearance gives loop closure for free. |
+| Textureless but geometrically rich — bare corridors with doorways and furniture, warehouse aisles | **Lidar** | Blank walls give a camera nothing; the shape of the space still constrains ICP. |
+| Dark, or lighting that changes abruptly | **Lidar** | A camera is simply blind. Lidar does not care. |
+| Geometrically plain but visually rich — a large open hall with a patterned floor, textured flat walls | **Camera** | [Degenerate geometry](doc/icp_odometry.md#degenerate-geometry) defeats ICP here, while the texture is exactly what a camera needs. |
+| Both plain and textureless — an empty warehouse, a long featureless tunnel | **Wheel odometry**, with either as a corrector | Neither cue is present. This is the case where wheel odometry carries the pose. |
+| Repetitive and self-similar — tiled floors, rows of racking, a long colonnade | **Wheel odometry as the guess**, with either on top | Both cues are present but *ambiguous*: a camera matches the wrong copy of a feature, a lidar the wrong bay of shelving. See [Repetitive patterns](doc/rgbd_odometry.md#repetitive-patterns). |
+| Outdoors at range | **Stereo camera or 3D lidar** | RGB-D depth stops working outdoors; both of these keep going. |
+
+**Do not underestimate wheel odometry.** On a wheeled robot it is locally excellent and only drifts over distance — the opposite failure from both of the above, which are locally noisy but not systematically biased. It is also the only one of the three that keeps working when the environment offers no cue at all — and, because it is indifferent to what the scene *looks* like, the only one that is not fooled when the scene repeats itself.
+
+**Fuse the wheels with an IMU before feeding them in.** [`robot_localization`](https://docs.ros.org/en/jazzy/p/robot_localization/) is the standard way: its EKF combines wheel odometry with IMU orientation and angular rates into one filtered `odom` topic, which is a markedly better guess than the wheels alone. [FusionCore](https://github.com/manankharwar/fusioncore) is another EKF that does the same job. The IMU fixes exactly what encoders are worst at — yaw through a turn, and wheel slip, which encoders report as motion that never happened. Where the camera or lidar fails outright, that filtered estimate is what carries the robot through, and a pipeline built this way degrades instead of breaking.
+
+Which is why the robust arrangement is rarely one of them alone: feed wheel odometry in as `guess_frame_id` and the registration starts near the answer every frame. That covers the camera's fast-motion and blank-wall failures and the lidar's degenerate-corridor failure, while the camera or lidar in turn corrects the wheels' drift. See [Feeding in an external guess](#feeding-in-an-external-guess).
+
+**For 2D indoor odometry a lidar usually costs less computation.** Registering a few hundred scan points is far less CPU than detecting, describing and matching visual features on every frame, and it needs no GPU — which is what decides whether odometry keeps up on the small onboard computers these robots carry.
+
+With both a camera and a lidar, the usual arrangement is `icp_odometry` for the pose and the camera for appearance — see [Combining a camera and a lidar](doc/icp_odometry.md#combining-a-camera-and-a-lidar).
+
+## Library
+
+The package installs a C++ library, documented in the [C++ API reference](https://docs.ros.org/en/jazzy/p/rtabmap_odom/generated/index.html) generated from the headers.
+
+**`OdometryROS`** is the base class all three nodes derive from, and it is where most of this package's behaviour actually lives. It owns the RTAB-Map `Odometry` object, the pose integration, the TF broadcast, the IMU intake, the services and the diagnostics. Each node subclasses it to do one thing: turn its own topics into a `rtabmap::SensorData` and hand it over. That is why the three nodes share nearly all of their parameters and publish exactly the same topics.
+
+It also runs the registration on **its own thread**. A frame arriving while the previous one is still being processed does not block the subscription callback; see [Update rates and dropped frames](#update-rates-and-dropped-frames).
+
+## Conventions
+
+These apply to all three nodes.
+
+### Frames and TF
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `frame_id` | `string` | `"base_link"` | The robot frame being tracked. The pose published is this frame's, not the sensor's — the sensor-to-robot transform is read from TF. |
+| `odom_frame_id` | `string` | `"odom"` | The fixed frame the pose is expressed in. |
+| `publish_tf` | `bool` | `true` | Broadcast the pose on TF. **Turn this off if something else already publishes that transform**, or the two fight and TF alternates between them. What exactly is broadcast depends on `guess_frame_id`: without it, `odom_frame_id` → `frame_id`; with it, a correction `odom_frame_id` → `guess_frame_id` ([why](#it-also-keeps-tf-alive-through-a-failure)). |
+| `wait_for_transform` | `double` | `0.1` | Seconds to wait for a needed transform before giving up on the frame. |
+| `initial_pose` | `string` | `""` | Starting pose, `"x y z roll pitch yaw"`. Also settable at runtime through `reset_odom_to_pose`. |
+| `ground_truth_frame_id` | `string` | `""` | When set, the pose is taken from this TF instead of being computed — for replaying a dataset with a known trajectory. |
+| `ground_truth_base_frame_id` | `string` | value of `frame_id` | The robot frame within the ground truth TF tree. |
+| `guess_frame_id` | `string` | `""` | A frame carrying another odometry source, used as the initial guess for each registration. Documented with its companions under [Feeding in an external guess](#feeding-in-an-external-guess) -- **the highest-value parameter here for a wheeled robot**. |
+
+The sensor must be connected to `frame_id` in TF **before the first frame arrives**, or that frame is dropped with a warning. A static publisher is the usual answer.
+
+### RTAB-Map's own parameters
+
+Everything in RTAB-Map's odometry parameter set is exposed as a ROS parameter **under its RTAB-Map name**, so tuning is done directly:
+
+```bash
+ros2 run rtabmap_odom rgbd_odometry --ros-args \
+  -p "Odom/Strategy:='1'" \
+  -p "Vis/MinInliers:='15'" \
+  -p "Odom/ResetCountdown:='1'"
+```
+
+**Note the quoting.** Every RTAB-Map parameter is declared as a **string**, whatever it looks like, because that is how RTAB-Map's own parameter map stores them. Writing `-p Odom/Strategy:=1` makes ROS infer an integer, and the node throws on startup rather than starting with the wrong value:
+
+```
+parameter 'Odom/Strategy' has invalid type: Wrong parameter type,
+parameter {Odom/Strategy} is of type {string}, setting it to {integer} is not allowed.
+```
+
+The inner quotes are what keeps it a string. Shell quotes alone do not help, since the value is parsed as YAML after the shell is done with it. In a launch file the same rule reads naturally: `{'Odom/Strategy': '1'}`.
+
+This applies **only** to RTAB-Map's own parameters. The nodes' ROS parameters -- `frame_id`, `publish_tf`, `scan_voxel_size`, `approx_sync` -- are declared with their real types and take plain values.
+
+Which parameters exist depends on the node: each declares the set matching its sensor, so `Vis/*` appears on the visual nodes and `Icp/*` only on `icp_odometry`. `ros2 param list` on a running node is the authoritative list; the meaning of each is in [RTAB-Map's parameter reference](https://github.com/introlab/rtabmap/blob/master/corelib/include/rtabmap/core/Parameters.h).
+
+The two worth knowing before anything else:
+
+- **`Odom/Strategy`** selects the registration algorithm — `0` frame-to-map (default, more accurate), `1` frame-to-frame (cheaper), and others for the external VO libraries RTAB-Map can be built against.
+- **`Odom/ResetCountdown`** automatically resets odometry after this many consecutive lost frames instead of staying lost forever. `0` disables it, which is the default.
+
+`config_path` loads the same parameters from an INI file; only the odometry ones are taken from it.
+
+### Feeding in an external guess
+
+Registration works far better when it starts near the answer. Two ways to supply one:
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `guess_frame_id` | `string` | `""` | A TF frame carrying another odometry source — wheels, IMU-integrated, a base driver. Its motion between frames becomes the initial guess. |
+| `guess_min_translation` | `double` | `0.0` | Skip frames whose guessed motion is below this, in meters. `0` disables. |
+| `guess_min_rotation` | `double` | `0.0` | Same, in radians. |
+| `guess_min_time` | `double` | `0.0` | Same, in seconds. |
+| `guess_linear_variance` | `double` | `0.001` | Covariance of the published pose when the guess is used directly. |
+| `guess_angular_variance` | `double` | `0.001` | Same, rotational. |
+
+**`guess_frame_id` is the single biggest improvement available to a wheeled robot.** Wheel odometry is locally excellent and globally hopeless; visual and ICP registration is the reverse. Giving the registration a wheel-odometry guess makes it converge more often, faster, and survive the frames where the camera sees nothing.
+
+The `guess_min_*` parameters additionally suppress processing while the robot is stationary, which stops a static scene from accumulating drift and saves the CPU.
+
+#### It also keeps TF alive through a failure
+
+Setting `guess_frame_id` also changes *how* the pose is broadcast. This is worth understanding before odometry fails on a real robot, because it decides what the rest of the system sees while it is lost.
+
+With a guess frame configured, the node no longer publishes `odom_frame_id` → `frame_id` directly. It publishes a **correction** instead, `odom_frame_id` → `guess_frame_id`.
+
+So the guess source keeps the conventional `odom` frame -- whatever produces it, the robot's driver or [`robot_localization`](https://docs.ros.org/en/jazzy/p/robot_localization/) -- and **this node takes a different name for its own `odom_frame_id`**. The examples in this repository use `vo` for the visual nodes and `icp_odom` for the lidar one:
+
+```mermaid
+flowchart TD
+    ODOM(["/vo<br><i>odom_frame_id</i>"])
+    GUESS(["/odom<br><i>guess_frame_id</i>"])
+    BASE(["/base_link<br><i>frame_id</i>"])
+    SENSOR(["/camera or /lidar<br><i>the sensor's header.frame_id</i>"])
+    ODOM -->|correction, this node<br>e.g. ~10 Hz, ~50 ms delay| GUESS
+    GUESS -->|robot driver or robot_localization<br>e.g. ~50 Hz, ~1 ms delay| BASE
+    BASE -->|static| SENSOR
+```
+
+*The rates and delays above are examples only — yours depend on the sensor, the base driver and the computer.*
+
+**That chain keeps being published while registration is lost.** The correction freezes at the last successfully computed pose composed with the motion the guess has accumulated since, so `base_link` keeps moving in TF at the guess source's rate, driven entirely by the guess. Nothing downstream stalls or jumps; the pose just accumulates that source's drift until registration recovers. Without `guess_frame_id` there is no correction to publish and **no TF at all is broadcast while lost**, which is what breaks the tree.
+
+Pair it with `Odom/ResetCountdown` and the recovery is complete. Take a robot turning to face a white wall: visual odometry loses tracking, TF keeps flowing from the wheels, and after the configured number of failed frames the odometry resets — not to where it was when it got lost, but to `last computed pose × guess motion`, which is where the wheels say the robot has got to in the meantime. Registration restarts from there and the trajectory carries on with only the drift the wheels accumulated.
+
+What it resets to depends on what is available, in this order:
+
+1. **A guess** — resets to the last pose composed with the guess motion, as above.
+2. **No guess, but `odom_frame_id` → `frame_id` exists in TF at the sensor frame's stamp** — resets to that pose. This is the `publish_tf:=false` arrangement: this node publishes only its odometry topic, [`robot_localization`](https://docs.ros.org/en/jazzy/p/robot_localization/) fuses that topic with the wheels and the IMU, and the filter owns the transform. The reset therefore lands on the filter's current estimate — the odometry gets restarted from where the fused solution says the robot is, having contributed to that solution itself while it was working. `publish_tf` has to be off for this to mean anything, and **`publish_null_when_lost` should be off too**: the null pose is a signal for consumers that read it as one, and a filter fusing this topic is not — it would be handed an invalid pose to fuse. With it off the node simply stops publishing while lost, and the filter carries on from its other inputs until registration recovers.
+3. **Neither** — resets to the last computed pose, so the robot resumes believing it never moved while lost.
+
+After an automatic reset the countdown is left armed, so if odometry still cannot initialize on the next frames it keeps re-resetting to the latest guess rather than getting stuck.
+
+### IMU
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `wait_imu_to_init` | `bool` | `false` | Hold off until an IMU message has arrived, so gravity is known from the first frame. |
+| `imu_queue_size` | `int` | `200` | Depth of the IMU buffer. IMUs run far faster than cameras; this is why it is large. |
+| `qos_imu` | `int` | value of `qos` | Reliability of the `imu` subscription. |
+| `always_check_imu_tf` | `bool` | `false` | Re-read the IMU-to-robot transform every message rather than caching it. |
+
+The `imu` topic is optional on all three nodes. Supplying it lets odometry know which way is down, which constrains roll and pitch — worth doing on any robot that has an IMU, and close to mandatory for a handheld or aerial one.
+
+**With no `guess_frame_id`, the IMU also supplies the rotation half of each frame's guess.** The rotation measured between the previous frame and this one becomes the guess's orientation, leaving the translation to the motion model. That is often the difference between tracking a fast turn and losing it, since rotation is what breaks feature matching first. An external guess takes precedence when there is one: `guess_frame_id` is used whole, and the IMU is not consulted for the guess at all.
+
+### Update rates and dropped frames
+
+Registration runs on its own thread, so a slow frame does not block the subscription. What happens to the frames arriving meanwhile is a choice:
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `always_process_most_recent_frame` | `bool` | `true` | Drop frames that arrive while registration is still running and take the newest. `false` registers every frame in order, on the subscription thread. |
+| `expected_update_rate` | `double` | `0.0` | The rate frames are expected at, in Hz. Used only when `max_update_rate` is unset, and it is a **ceiling**: a frame arriving sooner than `1/expected_update_rate` after the last one is skipped, with a warning that the input is faster than expected. `0` disables. |
+| `max_update_rate` | `double` | `0.0` | Throttle registration to at most this rate, skipping frames silently. Takes precedence over `expected_update_rate`. `0` disables. |
+| `min_update_rate` | `double` | `0.0` | Treat odometry as **lost and reset it** when more than `1/min_update_rate` passes between updates — the motion assumption no longer holds across a gap that long. `0` disables. |
+
+**`always_process_most_recent_frame` already bounds the delay.** A frame arriving while registration is still running is dropped on the spot rather than queued, so the worker always picks up the newest frame and the published pose is at most one registration behind the sensor. No backlog ever forms. `/diagnostics` reports how many frames went this way.
+
+That is why **`max_update_rate` is about CPU, not latency**: given the skipping above, the worst-case delay is roughly the same whether it is set or not. What it changes is how many frames get registered at all. Set it to give the rest of the robot its cores back — not to make the pose fresher, which it will not do.
+
+Setting `always_process_most_recent_frame:=false` is the opposite trade: every frame is registered, in order, on the subscription thread. That is what you want when replaying a bag, where dropping frames loses data you meant to process.
+
+### Lost frames, resets and new maps
+
+A frame that cannot be registered is *lost*: the node publishes an all-zero pose with `9999` down the diagonal of both covariance matrices, which says there is no pose here to use.
+
+The first frame after a reset — from `reset_odom`, `reset_odom_to_pose` or `Odom/ResetCountdown` — carries the same `9999` for a different reason. It is an *initialization* rather than a registration: nothing to measure against, no velocity to carry over. Its pose is real and meant to be used; what the covariance says is that it does not continue the last valid one.
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `publish_null_when_lost` | `bool` | `true` | Publish a null pose, with `9999` on the covariance diagonals, when a frame cannot be registered. `false` publishes nothing. |
+
+**Leave it on, unless a filter is consuming this topic.** A consumer that sees the null message knows odometry is lost; one that sees nothing cannot tell that apart from a node that died or a topic that was never connected. `rtabmap` relies on it to know the frame should not be mapped.
+
+`rtabmap` reads an identity pose, or both covariances at `9999`, as a discontinuity, and starts a **new map** rather than deforming the graph across a jump the robot never made:
+
+```
+Odometry is reset (identity pose or high variance detected). Increment map id!
+```
+
+While it is lost, `publish_null_when_lost:=true` publishes a null pose and no velocity for every frame, both marked `9999`. With `:=false` it publishes nothing — except that a guess frame keeps it going: every frame that re-initialises the map, which `Odom/ResetCountdown` makes frequent, is published with the guess's pose and confidence, so the topic has no gap for as long as the guess is there. What differs between configurations is the first frame after the reset, and where it restarts from:
+
+| | First frame after the reset | Second frame | TF while lost | `rtabmap` |
+|---|---|---|---|---|
+| `publish_null_when_lost:=true` (default), with or without a guess | recovered pose, `9999` on both pose and velocity | registered, from the recovered pose | unbroken with a guess, absent without one | new map |
+| `publish_null_when_lost:=false` with `guess_frame_id` | recovered pose and the guess's velocity, both with the guess's covariance | registered, from the recovered pose | unbroken | one session |
+| `publish_null_when_lost:=false`, `publish_tf:=false`, another node publishing `odom` → `base_link` | not published | registered, from the recovered pose | unbroken, published by the other node | one session |
+| `publish_null_when_lost:=false` with neither | not published | registered, from the pose held before the loss | absent until it recovers | one session, across the gap |
+
+*Registered* is the ordinary case: a pose and a velocity measured against the previous valid frame, with the covariance the registration computed.
+
+The two middle rows are the ones to build on. Either an external source is named through `guess_frame_id`, and the restarting frame is published as a continuation of the trajectory — the poses *and* the covariances staying continuous for as long as that guess is published — or a filter such as `robot_localization` owns `odom` → `base_link`, and the reset adopts whatever pose it holds.
+
+**The last row is a trap.** With nothing to say where the robot went while the odometry was lost, the reset resumes at the pose from before it, that motion is dropped from the trajectory, and since no `9999` ever reaches `rtabmap` the map is deformed across the gap rather than split at it. The node reports that combination as an error at startup.
+
+For finer control, put an intermediate node between this one and `rtabmap` and let it set the covariances itself. It decides what counts as a discontinuity, instead of that being inferred from the reset alone — starting a new map when the guess frame has gone quiet, say, and the newly computed pose may be wrong even though registration reported success.
+
+Recovering from lost is what `Odom/ResetCountdown` is for, or the `reset_odom` service. Combined with `guess_frame_id` it also keeps the TF tree intact throughout — see [It also keeps TF alive through a failure](#it-also-keeps-tf-alive-through-a-failure).
+
+## Services
+
+| Service | Type | Description |
+|---|---|---|
+| `reset_odom` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Drop the internal local map and restart the pose — at the identity, or, with `guess_frame_id` configured, at whatever pose the guess frame currently holds, so that odometry restarts where the other source says the robot is. |
+| `reset_odom_to_pose` | [`rtabmap_msgs/srv/ResetPose`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/ResetPose.html) | Reset to a given `x y z roll pitch yaw`. |
+| `pause_odom` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Stop processing incoming frames. |
+| `resume_odom` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Resume. |
+| `log_debug`, `log_info`, `log_warning`, `log_error` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Change RTAB-Map's own log level at runtime. |
+
+## Published topics
+
+Common to all three nodes. **Every one of them, `odom` included, is published only when something is subscribed** -- the work of building each message is skipped otherwise. The TF broadcast is not gated this way and happens whenever `publish_tf` is on -- except while registration is lost with no guess frame configured, when there is nothing to broadcast.
+
+| Topic | Type | Description |
+|---|---|---|
+| `odom` | [`nav_msgs/msg/Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) | The pose and velocity. Covariance is meaningful: it grows with registration uncertainty, and is `9999` on the diagonal when lost. |
+| `odom_info` | [`rtabmap_msgs/msg/OdomInfo`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/OdomInfo.html) | Everything about how the frame was registered — inlier count, matches, features, timings. The first thing to look at when odometry misbehaves. |
+| `odom_info_lite` | [`rtabmap_msgs/msg/OdomInfo`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/OdomInfo.html) | The same without the per-feature arrays, for logging or a slow link. |
+| `odom_local_map` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The feature map the current frame was registered against. Visual paths only — built from the frame's visual words, so `icp_odometry` never fills it. |
+| `odom_local_scan_map` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The scan map, the ICP path's equivalent of `odom_local_map`. |
+| `odom_last_frame` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The current frame's **features**, in the odom frame — not its scan or its pixels. Visual paths only, for the same reason as `odom_local_map`; for the filtered scan see `odom_sensor_data/*`. |
+| `odom_rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The frame **as odometry processed it**, not the input as it arrived. See [Outputting filtered scans and features](#outputting-filtered-scans-and-features). |
+| `odom_sensor_data/raw`, `/features`, `/compressed` | [`rtabmap_msgs/msg/SensorData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/SensorData.html) | The same frame as `SensorData`. `/features` strips the images and scan and keeps only the extracted features; `/compressed` carries JPEG/PNG images instead of raw. |
+
+### Outputting filtered scans and features
+
+`odom_rgbd_image` and `odom_sensor_data/*` republish the frame **after** odometry has worked on it, which is the point of them — they are what odometry actually registered, not a copy of the input:
+
+- **Features are included.** Registration writes the keypoints, their 3D positions and their descriptors back into the frame, so these topics carry them. `odom_sensor_data/features` is that alone, with the images and scan removed.
+- **The scan is the filtered one.** `icp_odometry` builds the frame after deskewing, voxelization, range filtering and normal estimation, so what comes out here is the decimated cloud ICP saw — not the raw sweep the lidar published. Subscribe to the driver's topic if you want the original.
+- **Images are converted.** `rgbd_odometry` hands over grayscale unless `keep_color` is set, so that is what these carry too.
+
+## Diagnostics
+
+All three publish to `/diagnostics`: the input rate, the output rate, and how many frames were processed versus dropped. A healthy input rate with a low output rate means frames are arriving but not registering — check `odom_info` before touching anything else.
+
+## License
+
+BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license).

rtabmap_odom/doc/icp_odometry.md

@@ -0,0 +1,238 @@
+# icp_odometry
+
+Odometry from a 2D or 3D lidar, by registering each scan against the previous one with ICP.
+
+No features and no appearance: the motion is whatever transform best aligns this scan's points with the last. That makes it indifferent to lighting and texture — it works in the dark, and on the blank white corridor where [rgbd_odometry](rgbd_odometry.md) has nothing to track.
+
+What it is sensitive to instead is **geometry**. ICP can only recover motion that the scene's shape constrains, and a scene can fail to constrain it: see [Degenerate geometry](#degenerate-geometry), which is the failure mode worth understanding before deploying this.
+
+The shared parameters — frames, TF, guesses, the IMU, RTAB-Map's own, the services — are in the [package README](../README.md#conventions). This page covers what is specific to this node.
+
+## Contents
+
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+  - [Reusing the filtered scan downstream](#reusing-the-filtered-scan-downstream)
+- [Parameters](#parameters)
+  - [Where these defaults come from](#where-these-defaults-come-from)
+  - [Making the correspondence ratio mean something](#making-the-correspondence-ratio-mean-something)
+- [Preparing the scan](#preparing-the-scan)
+- [Deskewing](#deskewing)
+- [Degenerate geometry](#degenerate-geometry)
+- [Combining a camera and a lidar](#combining-a-camera-and-a-lidar)
+- [When it loses track](#when-it-loses-track)
+
+## Usage
+
+2D lidar:
+
+```bash
+ros2 run rtabmap_odom icp_odometry --ros-args \
+  -r scan:=/scan \
+  -p frame_id:=base_link
+```
+
+3D lidar:
+
+```bash
+ros2 run rtabmap_odom icp_odometry --ros-args \
+  -r scan_cloud:=/velodyne_points \
+  -p frame_id:=base_link \
+  -p "Icp/PointToPlane:='true'" \
+  -p scan_normal_k:=10 \
+  -p scan_voxel_size:=0.1
+```
+
+```python
+ComposableNode(
+    package='rtabmap_odom',
+    plugin='rtabmap_odom::ICPOdometry',
+    name='icp_odometry',
+    parameters=[{'frame_id': 'base_link',
+                 'scan_voxel_size': 0.1,
+                 'scan_normal_k': 10,
+                 'Icp/PointToPlane': 'true'}],
+    remappings=[('scan_cloud', '/velodyne_points')])
+```
+
+## Subscribed Topics
+
+One of the two scan topics, not both.
+
+| Topic | Type | Description |
+|---|---|---|
+| `scan` | [`sensor_msgs/msg/LaserScan`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/LaserScan.html) | A 2D lidar. |
+| `scan_cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | A 3D lidar, or a 2D one already converted to a cloud. |
+| `imu` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Optional, and more useful here than elsewhere — it pins roll and pitch, which a lidar alone constrains poorly. |
+
+## Published Topics
+
+Most are common to all three nodes; see [the README](../README.md#published-topics). Two belong to this path:
+
+| Topic | Type | Description |
+|---|---|---|
+| `odom_local_scan_map` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The accumulated scan map the current scan was registered against. |
+| `odom_filtered_input_scan` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The input scan **after** deskewing, voxelization, range filtering and normal estimation — exactly what ICP registered, carrying the original header. |
+
+### Reusing the filtered scan downstream
+
+Remap `odom_filtered_input_scan` onto `rtabmap`'s `scan_cloud` and the map is built from the scan this node already prepared, rather than from the raw sweep.
+
+```mermaid
+flowchart LR
+    LIDAR["lidar"]
+    ICP["icp_odometry"]
+    MAP["rtabmap<br>subscribe_scan_cloud:=true"]
+    LIDAR -->|scan_cloud| ICP
+    ICP -->|odom_filtered_input_scan| MAP
+    ICP -->|odom + TF| MAP
+```
+
+That skips the expensive half twice over. Voxelization and normal estimation are not repeated, since the cloud arrives already decimated and carrying `normal_*` fields, and the deskewing this node did is carried along with it.
+
+The alternative for deskewing is to do it **before** odometry, with [`lidar_deskewing`](../../rtabmap_util/doc/lidar_deskewing.md), and fan the corrected cloud out to both nodes:
+
+```mermaid
+flowchart LR
+    LIDAR["lidar"]
+    DESKEW["lidar_deskewing"]
+    ICP["icp_odometry"]
+    MAP["rtabmap<br>subscribe_scan_cloud:=true"]
+    LIDAR -->|scan_cloud| DESKEW
+    DESKEW -->|deskewed cloud| ICP & MAP
+    ICP -->|odom + TF| MAP
+```
+
+That is the only way to give `rtabmap` **every point of the sweep**. `odom_filtered_input_scan` carries the decimated cloud ICP registered, so a map built from it inherits whatever voxelization odometry applied — and outdoors that is 30 to 50 cm. Deskewing upstream separates the two: odometry can filter as hard as it likes while the map is built from the full-resolution cloud, at the cost of an extra node and an extra copy of every sweep.
+
+## Parameters
+
+Specific to this node: the scan is filtered **before** ICP sees it, and these control that. The registration itself is tuned through RTAB-Map's `Icp/*` parameters.
+
+These two sets overlap, and the node resolves the overlap for you — see [Where these defaults come from](#where-these-defaults-come-from), because the defaults below are not what the source's initializers suggest.
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `scan_voxel_size` | `double` | `0.05` | Downsample to one point per voxel of this size, in meters. From `Icp/VoxelSize`. `0` disables it here. |
+| `scan_downsampling_step` | `int` | `1` | Keep every Nth point. From `Icp/DownsamplingStep`. Cheaper than voxelization but density-dependent; prefer `scan_voxel_size`. |
+| `scan_range_min` | `double` | `0.0` | Drop points closer than this, in meters. From `Icp/RangeMin`. `0` disables. Useful against the robot's own body. |
+| `scan_range_max` | `double` | `0.0` | Drop points farther than this. From `Icp/RangeMax`. `0` disables. |
+| `scan_normal_k` | `int` | `5` | Estimate each point's normal from this many neighbours. From `Icp/PointToPlaneK`. **Point-to-plane ICP needs normals**; without them it has nothing to work with. |
+| `scan_normal_radius` | `double` | `0.0` | Estimate normals from neighbours within this radius instead. From `Icp/PointToPlaneRadius`. `0` disables. |
+| `scan_normal_ground_up` | `double` | `0.0` | Force normals to point upward when within this dot-product threshold of vertical. From `Icp/PointToPlaneGroundNormalsUp`. Helps on ground planes. |
+| `scan_cloud_max_points` | `int` | `-1` | How many points a full sweep of the lidar holds. It is the **denominator of the correspondence ratio** — see [Making the correspondence ratio mean something](#making-the-correspondence-ratio-mean-something). `-1` leaves it unset; an organized cloud fills it in automatically. |
+| `scan_cloud_is_2d` | `bool` | `false` | Treat `scan_cloud` as a planar scan even though it carries a `z` field, so it is registered as a 2D scan rather than a 3D one. For a 2D lidar already converted to a cloud. |
+| `deskewing` | `bool` | `false` | Correct for motion during the sweep. See [Deskewing](#deskewing). |
+| `deskewing_slerp` | `bool` | `false` | Interpolate the deskewing transform rather than looking up TF per point. Faster, slightly less accurate. |
+| `topic_queue_size` | `int` | `1` | Queue depth of the scan subscription. Deliberately small: a stale scan is worse than a dropped one. |
+
+### Where these defaults come from
+
+Filtering a scan and filtering it again inside ICP would be wasted work, so at startup the node moves each filter from RTAB-Map's parameter to its own:
+
+> `IcpOdometry: Transferring value 5 of "Icp/PointToPlaneK" to ros parameter "scan_normal_k" for convenience.`
+
+That log line is normal, not a warning about your configuration. Each `scan_*` parameter above **takes its default from the matching `Icp/*` parameter**, and the `Icp/*` one is then set to `0` so the filter runs once, here, rather than twice. This is why `scan_voxel_size` is `0.05` and `scan_normal_k` is `5` out of the box rather than disabled.
+
+Setting the ROS parameter explicitly wins: the transfer is skipped and the `Icp/*` value is zeroed instead. Setting **both** is the case to avoid — the node warns that both are set, and the scan is then filtered twice:
+
+```
+IcpOdometry: Both parameter "Icp/VoxelSize" and ros parameter "scan_voxel_size" are set.
+```
+
+So tune through `scan_*` **or** through `Icp/*`, not both.
+
+### Making the correspondence ratio mean something
+
+`Icp/CorrespondenceRatio` decides whether a registration is trustworthy: the points ICP managed to pair, over the points it could have paired. `scan_cloud_max_points` is what sets that second number. Set it to the theoretical maximum points per sweep. No need to set it explicitly for organized clouds though, `width × height` will be used as maximum points.
+
+Left at `-1`, the denominator becomes the size of the larger of the two scans being matched (for dense clouds). Take two scans that came back with 30 and 50 points — a lidar staring at open space, where most rays returned nothing. Dividing by 50 says "we matched most of what we saw", and the ratio looks healthy. But the sensor emits 10000 rays a sweep, so 50 returns means almost nothing was in range, and the registration is resting on nearly no evidence. Told that a full sweep is 10000 points, ICP divides by that instead and the ratio collapses to what the overlap actually was, so the threshold rejects the frame.
+
+## Preparing the scan
+
+ICP cost grows with the number of points, and a 3D lidar produces far more than registration needs — a 64-beam sensor is a hundred thousand points per sweep, and aligning them all is both slow and *no more accurate* than aligning a well-spread subset. Voxelization is therefore on by default at 5 cm.
+
+What it buys beyond speed is even density, which matters more than the point count: a raw lidar sweep is dense near the sensor and sparse far away, so an unvoxelized ICP is dominated by whatever is closest — often the robot itself or the ground right under it. Size `scan_voxel_size` to the environment: **0.05 to 0.2 m indoors**, and **0.3 to 0.5 m outdoors**, where the scene is far larger and the extra resolution buys nothing but CPU.
+
+**Move `Icp/MaxCorrespondenceDistance` with it — a good rule of thumb is ten times the voxel size.** The two are coupled: voxelizing at 0.3 m leaves neighbouring points that far apart, so a correspondence distance of 0.1 m cannot pair anything and ICP returns nothing at all.
+
+**Point-to-plane ICP converges better than point-to-point** on the flat surfaces that dominate most environments, and it is what the default `scan_normal_k` of 5 is there to support:
+
+```bash
+-p "Icp/PointToPlane:='true'" -p scan_normal_k:=10
+```
+
+The quoting is not optional: RTAB-Map parameters are strings, and an unquoted `true` makes the node throw on startup ([why](../README.md#rtab-maps-own-parameters)).
+
+Whether it is on by default depends on how RTAB-Map was built — `Icp/PointToPlane` defaults to `true` only with libpointmatcher available, and `false` otherwise — so set it explicitly if you care.
+
+`scan_range_min` is worth setting on any robot whose lidar can see parts of itself. Those points are perfectly self-consistent between scans, so they pull the alignment toward "no motion" — a bias that looks like the robot under-travelling rather than like an error.
+
+## Deskewing
+
+A spinning lidar measures its points over a whole revolution, not at an instant. If the robot moves during that revolution, the scan is a smear — the points are in a frame that no longer exists by the time the sweep ends. At walking pace with a 10 Hz lidar this is centimeters; on a fast vehicle it dominates the error budget.
+
+`deskewing:=true` corrects it. It needs the cloud to carry **per-point timestamps**, in a field named `t`, `time`, `stamps` or `timestamp`; without one the node logs an error and drops the frame rather than guessing.
+
+There are two ways it gets the motion to correct with, and which one is used depends on whether you gave it an external guess:
+
+- **With `guess_frame_id` set** — the motion comes from that TF. This is the accurate route, and the reason to pair deskewing with wheel odometry or an IMU-integrated frame.
+- **Without it** — a constant-velocity model from the previous frame's estimate. It cannot deskew the very first frame, and it degrades exactly when velocity changes fastest, which is when deskewing matters most.
+
+`deskewing_slerp` interpolates between the sweep's endpoints rather than looking up a transform per point. Much cheaper, and accurate enough unless the motion within one sweep is strongly non-linear.
+
+## Degenerate geometry
+
+This is the failure that matters, and it is not a bug. ICP recovers only the motion the scene constrains, and some scenes do not constrain all of it:
+
+- **A long featureless corridor** does not constrain motion *along* the corridor. The walls look identical a meter forward, so ICP happily reports no motion while the robot drives. The map then folds the corridor up into a fraction of its length.
+- **A large open space** with everything out of range constrains nothing at all.
+- **A flat plane** — a warehouse floor to a horizontal 2D lidar — constrains height and tilt but not translation.
+
+RTAB-Map detects this rather than walking into it, but only on the point-to-plane path. The defences, in order of effectiveness:
+
+1. **`guess_frame_id` with wheel odometry.** The guess supplies the motion ICP cannot see, and ICP corrects the part it can. This turns the corridor case from a failure into a non-issue, and it is why lidar odometry on a wheeled robot should essentially always have it.
+2. **The structural complexity check**, which is the built-in one. With `Icp/PointToPlane` on, a scan whose normals fail to span the space — the definition of a corridor — scores below `Icp/PointToPlaneMinComplexity` (`0.02`) and is handled by `Icp/PointToPlaneLowComplexityStrategy` instead of being trusted:
+
+   | Value | Behaviour |
+   |---|---|
+   | `0` | Reject the transform outright: the frame is reported lost. |
+   | `1` *(default)* | Recompute with point-to-point and constrain the correction to the axes that *are* observable — in a corridor, y and yaw are kept and **x is taken from the guess**. |
+   | `2` | Recompute with point-to-point and accept the result as is. |
+   | `3` | Keep the point-to-plane transform, with the same axis-constrained projection as `1`. |
+
+   The default pairs with defence 1: it detects the unobservable axis and hands that axis to the guess. Without a `guess_frame_id` there is nothing to hand it to, which is why the two belong together.
+3. **A 3D lidar instead of a 2D one**, which sees ceiling, floor and doorways that a horizontal slice misses.
+4. **`Icp/CorrespondenceRatio`** to reject registrations supported by too few correspondences, so a bad frame is reported lost rather than silently accepted.
+
+With `Icp/PointToPlane` off, none of the complexity machinery runs: there are no normals to measure, so a degenerate scan is registered and trusted like any other.
+
+## Combining a camera and a lidar
+
+With both sensors, the usual arrangement is `icp_odometry` for the pose and the camera for appearance:
+
+```mermaid
+flowchart LR
+    LIDAR["lidar"]
+    CAM["RGB-D camera"]
+    ICP["icp_odometry"]
+    SYNC["rgbd_sync"]
+    ODOM(["odom + TF"])
+    MAP["rtabmap<br>subscribe_rgbd + subscribe_scan_cloud"]
+    LIDAR --> ICP --> ODOM --> MAP
+    LIDAR --> MAP
+    CAM --> SYNC --> MAP
+```
+
+Lidar geometry is the more reliable pose source, while loop closure detection is appearance-based and wants the images. `rtabmap` then subscribes to the camera, the scan and this node's odometry together.
+
+## When it loses track
+
+`odom_info` carries the ICP result. The numbers to look at are the correspondence count and ratio: too few correspondences means the scans do not overlap enough, whether because the robot moved too far between them, the range filters are too aggressive, or the scene genuinely changed.
+
+- **Scans too far apart** — the lidar rate is too low for the speed, or `max_update_rate` is throttling too hard.
+- **`Icp/MaxCorrespondenceDistance` too small** — ICP never associates the points at all. It has to be larger than the motion between scans; too large and it associates the wrong things.
+- **Everything filtered away** — check `scan_range_min`/`scan_range_max` and `scan_voxel_size` against the actual scale of the environment.
+
+As everywhere else in this package, `Odom/ResetCountdown` recovers automatically from a lost state instead of staying lost.

rtabmap_odom/doc/rgbd_odometry.md

@@ -0,0 +1,211 @@
+# rgbd_odometry
+
+Visual odometry from a color image, a registered depth image and a calibration.
+
+Each frame's visual features are matched against the previous frame — or against a small local map of recent features — and the camera motion that best explains the matches becomes the pose. Depth turns the 2D feature matches into 3D correspondences, which is what makes the scale real rather than arbitrary.
+
+Use it when an RGB-D camera is the main sensor. For a stereo pair use [stereo_odometry](stereo_odometry.md); for a lidar, [icp_odometry](icp_odometry.md). All three publish the same topics and share the parameters in the [package README](../README.md#conventions), which covers frames, TF, the RTAB-Map parameters, guesses, the IMU and the services. This page covers what is specific to this node.
+
+## Contents
+
+- [Pipeline arrangements](#pipeline-arrangements)
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Parameters](#parameters)
+- [Synchronization](#synchronization)
+- [Several cameras](#several-cameras)
+- [Repetitive patterns](#repetitive-patterns)
+- [When it loses track](#when-it-loses-track)
+
+## Pipeline arrangements
+
+A camera-only pipeline, with the camera synchronized once and fanned out -- the arrangement [Synchronization](#synchronization) recommends:
+
+```mermaid
+flowchart LR
+    CAM["RGB-D camera"]
+    SYNC["rgbd_sync"]
+    ODOM["rgbd_odometry<br>subscribe_rgbd:=true"]
+    MAP["rtabmap<br>subscribe_rgbd:=true"]
+    CAM -->|rgb/image<br>depth/image<br>rgb/camera_info| SYNC
+    SYNC -->|rgbd_image| ODOM & MAP
+    ODOM -->|odom + TF| MAP
+```
+
+Without `rgbd_sync`, both nodes subscribe to the three raw topics and each synchronizes them independently -- which works, but lets the two settle on different pairings.
+
+Another arrangement drops `rgbd_sync` altogether and feeds `rtabmap` from **this node's own output** instead:
+
+```mermaid
+flowchart LR
+    CAM["RGB-D camera"]
+    ODOM["rgbd_odometry"]
+    MAP["rtabmap<br>subscribe_rgbd or subscribe_sensor_data"]
+    CAM -->|rgb/image<br>depth/image<br>rgb/camera_info| ODOM
+    ODOM -->|odom_rgbd_image<br>or odom_sensor_data| MAP
+    ODOM -->|odom + TF| MAP
+```
+
+Remap `rtabmap`'s `rgbd_image` to `odom_rgbd_image`, or set `subscribe_sensor_data` and remap to `odom_sensor_data/raw`. Two things come for free:
+
+- **No separate synchronization.** This node already matched the three topics to register the frame, and republishes the result, so there is no `rgbd_sync` to run and no second synchronizer to agree with.
+- **The features are reused.** `odom_sensor_data` carries the keypoints, their 3D positions and their descriptors that odometry extracted; they survive the conversion back into RTAB-Map on the other side, so `rtabmap` does not redo feature detection and descriptor extraction.
+
+## Usage
+
+Against a camera's raw topics:
+
+```bash
+ros2 run rtabmap_odom rgbd_odometry --ros-args \
+  -r rgb/image:=/camera/color/image_raw \
+  -r depth/image:=/camera/depth/image_rect_raw \
+  -r rgb/camera_info:=/camera/color/camera_info \
+  -p frame_id:=base_link
+```
+
+Against an [`rgbd_sync`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_sync) output, which is the better arrangement when anything else consumes the same camera:
+
+```bash
+ros2 run rtabmap_odom rgbd_odometry --ros-args \
+  -p subscribe_rgbd:=true \
+  -r rgbd_image:=/camera/rgbd_image \
+  -p frame_id:=base_link
+```
+
+```python
+ComposableNode(
+    package='rtabmap_odom',
+    plugin='rtabmap_odom::RGBDOdometry',
+    name='rgbd_odometry',
+    parameters=[{'frame_id': 'base_link', 'subscribe_rgbd': True}],
+    remappings=[('rgbd_image', '/camera/rgbd_image')])
+```
+
+## Subscribed Topics
+
+Which topics are used depends on `subscribe_rgbd` and `rgbd_cameras`.
+
+**Default** — `subscribe_rgbd:=false`:
+
+| Topic | Type | Description |
+|---|---|---|
+| `rgb/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Color or mono image. Goes through `image_transport`. |
+| `depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Depth registered to the color camera. `16UC1` in millimeters or `32FC1` in meters. |
+| `rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Calibration of the color camera. |
+
+**With `subscribe_rgbd:=true`**, one pre-synchronized message instead of three topics:
+
+| `rgbd_cameras` | Topic | Type |
+|---|---|---|
+| `1` (default) | `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) |
+| `2`–`6` | `rgbd_image0` … `rgbd_image5` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) |
+| `0` | `rgbd_images` | [`rtabmap_msgs/msg/RGBDImages`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImages.html) |
+
+`rgbd_cameras:=0` takes any number of cameras in a single message, which is what [`rgbdx_sync`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_sync) produces — the route that needs no rebuild and the only one that goes past six.
+
+| Topic | Type | Description |
+|---|---|---|
+| `imu` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Optional. Constrains roll and pitch; see [the README](../README.md#imu). |
+
+## Published Topics
+
+`odom`, `odom_info`, `odom_local_map`, `odom_last_frame`, `odom_rgbd_image` and the rest are common to all three nodes and documented in [the README](../README.md#published-topics).
+
+## Parameters
+
+Specific to this node. The shared ones — `frame_id`, `publish_tf`, `guess_frame_id`, `max_update_rate`, all of RTAB-Map's own — are in [the README](../README.md#conventions).
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `subscribe_rgbd` | `bool` | `false` | Take a pre-synchronized `RGBDImage` instead of three raw topics. |
+| `rgbd_cameras` | `int` | `1` | Number of `RGBDImage` topics. `0` means one `RGBDImages` topic carrying any number. Only with `subscribe_rgbd:=true`. |
+| `approx_sync` | `bool` | `true` | Match the raw topics by nearest stamp. See [Synchronization](#synchronization). |
+| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. |
+| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. |
+| `sync_queue_size` | `int` | `5` | Queue depth of the synchronizer. |
+| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. Copied to it with a warning. |
+| `qos_camera_info` | `int` | value of `qos` | Reliability of the `rgb/camera_info` subscription alone. |
+| `keep_color` | `bool` | `false` | Keep the color image in the data handed to the odometry instead of converting to grayscale. Registration is grayscale either way; this only matters for what downstream consumers of `odom_rgbd_image` receive. |
+| `image_transport` | `string` | `"raw"` | Transport for `rgb/image`, e.g. `compressed`. |
+| `depth_transport` | `string` | `"raw"` | Transport for `depth/image`, e.g. `compressedDepth`. |
+| `rgb_transport` | `string` | — | **Deprecated**, renamed to `image_transport`. |
+
+## Synchronization
+
+With `subscribe_rgbd:=false` this node runs its own synchronizer over the three raw topics, and the same trade-off applies as everywhere else in this stack: **exact matching is cheaper and cannot mismatch, but publishes nothing at all if the stamps differ by a nanosecond**. The default here is approximate because many RGB-D cameras do not stamp color and depth identically.
+
+When several nodes consume the same camera — odometry and `rtabmap`, usually — **synchronize once with `rgbd_sync` and set `subscribe_rgbd:=true` on both**. Two independent approximate synchronizers over the same three topics can settle on different pairings, and then `rtabmap` maps a frame at a pose computed from a different one. Feeding both from one `RGBDImage` removes the possibility.
+
+`approx_sync_max_interval` is worth setting whenever approximate matching stays on: without it, a camera that stalls and resumes silently pairs a fresh color frame with a stale depth frame. A tenth of the frame period is a reasonable start.
+
+## Several cameras
+
+More cameras means more of the scene is textured enough to track, which is the usual reason visual odometry fails indoors. Point them in different directions rather than overlapping.
+
+Two routes, both requiring the frames to be synchronized and each camera to be in TF:
+
+- **`rgbd_cameras:=2..6`** subscribes to `rgbd_image0`…`rgbd_imageN` and synchronizes them here.
+- **`rgbd_cameras:=0`** takes one `rgbd_images` topic from `rgbdx_sync`, which has no upper limit and needs no rebuild.
+
+**RTAB-Map has to be built with OpenGV for this.** The default motion estimation is PnP (`Vis/EstimationType=1`, 3D→2D), and the multi-camera version of it lives in OpenGV. Without that dependency the registration refuses to run and says so:
+
+```
+Multi-camera 2D-3D PnP registration is only available if rtabmap is built with
+OpenGV dependency. Use 3D-3D registration approach instead for multi-camera.
+```
+
+Check with `rtabmap --version`, which prints a `With OpenGV:` line. If it says `false`, either rebuild RTAB-Map against OpenGV or switch to `Vis/EstimationType:='0'` (3D→3D), which needs no extra dependency but registers point cloud to point cloud rather than reprojecting, and is the weaker estimator when depth is noisy.
+
+**Hardware-synchronize the cameras if you can.** The node treats the set as one rigid observation at one timestamp: features from every camera are registered together, with the extrinsics from TF held fixed. There is no equivalent of lidar deskewing here — it cannot estimate the motion that happened *within* the rig between one camera's exposure and the next. If the cameras fire at different instants while the robot moves, that motion is absorbed as though the rig had flexed, and the registration is pulled off by however far the robot travelled in between.
+
+Synchronizing the topics is not the same thing: `approx_sync` only decides which frames are grouped, it cannot undo an exposure that happened 20 ms later than its neighbour's.
+
+Calibration matters more with several cameras than with one for the same reason: the extrinsics between them come from TF, and an error there shows up as a constant bias in the estimated motion rather than as an obvious failure.
+
+## Features computed elsewhere
+
+`RGBDImage` has fields for local features — `key_points`, `points` and `descriptors` — and when a frame arrives with them filled, this node hands them to the odometry as they are instead of detecting and describing anything. RTAB-Map extracts features only from a frame that brought none, so nothing is recomputed.
+
+This is for a camera, or a driver, that already does the extraction: on a multi-camera rig it is most of the per-frame work, and it can be done once and shared with `rtabmap` rather than repeated in each node.
+
+What a publisher has to get right:
+
+- **One entry per camera**, in the same order as the images, for `rgbd_cameras:=0` as well as the numbered topics.
+- **Keypoints in their own camera's image coordinates.** The node stitches the images side by side and shifts each camera's keypoints by the images that precede it.
+- **3D points in that camera's optical frame.** They are brought into `frame_id` with the camera's transform from TF, the same one used for the calibration.
+- **Descriptors compressed** with `rtabmap::compressData()`, one row per keypoint, the same type for every camera.
+- **Equal counts.** `points` and `descriptors` may be left empty, but if they are there, they must have as many entries as there are keypoints. A frame whose three disagree has its features dropped with an error rather than used out of step.
+
+Both images may be left out entirely: the depth image's job was to give the keypoints their depth and they arrive with it, and the color image's was to have features found in it. A frame is then its calibration and its features, which is the whole point — the images are nearly all of the bandwidth. The `camera_info` of each camera has to be there either way, as it is what says how big the image would have been and, through its `frame_id`, where the camera is.
+
+What stops applying, since nothing is extracted: `Vis/MaxFeatures`, `Vis/DepthAsMask`, the detector chosen with `Kp/DetectorStrategy`, and the depth bounds `Vis/MinDepth` and `Vis/MaxDepth`. Whatever is published is what gets registered, so the publisher owns those decisions. `Vis/CorType` must stay at `0` (feature matching); optical flow (`1`) reads the images themselves and has nothing to work with here.
+
+## Repetitive patterns
+
+Not every failure announces itself. A scene full of identical detail — a tiled floor, rows of identical shelving, a patterned carpet, a brick wall — hands the matcher plenty of features and plenty of confident matches, just not always the *right* ones. One tile matched to its neighbour looks like a perfectly good inlier, and the pose comes out shifted by exactly one tile. Inlier counts stay healthy, nothing is reported lost, and the trajectory drifts in steps.
+
+The defence is to constrain **where** a match is allowed to come from:
+
+- **`guess_frame_id`** gives each feature a predicted image position, from wheel odometry or another external source.
+- **`Vis/CorGuessWinSize`** bounds the search around that prediction — 40 pixels by default. Reducing it, to 10 or 20, means a feature can only match something close to where the guess says it should be, so the identical neighbour one tile away is never a candidate.
+
+## When it loses track
+
+Visual odometry fails when there is nothing to match: a blank wall, a dark room, motion blur, or a scene where everything moved. The node then publishes a null pose (see [the README](../README.md#lost-frames-resets-and-new-maps)) and `odom_info` says why.
+
+Look at `odom_info` first — `inliers` is the number that matters:
+
+```bash
+ros2 topic echo /odom_info --field inliers
+```
+
+Inliers falling below `Vis/MinInliers` (default 20) is the definition of a lost frame. Whether the fix is more features, a better guess or a different strategy depends on which part is short:
+
+- **Few features detected at all** — the scene is untextured or too dark. Lowering `Vis/MinInliers` lets frames register on fewer matches, but a pose resting on a handful of inliers is poorly constrained and drifts badly — it buys continuity at the price of accuracy. The real fixes are physical. If it is dark, add a light — a spotlight on the robot restores texture the camera can track, and costs far less than changing sensor. Otherwise it is **more field of view**: a wider lens, or [several cameras](#several-cameras) pointed in different directions. It only takes one textured patch somewhere in view to track, so a blank wall filling a narrow FOV stops being a problem the moment the rig can also see the ceiling or a doorway. Failing that, the camera is the wrong sensor here — a lidar if the scene has geometry, wheel odometry if it has neither. See [Choosing a sensor modality for the environment](../README.md#choosing-a-sensor-modality-for-the-environment).
+- **Features detected but few matched** — motion is too fast for the search window, or the frame rate is too low. A `guess_frame_id` from wheel odometry is what helps most here.
+- **Matched but rejected as outliers** — usually a moving scene, or depth that does not agree with the color image. Check that depth really is registered to color.
+
+`Odom/ResetCountdown` gets the node out of a lost state automatically instead of leaving it lost until something calls `reset_odom`.
+
+**A camera alone is not a great odometry source on a wheeled robot.** If the base publishes wheel odometry, feeding it in through `guess_frame_id` is worth more than any amount of tuning here.

rtabmap_odom/doc/stereo_odometry.md

@@ -0,0 +1,219 @@
+# stereo_odometry
+
+Visual odometry from a stereo pair.
+
+Features are found in the left image and matched into the right one to get their depth by disparity, then matched against the previous frame to get the motion. It is the same registration as [rgbd_odometry](rgbd_odometry.md); only the source of depth differs — computed here from the pair rather than measured by the sensor.
+
+That difference is the reason to choose it. A stereo pair works outdoors and at range, where the projected-pattern depth of an RGB-D camera returns nothing, and its accuracy degrades gracefully with distance instead of cutting off. The cost is that depth is only available where there is texture to match, and that it depends on a good stereo calibration.
+
+The shared parameters — frames, TF, guesses, the IMU, RTAB-Map's own parameters, the services — are in the [package README](../README.md#conventions). This page covers what is specific to this node.
+
+## Contents
+
+- [Pipeline arrangements](#pipeline-arrangements)
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Parameters](#parameters)
+- [Synchronization](#synchronization)
+- [Getting the scale right](#getting-the-scale-right)
+- [Features computed elsewhere](#features-computed-elsewhere)
+- [Repetitive patterns](#repetitive-patterns)
+- [When it loses track](#when-it-loses-track)
+
+## Pipeline arrangements
+
+A typical stereo pipeline, rectification included:
+
+```mermaid
+flowchart LR
+    CAM["stereo driver"]
+    PROC["stereo_image_proc"]
+    SYNC["stereo_sync"]
+    ODOM["stereo_odometry"]
+    MAP["rtabmap"]
+    CAM -->|left/image_raw<br>right/image_raw<br>camera_info x2| PROC
+    PROC -->|left/image_rect<br>right/image_rect<br>camera_info x2| SYNC
+    SYNC -->|rgbd_image| ODOM & MAP
+    ODOM -->|odom + TF| MAP
+```
+
+`stereo_image_proc` can be dropped when the driver already publishes rectified images:
+
+```mermaid
+flowchart LR
+    CAM["stereo driver<br>publishing rectified images"]
+    SYNC["stereo_sync"]
+    ODOM["stereo_odometry"]
+    MAP["rtabmap"]
+    CAM -->|left/image_rect<br>right/image_rect<br>camera_info x2| SYNC
+    SYNC -->|rgbd_image| ODOM & MAP
+    ODOM -->|odom + TF| MAP
+```
+
+The same shortcut as on the RGB-D side is available here: drop `stereo_sync` and feed `rtabmap` from **this node's own output**.
+
+```mermaid
+flowchart LR
+    CAM["stereo driver<br>publishing rectified images"]
+    ODOM["stereo_odometry"]
+    MAP["rtabmap<br>subscribe_rgbd or subscribe_sensor_data"]
+    CAM -->|left/image_rect<br>right/image_rect<br>camera_info x2| ODOM
+    ODOM -->|odom_rgbd_image<br>or odom_sensor_data| MAP
+    ODOM -->|odom + TF| MAP
+```
+
+Remap `rtabmap`'s `rgbd_image` to `odom_rgbd_image`, or set `subscribe_sensor_data` and remap to `odom_sensor_data/raw`. The stereo pair survives the trip intact -- the left image, the right image and both calibrations travel in the one message, exactly as `stereo_sync` would have packed them -- and the features this node extracted come with it, so `rtabmap` does not redo feature detection and descriptor extraction.
+
+Everything above hands the node rectified images. It can also take the raw pair, straight from the driver:
+
+```mermaid
+flowchart LR
+    CAM["stereo driver"]
+    ODOM["stereo_odometry<br>Rtabmap/ImagesAlreadyRectified:=false"]
+    MAP["rtabmap<br>subscribe_rgbd or subscribe_sensor_data"]
+    CAM -->|left/image_raw<br>right/image_raw<br>camera_info x2| ODOM
+    ODOM -->|odom_rgbd_image<br>or odom_sensor_data| MAP
+    ODOM -->|odom + TF| MAP
+```
+
+Two different things can make that work:
+
+- **The odometry rectifies the pair itself.** With `Rtabmap/ImagesAlreadyRectified:=false` it builds a rectification map from the calibration and applies it to every frame, saying so once:
+
+  ```
+  Rtabmap/ImagesAlreadyRectified parameter is set to false but the selected odometry
+  approach cannot process raw stereo images. We will rectify them for convenience.
+  ```
+
+  It needs the geometry between the two cameras to do that — the right `camera_info` carrying `P(0,3)`, or TF between the two camera frames. If a rectification map cannot be built from what the calibration says, the frame is refused rather than registered wrong.
+
+- **The odometry takes them raw.** A few approaches do their own undistortion and want the unrectified images: `Odom/Strategy` `6` (OKVIS), `8` (MSCKF), `9` (VINS-Fusion) and `10` (OpenVINS), each available only if RTAB-Map was built against that library. Nothing rectifies anything then, and `Rtabmap/ImagesAlreadyRectified:=false` simply tells the pipeline to leave the images alone.
+
+Which of the two applies decides what `rtabmap` needs when it is fed from this node's output. If the odometry rectified the pair, the rectified images are what travels on -- they replace the raw ones in the frame -- and `rtabmap` keeps `Rtabmap/ImagesAlreadyRectified` at its default `true`. If the odometry took them raw, they arrive raw, and `rtabmap` needs `Rtabmap/ImagesAlreadyRectified:=false` of its own to rectify them again on its side. Set `Mem/UseOdomFeatures:=false` along with it, since it defaults to `true`: `rtabmap` rectifies a stereo pair but does not currently map features that travelled with the frame into the rectified image, so any it reused would be read against the wrong one.
+
+Rectifying here costs what `stereo_image_proc` would have cost, but only on the frames the odometry actually registers. When it runs slower than the camera — throttled by `max_update_rate`, or dropping frames that arrive while a registration is still running — the rectification happens at the odometry's rate instead of the camera's, and every frame `stereo_image_proc` would have rectified for nothing is saved. Where something else needs the whole stream rectified, the first arrangement is still the one to use.
+
+## Usage
+
+```bash
+ros2 run rtabmap_odom stereo_odometry --ros-args \
+  -r left/image_rect:=/stereo/left/image_rect \
+  -r right/image_rect:=/stereo/right/image_rect \
+  -r left/camera_info:=/stereo/left/camera_info \
+  -r right/camera_info:=/stereo/right/camera_info \
+  -p frame_id:=base_link
+```
+
+```python
+ComposableNode(
+    package='rtabmap_odom',
+    plugin='rtabmap_odom::StereoOdometry',
+    name='stereo_odometry',
+    parameters=[{'frame_id': 'base_link'}],
+    remappings=[('left/image_rect', '/stereo/left/image_rect'),
+                ('right/image_rect', '/stereo/right/image_rect'),
+                ('left/camera_info', '/stereo/left/camera_info'),
+                ('right/camera_info', '/stereo/right/camera_info')])
+```
+
+**The images are normally rectified**, which is what the `image_rect` topic names assume, and what [`stereo_image_proc`](https://docs.ros.org/en/jazzy/p/stereo_image_proc/) produces when the driver does not.
+
+They do not have to be. RTAB-Map can rectify them itself from the calibration -- set `Rtabmap/ImagesAlreadyRectified:=false` and feed it the raw pair with distortion coefficients in the `camera_info`. What does not work is the silent middle case: **unrectified images with that parameter left at its default of `true`**. Nothing fails loudly; disparity is computed across rows that no longer correspond, giving depths that are wrong in a smoothly varying way and a trajectory that is wrong without looking broken.
+
+## Subscribed Topics
+
+**Default** — `subscribe_rgbd:=false`:
+
+| Topic | Type | Description |
+|---|---|---|
+| `left/image_rect` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified left image, color or mono. |
+| `right/image_rect` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified right image, color or mono. |
+| `left/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Left calibration. |
+| `right/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Right calibration. Its `P` matrix carries the baseline, which sets the scale of the whole trajectory. |
+
+**With `subscribe_rgbd:=true`**, one pre-synchronized message from [`stereo_sync`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_sync):
+
+| `rgbd_cameras` | Topic | Type |
+|---|---|---|
+| `1` (default) | `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) |
+| `2`–`6` | `rgbd_image0` … `rgbd_image5` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) |
+| `0` | `rgbd_images` | [`rtabmap_msgs/msg/RGBDImages`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImages.html) |
+
+| Topic | Type | Description |
+|---|---|---|
+| `imu` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Optional. See [the README](../README.md#imu). |
+
+## Published Topics
+
+Common to all three nodes; see [the README](../README.md#published-topics).
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `subscribe_rgbd` | `bool` | `false` | Take a pre-synchronized `RGBDImage` from `stereo_sync` instead of four raw topics. |
+| `rgbd_cameras` | `int` | `1` | Number of `RGBDImage` topics. `0` means one `RGBDImages` topic. Only with `subscribe_rgbd:=true`. More than one needs RTAB-Map built with OpenGV, and the cameras hardware-synchronized — see [Several cameras](rgbd_odometry.md#several-cameras). |
+| `approx_sync` | `bool` | `false` | Match the raw topics by nearest stamp. **Defaults to exact**, unlike `rgbd_odometry` — see [Synchronization](#synchronization). |
+| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. Only used when `approx_sync` is on. |
+| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. |
+| `sync_queue_size` | `int` | `5` | Queue depth of the synchronizer. |
+| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. |
+| `qos_camera_info` | `int` | value of `qos` | Reliability of the two `camera_info` subscriptions. |
+| `keep_color` | `bool` | `false` | Keep the left image in color in the data passed on, rather than converting to grayscale. Registration is grayscale either way. |
+| `image_transport` | `string` | `"raw"` | Transport for both image topics. |
+
+## Synchronization
+
+**This node defaults to exact matching**, because a stereo pair is normally hardware-triggered and the two images therefore carry identical stamps. That is the right default: exact matching is cheaper and cannot pair the left image with the wrong right one — and a mismatched stereo pair does not produce an error, it produces wrong disparities and a wrong trajectory.
+
+The failure mode to recognize is the other one: if the stamps are *not* identical, **nothing is ever published and nothing says why**. Check before assuming the node is broken:
+
+```bash
+ros2 topic echo --once /stereo/left/image_rect --field header.stamp
+ros2 topic echo --once /stereo/right/image_rect --field header.stamp
+```
+
+If they differ, set `approx_sync:=true` and set `approx_sync_max_interval` to something tight — a stereo pair whose images are more than a fraction of a frame apart is not usable for disparity regardless.
+
+## Getting the scale right
+
+Everything about a stereo trajectory's scale comes from the **baseline**, which this node reads from the right camera's `P` matrix (`P[3] = -fx * baseline`). Two consequences:
+
+- A `right/camera_info` whose `P` matrix is all zeros — which some drivers publish before calibration is loaded — gives a zero baseline and no usable depth at all.
+- A calibration whose baseline is off by a few percent produces a trajectory off by the same few percent, consistently, with nothing else looking wrong.
+
+If the map comes out uniformly too large or too small, check the baseline before anything else.
+
+## Features computed elsewhere
+
+`RGBDImage` has fields for local features — `key_points`, `points` and `descriptors` — and when a frame arrives with them filled, this node hands them to the odometry as they are instead of detecting and describing anything. RTAB-Map extracts features only from a frame that brought none, so nothing is recomputed, and neither is the disparity search that would otherwise give each feature its depth.
+
+This is for a camera, or a driver, that already does the extraction: on a multi-camera rig it is most of the per-frame work, and it can be done once and shared with `rtabmap` rather than repeated in each node.
+
+What a publisher has to get right:
+
+- **One entry per camera**, in the same order as the images, for `rgbd_cameras:=0` as well as the numbered topics.
+- **Keypoints in their own camera's left image.** The node stitches the left images side by side and shifts each camera's keypoints by the images that precede it. A stereo pair's features belong to the left image; nothing is expected in the right one.
+- **3D points in that camera's left optical frame.** They are brought into `frame_id` with the camera's transform from TF, the same one used for the calibration.
+- **Descriptors compressed** with `rtabmap::compressData()`, one row per keypoint, the same type for every camera.
+- **Equal counts.** `points` and `descriptors` may be left empty, but if they are there, they must have as many entries as there are keypoints. A frame whose three disagree has its features dropped with an error rather than used out of step.
+
+Both images may be left out entirely — the left image's job was to have features found in it, the right one's to give them their disparity, and they arrive with their 3D positions already. A frame is then its two calibrations and its features, which is the whole point: the images are nearly all of the bandwidth. Both `camera_info` still have to be there, the left one saying how big the image would have been and where the camera is, the right one carrying the baseline in `P(0,3)` — without it there is no scale, features or not.
+
+What stops applying, since nothing is extracted: `Vis/MaxFeatures`, the detector chosen with `Kp/DetectorStrategy`, and the depth bounds `Vis/MinDepth` and `Vis/MaxDepth`. Whatever is published is what gets registered, so the publisher owns those decisions. `Vis/CorType` must stay at `0` (feature matching); optical flow (`1`) reads the images themselves and has nothing to work with here.
+
+## Repetitive patterns
+
+Identical detail repeated across the scene — a tiled floor, rows of shelving, a brick wall — lets the matcher pair a feature with the wrong copy of itself, which drifts the trajectory by exactly one repeat while the inlier count stays healthy and nothing is reported lost. It bites stereo twice over, since the same ambiguity also misplaces the left/right match that sets the depth.
+
+The fix is the same as for [rgbd_odometry](rgbd_odometry.md#repetitive-patterns): an external guess through `guess_frame_id`, with `Vis/CorGuessWinSize` reduced so a match has to come from close to where the guess predicts. `Stereo/WinWidth` and `Stereo/WinHeight` matter here too — a correlation window smaller than the repeating pattern has nothing unique to lock onto.
+
+## When it loses track
+
+The same diagnosis as [rgbd_odometry](rgbd_odometry.md#when-it-loses-track) — `odom_info`'s `inliers` is the number to watch — with two failure modes specific to stereo:
+
+- **Poor rectification.** Matched features should lie on the same image row. If they do not, either the pair is unrectified while `Rtabmap/ImagesAlreadyRectified` is `true`, or the calibration itself is off.
+- **Untextured scene.** With no texture there is nothing to match *between* left and right either, so there is no depth at all — worse than the RGB-D case, where the sensor still measures wrong depth on a blank wall.
+
+`Stereo/*` parameters tune the disparity matching itself: `Stereo/MaxDisparity` bounds how close a point can be, `Stereo/WinWidth` and `Stereo/WinHeight` the correlation window. They are listed by `ros2 param list` like every other RTAB-Map parameter.

rtabmap_odom/include/rtabmap_odom/OdometryROS.h

@@ -58,57 +58,144 @@ namespace rtabmap {
 class Odometry;
 }
 
+/**
+ * @file
+ * @brief The node the three odometry nodes of this package are built on.
+ */
+
 namespace rtabmap_odom {
 
+/**
+ * @brief Runs RTAB-Map's odometry as a ROS node: everything but the subscriptions.
+ *
+ * `rgbd_odometry`, `stereo_odometry` and `icp_odometry` differ only in what they listen
+ * to. Each turns its own topics into a rtabmap::SensorData and hands it to processData();
+ * from there on this class does the work -- registration, pose integration, the `odom`
+ * topic and its TF, the IMU intake, the services, the diagnostics, the reset policy when
+ * tracking is lost. That is why the three nodes share nearly all of their parameters and
+ * publish the same topics.
+ *
+ * A subclass is expected to:
+ * - call init() from its constructor, saying which families of RTAB-Map parameters it
+ *   accepts, which decides both the defaults and what the node will accept being set;
+ * - create its subscriptions in onOdomInit() and describe them with initDiagnosticMsg();
+ * - call tick() when a message arrives and processData() once a frame is complete;
+ * - implement flushCallbacks(), so that a reset can drop whatever its synchronizer holds.
+ *
+ * The class is also a UThread. By default the frame handed to processData() is passed to
+ * that thread and the callback returns at once, so a slow registration cannot block the
+ * executor; a frame arriving while the thread is busy is dropped rather than queued. With
+ * `always_process_most_recent_frame:=false` it is registered on the calling thread
+ * instead, which keeps every frame at the cost of holding up the executor.
+ *
+ * @see the package README for the parameters and topics these nodes have in common.
+ */
 class OdometryROS : public rclcpp::Node, public UThread
 {
 
 public:
+	/// Constructs the node under its default name.
 	explicit OdometryROS(const rclcpp::NodeOptions & options);
+	/// Constructs the node under @p name, which is what the three nodes use.
 	explicit OdometryROS(const std::string & name, const rclcpp::NodeOptions & options);
 	virtual ~OdometryROS();
 
+	/**
+	 * @brief Hands a complete frame to the odometry; called by a subclass's callback.
+	 * @param[in,out] data   the frame to register, which comes back carrying the
+	 *                       features the odometry ended up using
+	 * @param[in]     header stamp and frame of the data, used to publish the result
+	 *
+	 * The frame is either queued for the worker thread or registered right here,
+	 * depending on `always_process_most_recent_frame`. Either way, a frame that arrives
+	 * while the previous one is still being registered is dropped: the odometry stays on
+	 * the newest data rather than falling behind.
+	 */
 	void processData(rtabmap::SensorData & data, const std_msgs::msg::Header & header);
 
+	/// `reset_odom` service: starts a new map at the origin, or at the guess frame's pose.
 	void resetOdom(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	/// `reset_odom_to_pose` service: starts a new map at the pose given in the request.
 	void resetToPose(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::ResetPose::Request>, std::shared_ptr<rtabmap_msgs::srv::ResetPose::Response>);
+	/// `pause_odom` service: keeps the subscriptions but stops registering what arrives.
 	void pause(const std::shared_ptr<rmw_request_id_t>,	const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	/// `resume_odom` service: registers again, starting from the next frame.
 	void resume(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	/// `log_debug` service: raises RTAB-Map's own log level to debug at runtime.
 	void setLogDebug(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	/// `log_info` service; see setLogDebug().
 	void setLogInfo(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	/// `log_warning` service; see setLogDebug().
 	void setLogWarn(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	/// `log_error` service; see setLogDebug().
 	void setLogError(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
 
+	/// The robot frame the odometry is computed for, `frame_id`.
 	const std::string & frameId() const {return frameId_;}
+	/// The frame the estimated poses are expressed in, `odom_frame_id`.
 	const std::string & odomFrameId() const {return odomFrameId_;}
+	/// The frame an external motion guess is read from, `guess_frame_id`; empty if unused.
 	const std::string & guessFrameId() const {return guessFrameId_;}
+	/// The RTAB-Map parameters this node was configured with, defaults included.
 	const rtabmap::ParametersMap & parameters() const {return parameters_;}
+	/// Whether the `pause_odom` service has been called and not resumed since.
 	bool isPaused() const {return paused_;}
 
 protected:
+	/**
+	 * @brief Declares the node's parameters and creates the odometry; call it last in the
+	 *        subclass constructor.
+	 * @param[in] stereoParams true if the node takes RTAB-Map's stereo parameters
+	 * @param[in] visParams    true if it takes the visual registration ones
+	 * @param[in] icpParams    true if it takes the scan matching ones
+	 *
+	 * The three flags decide which RTAB-Map parameters the node declares, and so which
+	 * ones it accepts being set: `icp_odometry` refuses a `Vis/` parameter and the other
+	 * two refuse an `Icp/` one. onOdomInit() is called at the end, for the subclass to
+	 * create its subscriptions.
+	 */
 	void init(bool stereoParams, bool visParams, bool icpParams);
+	/// The reliability the subclass should give its own subscriptions, from `qos`.
 	rmw_qos_reliability_policy_t qos() const {return qos_;}
+	/**
+	 * @brief Starts the diagnostics, once the subclass knows what it subscribed to.
+	 * @param[in] subscribedTopicsMsg the human readable list logged at startup and
+	 *                                repeated in the "no data received" warning
+	 * @param[in] approxSync          whether the subclass matches stamps approximately,
+	 *                                which that warning mentions as a likely cause
+	 * @param[in] subscribedTopic     the one topic whose rate is watched, if any
+	 */
 	void initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic = "");
 
+	/// Drops whatever the subclass's synchronizer holds; called when the odometry resets.
 	virtual void flushCallbacks() {};
+	/// The node's TF buffer, for the subclass to look up its sensors' frames.
 	tf2_ros::Buffer & tfBuffer() {return *tfBuffer_;}
+	/// How long a TF lookup may block, from `wait_for_transform`.
 	const double & waitForTransform() const {return waitForTransform_;}
+	/// The velocity of the last registered frame, null when there is no estimate yet.
 	rtabmap::Transform velocityGuess() const;
+	/// Stamp of the last registered frame, 0 before the first one.
 	double previousStamp() const {return previousStamp_;}
+	/// Called after a frame has been registered and published, for a subclass to add to it.
 	virtual void postProcessData(const rtabmap::SensorData & /*data*/, const std_msgs::msg::Header & /*header*/) const {}
 
 private:
 	void processData();
 	virtual void mainLoop();
 	virtual void mainLoopKill();
+	/// Lets a subclass adjust the RTAB-Map parameters before the odometry is created.
 	virtual void updateParameters(rtabmap::ParametersMap &) {}
+	/// Called at the end of init(), where a subclass creates its subscriptions.
 	virtual void onOdomInit() {}
 
 	void callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg);
 	void reset(const rtabmap::Transform & pose = rtabmap::Transform::getIdentity());
 
 protected:
+	/// The callback group the subclass's sensor subscriptions belong to.
 	rclcpp::CallbackGroup::SharedPtr dataCallbackGroup_;
+	/// Reports the arrival of an input message to the diagnostics, before anything else.
 	void tick(const rclcpp::Time & stamp);
 
 private:

rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp

@@ -44,6 +44,17 @@ using namespace rtabmap;
 namespace rtabmap_odom
 {
 
+/**
+ * @brief Odometry from a laser scanner, 2D or 3D, by scan matching.
+ *
+ * Takes a sensor_msgs::msg::LaserScan or a sensor_msgs::msg::PointCloud2 and registers
+ * each scan against the previous ones with ICP. A cloud whose points carry their own
+ * timestamps is deskewed first, using TF or the last known velocity, since a scan taken
+ * while the robot moves is not one rigid observation.
+ *
+ * @see doc/icp_odometry.md for the topics, the parameters and the shapes it can and
+ *      cannot constrain.
+ */
 class ICPOdometry : public rtabmap_odom::OdometryROS
 {
 public:

rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp

@@ -38,6 +38,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <image_transport/subscriber_filter.hpp>
 
 #include <sensor_msgs/msg/image.hpp>
+#include <rtabmap_msgs/msg/key_point.hpp>
+#include <rtabmap_msgs/msg/point3f.hpp>
 #include <rtabmap_msgs/msg/rgbd_image.hpp>
 #include <rtabmap_msgs/msg/rgbd_images.hpp>
 
@@ -50,6 +52,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap_odom
 {
 
+/**
+ * @brief Odometry from an RGB-D camera, or from several on one rig.
+ *
+ * Takes either the three raw topics of a camera (`rgb/image`, `depth/image`,
+ * `rgb/camera_info`) or pre-synchronized rtabmap_msgs::msg::RGBDImage messages, one per
+ * camera, and registers each frame's visual features against a local feature map. A
+ * frame that arrives with its own keypoints, 3D points and descriptors is registered
+ * with those rather than having them extracted again.
+ *
+ * @see doc/rgbd_odometry.md for the topics, the parameters and what to do when it loses
+ *      tracking.
+ */
 class RGBDOdometry : public rtabmap_odom::OdometryROS
 {
 public:
@@ -61,10 +75,19 @@ private:
 	virtual void updateParameters(rtabmap::ParametersMap & parameters);
 	virtual void onOdomInit();
 
+	/**
+	 * Local features, when the input topic carries them, are indexed per camera like
+	 * the images are: one entry per camera, in the same order. They are optional, and
+	 * a frame that comes without them is processed exactly as before, the features
+	 * being extracted from the images downstream.
+	 */
 	void commonCallback(
 				const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
 				const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
-				const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos);
+				const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos,
+				const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs = {},
+				const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs = {},
+				const std::vector<cv::Mat> & localDescriptorsMsgs = {});
 
 	void callback(
 			const sensor_msgs::msg::Image::ConstSharedPtr image,

rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp

@@ -43,12 +43,25 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <cv_bridge/cv_bridge.hpp>
 #endif
 #include <sensor_msgs/msg/image.hpp>
+#include <rtabmap_msgs/msg/key_point.hpp>
+#include <rtabmap_msgs/msg/point3f.hpp>
 #include <rtabmap_msgs/msg/rgbd_image.hpp>
 #include <rtabmap_msgs/msg/rgbd_images.hpp>
 
 namespace rtabmap_odom
 {
 
+/**
+ * @brief Odometry from a stereo pair, or from several on one rig.
+ *
+ * Takes either the four raw topics of a stereo camera (left and right image plus their
+ * calibrations) or pre-synchronized rtabmap_msgs::msg::RGBDImage messages carrying the
+ * pair, one per camera. The right camera's `P(0,3)` is what gives the trajectory its
+ * scale. A frame that arrives with its own keypoints, 3D points and descriptors is
+ * registered with those rather than having them extracted again.
+ *
+ * @see doc/stereo_odometry.md for the topics, the parameters and the scale it depends on.
+ */
 class StereoOdometry : public rtabmap_odom::OdometryROS
 {
 public:
@@ -60,11 +73,20 @@ private:
 	virtual void updateParameters(rtabmap::ParametersMap & parameters);
 	virtual void onOdomInit();
 
+	/**
+	 * Local features, when the input topic carries them, are indexed per camera like
+	 * the images are: one entry per camera, in the same order, and placed in that
+	 * camera's left image. They are optional, and a frame that comes without them is
+	 * processed exactly as before, the features being extracted downstream.
+	 */
 	void commonCallback(
 			const std::vector<cv_bridge::CvImageConstPtr> & leftImages,
 			const std::vector<cv_bridge::CvImageConstPtr> & rightImages,
 			const std::vector<sensor_msgs::msg::CameraInfo>& leftCameraInfos,
-			const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos);
+			const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos,
+			const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs = {},
+			const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs = {},
+			const std::vector<cv::Mat> & localDescriptorsMsgs = {});
 
 	void callback(
 			const sensor_msgs::msg::Image::ConstSharedPtr imageRectLeft,

rtabmap_odom/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_odom</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's odometry package.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -30,8 +30,14 @@
   <depend>rtabmap_util</depend>
   <depend>rtabmap_sync</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
+  <!-- The odometry tests replay recorded sensor data from test/data/lidar. -->
+  <test_depend>rosbag2_cpp</test_depend>
+  <test_depend>rosbag2_storage_mcap</test_depend>
+
   <export>
 	<build_type>ament_cmake</build_type>
+	<rosdoc2>rosdoc2.yaml</rosdoc2>
   </export>
 
 </package>

rtabmap_odom/rosdoc2.yaml

@@ -0,0 +1,35 @@
+## Configuration for rosdoc2, the documentation generator used by docs.ros.org.
+## Regenerate the annotated default with:
+##   rosdoc2 default_config --package-path rtabmap_odom
+## Build the docs locally with:
+##   rosdoc2 build --package-path rtabmap_odom --output-directory doc_output
+
+## This 'attic section' self-documents this file's type and version.
+type: 'rosdoc2 config'
+version: 1
+
+---
+
+settings:
+    ## Generate the standard index page from package.xml (description, maintainer,
+    ## license, links) and a table of contents for the builders below.
+    generate_package_index: true
+
+    ## This is an ament_cmake package, so doxygen runs on the public headers by
+    ## default and there are no Python modules to document.
+    always_run_doxygen: false
+    always_run_sphinx_apidoc: false
+
+builders:
+    ## Doxygen parses the public C++ API out of include/.
+    - doxygen: {
+        name: 'rtabmap_odom Public C/C++ API',
+        output_dir: 'generated/doxygen'
+    }
+    ## Sphinx renders the landing page and pulls the Doxygen XML in through
+    ## breathe/exhale so the API is browsable alongside the narrative docs.
+    - sphinx: {
+        name: 'rtabmap_odom',
+        doxygen_xml_directory: 'generated/doxygen/xml',
+        output_dir: ''
+    }

rtabmap_odom/src/OdometryROS.cpp

@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <rtabmap_conversions/PointCloudConversion.h>
 #include "rtabmap_odom/OdometryROS.h"
 
 #include <sensor_msgs/msg/image.hpp>
@@ -61,6 +62,44 @@ using namespace rtabmap;
 
 namespace rtabmap_odom {
 
+namespace {
+
+/**
+ * @brief The covariance of a pose that came from the guess frame instead of registration.
+ *
+ * Used wherever the guess is what the published pose rests on: a frame the odometry did
+ * not update because it had not moved enough, and the frame that restarts the map after
+ * a reset. Nothing was measured in either case, so the confidence is the one the guess
+ * was declared to have rather than anything the registration computed.
+ */
+cv::Mat guessCovariance(double linearVariance, double angularVariance)
+{
+	cv::Mat covariance = cv::Mat::zeros(6,6,CV_64FC1);
+	covariance.at<double>(0,0) = linearVariance;  // xx
+	covariance.at<double>(1,1) = linearVariance;  // yy
+	covariance.at<double>(2,2) = linearVariance;  // zz
+	covariance.at<double>(3,3) = angularVariance; // rr
+	covariance.at<double>(4,4) = angularVariance; // pp
+	covariance.at<double>(5,5) = angularVariance; // yawyaw
+	return covariance;
+}
+
+/**
+ * @brief The velocity a motion implies, for a frame with no registration to measure one.
+ *
+ * Named apart from the guess itself so that it can be called where a `guessVelocity`
+ * variable is in scope.
+ */
+rtabmap::Transform velocityFrom(const rtabmap::Transform & motion, double dt)
+{
+	UASSERT(dt > 0.0);
+	float x,y,z,roll,pitch,yaw;
+	motion.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+	return rtabmap::Transform(x/dt, y/dt, z/dt, roll/dt, pitch/dt, yaw/dt);
+}
+
+}  // namespace
+
 OdometryROS::OdometryROS(const rclcpp::NodeOptions & options) :
 		OdometryROS("odometry", options)
 	{}
@@ -83,6 +122,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	publishNullWhenLost_(true),
 	publishCompressedSensorData_(false),
 	qos_(RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT),
+	bufferedDataToProcess_(false),
 	paused_(false),
 	resetCountdown_(0),
 	resetCurrentCount_(0),
@@ -127,7 +167,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 
 	tfBuffer_ = std::make_shared<tf2_ros::Buffer>(get_clock());
 	tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
-	tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
+	tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
 
 	std::string initialPoseStr;
 	frameId_ = this->declare_parameter("frame_id", frameId_);
@@ -194,6 +234,17 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 				"are the same frame (value=\"%s\"). \"guess_frame_id\" is disabled.", odomFrameId_.c_str());
 		guessFrameId_.clear();
 	}
+	if(!publishNullWhenLost_ && guessFrameId_.empty() && publishTf_)
+	{
+		RCLCPP_ERROR(this->get_logger(), "\"publish_null_when_lost\" is false, but nothing can "
+				"say where odometry restarts after being lost: \"guess_frame_id\" is not set and "
+				"\"publish_tf\" is true, so the %s->%s fallback returns this node's own pose. "
+				"Whatever the robot did while lost will be silently dropped from the trajectory "
+				"and mapped across. Set \"guess_frame_id\", or set \"publish_tf\" to false if "
+				"another node (e.g. robot_localization) publishes %s->%s, or leave "
+				"\"publish_null_when_lost\" true.",
+				odomFrameId_.c_str(), frameId_.c_str(), odomFrameId_.c_str(), frameId_.c_str());
+	}
 	RCLCPP_INFO(this->get_logger(), "Odometry: frame_id               = %s", frameId_.c_str());
 	RCLCPP_INFO(this->get_logger(), "Odometry: odom_frame_id          = %s", odomFrameId_.c_str());
 	RCLCPP_INFO(this->get_logger(), "Odometry: publish_tf             = %s", publishTf_?"true":"false");
@@ -457,14 +508,14 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg)
 				imus_.erase(imus_.begin());
 			}
 		}
-		if(dataMutex_.lockTry() == 0)
+		UScopeMutex dataLock(dataMutex_, false);
+		if(dataLock.lockTry() == 0)
 		{
 			if(bufferedDataToProcess_ && rtabmap_conversions::timestampFromROS(dataHeaderToProcess_.stamp) <= stamp)
 			{
 				bufferedDataToProcess_ = false;
 				dataReady_.release();
 			}
-			dataMutex_.unlock();
 		}
 	}
 }
@@ -473,7 +524,8 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
 {
 	//RCLCPP_WARN(get_logger(), "Received image: %f delay=%f", data.stamp(), (now() - header.stamp).seconds());
 	double clockNow = rtabmap_conversions::timestampFromROS(now());
-	if(dataMutex_.lockTry() == 0)
+	UScopeMutex dataLock(dataMutex_, false);
+	if(dataLock.lockTry() == 0)
 	{
 		if(bufferedDataToProcess_) {
 			RCLCPP_ERROR(this->get_logger(), "We didn't receive IMU newer than previous image/scan (%f) and we just received a new image/scan (%f). The previous image/scan is dropped! Make sure IMU is published faster and with less delay than the image/scan.",
@@ -486,7 +538,7 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
 		if(alwaysProcessMostRecentFrame_) {
 			dataReady_.release();
 		}
-		dataMutex_.unlock();
+		dataLock.unlock(); // processData() below must run unlocked
 		++processedMsgs_;
 		if(!alwaysProcessMostRecentFrame_) {
 			processData();
@@ -628,8 +680,18 @@ void OdometryROS::processData()
 		imuProcessed_ = true;
 	}
 
+	// Whether this is a frame at all, as opposed to an IMU-only update. Neither the image
+	// nor the features answer that on their own: a frame that brings its own features has
+	// no image, and a frame of an empty scene has no feature. The calibration does, being
+	// there whenever a camera produced the data -- the same rule RTAB-Map's own
+	// Odometry::process() applies before registering anything.
+	const bool isFrame = !data.imageRaw().empty() ||
+			!data.cameraModels().empty() ||
+			!data.stereoCameraModels().empty() ||
+			!data.laserScanRaw().isEmpty();
+
 	Transform groundTruth;
-	if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
+	if(isFrame)
 	{
 		// Detect time jump in the past
 		double clockNow = now().seconds();
@@ -688,7 +750,7 @@ void OdometryROS::processData()
 		{
 			groundTruth = rtabmap_conversions::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, header.stamp, *tfBuffer_, waitForTransform_);
 
-			if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
+			if(isFrame)
 			{
 				// Use only XYZ to handle the case odometry was previously initialized with IMU,
 				// we assume that the ground truth contains also a real initial orientation
@@ -716,39 +778,6 @@ void OdometryROS::processData()
 		}
 	}
 
-	bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ > 1.0/minUpdateRate_;
-	if(tooOldPreviousData)
-	{
-		RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset because last update "
-				"is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.",
-				rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp));
-
-		if(!guess_.isNull())
-		{
-			RCLCPP_WARN(this->get_logger(), "Odometry automatically reset based on latest guess available from TF (%s->%s, moved %s since got lost)!",
-					guessFrameId_.c_str(), frameId_.c_str(), guess_.prettyPrint().c_str());
-			odometry_->reset(odometry_->getPose() * guess_);
-			guess_.setNull();
-			guessPreviousPose_.setNull();
-		}
-		else
-		{
-			// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
-			Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_);
-			if(tfPose.isNull())
-			{
-				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest computed pose!");
-				odometry_->reset(odometry_->getPose());
-			}
-			else
-			{
-				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
-						odomFrameId_.c_str(), frameId_.c_str());
-				odometry_->reset(tfPose);
-			}
-		}
-	}
-	
 	bool skipOdometryUpdate = false;
 
 	rtabmap::Transform pose;
@@ -756,23 +785,36 @@ void OdometryROS::processData()
 	rtabmap::Transform guessVelocity;
 
 	Transform guessCurrentPose;
+	// Whether the guess has a previous pose to be relative to, which decides how the pose
+	// is seeded from it further down. It is cleared by reset(), so a reset asked for
+	// through a service restarts at the guess frame while an automatic one continues from
+	// the pose it has just carried forward.
+	bool guessIsTheFirstOne = false;
 	if(!guessFrameId_.empty())
 	{
 		guessCurrentPose = rtabmap_conversions::getTransform(guessFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_);
 
 		Transform previousPose = guessPreviousPose_;
-		if(guessPreviousPose_.isNull())
+		guessIsTheFirstOne = guessPreviousPose_.isNull();
+		if(guessIsTheFirstOne)
 		{
 			previousPose = guessCurrentPose;
-			if(!guessCurrentPose.isNull() && odometry_->getPose().isIdentity())
-			{
-				RCLCPP_INFO(get_logger(), "Odometry: init pose with guess %s", guessCurrentPose.prettyPrint().c_str());
-				odometry_->reset(guessCurrentPose);
-			}
 		}
 
 		if(!previousPose.isNull() && !guessCurrentPose.isNull())
 		{
+			// What the guess frame says the robot is doing. This is what gets published
+			// for a frame with no registration behind it -- one skipped for not having
+			// moved enough, or one starting a new map, whose twist would otherwise be
+			// unknown although its pose comes from the guess. It is dropped further down
+			// as soon as the registration has a velocity of its own to report.
+			if(previousStamp_ > 0.0 &&
+			   rtabmap_conversions::timestampFromROS(header.stamp) > previousStamp_)
+			{
+				guessVelocity = velocityFrom(previousPose.inverse() * guessCurrentPose,
+						rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_);
+			}
+
 			if(guess_.isNull())
 			{
 				guess_ = previousPose.inverse() * guessCurrentPose;
@@ -791,20 +833,7 @@ void OdometryROS::processData()
 				{
 					// Ignore odometry update, we didn't move enough
 					pose = odometry_->getPose() * guess_;
-					info.reg.covariance = cv::Mat::zeros(6,6,CV_64FC1);
-					info.reg.covariance.at<double>(0,0) = guessLinearVariance_;  // xx
-					info.reg.covariance.at<double>(1,1) = guessLinearVariance_;  // yy
-					info.reg.covariance.at<double>(2,2) = guessLinearVariance_; // zz
-					info.reg.covariance.at<double>(3,3) = guessAngularVariance_; // rr
-					info.reg.covariance.at<double>(4,4) = guessAngularVariance_; // pp
-					info.reg.covariance.at<double>(5,5) = guessAngularVariance_; // yawyaw
 
-					//set velocity
-					double dt = rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_;
-					UASSERT(dt>0.0);
-					// use part of guess matching dt
-					(previousPose.inverse() * guessCurrentPose).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-					guessVelocity = rtabmap::Transform(x/dt, y/dt, z/dt, roll/dt, pitch/dt, yaw/dt);
 					skipOdometryUpdate = true;
 				}
 			}
@@ -817,16 +846,117 @@ void OdometryROS::processData()
 		}
 	}
 
+	// Handled here rather than before the guess is computed: guess_ only holds the motion
+	// since the previous frame once the block above has run, and resetting without it
+	// throws away everything the guess source measured across the gap -- which is exactly
+	// what this reset is supposed to carry over.
+	bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ > 1.0/minUpdateRate_;
+	if(tooOldPreviousData)
+	{
+		RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset because last update "
+				"is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.",
+				rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp));
+
+		if(!guess_.isNull())
+		{
+			RCLCPP_WARN(this->get_logger(), "Odometry automatically reset based on latest guess available from TF (%s->%s, moved %s since got lost)!",
+					guessFrameId_.c_str(), frameId_.c_str(), guess_.prettyPrint().c_str());
+			odometry_->reset(odometry_->getPose() * guess_);
+			// Cleared because it has just been applied: the odometry now starts from a
+			// pose that already includes it, and leaving it would have the registration
+			// apply it a second time on the frame that initialises the new map.
+			// guessPreviousPose_ is kept, so the next frame measures its motion from this
+			// one rather than starting over and losing a frame of it.
+			guess_.setNull();
+		}
+		else
+		{
+			// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
+			Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_);
+			if(tfPose.isNull())
+			{
+				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest computed pose!");
+				odometry_->reset(odometry_->getPose());
+			}
+			else
+			{
+				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
+						odomFrameId_.c_str(), frameId_.c_str());
+				odometry_->reset(tfPose);
+			}
+		}
+	}
+
 	// process data
 	rclcpp::Time timeStart = rclcpp::Clock().now();
 	if(!groundTruth.isNull())
 	{
 		data.setGroundTruth(groundTruth);
 	}
+	// Set when the guess has already been folded into the pose below, so that a reset
+	// later in this frame does not go looking for a fallback that is no longer needed.
+	bool poseCarriedByGuess = false;
+	// Set when this frame starts a new map and the guess frame says where, which is what
+	// makes the trajectory it starts continuous with the one before it.
+	bool initialisedOnGuess = false;
 	if(!skipOdometryUpdate)
 	{
+		// This frame will initialise the odometry's map whenever no frame has been
+		// registered since the last reset -- at startup, after a service reset, or on
+		// recovery from an automatic one. Registration then returns no motion, so the pose
+		// has to be put where the guess says the robot is *before* the frame is processed:
+		// afterwards the map is already anchored in the wrong place, and the next
+		// registration measures the difference against that anchor and takes the correction
+		// straight back out. Resetting here costs nothing, the map being empty either way.
+		//
+		// There are two ways to be right, depending on what the guess can say:
+		initialisedOnGuess = odometry_->framesProcessed() == 0 && !guessCurrentPose.isNull();
+		if(initialisedOnGuess)
+		{
+			if(guessIsTheFirstOne)
+			{
+				// Nothing to be relative to. Adopt the guess source's own coordinates, so
+				// that odometry restarts where the guess says it is rather than at the
+				// origin. A pose asked for explicitly through reset_odom_to_pose is left
+				// alone: only an odometry still sitting at the identity is seeded this way.
+				if(odometry_->getPose().isIdentity())
+				{
+					RCLCPP_INFO(get_logger(), "Odometry: init pose with guess %s",
+							guessCurrentPose.prettyPrint().c_str());
+					odometry_->reset(guessCurrentPose);
+				}
+			}
+			else if(!guess_.isNull() && !guess_.isIdentity())
+			{
+				// There is a previous guess pose, so the guess describes real motion since
+				// the frame before this one -- which an automatic reset has just carried the
+				// pose through. Advance by it and the trajectory stays continuous; drop it
+				// and the new map is anchored a frame behind, once per reset, accumulating.
+				RCLCPP_DEBUG(this->get_logger(), "Odometry: advancing the pose by the guess "
+						"(%s) before the map is initialised, so the motion measured since the "
+						"previous frame is not lost.", guess_.prettyPrint().c_str());
+				odometry_->reset(odometry_->getPose() * guess_);
+				guess_.setNull();
+				poseCarriedByGuess = true;
+			}
+		}
 		pose = odometry_->process(data, guess_, &info);
 	}
+
+	// 9999 on both covariances is how rtabmap is told a frame starts a new map. When the
+	// guess frame says where it starts, and publish_null_when_lost says this consumer
+	// wants poses rather than the news of a reset, it goes out as a continuation instead.
+	const bool publishAsContinuation = initialisedOnGuess && !publishNullWhenLost_ && !pose.isNull();
+	if(skipOdometryUpdate || publishAsContinuation)
+	{
+		// Both rest on the guess rather than on a registration: its confidence, its velocity.
+		info.reg.covariance = guessCovariance(guessLinearVariance_, guessAngularVariance_);
+	}
+	else
+	{
+		// The registration measured this one, so its velocity is the one to publish.
+		guessVelocity.setNull();
+	}
 	if(!pose.isNull())
 	{
 		if(!skipOdometryUpdate) {
@@ -909,11 +1039,12 @@ void OdometryROS::processData()
 			if(setTwist)
 			{
 				float x,y,z,roll,pitch,yaw;
-				if(skipOdometryUpdate) {
-					UASSERT(!guessVelocity.isNull());
-					guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-				} else {
+				// Whatever is left of the two: the registration's own velocity, or the
+				// guess's where the frame had no registration to give one.
+				if(guessVelocity.isNull()) {
 					odometry_->getVelocityGuess().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+				} else {
+					guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 				}
 				odom.twist.twist.linear.x = x;
 				odom.twist.twist.linear.y = y;
@@ -931,7 +1062,7 @@ void OdometryROS::processData()
 			odom.twist.covariance.at(35) = setTwist?info.reg.covariance.at<double>(5,5):BAD_COVARIANCE; // yawyaw
 
 			//publish the message
-			if(setTwist || publishNullWhenLost_)
+			if(setTwist || publishNullWhenLost_ || publishAsContinuation)
 			{
 				odomPub_->publish(odom);
 			}
@@ -953,7 +1084,7 @@ void OdometryROS::processData()
 				cloud.push_back(pt);
 			}
 			sensor_msgs::msg::PointCloud2 cloudMsg;
-			pcl::toROSMsg(cloud, cloudMsg);
+			rtabmap_conversions::toPointCloud2Msg(cloud, cloudMsg);
 			cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
 			cloudMsg.header.frame_id = odomFrameId_;
 			odomLocalMap_->publish(cloudMsg);
@@ -976,7 +1107,7 @@ void OdometryROS::processData()
 					}
 
 					sensor_msgs::msg::PointCloud2 cloudMsg;
-					pcl::toROSMsg(cloud, cloudMsg);
+					rtabmap_conversions::toPointCloud2Msg(cloud, cloudMsg);
 					cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
 					cloudMsg.header.frame_id = odomFrameId_;
 					odomLastFrame_->publish(cloudMsg);
@@ -996,7 +1127,7 @@ void OdometryROS::processData()
 						cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
 					}
 					sensor_msgs::msg::PointCloud2 cloudMsg;
-					pcl::toROSMsg(cloud, cloudMsg);
+					rtabmap_conversions::toPointCloud2Msg(cloud, cloudMsg);
 					cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
 					cloudMsg.header.frame_id = odomFrameId_;
 					odomLastFrame_->publish(cloudMsg);
@@ -1010,22 +1141,22 @@ void OdometryROS::processData()
 			if(info.localScanMap.hasNormals() && info.localScanMap.hasIntensity())
 			{
 				pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud = util3d::laserScanToPointCloudINormal(info.localScanMap, info.localScanMap.localTransform());
-				pcl::toROSMsg(*cloud, cloudMsg);
+				rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg);
 			}
 			else if(info.localScanMap.hasNormals())
 			{
 				pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(info.localScanMap, info.localScanMap.localTransform());
-				pcl::toROSMsg(*cloud, cloudMsg);
+				rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg);
 			}
 			else if(info.localScanMap.hasIntensity())
 			{
 				pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = util3d::laserScanToPointCloudI(info.localScanMap, info.localScanMap.localTransform());
-				pcl::toROSMsg(*cloud, cloudMsg);
+				rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg);
 			}
 			else
 			{
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(info.localScanMap, info.localScanMap.localTransform());
-				pcl::toROSMsg(*cloud, cloudMsg);
+				rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg);
 			}
 
 			cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
@@ -1106,6 +1237,17 @@ void OdometryROS::processData()
 				odometry_->reset(odometry_->getPose() * guess_);
 				guess_.setNull();
 			}
+			else if(poseCarriedByGuess)
+			{
+				// The guess was folded into the pose before this frame was processed, so the
+				// pose already covers the motion since the last one. Going to TF for a
+				// fallback here would block for wait_for_transform on every lost frame and
+				// answer a question that has already been answered.
+				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset, carrying the "
+						"latest guess from TF (%s->%s) that was already applied to the pose!",
+						guessFrameId_.c_str(), frameId_.c_str());
+				odometry_->reset(odometry_->getPose());
+			}
 			else
 			{
 				// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
@@ -1319,6 +1461,11 @@ void OdometryROS::reset(const Transform & pose)
 	UScopeMutex lock(dataMutex_);
 	odometry_->reset(pose);
 	guess_.setNull();
+	// Clearing this is what tells the next frame to restart from the guess frame rather
+	// than continue from here: the seeding step below cannot tell a reset asked for
+	// through a service from one the node decided on its own, and reads this instead. The
+	// automatic resets deliberately leave it alone, so that they carry on from the pose
+	// they just moved.
 	guessPreviousPose_.setNull();
 	previousStamp_ = 0.0;
 	previousClockTime_ = 0.0;

rtabmap_odom/src/RGBDOdometryNode.cpp

@@ -71,7 +71,9 @@ int main(int argc, char **argv)
 	}
 
 #ifdef RTABMAP_PYTHON
-	rtabmap::PythonInterface pythonInterface;
+	// Initialize the embedded python interpreter on the main thread, as
+	// the nodelet below is loaded in a worker thread.
+	rtabmap::PythonInterface::instance("rgbd_odometry");
 #endif
 	rclcpp::init(argc, argv);
 	rclcpp::NodeOptions options;

rtabmap_odom/src/StereoOdometryNode.cpp

@@ -71,7 +71,9 @@ int main(int argc, char **argv)
 	}
 
 #ifdef RTABMAP_PYTHON
-	rtabmap::PythonInterface pythonInterface;
+	// Initialize the embedded python interpreter on the main thread, as
+	// the nodelet below is loaded in a worker thread.
+	rtabmap::PythonInterface::instance("stereo_odometry");
 #endif
 	rclcpp::init(argc, argv);
 	rclcpp::NodeOptions options;

rtabmap_odom/src/nodelets/icp_odometry.cpp

@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <rtabmap_conversions/PointCloudConversion.h>
 #include <rtabmap_odom/icp_odometry.hpp>
 
 #include <laser_geometry/laser_geometry.hpp>
@@ -69,6 +70,7 @@ ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) :
 
 ICPOdometry::~ICPOdometry()
 {
+	this->join(true);
 }
 
 void ICPOdometry::onOdomInit()
@@ -313,7 +315,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 				scanMsg->header.frame_id,
 				guessFrameId().empty()?frameId():guessFrameId(),
 				scanMsg->header.stamp,
-				rclcpp::Time(scanMsg->header.stamp.sec, scanMsg->header.stamp.nanosec) + rclcpp::Duration::from_seconds(scanMsg->ranges.size()*scanMsg->time_increment),
+				rclcpp::Time(scanMsg->header.stamp.sec, scanMsg->header.stamp.nanosec) + rclcpp::Duration::from_seconds((scanMsg->ranges.empty()?0:scanMsg->ranges.size()-1)*scanMsg->time_increment),
 				this->tfBuffer(),
 				this->waitForTransform());
 		if(tmpT.isNull())
@@ -333,7 +335,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 		{
 			// deskew with constant velocity model (we are in frameId)
 			sensor_msgs::msg::PointCloud2 scanOutDeskewed;
-			if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
+			if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, velocityGuess()))
 			{
 				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
 				return;
@@ -362,7 +364,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 		{
 			// deskew with constant velocity model
 			sensor_msgs::msg::PointCloud2 scanOutDeskewed;
-			if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
+			if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, velocityGuess()))
 			{
 				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
 				return;
@@ -399,12 +401,12 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 
 	if(hasIntensity)
 	{
-		pcl::fromROSMsg(scanOut, *pclScanI);
+		rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScanI);
 		pclScanI->is_dense = true;
 	}
 	else
 	{
-		pcl::fromROSMsg(scanOut, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScan);
 		pclScan->is_dense = true;
 	}
 
@@ -519,7 +521,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr pointCloudMsg)
 {
 	UASSERT_MSG(pointCloudMsg->data.size() == pointCloudMsg->row_step*pointCloudMsg->height,
-			uFormat("data=%d row_step=%d height=%d", pointCloudMsg->data.size(), pointCloudMsg->row_step, pointCloudMsg->height).c_str());
+			uFormat("data=%d row_step=%d height=%d", (int)pointCloudMsg->data.size(), (int)pointCloudMsg->row_step, (int)pointCloudMsg->height).c_str());
 
 	if(scanReceived_)
 	{
@@ -583,7 +585,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 			}
 
 			std::shared_ptr<sensor_msgs::msg::PointCloud2> cloudDeskewed(new sensor_msgs::msg::PointCloud2);
-			if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, previousStamp(), velocityGuess()))
+			if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, velocityGuess()))
 			{
 				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
 				return;
@@ -668,7 +670,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 	if(hasNormals && hasIntensity)
 	{
 		pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
-		pcl::fromROSMsg(*cloudMsg, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan);
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -686,7 +688,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 	else if(hasNormals)
 	{
 		pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
-		pcl::fromROSMsg(*cloudMsg, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan);
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -704,7 +706,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 	else if(hasIntensity)
 	{
 		pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
-		pcl::fromROSMsg(*cloudMsg, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan);
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -750,7 +752,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 	else
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
-		pcl::fromROSMsg(*cloudMsg, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan);
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);

rtabmap_odom/src/nodelets/rgbd_odometry.cpp

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_conversions/MsgConversion.h"
 #include <rtabmap_msgs/msg/rgbd_images.hpp>
 
+#include <rtabmap/core/Compression.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/ULogger.h>
@@ -73,6 +74,8 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
 
 RGBDOdometry::~RGBDOdometry()
 {
+	this->join(true);
+
 	delete approxSync_;
 	delete exactSync_;
 	delete approxSync2_;
@@ -435,40 +438,58 @@ void RGBDOdometry::updateParameters(ParametersMap & parameters)
 void RGBDOdometry::commonCallback(
 			const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
 			const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
-			const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos)
+			const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos,
+			const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs,
+			const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs,
+			const std::vector<cv::Mat> & localDescriptorsMsgs)
 {
 	UASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size());
 	rclcpp::Time higherStamp;
 	UASSERT_MSG(rgbImages[0], "RGB image is null!");
-	int imageWidth = rgbImages[0]->image.cols;
-	int imageHeight = rgbImages[0]->image.rows;
 	UASSERT_MSG(depthImages[0], "Depth image is null!");
+
+	// The images are what the local features would otherwise be extracted from, so a frame
+	// that brings its own can leave them out -- which is nearly all of the bandwidth. It
+	// then describes itself with its calibration alone: how big the image would have been,
+	// where the camera is, what it sees. (An RGB-D message with no depth image at all also
+	// used to divide by zero below.)
+	const bool hasRgb = !rgbImages[0]->image.empty();
+	const bool hasDepth = !depthImages[0]->image.empty();
+
+	int imageWidth = hasRgb?rgbImages[0]->image.cols:(int)cameraInfos[0].width;
+	int imageHeight = hasRgb?rgbImages[0]->image.rows:(int)cameraInfos[0].height;
 	int depthWidth = depthImages[0]->image.cols;
 	int depthHeight = depthImages[0]->image.rows;
 
-	UASSERT_MSG(
-			imageWidth/depthWidth == imageHeight/depthHeight,
-			uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
+	if(hasDepth)
+	{
+		UASSERT_MSG(
+				imageWidth/depthWidth == imageHeight/depthHeight,
+				uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
+	}
 
 	int cameraCount = rgbImages.size();
 	cv::Mat rgb;
 	cv::Mat depth;
 	std::vector<rtabmap::CameraModel> cameraModels;
+	std::vector<cv::KeyPoint> keypoints;
+	std::vector<cv::Point3f> points3d;
+	cv::Mat descriptors;
 	for(unsigned int i=0; i<rgbImages.size(); ++i)
 	{
 		UASSERT_MSG(rgbImages[i], uFormat("RGB image is null for camera %d", i).c_str());
 		UASSERT_MSG(depthImages[i], uFormat("Depth image is null for camera %d", i).c_str());
-		if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+		if((hasRgb && !(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
-			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
-			!(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0)) ||
+			(hasDepth && !(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
 			 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
-			 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
+			 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)))
 		{
 			RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and "
 			"image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s",
@@ -476,20 +497,33 @@ void RGBDOdometry::commonCallback(
 				depthImages[i]->encoding.c_str());
 			return;
 		}
-		UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
-				uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
-						imageWidth,
-						rgbImages[i]->image.cols,
-						imageHeight,
-						rgbImages[i]->image.rows).c_str());
-		UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight,
-				uFormat("depthWidth=%d vs %d depthHeight=%d vs %d",
-						depthWidth,
-						depthImages[i]->image.cols,
-						depthHeight,
-						depthImages[i]->image.rows).c_str());
+		if(hasRgb)
+		{
+			UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
+					uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
+							imageWidth,
+							rgbImages[i]->image.cols,
+							imageHeight,
+							rgbImages[i]->image.rows).c_str());
+		}
+		if(hasDepth)
+		{
+			UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight,
+					uFormat("depthWidth=%d vs %d depthHeight=%d vs %d",
+							depthWidth,
+							depthImages[i]->image.cols,
+							depthHeight,
+							depthImages[i]->image.rows).c_str());
+		}
 
-		rclcpp::Time stamp = rtabmap_conversions::timestampFromROS(rgbImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(depthImages[i]->header.stamp)?rgbImages[i]->header.stamp:depthImages[i]->header.stamp;
+		// An image that is not there carries no header either, so a frame that has none is
+		// stamped and placed by its calibration, which is all it has.
+		const std::string & cameraFrameId = hasRgb?rgbImages[i]->header.frame_id:cameraInfos[i].header.frame_id;
+		rclcpp::Time stamp = cameraInfos[i].header.stamp;
+		if(hasRgb || hasDepth)
+		{
+			stamp = rtabmap_conversions::timestampFromROS(rgbImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(depthImages[i]->header.stamp)?rgbImages[i]->header.stamp:depthImages[i]->header.stamp;
+		}
 
 		if(i == 0)
 		{
@@ -500,7 +534,7 @@ void RGBDOdometry::commonCallback(
 			higherStamp = stamp;
 		}
 
-		Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp, tfBuffer(), waitForTransform());
+		Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), cameraFrameId, stamp, tfBuffer(), waitForTransform());
 		if(localTransform.isNull())
 		{
 			return;
@@ -528,53 +562,75 @@ void RGBDOdometry::commonCallback(
 			}
 		}
 
-		cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
-		if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
-		   rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
+		if(hasRgb)
 		{
-			if(keepColor_ && rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+			cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
+			if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
+			   rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
 			{
-				ptrImage = cv_bridge::cvtColor(rgbImages[i], "bgr8");
+				if(keepColor_ && rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+				{
+					ptrImage = cv_bridge::cvtColor(rgbImages[i], "bgr8");
+				}
+				else
+				{
+					ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
+				}
+			}
+
+			// initialize
+			if(rgb.empty())
+			{
+				rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
+			}
+
+			if(ptrImage->image.type() == rgb.type())
+			{
+				ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
 			}
 			else
 			{
-				ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
+				RCLCPP_ERROR(this->get_logger(), "Some RGB images are not the same type! %d vs %d", ptrImage->image.type(), rgb.type());
+				return;
 			}
 		}
 
-		cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
+		if(hasDepth)
+		{
+			cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
+			if(depth.empty())
+			{
+				depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
+			}
 
-		// initialize
-		if(rgb.empty())
-		{
-			rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
-		}
-		if(depth.empty())
-		{
-			depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
-		}
-
-		if(ptrImage->image.type() == rgb.type())
-		{
-			ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
-		}
-		else
-		{
-			RCLCPP_ERROR(this->get_logger(), "Some RGB images are not the same type! %d vs %d", ptrImage->image.type(), rgb.type());
-			return;
-		}
-
-		if(ptrDepth->image.type() == depth.type())
-		{
-			ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
-		}
-		else
-		{
-			RCLCPP_ERROR(this->get_logger(), "Some Depth images are not the same type! %d vs %d", ptrDepth->image.type(), depth.type());
-			return;
+			if(ptrDepth->image.type() == depth.type())
+			{
+				ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
+			}
+			else
+			{
+				RCLCPP_ERROR(this->get_logger(), "Some Depth images are not the same type! %d vs %d", ptrDepth->image.type(), depth.type());
+				return;
+			}
 		}
 
 		cameraModels.push_back(rtabmap_conversions::cameraModelFromROS(cameraInfos[i], localTransform));
+
+		// The images of all cameras are stitched side by side above, so the keypoints of
+		// camera i are shifted by as many images as come before it, and their 3D points,
+		// which arrive in that camera's optical frame, are brought back to the base frame.
+		if(localKeyPointsMsgs.size() == rgbImages.size())
+		{
+			rtabmap_conversions::keypointsFromROS(localKeyPointsMsgs[i], keypoints, imageWidth*i);
+		}
+		if(localPoints3dMsgs.size() == rgbImages.size())
+		{
+			rtabmap_conversions::points3fFromROS(localPoints3dMsgs[i], points3d, localTransform);
+		}
+		if(localDescriptorsMsgs.size() == rgbImages.size())
+		{
+			descriptors.push_back(localDescriptorsMsgs[i]);
+		}
 	}
 
 	rtabmap::SensorData data(
@@ -584,9 +640,30 @@ void RGBDOdometry::commonCallback(
 			0,
 			rtabmap_conversions::timestampFromROS(higherStamp));
 
+	// Features that came with the frame are used as they are: the odometry then skips
+	// detection, description and the depth lookup that would otherwise rebuild them
+	// (see RegistrationVis, which extracts only when the frame carries no keypoints).
+	// They are dropped rather than trusted if the three of them disagree, as using them
+	// out of step would silently mismatch keypoints with their descriptors or 3D points.
+	if(!keypoints.empty())
+	{
+		if((!points3d.empty() && points3d.size() != keypoints.size()) ||
+		   (!descriptors.empty() && descriptors.rows != (int)keypoints.size()))
+		{
+			RCLCPP_ERROR(this->get_logger(), "Ignoring the local features received with this frame: "
+					"%d keypoints, %d 3D points and %d descriptors, which should be the same count "
+					"(or none at all for the 3D points and the descriptors).",
+					(int)keypoints.size(), (int)points3d.size(), descriptors.rows);
+		}
+		else
+		{
+			data.setFeatures(keypoints, points3d, descriptors);
+		}
+	}
+
 	std_msgs::msg::Header header;
 	header.stamp = higherStamp;
-	header.frame_id = rgbImages.size()==1?rgbImages[0]->header.frame_id:"";
+	header.frame_id = rgbImages.size()==1?(hasRgb?rgbImages[0]->header.frame_id:cameraInfos[0].header.frame_id):"";
 	this->processData(data, header);
 }
 
@@ -627,6 +704,27 @@ void RGBDOdometry::callback(
 	}
 }
 
+namespace {
+
+/**
+ * @brief Collects the local features one camera's image carries, cameras in order.
+ *
+ * An image that carries none pushes empty entries rather than nothing, so that the
+ * per-camera indexing still lines up with the images.
+ */
+void appendLocalFeatures(
+		const rtabmap_msgs::msg::RGBDImage & image,
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & keyPoints,
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & points3d,
+		std::vector<cv::Mat> & descriptors)
+{
+	keyPoints.push_back(image.key_points);
+	points3d.push_back(image.points);
+	descriptors.push_back(rtabmap::uncompressData(image.descriptors));
+}
+
+}  // namespace
+
 void RGBDOdometry::callbackRGBDX(
 		const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images)
 {
@@ -642,13 +740,17 @@ void RGBDOdometry::callbackRGBDX(
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(images->rgbd_images.size());
 		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(images->rgbd_images.size());
 		std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
 		for(size_t i=0; i<images->rgbd_images.size(); ++i)
 		{
 			rtabmap_conversions::toCvShare(images->rgbd_images[i], images, imageMsgs[i], depthMsgs[i]);
 			infoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
+			appendLocalFeatures(images->rgbd_images[i], localKeyPoints, localPoints3d, localDescriptors);
 		}
 
-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -665,7 +767,12 @@ void RGBDOdometry::callbackRGBD(
 		rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
 		infoMsgs.push_back(image->rgb_camera_info);
 
-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -685,7 +792,13 @@ void RGBDOdometry::callbackRGBD2(
 		infoMsgs.push_back(image->rgb_camera_info);
 		infoMsgs.push_back(image2->rgb_camera_info);
 
-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -708,7 +821,14 @@ void RGBDOdometry::callbackRGBD3(
 		infoMsgs.push_back(image2->rgb_camera_info);
 		infoMsgs.push_back(image3->rgb_camera_info);
 
-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -734,7 +854,15 @@ void RGBDOdometry::callbackRGBD4(
 		infoMsgs.push_back(image3->rgb_camera_info);
 		infoMsgs.push_back(image4->rgb_camera_info);
 
-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -763,7 +891,16 @@ void RGBDOdometry::callbackRGBD5(
 		infoMsgs.push_back(image4->rgb_camera_info);
 		infoMsgs.push_back(image5->rgb_camera_info);
 
-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -795,7 +932,17 @@ void RGBDOdometry::callbackRGBD6(
 		infoMsgs.push_back(image5->rgb_camera_info);
 		infoMsgs.push_back(image6->rgb_camera_info);
 
-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image6, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 

rtabmap_odom/src/nodelets/stereo_odometry.cpp

@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
 #include <rtabmap/core/Odometry.h>
 
 using namespace rtabmap;
@@ -73,6 +74,8 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
 
 StereoOdometry::~StereoOdometry()
 {
+	this->join(true);
+
 	delete approxSync_;
 	delete exactSync_;
 	delete approxSync2_;
@@ -407,29 +410,46 @@ void StereoOdometry::commonCallback(
 		const std::vector<cv_bridge::CvImageConstPtr> & leftImages,
 		const std::vector<cv_bridge::CvImageConstPtr> & rightImages,
 		const std::vector<sensor_msgs::msg::CameraInfo>& leftCameraInfos,
-		const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos)
+		const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos,
+		const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs,
+		const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs,
+		const std::vector<cv::Mat> & localDescriptorsMsgs)
 {
 	UASSERT(leftImages.size() > 0 &&
 			leftImages.size() == rightImages.size() &&
 			leftImages.size() == leftCameraInfos.size() &&
 			rightImages.size() == rightCameraInfos.size());
 	rclcpp::Time higherStamp;
-	int leftWidth = leftImages[0]->image.cols;
-	int leftHeight = leftImages[0]->image.rows;
+
+	// The images are what the local features would otherwise be extracted from, so a frame
+	// that brings its own can leave them out -- which is nearly all of the bandwidth. It
+	// then describes itself with its calibration alone: how big the left image would have
+	// been, where the rig is, how far apart the two cameras are.
+	const bool hasImages = !leftImages[0]->image.empty() && !rightImages[0]->image.empty();
+
+	int leftWidth = hasImages?leftImages[0]->image.cols:(int)leftCameraInfos[0].width;
+	int leftHeight = hasImages?leftImages[0]->image.rows:(int)leftCameraInfos[0].height;
 	int rightWidth = rightImages[0]->image.cols;
 	int rightHeight = rightImages[0]->image.rows;
 
-	UASSERT_MSG(
-			leftWidth == rightWidth && leftHeight == rightHeight,
-		uFormat("left=%dx%d right=%dx%d", leftWidth, leftHeight, rightWidth, rightHeight).c_str());
+	if(hasImages)
+	{
+		UASSERT_MSG(
+				leftWidth == rightWidth && leftHeight == rightHeight,
+			uFormat("left=%dx%d right=%dx%d", leftWidth, leftHeight, rightWidth, rightHeight).c_str());
+	}
 
 	int cameraCount = leftImages.size();
 	cv::Mat left;
 	cv::Mat right;
 	std::vector<rtabmap::StereoCameraModel> cameraModels;
+	std::vector<cv::KeyPoint> keypoints;
+	std::vector<cv::Point3f> points3d;
+	cv::Mat descriptors;
 	for(unsigned int i=0; i<leftImages.size(); ++i)
 	{
-		if(!(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+		if(hasImages &&
+		   (!(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
 			 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 			 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 			 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
@@ -442,14 +462,21 @@ void StereoOdometry::commonCallback(
 			  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 			  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
 			  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
-			  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
+			  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0)))
 		{
 			RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
 					leftImages[i]->encoding.c_str(), rightImages[i]->encoding.c_str());
 			return;
 		}
 
-		rclcpp::Time stamp = rtabmap_conversions::timestampFromROS(leftImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(rightImages[i]->header.stamp)?leftImages[i]->header.stamp:rightImages[i]->header.stamp;
+		// An image that is not there carries no header either, so a frame that has none is
+		// stamped and placed by its calibration, which is all it has.
+		const std::string & cameraFrameId = hasImages?leftImages[i]->header.frame_id:leftCameraInfos[i].header.frame_id;
+		rclcpp::Time stamp = leftCameraInfos[i].header.stamp;
+		if(hasImages)
+		{
+			stamp = rtabmap_conversions::timestampFromROS(leftImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(rightImages[i]->header.stamp)?leftImages[i]->header.stamp:rightImages[i]->header.stamp;
+		}
 
 		if(i == 0)
 		{
@@ -460,7 +487,7 @@ void StereoOdometry::commonCallback(
 			higherStamp = stamp;
 		}
 
-		Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), leftImages[i]->header.frame_id, stamp, tfBuffer(), waitForTransform());
+		Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), cameraFrameId, stamp, tfBuffer(), waitForTransform());
 		if(localTransform.isNull())
 		{
 			return;
@@ -488,7 +515,13 @@ void StereoOdometry::commonCallback(
 			}
 		}
 
-		if(!leftImages[i]->image.empty() && !rightImages[i]->image.empty())
+		if(hasImages != (!leftImages[i]->image.empty() && !rightImages[i]->image.empty()))
+		{
+			RCLCPP_ERROR(this->get_logger(), "Odom: camera %d of this frame has images while "
+					"another one doesn't (or the other way around)?!?", i);
+			return;
+		}
+
 		{
 			bool alreadyRectified = true;
 			Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
@@ -602,62 +635,76 @@ void StereoOdometry::commonCallback(
 					shown = true;
 				}
 			}
-			cv_bridge::CvImageConstPtr ptrLeft = leftImages[i];
-			if(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
-			   leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
+			if(hasImages)
 			{
-				if(keepColor_ && leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+				cv_bridge::CvImageConstPtr ptrLeft = leftImages[i];
+				if(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
+				   leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
 				{
-					ptrLeft = cv_bridge::cvtColor(leftImages[i], "bgr8");
+					if(keepColor_ && leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+					{
+						ptrLeft = cv_bridge::cvtColor(leftImages[i], "bgr8");
+					}
+					else
+					{
+						ptrLeft = cv_bridge::cvtColor(leftImages[i], "mono8");
+					}
+				}
+				cv_bridge::CvImageConstPtr ptrRight = rightImages[i];
+				if(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
+				   rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
+				{
+					ptrRight = cv_bridge::cvtColor(rightImages[i], "mono8");
+				}
+
+				// initialize
+				if(left.empty())
+				{
+					left = cv::Mat(leftHeight, leftWidth*cameraCount, ptrLeft->image.type());
+				}
+				if(right.empty())
+				{
+					right = cv::Mat(rightHeight, rightWidth*cameraCount, ptrRight->image.type());
+				}
+
+				if(ptrLeft->image.type() == left.type())
+				{
+					ptrLeft->image.copyTo(cv::Mat(left, cv::Rect(i*leftWidth, 0, leftWidth, leftHeight)));
 				}
 				else
 				{
-					ptrLeft = cv_bridge::cvtColor(leftImages[i], "mono8");
+					RCLCPP_ERROR(this->get_logger(), "Some left images are not the same type! %d vs %d", ptrLeft->image.type(), left.type());
+					return;
 				}
-			}
-			cv_bridge::CvImageConstPtr ptrRight = rightImages[i];
-			if(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
-			   rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
-			{
-				ptrRight = cv_bridge::cvtColor(rightImages[i], "mono8");
-			}
 
-			// initialize
-			if(left.empty())
-			{
-				left = cv::Mat(leftHeight, leftWidth*cameraCount, ptrLeft->image.type());
-			}
-			if(right.empty())
-			{
-				right = cv::Mat(rightHeight, rightWidth*cameraCount, ptrRight->image.type());
-			}
-
-			if(ptrLeft->image.type() == left.type())
-			{
-				ptrLeft->image.copyTo(cv::Mat(left, cv::Rect(i*leftWidth, 0, leftWidth, leftHeight)));
-			}
-			else
-			{
-				RCLCPP_ERROR(this->get_logger(), "Some left images are not the same type! %d vs %d", ptrLeft->image.type(), left.type());
-				return;
-			}
-
-			if(ptrRight->image.type() == right.type())
-			{
-				ptrRight->image.copyTo(cv::Mat(right, cv::Rect(i*rightWidth, 0, rightWidth, rightHeight)));
-			}
-			else
-			{
-				RCLCPP_ERROR(this->get_logger(), "Some right images are not the same type! %d vs %d", ptrRight->image.type(), right.type());
-				return;
+				if(ptrRight->image.type() == right.type())
+				{
+					ptrRight->image.copyTo(cv::Mat(right, cv::Rect(i*rightWidth, 0, rightWidth, rightHeight)));
+				}
+				else
+				{
+					RCLCPP_ERROR(this->get_logger(), "Some right images are not the same type! %d vs %d", ptrRight->image.type(), right.type());
+					return;
+				}
 			}
 
 			cameraModels.push_back(stereoModel);
 		}
-		else
+
+		// The left images of all cameras are stitched side by side above, so the keypoints
+		// of camera i are shifted by as many images as come before it, and their 3D points,
+		// which arrive in that camera's optical frame, are brought back to the base frame.
+		if(localKeyPointsMsgs.size() == leftImages.size())
 		{
-			RCLCPP_ERROR(this->get_logger(), "Odom: input images empty?!?");
-			return;
+			rtabmap_conversions::keypointsFromROS(localKeyPointsMsgs[i], keypoints, leftWidth*i);
+		}
+		if(localPoints3dMsgs.size() == leftImages.size())
+		{
+			rtabmap_conversions::points3fFromROS(localPoints3dMsgs[i], points3d, localTransform);
+		}
+		if(localDescriptorsMsgs.size() == leftImages.size())
+		{
+			descriptors.push_back(localDescriptorsMsgs[i]);
 		}
 	}
 
@@ -669,9 +716,30 @@ void StereoOdometry::commonCallback(
 			0,
 			rtabmap_conversions::timestampFromROS(higherStamp));
 
+	// Features that came with the frame are used as they are: the odometry then skips
+	// detection, description and the disparity search that would otherwise rebuild them
+	// (see RegistrationVis, which extracts only when the frame carries no keypoints).
+	// They are dropped rather than trusted if the three of them disagree, as using them
+	// out of step would silently mismatch keypoints with their descriptors or 3D points.
+	if(!keypoints.empty())
+	{
+		if((!points3d.empty() && points3d.size() != keypoints.size()) ||
+		   (!descriptors.empty() && descriptors.rows != (int)keypoints.size()))
+		{
+			RCLCPP_ERROR(this->get_logger(), "Ignoring the local features received with this frame: "
+					"%d keypoints, %d 3D points and %d descriptors, which should be the same count "
+					"(or none at all for the 3D points and the descriptors).",
+					(int)keypoints.size(), (int)points3d.size(), descriptors.rows);
+		}
+		else
+		{
+			data.setFeatures(keypoints, points3d, descriptors);
+		}
+	}
+
 	std_msgs::msg::Header header;
 	header.stamp = higherStamp;
-	header.frame_id = leftImages.size()==1?leftImages[0]->header.frame_id:"";
+	header.frame_id = leftImages.size()==1?(hasImages?leftImages[0]->header.frame_id:leftCameraInfos[0].header.frame_id):"";
 	this->processData(data, header);
 }
 
@@ -715,6 +783,27 @@ void StereoOdometry::callback(
 	}
 }
 
+namespace {
+
+/**
+ * @brief Collects the local features one camera's frame carries, cameras in order.
+ *
+ * A frame that carries none pushes empty entries rather than nothing, so that the
+ * per-camera indexing still lines up with the images.
+ */
+void appendLocalFeatures(
+		const rtabmap_msgs::msg::RGBDImage & image,
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & keyPoints,
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & points3d,
+		std::vector<cv::Mat> & descriptors)
+{
+	keyPoints.push_back(image.key_points);
+	points3d.push_back(image.points);
+	descriptors.push_back(rtabmap::uncompressData(image.descriptors));
+}
+
+}  // namespace
+
 void StereoOdometry::callbackRGBD(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
 {
@@ -730,7 +819,12 @@ void StereoOdometry::callbackRGBD(
 		leftInfoMsgs.push_back(image->rgb_camera_info);
 		rightInfoMsgs.push_back(image->depth_camera_info);
 
-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -750,14 +844,18 @@ void StereoOdometry::callbackRGBDX(
 		std::vector<cv_bridge::CvImageConstPtr> rightMsgs(images->rgbd_images.size());
 		std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
 		std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
 		for(size_t i=0; i<images->rgbd_images.size(); ++i)
 		{
 			rtabmap_conversions::toCvShare(images->rgbd_images[i], images, leftMsgs[i], rightMsgs[i]);
 			leftInfoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
 			rightInfoMsgs.push_back(images->rgbd_images[i].depth_camera_info);
+			appendLocalFeatures(images->rgbd_images[i], localKeyPoints, localPoints3d, localDescriptors);
 		}
 
-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -780,7 +878,13 @@ void StereoOdometry::callbackRGBD2(
 		rightInfoMsgs.push_back(image->depth_camera_info);
 		rightInfoMsgs.push_back(image2->depth_camera_info);
 
-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -807,7 +911,14 @@ void StereoOdometry::callbackRGBD3(
 		rightInfoMsgs.push_back(image2->depth_camera_info);
 		rightInfoMsgs.push_back(image3->depth_camera_info);
 
-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -838,7 +949,15 @@ void StereoOdometry::callbackRGBD4(
 		rightInfoMsgs.push_back(image3->depth_camera_info);
 		rightInfoMsgs.push_back(image4->depth_camera_info);
 
-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -873,7 +992,16 @@ void StereoOdometry::callbackRGBD5(
 		rightInfoMsgs.push_back(image4->depth_camera_info);
 		rightInfoMsgs.push_back(image5->depth_camera_info);
 
-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -912,7 +1040,17 @@ void StereoOdometry::callbackRGBD6(
 		rightInfoMsgs.push_back(image5->depth_camera_info);
 		rightInfoMsgs.push_back(image6->depth_camera_info);
 
-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image6, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 

rtabmap_odom/test/bag_playback.hpp

@@ -0,0 +1,79 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_ODOM_BAG_PLAYBACK_HPP_
+#define RTABMAP_ODOM_BAG_PLAYBACK_HPP_
+
+#include <rclcpp/serialization.hpp>
+#include <rosbag2_cpp/reader.hpp>
+
+#include <string>
+#include <vector>
+
+#include "test_data.hpp"
+
+/**
+ * @file
+ * @brief Reads recorded messages out of a bag, for tests that need real sensor input.
+ *
+ * The messages are read and replayed by the test itself rather than by `ros2 bag play`:
+ * no second process, no wall-clock pacing, and the test controls exactly when each
+ * message reaches the node. What the recording provides is the part that cannot be
+ * written by hand -- a real driver's cloud layout and a dense TF history around it.
+ */
+
+namespace rtabmap_odom_test {
+
+/**
+ * @brief The Ouster recording in test/data/lidar; see that directory's README.
+ *
+ * Two sweeps half a mast turn apart, from a platform that never moves: the only thing
+ * between them is the mast's rotation, which TF describes in full.
+ */
+inline std::string ousterHalfTurnBag()
+{
+	return testDataRoot() + "/lidar/ouster_half_turn";
+}
+
+/**
+ * @brief Every message recorded on @p topic, deserialized.
+ *
+ * Returns an empty vector if the bag or the topic is missing, which the caller is
+ * expected to assert on -- a silently empty fixture would make a test pass for the wrong
+ * reason.
+ */
+template <typename MsgT>
+std::vector<MsgT> readBagMessages(const std::string & bagPath, const std::string & topic)
+{
+	std::vector<MsgT> messages;
+	rosbag2_cpp::Reader reader;
+	try
+	{
+		reader.open(bagPath);
+	}
+	catch(const std::exception & e)
+	{
+		return messages;
+	}
+
+	rclcpp::Serialization<MsgT> serialization;
+	while(reader.has_next())
+	{
+		const std::shared_ptr<rosbag2_storage::SerializedBagMessage> message = reader.read_next();
+		if(message->topic_name != topic)
+		{
+			continue;
+		}
+		rclcpp::SerializedMessage serialized(*message->serialized_data);
+		MsgT deserialized;
+		serialization.deserialize_message(&serialized, &deserialized);
+		messages.push_back(deserialized);
+	}
+	return messages;
+}
+
+}  // namespace rtabmap_odom_test
+
+#endif /* RTABMAP_ODOM_BAG_PLAYBACK_HPP_ */

rtabmap_odom/test/camera_rig.hpp

@@ -0,0 +1,319 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_ODOM_CAMERA_RIG_HPP_
+#define RTABMAP_ODOM_CAMERA_RIG_HPP_
+
+#include <geometry_msgs/msg/transform_stamped.hpp>
+
+#include <rtabmap_msgs/msg/rgbd_images.hpp>
+
+#include <rtabmap_conversions/MsgConversion.h>
+
+#include <rtabmap/core/CameraModel.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/core/util3d_transforms.h>
+
+#include <opencv2/core/core.hpp>
+
+#include <algorithm>
+#include <cmath>
+#include <random>
+#include <string>
+#include <vector>
+
+#include "msg_builders.hpp"
+
+/**
+ * @file
+ * @brief A camera rig in a world of points, for the multi-camera odometry tests.
+ *
+ * Several cameras looking outward from one body is the case that cannot be covered with
+ * recorded frames: it needs a calibrated rig, a scene all of them can see, and ground
+ * truth for where the body went. So the scene is made rather than recorded -- points
+ * scattered around the rig, projected into each camera at each pose along a known
+ * trajectory, which is the approach of RTAB-Map's own multi-camera tests
+ * (`EstimateMotion3DTo2DMultiCam*` in corelib/test/test_util3d_motion_estimation.cpp and
+ * the four-camera rig of test_optimizer.cpp).
+ *
+ * What comes out is the *features*, not imagery: keypoints, their 3D positions and a
+ * descriptor per point, which is what a camera driver doing its own feature extraction
+ * would publish on `rgbd_images`, and all it needs to publish. The frames carry no image
+ * at all, so a run that recovers the trajectory can only have done it from them.
+ */
+
+namespace rtabmap_odom_test {
+
+/// Descriptors are matched between frames, so the same point has to keep the same one.
+const int kRigDescriptorSize = 32;
+
+/**
+ * @brief Cameras looking outward from one body, and the points they see.
+ *
+ * The cameras are spread evenly around the body and mounted on its rim, so opposite
+ * cameras are a real distance apart rather than sharing an optical centre.
+ */
+struct CameraRig
+{
+	int width = 0;
+	int height = 0;
+	double fx = 0.0;
+
+	/// base_link -> camera i, optical frame, in the order the cameras are published.
+	std::vector<rtabmap::Transform> localTransforms;
+	std::vector<std::string> frameIds;
+
+	/// The world: points in the odometry frame, and one descriptor row per point.
+	std::vector<cv::Point3f> points;
+	cv::Mat descriptors;
+
+	size_t cameras() const { return localTransforms.size(); }
+
+	/// Camera @p i as RTAB-Map sees it, intrinsics and mounting together.
+	rtabmap::CameraModel model(size_t i) const
+	{
+		return rtabmap::CameraModel(fx, fx, width/2.0, height/2.0,
+				localTransforms[i], 0, cv::Size(width, height));
+	}
+};
+
+/**
+ * @brief A rig of @p cameras cameras in a world of @p numPoints points.
+ *
+ * The horizontal field of view is 360/cameras degrees, up to 90, so the cameras tile as
+ * much of the circle as they can without overlapping: a point is then seen by at most one
+ * of them, which keeps every descriptor unique within a frame. Two cameras seeing the same
+ * point would put two identical descriptors in the same frame, and the ratio test that
+ * accepts a match only when the best candidate is clearly better than the second would
+ * throw both away.
+ *
+ * The points sit in a box wider than the trajectory, minus a hole around it: something
+ * closer than @p minRange would swing through a camera's field of view, or behind it,
+ * over the course of the run.
+ */
+inline CameraRig makeCameraRig(
+		int cameras = 4,
+		int numPoints = 300,
+		float boxXY = 8.0f,
+		float boxZ = 1.5f,
+		float minRange = 2.5f,
+		int width = 160,
+		int height = 120,
+		float rimRadius = 0.175f,
+		float rimHeight = 0.05f,
+		uint32_t seed = 7)
+{
+	CameraRig rig;
+	rig.width = width;
+	rig.height = height;
+	// Half the horizontal field of view spans half the angle between two cameras, capped
+	// at 45 degrees: one or two cameras would otherwise be asked for a 360 or 180 degree
+	// view, which no pinhole model has. The cap only widens the gaps between cameras, so
+	// a point is still seen by at most one of them.
+	rig.fx = (width/2.0) / std::tan(std::min(M_PI/double(cameras), M_PI/4.0));
+
+	for(int i=0; i<cameras; ++i)
+	{
+		const float yaw = 2.0f*float(M_PI)*float(i)/float(cameras);
+		rig.localTransforms.push_back(
+				rtabmap::Transform(rimRadius*std::cos(yaw), rimRadius*std::sin(yaw), rimHeight,
+						0.0f, 0.0f, yaw)
+				* rtabmap::CameraModel::opticalRotation());
+		rig.frameIds.push_back("camera" + std::to_string(i));
+	}
+
+	std::mt19937 rng(seed);
+	std::uniform_real_distribution<float> distXY(-boxXY, boxXY);
+	std::uniform_real_distribution<float> distZ(-boxZ, boxZ);
+	std::normal_distribution<float> distDescriptor(0.0f, 1.0f);
+
+	rig.descriptors = cv::Mat(numPoints, kRigDescriptorSize, CV_32FC1);
+	for(int i=0; i<numPoints; )
+	{
+		const cv::Point3f point(distXY(rng), distXY(rng), distZ(rng));
+		if(std::sqrt(point.x*point.x + point.y*point.y) < minRange)
+		{
+			continue;
+		}
+		rig.points.push_back(point);
+		for(int c=0; c<kRigDescriptorSize; ++c)
+		{
+			rig.descriptors.at<float>(i, c) = distDescriptor(rng);
+		}
+		++i;
+	}
+	return rig;
+}
+
+/// The mounting of each camera, as the rig's driver would publish it on /tf_static.
+inline std::vector<geometry_msgs::msg::TransformStamped> cameraRigTransforms(
+		const CameraRig & rig, const rclcpp::Time & stamp,
+		const std::string & baseFrame = "base_link")
+{
+	std::vector<geometry_msgs::msg::TransformStamped> transforms;
+	for(size_t i=0; i<rig.cameras(); ++i)
+	{
+		geometry_msgs::msg::TransformStamped tf;
+		tf.header.stamp = stamp;
+		tf.header.frame_id = baseFrame;
+		tf.child_frame_id = rig.frameIds[i];
+		rtabmap_conversions::transformToGeometryMsg(rig.localTransforms[i], tf.transform);
+		transforms.push_back(tf);
+	}
+	return transforms;
+}
+
+/// What one camera of the rig saw: its keypoints, their 3D points and their descriptors.
+struct RigObservations
+{
+	std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > keyPoints;
+	std::vector<std::vector<rtabmap_msgs::msg::Point3f> > points;
+	std::vector<cv::Mat> descriptors;
+};
+
+/**
+ * @brief What the rig sees from @p pose, one entry per camera.
+ *
+ * Each point is given to the first camera that has it in view, so no point is reported
+ * twice. The keypoints are in their own camera's image, the 3D points in their own
+ * camera's optical frame, and the descriptors in the order of the keypoints -- which is
+ * how a driver publishes them, and what the node has to reassemble.
+ */
+inline RigObservations observeCameraRig(const CameraRig & rig, const rtabmap::Transform & pose)
+{
+	const size_t cameras = rig.cameras();
+	std::vector<rtabmap::CameraModel> models;
+	std::vector<rtabmap::Transform> worldToCamera;
+	for(size_t i=0; i<cameras; ++i)
+	{
+		models.push_back(rig.model(i));
+		worldToCamera.push_back((pose * rig.localTransforms[i]).inverse());
+	}
+
+	RigObservations seen;
+	seen.keyPoints.resize(cameras);
+	seen.points.resize(cameras);
+	seen.descriptors.resize(cameras);
+
+	for(size_t p=0; p<rig.points.size(); ++p)
+	{
+		for(size_t i=0; i<cameras; ++i)
+		{
+			const cv::Point3f inCamera =
+					rtabmap::util3d::transformPoint(rig.points[p], worldToCamera[i]);
+			if(inCamera.z <= 0.1f)
+			{
+				continue;
+			}
+			float u = 0.0f;
+			float v = 0.0f;
+			models[i].reproject(inCamera.x, inCamera.y, inCamera.z, u, v);
+			// Truncation would let a point just off the left or top edge through, at a
+			// negative pixel the odometry drops later, so the bounds are checked directly.
+			if(u < 0.0f || v < 0.0f || u >= float(rig.width) || v >= float(rig.height))
+			{
+				continue;
+			}
+
+			rtabmap_msgs::msg::KeyPoint keyPoint;
+			keyPoint.pt.x = u;
+			keyPoint.pt.y = v;
+			keyPoint.size = 3;
+			keyPoint.response = 1.0f;
+			seen.keyPoints[i].push_back(keyPoint);
+
+			rtabmap_msgs::msg::Point3f point;
+			point.x = inCamera.x;
+			point.y = inCamera.y;
+			point.z = inCamera.z;
+			seen.points[i].push_back(point);
+
+			seen.descriptors[i].push_back(rig.descriptors.row(int(p)));
+			break;
+		}
+	}
+	return seen;
+}
+
+/**
+ * @brief The rig's observations from @p pose as RGB-D frames, one per camera.
+ *
+ * @p withImages attaches a blank image to each camera. There is nothing to find in it,
+ * but the odometry only takes the paths that touch images when one is there.
+ */
+inline rtabmap_msgs::msg::RGBDImages cameraRigFrame(
+		const CameraRig & rig, const rtabmap::Transform & pose, double stamp,
+		bool withImages = false)
+{
+	const RigObservations seen = observeCameraRig(rig, pose);
+
+	rtabmap_msgs::msg::RGBDImages msg;
+	msg.header.stamp = stampOf(stamp);
+	msg.header.frame_id = rig.frameIds[0];
+	for(size_t i=0; i<rig.cameras(); ++i)
+	{
+		rtabmap_msgs::msg::RGBDImage image;
+		image.header.stamp = msg.header.stamp;
+		image.header.frame_id = rig.frameIds[i];
+		// No image at all by default, neither color nor depth: this frame is its
+		// calibration and its features, which is everything a camera doing its own
+		// extraction has to send.
+		if(withImages)
+		{
+			image.rgb = makeImage(rig.frameIds[i], stamp,
+					cv::Mat::zeros(rig.height, rig.width, CV_8UC1), "mono8");
+		}
+		image.rgb_camera_info = makeCameraInfo(
+				rig.frameIds[i], stamp, rig.width, rig.height, 0.0, rig.fx);
+		image.depth_camera_info = image.rgb_camera_info;
+		image.key_points = seen.keyPoints[i];
+		image.points = seen.points[i];
+		image.descriptors = rtabmap::compressData(seen.descriptors[i]);
+		msg.rgbd_images.push_back(image);
+	}
+	return msg;
+}
+
+/**
+ * @brief The same observations as stereo frames: left camera plus a right one @p baseline
+ * to its side.
+ *
+ * `RGBDImage` carries a stereo pair as its color and depth fields, so this differs from
+ * the RGB-D frames above only in the second calibration, whose `P(0,3)` is what tells the
+ * node how far apart the two cameras are. The features belong to the left image either
+ * way, which is where a stereo pipeline finds them.
+ */
+inline rtabmap_msgs::msg::RGBDImages cameraRigStereoFrame(
+		const CameraRig & rig, const rtabmap::Transform & pose, double stamp,
+		double baseline = 0.12, bool withImages = false)
+{
+	rtabmap_msgs::msg::RGBDImages msg = cameraRigFrame(rig, pose, stamp, withImages);
+	for(size_t i=0; i<msg.rgbd_images.size(); ++i)
+	{
+		msg.rgbd_images[i].depth_camera_info = makeCameraInfo(
+				rig.frameIds[i], stamp, rig.width, rig.height, -baseline*rig.fx, rig.fx);
+		if(withImages)
+		{
+			msg.rgbd_images[i].depth = makeImage(rig.frameIds[i], stamp,
+					cv::Mat::zeros(rig.height, rig.width, CV_8UC1), "mono8");
+		}
+	}
+	return msg;
+}
+
+/// How many features @p frame carries, all cameras together.
+inline size_t cameraRigFeatureCount(const rtabmap_msgs::msg::RGBDImages & frame)
+{
+	size_t count = 0;
+	for(size_t i=0; i<frame.rgbd_images.size(); ++i)
+	{
+		count += frame.rgbd_images[i].key_points.size();
+	}
+	return count;
+}
+
+}  // namespace rtabmap_odom_test
+
+#endif /* RTABMAP_ODOM_CAMERA_RIG_HPP_ */

rtabmap_odom/test/data/README.md

@@ -0,0 +1,103 @@
+# Test data
+
+Real frames for the odometry node tests, so that they register actual imagery instead of
+synthetic noise. Synthetic textures give a detector corners that match nothing between
+frames, which makes a "motion" that only proves the node did not crash.
+
+Everything here is BSD-3-Clause, same authors and same terms as the rest of this
+repository.
+
+| Path | Origin | Used by |
+| --- | --- | --- |
+| `stereo/rect/{left,right}/{50,60}.jpg` | [RTAB-Map `data/stereo_rect`](https://github.com/introlab/rtabmap/tree/master/data/stereo_rect) | `test_stereo_odometry.cpp` |
+| `stereo/rect/stereo_{left,right}.yaml` | [RTAB-Map `data/stereo_rect`](https://github.com/introlab/rtabmap/tree/master/data/stereo_rect) | `test_stereo_odometry.cpp` |
+| `stereo/raw/{left,right}/{420,425}.jpg` | frames 420 and 425 (21.00 s and 21.25 s at 20 Hz) of the [stereo indoor tutorial](https://github.com/introlab/rtabmap/wiki/Stereo-mapping) test sequence | `test_stereo_odometry.cpp` |
+| `stereo/raw/stereo_{left,right}.yaml`, `stereo/raw/stereo_pose.yaml` | that rig's own calibration (`stereo_tutorial_*`) | `test_stereo_odometry.cpp` |
+| `lidar/ouster_half_turn/` | frames 1613418430.682 and 1613418435.082 of an Ouster recording on a rotating mast | `test_icp_odometry.cpp` |
+| `rgbd/rgb/{17,154}.jpg`, `rgbd/depth/{17,154}.png` | [RTAB-Map `data/rgbd`](https://github.com/introlab/rtabmap/tree/master/data/rgbd) | `test_rgbd_odometry.cpp` |
+| `rgbd/calib/{17,154}.yaml` | [RTAB-Map `data/rgbd`](https://github.com/introlab/rtabmap/tree/master/data/rgbd) | `test_rgbd_odometry.cpp` |
+
+They are vendored rather than read from an RTAB-Map checkout because RTAB-Map reaches us
+as an installed library: the `introlab3it/rtabmap` images used by CI delete the source tree
+after `make install`, and the ROS buildfarm has no network during a build. A copy here is
+what makes these tests run everywhere rather than skip.
+
+## What the frames are
+
+Two frames of each kind is the minimum that says anything: the first initialises the
+odometry at the origin, the second has to be registered against it.
+
+- **`stereo/rect`** -- `50` and `60`, two rectified pairs of the same scene a short motion
+  apart. The estimate comes out at 0.171 m, steady to well under a centimetre across runs.
+  These back `corelib/test/test_odometry.cpp` upstream, so a failure here that also fails
+  there is an RTAB-Map issue rather than a ROS one.
+- **`stereo/raw`** -- `420` and `425`, an unrectified pair a quarter second apart, for the
+  `Rtabmap/ImagesAlreadyRectified:=false` path described in `doc/stereo_odometry.md`, and
+  for pinning what happens when that parameter is left at its default on distorted images.
+  A quarter second of walking forward, ~0.233 m, reproduced to a fraction of a percent
+  across runs. Wider gaps in the same window register too, but not reliably: 21.00 s to
+  22.00 s is ~1.04 m at around 45 inliers and lost tracking outright in one run out of
+  eight, where this pair holds 210 or more.
+- **`lidar/ouster_half_turn`** -- two Ouster sweeps 4.40 s apart, recorded as a ROS 2 mcap
+  bag (`/tf`, `/tf_static`, `/os_cloud_node/points`) rather than as loose files, because
+  what makes them worth keeping is the TF history around them.
+
+  The sensor sits on a mast turning at ~42 deg/s while the platform stays put, so it moves
+  through its own 0.1 s sweep and the cloud comes off the driver skewed. The two sweeps are
+  half a turn apart (184.5 deg), which puts the skew in opposite directions and makes a
+  missing correction obvious. And because the platform never moves -- `base_link` ->
+  `box_link` does not translate by a single millimetre over the whole recording -- the
+  answer is known: the transform between the two is the identity. That is what the test
+  measures against, rather than a value taken from a previous run.
+
+  TF runs from 0.1 s before each sweep to 0.1 s past its end, with nothing in between: the
+  gap holds transforms nobody looks up, and keeping them would have tripled the file.
+- **`rgbd`** -- `17` and `154`, two frames of a hand-held Kinect sequence, far enough apart
+  that losing tracking between them is a legitimate outcome.
+
+In every set the left image is color and the right one grayscale, as the cameras recorded
+them.
+
+Both stereo sets come from the same 640x480 rig, but **not** from the same calibration, and
+the two are not interchangeable:
+
+| | `stereo/rect` | `stereo/raw` |
+| --- | --- | --- |
+| `distortion_coefficients` | zeros | the lens's real plumb_bob values (~-0.34) |
+| `rectification_matrix` | identity | the rotation into the rectified frame |
+| `projection_matrix` fx | 487.61 | 500.22 |
+| baseline (`-Tx/fx`) | 0.1197 m | 0.1197 m |
+
+Rectification is what leaves a calibration with no distortion and an identity rotation, so
+the rectified file describes the output of `stereo_image_proc`, not what the camera
+produced. Handing it to a raw pair claims a distortion-free lens the images do not have:
+doing that costs roughly a third of the inliers (110 against 214) and triples the reported
+standard deviation. `stereo_pose.yaml` holds the rig's measured extrinsics -- ~12 cm along
+x plus a few milliradians of rotation -- which the tests publish as the TF between
+`camera_left` and `camera_right`, the transform the node looks up when it has to rectify
+the pair itself.
+
+## File formats
+
+The images are copied as they are. The calibration files differ from their originals by one
+line: the format directive is commented out (`#%YAML:1.0`, with no `---`), which is the ROS
+flavour of the same file. `%YAML:1.0` is not a valid YAML directive, so plain YAML parsers
+-- `camera_info_manager`, `rosparam`, PyYAML -- reject the original form.
+`test/test_data.hpp` puts the directive back in memory before handing the text to
+`cv::FileStorage`, so the files stay readable by both.
+
+Note that they carry no OpenCV `dt` field either, so `>> cv::Mat` cannot read them;
+`rows`/`cols`/`data` are read element by element, as RTAB-Map's own `CameraModel::load`
+does.
+
+The depth images are 16-bit millimetres. `rgbd/calib/*.yaml` also carries a
+`local_transform` (the optical-frame-to-robot transform RTAB-Map stores with the camera
+model); the ROS nodes take that from TF instead, so the tests publish it as a `base_link`
+-> `camera` static transform rather than reading it here.
+
+## Using them
+
+`test/test_data.hpp` loads these into `cv::Mat`, `sensor_msgs/CameraInfo` and
+`geometry_msgs/Transform`. CMake passes the directory as `RTABMAP_ODOM_TEST_DATA_ROOT`,
+pointing into the source tree: the test binaries are not installed, and neither are these
+files.

rtabmap_odom/test/data/lidar/ouster_half_turn/metadata.yaml

@@ -0,0 +1,52 @@
+rosbag2_bagfile_information:
+  compression_format: ''
+  compression_mode: ''
+  custom_data: null
+  duration:
+    nanoseconds: 5066021600
+  files:
+  - duration:
+      nanoseconds: 5066021600
+    message_count: 120
+    path: ouster_half_turn.mcap
+    starting_time:
+      nanoseconds_since_epoch: 1613418430206475184
+  message_count: 120
+  relative_file_paths:
+  - ouster_half_turn.mcap
+  ros_distro: rosbags
+  starting_time:
+    nanoseconds_since_epoch: 1613418430206475184
+  storage_identifier: mcap
+  topics_with_message_count:
+  - message_count: 2
+    topic_metadata:
+      name: /tf_static
+      offered_qos_profiles: "- avoid_ros_namespace_conventions: false\n  deadline:
+        {nsec: 0, sec: 0}\n  depth: 10\n  durability: 1\n  history: 1\n  lifespan:
+        {nsec: 0, sec: 0}\n  liveliness: 1\n  liveliness_lease_duration: {nsec: 0,
+        sec: 0}\n  reliability: 1"
+      serialization_format: cdr
+      type: tf2_msgs/msg/TFMessage
+      type_description_hash: RIHS01_e369d0f05a23ae52508854b66f6aa0437f3449d652e8cbf22d5abe85d020f087
+  - message_count: 116
+    topic_metadata:
+      name: /tf
+      offered_qos_profiles: "- avoid_ros_namespace_conventions: false\n  deadline:
+        {nsec: 0, sec: 0}\n  depth: 10\n  durability: 2\n  history: 1\n  lifespan:
+        {nsec: 0, sec: 0}\n  liveliness: 1\n  liveliness_lease_duration: {nsec: 0,
+        sec: 0}\n  reliability: 1"
+      serialization_format: cdr
+      type: tf2_msgs/msg/TFMessage
+      type_description_hash: RIHS01_e369d0f05a23ae52508854b66f6aa0437f3449d652e8cbf22d5abe85d020f087
+  - message_count: 2
+    topic_metadata:
+      name: /os_cloud_node/points
+      offered_qos_profiles: "- avoid_ros_namespace_conventions: false\n  deadline:
+        {nsec: 0, sec: 0}\n  depth: 10\n  durability: 2\n  history: 1\n  lifespan:
+        {nsec: 0, sec: 0}\n  liveliness: 1\n  liveliness_lease_duration: {nsec: 0,
+        sec: 0}\n  reliability: 1"
+      serialization_format: cdr
+      type: sensor_msgs/msg/PointCloud2
+      type_description_hash: RIHS01_9198cabf7da3796ae6fe19c4cb3bdd3525492988c70522628af5daa124bae2b5
+  version: 8

rtabmap_odom/test/data/rgbd/calib/154.yaml

@@ -0,0 +1,16 @@
+#%YAML:1.0
+camera_name: "154"
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 525., 0., 3.1950000000000000e+02, 0., 525.,
+       2.3950000000000000e+02, 0., 0., 1. ]
+local_transform:
+   rows: 3
+   cols: 4
+   data: [ -1.09767914e-03, 5.99460304e-02, 9.98201132e-01,
+       4.20193411e-02, -9.99999523e-01, -6.60419464e-05, -1.09562278e-03,
+       -8.86659764e-05, 8.94069672e-08, -9.98201728e-01, 5.99459410e-02,
+       4.28920656e-01 ]

rtabmap_odom/test/data/rgbd/calib/17.yaml

@@ -0,0 +1,16 @@
+#%YAML:1.0
+camera_name: "17"
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 525., 0., 3.1950000000000000e+02, 0., 525.,
+       2.3950000000000000e+02, 0., 0., 1. ]
+local_transform:
+   rows: 3
+   cols: 4
+   data: [ -1.46162510e-03, 5.99460006e-02, 9.98200655e-01,
+       6.20153509e-02, -9.99999106e-01, -8.77380371e-05, -1.45888329e-03,
+       -1.63501027e-04, -2.98023224e-08, -9.98201728e-01, 5.99459410e-02,
+       4.28920656e-01 ]

rtabmap_odom/test/data/stereo/raw/stereo_left.yaml

@@ -0,0 +1,34 @@
+#%YAML:1.0
+camera_name: stereo_tutorial_left
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 5.2500741669069100e+02, 0., 3.1964931134995413e+02, 0.,
+       5.2447137699104690e+02, 2.4893842459131687e+02, 0., 0., 1. ]
+distortion_coefficients:
+   rows: 1
+   cols: 5
+   data: [ -3.4256158963391159e-01, 1.5067561187251743e-01,
+       -1.1171618396794343e-03, -1.1496904882258663e-03,
+       -3.4792309849001175e-02 ]
+distortion_model: plumb_bob
+rectification_matrix:
+   rows: 3
+   cols: 3
+   data: [ 9.9847710457215999e-01, -7.5204006718293083e-03,
+       -5.4652678058179430e-02, 7.6102614351676607e-03,
+       9.9997001005750097e-01, 1.4362821762221084e-03,
+       5.4640237610064049e-02, -1.8500160368176528e-03,
+       9.9850439251641721e-01 ]
+projection_matrix:
+   rows: 3
+   cols: 4
+   data: [ 5.0021545003868783e+02, 0., 3.5945997238159180e+02, 0., 0.,
+       5.0021545003868783e+02, 2.4750019836425781e+02, 0., 0., 0., 1.,
+       0. ]
+local_transform:
+   rows: 3
+   cols: 4
+   data: [ 0., 0., 1., 0., -1., 0., 0., 0., 0., -1., 0., 0. ]

rtabmap_odom/test/data/stereo/raw/stereo_pose.yaml

@@ -0,0 +1,30 @@
+#%YAML:1.0
+camera_name: stereo_tutorial
+rotation_matrix:
+   rows: 3
+   cols: 3
+   data: [ 9.9998244174079198e-01, -9.1486052999498408e-04,
+       5.8548475927491656e-03, 8.9587501387632107e-04,
+       9.9999433529188531e-01, 3.2445018261516327e-03,
+       -5.8577826934067389e-03, -3.2391996466791719e-03,
+       9.9997759673282971e-01 ]
+translation_matrix:
+   rows: 3
+   cols: 1
+   data: [ -1.1944376542810328e-01, 8.1410498203477041e-04,
+       7.2368896323297812e-03 ]
+essential_matrix:
+   rows: 3
+   cols: 3
+   data: [ -1.1252198674164329e-05, -7.2394856860325228e-03,
+       7.9060664179560138e-04, 6.5370869431856798e-03,
+       -3.9352294745729046e-04, 1.1948346038335741e-01,
+       -9.2109737277881536e-04, -1.1944234402152067e-01,
+       -3.9230197564821978e-04 ]
+fundamental_matrix:
+   rows: 3
+   cols: 3
+   data: [ -2.5309754099766013e-08, -1.6300536361959590e-05,
+       4.9995535977749410e-03, 1.4720205166478256e-05,
+       -8.8704021902692202e-07, 1.3677019067204871e-01,
+       -4.6630670977502930e-03, -1.3776019369349130e-01, 1. ]

rtabmap_odom/test/data/stereo/raw/stereo_right.yaml

@@ -0,0 +1,34 @@
+#%YAML:1.0
+camera_name: stereo_tutorial_right
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 5.3173015723615038e+02, 0., 3.0841183762414585e+02, 0.,
+       5.3114393189729469e+02, 2.4247036103612575e+02, 0., 0., 1. ]
+distortion_coefficients:
+   rows: 1
+   cols: 5
+   data: [ -3.2382605112765667e-01, 4.3067676369775459e-02,
+       -1.0212741406694578e-03, 5.1816193644504472e-04,
+       9.9965184406768520e-02 ]
+distortion_model: plumb_bob
+rectification_matrix:
+   rows: 3
+   cols: 3
+   data: [ 9.9814647006952373e-01, -6.8031680948065307e-03,
+       -6.0475955483342586e-02, 6.7037039320888168e-03,
+       9.9997582338248303e-01, -1.8474317621778344e-03,
+       6.0487061768119681e-02, 1.4385945915416094e-03,
+       9.9816794468879888e-01 ]
+projection_matrix:
+   rows: 3
+   cols: 4
+   data: [ 5.0021545003868783e+02, 0., 3.5945997238159180e+02,
+       -5.9858566522579160e+01, 0., 5.0021545003868783e+02,
+       2.4750019836425781e+02, 0., 0., 0., 1., 0. ]
+local_transform:
+   rows: 3
+   cols: 4
+   data: [ 0., 0., 1., 0., -1., 0., 0., 0., 0., -1., 0., 0. ]

rtabmap_odom/test/data/stereo/rect/stereo_left.yaml

@@ -0,0 +1,24 @@
+#%YAML:1.0
+camera_name: stereo_left
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02, 0.,
+       4.8760873413085938e+02, 2.4944424438476562e+02, 0., 0., 1. ]
+distortion_coefficients:
+   rows: 1
+   cols: 5
+   data: [ 0., 0., 0., 0., 0. ]
+distortion_model: plumb_bob
+rectification_matrix:
+   rows: 3
+   cols: 3
+   data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
+projection_matrix:
+   rows: 3
+   cols: 4
+   data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02, 0., 0.,
+       4.8760873413085938e+02, 2.4944424438476562e+02, 0., 0., 0., 1.,
+       0. ]

rtabmap_odom/test/data/stereo/rect/stereo_right.yaml

@@ -0,0 +1,24 @@
+#%YAML:1.0
+camera_name: stereo_right
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02, 0.,
+       4.8760873413085938e+02, 2.4944424438476562e+02, 0., 0., 1. ]
+distortion_coefficients:
+   rows: 1
+   cols: 5
+   data: [ 0., 0., 0., 0., 0. ]
+distortion_model: plumb_bob
+rectification_matrix:
+   rows: 3
+   cols: 3
+   data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
+projection_matrix:
+   rows: 3
+   cols: 4
+   data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02,
+       -5.8362700032946350e+01, 0., 4.8760873413085938e+02,
+       2.4944424438476562e+02, 0., 0., 0., 1., 0. ]

rtabmap_odom/test/msg_builders.hpp

@@ -0,0 +1,362 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_ODOM_MSG_BUILDERS_HPP_
+#define RTABMAP_ODOM_MSG_BUILDERS_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/image_encodings.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rtabmap_msgs/msg/odom_info.hpp>
+#include <rtabmap_msgs/msg/rgbd_image.hpp>
+#include <rtabmap_msgs/msg/scan_descriptor.hpp>
+#include <rtabmap_msgs/msg/sensor_data.hpp>
+#include <rtabmap_msgs/msg/user_data.hpp>
+
+#include <opencv2/core/core.hpp>
+#ifdef PRE_ROS_IRON
+#include <cv_bridge/cv_bridge.h>
+#else
+#include <cv_bridge/cv_bridge.hpp>
+#endif
+
+#include <string>
+#include <vector>
+
+namespace rtabmap_odom_test {
+
+/// A ROS time from a double, the way sensor stamps are written throughout these tests.
+inline rclcpp::Time stampOf(double seconds)
+{
+	return rclcpp::Time(
+			int32_t(seconds), uint32_t((seconds - int32_t(seconds)) * 1e9), RCL_ROS_TIME);
+}
+
+/// A rectified pinhole CameraInfo; @p tx is P(0,3), non-zero for a stereo right camera.
+inline sensor_msgs::msg::CameraInfo makeCameraInfo(
+		const std::string & frameId, double stamp, int width = 8, int height = 8,
+		double tx = 0.0, double fx = 100.0)
+{
+	sensor_msgs::msg::CameraInfo info;
+	info.header.frame_id = frameId;
+	info.header.stamp = stampOf(stamp);
+	info.width = width;
+	info.height = height;
+	info.distortion_model = "plumb_bob";
+	info.d = {0.0, 0.0, 0.0, 0.0, 0.0};
+	info.k = {fx, 0.0, width/2.0, 0.0, fx, height/2.0, 0.0, 0.0, 1.0};
+	info.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
+	info.p = {fx, 0.0, width/2.0, tx, 0.0, fx, height/2.0, 0.0, 0.0, 0.0, 1.0, 0.0};
+	return info;
+}
+
+inline sensor_msgs::msg::Image makeImage(
+		const std::string & frameId, double stamp,
+		const cv::Mat & image, const std::string & encoding)
+{
+	std_msgs::msg::Header header;
+	header.frame_id = frameId;
+	header.stamp = stampOf(stamp);
+	sensor_msgs::msg::Image msg;
+	cv_bridge::CvImage(header, encoding, image).toImageMsg(msg);
+	return msg;
+}
+
+/// A bgr8 color image of a single flat color.
+inline sensor_msgs::msg::Image makeRgbImage(
+		const std::string & frameId, double stamp, int width = 8, int height = 8,
+		const cv::Scalar & color = cv::Scalar(10, 20, 30))
+{
+	return makeImage(frameId, stamp, cv::Mat(height, width, CV_8UC3, color), "bgr8");
+}
+
+/// A 16UC1 depth image in millimeters, the encoding the RGB-D drivers publish.
+inline sensor_msgs::msg::Image makeDepthImage(
+		const std::string & frameId, double stamp, int width = 8, int height = 8,
+		uint16_t millimeters = 1500)
+{
+	return makeImage(frameId, stamp,
+			cv::Mat(height, width, CV_16UC1, cv::Scalar(millimeters)), "16UC1");
+}
+
+/// A mono8 image, used as a stereo left or right frame.
+inline sensor_msgs::msg::Image makeMonoImage(
+		const std::string & frameId, double stamp, int width = 8, int height = 8,
+		uint8_t value = 60)
+{
+	return makeImage(frameId, stamp,
+			cv::Mat(height, width, CV_8UC1, cv::Scalar(value)), "mono8");
+}
+
+/// An RGB-D message with raw bgr8 color and 16UC1 depth, as rgbd_sync publishes it.
+inline rtabmap_msgs::msg::RGBDImage makeRGBDImage(
+		const std::string & frameId, double stamp, int width = 8, int height = 8,
+		const cv::Scalar & rgbColor = cv::Scalar(10, 20, 30), uint16_t depthValue = 1500)
+{
+	rtabmap_msgs::msg::RGBDImage msg;
+	msg.header.frame_id = frameId;
+	msg.header.stamp = stampOf(stamp);
+	msg.rgb = makeRgbImage(frameId, stamp, width, height, rgbColor);
+	msg.depth = makeDepthImage(frameId, stamp, width, height, depthValue);
+	msg.rgb_camera_info = makeCameraInfo(frameId, stamp, width, height);
+	msg.depth_camera_info = makeCameraInfo(frameId, stamp, width, height);
+	return msg;
+}
+
+/// A flat LaserScan of @p count equal ranges over 180 degrees.
+inline sensor_msgs::msg::LaserScan makeLaserScan(
+		const std::string & frameId, double stamp, size_t count = 10, float range = 2.0f)
+{
+	sensor_msgs::msg::LaserScan scan;
+	scan.header.frame_id = frameId;
+	scan.header.stamp = stampOf(stamp);
+	scan.angle_min = -M_PI_2;
+	scan.angle_max = M_PI_2;
+	scan.angle_increment = count > 1 ? float(M_PI / double(count - 1)) : float(M_PI);
+	scan.time_increment = 0.0f;
+	scan.scan_time = 0.1f;
+	scan.range_min = 0.1f;
+	scan.range_max = 10.0f;
+	scan.ranges.assign(count, range);
+	return scan;
+}
+
+/// A dense unorganized XYZ float cloud, the shape a 3D lidar driver publishes.
+inline sensor_msgs::msg::PointCloud2 makeXYZCloud(
+		const std::string & frameId, double stamp,
+		const std::vector<cv::Point3f> & points)
+{
+	sensor_msgs::msg::PointCloud2 cloud;
+	cloud.header.frame_id = frameId;
+	cloud.header.stamp = stampOf(stamp);
+	cloud.height = 1;
+	cloud.width = points.size();
+	cloud.is_bigendian = false;
+	cloud.is_dense = true;
+
+	cloud.fields.resize(3);
+	const char * names[3] = {"x", "y", "z"};
+	for(int i=0; i<3; ++i)
+	{
+		cloud.fields[i].name = names[i];
+		cloud.fields[i].offset = 4 * i;
+		cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
+		cloud.fields[i].count = 1;
+	}
+	cloud.point_step = 12;
+	cloud.row_step = cloud.point_step * cloud.width;
+	cloud.data.resize(cloud.row_step * cloud.height);
+
+	for(size_t i=0; i<points.size(); ++i)
+	{
+		float * p = reinterpret_cast<float *>(&cloud.data[i * cloud.point_step]);
+		p[0] = points[i].x;
+		p[1] = points[i].y;
+		p[2] = points[i].z;
+	}
+	return cloud;
+}
+
+/**
+ * @brief An XYZ cloud carrying the optional fields a 3D lidar may add.
+ *
+ * `intensity` and the `normal_*`/`curvature` group each change which PCL point type the
+ * odometry converts the cloud into, so a driver that sends them takes a different path
+ * through the node than one that sends plain XYZ. `t` is the per-point offset from the
+ * header stamp that deskewing needs, spread evenly over @p sweep seconds.
+ */
+inline sensor_msgs::msg::PointCloud2 makeCloudWithFields(
+		const std::string & frameId, double stamp,
+		const std::vector<cv::Point3f> & points,
+		bool withIntensity, bool withNormals,
+		const cv::Point3f & normal = cv::Point3f(0, 0, 1),
+		bool withTime = false, float sweep = 0.01f)
+{
+	sensor_msgs::msg::PointCloud2 cloud;
+	cloud.header.frame_id = frameId;
+	cloud.header.stamp = stampOf(stamp);
+	cloud.height = 1;
+	cloud.width = points.size();
+	cloud.is_bigendian = false;
+	cloud.is_dense = true;
+
+	std::vector<std::string> names = {"x", "y", "z"};
+	if(withIntensity)
+	{
+		names.push_back("intensity");
+	}
+	if(withNormals)
+	{
+		names.push_back("normal_x");
+		names.push_back("normal_y");
+		names.push_back("normal_z");
+		names.push_back("curvature");
+	}
+	if(withTime)
+	{
+		names.push_back("t");
+	}
+	cloud.fields.resize(names.size());
+	for(size_t i=0; i<names.size(); ++i)
+	{
+		cloud.fields[i].name = names[i];
+		cloud.fields[i].offset = uint32_t(4 * i);
+		cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
+		cloud.fields[i].count = 1;
+	}
+	cloud.point_step = uint32_t(4 * names.size());
+	cloud.row_step = cloud.point_step * cloud.width;
+	cloud.data.resize(size_t(cloud.row_step) * cloud.height);
+
+	for(size_t i=0; i<points.size(); ++i)
+	{
+		float * p = reinterpret_cast<float *>(&cloud.data[i * cloud.point_step]);
+		size_t f = 0;
+		p[f++] = points[i].x;
+		p[f++] = points[i].y;
+		p[f++] = points[i].z;
+		if(withIntensity)
+		{
+			p[f++] = float(i % 256);
+		}
+		if(withNormals)
+		{
+			p[f++] = normal.x;
+			p[f++] = normal.y;
+			p[f++] = normal.z;
+			p[f++] = 0.0f;   // curvature
+		}
+		if(withTime)
+		{
+			p[f++] = points.size() > 1 ?
+					sweep * float(i) / float(points.size() - 1) : 0.0f;
+		}
+	}
+	return cloud;
+}
+
+/// A small cloud on a line, enough to tell one scan from another.
+inline sensor_msgs::msg::PointCloud2 makeScanCloud(
+		const std::string & frameId, double stamp, size_t count = 4)
+{
+	std::vector<cv::Point3f> points;
+	points.reserve(count);
+	for(size_t i=0; i<count; ++i)
+	{
+		points.push_back(cv::Point3f(1.0f + float(i), 0.0f, 0.0f));
+	}
+	return makeXYZCloud(frameId, stamp, points);
+}
+
+/// A ScanDescriptor carrying a 2D scan, a 3D scan, or both, and optionally a descriptor.
+inline rtabmap_msgs::msg::ScanDescriptor makeScanDescriptor(
+		const std::string & frameId, double stamp,
+		bool with2d = true, bool with3d = false, bool withGlobalDescriptor = false)
+{
+	rtabmap_msgs::msg::ScanDescriptor msg;
+	msg.header.frame_id = frameId;
+	msg.header.stamp = stampOf(stamp);
+	if(with2d)
+	{
+		msg.scan = makeLaserScan(frameId, stamp);
+	}
+	if(with3d)
+	{
+		msg.scan_cloud = makeScanCloud(frameId, stamp);
+	}
+	if(withGlobalDescriptor)
+	{
+		// Only "not empty" matters here: consumers pass the payload straight to
+		// RTAB-Map, which is what knows how to decode it.
+		msg.global_descriptor.header = msg.header;
+		msg.global_descriptor.data = {1, 2, 3, 4};
+	}
+	return msg;
+}
+
+/// An identity-pose odometry message at @p x meters along the x axis.
+inline nav_msgs::msg::Odometry makeOdometry(
+		const std::string & frameId, double stamp, double x = 0.0,
+		const std::string & childFrameId = "base_link")
+{
+	nav_msgs::msg::Odometry msg;
+	msg.header.frame_id = frameId;
+	msg.header.stamp = stampOf(stamp);
+	msg.child_frame_id = childFrameId;
+	msg.pose.pose.position.x = x;
+	msg.pose.pose.orientation.w = 1.0;
+	return msg;
+}
+
+inline rtabmap_msgs::msg::OdomInfo makeOdomInfo(
+		const std::string & frameId, double stamp, int inliers = 50)
+{
+	rtabmap_msgs::msg::OdomInfo msg;
+	msg.header.frame_id = frameId;
+	msg.header.stamp = stampOf(stamp);
+	msg.inliers = inliers;
+	msg.matches = inliers;
+	return msg;
+}
+
+/// A SensorData carrying one RGB-D camera, as rtabmap_odom republishes it.
+inline rtabmap_msgs::msg::SensorData makeSensorData(
+		const std::string & frameId, double stamp, int width = 8, int height = 8)
+{
+	rtabmap_msgs::msg::SensorData msg;
+	msg.header.frame_id = frameId;
+	msg.header.stamp = stampOf(stamp);
+	msg.left = makeRgbImage(frameId, stamp, width, height);
+	msg.right = makeDepthImage(frameId, stamp, width, height);
+	msg.left_camera_info.push_back(makeCameraInfo(frameId, stamp, width, height));
+	msg.right_camera_info.push_back(makeCameraInfo(frameId, stamp, width, height));
+	geometry_msgs::msg::Transform localTransform;
+	localTransform.rotation.w = 1.0;
+	msg.local_transform.push_back(localTransform);
+	return msg;
+}
+
+/// An uncompressed user data matrix (several rows, so it is not taken as compressed).
+inline rtabmap_msgs::msg::UserData makeUserData(
+		const std::string & frameId, double stamp)
+{
+	rtabmap_msgs::msg::UserData msg;
+	msg.header.frame_id = frameId;
+	msg.header.stamp = stampOf(stamp);
+	msg.rows = 2;
+	msg.cols = 2;
+	msg.type = CV_8UC1;
+	msg.data = {1, 2, 3, 4};
+	return msg;
+}
+
+/**
+ * @brief Reads the x/y/z of a point from any FLOAT32 xyz cloud.
+ *
+ * Looks the offsets up in the field list rather than assuming they are 0/4/8.
+ */
+inline cv::Point3f readXYZ(const sensor_msgs::msg::PointCloud2 & cloud, size_t index)
+{
+	uint32_t xOffset = 0, yOffset = 4, zOffset = 8;
+	for(size_t i=0; i<cloud.fields.size(); ++i)
+	{
+		if(cloud.fields[i].name == "x") { xOffset = cloud.fields[i].offset; }
+		else if(cloud.fields[i].name == "y") { yOffset = cloud.fields[i].offset; }
+		else if(cloud.fields[i].name == "z") { zOffset = cloud.fields[i].offset; }
+	}
+	const unsigned char * base = &cloud.data[index * cloud.point_step];
+	return cv::Point3f(
+			*reinterpret_cast<const float *>(base + xOffset),
+			*reinterpret_cast<const float *>(base + yOffset),
+			*reinterpret_cast<const float *>(base + zOffset));
+}
+
+}  // namespace rtabmap_odom_test
+
+#endif /* RTABMAP_ODOM_MSG_BUILDERS_HPP_ */

rtabmap_odom/test/node_test_utils.hpp

@@ -0,0 +1,256 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE AUTHOR 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 RTABMAP_ODOM_NODE_TEST_UTILS_HPP_
+#define RTABMAP_ODOM_NODE_TEST_UTILS_HPP_
+
+#include <gtest/gtest.h>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+namespace rtabmap_odom_test {
+
+/**
+ * @brief Brings rclcpp up once for the whole test binary.
+ *
+ * Registered as a gtest global environment so it runs before the first test and shuts
+ * down after the last one, which keeps gtest_main usable.
+ */
+class RclcppEnvironment : public ::testing::Environment
+{
+public:
+	void SetUp() override
+	{
+		if(!rclcpp::ok())
+		{
+			rclcpp::init(0, nullptr);
+		}
+	}
+	void TearDown() override
+	{
+		if(rclcpp::ok())
+		{
+			rclcpp::shutdown();
+		}
+	}
+};
+
+/// Registers RclcppEnvironment. Call once at file scope in each test binary.
+inline ::testing::Environment * registerRclcppEnvironment()
+{
+	static ::testing::Environment * const env =
+			::testing::AddGlobalTestEnvironment(new RclcppEnvironment);
+	return env;
+}
+
+/**
+ * @brief Base fixture for driving a node under test over real ROS topics.
+ *
+ * The node under test and a helper node share one single-threaded executor, so
+ * publishing, the node's callback and the assertion all happen on the same thread and
+ * the tests stay deterministic. No launch files and no separate processes are involved:
+ * everything runs in the gtest binary.
+ */
+class NodeTest : public ::testing::Test
+{
+protected:
+	void SetUp() override
+	{
+		executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+		helper_ = std::make_shared<rclcpp::Node>("rtabmap_odom_test_helper");
+		executor_->add_node(helper_);
+	}
+
+	void TearDown() override
+	{
+		for(const rclcpp::Node::SharedPtr & node : nodes_)
+		{
+			executor_->remove_node(node);
+		}
+		nodes_.clear();
+		executor_->remove_node(helper_);
+		helper_.reset();
+		executor_.reset();
+	}
+
+	/**
+	 * @brief Adds a node under test to the shared executor and keeps it alive for the test.
+	 *
+	 * The wait is for tf2_ros, not for anything the test does with the node.
+	 * ~TransformListener cancels its worker's executor and joins it without ordering the
+	 * cancel after the worker reached spin(), so a node dropped microseconds after it was
+	 * built -- which a test that only reads a parameter back does -- hangs the binary for
+	 * good (ros2/geometry2#517). The window is a few instructions wide and nothing here
+	 * can observe that thread, so this buys time instead. Drop it once #752 lands.
+	 */
+	template <typename NodeT>
+	std::shared_ptr<NodeT> addNode(const std::shared_ptr<NodeT> & node)
+	{
+		executor_->add_node(node);
+		nodes_.push_back(node);
+		spinFor(std::chrono::milliseconds(50));
+		return node;
+	}
+
+	/// The helper node, used to publish inputs and subscribe to outputs.
+	rclcpp::Node::SharedPtr helper() { return helper_; }
+
+	/**
+	 * @brief Spins until @p done returns true, or the timeout elapses.
+	 * @return true if @p done became true
+	 */
+	bool spinUntil(
+			const std::function<bool()> & done,
+			std::chrono::milliseconds timeout = std::chrono::milliseconds(5000))
+	{
+		const std::chrono::steady_clock::time_point deadline =
+				std::chrono::steady_clock::now() + timeout;
+		while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
+		{
+			if(done())
+			{
+				return true;
+			}
+			executor_->spin_once(std::chrono::milliseconds(10));
+		}
+		return done();
+	}
+
+	/// Spins for a fixed duration, for the "nothing should happen" assertions.
+	void spinFor(std::chrono::milliseconds duration)
+	{
+		const std::chrono::steady_clock::time_point deadline =
+				std::chrono::steady_clock::now() + duration;
+		while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
+		{
+			executor_->spin_once(std::chrono::milliseconds(10));
+		}
+	}
+
+	/**
+	 * @brief Waits until @p publisher has at least @p count matched subscriptions.
+	 *
+	 * Publishing before the node under test has discovered the topic silently drops the
+	 * message, which is the most common cause of a flaky in-process node test.
+	 */
+	template <typename PublisherT>
+	bool waitForSubscriber(const PublisherT & publisher, size_t count = 1)
+	{
+		return spinUntil([&]() { return publisher->get_subscription_count() >= count; });
+	}
+
+	/**
+	 * @brief Waits until @p subscription sees at least one publisher.
+	 *
+	 * Every node here publishes only when it has subscribers, so the test's subscription
+	 * has to be discovered before the input is sent.
+	 */
+	template <typename SubscriptionT>
+	bool waitForPublisher(const SubscriptionT & subscription, size_t count = 1)
+	{
+		return spinUntil([&]() { return subscription->get_publisher_count() >= count; });
+	}
+
+	/// Collects every message received on @p topic, for later assertions.
+	template <typename MsgT>
+	struct Collector
+	{
+		typename rclcpp::Subscription<MsgT>::SharedPtr subscription;
+		std::vector<typename MsgT::ConstSharedPtr> messages;
+		size_t size() const { return messages.size(); }
+		bool empty() const { return messages.empty(); }
+
+		/**
+		 * @brief The last (first) message received.
+		 *
+		 * A test that reads these without having waited for the topic it is reading --
+		 * having waited for a different one, say -- gets a legible failure rather than a
+		 * segmentation fault: std::vector::back() on an empty vector dereferences
+		 * nullptr-1, which crashes the whole binary and takes the rest of its tests with
+		 * it. gtest turns the exception into a failure of the test that threw it.
+		 */
+		const MsgT & back() const { return *checked(messages.empty()?0:&messages.back()); }
+		const MsgT & front() const { return *checked(messages.empty()?0:&messages.front()); }
+
+	private:
+		const typename MsgT::ConstSharedPtr & checked(
+				const typename MsgT::ConstSharedPtr * msg) const
+		{
+			if(msg == 0)
+			{
+				throw std::out_of_range(
+						std::string("nothing was received on \"") +
+						(subscription?subscription->get_topic_name():"?") +
+						"\", so there is no message to read: wait for it to arrive first");
+			}
+			return *msg;
+		}
+	};
+
+	/**
+	 * @brief Subscribes the helper node to @p topic and records everything it receives.
+	 *
+	 * The callback holds the collector weakly. Capturing it by shared_ptr would close a
+	 * cycle -- collector owns the subscription, the subscription owns the callback, the
+	 * callback owns the collector -- and neither would ever be freed. A subscription that
+	 * outlives its test keeps the helper node's rcl handle alive with it, which leaves the
+	 * node's rosout publisher registered and greets the next test with "Publisher already
+	 * registered for node name: 'rtabmap_odom_test_helper'".
+	 */
+	template <typename MsgT>
+	std::shared_ptr<Collector<MsgT>> collect(
+			const std::string & topic, const rclcpp::QoS & qos = rclcpp::QoS(10))
+	{
+		std::shared_ptr<Collector<MsgT>> collector = std::make_shared<Collector<MsgT>>();
+		std::weak_ptr<Collector<MsgT>> weak = collector;
+		collector->subscription = helper_->create_subscription<MsgT>(
+				topic, qos,
+				[weak](const typename MsgT::ConstSharedPtr msg) {
+					if(std::shared_ptr<Collector<MsgT>> collector = weak.lock())
+					{
+						collector->messages.push_back(msg);
+					}
+				});
+		return collector;
+	}
+
+private:
+	rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+	rclcpp::Node::SharedPtr helper_;
+	std::vector<rclcpp::Node::SharedPtr> nodes_;
+};
+
+}  // namespace rtabmap_odom_test
+
+#endif /* RTABMAP_ODOM_NODE_TEST_UTILS_HPP_ */

rtabmap_odom/test/scan_scenes.hpp

@@ -0,0 +1,115 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_ODOM_SCAN_SCENES_HPP_
+#define RTABMAP_ODOM_SCAN_SCENES_HPP_
+
+#include <opencv2/core/core.hpp>
+
+#include <cmath>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <vector>
+
+namespace rtabmap_odom_test {
+
+/**
+ * A 3D corner -- floor plus two walls -- so all six degrees of freedom are constrained.
+ *
+ * Ported from makeCorner3D() in RTAB-Map's corelib/test/test_odometry.cpp. The jitter is
+ * not decoration: a perfectly flat lattice gives degenerate per-point normals and ICP
+ * finds no correspondences at all.
+ */
+inline std::vector<cv::Point3f> corner3D(
+		const cv::Point3f & offset = cv::Point3f(0,0,0),
+		float length = 4.0f, int pointsPerSurface = 400, uint64_t seed = 0xC0FFEE)
+{
+	cv::RNG rng(seed);
+	const float half = 0.5f * length;
+	std::vector<cv::Point3f> points;
+	points.reserve(3 * pointsPerSurface);
+	for(int i=0; i<pointsPerSurface; ++i)  // floor z=-half
+	{
+		points.push_back(cv::Point3f(rng.uniform(-half, half), rng.uniform(-half, half),
+				-half + float(rng.gaussian(0.005))) - offset);
+	}
+	for(int i=0; i<pointsPerSurface; ++i)  // wall x=-half
+	{
+		points.push_back(cv::Point3f(-half + float(rng.gaussian(0.005)),
+				rng.uniform(-half, half), rng.uniform(-half, half)) - offset);
+	}
+	for(int i=0; i<pointsPerSurface; ++i)  // wall y=-half
+	{
+		points.push_back(cv::Point3f(rng.uniform(-half, half),
+				-half + float(rng.gaussian(0.005)), rng.uniform(-half, half)) - offset);
+	}
+	return points;
+}
+
+/**
+ * @brief The same corner seen after the sensor has turned @p yaw about z.
+ *
+ * The corner does not move; the sensor does, so in the sensor's own frame every point
+ * turns the other way. This is what a scan taken after a rotation looks like.
+ */
+inline std::vector<cv::Point3f> corner3DTurned(double yaw)
+{
+	const double c = std::cos(-yaw);
+	const double s = std::sin(-yaw);
+	std::vector<cv::Point3f> points = corner3D();
+	for(cv::Point3f & p : points)
+	{
+		const float x = p.x;
+		p.x = float(c * x - s * p.y);
+		p.y = float(s * x + c * p.y);
+	}
+	return points;
+}
+
+/**
+ * @brief Range to a 2D corner from a sensor at (@p sensorX, @p sensorY) looking along +x.
+ *
+ * Two perpendicular walls, one ahead and one to the left. A single wall would leave the
+ * motion along it unobservable and ICP would settle wherever it started; the corner pins
+ * both axes and the heading.
+ *
+ * @return the nearer wall along the ray, or 0 if the ray reaches neither
+ */
+inline float corner2DRange(
+		double sensorX, double sensorY, double angle,
+		float frontWall = 5.0f, float leftWall = 3.0f)
+{
+	const double dx = std::cos(angle);
+	const double dy = std::sin(angle);
+	double best = 0.0;
+	if(dx > 1e-6)
+	{
+		best = (frontWall - sensorX) / dx;
+	}
+	if(dy > 1e-6)
+	{
+		const double toLeft = (leftWall - sensorY) / dy;
+		best = (best <= 0.0 || toLeft < best) ? toLeft : best;
+	}
+	return float(best);
+}
+
+/**
+ * The ICP settings RTAB-Map's own odometry tests use for this scene: point-to-point, no
+ * voxelization, and a correspondence ratio low enough for a synthetic scan.
+ */
+inline std::vector<rclcpp::Parameter> icpTestParameters()
+{
+	return {
+		rclcpp::Parameter("Icp/PointToPlane", "false"),
+		rclcpp::Parameter("scan_voxel_size", 0.0),
+		rclcpp::Parameter("Icp/CorrespondenceRatio", "0.1"),
+	};
+}
+
+}  // namespace rtabmap_odom_test
+
+#endif /* RTABMAP_ODOM_SCAN_SCENES_HPP_ */

rtabmap_odom/test/test_data.hpp

@@ -0,0 +1,268 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_ODOM_TEST_DATA_HPP_
+#define RTABMAP_ODOM_TEST_DATA_HPP_
+
+#include <geometry_msgs/msg/transform.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+
+#include <tf2/LinearMath/Matrix3x3.hpp>
+#include <tf2/LinearMath/Quaternion.hpp>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgcodecs.hpp>
+
+#include <fstream>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include "msg_builders.hpp"
+
+/**
+ * @file
+ * @brief Real frames for the odometry node tests, from test/data.
+ *
+ * Visual odometry needs imagery it can actually track: synthetic noise gives a detector
+ * corners that match nothing between frames, so a generated "motion" tells us only that
+ * the node did not crash. These loaders hand the tests the same stereo pairs and RGB-D
+ * frames that RTAB-Map registers in corelib/test/test_odometry.cpp, which lets a ROS test
+ * assert that a plausible transform came out the other end.
+ *
+ * See test/data/README.md for where the files come from and why they are vendored.
+ */
+
+namespace rtabmap_odom_test {
+
+/// Root of the vendored fixtures, set by CMake to test/data in the source tree.
+inline std::string testDataRoot()
+{
+	return std::string(RTABMAP_ODOM_TEST_DATA_ROOT);
+}
+
+/**
+ * @brief Opens a ROS camera calibration file with OpenCV's parser.
+ *
+ * The files are the ROS flavour: the format line is commented out (`#%YAML:1.0`) so that
+ * a plain YAML parser -- camera_info_manager, rosparam, PyYAML -- accepts them, since
+ * `%YAML:1.0` is not a valid YAML directive and makes those parsers fail. cv::FileStorage
+ * wants the directive, so it is put back here, in memory, and the file on disk stays
+ * loadable by both.
+ *
+ * @return a closed FileStorage if the file is missing
+ */
+inline cv::FileStorage openCalibration(const std::string & path)
+{
+	std::ifstream file(path.c_str());
+	if(!file.is_open())
+	{
+		return cv::FileStorage();
+	}
+	std::ostringstream buffer;
+	buffer << file.rdbuf();
+	std::string text = buffer.str();
+
+	const std::string rosHeader = "#%YAML:1.0";
+	if(text.compare(0, rosHeader.size(), rosHeader) == 0)
+	{
+		text = "%YAML:1.0\n---" + text.substr(rosHeader.size());
+	}
+	return cv::FileStorage(text, cv::FileStorage::READ | cv::FileStorage::MEMORY);
+}
+
+/**
+ * @brief Reads a rows/cols/data matrix from a calibration file node.
+ *
+ * ROS calibration files have no OpenCV `dt` field, so `>> cv::Mat` cannot read them;
+ * the elements are taken one by one instead, as RTAB-Map's CameraModel::load does.
+ *
+ * @return an empty vector if the node is missing or malformed
+ */
+inline std::vector<double> readCalibrationMatrix(
+		cv::FileStorage & fs, const std::string & name, int rows, int cols)
+{
+	const cv::FileNode node = fs[name];
+	if(node.empty())
+	{
+		return std::vector<double>();
+	}
+	std::vector<double> data;
+	node["data"] >> data;
+	if((int)node["rows"] != rows || (int)node["cols"] != cols ||
+	   data.size() != size_t(rows * cols))
+	{
+		return std::vector<double>();
+	}
+	return data;
+}
+
+/**
+ * @brief A CameraInfo from a ROS calibration file.
+ *
+ * The RGB-D files carry only camera_matrix, so R falls back to identity and P to [K|0] --
+ * which is what a driver publishes for an already-rectified monocular camera anyway.
+ */
+inline sensor_msgs::msg::CameraInfo cameraInfoFromCalibration(
+		const std::string & path, const std::string & frameId, double stamp)
+{
+	sensor_msgs::msg::CameraInfo info;
+	info.header.frame_id = frameId;
+	info.header.stamp = stampOf(stamp);
+
+	cv::FileStorage fs = openCalibration(path);
+	if(!fs.isOpened())
+	{
+		return info;
+	}
+	info.width = uint32_t((int)fs["image_width"]);
+	info.height = uint32_t((int)fs["image_height"]);
+	info.distortion_model = fs["distortion_model"].isString() ?
+			(std::string)fs["distortion_model"] : std::string("plumb_bob");
+
+	const std::vector<double> k = readCalibrationMatrix(fs, "camera_matrix", 3, 3);
+	const std::vector<double> r = readCalibrationMatrix(fs, "rectification_matrix", 3, 3);
+	const std::vector<double> p = readCalibrationMatrix(fs, "projection_matrix", 3, 4);
+	const std::vector<double> d = readCalibrationMatrix(fs, "distortion_coefficients", 1, 5);
+
+	info.d.assign(5, 0.0);
+	for(size_t i=0; i<d.size() && i<info.d.size(); ++i)
+	{
+		info.d[i] = d[i];
+	}
+	for(size_t i=0; i<9; ++i)
+	{
+		info.k[i] = k.empty() ? 0.0 : k[i];
+		info.r[i] = r.empty() ? (i%4 == 0 ? 1.0 : 0.0) : r[i];
+	}
+	for(size_t i=0; i<12; ++i)
+	{
+		info.p[i] = p.empty() ? (i%4 == 3 ? 0.0 : info.k[(i/4)*3 + i%4]) : p[i];
+	}
+	return info;
+}
+
+// ---------------------------------------------------------------------------
+// Stereo pairs: data/stereo, from one 640x480 rig at 20 Hz.
+//
+// "rect" holds rectified pairs (frames 50 and 60), what the nodes expect on
+// left/image_rect by default. "raw" holds pairs straight off the camera (frames 420 and
+// 425), for the Rtabmap/ImagesAlreadyRectified path.
+//
+// Each set carries the calibration that describes it, and the two are not
+// interchangeable: the rectified one has no distortion and an identity rectification
+// matrix, because that is what rectification leaves behind, while the raw one carries the
+// lens's real plumb_bob coefficients and the rotation into the rectified frame. Handing
+// the rectified calibration to a raw pair claims a distortion-free lens the images do not
+// have.
+// ---------------------------------------------------------------------------
+
+/// "rect" or "raw"; a rectified pair unless a test says otherwise.
+enum StereoSet { kRectified, kRaw };
+
+inline std::string stereoSetDir(StereoSet set)
+{
+	return testDataRoot() + (set == kRaw ? "/stereo/raw" : "/stereo/rect");
+}
+
+/// The left image of a pair, in color as a stereo driver publishes it (bgr8).
+inline cv::Mat stereoLeftImage(const std::string & name, StereoSet set = kRectified)
+{
+	return cv::imread(stereoSetDir(set) + "/left/" + name + ".jpg", cv::IMREAD_COLOR);
+}
+
+/// The right image of a pair, grayscale (mono8), which is all the matcher uses.
+inline cv::Mat stereoRightImage(const std::string & name, StereoSet set = kRectified)
+{
+	return cv::imread(stereoSetDir(set) + "/right/" + name + ".jpg", cv::IMREAD_GRAYSCALE);
+}
+
+inline sensor_msgs::msg::CameraInfo stereoLeftInfo(
+		const std::string & frameId, double stamp, StereoSet set = kRectified)
+{
+	return cameraInfoFromCalibration(stereoSetDir(set) + "/stereo_left.yaml", frameId, stamp);
+}
+
+/// The right camera's P(0,3) is -fx * baseline, which is where the stereo scale comes from.
+inline sensor_msgs::msg::CameraInfo stereoRightInfo(
+		const std::string & frameId, double stamp, StereoSet set = kRectified)
+{
+	return cameraInfoFromCalibration(stereoSetDir(set) + "/stereo_right.yaml", frameId, stamp);
+}
+
+/**
+ * @brief The right camera's pose in the left camera's frame, from the rig's stereo
+ *        calibration (data/stereo/raw/stereo_pose.yaml).
+ *
+ * Stereo calibration stores the transform the other way round -- a point in left
+ * coordinates maps to the right camera as X_right = R * X_left + T -- so this inverts it
+ * into what TF wants to publish, camera_left -> camera_right. Feeding it to TF is what
+ * lets Rtabmap/ImagesAlreadyRectified:=false rectify the pair against the rig's real
+ * extrinsics, small inter-camera rotation included, rather than an assumed ideal baseline.
+ *
+ * @return an identity transform if the file is missing
+ */
+inline geometry_msgs::msg::Transform stereoRightInLeftFrame()
+{
+	geometry_msgs::msg::Transform transform;
+	transform.rotation.w = 1.0;
+
+	cv::FileStorage fs = openCalibration(stereoSetDir(kRaw) + "/stereo_pose.yaml");
+	if(!fs.isOpened())
+	{
+		return transform;
+	}
+	const std::vector<double> r = readCalibrationMatrix(fs, "rotation_matrix", 3, 3);
+	const std::vector<double> t = readCalibrationMatrix(fs, "translation_matrix", 3, 1);
+	if(r.empty() || t.empty())
+	{
+		return transform;
+	}
+
+	// (R, T) -> (R', -R'T)
+	const tf2::Matrix3x3 rotation(
+			r[0], r[1], r[2],
+			r[3], r[4], r[5],
+			r[6], r[7], r[8]);
+	const tf2::Matrix3x3 inverse = rotation.transpose();
+	const tf2::Vector3 translation = inverse * -tf2::Vector3(t[0], t[1], t[2]);
+
+	tf2::Quaternion q;
+	inverse.getRotation(q);
+	transform.rotation.x = q.x();
+	transform.rotation.y = q.y();
+	transform.rotation.z = q.z();
+	transform.rotation.w = q.w();
+	transform.translation.x = translation.x();
+	transform.translation.y = translation.y();
+	transform.translation.z = translation.z();
+	return transform;
+}
+
+// ---------------------------------------------------------------------------
+// RGB-D frames: data/rgbd, frames "17" and "154".
+// ---------------------------------------------------------------------------
+
+inline cv::Mat rgbdColorImage(const std::string & name)
+{
+	return cv::imread(testDataRoot() + "/rgbd/rgb/" + name + ".jpg", cv::IMREAD_COLOR);
+}
+
+/// 16-bit millimetres, the encoding the RGB-D drivers publish (16UC1).
+inline cv::Mat rgbdDepthImage(const std::string & name)
+{
+	return cv::imread(testDataRoot() + "/rgbd/depth/" + name + ".png", cv::IMREAD_UNCHANGED);
+}
+
+inline sensor_msgs::msg::CameraInfo rgbdInfo(
+		const std::string & name, const std::string & frameId, double stamp)
+{
+	return cameraInfoFromCalibration(
+			testDataRoot() + "/rgbd/calib/" + name + ".yaml", frameId, stamp);
+}
+
+}  // namespace rtabmap_odom_test
+
+#endif /* RTABMAP_ODOM_TEST_DATA_HPP_ */

rtabmap_odom/test/test_rgbd_odometry.cpp

@@ -0,0 +1,800 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#include <gtest/gtest.h>
+
+#include <tf2_ros/static_transform_broadcaster.hpp>
+
+#include <std_srvs/srv/empty.hpp>
+
+#include <rtabmap_msgs/msg/odom_info.hpp>
+#include <rtabmap_msgs/msg/rgbd_images.hpp>
+
+#include <rtabmap_odom/rgbd_odometry.hpp>
+
+#include <cmath>
+
+#include <rtabmap/core/Version.h>
+
+#include "camera_rig.hpp"
+#include "msg_builders.hpp"
+#include "node_test_utils.hpp"
+#include "test_data.hpp"
+
+namespace rtabmap_odom_test {
+
+namespace {
+
+::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
+
+/**
+ * The frames that carry a scene come from test/data/rgbd -- the same two RGB-D frames
+ * RTAB-Map registers in corelib/test/test_odometry.cpp. These tests assert the ROS-level
+ * contract (which topics are subscribed, what is published, how the parameters wire up)
+ * on real input rather than the accuracy of the registration, which is that test's
+ * business.
+ */
+const char * const kFrame = "17";
+const char * const kLaterFrame = "154";   // much further along the sequence
+
+/// The blank scene the "lost tracking" tests need is synthetic: there is nothing to see in it.
+const int kBlankWidth = 160;
+const int kBlankHeight = 120;
+
+double translationNorm(const nav_msgs::msg::Odometry & odom)
+{
+	const geometry_msgs::msg::Point & p = odom.pose.pose.position;
+	return std::sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
+}
+
+/// The angle of the pose's rotation, in radians.
+double rotationAngle(const nav_msgs::msg::Odometry & odom)
+{
+	const geometry_msgs::msg::Quaternion & q = odom.pose.pose.orientation;
+	return 2.0 * std::acos(std::min(1.0, std::fabs(q.w)));
+}
+
+/// rtabmap_odom marks a pose it does not trust with a 9999 covariance rather than staying silent.
+bool isLost(const nav_msgs::msg::Odometry & odom)
+{
+	return odom.pose.covariance[0] >= 9999.0;
+}
+
+class RgbdOdometryTest : public NodeTest
+{
+protected:
+	void publishSensorTf()
+	{
+		staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+		geometry_msgs::msg::TransformStamped tf;
+		tf.header.stamp = helper()->now();
+		tf.header.frame_id = "base_link";
+		tf.child_frame_id = "camera";
+		tf.transform.rotation.w = 1.0;
+		staticTf_->sendTransform(tf);
+	}
+
+	/**
+	 * @brief Waits until the node under test has subscribed to /tf_static.
+	 *
+	 * The fixture sends the sensor transform before the node exists, so the node's TF
+	 * listener only sees it as the retained transient-local message it gets on discovery.
+	 * Publishing an image before that arrives makes the node drop the frame after its
+	 * 100 ms wait_for_transform, for no reason the test can see.
+	 */
+	bool waitForTfListener()
+	{
+		if(!staticTf_)
+		{
+			return true;
+		}
+		return spinUntil([&]() { return helper()->count_subscribers("/tf_static") >= 1; });
+	}
+
+	std::shared_ptr<rtabmap_odom::RGBDOdometry> makeNode(
+			std::vector<rclcpp::Parameter> params = {})
+	{
+		// Defaults first, so a test that passes the same parameter overrides them.
+		//
+		// always_process_most_recent_frame:=false is what the node itself recommends for
+		// data that arrives faster than its stamps: these tests publish a whole sequence
+		// back to back with stamps a tenth of a second apart, and when the executor is
+		// slow enough that two of them land in the same spin -- a loaded CI runner, a
+		// single core -- the node drops the second as a replay glitch and the test waits
+		// for a message that will never come. It also keeps processing on the calling
+		// thread instead of the node's worker, which is what makes these tests observable
+		// at all: the odometry is finished by the time the publish returns.
+		std::vector<rclcpp::Parameter> all = {
+			rclcpp::Parameter("frame_id", "base_link"),
+			rclcpp::Parameter("publish_tf", false),
+			rclcpp::Parameter("always_process_most_recent_frame", false),
+		};
+		all.insert(all.end(), params.begin(), params.end());
+		rclcpp::NodeOptions options;
+		options.parameter_overrides(all);
+		std::shared_ptr<rtabmap_odom::RGBDOdometry> node =
+				addNode(std::make_shared<rtabmap_odom::RGBDOdometry>(options));
+		waitForTfListener();
+		return node;
+	}
+
+	/// Calls one of the node's std_srvs/Empty services and waits for the answer.
+	bool callEmptyService(const std::string & name)
+	{
+		rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client =
+				helper()->create_client<std_srvs::srv::Empty>("/rgbd_odometry/" + name);
+		if(!spinUntil([&]() { return client->service_is_ready(); }))
+		{
+			return false;
+		}
+		std::shared_future<std_srvs::srv::Empty::Response::SharedPtr> future =
+				client->async_send_request(
+						std::make_shared<std_srvs::srv::Empty::Request>()).future.share();
+		return spinUntil([&]() {
+			return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; });
+	}
+
+	/// Where each camera of a rig is mounted, as its driver would publish it once.
+	void publishRigTf(const CameraRig & rig)
+	{
+		staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+		staticTf_->sendTransform(cameraRigTransforms(rig, helper()->now()));
+	}
+
+	/// One frame from test/data/rgbd, as rgbd_sync would deliver it: bgr8 plus 16UC1 millimetres.
+	rtabmap_msgs::msg::RGBDImage makeFrame(const std::string & name, double stamp)
+	{
+		const cv::Mat rgb = rgbdColorImage(name);
+		const cv::Mat depth = rgbdDepthImage(name);
+		EXPECT_FALSE(rgb.empty()) << "test/data/rgbd/rgb/" << name << ".jpg missing";
+		EXPECT_FALSE(depth.empty()) << "test/data/rgbd/depth/" << name << ".png missing";
+
+		rtabmap_msgs::msg::RGBDImage msg;
+		msg.header.frame_id = "camera";
+		msg.header.stamp = stampOf(stamp);
+		msg.rgb = makeImage("camera", stamp, rgb, "bgr8");
+		msg.depth = makeImage("camera", stamp, depth, "16UC1");
+		msg.rgb_camera_info = rgbdInfo(name, "camera", stamp);
+		msg.depth_camera_info = msg.rgb_camera_info;
+		return msg;
+	}
+
+	/// A scene with nothing in it: no features to detect, no motion to recover.
+	rtabmap_msgs::msg::RGBDImage makeBlankFrame(double stamp)
+	{
+		rtabmap_msgs::msg::RGBDImage msg;
+		msg.header.frame_id = "camera";
+		msg.header.stamp = stampOf(stamp);
+		msg.rgb = makeImage("camera", stamp,
+				cv::Mat::zeros(kBlankHeight, kBlankWidth, CV_8UC1), "mono8");
+		msg.depth = makeImage("camera", stamp,
+				cv::Mat(kBlankHeight, kBlankWidth, CV_32FC1, cv::Scalar(2.0f)), "32FC1");
+		msg.rgb_camera_info = makeCameraInfo("camera", stamp, kBlankWidth, kBlankHeight);
+		msg.depth_camera_info = msg.rgb_camera_info;
+		return msg;
+	}
+
+	std::shared_ptr<tf2_ros::StaticTransformBroadcaster> staticTf_;
+};
+
+/// The vendored calibration has to survive the trip through CameraInfo, or nothing below means anything.
+TEST_F(RgbdOdometryTest, the_test_calibration_describes_the_camera)
+{
+	const sensor_msgs::msg::CameraInfo info = rgbdInfo(kFrame, "camera", 1.0);
+
+	ASSERT_EQ(640u, info.width);
+	ASSERT_EQ(480u, info.height);
+	EXPECT_DOUBLE_EQ(525.0, info.k[0]);
+	EXPECT_DOUBLE_EQ(525.0, info.k[4]);
+	// No projection_matrix in the file: P falls back to [K|0], an already-rectified camera.
+	EXPECT_DOUBLE_EQ(info.k[0], info.p[0]);
+	EXPECT_DOUBLE_EQ(0.0, info.p[3]);
+}
+
+/// By default the node takes the three raw camera topics.
+TEST_F(RgbdOdometryTest, subscribes_to_the_raw_camera_topics_by_default)
+{
+	publishSensorTf();
+	makeNode();
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
+			helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
+			helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
+			helper()->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 10);
+
+	EXPECT_TRUE(waitForSubscriber(rgb));
+	EXPECT_TRUE(waitForSubscriber(depth));
+	EXPECT_TRUE(waitForSubscriber(info));
+}
+
+/// A synchronized set of the three raw topics produces one odometry message, at the origin.
+TEST_F(RgbdOdometryTest, publishes_odom_for_a_synchronized_raw_frame)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode();
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
+			helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
+			helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
+			helper()->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 10);
+	ASSERT_TRUE(waitForSubscriber(rgb));
+	ASSERT_TRUE(waitForSubscriber(depth));
+	ASSERT_TRUE(waitForSubscriber(info));
+
+	// Identical stamps, so this works under either synchronization policy.
+	const rtabmap_msgs::msg::RGBDImage frame = makeFrame(kFrame, 1.0);
+	rgb->publish(frame.rgb);
+	depth->publish(frame.depth);
+	info->publish(frame.rgb_camera_info);
+
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	EXPECT_EQ("odom", odom->back().header.frame_id);
+	EXPECT_EQ("base_link", odom->back().child_frame_id);
+	// The first frame has nothing to register against: it defines the origin.
+	EXPECT_NEAR(0.0, translationNorm(odom->back()), 1e-9);
+}
+
+/// subscribe_rgbd swaps the three topics for one pre-synchronized RGBDImage.
+TEST_F(RgbdOdometryTest, subscribe_rgbd_takes_a_single_rgbd_image_topic)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeFrame(kFrame, 1.0));
+	EXPECT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+}
+
+/**
+ * The same frame twice: the registration runs on real features and depth, and the only
+ * answer consistent with the input is "I have not moved". A node that mangles the depth
+ * units or the calibration on the way into RTAB-Map fails here, where the textureless
+ * scenes below cannot tell the difference.
+ */
+TEST_F(RgbdOdometryTest, registers_a_repeated_frame_as_no_motion)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(info->subscription));
+
+	pub->publish(makeFrame(kFrame, 1.0));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	pub->publish(makeFrame(kFrame, 1.1));
+	// Both collectors: odom and odom_info are published separately, and the assertions
+	// below compare the second of each.
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; }));
+
+	const nav_msgs::msg::Odometry & second = odom->back();
+	ASSERT_FALSE(isLost(second)) << "lost tracking on a frame identical to the previous one";
+	EXPECT_GT(info->back().features, 20) << "no features found in a real scene";
+	EXPECT_GT(info->back().inliers, 20)
+			<< "too few inliers (matches=" << info->back().matches << ")";
+	// Exactly zero on this build, in both translation and rotation; a millimetre and a
+	// milliradian leave room for a backend that answers with rounding noise instead.
+	EXPECT_LT(translationNorm(second), 0.001) << "motion reported between identical frames";
+	EXPECT_LT(rotationAngle(second), 0.001) << "rotation reported between identical frames";
+}
+
+/**
+ * Frames 17 and 154 are far apart in the sequence, so losing tracking is a legitimate
+ * outcome; what must hold is that the node's answer agrees with itself -- either a pose
+ * it stands behind, of a plausible size, or one flagged as unusable.
+ */
+TEST_F(RgbdOdometryTest, stays_consistent_between_two_distant_frames)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeFrame(kFrame, 1.0));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	pub->publish(makeFrame(kLaterFrame, 1.1));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; }));
+
+	const nav_msgs::msg::Odometry & second = odom->back();
+	if(!isLost(second))
+	{
+		// 0.41 to 0.46 m over ten runs here. The bound stays a plausibility check rather
+		// than a fit: how far apart these two frames land is the registration's business,
+		// and this test's claim is only that the answer is not nonsense.
+		EXPECT_LT(translationNorm(second), 2.0)
+				<< "implausible jump of " << translationNorm(second) << " m";
+	}
+}
+
+/**
+ * @brief Two to six cameras, each on its own numbered topic.
+ *
+ * Above six the node has no synchronizer for it and says to use rgbd_cameras:=0 with the
+ * rgbd_images topic instead, so six is where this stops. Only the subscriptions are
+ * checked: one numbered topic per camera, none left behind.
+ */
+class RgbdOdometryCamerasTest :
+		public RgbdOdometryTest,
+		public ::testing::WithParamInterface<int>
+{
+};
+
+TEST_P(RgbdOdometryCamerasTest, subscribes_to_one_numbered_topic_per_camera)
+{
+	const int cameras = GetParam();
+	publishSensorTf();
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", cameras)});
+
+	// All of them first, so they are discovered together rather than one wait after another.
+	std::vector<rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr> publishers;
+	for(int i=0; i<cameras; ++i)
+	{
+		publishers.push_back(helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
+				"rgbd_image" + std::to_string(i), 10));
+	}
+
+	for(int i=0; i<cameras; ++i)
+	{
+		EXPECT_TRUE(waitForSubscriber(publishers[i]))
+				<< "rgbd_cameras:=" << cameras << " left rgbd_image" << i << " unsubscribed";
+	}
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		RgbdCameras,
+		RgbdOdometryCamerasTest,
+		::testing::Range(2, 7),
+		[](const ::testing::TestParamInfo<int> & info) {
+			return std::to_string(info.param) + "_cameras";
+		});
+
+/// rgbd_cameras:=0 takes any number of cameras in one RGBDImages message.
+TEST_F(RgbdOdometryTest, rgbd_cameras_zero_takes_an_rgbd_images_topic)
+{
+	publishSensorTf();
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", 0)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
+
+	EXPECT_TRUE(waitForSubscriber(pub));
+}
+
+/// This node matches by nearest stamp unless told otherwise; stereo_odometry does not.
+TEST_F(RgbdOdometryTest, approx_sync_is_on_by_default)
+{
+	publishSensorTf();
+	std::shared_ptr<rtabmap_odom::RGBDOdometry> node = makeNode();
+
+	EXPECT_TRUE(node->get_parameter("approx_sync").as_bool());
+}
+
+/**
+ * A textureless scene is the documented failure: there is nothing to match, so the frame
+ * is lost and the node says so with a null pose rather than publishing nothing.
+ * See "When it loses track" in doc/rgbd_odometry.md.
+ */
+TEST_F(RgbdOdometryTest, reports_lost_with_a_null_pose_on_a_textureless_scene)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(info->subscription));
+
+	pub->publish(makeBlankFrame(1.0));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	pub->publish(makeBlankFrame(1.1));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; }));
+
+	// Nothing to register against: no features, and the pose carries the "do not use me"
+	// covariance rather than the node going silent.
+	EXPECT_EQ(0, info->back().features);
+	EXPECT_TRUE(isLost(odom->back()));
+}
+
+/// publish_null_when_lost:=false makes the node go silent instead.
+TEST_F(RgbdOdometryTest, publishes_nothing_when_lost_if_null_publishing_is_off)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("publish_null_when_lost", false)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeBlankFrame(1.0));
+	pub->publish(makeBlankFrame(1.1));
+	spinFor(std::chrono::milliseconds(1500));
+
+	EXPECT_TRUE(odom->empty());
+}
+
+
+/**
+ * keep_color decides what reaches RTAB-Map from a color image, and therefore what the
+ * node republishes: the matcher works in grayscale, so the color is dropped unless asked
+ * for. Same contract as stereo_odometry, checked here because the doc states it of this
+ * node too.
+ */
+TEST_F(RgbdOdometryTest, keeps_the_image_in_color_when_asked)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> frames =
+			collect<rtabmap_msgs::msg::RGBDImage>("odom_rgbd_image");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("keep_color", true)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(frames->subscription));
+
+	pub->publish(makeFrame(kFrame, 1.0));
+	ASSERT_TRUE(spinUntil([&]() { return !frames->empty(); }));
+	EXPECT_EQ("bgr8", frames->back().rgb.encoding);
+}
+
+/// Off by default: what reaches RTAB-Map, and comes back out, is grayscale.
+TEST_F(RgbdOdometryTest, converts_the_image_to_grayscale_by_default)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> frames =
+			collect<rtabmap_msgs::msg::RGBDImage>("odom_rgbd_image");
+	std::shared_ptr<rtabmap_odom::RGBDOdometry> node =
+			makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
+	EXPECT_FALSE(node->get_parameter("keep_color").as_bool());
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(frames->subscription));
+
+	pub->publish(makeFrame(kFrame, 1.0));
+	ASSERT_TRUE(spinUntil([&]() { return !frames->empty(); }));
+	EXPECT_EQ("mono8", frames->back().rgb.encoding);
+}
+
+/**
+ * The two feature topics the lidar node cannot fill: both are built from the frame's
+ * visual words, so they carry content only on the visual paths. `odom_local_map` is the
+ * map the frame was registered against, `odom_last_frame` the frame's own features, both
+ * in the odom frame.
+ */
+TEST_F(RgbdOdometryTest, publishes_the_feature_map_and_the_frame_that_registered_against_it)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> localMap =
+			collect<sensor_msgs::msg::PointCloud2>("odom_local_map");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> lastFrame =
+			collect<sensor_msgs::msg::PointCloud2>("odom_last_frame");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(localMap->subscription));
+	ASSERT_TRUE(waitForPublisher(lastFrame->subscription));
+
+	pub->publish(makeFrame(kFrame, 1.0));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	pub->publish(makeFrame(kFrame, 1.1));
+	// All three collectors: the clouds are published after the odometry of the same frame,
+	// so waiting for odom alone leaves them one spin behind.
+	ASSERT_TRUE(spinUntil([&]() {
+				return odom->size() >= 2 && !localMap->empty() && !lastFrame->empty(); }));
+
+	// 534 features on this frame, in both, expressed in the odometry frame.
+	ASSERT_FALSE(localMap->empty()) << "no feature map was published";
+	ASSERT_FALSE(lastFrame->empty()) << "no frame features were published";
+	EXPECT_GT(localMap->back().width, 0u);
+	EXPECT_GT(lastFrame->back().width, 0u);
+	EXPECT_EQ("odom", lastFrame->back().header.frame_id)
+			<< "these are published in the odometry frame, not the sensor's";
+}
+
+
+/**
+ * @brief A four-camera rig, driven a metre through a world of points.
+ *
+ * The frames carry their features -- keypoints, 3D points, descriptors -- and no image at
+ * all, as a driver that does its own extraction publishes them. So the trajectory below
+ * can only come from the features: the control test that follows runs the same frames
+ * with them stripped off, and it finds nothing.
+ *
+ * Both estimation types the multi-camera case supports are run. Vis/EstimationType=0
+ * aligns the two sets of 3D points, which needs nothing extra; =1 solves a PnP across
+ * all four cameras at once, which RTAB-Map hands to OpenGV and cannot do without it.
+ */
+class RgbdOdometryRigTest :
+		public RgbdOdometryTest,
+		public ::testing::WithParamInterface<int>
+{
+};
+
+TEST_P(RgbdOdometryRigTest, recovers_the_trajectory_of_a_rig_from_the_features_it_is_given)
+{
+	const int estimationType = GetParam();
+#ifndef RTABMAP_OPENGV
+	if(estimationType == 1)
+	{
+		GTEST_SKIP() << "a multi-camera PnP is solved by OpenGV, which RTAB-Map was built without";
+	}
+#endif
+
+	const CameraRig rig = makeCameraRig();
+	publishRigTf(rig);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", 0),
+	          rclcpp::Parameter("Vis/EstimationType", std::to_string(estimationType))});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(info->subscription));
+
+	// A metre forward, ten centimetres at a time.
+	const int frames = 11;
+	rtabmap_msgs::msg::RGBDImages lastFrame;
+	for(int i=0; i<frames; ++i)
+	{
+		lastFrame = cameraRigFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i);
+		ASSERT_EQ(rig.cameras(), lastFrame.rgbd_images.size());
+		pub->publish(lastFrame);
+		// Both collectors: odom and odom_info are published separately, and the feature
+		// count asserted below is read from the odom_info of this same frame.
+		ASSERT_TRUE(spinUntil([&]() {
+					return odom->size() >= size_t(i+1) && info->size() >= size_t(i+1); }))
+				<< "nothing came back for frame " << i;
+	}
+
+	const nav_msgs::msg::Odometry & last = odom->back();
+	ASSERT_FALSE(isLost(last)) << "lost tracking on a rig that sees the whole scene";
+	EXPECT_NEAR(1.0, last.pose.pose.position.x, 0.05)
+			<< "the rig travelled a metre along x";
+	EXPECT_NEAR(0.0, last.pose.pose.position.y, 0.05);
+	EXPECT_NEAR(0.0, last.pose.pose.position.z, 0.05);
+	EXPECT_NEAR(0.0, rotationAngle(last), 0.05) << "the rig never turned";
+
+	// The frame's own features, reassembled from the four cameras and used as they are.
+	// A couple can go missing on the way: RTAB-Map drops a feature whose descriptor lands
+	// on the same visual word as another one of the same frame, both being ambiguous then
+	// (the `count(*iter) == 1` guards in RegistrationVis). What matters here is that the
+	// number is the frame's own and not zero, which is all a blank image could give.
+	const int sent = int(cameraRigFeatureCount(lastFrame));
+	EXPECT_LE(info->back().features, sent);
+	EXPECT_GT(info->back().features, sent - 10)
+			<< "the node did not use the features the frame came with";
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		EstimationTypes,
+		RgbdOdometryRigTest,
+		::testing::Values(0, 1),
+		[](const ::testing::TestParamInfo<int> & info) {
+			return info.param == 0 ? std::string("3d_to_3d") : std::string("pnp_across_cameras");
+		});
+
+/**
+ * The control for the test above: the same frames with the features stripped off. What is
+ * left is four calibrations and nothing to see, which the node is right to process -- an
+ * empty scene is still a frame -- and right to report lost.
+ */
+TEST_F(RgbdOdometryRigTest, the_same_frames_without_their_features_have_nothing_to_track)
+{
+	const CameraRig rig = makeCameraRig();
+	publishRigTf(rig);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", 0)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(info->subscription));
+
+	for(int i=0; i<2; ++i)
+	{
+		rtabmap_msgs::msg::RGBDImages frame =
+				cameraRigFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i);
+		for(size_t c=0; c<frame.rgbd_images.size(); ++c)
+		{
+			frame.rgbd_images[c].key_points.clear();
+			frame.rgbd_images[c].points.clear();
+			frame.rgbd_images[c].descriptors.clear();
+		}
+		pub->publish(frame);
+		ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }));
+	}
+
+	ASSERT_TRUE(spinUntil([&]() { return info->size() >= 2; }));
+	EXPECT_EQ(0, info->back().features) << "features appeared from a frame that has none";
+	EXPECT_TRUE(isLost(odom->back()));
+}
+
+/**
+ * Odom/ImageDecimation shrinks the image before registering it, and scales the
+ * calibration to match. Features that arrived with the frame are placed in the full size
+ * image, so they have to be brought down with it: read against a calibration half their
+ * scale, a rig's keypoints land in the wrong camera altogether.
+ *
+ * The frames here carry a blank image for the decimation to have something to work on,
+ * and the trajectory has to come out the same as without it.
+ */
+TEST_F(RgbdOdometryRigTest, decimation_brings_the_given_features_down_with_the_image)
+{
+#ifndef RTABMAP_OPENGV
+	// Only a PnP reads the keypoints this test is about; a 3D-to-3D registration would
+	// come out right even with every one of them misplaced.
+	GTEST_SKIP() << "a multi-camera PnP is solved by OpenGV, which RTAB-Map was built without";
+#endif
+
+	const CameraRig rig = makeCameraRig();
+	publishRigTf(rig);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", 0),
+	          rclcpp::Parameter("Odom/ImageDecimation", "2")});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	const int frames = 11;
+	for(int i=0; i<frames; ++i)
+	{
+		pub->publish(cameraRigFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0),
+				1.0 + 0.1*i, /*withImages=*/true));
+		ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }))
+				<< "nothing came back for frame " << i;
+	}
+
+	const nav_msgs::msg::Odometry & last = odom->back();
+	ASSERT_FALSE(isLost(last)) << "the features were lost on the way into the decimated frame";
+	EXPECT_NEAR(1.0, last.pose.pose.position.x, 0.05);
+	EXPECT_NEAR(0.0, last.pose.pose.position.y, 0.05);
+	EXPECT_NEAR(0.0, last.pose.pose.position.z, 0.05);
+}
+
+/**
+ * @brief The same rig on the numbered topics, one to six cameras.
+ *
+ * `rgbd_cameras:=N` subscribes to N topics and synchronizes them with a callback of its
+ * own per N, six of them in all. The test above drives the `rgbd_cameras:=0` one; these
+ * drive the rest, by publishing each camera of the rig on its own topic and asking for
+ * the same metre back.
+ *
+ * Each of them runs both estimation types, except that a multi-camera PnP needs OpenGV
+ * and is skipped when RTAB-Map was built without it. A single camera does not, so that
+ * one is run either way.
+ */
+class RgbdOdometryRigCamerasTest :
+		public RgbdOdometryTest,
+		public ::testing::WithParamInterface<std::tuple<int, bool, int>>
+{
+};
+
+TEST_P(RgbdOdometryRigCamerasTest, recovers_the_trajectory_from_the_numbered_topics)
+{
+	const int cameras = std::get<0>(GetParam());
+	const bool approxSync = std::get<1>(GetParam());
+	const int estimationType = std::get<2>(GetParam());
+#ifndef RTABMAP_OPENGV
+	if(estimationType == 1 && cameras > 1)
+	{
+		GTEST_SKIP() << "a multi-camera PnP is solved by OpenGV, which RTAB-Map was built without";
+	}
+#endif
+
+	const CameraRig rig = makeCameraRig(cameras);
+	publishRigTf(rig);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", cameras),
+	          rclcpp::Parameter("approx_sync", approxSync),
+	          rclcpp::Parameter("Vis/EstimationType", std::to_string(estimationType))});
+
+	// One camera listens on rgbd_image, more than one on rgbd_image0..N-1.
+	std::vector<rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr> publishers;
+	for(int i=0; i<cameras; ++i)
+	{
+		publishers.push_back(helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
+				cameras == 1 ? "rgbd_image" : "rgbd_image" + std::to_string(i), 10));
+	}
+	for(int i=0; i<cameras; ++i)
+	{
+		ASSERT_TRUE(waitForSubscriber(publishers[i])) << "camera " << i << " has no subscriber";
+	}
+
+	const int frames = 11;
+	for(int i=0; i<frames; ++i)
+	{
+		const rtabmap_msgs::msg::RGBDImages frame =
+				cameraRigFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i);
+		ASSERT_EQ(size_t(cameras), frame.rgbd_images.size());
+		for(int c=0; c<cameras; ++c)
+		{
+			publishers[c]->publish(frame.rgbd_images[c]);
+		}
+		ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }))
+				<< "nothing came back for frame " << i;
+	}
+
+	const nav_msgs::msg::Odometry & last = odom->back();
+	ASSERT_FALSE(isLost(last)) << "lost tracking with " << cameras << " camera(s)";
+	EXPECT_NEAR(1.0, last.pose.pose.position.x, 0.05) << "the rig travelled a metre along x";
+	EXPECT_NEAR(0.0, last.pose.pose.position.y, 0.05);
+	EXPECT_NEAR(0.0, last.pose.pose.position.z, 0.05);
+
+	// A reset tears the synchronizer down and builds it again -- one per camera count,
+	// and a different one for each of the two sync policies. Frames have to keep arriving
+	// through the new one.
+	ASSERT_TRUE(callEmptyService("reset_odom"));
+	const size_t beforeReset = odom->size();
+	const rtabmap_msgs::msg::RGBDImages frame =
+			cameraRigFrame(rig, rtabmap::Transform(1.1f, 0, 0, 0, 0, 0), 2.1);
+	for(int c=0; c<cameras; ++c)
+	{
+		publishers[c]->publish(frame.rgbd_images[c]);
+	}
+	EXPECT_TRUE(spinUntil([&]() { return odom->size() > beforeReset; }))
+			<< "nothing came back after the reset rebuilt the synchronizer";
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		RgbdCameras,
+		RgbdOdometryRigCamerasTest,
+		::testing::Combine(::testing::Range(1, 7), ::testing::Bool(), ::testing::Values(0, 1)),
+		[](const ::testing::TestParamInfo<std::tuple<int, bool, int>> & info) {
+			const int cameras = std::get<0>(info.param);
+			return std::to_string(cameras) + (cameras == 1 ? "_camera_" : "_cameras_") +
+					(std::get<1>(info.param) ? "approx_sync_" : "exact_sync_") +
+					(std::get<2>(info.param) == 0 ? "3d_to_3d" : "pnp");
+		});
+
+}  // namespace
+}  // namespace rtabmap_odom_test

rtabmap_python/README.md

@@ -0,0 +1,45 @@
+# rtabmap_python
+
+Python helpers for reading and writing the binary formats [RTAB-Map](https://github.com/introlab/rtabmap) uses.
+
+RTAB-Map is a C++ library, and the data it hands to ROS is not always plain ROS types. Several `rtabmap_msgs` fields — and every blob in an `.db` database — carry a matrix in RTAB-Map's own compressed encoding rather than as a `sensor_msgs/Image` or an array. This package is the Python side of that encoding, for scripts that read those fields without going through the C++ library.
+
+There are no nodes here. It is an `ament_python` package that installs one importable module.
+
+## Contents
+
+- [Module](#module)
+- [Things worth knowing](#things-worth-knowing)
+- [License](#license)
+
+## Module
+
+`rtabmap_python.cv_compression` — a single-channel `cv::Mat` to and from bytes.
+
+| Function | Description |
+|---|---|
+| `compress(data)` | 1-D or 2-D numpy array → `bytearray`. |
+| `uncompress(data)` | those bytes → 2-D numpy array. |
+
+```python
+import numpy as np
+from rtabmap_python.cv_compression import compress, uncompress
+
+scan = np.zeros((360, 2), dtype=np.float32)
+blob = compress(scan)          # what the message field carries
+restored = uncompress(blob)    # (360, 2) float32
+```
+
+The encoding is a zlib stream followed by a 12-byte trailer holding rows, cols and the element type as three `int32`. It matches `compressData()` and `uncompressData()` in RTAB-Map's `corelib/src/Compression.cpp` byte for byte, so either side can read what the other wrote. The [module docstring](rtabmap_python/cv_compression.py) has the exact layout, and the generated [Python API reference](https://docs.ros.org/en/jazzy/p/rtabmap_python/) renders it alongside the two functions.
+
+## Things worth knowing
+
+**The result is read-only.** `uncompress` views the decompressed buffer instead of copying it, so the array it returns has `writeable=False` and assigning into it raises. Call `.copy()` if you need to modify it.
+
+**Single-channel only.** The C++ encoder packs the channel count into the type code; the tables here cover the single-channel depths `CV_8U` through `CV_64F`. A multi-channel matrix written by the C++ side raises `KeyError` rather than decoding wrongly. So does an unsupported dtype on the way in — `int64` and `float16` have no encoding.
+
+**The trailer is host-endian**, because the C++ side writes raw `int`s. A blob is not portable between machines of opposite endianness.
+
+## License
+
+BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license).

rtabmap_python/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_python</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's python package.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -10,6 +10,8 @@
   <url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
   <url type="repository">https://github.com/introlab/rtabmap_ros</url>
 
+  <depend>python3-numpy</depend>
+
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>
@@ -17,5 +19,6 @@
 
   <export>
     <build_type>ament_python</build_type>
+    <rosdoc2>rosdoc2.yaml</rosdoc2>
   </export>
 </package>

rtabmap_python/rosdoc2.yaml

@@ -0,0 +1,29 @@
+## Configuration for rosdoc2, the documentation generator used by docs.ros.org.
+## Regenerate the annotated default with:
+##   rosdoc2 default_config --package-path rtabmap_python
+## Build the docs locally with:
+##   rosdoc2 build --package-path rtabmap_python --output-directory doc_output
+
+## This 'attic section' self-documents this file's type and version.
+type: 'rosdoc2 config'
+version: 1
+
+---
+
+settings:
+    ## Generate the standard index page from package.xml (description, maintainer,
+    ## license, links) and a table of contents for the builders below.
+    generate_package_index: true
+
+    ## This is an ament_python package: there are no C/C++ headers to parse, and the
+    ## API reference comes from the module docstrings via sphinx-apidoc.
+    always_run_doxygen: false
+    always_run_sphinx_apidoc: true
+
+builders:
+    ## Sphinx renders the landing page and the autodoc pages sphinx-apidoc produces
+    ## from rtabmap_python/.
+    - sphinx: {
+        name: 'rtabmap_python',
+        output_dir: ''
+    }

rtabmap_python/rtabmap_python/cv_compression.py

@@ -27,6 +27,35 @@
 # POSSIBILITY OF SUCH DAMAGE.
 
 
+"""
+Compress numpy arrays into RTAB-Map's ``cv::Mat`` wire format.
+
+RTAB-Map stores and transmits matrices -- images, laser scans, descriptors -- as a zlib
+payload followed by a 12-byte trailer recording the shape and the element type. Database
+blobs and the compressed fields of ``rtabmap_msgs`` messages both use it.
+
+The layout is::
+
+    [ zlib stream of the elements in C order ][ rows ][ cols ][ type ]
+                                               int32   int32   int32
+
+The three trailer fields are written with ``struct`` format ``'iii'`` -- native byte order
+and size, matching the C++ side's raw ``int`` writes. That makes the encoding
+**host-endian**, so a blob does not travel between machines of opposite endianness.
+
+``type`` is the OpenCV depth of the elements: 0 ``CV_8U``, 1 ``CV_8S``, 2 ``CV_16U``,
+3 ``CV_16S``, 4 ``CV_32S``, 5 ``CV_32F``, 6 ``CV_64F``.
+
+These two functions are the Python side of that format. They match ``compressData()`` and
+``uncompressData()`` in RTAB-Map's ``corelib/src/Compression.cpp`` byte for byte, so a
+matrix written by either side can be read by the other.
+
+Single-channel matrices only. The C++ encoder packs the channel count into the type code
+alongside the depth; the tables here cover the single-channel depths ``CV_8U`` through
+``CV_64F``, which is what the codes 0 to 6 mean.
+"""
+
+
 import struct
 import zlib
 
@@ -34,6 +63,20 @@ import numpy as np
 
 
 def compress(data):
+    """
+    Compress a 1-D or 2-D array into RTAB-Map's format.
+
+    :param data: a single-channel array whose dtype is one of ``uint8``, ``int8``,
+        ``uint16``, ``int16``, ``int32``, ``float32`` or ``float64``. A 1-D array of
+        length ``n`` is recorded as a 1-by-``n`` matrix, which is the shape
+        :func:`uncompress` gives back. Any memory layout is accepted; the bytes are
+        always written in C order.
+    :returns: a ``bytearray`` holding the zlib payload followed by the trailer described
+        in the module docstring.
+    :raises AssertionError: if ``data`` has more than two dimensions.
+    :raises KeyError: if its dtype is not one of the seven above -- ``int64`` and
+        ``float16`` have no encoding in this format.
+    """
     assert data.ndim == 1 or data.ndim == 2
 
     dim1 = 1
@@ -63,6 +106,16 @@ def compress(data):
 
 
 def uncompress(data):
+    """
+    Restore an array written by :func:`compress` or by RTAB-Map's C++ side.
+
+    :param data: a bytes-like object laid out as :func:`compress` returns.
+    :returns: a 2-D array of the recorded shape and dtype. It is 2-D even when
+        :func:`compress` was handed a 1-D array, and it is **read-only**: it views the
+        decompressed buffer instead of copying it, so call ``.copy()`` before writing.
+    :raises KeyError: if the trailer's type code is not a single-channel depth 0 to 6,
+        which is what a multi-channel matrix from the C++ side encodes to.
+    """
     cvtype_to_numpy_type = {
         0: 'uint8',
         1: 'int8',

rtabmap_python/test/test_cv_compression.py

@@ -0,0 +1,178 @@
+# Copyright 2025 matlabbe
+#
+# 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 matlabbe 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.
+
+
+"""Tests for :mod:`rtabmap_python.cv_compression`."""
+
+import struct
+import zlib
+
+import numpy as np
+import pytest
+
+from rtabmap_python.cv_compression import compress, uncompress
+
+
+# The single-channel depths the format encodes, with the codes RTAB-Map's
+# serializeMatType() gives them: CV_8U, CV_8S, CV_16U, CV_16S, CV_32S, CV_32F, CV_64F.
+SUPPORTED_TYPES = [
+    ('uint8', 0),
+    ('int8', 1),
+    ('uint16', 2),
+    ('int16', 3),
+    ('int32', 4),
+    ('float32', 5),
+    ('float64', 6),
+]
+
+# Three int32: rows, cols, type code.
+TRAILER_SIZE = 3 * 4
+
+
+@pytest.mark.parametrize('dtype,code', SUPPORTED_TYPES)
+def test_roundtrip_preserves_shape_dtype_and_values(dtype, code):
+    """Every supported depth survives a compress/uncompress cycle unchanged."""
+    data = np.arange(12, dtype=dtype).reshape(3, 4)
+
+    result = uncompress(compress(data))
+
+    assert result.shape == (3, 4)
+    assert result.dtype == np.dtype(dtype)
+    assert np.array_equal(result, data)
+
+
+@pytest.mark.parametrize('dtype,code', SUPPORTED_TYPES)
+def test_trailer_records_rows_cols_and_type_code(dtype, code):
+    """The last 12 bytes are rows, cols and the type code, as the C++ side writes them."""
+    data = np.zeros((3, 4), dtype=dtype)
+
+    trailer = bytes(compress(data)[-TRAILER_SIZE:])
+
+    assert trailer == struct.pack('iii', 3, 4, code)
+
+
+def test_payload_is_plain_zlib_of_the_c_order_bytes():
+    """Everything before the trailer is a zlib stream, so the C++ side can inflate it."""
+    data = np.arange(6, dtype=np.uint8)
+
+    payload = bytes(compress(data)[:-TRAILER_SIZE])
+
+    assert zlib.decompress(payload) == data.tobytes()
+
+
+def test_compress_returns_a_bytearray():
+    """The return type is a bytearray, which is what the message fields expect."""
+    assert isinstance(compress(np.zeros(4, dtype=np.uint8)), bytearray)
+
+
+def test_one_dimensional_input_comes_back_as_a_single_row():
+    """A 1-D array is recorded as 1-by-n, so the roundtrip is not shape-preserving."""
+    data = np.arange(5, dtype=np.float32)
+
+    result = uncompress(compress(data))
+
+    assert result.shape == (1, 5)
+    assert np.array_equal(result.ravel(), data)
+
+
+@pytest.mark.parametrize('shape', [(1, 5), (5, 1), (2, 3)])
+def test_two_dimensional_shapes_are_preserved_exactly(shape):
+    """Rows and cols are recorded separately, so no 2-D shape is transposed or flattened."""
+    data = np.arange(5 if 1 in shape else 6, dtype=np.uint8).reshape(shape)
+
+    assert uncompress(compress(data)).shape == shape
+
+
+def test_non_contiguous_input_roundtrips():
+    """A transposed view is written in C order, so it reads back as the same matrix."""
+    data = np.arange(12, dtype=np.int16).reshape(3, 4).T
+    assert not data.flags.c_contiguous
+
+    result = uncompress(compress(data))
+
+    assert result.shape == (4, 3)
+    assert np.array_equal(result, data)
+
+
+def test_empty_array_roundtrips_as_an_empty_row():
+    """An empty array is not a special case; it comes back as a 1-by-0 matrix."""
+    result = uncompress(compress(np.array([], dtype=np.uint8)))
+
+    assert result.shape == (1, 0)
+    assert result.dtype == np.uint8
+
+
+def test_uncompressed_array_is_read_only():
+    """uncompress() views the decompressed buffer rather than copying it."""
+    result = uncompress(compress(np.arange(4, dtype=np.uint8)))
+
+    assert not result.flags.writeable
+    with pytest.raises(ValueError):
+        result[0, 0] = 1
+    # .copy() is the way out, as the docstring says.
+    assert result.copy().flags.writeable
+
+
+def test_accepts_bytes_as_well_as_bytearray():
+    """uncompress() reads whatever compress() produced, converted or not."""
+    data = np.arange(8, dtype=np.uint16).reshape(2, 4)
+
+    result = uncompress(bytes(compress(data)))
+
+    assert np.array_equal(result, data)
+
+
+def test_larger_matrix_roundtrips():
+    """A matrix big enough to actually exercise zlib, with non-trivial content."""
+    rng = np.random.default_rng(42)
+    data = rng.integers(0, 255, size=(120, 160), dtype=np.uint8)
+
+    assert np.array_equal(uncompress(compress(data)), data)
+
+
+def test_rejects_more_than_two_dimensions():
+    """The format has no encoding for a third dimension, so compress() refuses one."""
+    with pytest.raises(AssertionError):
+        compress(np.zeros((2, 2, 2), dtype=np.uint8))
+
+
+@pytest.mark.parametrize('dtype', ['int64', 'uint64', 'float16'])
+def test_rejects_unsupported_dtype(dtype):
+    """Depths outside CV_8U..CV_64F have no type code and raise rather than truncate."""
+    with pytest.raises(KeyError):
+        compress(np.zeros(4, dtype=dtype))
+
+
+def test_rejects_unknown_type_code():
+    """A trailer from a multi-channel C++ matrix encodes a code this table lacks."""
+    payload = bytes(compress(np.zeros((2, 2), dtype=np.uint8))[:-TRAILER_SIZE])
+    # What serializeMatType() returns for a 3-channel CV_8U matrix: depth + ((cn - 1) << 3).
+    forged = bytearray(payload) + struct.pack('iii', 2, 2, 0 + ((3 - 1) << 3))
+
+    with pytest.raises(KeyError):
+        uncompress(forged)

rtabmap_ros/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_ros</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>
     RTAB-Map Stack
   </description>

rtabmap_rviz_plugins/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_rviz_plugins</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's rviz plugins.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

rtabmap_rviz_plugins/src/MapCloudDisplay.cpp

@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <rtabmap_conversions/PointCloudConversion.h>
 #include "rtabmap_rviz_plugins/MapCloudDisplay.h"
 
 #include <QApplication>
@@ -325,7 +326,7 @@ void MapCloudDisplay::processMapData(const rtabmap_msgs::msg::MapData& map)
 
 					if(!cloud->empty())
 					{
-						pcl::toROSMsg(*cloud, *cloudMsg);
+						rtabmap_conversions::toPointCloud2Msg(*cloud, *cloudMsg);
 					}
 				}
 			}
@@ -352,7 +353,7 @@ void MapCloudDisplay::processMapData(const rtabmap_msgs::msg::MapData& map)
 
 				if(!cloud->empty())
 				{
-					pcl::toROSMsg(*cloud, *cloudMsg);
+					rtabmap_conversions::toPointCloud2Msg(*cloud, *cloudMsg);
 				}
 			}
 

rtabmap_rviz_plugins/src/MapGraphDisplay.cpp

@@ -26,6 +26,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "rtabmap_rviz_plugins/MapGraphDisplay.h"
+
+// rviz's headers forward declare these, so the ones actually used here are included
+// directly rather than counting on what rviz happens to pull in.
+#include <OgreColourValue.h>
+#include <OgreManualObject.h>
+#include <OgreMatrix4.h>
+#include <OgreQuaternion.h>
+#include <OgreRenderOperation.h>
+#include <OgreSceneManager.h>
+#include <OgreSceneNode.h>
+#include <OgreVector.h>
+
 #include <rviz_common/display_context.hpp>
 #include "rviz_common/properties/color_property.hpp"
 #include "rviz_common/properties/float_property.hpp"

rtabmap_slam/CMakeLists.txt

@@ -240,4 +240,43 @@ install(DIRECTORY include/
    FILES_MATCHING PATTERN "*.h"
 )
 
+#############
+## Testing ##
+#############
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(rtabmap_conversions REQUIRED)
+
+  # Each test binary drives its own rtabmap node: a crash or a stuck executor in one
+  # cannot take the others down, and each starts from a clean DDS graph.
+  #
+  # Each binary also gets its own DDS domain. colcon tests packages in parallel and ctest
+  # can run these binaries in parallel, while these suites share topic names -- odom,
+  # scan, info -- with the other packages' suites. On a shared domain they discover each
+  # other's publishers and assertions then see traffic the test never sent. rtabmap_util
+  # numbers from 30, rtabmap_sync from 50 and rtabmap_odom from 70; keep the ranges apart.
+  set(rtabmap_slam_test_domain_id 90)
+  macro(rtabmap_slam_add_node_test test_name)
+    ament_add_gtest(${test_name} test/${test_name}.cpp
+      ENV ROS_DOMAIN_ID=${rtabmap_slam_test_domain_id}
+      TIMEOUT 300)
+    math(EXPR rtabmap_slam_test_domain_id "${rtabmap_slam_test_domain_id} + 1")
+    if(TARGET ${test_name})
+      target_include_directories(${test_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test)
+      target_link_libraries(${test_name} rtabmap_slam_plugins)
+      if("$ENV{ROS_DISTRO}" STRLESS "lyrical")
+        ament_target_dependencies(${test_name} ${AmentLibraries} rtabmap_conversions)
+      else()
+        target_link_libraries(${test_name} ${Libraries} rtabmap_conversions::rtabmap_conversions)
+      endif()
+    endif()
+  endmacro()
+
+  rtabmap_slam_add_node_test(test_core_wrapper_parameters)
+  rtabmap_slam_add_node_test(test_core_wrapper_mapping)
+  rtabmap_slam_add_node_test(test_core_wrapper_services)
+  rtabmap_slam_add_node_test(test_core_wrapper_planning)
+  rtabmap_slam_add_node_test(test_core_wrapper_inputs)
+endif()
+
 ament_package()

rtabmap_slam/README.md

@@ -0,0 +1,111 @@
+# rtabmap_slam
+
+The SLAM node of [RTAB-Map](https://github.com/introlab/rtabmap): it takes a pose from odometry and data from the sensors, builds a graph of where the robot has been, and corrects that graph whenever it recognizes a place it has seen before — in the current run or in a previous one, so a map can be extended over [several sessions](#the-database).
+
+## Contents
+
+- [Nodes](#nodes)
+- [Conventions](#conventions)
+  - [Frames and TF](#frames-and-tf)
+  - [The database](#the-database)
+  - [Update rate and dropped updates](#update-rate-and-dropped-updates)
+  - [Odometry, covariance and new maps](#odometry-covariance-and-new-maps)
+  - [Mapping and localization](#mapping-and-localization)
+- [License](#license)
+
+## Nodes
+
+| Node | Description |
+|---|---|
+| [rtabmap](doc/rtabmap.md) | Graph SLAM with appearance- and proximity-based loop closure detection, memory management, map assembly and planning on the graph. |
+
+```mermaid
+flowchart LR
+    SYNC["<div style='text-align:left'><b>synchronized</b><br>RGB-D camera(s)<br>Stereo camera(s)<br>2D LiDAR<br>3D LiDAR<br>Odometry</div>"]
+    ASYNC["<div style='text-align:left'><b>asynchronous</b><br>IMU<br>GPS<br>Landmarks (markers, tags, fiducials)</div>"]
+    RTAB(["<b>rtabmap</b>"])
+    GRAPH["Graph"]
+    INFO["Info"]
+    MAPS["<div style='text-align:left'><b>maps</b><br>2D occupancy grid<br>OctoMap<br>Elevation map<br>3D point cloud</div>"]
+    TF["TF map → odom"]
+    SYNC --> RTAB
+    ASYNC --> RTAB
+    RTAB --> GRAPH
+    RTAB --> INFO
+    RTAB --> MAPS
+    RTAB --> TF
+```
+
+## Conventions
+
+### Frames and TF
+
+The node publishes `map` → `odom`, the correction from the optimized graph; odometry publishes `odom` → `base_link`, and the sensors are attached to `base_link`. See [Frames and TF](doc/rtabmap.md#frames-and-tf) for the parameters.
+
+```mermaid
+flowchart TB
+    MAP(["map<br><i>map_frame_id</i>"])
+    ODOM(["odom<br><i>odometry frame</i>"])
+    BASE(["base_link<br><i>frame_id</i>"])
+    SENSOR(["camera, lidar, imu..."])
+    MAP -->|this node| ODOM
+    ODOM -->|odometry| BASE
+    BASE -->|static, URDF| SENSOR
+```
+
+### The database
+
+**The map is stored in `database_path`**, `~/.ros/rtabmap.db` by default (under `$ROS_HOME` if that is set). Set `delete_db_on_start`, or pass `-d` as an argument, to start from an empty one. Otherwise restarting on an existing database continues it: the map is reloaded, the next update starts a **new session**, and a loop closure between the new session and an old one merges the two. That is how a map is extended over several runs.
+
+**The database is saved on shutdown**. A node that is killed rather than shut down loses whatever had not been written yet.
+
+**The database also remembers the parameters it was built with**, and reopening it without setting them again brings them back. For example, a map made with ICP registration keeps using ICP, which is what makes the new session compatible with the old ones. Anything set explicitly still wins, and `delete_db_on_start` forgets them along with the map.
+
+### Update rate and dropped updates
+
+`Rtabmap/DetectionRate` is how many updates per second are processed. It is 1 Hz by default because SLAM does not need more: odometry carries the pose between nodes, and each node costs memory, loop closure detection and optimization time for as long as the map exists.
+
+> **Warning: `Rtabmap/DetectionRate` at `0` with sensor updates faster than about 2 Hz makes loop closure detection, graph optimization and map generation intractable fast.** Every update then becomes a node, and each node is compared against all the nodes in working memory, adds a pose to optimize and data to assemble into the maps, so the map, and the time each update takes, grow at the sensors' rate until the node cannot keep up. Keep a detection rate of 1 to 2 Hz, or bound working memory with `Rtabmap/TimeThr` or `Rtabmap/MemoryThr`.
+
+**A robot standing still does not grow the map.** An update that moved less than both `RGBD/LinearUpdate` and `RGBD/AngularUpdate` since the last node is still used to detect loop closures, and then dropped. Set both to `0` to add a node every time.
+
+**An update arriving while the previous one is still being processed is dropped**, not queued. SLAM time grows with the map, so a queue would only fall further behind; dropping keeps the node on the newest data. `info` shows how long each update took (`RtabmapROS/TimeTotal/ms`), and `/diagnostics` how many arrived versus how many were processed.
+
+### Odometry, covariance and new maps
+
+The link between two consecutive nodes is the odometry between them, weighted by its covariance — its inverse becomes the link's information matrix, so the optimizer knows how far to trust each one.
+
+Which covariance is used:
+
+- **The twist covariance, if it is set.** It is the uncertainty of the motion since the previous message, which is what a link between two nodes is.
+- **Otherwise half the pose covariance**, for odometry sources that only fill that one. This assumes it is the error of the motion since the previous message, as visual odometry often publishes it, not the unbounded uncertainty of the pose estimated by a filtered odometry.
+- **Otherwise `odom_tf_linear_variance` and `odom_tf_angular_variance`** (`0.001` by default), for a covariance that is zero, not finite, or exactly `1` — which is what several drivers publish to mean "not set". Many do publish zeros, and taking those at face value would make each link infinitely confident.
+
+Between two nodes, the largest covariance seen is kept, so updates dropped by the rate do not make the link look more certain than any of the motions that made it up.
+
+**An odometry reset starts a new map**, in the same database, rather than deforming the graph across a jump the robot never made:
+
+```
+Odometry is reset (identity pose or high variance detected). Increment map id!
+```
+
+A reset is an identity pose after a non-identity one, or `9999` on both the pose and the twist covariance diagonals — which is what the [odometry nodes publish](../rtabmap_odom/README.md#lost-frames-resets-and-new-maps) when they lose track or restart. Odometry read from TF has no covariance, so only the identity pose counts there. The new map is merged back into the old one on the first loop closure between them.
+
+A consequence worth knowing: an odometry that returns to *exactly* the identity is taken for a reset. Real odometry never does, but a simulator or a test that drives back to the origin will.
+
+**`staleness_factor`** treats a long silence the same way. With `Rtabmap/DetectionRate` at 1 Hz and a factor of 2, an update more than 2 seconds after the previous one starts a new map. It is for odometry sources that go quiet instead of reporting a reset — a gap that long means the motion across it is not known, even if the next pose looks plausible.
+
+### Mapping and localization
+
+`Mem/IncrementalMemory` chooses between the two:
+
+- **`true`, mapping (SLAM)**, the default. Updates become nodes and the map grows. This is the mode to create a map of the environment.
+- **`false`, localization.** The map is loaded and not extended: each update is compared against it, localizes the robot if it matches, and is not added to the database. This is the mode to localize in a map already recorded, without increasing CPU and RAM usage, since the map is kept fixed.
+
+`set_mode_localization` and `set_mode_mapping` switch at runtime. Going back to mapping starts a new session, since nothing links where the robot is now to where it left the map — until a loop closure does.
+
+See [Localization](doc/rtabmap.md#localization) for where the robot starts on the map in localization mode.
+
+## License
+
+BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license).

rtabmap_slam/doc/rtabmap.md

@@ -0,0 +1,623 @@
+# rtabmap
+
+Graph SLAM: each update that moved far enough becomes a node, linked to the previous one by odometry and to earlier ones by the loop closures found, and the graph is optimized every time a loop closure is added.
+
+Loop closures are found two ways:
+
+- **Appearance-based**: the node's visual words are compared against every node in working memory with an incremental bag-of-words (BoW) approach, which is independent of the odometry pose, and so of its drift.
+- **Proximity-based**: the node is registered against the nodes the graph says are nearby, based on the previous localization and the current odometry pose. This is what a lidar-only setup relies on.
+
+**Memory management**: working memory can be bounded, by update time (`Rtabmap/TimeThr`, in ms) or by node count (`Rtabmap/MemoryThr`): older nodes are then moved to the database and brought back when the robot returns near them, so the update time stays flat on large maps. Both are `0` by default, which leaves working memory unbounded: every node stays in it, and the update time grows with the map. Before enabling memory management, we strongly recommend reading [Long-Term Online Multi-Session Graph-Based SPLAM with Memory Management](https://arxiv.org/abs/2301.00050), which explains how it works and what it implies for mapping, localization and planning.
+
+## Contents
+
+- [Usage](#usage)
+- [Choosing the inputs](#choosing-the-inputs)
+  - [RGB-D camera (RGB-D visual SLAM)](#rgb-d-camera-rgb-d-visual-slam)
+  - [Stereo camera (stereo visual SLAM)](#stereo-camera-stereo-visual-slam)
+  - [RGB-D or stereo camera and lidar](#rgb-d-or-stereo-camera-and-lidar)
+  - [Several RGB-D or stereo cameras](#several-rgb-d-or-stereo-cameras)
+  - [Several RGB-D or stereo cameras and lidar](#several-rgb-d-or-stereo-cameras-and-lidar)
+  - [Lidar alone](#lidar-alone)
+  - [RGB camera with odometry](#rgb-camera-with-odometry)
+  - [RGB camera alone (appearance-based loop closure detection)](#rgb-camera-alone-appearance-based-loop-closure-detection)
+- [Odometry from TF](#odometry-from-tf)
+- [Automatic adjustments](#automatic-adjustments)
+- [Sensors not stamped together](#sensors-not-stamped-together)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Services](#services)
+- [Parameters](#parameters)
+  - [RTAB-Map's own parameters](#rtab-maps-own-parameters)
+- [Frames and TF](#frames-and-tf)
+- [Asynchronous inputs](#asynchronous-inputs)
+  - [Landmarks](#landmarks)
+  - [GPS and global pose](#gps-and-global-pose)
+  - [IMU](#imu)
+  - [User data and environment sensors](#user-data-and-environment-sensors)
+  - [Intermediate odometry](#intermediate-odometry)
+- [Deriving missing data](#deriving-missing-data)
+- [Localization](#localization)
+- [Planning](#planning)
+- [Diagnostics](#diagnostics)
+
+## Usage
+
+RGB-D camera, with odometry from [rgbd_odometry](../../rtabmap_odom/doc/rgbd_odometry.md) or any other source on `odom`, and the camera synchronized by [rgbd_sync](../../rtabmap_sync/doc/rgbd_sync.md):
+
+```bash
+ros2 run rtabmap_slam rtabmap --ros-args \
+  -p subscribe_depth:=false -p subscribe_rgb:=false -p subscribe_rgbd:=true \
+  -p frame_id:=base_link \
+  -r rgbd_image:=/camera/rgbd_image \
+  -r odom:=/odom
+```
+
+2D lidar, with odometry from TF:
+
+```bash
+ros2 run rtabmap_slam rtabmap --ros-args \
+  -p subscribe_depth:=false -p subscribe_rgb:=false -p subscribe_scan:=true \
+  -p frame_id:=base_link \
+  -p odom_frame_id:=odom \
+  -p "Reg/Force3DoF:='true'" \
+  -r scan:=/scan
+```
+
+```python
+ComposableNode(
+    package='rtabmap_slam',
+    plugin='rtabmap_slam::CoreWrapper',
+    name='rtabmap',
+    parameters=[{'frame_id': 'base_link',
+                 'subscribe_depth': False,
+                 'subscribe_rgb': False,
+                 'subscribe_rgbd': True,
+                 'subscribe_scan': True,
+                 'approx_sync': True,
+                 'RGBD/LinearUpdate': '0.1',
+                 'Reg/Force3DoF': 'true'}],
+    remappings=[('rgbd_image', '/camera/rgbd_image'),
+                ('scan', '/scan'),
+                ('odom', '/odom')])
+```
+
+The executable runs the node on a **multi-threaded** executor, and the node relies on it. SLAM runs in its own callback group, so the synchronized inputs keep arriving while an update is being processed; the asynchronous inputs (GPS, IMU, landmarks, user data...) have groups of their own, so they are buffered rather than blocked. Loaded into a single-threaded component container, it still works, but everything is serialized behind the SLAM update.
+
+In a component container with intra-process communication enabled (`use_intra_process_comms`), the latched publishers (`mapGraph` and the [`MapsManager`](../../rtabmap_util/README.md#mapsmanager) maps, with `latch` on, the default) automatically opt out of it, since intra-process communication does not support transient local durability. The other publishers keep the container's setting, and with `latch` off, all of them do.
+
+[`rtabmap_launch`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_launch) wraps all of this, together with odometry and `rtabmap_viz`, and is where most setups should start.
+
+## Choosing the inputs
+
+The common setups are below. How the input topics are synchronized — `approx_sync`, `topic_queue_size`, `sync_queue_size` and the `qos*` parameters — is documented in [rtabmap_sync](../../rtabmap_sync/README.md#conventions).
+
+### RGB-D camera (RGB-D visual SLAM)
+
+```yaml
+subscribe_depth: true   # default
+subscribe_rgb: true   # default
+```
+
+This is the legacy default. The recommended way is instead to synchronize the camera topics together with [rgbd_sync](../../rtabmap_sync/doc/rgbd_sync.md), and subscribe to its `rgbd_image`, as in [RGB-D or stereo camera and lidar](#rgb-d-or-stereo-camera-and-lidar) without the lidar.
+
+```mermaid
+%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
+flowchart LR
+    CAM(["rgb/image<br>depth/image<br>rgb/camera_info"])
+    ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
+    R["<b>rtabmap</b>"]
+    CAM --> R
+    ODOM --> R
+```
+
+### Stereo camera (stereo visual SLAM)
+
+```yaml
+subscribe_stereo: true
+subscribe_depth: false
+subscribe_rgb: false
+```
+
+`approx_sync` defaults to `false` here: the left and right images, and the odometry, are expected with exactly the same stamp, as when the odometry comes from [stereo_odometry](../../rtabmap_odom/doc/stereo_odometry.md) on the same camera. The images are assumed to be already rectified; if they are not, set `Rtabmap/ImagesAlreadyRectified` to `false` to rectify them here, at rtabmap's update rate.
+
+```mermaid
+%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
+flowchart LR
+    CAM(["left/image_rect<br>left/camera_info<br>right/image_rect<br>right/camera_info"])
+    ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
+    R["<b>rtabmap</b>"]
+    CAM --> R
+    ODOM --> R
+```
+
+### RGB-D or stereo camera and lidar
+
+```yaml
+subscribe_rgbd: true
+subscribe_scan: true   # for a 2D lidar
+#subscribe_scan_cloud: true   # for a 3D lidar
+subscribe_depth: false
+subscribe_rgb: false
+```
+
+The camera comes as one `rgbd_image`, from [rgbd_sync](../../rtabmap_sync/doc/rgbd_sync.md) for an RGB-D camera or [stereo_sync](../../rtabmap_sync/doc/stereo_sync.md) for a stereo camera. Loop closures are still detected visually; with `Reg/Strategy` set to `1`, they are then refined with the lidar (ICP). `RGBD/NeighborLinkRefining` set to `true` also refines, with that registration, the link between each new node and the previous one, which corrects the odometry.
+
+```mermaid
+%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
+flowchart LR
+    SYNC["rgbd_sync<br>or stereo_sync"]
+    SCAN(["scan or scan_cloud"])
+    ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
+    R["<b>rtabmap</b>"]
+    SYNC -->|rgbd_image| R
+    SCAN --> R
+    ODOM --> R
+```
+
+### Several RGB-D or stereo cameras
+
+```yaml
+subscribe_rgbd: true
+rgbd_cameras: 4
+subscribe_depth: false
+subscribe_rgb: false
+```
+
+Each camera is synchronized by its own [rgbd_sync](../../rtabmap_sync/doc/rgbd_sync.md) or [stereo_sync](../../rtabmap_sync/doc/stereo_sync.md), and rtabmap subscribes to their `rgbd_image0`, `rgbd_image1`... directly. This requires `rtabmap_sync` built with [`RTABMAP_SYNC_MULTI_RGBD`](../../rtabmap_sync/README.md#build-options), which is off by default; otherwise, see [Several RGB-D or stereo cameras and lidar](#several-rgb-d-or-stereo-cameras-and-lidar).
+
+```mermaid
+%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
+flowchart LR
+    S0["rgbd_sync<br>or stereo_sync"]
+    S1["rgbd_sync<br>or stereo_sync"]
+    S2["rgbd_sync<br>or stereo_sync"]
+    S3["rgbd_sync<br>or stereo_sync"]
+    ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
+    R["<b>rtabmap</b>"]
+    S0 -->|rgbd_image0| R
+    S1 -->|rgbd_image1| R
+    S2 -->|rgbd_image2| R
+    S3 -->|rgbd_image3| R
+    ODOM --> R
+```
+
+### Several RGB-D or stereo cameras and lidar
+
+```yaml
+subscribe_rgbd: true
+rgbd_cameras: 0
+subscribe_scan: true   # optional, for a 2D lidar
+#subscribe_scan_cloud: true   # optional, for a 3D lidar
+subscribe_depth: false
+subscribe_rgb: false
+```
+
+[rgbdx_sync](../../rtabmap_sync/doc/rgbdx_sync.md) combines the cameras' `rgbd_image` into one `rgbd_images`. This works without any build option.
+
+```mermaid
+%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
+flowchart LR
+    S0["rgbd_sync<br>or stereo_sync"]
+    S1["rgbd_sync<br>or stereo_sync"]
+    S2["rgbd_sync<br>or stereo_sync"]
+    S3["rgbd_sync<br>or stereo_sync"]
+    X["rgbdx_sync"]
+    SCAN(["scan or scan_cloud"])
+    ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
+    R["<b>rtabmap</b>"]
+    S0 -->|rgbd_image0| X
+    S1 -->|rgbd_image1| X
+    S2 -->|rgbd_image2| X
+    S3 -->|rgbd_image3| X
+    X -->|rgbd_images| R
+    SCAN --> R
+    ODOM --> R
+```
+
+### Lidar alone
+
+```yaml
+subscribe_scan: true   # for a 2D lidar
+#subscribe_scan_cloud: true   # for a 3D lidar
+subscribe_depth: false
+subscribe_rgb: false
+```
+
+There are no images: bag-of-words is disabled, and loop closures are found by proximity alone.
+
+```mermaid
+%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
+flowchart LR
+    SCAN(["scan or scan_cloud"])
+    ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
+    R["<b>rtabmap</b>"]
+    SCAN --> R
+    ODOM --> R
+```
+
+### RGB camera with odometry
+
+```yaml
+subscribe_depth: false
+subscribe_rgb: true   # default
+```
+
+Without depth, the images cannot build a metric map, so this is mainly useful in localization mode, to localize a single camera on a map built with a depth camera. [rgb_sync](../../rtabmap_sync/doc/rgb_sync.md) can also be used to synchronize the image with its camera_info, and rtabmap then subscribes to its `rgbd_image` with `subscribe_rgbd:=true` and `subscribe_rgb:=false`.
+
+```mermaid
+%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
+flowchart LR
+    CAM(["rgb/image<br>rgb/camera_info"])
+    ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
+    R["<b>rtabmap</b>"]
+    CAM --> R
+    ODOM --> R
+```
+
+### RGB camera alone (appearance-based loop closure detection)
+
+```yaml
+subscribe_depth: false
+subscribe_rgb: false
+subscribe_odom: false
+RGBD/Enabled: "false"
+```
+
+The node then subscribes to `image` and only detects loop closures between images: no odometry, no graph optimization, no metric map.
+
+```mermaid
+%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
+flowchart LR
+    CAM(["image"])
+    R["<b>rtabmap</b>"]
+    CAM --> R
+```
+
+## Odometry from TF
+
+Setting `odom_frame_id` reads odometry from TF instead: `odom_frame_id` → `frame_id` is looked up at the stamp of each sensor message, and `subscribe_odom` is turned off. This is the natural arrangement when the odometry source publishes only TF, and it saves synchronizing one more topic.
+
+The trade-off is covariance: TF has none, so every link gets `odom_tf_linear_variance` and `odom_tf_angular_variance`, and an odometry reset can only be recognized by an identity pose — not by the `9999` covariance the [odometry nodes](../../rtabmap_odom/README.md#lost-frames-resets-and-new-maps) publish when they lose track. Prefer the topic when the source provides a meaningful covariance.
+
+A sensor message whose stamp cannot be found in TF within `wait_for_transform` is dropped. The same goes for a sensor frame that is not connected to `frame_id`.
+
+## Automatic adjustments
+
+Some RTAB-Map defaults only make sense for a camera. With a lidar, or without a camera, the node adjusts them — for example, the occupancy grid built from the scan, and loop closures registered with ICP — unless they were set explicitly, and the log says what it changed.
+
+## Sensors not stamped together
+
+On a real robot the sensors are rarely stamped together: odometry, a lidar and cameras run at their own rates and are triggered independently. The synchronizer (`approx_sync`) groups the closest messages into one update, and the node then makes them consistent in time:
+
+- **The node's stamp is the lidar's**, when there is one, or else the first camera's.
+- **The odometry is taken at that stamp**, interpolated in TF between odometry samples. Without odometry in TF, the synchronized odometry message is used as it is, pose and stamp: the node then takes the odometry's stamp rather than the lidar's.
+- **The link's covariance is the synchronized odometry message's**, not the last one received: the largest among the updates merged into the node.
+
+`odom_sensor_sync`, on by default, uses the odometry in TF to correct each sensor for the robot's motion:
+
+- **Each camera** is moved by the motion between its own stamp and the node's: an image taken 15 ms after the lidar is placed where the robot was 15 ms later. With several cameras triggered one after the other -- in one `rgbd_images` message or on separate topics -- each keeps its own stamp and is placed separately.
+- **A 3D cloud** (`scan_cloud`) is assumed already deskewed, and is moved as a whole, the same way. Deskew it upstream, with [`lidar_deskewing`](../../rtabmap_util/doc/lidar_deskewing.md) or [`icp_odometry`'s deskewing](../../rtabmap_odom/doc/icp_odometry.md#deskewing).
+- **A 2D scan** (`scan`) is deskewed ray by ray, when its `time_increment` is set: each ray is placed where the robot was when it was measured. That needs the odometry in TF across the whole sweep, within `wait_for_transform`.
+
+**Without odometry in TF** -- odometry published as a topic only -- none of this is possible, and the sensors are used as they are: cameras at their mount with a warning, 2D scans without deskewing with a warning shown once. Nothing is dropped.
+
+With `odom_sensor_sync` off, every sensor is placed at its mount, as if it had been stamped with the lidar, and 2D scans are not deskewed. On a moving robot, that costs centimeters: a camera triggered 15 ms late on a robot turning at 0.5 rad/s misplaces what it sees 3 m away by 2 cm, and a 0.1 s lidar sweep at 1 m/s bends the scan by 10 cm.
+
+## Subscribed Topics
+
+**Synchronized** — see [Choosing the inputs](#choosing-the-inputs).
+
+| Topic | Type | Description |
+|---|---|---|
+| `rgb/image`, `depth/image`, `rgb/camera_info` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html), [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | RGB-D camera, depth registered to color. |
+| `left/image_rect`, `right/image_rect`, `left/camera_info`, `right/camera_info` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html), [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Stereo camera. |
+| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | One camera, from [rgbd_sync](../../rtabmap_sync/doc/rgbd_sync.md), [stereo_sync](../../rtabmap_sync/doc/stereo_sync.md) or [rgb_sync](../../rtabmap_sync/doc/rgb_sync.md). |
+| `rgbd_images` | [`rtabmap_msgs/msg/RGBDImages`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImages.html) | Several cameras, from [rgbdx_sync](../../rtabmap_sync/doc/rgbdx_sync.md). |
+| `scan` | [`sensor_msgs/msg/LaserScan`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/LaserScan.html) | 2D lidar. |
+| `scan_cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | 3D lidar, or [`icp_odometry`](../../rtabmap_odom/doc/icp_odometry.md)'s [filtered scan](../../rtabmap_odom/doc/icp_odometry.md#reusing-the-filtered-scan-downstream). |
+| `scan_descriptor` | [`rtabmap_msgs/msg/ScanDescriptor`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/ScanDescriptor.html) | A scan with a global descriptor for loop closure detection. |
+| `sensor_data` | [`rtabmap_msgs/msg/SensorData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/SensorData.html) | Everything a node holds in one message, as the odometry nodes republish it on `odom_sensor_data/*`. |
+| `odom` | [`nav_msgs/msg/Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) | Odometry. Mainly used for its covariance, which weights the link between consecutive nodes in the graph (see [Odometry, covariance and new maps](../README.md#odometry-covariance-and-new-maps)). The pose is taken from TF at the sensors' stamp when available; the message's pose is used otherwise. |
+| `odom_info` | [`rtabmap_msgs/msg/OdomInfo`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/OdomInfo.html) | From an `rtabmap_odom` node: its statistics are stored with the node, and its measured motion gives the velocity. |
+| `user_data` | [`rtabmap_msgs/msg/UserData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/UserData.html) | Arbitrary data stored with the node. |
+
+**Asynchronous** — buffered, and attached to the next node. See [Asynchronous inputs](#asynchronous-inputs).
+
+| Topic | Type | Description |
+|---|---|---|
+| `user_data_async` | [`rtabmap_msgs/msg/UserData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/UserData.html) | Arbitrary data for the next node. See [User data and environment sensors](#user-data-and-environment-sensors). |
+| `gps/fix` | [`sensor_msgs/msg/NavSatFix`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/NavSatFix.html) | GPS. See [GPS and global pose](#gps-and-global-pose). |
+| `global_pose` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseWithCovarianceStamped.html) | An absolute pose from outside, added as a prior. See [GPS and global pose](#gps-and-global-pose). |
+| `imu` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Only the orientation is used, for gravity constraints: it must already be estimated, by [`imu_filter_madgwick` or `imu_complementary_filter`](https://github.com/CCNYRoboticsLab/imu_tools) for example. See [IMU](#imu). |
+| `landmark_detection`, `landmark_detections` | [`rtabmap_msgs/msg/LandmarkDetection`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/LandmarkDetection.html), [`rtabmap_msgs/msg/LandmarkDetections`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/LandmarkDetections.html) | Fiducials or any other identified landmark. See [Landmarks](#landmarks). |
+| `apriltag/detections` | [`apriltag_msgs/msg/AprilTagDetectionArray`](https://github.com/christianrauch/apriltag_msgs/blob/master/msg/AprilTagDetectionArray.msg) | Landmarks straight from [apriltag_ros](https://github.com/christianrauch/apriltag_ros). `tag_detections` is its deprecated name. |
+| `aruco/detections` | [`aruco_msgs/msg/MarkerArray`](https://docs.ros.org/en/jazzy/p/aruco_msgs/msg/MarkerArray.html) | Landmarks straight from [aruco_ros](https://github.com/pal-robotics/aruco_ros). |
+| `aruco_opencv/detections` | [`aruco_opencv_msgs/msg/ArucoDetection`](https://docs.ros.org/en/jazzy/p/aruco_opencv_msgs/msg/ArucoDetection.html) | Landmarks straight from [ros_aruco_opencv](https://github.com/fictionlab/ros_aruco_opencv). |
+| `aruco_markers/detections` | [`aruco_markers_msgs/msg/MarkerArray`](https://docs.ros.org/en/jazzy/p/aruco_markers_msgs/msg/MarkerArray.html) | Landmarks straight from [aruco_markers](https://github.com/namo-robotics/aruco_markers). |
+| `aruco_interfaces/detections` | [`ros2_aruco_interfaces/msg/ArucoMarkers`](https://github.com/JMU-ROBOTICS-VIVA/ros2_aruco/blob/main/ros2_aruco_interfaces/msg/ArucoMarkers.msg) | Landmarks straight from [ros2_aruco](https://github.com/JMU-ROBOTICS-VIVA/ros2_aruco). |
+| `env_sensor` | [`rtabmap_msgs/msg/EnvSensor`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/EnvSensor.html) | A scalar reading stored with the next node: WiFi signal strength, or one of the [environment sensors Android devices have](https://developer.android.com/develop/sensors-and-location/sensors/sensors_environment). See [User data and environment sensors](#user-data-and-environment-sensors). |
+| `inter_odom` | [`nav_msgs/msg/Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) | A faster odometry, to fill the gaps between nodes with intermediate nodes. See [Intermediate odometry](#intermediate-odometry). |
+| `inter_odom_info` | [`rtabmap_msgs/msg/OdomInfo`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/OdomInfo.html) | Its statistics, with `subscribe_inter_odom_info`. See [Intermediate odometry](#intermediate-odometry). |
+
+Each detector topic (`apriltag/detections` to `aruco_interfaces/detections`) exists only when this package was built with that detector's messages package. They all feed the same landmarks as `landmark_detection`; see [Landmarks](#landmarks).
+
+**Commands**
+
+| Topic | Type | Description |
+|---|---|---|
+| `initialpose` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseWithCovarianceStamped.html) | Where the robot is, in localization mode. |
+| `goal` | [`geometry_msgs/msg/PoseStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseStamped.html) | A goal pose to plan to. |
+| `goal_node` | [`rtabmap_msgs/msg/Goal`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/Goal.html) | A goal node, by id or label. |
+| `~/republish_node_data` | [`std_msgs/msg/Int32MultiArray`](https://docs.ros.org/en/jazzy/p/std_msgs/msg/Int32MultiArray.html) | Node ids whose data to include in the next `mapData`, for a visualizer catching up on a map it joined late. |
+
+## Published Topics
+
+**Every topic is published only when something is subscribed** — the work of building each message is skipped otherwise. The TF broadcast is not gated this way.
+
+| Topic | Type | Description |
+|---|---|---|
+| `info` | [`rtabmap_msgs/msg/Info`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/Info.html) | Everything about the last update: the node id, loop closure and proximity detection results, and all of RTAB-Map's statistics with their timings. One per processed update — intermediate nodes excepted. The first thing to look at when the map misbehaves. |
+| `mapData` | [`rtabmap_msgs/msg/MapData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/MapData.html) | The optimized graph, plus the data of the node just added. What `rtabmap_viz` and [map_assembler](../../rtabmap_util/doc/map_assembler.md) consume. |
+| `mapGraph` | [`rtabmap_msgs/msg/MapGraph`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/MapGraph.html) | The optimized graph alone: poses, links and the map → odom correction. Latched when `latch` is on. |
+| `mapPath` | [`nav_msgs/msg/Path`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Path.html) | The optimized trajectory, for display. |
+| `mapOdomCache` | [`rtabmap_msgs/msg/MapGraph`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/MapGraph.html) | In localization mode, the recent odometry poses kept to localize against, with their links to the map. |
+| `localization_pose` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseWithCovarianceStamped.html) | The robot in the map frame after each update, with RTAB-Map's covariance — in mapping mode, the odometry's accumulated along the graph. See [Localization](#localization). |
+| `landmarks` | [`geometry_msgs/msg/PoseArray`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseArray.html) | The optimized landmark poses. |
+| `labels` | [`visualization_msgs/msg/MarkerArray`](https://docs.ros.org/en/jazzy/p/visualization_msgs/msg/MarkerArray.html) | Node ids, labels and landmark ids as text, for RViz. |
+| `local_grid_obstacle`, `local_grid_empty`, `local_grid_ground` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The local occupancy grid of the node just added, in `frame_id`. |
+| `map`, `grid_prob_map`, `cloud_map`, `cloud_obstacles`, `cloud_ground`, `octomap_*`, `elevation_map` | [`nav_msgs/msg/OccupancyGrid`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/OccupancyGrid.html), [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html), [`octomap_msgs/msg/Octomap`](https://docs.ros.org/en/jazzy/p/octomap_msgs/msg/Octomap.html), [`grid_map_msgs/msg/GridMap`](https://github.com/ANYbotics/grid_map/blob/master/grid_map_msgs/msg/GridMap.msg) | The assembled maps, from [`MapsManager`](../../rtabmap_util/README.md#mapsmanager), which lists them. |
+| `goal_out`, `goal_reached`, `global_path`, `local_path`, `global_path_nodes`, `local_path_nodes` | [`geometry_msgs/msg/PoseStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseStamped.html), [`std_msgs/msg/Bool`](https://docs.ros.org/en/jazzy/p/std_msgs/msg/Bool.html), [`nav_msgs/msg/Path`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Path.html), [`rtabmap_msgs/msg/Path`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/Path.html) | Planning. See [Planning](#planning). |
+
+## Services
+
+All under the node's name: `/rtabmap/reset`, not `/reset`. They run in the same callback group as SLAM, so a call waits for the current update to finish, and no update runs while a service does.
+
+| Service | Type | Description |
+|---|---|---|
+| `reset` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | **Erase the map**, in memory and in the database. Node ids start over from 1. |
+| `trigger_new_map` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Start a new session in the same database; the old one is kept. |
+| `pause`, `resume` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Stop taking input, and start again. Input received while paused is dropped, not queued. Mirrored in the `is_rtabmap_paused` parameter, which can also start the node paused. |
+| `set_mode_localization`, `set_mode_mapping` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | See [Mapping and localization](../README.md#mapping-and-localization). |
+| `backup` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Save the database now, copy it to `<database_path>.back`, and carry on in a new session. |
+| `load_database` | [`rtabmap_msgs/srv/LoadDatabase`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/LoadDatabase.html) | Save the current map and switch to another database; `clear` empties the target first. The current parameters are kept — a warning lists those the target database was built with differently. |
+| `update_parameters` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Re-read every RTAB-Map ROS parameter and apply it. |
+| `get_map_data` | [`rtabmap_msgs/srv/GetMap`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GetMap.html) | The graph and its nodes. `graph_only` leaves out their images, scans and user data, which are most of the size; `global_map` includes the nodes not in working memory; `optimized` returns optimized poses rather than odometry ones. |
+| `get_map_data2` | [`rtabmap_msgs/srv/GetMap2`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GetMap2.html) | The same, choosing each kind of node data separately. |
+| `get_node_data` | [`rtabmap_msgs/srv/GetNodeData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GetNodeData.html) | Given nodes, with the data asked for. No id means the latest node. |
+| `get_map`, `get_prob_map` | [`nav_msgs/srv/GetMap`](https://docs.ros.org/en/jazzy/p/nav_msgs/srv/GetMap.html) | The occupancy grid, as trinary or as probabilities. Empty if the map has no grid. |
+| `publish_map` | [`rtabmap_msgs/srv/PublishMap`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/PublishMap.html) | Republish the map on the topics that have subscribers, with the same `global_map`, `optimized` and `graph_only` options. |
+| `get_nodes_in_radius` | [`rtabmap_msgs/srv/GetNodesInRadius`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GetNodesInRadius.html) | Nodes within a radius of a node (not counting it) or of a position, which is used when `node_id` is 0 and it is not the origin. |
+| `set_label`, `list_labels`, `remove_label` | [`rtabmap_msgs/srv/SetLabel`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/SetLabel.html), [`rtabmap_msgs/srv/ListLabels`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/ListLabels.html), [`rtabmap_msgs/srv/RemoveLabel`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/RemoveLabel.html) | Name nodes, so a goal can be `"kitchen"` rather than an id. Node 0 means the latest node. A label is unique in the map. |
+| `add_link` | [`rtabmap_msgs/srv/AddLink`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/AddLink.html) | Add a constraint found outside the node — a loop closure from another process, for instance. |
+| `detect_more_loop_closures` | [`rtabmap_msgs/srv/DetectMoreLoopClosures`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/DetectMoreLoopClosures.html) | Post-processing: look for loop closures between nodes close to each other in the optimized graph. |
+| `global_bundle_adjustment` | [`rtabmap_msgs/srv/GlobalBundleAdjustment`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GlobalBundleAdjustment.html) | Post-processing: refine the graph with bundle adjustment on the visual features. |
+| `cleanup_local_grids` | [`rtabmap_msgs/srv/CleanupLocalGrids`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/CleanupLocalGrids.html) | Post-processing: remove from each node's local grid the obstacles the global map says are free — people who walked through, for instance. |
+| `set_goal`, `cancel_goal`, `get_plan`, `get_plan_nodes` | [`rtabmap_msgs/srv/SetGoal`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/SetGoal.html), [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html), [`nav_msgs/srv/GetPlan`](https://docs.ros.org/en/jazzy/p/nav_msgs/srv/GetPlan.html), [`rtabmap_msgs/srv/GetPlan`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GetPlan.html) | Planning. See [Planning](#planning). |
+| `octomap_binary`, `octomap_full` | [`octomap_msgs/srv/GetOctomap`](https://docs.ros.org/en/jazzy/p/octomap_msgs/srv/GetOctomap.html) | The octomap. Only with RTAB-Map built with OctoMap and this package built with `octomap_msgs`. |
+| `log_debug`, `log_info`, `log_warning`, `log_error` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Set RTAB-Map's own log level, independently from ROS's. |
+
+## Parameters
+
+The node's own ROS parameters, with their real types. The ones about frames are in [Frames and TF](#frames-and-tf); the input ones in [Choosing the inputs](#choosing-the-inputs); map assembly (`map_*`, `cloud_*`, `octomap_*`, `latch`) with [`MapsManager`](../../rtabmap_util/README.md#mapsmanager).
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `database_path` | `string` | `"~/.ros/rtabmap.db"` | The map. `~` is expanded, and a relative path is taken from the working directory of the process. Under `$ROS_HOME` if that is set. |
+| `delete_db_on_start` | `bool` | `false` | Start from an empty map. `-d` or `--delete_db_on_start` as an argument does the same. |
+| `use_saved_map` | `bool` | `true` | Load the occupancy grid saved in the database at startup, instead of reassembling it from the nodes. |
+| `config_path` | `string` | `""` | INI file of RTAB-Map parameters, read at startup and written on shutdown. |
+| `is_rtabmap_paused` | `bool` | `false` | Start paused, waiting for the `resume` service. |
+| `initial_pose` | `string` | `""` | `"x y z roll pitch yaw"` to start from in localization mode. See [Localization](#localization). |
+| `pub_loc_pose_only_when_localizing` | `bool` | `false` | Publish `localization_pose` only on updates that found a loop closure, a proximity detection or a landmark. |
+| `loc_thr` | `double` | `0.0` | Localization error, in meters, above which diagnostics report an error. Localization mode only; `0` disables. |
+| `odom_tf_linear_variance` | `double` | `0.001` | Translational variance used when the odometry carries no usable covariance. See [Odometry, covariance and new maps](../README.md#odometry-covariance-and-new-maps). |
+| `odom_tf_angular_variance` | `double` | `0.001` | Rotational variance used when the odometry carries no usable covariance. |
+| `staleness_factor` | `double` | `0.0` | Start a new map after a gap longer than this many detection periods. `0` disables; values under `1` are refused and disable it too. See [Odometry, covariance and new maps](../README.md#odometry-covariance-and-new-maps). |
+| `landmark_linear_variance` | `double` | `0.001` | Translational variance of a landmark detection that carries no covariance. |
+| `landmark_angular_variance` | `double` | `0.001` | Rotational variance of a landmark detection that carries no covariance. |
+| `use_action_for_goal` | `bool` | `false` | Send goals to nav2's `navigate_to_pose` action instead of publishing them on `goal_out`. Requires the package built with `nav2_msgs`. |
+| `gen_scan` | `bool` | `false` | Derive a 2D scan from the depth image(s) when no scan is subscribed. See [Deriving missing data](#deriving-missing-data). |
+| `gen_scan_max_depth` | `double` | `4.0` | Farthest depth used for it, in meters. |
+| `gen_scan_min_depth` | `double` | `0.0` | Nearest. |
+| `gen_depth` | `bool` | `false` | Project `scan_cloud` into the camera to make a depth image, for an RGB camera with a lidar. |
+| `gen_depth_decimation` | `int` | `1` | Resolution divider for it; must divide the image size. |
+| `gen_depth_fill_holes_size` | `int` | `0` | Fill holes up to this many pixels. `0` disables. |
+| `gen_depth_fill_iterations` | `int` | `1` | Hole-filling passes. |
+| `gen_depth_fill_holes_error` | `double` | `0.1` | Maximum depth difference, in meters, across a hole for it to be filled. |
+| `stereo_to_depth` | `bool` | `false` | Compute a depth image from the stereo pair (with the `StereoBM/*` parameters) and map it as RGB-D. |
+| `scan_cloud_max_points` | `int` | `0` | Points in a full `scan_cloud` sweep, for an organized or fixed-size cloud; used by ICP as the reference for its correspondence ratio. `0` takes each cloud's own size. |
+| `scan_cloud_is_2d` | `bool` | `false` | `scan_cloud` is a 2D lidar published as a cloud. |
+| `odom_sensor_sync` | `bool` | `true` | Place each sensor where the robot was at that sensor's stamp, and deskew 2D scans ray by ray, using the odometry in TF. See [Sensors not stamped together](#sensors-not-stamped-together). |
+| `subscribe_inter_odom_info` | `bool` | `false` | Synchronize `inter_odom` with `inter_odom_info`. See [Intermediate odometry](#intermediate-odometry). |
+| `log_to_rosout_level` | `int` | `4` | RTAB-Map's own log messages at or above this level (`0` debug to `4` fatal) are forwarded to `/rosout`. |
+| `qos_gps`, `qos_imu`, `qos_env_sensor` | `int` | `0` | Reliability of those subscriptions: `0` system default, `1` reliable, `2` best effort. |
+
+And every RTAB-Map parameter, as strings, as described below.
+
+### RTAB-Map's own parameters
+
+Everything in RTAB-Map's parameter set is exposed as a ROS parameter **under its RTAB-Map name**, except the odometry ones (`Odom/*`, `OdomF2M/*`...), which belong to the [odometry nodes](../../rtabmap_odom/README.md):
+
+```bash
+ros2 run rtabmap_slam rtabmap --ros-args \
+  -p "Rtabmap/DetectionRate:='2'" \
+  -p "RGBD/LinearUpdate:='0.2'" \
+  -p "Mem/IncrementalMemory:='false'"
+```
+
+**Every RTAB-Map parameter is declared as a string**, whatever it looks like, because that is how RTAB-Map's own parameter map stores them. `-p RGBD/LinearUpdate:=0.2` makes ROS infer a double, and the node throws on startup. The inner quotes are what keep it a string; in a launch file, `{'RGBD/LinearUpdate': '0.2'}`. The node's own ROS parameters — `frame_id`, `publish_tf`, `subscribe_scan` — have their real types and take plain values.
+
+`rtabmap --params` prints them all with their defaults and descriptions, and so does [RTAB-Map's parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html). Two defaults differ from RTAB-Map's own: **`RGBD/CreateOccupancyGrid` is `true`**, since a robot's map is usually meant for navigation, and **`Rtabmap/WorkingDirectory` is `$ROS_HOME`**, or `~/.ros`.
+
+A value can come from several places. From the highest priority to the lowest:
+
+1. **Arguments**, `--Param/Name value` after the executable name, or in a launch file's `arguments=[...]`.
+2. **ROS parameters.**
+3. **`config_path`**, an INI file of RTAB-Map parameters. The node writes its parameters back to it on shutdown.
+4. **The node's own adjustments to its inputs** — ICP registration for a lidar with no camera, for example. See [Automatic adjustments](#automatic-adjustments).
+5. **The database**, which remembers the parameters it was built with. See [The database](../README.md#the-database).
+6. The defaults.
+
+A parameter changed while the node runs, with `ros2 param set`, is applied straight away. `update_parameters` re-reads them all, for a change the node might have missed.
+
+Parameters RTAB-Map has renamed are still accepted under their old name, with a warning naming the new one — worth heeding, since the old names are not declared and so do not show up in `ros2 param list`.
+
+The ones that set how often a node is added, explained in [Update rate and dropped updates](../README.md#update-rate-and-dropped-updates):
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `Rtabmap/DetectionRate` | `string` | `"1"` | Updates per second, in Hz. `0` processes every one; see the [warning](../README.md#update-rate-and-dropped-updates). |
+| `Rtabmap/CreateIntermediateNodes` | `string` | `"false"` | Keep the updates that `Rtabmap/DetectionRate` would skip, as intermediate nodes instead. |
+| `RGBD/LinearUpdate` | `string` | `"0.1"` | Minimum distance, in meters, the robot must have moved for an update to add a node. |
+| `RGBD/AngularUpdate` | `string` | `"0.1"` | Minimum rotation, in radians, the robot must have made for an update to add a node. |
+
+## Frames and TF
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `frame_id` | `string` | `"base_link"` | The robot frame. Every sensor is placed relative to it through TF. |
+| `odom_frame_id` | `string` | `""` | Read odometry from TF, as `odom_frame_id` → `frame_id` at each sensor stamp, instead of from the `odom` topic. Setting it forces `subscribe_odom` off. See [Odometry from TF](#odometry-from-tf). |
+| `odom_frame_id_init` | `string` | `""` | The odometry frame to publish `map` → it from the start, before any odometry has been received. Ignored when `odom_frame_id` is set. |
+| `map_frame_id` | `string` | `"map"` | The map frame, on TF and in the header of everything published in it. |
+| `publish_tf` | `bool` | `true` | Publish `map_frame_id` → odometry frame. |
+| `tf_delay` | `double` | `0.05` | Period of that publication, in seconds (20 Hz). `0` disables it. |
+| `tf_tolerance` | `double` | `0.1` | How far in the future the transform is stamped, in seconds, so that lookups at the latest sensor stamp do not have to wait for it. |
+| `wait_for_transform` | `double` | `0.2` | Seconds to wait for a TF lookup before giving up on it. |
+| `ground_truth_frame_id` | `string` | `""` | The fixed frame of a ground truth system, for example `world` published by an external localization system like Vicon or OptiTrack. `ground_truth_frame_id` → `ground_truth_base_frame_id` is looked up and stored with each node, for evaluating a trajectory afterwards. |
+| `ground_truth_base_frame_id` | `string` | value of `frame_id` | The robot frame in the ground truth tree, for example `base_link_gt`. To avoid breaking the TF tree, it represents the same frame as `frame_id`, but in a parallel TF tree, so that the robot frame does not get two parents. |
+
+**This node publishes exactly one transform: `map` → `odom`.** It is the correction that puts the odometry frame where the optimized graph says it belongs — the identity until a loop closure moves it. Odometry keeps publishing `odom` → `base_link`, and the sensors must be attached to `base_link` in TF, as in the [TF tree](../README.md#frames-and-tf).
+
+The odometry frame is taken from the odometry messages themselves, so the transform only starts once the first update has been processed — unless `odom_frame_id` or `odom_frame_id_init` says what it will be. It is published from a thread of its own at a fixed rate, independently from how fast SLAM runs.
+
+**With `Optimizer/Iterations` set to `0`, the `map` → `odom` transform is not published at all**, even with `publish_tf` on: with graph optimization disabled there is no correction to publish. That is the arrangement where another node optimizes the graph and publishes the transform instead.
+
+## Asynchronous inputs
+
+These are not synchronized with the sensors. Each is buffered as it arrives and attached to the next update, then cleared, so each value is stored with one node only. They are received on callback groups of their own, and keep being buffered while an update is processed.
+
+### Landmarks
+
+A landmark is anything recognized with an identity and a pose relative to the robot — typically a fiducial marker. It becomes a node of the graph under the **negative** of its id, linked to each node that saw it, so seeing the same marker again is a loop closure however far the odometry has drifted.
+
+- **Ids must be positive.** A detection with id 0 or less is refused.
+- The detection's frame must be in TF, connected to `frame_id`. Its pose is also corrected for the motion between its stamp and the node's, with the odometry in TF.
+- Without a covariance in the message, `landmark_linear_variance` and `landmark_angular_variance` are used. Their default of `0.001` is a standard deviation of about 3 cm, fitting a marker seen close; raise them for markers seen far away.
+- Between two updates, only the latest detection of each id is kept.
+
+`apriltag/detections` expects the apriltag_ros convention, where each detection is also published on TF as `family:id` from the camera frame; the pose is taken from there.
+
+The optimized landmarks are published on `landmarks`, and their ids on `labels`.
+
+**Landmarks can place the map in the world.** `Marker/Priors` gives some of them known world poses, `"id x y z roll pitch yaw"` with angles in radians, several separated by `|`: `"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57"` puts marker 2 one meter in front of marker 1, turned 90 degrees. As soon as one of them is seen, the map is transformed into that world frame: the robot's poses, and `map`, are then world coordinates. The priors are weighted by `Marker/PriorsVarianceLinear` and `Marker/PriorsVarianceAngular` (`0.001` by default). **They only apply with `Optimizer/PriorsIgnored` set to `false`**; at its default, `true`, they are ignored without a warning, like the GPS and global pose priors.
+
+### GPS and global pose
+
+**`gps/fix`** stores a GPS fix with the node closest in time to it, provided it is within one detection period of it (any, with `Rtabmap/DetectionRate` at `0`). Its error is the square root of the largest position variance, or 10 m when the covariance type is unknown. The fixes are stored for export and for georeferencing the map; `Rtabmap/LoopGPS` also uses them to discard loop closure candidates that are too far apart.
+
+**`global_pose`** is an absolute pose from outside — a motion capture system, a localization against another map. It is added to the node as a **pose prior**: a link from the node to itself, weighted by the message's covariance. The same time window applies. The message's frame is taken as the sensor frame, and it is transformed to `frame_id` with TF.
+
+**Priors are stored, but ignored by the optimizer by default.** GPS and global poses only pull the graph once `Optimizer/PriorsIgnored` is `false`, with an optimizer that supports them (g2o, GTSAM).
+
+### IMU
+
+The orientation from `imu`, interpolated at the node's stamp, is transformed to `frame_id` and turned into a **gravity constraint**: a link from the node to itself that holds its roll and pitch, used by the optimizer when `Optimizer/GravitySigma` is above `0` and the optimizer supports it (g2o, GTSAM). It keeps a long 3D map level where odometry alone would let it bend.
+
+- **Only the orientation is used**; the angular velocity and linear acceleration are ignored. Most IMU drivers publish raw rates and accelerations only: estimate the orientation first with a filter such as [`imu_filter_madgwick` or `imu_complementary_filter`](https://github.com/CCNYRoboticsLab/imu_tools), and feed its output here.
+- An IMU message with no orientation (all zeros) is ignored.
+- The node's stamp must match an IMU message or lie between two, or the IMU is not used for that node.
+- The IMU frame must not change: a message from another frame clears the buffer, since it means two sources are publishing on the same topic.
+
+### User data and environment sensors
+
+**`user_data_async`** is arbitrary data — a matrix, or bytes — stored with the next node only. It cannot be combined with the synchronized `user_data`: when both are present, the asynchronous one is dropped with a warning. The same goes for `sensor_data`, whose message has a user data field of its own: the async user data is attached when that field is empty, and dropped with a warning when it is set, never carried over to a later node.
+
+**`env_sensor`** readings are stored with the next node, the latest value of each type. The types mirror the [environment sensors Android devices have](https://developer.android.com/develop/sensors-and-location/sensors/sensors_environment), plus WiFi and custom values:
+
+| `type` | Reading | Unit |
+|---|---|---|
+| `TYPE_WIFI_SIGNAL_STRENGTH` | WiFi signal strength | dBm |
+| `TYPE_AMBIENT_TEMPERATURE` | Ambient temperature | °C |
+| `TYPE_AMBIENT_AIR_PRESSURE` | Air pressure | hPa |
+| `TYPE_AMBIENT_LIGHT` | Illuminance | lx |
+| `TYPE_AMBIENT_RELATIVE_HUMIDITY` | Relative humidity | % |
+| `TYPE_CUSTOM1` to `TYPE_CUSTOM9` | Anything else | yours |
+
+### Intermediate odometry
+
+**Intermediate nodes** record the trajectory between two nodes, with no loop closure detection on them. `Rtabmap/CreateIntermediateNodes` makes them two ways:
+
+- **With `Rtabmap/DetectionRate` above `0`**, the updates that arrive too soon after the last processed one, and would be skipped, become intermediate nodes instead (see [Update rate and dropped updates](../README.md#update-rate-and-dropped-updates)). They can only come as fast as the synchronized sensor updates, since they are those updates, and keep their sensor data only with `Mem/IntermediateNodeDataKept`, which helps for building a map from every scan, at the price of a larger database.
+- **With `Rtabmap/DetectionRate` at `0`**, every update is already a full node -- which is only tractable with slow sensor updates, see the [warning](../README.md#update-rate-and-dropped-updates) -- and `inter_odom` adds poses between them: a faster odometry, whose messages between two updates become intermediate nodes without sensor data. That is for when the sensor updates are slow (2 Hz or less) while the odometry is fast (10 Hz or more): the trajectory is then as dense as the odometry rather than as the sensors.
+
+`inter_odom` is only subscribed when the node starts with `Rtabmap/CreateIntermediateNodes` on and `Rtabmap/DetectionRate` at `0` (every update processed); changing either later has no effect on it. Intermediate poses are only added once the map has a node.
+
+With `subscribe_inter_odom_info`, `inter_odom` is synchronized by exact stamp with `inter_odom_info`, the `OdomInfo` of an `rtabmap_odom` node: each intermediate node then also stores that odometry's statistics, and its velocity is taken from the measured motion. A message on one topic without its match on the other is not used.
+
+## Deriving missing data
+
+**`gen_scan`** makes a 2D scan out of the depth image: its middle row, between `gen_scan_min_depth` and `gen_scan_max_depth`, as a lidar at the camera's height would see it. With a depth camera and no lidar, this lets the occupancy grid be built the way it would be from a lidar — it also triggers the scan [adjustments](#automatic-adjustments) — and lets proximity detection register scans. **The cameras must be level**, looking parallel to the ground, as for [depthimage_to_laserscan](https://github.com/ros-perception/depthimage_to_laserscan): the middle row of a tilted camera sees the floor or the ceiling, not the walls around the robot.
+
+**`gen_depth`** goes the other way: with an RGB camera (`subscribe_rgb`) and a lidar (`subscribe_scan_cloud`), the cloud is projected into the camera to give a sparse depth image, filled by `gen_depth_fill_*`, so visual loop closures get 3D features.
+
+**`stereo_to_depth`** computes a dense depth image from a stereo pair, so a stereo camera is mapped like an RGB-D one — denser grids and clouds, at the cost of the disparity computation.
+
+## Localization
+
+`localization_pose` is the robot's pose in the map frame — `map` → `odom` composed with the odometry — after each update, with RTAB-Map's covariance. While mapping, that is the odometry covariance accumulated along the graph, growing with distance until a loop closure brings it down. In localization mode before the first loop closure, it is `9999`: the robot is not localized yet.
+
+In localization mode (`Mem/IncrementalMemory` at `false`, see the [README](../README.md#mapping-and-localization)), the robot is placed on the map by the first loop closure. Until then:
+
+- **The `initial_pose` parameter**, `"x y z roll pitch yaw"`, read at startup, says where the robot starts, and the odometry is added to it.
+- **The `initialpose` topic** ([`geometry_msgs/msg/PoseWithCovarianceStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseWithCovarianceStamped.html)), as RViz's *2D Pose Estimate* publishes it, does the same at any time. A pose in another frame is transformed to the map frame with TF; one without a frame is taken as being in the map frame.
+- **Without either**, the robot is assumed to restart at the last localization pose saved in the database, where it was when the node last shut down. With **`RGBD/StartAtOrigin`** set to `true`, it is assumed to start at the map's origin instead.
+
+All three are ignored in mapping mode, `initial_pose` and `initialpose` with a warning.
+
+`pub_loc_pose_only_when_localizing` restricts `localization_pose` to the updates that actually localized — found a loop closure, a proximity detection or a landmark — for a consumer that should only hear about corrections.
+
+## Planning
+
+The node plans on its own graph: a goal is a node, the plan is the chain of nodes leading to it, and a local planner is handed the next one to reach. That gives global planning across a map the local planner cannot see all of — through areas the robot has mapped, and only those.
+
+**It does not replace nav2's planner: it is a layer over it, there for memory management.** With working memory bounded (`Rtabmap/TimeThr` or `Rtabmap/MemoryThr`), the nodes moved to long-term memory stop contributing to the occupancy grid, so parts of the map published on `map` disappear over time, and nav2 alone cannot plan to them. RTAB-Map's graph still holds them: it can plan to a node in long-term memory, and as the robot moves toward it, it brings back the areas ahead of the robot, so the robot stays localized and the map around it is there for nav2 again. The plan and the retrieval are described in [Long-Term Online Multi-Session Graph-Based SPLAM with Memory Management](https://arxiv.org/abs/2301.00050) (Labbé and Michaud, *Autonomous Robots*).
+
+```mermaid
+flowchart LR
+    GOAL(["goal, goal_node<br>or set_goal"])
+    RTAB["rtabmap<br><i>global plan on the graph</i>"]
+    NAV2["nav2<br><i>planner and controller</i>"]
+    GOAL --> RTAB
+    RTAB -->|"map (occupancy grid)"| NAV2
+    RTAB -->|"next node: navigate_to_pose action<br>or goal_out topic"| NAV2
+    NAV2 -->|action result| RTAB
+```
+
+**Setting a goal:**
+
+| How | Goal |
+|---|---|
+| `set_goal` service | A node id, or a label. Returns the planned path and the planning time. |
+| `goal_node` topic | A node id, or a label. A message with neither is refused. |
+| `goal` topic | A pose, in the map frame or any frame TF can transform to it. A pose in a frame it cannot is refused. |
+
+**A pose goal within `RGBD/LocalRadius` (10 m by default) of the robot is not planned through the graph**: the plan is the node the robot is at, followed by the pose itself, and it is up to the local planner to get there. Further away, the plan goes through the graph to the node nearest the pose, and the pose is appended after it.
+
+**Following it:**
+
+- `goal_out` is the next node to reach, as a pose in the map frame, sent again whenever it changes. Point a local planner at it — or set `use_action_for_goal` to send it to nav2's `navigate_to_pose` action instead. nav2 listens for goals on `goal_pose`, so to use the topic with nav2, remap `goal_out` to `goal_pose`.
+- `global_path` and `global_path_nodes` are the whole plan, as poses and as node ids; a pose goal appears at the end with node id `0`. `local_path` and `local_path_nodes` are the part of it within the local radius.
+- `goal_reached` says `true` once the robot is within `RGBD/GoalReachedRadius` (0.5 m by default) of the goal — straight away if it already is — and `false` when planning fails, the goal cannot be found or transformed, the plan is cancelled, or the robot strays too far from the path.
+
+`cancel_goal` abandons the plan (and cancels the nav2 goal, if any).
+
+**`get_plan`** (`nav_msgs/srv/GetPlan`) and **`get_plan_nodes`** compute a plan and return it, without following it or publishing anything. `get_plan` answers in the goal's frame; `get_plan_nodes` also takes a node id and returns the node ids along the plan.
+
+Labels, set with `set_label`, are what make goals readable: `set_goal` with `node_label: "kitchen"` rather than an id that changes from one map to the next.
+
+## Diagnostics
+
+`/diagnostics` carries the input and output rates of the synchronizer, as for every [`rtabmap_sync`](../../rtabmap_sync/README.md#library) consumer: a healthy input rate with a low output rate means updates arrive but are dropped — by the rate, or because SLAM takes longer than the period.
+
+In localization mode with `loc_thr` set, a *Localization status* entry says whether the robot is localized: an error with `Not localized!` until a loop closure has placed it, an error with `Localization error is high!` while the localization error — the square root of the largest translational variance — is over `loc_thr` meters, and OK under it. It is only set up when the node **starts** in localization mode.

rtabmap_slam/include/rtabmap_slam/CoreWrapper.h

@@ -121,9 +121,35 @@ class StereoDense;
 
 namespace rtabmap_slam {
 
+/**
+ * @brief The `rtabmap` node: graph SLAM around an rtabmap::Rtabmap instance.
+ *
+ * Registered as the `rtabmap_slam::CoreWrapper` component, and run by the `rtabmap`
+ * executable. The node is always named `rtabmap` unless remapped, and advertises its
+ * services under that name (`/rtabmap/reset`...).
+ *
+ * The input topics come from rtabmap_sync::CommonDataSubscriber, chosen by the
+ * `subscribe_*` parameters; each synchronized update is converted to an
+ * rtabmap::SensorData and processed on a callback group of its own, while asynchronous
+ * inputs (GPS, IMU, landmarks, user data...) are buffered on theirs and attached to the
+ * next update. An update arriving while the previous one is still processed is dropped.
+ *
+ * The graph is published on `mapGraph`, `mapData` and `mapPath`, the assembled maps
+ * through an rtabmap_util::MapsManager, and the correction `map` -> odometry frame on TF.
+ * The database is saved when the node is destroyed.
+ *
+ * See the package README and doc/rtabmap.md for the topics, parameters and services.
+ */
 class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscriber
 {
 public:
+	/**
+	 * @brief Declares the parameters, opens the database and sets up every topic and
+	 *        service.
+	 *
+	 * RTAB-Map parameters are declared as strings under their RTAB-Map names, except the
+	 * odometry ones. Throws if one is given with another type.
+	 */
 	RTABMAP_SLAM_PUBLIC
 	explicit CoreWrapper(const rclcpp::NodeOptions & options);
 	virtual ~CoreWrapper();

rtabmap_slam/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_slam</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's SLAM package.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -34,9 +34,13 @@
   <depend>rtabmap_msgs</depend>
   <depend>rtabmap_util</depend>
   <depend>rtabmap_sync</depend>
-  
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>rtabmap_conversions</test_depend>
+
   <export>
 	<build_type>ament_cmake</build_type>
+	<rosdoc2>rosdoc2.yaml</rosdoc2>
   </export>
 
 </package>

rtabmap_slam/rosdoc2.yaml

@@ -0,0 +1,35 @@
+## Configuration for rosdoc2, the documentation generator used by docs.ros.org.
+## Regenerate the annotated default with:
+##   rosdoc2 default_config --package-path rtabmap_slam
+## Build the docs locally with:
+##   rosdoc2 build --package-path rtabmap_slam --output-directory doc_output
+
+## This 'attic section' self-documents this file's type and version.
+type: 'rosdoc2 config'
+version: 1
+
+---
+
+settings:
+    ## Generate the standard index page from package.xml (description, maintainer,
+    ## license, links) and a table of contents for the builders below.
+    generate_package_index: true
+
+    ## This is an ament_cmake package, so doxygen runs on the public headers by
+    ## default and there are no Python modules to document.
+    always_run_doxygen: false
+    always_run_sphinx_apidoc: false
+
+builders:
+    ## Doxygen parses the public C++ API out of include/.
+    - doxygen: {
+        name: 'rtabmap_slam Public C/C++ API',
+        output_dir: 'generated/doxygen'
+    }
+    ## Sphinx renders the landing page and pulls the Doxygen XML in through
+    ## breathe/exhale so the API is browsable alongside the narrative docs.
+    - sphinx: {
+        name: 'rtabmap_slam',
+        doxygen_xml_directory: 'generated/doxygen/xml',
+        output_dir: ''
+    }

rtabmap_slam/src/CoreNode.cpp

@@ -29,6 +29,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rclcpp/rclcpp.hpp"
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/core/Version.h>
+#ifdef RTABMAP_PYTHON
+#include <rtabmap/core/PythonInterface.h>
+#endif
 
 int main(int argc, char** argv)
 {
@@ -81,6 +85,12 @@ int main(int argc, char** argv)
 		arguments.push_back(argv[i]);
 	}
 
+#ifdef RTABMAP_PYTHON
+	// Initialize the embedded python interpreter on the main thread, as
+	// the nodelet below is loaded in a worker thread.
+	rtabmap::PythonInterface::instance("rtabmap");
+#endif	
+
 	rclcpp::init(argc, argv);
 	rclcpp::NodeOptions options;
 	options.arguments(arguments);