diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index 39b6a390..03d5ee80 100644 --- a/.github/workflows/coverage.yml +++ b/.github/workflows/coverage.yml @@ -60,7 +60,7 @@ jobs: # tested package depends on them (--packages-up-to), just not measured. - uses: ros-tooling/action-ros-ci@v0.4 with: - package-name: rtabmap_conversions rtabmap_util rtabmap_sync rtabmap_python + package-name: rtabmap_conversions rtabmap_util rtabmap_sync rtabmap_odom 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. @@ -120,7 +120,7 @@ jobs: . /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" + PKGS="rtabmap_conversions rtabmap_util rtabmap_sync rtabmap_odom" # 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. diff --git a/.github/workflows/docker-ros2.yml b/.github/workflows/docker-ros2.yml index 6c9f7957..541de0e0 100644 --- a/.github/workflows/docker-ros2.yml +++ b/.github/workflows/docker-ros2.yml @@ -12,56 +12,37 @@ concurrency: cancel-in-progress: ${{ github.event_name == 'pull_request' }} jobs: + # Images built from this tree (docker/*/latest), the ones a change here can break. docker: # 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: ubuntu-latest - + 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] 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 - + steps: - name: Checkout @@ -90,8 +71,73 @@ jobs: context: . push: ${{ github.event_name != 'pull_request' }} platforms: ${{ github.event_name == 'pull_request' && 'linux/amd64' || matrix.docker_platforms }} + # 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 }} no-cache: true cache-to: type=inline + # Images that install a released rtabmap_ros from apt (docker/): 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. + 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 diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 0723b173..6e0cf1f5 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -25,24 +25,26 @@ jobs: include: - ros_distro: humble skip_keys: '' - packages: 'rtabmap_ros rtabmap_conversions' + packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python' - ros_distro: jazzy skip_keys: '' - packages: 'rtabmap_ros rtabmap_conversions' + packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python' - ros_distro: kilted skip_keys: 'grid_map_ros' - packages: 'rtabmap_ros rtabmap_conversions' + packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python' - ros_distro: lyrical - skip_keys: 'nav2_bringup nav2_msgs velodyne grid_map_ros realsense2_camera libpointmatcher' - # rtabmap_costmap_plugins cannot be built, missing nav2 on lyrical, build other packages: - packages: 'rtabmap_launch rtabmap_demos rtabmap_python rtabmap_examples rtabmap_rviz_plugins rtabmap_conversions' + skip_keys: 'velodyne grid_map_ros libpointmatcher' + packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python' - ros_distro: rolling - # libpointmatcher and gtsam are rtabmap deps not yet available on rolling - # (same skip keys as introlab/rtabmap's cmake-ros workflow) - skip_keys: 'nav2_bringup nav2_msgs velodyne libpointmatcher gtsam' + # Rolling has moved to Ubuntu resolute and much of it has not been rebuilt for + # that yet, so rosdep finds nothing to install for these. They are all optional + # to rtabmap, which just builds without them. + skip_keys: 'velodyne libpointmatcher gtsam grid_map_ros' use_ros2_testing: true # Rolling is using ros2-testing (nightly) - # rtabmap_costmap_plugins cannot be built, missing nav2 on rolling, build other packages: - packages: 'rtabmap_launch rtabmap_demos rtabmap_python rtabmap_examples rtabmap_rviz_plugins rtabmap_conversions' + # nav2 is missing from resolute too, and it is not optional: rtabmap_slam and + # rtabmap_costmap_plugins need it to compile at all, and the rtabmap_ros + # metapackage pulls both in. So rolling names the packages it can build. + packages: 'rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python' fail-fast: false container: image: osrf/ros:${{ matrix.ros_distro }}-desktop-full diff --git a/README.md b/README.md index 51bb9c3c..0ede8901 100644 --- a/README.md +++ b/README.md @@ -36,7 +36,7 @@ The stack is split into small packages so a pipeline only pulls in what it uses. | Package | Description | |---|---| | `rtabmap_slam` | The `rtabmap` node itself: appearance-based loop closure detection, graph optimization, memory management and map assembly. | -| `rtabmap_odom` | Odometry nodes — `rgbd_odometry`, `stereo_odometry` and `icp_odometry`. Any external odometry can be used instead. | +| [`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 @@ -100,6 +100,15 @@ 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. diff --git a/codecov.yml b/codecov.yml index 2e315d02..fe2e77e4 100644 --- a/codecov.yml +++ b/codecov.yml @@ -56,6 +56,10 @@ component_management: name: rtabmap_sync paths: - rtabmap_sync/** + - component_id: rtabmap_odom + name: rtabmap_odom + paths: + - rtabmap_odom/** - component_id: rtabmap_python name: rtabmap_python paths: @@ -66,8 +70,8 @@ comment: behavior: default require_changes: true # stay quiet when coverage doesn't move -# Only rtabmap_conversions, rtabmap_util, rtabmap_sync and rtabmap_python have tests -# today, so +# Only rtabmap_conversions, rtabmap_util, rtabmap_sync, rtabmap_odom 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. @@ -77,7 +81,6 @@ ignore: - "rtabmap_examples/**" - "rtabmap_launch/**" - "rtabmap_msgs/**" - - "rtabmap_odom/**" - "rtabmap_rviz_plugins/**" - "rtabmap_slam/**" - "rtabmap_viz/**" diff --git a/docker/humble/latest/Dockerfile b/docker/humble/latest/Dockerfile index fcbb8551..cf80889e 100644 --- a/docker/humble/latest/Dockerfile +++ b/docker/humble/latest/Dockerfile @@ -4,14 +4,32 @@ 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/ + +# Tests run in the build layer, before the workspace is deleted, so none of them reach +# the image. CI asks for them; a target that is not the architecture being built on +# skips them, that one being emulated. Only buildx sets BUILDPLATFORM, so anything +# else building with RUN_TESTS=1 runs them. +ARG RUN_TESTS=0 +ARG BUILDPLATFORM +ARG TARGETPLATFORM + +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" ] && [ "${BUILDPLATFORM:-$TARGETPLATFORM}" = "$TARGETPLATFORM" ]; 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 diff --git a/docker/iron/latest/Dockerfile b/docker/iron/latest/Dockerfile index 3051956f..87b5919b 100644 --- a/docker/iron/latest/Dockerfile +++ b/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 diff --git a/docker/jazzy/latest/Dockerfile b/docker/jazzy/latest/Dockerfile index 62259cc4..95c67b07 100644 --- a/docker/jazzy/latest/Dockerfile +++ b/docker/jazzy/latest/Dockerfile @@ -6,14 +6,32 @@ 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/ && \ + 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/ + +# Tests run in the build layer, before the workspace is deleted, so none of them reach +# the image. CI asks for them; a target that is not the architecture being built on +# skips them, that one being emulated. Only buildx sets BUILDPLATFORM, so anything +# else building with RUN_TESTS=1 runs them. +ARG RUN_TESTS=0 +ARG BUILDPLATFORM +ARG TARGETPLATFORM + +RUN source /ros_entrypoint.sh && \ + cd ros2_ws && \ + export MAKEFLAGS="-j2" && \ 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 && \ + if [ "$RUN_TESTS" = "1" ] && [ "${BUILDPLATFORM:-$TARGETPLATFORM}" = "$TARGETPLATFORM" ]; 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 diff --git a/docker/kilted/latest/Dockerfile b/docker/kilted/latest/Dockerfile index 685cbec1..3190fbc1 100644 --- a/docker/kilted/latest/Dockerfile +++ b/docker/kilted/latest/Dockerfile @@ -6,14 +6,32 @@ 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/ && \ + 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/ + +# Tests run in the build layer, before the workspace is deleted, so none of them reach +# the image. CI asks for them; a target that is not the architecture being built on +# skips them, that one being emulated. Only buildx sets BUILDPLATFORM, so anything +# else building with RUN_TESTS=1 runs them. +ARG RUN_TESTS=0 +ARG BUILDPLATFORM +ARG TARGETPLATFORM + +RUN source /ros_entrypoint.sh && \ + cd ros2_ws && \ + export MAKEFLAGS="-j2" && \ 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 && \ + if [ "$RUN_TESTS" = "1" ] && [ "${BUILDPLATFORM:-$TARGETPLATFORM}" = "$TARGETPLATFORM" ]; 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 diff --git a/docker/lyrical/latest/Dockerfile b/docker/lyrical/latest/Dockerfile index 6517694d..0551179a 100644 --- a/docker/lyrical/latest/Dockerfile +++ b/docker/lyrical/latest/Dockerfile @@ -6,14 +6,32 @@ 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/ + +# Tests run in the build layer, before the workspace is deleted, so none of them reach +# the image. CI asks for them; a target that is not the architecture being built on +# skips them, that one being emulated. Only buildx sets BUILDPLATFORM, so anything +# else building with RUN_TESTS=1 runs them. +ARG RUN_TESTS=0 +ARG BUILDPLATFORM +ARG TARGETPLATFORM + +RUN source /ros_entrypoint.sh && \ + cd ros2_ws && \ + export MAKEFLAGS="-j2" && \ + 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 && \ + if [ "$RUN_TESTS" = "1" ] && [ "${BUILDPLATFORM:-$TARGETPLATFORM}" = "$TARGETPLATFORM" ]; 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 diff --git a/docker/verify_deps.sh b/docker/verify_deps.sh new file mode 100755 index 00000000..d59648c5 --- /dev/null +++ b/docker/verify_deps.sh @@ -0,0 +1,63 @@ +#!/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-.cmake next to ${targets}, every VTK::* target would have no IMPORTED_LOCATION (libvtk-dev is incomplete)" + fi +done + +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 and VTK sysroot look consistent" diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index 7300b3ea..f0db191f 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/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.10 REQUIRED) +find_package(RTABMap 0.23.12 REQUIRED) # libraries SET(Libraries diff --git a/rtabmap_conversions/README.md b/rtabmap_conversions/README.md index 91e04544..2909600e 100644 --- a/rtabmap_conversions/README.md +++ b/rtabmap_conversions/README.md @@ -6,6 +6,13 @@ This package is a library only — it contains no nodes, no launch files and no 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`: @@ -56,14 +63,6 @@ These cut across the whole API and are not obvious from the signatures. Per-func **`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. -## Building and testing - -```bash -colcon build --packages-select rtabmap_conversions -colcon test --packages-select rtabmap_conversions -colcon test-result --verbose -``` - ## License BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license). diff --git a/rtabmap_conversions/include/rtabmap_conversions/PointCloudConversion.h b/rtabmap_conversions/include/rtabmap_conversions/PointCloudConversion.h new file mode 100644 index 00000000..95d898a5 --- /dev/null +++ b/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 + +#include +#include + +/** + * @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 +void toPointCloud2Msg( + const pcl::PointCloud & cloud, sensor_msgs::msg::PointCloud2 & msg) +{ + if(!cloud.empty()) + { + pcl::toROSMsg(cloud, msg); + return; + } + + pcl::PointCloud 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 +void fromPointCloud2Msg( + const sensor_msgs::msg::PointCloud2 & msg, pcl::PointCloud & 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_ */ diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 1f2c4685..9e00d0a1 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.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 #include "rtabmap_conversions/MsgConversion.h" #include @@ -2808,7 +2809,7 @@ bool convertScanMsg( if(hasIntensity) { pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - 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; @@ -2816,7 +2817,7 @@ bool convertScanMsg( else { pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - 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; diff --git a/rtabmap_costmap_plugins/CMakeLists.txt b/rtabmap_costmap_plugins/CMakeLists.txt index d592a057..eab748d7 100644 --- a/rtabmap_costmap_plugins/CMakeLists.txt +++ b/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}) diff --git a/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp b/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp index affc632d..5e1115d1 100644 --- a/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp +++ b/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp @@ -284,6 +284,15 @@ protected: rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector 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 + T declareOrGetParameter(NodeT & node, const std::string & name, const T & defaultValue); + // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; }; diff --git a/rtabmap_costmap_plugins/src/voxel_layer.cpp b/rtabmap_costmap_plugins/src/voxel_layer.cpp index dcb6ca3f..27a99792 100644 --- a/rtabmap_costmap_plugins/src/voxel_layer.cpp +++ b/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 & observation) +{ + return *observation; +} + +} // namespace + +/// nav2 declared a layer's parameters through Layer::declareParameter up to kilted and +/// through the node itself after it. +template +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 observations, clearing_observations; +#else + std::vector 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::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(); - 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 iter_x(*(clearing_observation.cloud_), "x"); - sensor_msgs::PointCloud2ConstIterator iter_y(*(clearing_observation.cloud_), "y"); - sensor_msgs::PointCloud2ConstIterator iter_z(*(clearing_observation.cloud_), "z"); + sensor_msgs::PointCloud2ConstIterator iter_x(clearing_cloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(clearing_cloud, "y"); + sensor_msgs::PointCloud2ConstIterator 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_)); } diff --git a/rtabmap_odom/CMakeLists.txt b/rtabmap_odom/CMakeLists.txt index 7b10ad25..4a1a72e6 100644 --- a/rtabmap_odom/CMakeLists.txt +++ b/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() diff --git a/rtabmap_odom/README.md b/rtabmap_odom/README.md new file mode 100644 index 00000000..9c77e6cf --- /dev/null +++ b/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
odom_frame_id"]) + GUESS(["/odom
guess_frame_id"]) + BASE(["/base_link
frame_id"]) + SENSOR(["/camera or /lidar
the sensor's header.frame_id"]) + ODOM -->|correction, this node
e.g. ~10 Hz, ~50 ms delay| GUESS + GUESS -->|robot driver or robot_localization
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). diff --git a/rtabmap_odom/doc/icp_odometry.md b/rtabmap_odom/doc/icp_odometry.md new file mode 100644 index 00000000..1877614d --- /dev/null +++ b/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
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
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
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. diff --git a/rtabmap_odom/doc/rgbd_odometry.md b/rtabmap_odom/doc/rgbd_odometry.md new file mode 100644 index 00000000..5ba9220f --- /dev/null +++ b/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
subscribe_rgbd:=true"] + MAP["rtabmap
subscribe_rgbd:=true"] + CAM -->|rgb/image
depth/image
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
subscribe_rgbd or subscribe_sensor_data"] + CAM -->|rgb/image
depth/image
rgb/camera_info| ODOM + ODOM -->|odom_rgbd_image
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. diff --git a/rtabmap_odom/doc/stereo_odometry.md b/rtabmap_odom/doc/stereo_odometry.md new file mode 100644 index 00000000..d39a7062 --- /dev/null +++ b/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
right/image_raw
camera_info x2| PROC + PROC -->|left/image_rect
right/image_rect
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
publishing rectified images"] + SYNC["stereo_sync"] + ODOM["stereo_odometry"] + MAP["rtabmap"] + CAM -->|left/image_rect
right/image_rect
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
publishing rectified images"] + ODOM["stereo_odometry"] + MAP["rtabmap
subscribe_rgbd or subscribe_sensor_data"] + CAM -->|left/image_rect
right/image_rect
camera_info x2| ODOM + ODOM -->|odom_rgbd_image
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
Rtabmap/ImagesAlreadyRectified:=false"] + MAP["rtabmap
subscribe_rgbd or subscribe_sensor_data"] + CAM -->|left/image_raw
right/image_raw
camera_info x2| ODOM + ODOM -->|odom_rgbd_image
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. diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index ca0df5bc..b0e8328c 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/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, const std::shared_ptr, std::shared_ptr); + /// `reset_odom_to_pose` service: starts a new map at the pose given in the request. void resetToPose(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + /// `pause_odom` service: keeps the subscriptions but stops registering what arrives. void pause(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + /// `resume_odom` service: registers again, starting from the next frame. void resume(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + /// `log_debug` service: raises RTAB-Map's own log level to debug at runtime. void setLogDebug(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + /// `log_info` service; see setLogDebug(). void setLogInfo(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + /// `log_warning` service; see setLogDebug(). void setLogWarn(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + /// `log_error` service; see setLogDebug(). void setLogError(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + /// 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: diff --git a/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp index dc50b450..589866b1 100644 --- a/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp +++ b/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: diff --git a/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp index 52649daf..b6046da0 100644 --- a/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp +++ b/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp @@ -38,6 +38,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include +#include #include #include @@ -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 & rgbImages, const std::vector & depthImages, - const std::vector& cameraInfos); + const std::vector& cameraInfos, + const std::vector > & localKeyPointsMsgs = {}, + const std::vector > & localPoints3dMsgs = {}, + const std::vector & localDescriptorsMsgs = {}); void callback( const sensor_msgs::msg::Image::ConstSharedPtr image, diff --git a/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp index 5b47c8d0..96cdfbf5 100644 --- a/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp +++ b/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp @@ -43,12 +43,25 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif #include +#include +#include #include #include 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 & leftImages, const std::vector & rightImages, const std::vector& leftCameraInfos, - const std::vector& rightCameraInfos); + const std::vector& rightCameraInfos, + const std::vector > & localKeyPointsMsgs = {}, + const std::vector > & localPoints3dMsgs = {}, + const std::vector & localDescriptorsMsgs = {}); void callback( const sensor_msgs::msg::Image::ConstSharedPtr imageRectLeft, diff --git a/rtabmap_odom/package.xml b/rtabmap_odom/package.xml index 14870d32..292eeec8 100644 --- a/rtabmap_odom/package.xml +++ b/rtabmap_odom/package.xml @@ -30,8 +30,14 @@ rtabmap_util rtabmap_sync + ament_cmake_gtest + + rosbag2_cpp + rosbag2_storage_mcap + ament_cmake + rosdoc2.yaml diff --git a/rtabmap_odom/rosdoc2.yaml b/rtabmap_odom/rosdoc2.yaml new file mode 100644 index 00000000..6e3ef7d9 --- /dev/null +++ b/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: '' + } diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 5a270c75..40ec056f 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/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 #include "rtabmap_odom/OdometryROS.h" #include @@ -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(0,0) = linearVariance; // xx + covariance.at(1,1) = linearVariance; // yy + covariance.at(2,2) = linearVariance; // zz + covariance.at(3,3) = angularVariance; // rr + covariance.at(4,4) = angularVariance; // pp + covariance.at(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), @@ -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"); @@ -628,8 +679,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 +749,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 +777,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 +784,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 +832,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(0,0) = guessLinearVariance_; // xx - info.reg.covariance.at(1,1) = guessLinearVariance_; // yy - info.reg.covariance.at(2,2) = guessLinearVariance_; // zz - info.reg.covariance.at(3,3) = guessAngularVariance_; // rr - info.reg.covariance.at(4,4) = guessAngularVariance_; // pp - info.reg.covariance.at(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 +845,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 +1038,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 +1061,7 @@ void OdometryROS::processData() odom.twist.covariance.at(35) = setTwist?info.reg.covariance.at(5,5):BAD_COVARIANCE; // yawyaw //publish the message - if(setTwist || publishNullWhenLost_) + if(setTwist || publishNullWhenLost_ || publishAsContinuation) { odomPub_->publish(odom); } @@ -953,7 +1083,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 +1106,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 +1126,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 +1140,22 @@ void OdometryROS::processData() if(info.localScanMap.hasNormals() && info.localScanMap.hasIntensity()) { pcl::PointCloud::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::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::Ptr cloud = util3d::laserScanToPointCloudI(info.localScanMap, info.localScanMap.localTransform()); - pcl::toROSMsg(*cloud, cloudMsg); + rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg); } else { pcl::PointCloud::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 +1236,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 +1460,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; diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index 2281e7fb..f67453c8 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/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 #include #include @@ -69,6 +70,7 @@ ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) : ICPOdometry::~ICPOdometry() { + this->join(true); } void ICPOdometry::onOdomInit() @@ -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_) { @@ -668,7 +670,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p if(hasNormals && hasIntensity) { pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - 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::Ptr pclScan(new pcl::PointCloud); - 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::Ptr pclScan(new pcl::PointCloud); - 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::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*cloudMsg, *pclScan); + rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan); if(pclScan->size() && scanDownsamplingStep_ > 1) { pclScan = util3d::downsample(pclScan, scanDownsamplingStep_); diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index 97d48f6e..5f43acab 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/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 +#include #include #include #include @@ -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 & rgbImages, const std::vector & depthImages, - const std::vector& cameraInfos) + const std::vector& cameraInfos, + const std::vector > & localKeyPointsMsgs, + const std::vector > & localPoints3dMsgs, + const std::vector & 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 cameraModels; + std::vector keypoints; + std::vector points3d; + cv::Mat descriptors; for(unsigned int i=0; iencoding.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 > & keyPoints, + std::vector > & points3d, + std::vector & 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 imageMsgs(images->rgbd_images.size()); std::vector depthMsgs(images->rgbd_images.size()); std::vector infoMsgs; + std::vector > localKeyPoints; + std::vector > localPoints3d; + std::vector localDescriptors; for(size_t i=0; irgbd_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 > localKeyPoints; + std::vector > localPoints3d; + std::vector 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 > localKeyPoints; + std::vector > localPoints3d; + std::vector 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 > localKeyPoints; + std::vector > localPoints3d; + std::vector 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 > localKeyPoints; + std::vector > localPoints3d; + std::vector 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 > localKeyPoints; + std::vector > localPoints3d; + std::vector 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 > localKeyPoints; + std::vector > localPoints3d; + std::vector 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); } } diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index f56b2cc3..72cd9cb0 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp @@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include 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 & leftImages, const std::vector & rightImages, const std::vector& leftCameraInfos, - const std::vector& rightCameraInfos) + const std::vector& rightCameraInfos, + const std::vector > & localKeyPointsMsgs, + const std::vector > & localPoints3dMsgs, + const std::vector & 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 cameraModels; + std::vector keypoints; + std::vector points3d; + cv::Mat descriptors; for(unsigned int i=0; iencoding.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 > & keyPoints, + std::vector > & points3d, + std::vector & 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 > localKeyPoints; + std::vector > localPoints3d; + std::vector localDescriptors; + appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors); + + this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors); } } @@ -750,14 +844,18 @@ void StereoOdometry::callbackRGBDX( std::vector rightMsgs(images->rgbd_images.size()); std::vector leftInfoMsgs; std::vector rightInfoMsgs; + std::vector > localKeyPoints; + std::vector > localPoints3d; + std::vector localDescriptors; for(size_t i=0; irgbd_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 > localKeyPoints; + std::vector > localPoints3d; + std::vector 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 > localKeyPoints; + std::vector > localPoints3d; + std::vector 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 > localKeyPoints; + std::vector > localPoints3d; + std::vector 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 > localKeyPoints; + std::vector > localPoints3d; + std::vector 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 > localKeyPoints; + std::vector > localPoints3d; + std::vector 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); } } diff --git a/rtabmap_odom/test/bag_playback.hpp b/rtabmap_odom/test/bag_playback.hpp new file mode 100644 index 00000000..aa636031 --- /dev/null +++ b/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 +#include + +#include +#include + +#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 +std::vector readBagMessages(const std::string & bagPath, const std::string & topic) +{ + std::vector messages; + rosbag2_cpp::Reader reader; + try + { + reader.open(bagPath); + } + catch(const std::exception & e) + { + return messages; + } + + rclcpp::Serialization serialization; + while(reader.has_next()) + { + const std::shared_ptr 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_ */ diff --git a/rtabmap_odom/test/camera_rig.hpp b/rtabmap_odom/test/camera_rig.hpp new file mode 100644 index 00000000..77979a07 --- /dev/null +++ b/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 + +#include + +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#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 localTransforms; + std::vector frameIds; + + /// The world: points in the odometry frame, and one descriptor row per point. + std::vector 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 distXY(-boxXY, boxXY); + std::uniform_real_distribution distZ(-boxZ, boxZ); + std::normal_distribution distDescriptor(0.0f, 1.0f); + + rig.descriptors = cv::Mat(numPoints, kRigDescriptorSize, CV_32FC1); + for(int i=0; i(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 cameraRigTransforms( + const CameraRig & rig, const rclcpp::Time & stamp, + const std::string & baseFrame = "base_link") +{ + std::vector transforms; + for(size_t i=0; i > keyPoints; + std::vector > points; + std::vector 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 models; + std::vector worldToCamera; + for(size_t i=0; i= 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 + `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. diff --git a/rtabmap_odom/test/data/lidar/ouster_half_turn/metadata.yaml b/rtabmap_odom/test/data/lidar/ouster_half_turn/metadata.yaml new file mode 100644 index 00000000..c24a12b9 --- /dev/null +++ b/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 diff --git a/rtabmap_odom/test/data/lidar/ouster_half_turn/ouster_half_turn.mcap b/rtabmap_odom/test/data/lidar/ouster_half_turn/ouster_half_turn.mcap new file mode 100644 index 00000000..7e25ec58 Binary files /dev/null and b/rtabmap_odom/test/data/lidar/ouster_half_turn/ouster_half_turn.mcap differ diff --git a/rtabmap_odom/test/data/rgbd/calib/154.yaml b/rtabmap_odom/test/data/rgbd/calib/154.yaml new file mode 100644 index 00000000..650e8bb7 --- /dev/null +++ b/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 ] diff --git a/rtabmap_odom/test/data/rgbd/calib/17.yaml b/rtabmap_odom/test/data/rgbd/calib/17.yaml new file mode 100644 index 00000000..c635c70a --- /dev/null +++ b/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 ] diff --git a/rtabmap_odom/test/data/rgbd/depth/154.png b/rtabmap_odom/test/data/rgbd/depth/154.png new file mode 100644 index 00000000..8b861747 Binary files /dev/null and b/rtabmap_odom/test/data/rgbd/depth/154.png differ diff --git a/rtabmap_odom/test/data/rgbd/depth/17.png b/rtabmap_odom/test/data/rgbd/depth/17.png new file mode 100644 index 00000000..f950fb18 Binary files /dev/null and b/rtabmap_odom/test/data/rgbd/depth/17.png differ diff --git a/rtabmap_odom/test/data/rgbd/rgb/154.jpg b/rtabmap_odom/test/data/rgbd/rgb/154.jpg new file mode 100644 index 00000000..dec36ba9 Binary files /dev/null and b/rtabmap_odom/test/data/rgbd/rgb/154.jpg differ diff --git a/rtabmap_odom/test/data/rgbd/rgb/17.jpg b/rtabmap_odom/test/data/rgbd/rgb/17.jpg new file mode 100644 index 00000000..45a51a63 Binary files /dev/null and b/rtabmap_odom/test/data/rgbd/rgb/17.jpg differ diff --git a/rtabmap_odom/test/data/stereo/raw/left/420.jpg b/rtabmap_odom/test/data/stereo/raw/left/420.jpg new file mode 100644 index 00000000..1892922a Binary files /dev/null and b/rtabmap_odom/test/data/stereo/raw/left/420.jpg differ diff --git a/rtabmap_odom/test/data/stereo/raw/left/425.jpg b/rtabmap_odom/test/data/stereo/raw/left/425.jpg new file mode 100644 index 00000000..c9c48c53 Binary files /dev/null and b/rtabmap_odom/test/data/stereo/raw/left/425.jpg differ diff --git a/rtabmap_odom/test/data/stereo/raw/right/420.jpg b/rtabmap_odom/test/data/stereo/raw/right/420.jpg new file mode 100644 index 00000000..82350527 Binary files /dev/null and b/rtabmap_odom/test/data/stereo/raw/right/420.jpg differ diff --git a/rtabmap_odom/test/data/stereo/raw/right/425.jpg b/rtabmap_odom/test/data/stereo/raw/right/425.jpg new file mode 100644 index 00000000..ad2de7f0 Binary files /dev/null and b/rtabmap_odom/test/data/stereo/raw/right/425.jpg differ diff --git a/rtabmap_odom/test/data/stereo/raw/stereo_left.yaml b/rtabmap_odom/test/data/stereo/raw/stereo_left.yaml new file mode 100644 index 00000000..0cde8cb7 --- /dev/null +++ b/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. ] diff --git a/rtabmap_odom/test/data/stereo/raw/stereo_pose.yaml b/rtabmap_odom/test/data/stereo/raw/stereo_pose.yaml new file mode 100644 index 00000000..f5ab2be2 --- /dev/null +++ b/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. ] diff --git a/rtabmap_odom/test/data/stereo/raw/stereo_right.yaml b/rtabmap_odom/test/data/stereo/raw/stereo_right.yaml new file mode 100644 index 00000000..f0cbeacc --- /dev/null +++ b/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. ] diff --git a/rtabmap_odom/test/data/stereo/rect/left/50.jpg b/rtabmap_odom/test/data/stereo/rect/left/50.jpg new file mode 100644 index 00000000..93438f3c Binary files /dev/null and b/rtabmap_odom/test/data/stereo/rect/left/50.jpg differ diff --git a/rtabmap_odom/test/data/stereo/rect/left/60.jpg b/rtabmap_odom/test/data/stereo/rect/left/60.jpg new file mode 100644 index 00000000..04630c50 Binary files /dev/null and b/rtabmap_odom/test/data/stereo/rect/left/60.jpg differ diff --git a/rtabmap_odom/test/data/stereo/rect/right/50.jpg b/rtabmap_odom/test/data/stereo/rect/right/50.jpg new file mode 100644 index 00000000..f81336dc Binary files /dev/null and b/rtabmap_odom/test/data/stereo/rect/right/50.jpg differ diff --git a/rtabmap_odom/test/data/stereo/rect/right/60.jpg b/rtabmap_odom/test/data/stereo/rect/right/60.jpg new file mode 100644 index 00000000..3b10e301 Binary files /dev/null and b/rtabmap_odom/test/data/stereo/rect/right/60.jpg differ diff --git a/rtabmap_odom/test/data/stereo/rect/stereo_left.yaml b/rtabmap_odom/test/data/stereo/rect/stereo_left.yaml new file mode 100644 index 00000000..ecd65446 --- /dev/null +++ b/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. ] diff --git a/rtabmap_odom/test/data/stereo/rect/stereo_right.yaml b/rtabmap_odom/test/data/stereo/rect/stereo_right.yaml new file mode 100644 index 00000000..7fc7708a --- /dev/null +++ b/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. ] diff --git a/rtabmap_odom/test/msg_builders.hpp b/rtabmap_odom/test/msg_builders.hpp new file mode 100644 index 00000000..b6c0d5ac --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#ifdef PRE_ROS_IRON +#include +#else +#include +#endif + +#include +#include + +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 & 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(&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 & 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 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(&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 points; + points.reserve(count); + for(size_t i=0; i(base + xOffset), + *reinterpret_cast(base + yOffset), + *reinterpret_cast(base + zOffset)); +} + +} // namespace rtabmap_odom_test + +#endif /* RTABMAP_ODOM_MSG_BUILDERS_HPP_ */ diff --git a/rtabmap_odom/test/node_test_utils.hpp b/rtabmap_odom/test/node_test_utils.hpp new file mode 100644 index 00000000..ee9152ef --- /dev/null +++ b/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 + +#include + +#include +#include +#include +#include +#include +#include + +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(); + helper_ = std::make_shared("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 + std::shared_ptr addNode(const std::shared_ptr & 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 & 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 + 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 + 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 + struct Collector + { + typename rclcpp::Subscription::SharedPtr subscription; + std::vector 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 + std::shared_ptr> collect( + const std::string & topic, const rclcpp::QoS & qos = rclcpp::QoS(10)) + { + std::shared_ptr> collector = std::make_shared>(); + std::weak_ptr> weak = collector; + collector->subscription = helper_->create_subscription( + topic, qos, + [weak](const typename MsgT::ConstSharedPtr msg) { + if(std::shared_ptr> collector = weak.lock()) + { + collector->messages.push_back(msg); + } + }); + return collector; + } + +private: + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + rclcpp::Node::SharedPtr helper_; + std::vector nodes_; +}; + +} // namespace rtabmap_odom_test + +#endif /* RTABMAP_ODOM_NODE_TEST_UTILS_HPP_ */ diff --git a/rtabmap_odom/test/scan_scenes.hpp b/rtabmap_odom/test/scan_scenes.hpp new file mode 100644 index 00000000..ec67a00c --- /dev/null +++ b/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 + +#include + +#include + +#include + +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 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 points; + points.reserve(3 * pointsPerSurface); + for(int i=0; i corner3DTurned(double yaw) +{ + const double c = std::cos(-yaw); + const double s = std::sin(-yaw); + std::vector 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 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_ */ diff --git a/rtabmap_odom/test/test_data.hpp b/rtabmap_odom/test/test_data.hpp new file mode 100644 index 00000000..f2fece3b --- /dev/null +++ b/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 +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#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 readCalibrationMatrix( + cv::FileStorage & fs, const std::string & name, int rows, int cols) +{ + const cv::FileNode node = fs[name]; + if(node.empty()) + { + return std::vector(); + } + std::vector data; + node["data"] >> data; + if((int)node["rows"] != rows || (int)node["cols"] != cols || + data.size() != size_t(rows * cols)) + { + return std::vector(); + } + 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 k = readCalibrationMatrix(fs, "camera_matrix", 3, 3); + const std::vector r = readCalibrationMatrix(fs, "rectification_matrix", 3, 3); + const std::vector p = readCalibrationMatrix(fs, "projection_matrix", 3, 4); + const std::vector d = readCalibrationMatrix(fs, "distortion_coefficients", 1, 5); + + info.d.assign(5, 0.0); + for(size_t i=0; i 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 r = readCalibrationMatrix(fs, "rotation_matrix", 3, 3); + const std::vector 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_ */ diff --git a/rtabmap_odom/test/test_icp_odometry.cpp b/rtabmap_odom/test/test_icp_odometry.cpp new file mode 100644 index 00000000..eeac9099 --- /dev/null +++ b/rtabmap_odom/test/test_icp_odometry.cpp @@ -0,0 +1,2103 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include + +#include + + +#include +#include + +#include + +#include + +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "bag_playback.hpp" +#include "msg_builders.hpp" +#include "scan_scenes.hpp" +#include "node_test_utils.hpp" + +namespace rtabmap_odom_test { + +namespace { + +::testing::Environment * const kRclcppEnv = registerRclcppEnvironment(); + +/// 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; +} + +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 rotation carried by a geometry_msgs quaternion, in radians. +double rotationAngleOf(const geometry_msgs::msg::Quaternion & q) +{ + return 2.0 * std::acos(std::min(1.0, std::fabs(q.w))); +} + +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))); +} + +/** + * @brief The 2D corner seen from (@p x, @p y), every ray taken at the same instant. + * + * The skewed scans further down bend the walls with the robot's motion; this one does + * not, so it is what a sweep taken from a standstill looks like. @p withIntensities adds + * the channel a real lidar reports alongside the range, which is what decides whether the + * node carries the scan as PointXYZI or as PointXYZ. + */ +sensor_msgs::msg::LaserScan cornerScan( + double stamp, double x = 0.0, double y = 0.0, bool withIntensities = false, + float sweep = 0.0f) +{ + sensor_msgs::msg::LaserScan scan; + scan.header.frame_id = "lidar"; + scan.header.stamp = stampOf(stamp); + scan.angle_min = -1.0f; + scan.angle_max = 1.0f; + scan.angle_increment = 0.01f; + scan.scan_time = 0.1f; + scan.range_min = 0.1f; + scan.range_max = 30.0f; + const size_t rays = size_t((scan.angle_max - scan.angle_min) / scan.angle_increment) + 1; + // Zero unless the caller wants the per-ray stamps deskewing needs. + scan.time_increment = rays > 1 ? sweep / float(rays - 1) : 0.0f; + scan.ranges.resize(rays); + for(size_t i=0; i fieldValues( + const sensor_msgs::msg::PointCloud2 & cloud, const std::string & field) +{ + std::vector values; + for(const sensor_msgs::msg::PointField & f : cloud.fields) + { + if(f.name == field && f.datatype == sensor_msgs::msg::PointField::FLOAT32) + { + for(size_t i=0; i & values) +{ + return size_t(std::count_if(values.begin(), values.end(), + [](float value) { return value > 0.0f; })); +} + +/** + * @brief The share of @p cloud's points that still sit within @p maxDistance of @p reference. + * + * Deskewing moves points along the sweep rather than adding or removing them, and + * voxelizing the result renumbers whatever is left, so the two clouds cannot be compared + * index by index. This is RTAB-Map's own correspondence count, which does not care about + * the ordering: 1.0 means the two clouds are on top of each other, and anything less is + * how much of one moved away from the other. + */ +double correspondenceRatio( + const sensor_msgs::msg::PointCloud2 & cloud, + const sensor_msgs::msg::PointCloud2 & reference, + double maxDistance) +{ + pcl::PointCloud::Ptr source(new pcl::PointCloud); + pcl::PointCloud::Ptr target(new pcl::PointCloud); + rtabmap_conversions::fromPointCloud2Msg(cloud, *source); + rtabmap_conversions::fromPointCloud2Msg(reference, *target); + if(source->empty() || target->empty()) + { + return 0.0; + } + + double variance = 0.0; + int correspondences = 0; + rtabmap::util3d::computeVarianceAndCorrespondences( + source, target, maxDistance, variance, correspondences, false); + return double(correspondences) / double(source->size()); +} + +class IcpOdometryTest : public NodeTest +{ +protected: + /// The sensor has to be connected to frame_id in TF before the first frame arrives. + void publishSensorTf(const std::string & sensorFrame = "lidar") + { + staticTf_ = std::make_shared(*helper()); + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = helper()->now(); + tf.header.frame_id = "base_link"; + tf.child_frame_id = sensorFrame; + tf.transform.rotation.w = 1.0; + staticTf_->sendTransform(tf); + } + + /// Calls an Empty service the node advertises under its own name. + bool callEmptyService(const std::string & name) + { + rclcpp::Client::SharedPtr client = + helper()->create_client("/icp_odometry/" + name); + if(!spinUntil([&]() { return client->service_is_ready(); })) + { + return false; + } + std::shared_future future = + client->async_send_request( + std::make_shared()).future.share(); + return spinUntil([&]() { + return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; }); + } + + std::shared_ptr makeNode( + std::vector params = {}, const std::string & name = "") + { + // 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 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); + if(!name.empty()) + { + // A test that runs two nodes at once has to keep their names and their odom + // topics apart, or they publish over each other. + options.arguments({"--ros-args", "-r", "__node:=" + name, + "-r", "odom:=odom_" + name, + "-r", "odom_info:=odom_info_" + name, + "-r", "odom_sensor_data/raw:=odom_sensor_data_" + name + "/raw", + "-r", "odom_filtered_input_scan:=odom_filtered_input_scan_" + name}); + } + return addNode(std::make_shared(options)); + } + + /** + * @brief Publishes the recorded TF history, then hands the clouds over one at a time. + * + * All of TF goes out first, so every lookup the node makes is already in the buffer: + * the recording covers each sweep from end to end (see test/data/README.md), and + * replaying it up front removes any race between TF arriving and a cloud being + * processed. The clouds keep their recorded stamps and frame -- os_sensor, which TF + * ties back to base_link through the rig's rotating joint. + */ + + // ----------------------------------------------------------------------- + // A 2D lidar on a robot driving at a corner, for the LaserScan deskewing path. + // ----------------------------------------------------------------------- + + static constexpr double kScanSweep = 0.1; ///< first ray to last, seconds + static constexpr double kFirstScan = 1.0; ///< stamp of the first scan + static constexpr double kSecondScan = 1.5; ///< stamp of the second + + /** + * @brief Where the robot is at time @p t, in odom. + * + * It drives at 1 m/s through the first sweep and the gap after it, then stops before + * the second. That difference is the whole point: at a constant speed both sweeps bend + * by the same amount and even an unskewed registration lands in the right place, so + * the bug would hide. Braking makes the first scan bent and the second straight. + */ + static double robotX(double t) + { + const double cruise = kSecondScan - kScanSweep; // stops one sweep early + return t <= kFirstScan ? 0.0 + : (t < cruise ? (t - kFirstScan) : (cruise - kFirstScan)); + } + + /// What the odometry should report between the two scan stamps. + static double trueDisplacement() + { + return robotX(kSecondScan) - robotX(kFirstScan); + } + + /** + * @brief One scan of the corner, skewed by the robot's motion during the sweep. + * + * Each ray is cast from where the sensor actually was when that ray was taken, which + * is what a real lidar does and what makes the wall come out bent. + */ + sensor_msgs::msg::LaserScan makeSkewedCornerScan(double stamp) + { + sensor_msgs::msg::LaserScan scan; + scan.header.frame_id = "lidar"; + scan.header.stamp = stampOf(stamp); + scan.angle_min = -1.0f; + scan.angle_max = 1.0f; + scan.angle_increment = 0.01f; + scan.range_min = 0.1f; + scan.range_max = 30.0f; + const size_t rays = size_t((scan.angle_max - scan.angle_min) / scan.angle_increment) + 1; + scan.time_increment = float(kScanSweep / double(rays - 1)); + scan.scan_time = float(kScanSweep); + scan.ranges.resize(rays); + for(size_t i=0; i base_link along that trajectory, plus base_link -> lidar. + * + * Sampled at 100 Hz across both sweeps: laser_geometry interpolates between whatever + * TF holds, and the correction is only as good as the trajectory it can see. + */ + bool publishRobotTrajectory() + { + staticTf_ = std::make_shared(*helper()); + geometry_msgs::msg::TransformStamped sensor; + sensor.header.stamp = helper()->now(); + sensor.header.frame_id = "base_link"; + sensor.child_frame_id = "lidar"; + sensor.transform.rotation.w = 1.0; + staticTf_->sendTransform(sensor); + + rclcpp::Publisher::SharedPtr tf = + helper()->create_publisher("/tf", rclcpp::QoS(200)); + std::shared_ptr> echo = + collect("/tf", rclcpp::QoS(200)); + if(!waitForSubscriber(tf, 2)) + { + return false; + } + size_t published = 0; + for(double t = kFirstScan - 0.1; t <= kSecondScan + kScanSweep + 0.1; t += 0.01) + { + geometry_msgs::msg::TransformStamped pose; + pose.header.stamp = stampOf(t); + pose.header.frame_id = "odom"; + pose.child_frame_id = "base_link"; + pose.transform.translation.x = robotX(t); + pose.transform.rotation.w = 1.0; + tf2_msgs::msg::TFMessage message; + message.transforms.push_back(pose); + tf->publish(message); + if(++published % 10 == 0) + { + spinFor(std::chrono::milliseconds(5)); + } + } + tfPublisher_ = tf; + return spinUntil([&]() { return echo->size() >= published; }); + } + + struct Recording + { + std::vector staticTransforms; + std::vector transforms; + std::vector clouds; + bool valid() const + { + return !staticTransforms.empty() && !transforms.empty() && clouds.size() >= 2; + } + }; + + /** + * What the sensor turns between the two scans, whoever predicts it. + * + * 60 degrees is chosen: large enough that neither ICP backend finds it from an + * identity start -- both settle within 0.03 rad of no motion at all -- and clear of + * the corner scene's own 90 degree symmetry, where a wall matched onto the next wall + * would be a second, equally good answer. + */ + static constexpr double kPredictedTurn = 1.05; + /// Where the IMU's heading starts; see publishImuTurn() for why it is not zero. + static constexpr double kImuHeading = 0.2; + + /// base_link -> imu_link, which the IMU callback requires before it accepts anything. + void publishImuTf() + { + imuTf_ = std::make_shared(*helper()); + geometry_msgs::msg::TransformStamped sensor; + sensor.header.stamp = helper()->now(); + sensor.header.frame_id = "base_link"; + sensor.child_frame_id = "imu_link"; + sensor.transform.rotation.w = 1.0; + imuTf_->sendTransform(sensor); + } + + /// One IMU sample, heading @p yaw about z. + sensor_msgs::msg::Imu imuSample(double stamp, double yaw) + { + sensor_msgs::msg::Imu sample; + sample.header.frame_id = "imu_link"; + sample.header.stamp = stampOf(stamp); + sample.orientation.z = std::sin(yaw / 2.0); + sample.orientation.w = std::cos(yaw / 2.0); + return sample; + } + + /** + * @brief Publishes an IMU turning by kPredictedTurn between @p stamp and @p stamp + 0.1. + * + * The heading starts away from zero on purpose: RTAB-Map reads an orientation whose x, + * y and z are all zero as "not set" and ignores the sample, so an IMU sitting at + * exactly identity would leave the odometry with nothing to difference against and the + * test would pass while exercising nothing. + * + * The whole history goes out before any scan does: with wait_imu_to_init a frame is + * held back until an IMU sample at or after its stamp has arrived, and dropped when + * the next frame arrives without one. + */ + bool publishImuTurn(double stamp, double keepTurningTo = 0.0) + { + imuTf_ = std::make_shared(*helper()); + geometry_msgs::msg::TransformStamped sensor; + sensor.header.stamp = helper()->now(); + sensor.header.frame_id = "base_link"; + sensor.child_frame_id = "imu_link"; + sensor.transform.rotation.w = 1.0; + imuTf_->sendTransform(sensor); + + rclcpp::Publisher::SharedPtr imu = + helper()->create_publisher("imu", rclcpp::QoS(200)); + if(!waitForSubscriber(imu)) + { + return false; + } + const double last = keepTurningTo > 0.0 ? stamp + 0.3 : stamp + 0.15; + for(double t = stamp - 0.1; t <= last; t += 0.01) + { + double turn = t <= stamp ? 0.0 + : (t >= stamp + 0.1 ? kPredictedTurn : kPredictedTurn * (t - stamp) / 0.1); + if(keepTurningTo > 0.0 && t > stamp + 0.1) + { + // Carries on turning after the second frame's stamp, so that a guess built + // from the newest sample instead of the one at the stamp would show it. + const double past = std::min(1.0, (t - stamp - 0.1) / 0.2); + turn = kPredictedTurn + (keepTurningTo - kPredictedTurn) * past; + } + const double turned = kImuHeading + turn; + sensor_msgs::msg::Imu sample; + sample.header.frame_id = "imu_link"; + sample.header.stamp = stampOf(t); + sample.orientation.z = std::sin(turned / 2.0); + sample.orientation.w = std::cos(turned / 2.0); + imu->publish(sample); + } + imuPublisher_ = imu; + spinFor(std::chrono::milliseconds(200)); + return true; + } + + /** + * @brief Publishes wheel_odom -> base_link turning by kPredictedTurn, the same motion the + * IMU reports in publishImuTurn(). + * + * This is the other way to hand the odometry a prediction: a pose source in TF rather + * than an orientation on a topic. The node differences it between consecutive scan + * stamps and passes the result to ICP as the guess. + */ + bool publishGuessTurn(double stamp) + { + rclcpp::Publisher::SharedPtr tf = + helper()->create_publisher("/tf", rclcpp::QoS(200)); + std::shared_ptr> echo = + collect("/tf", rclcpp::QoS(200)); + if(!waitForSubscriber(tf, 2)) + { + return false; + } + size_t published = 0; + for(double t = stamp - 0.1; t <= stamp + 0.15; t += 0.01) + { + const double turned = t <= stamp ? 0.0 + : (t >= stamp + 0.1 ? kPredictedTurn : kPredictedTurn * (t - stamp) / 0.1); + geometry_msgs::msg::TransformStamped pose; + pose.header.stamp = stampOf(t); + pose.header.frame_id = "wheel_odom"; + pose.child_frame_id = "base_link"; + pose.transform.rotation.z = std::sin(turned / 2.0); + pose.transform.rotation.w = std::cos(turned / 2.0); + tf2_msgs::msg::TFMessage message; + message.transforms.push_back(pose); + tf->publish(message); + if(++published % 10 == 0) + { + spinFor(std::chrono::milliseconds(5)); + } + } + tfPublisher_ = tf; + return spinUntil([&]() { return echo->size() >= published; }); + } + + Recording readOusterRecording() + { + Recording recording; + recording.staticTransforms = + readBagMessages(ousterHalfTurnBag(), "/tf_static"); + recording.transforms = + readBagMessages(ousterHalfTurnBag(), "/tf"); + recording.clouds = readBagMessages( + ousterHalfTurnBag(), "/os_cloud_node/points"); + return recording; + } + + /** + * @brief Replays the recorded TF history, after the node under test exists. + * + * Order matters: /tf is a volatile topic, so transforms published before the node's + * listener has subscribed are simply dropped and every lookup then fails with "TF of + * received scan cloud is not set". /tf_static survives that (it is transient-local) + * which makes the mistake look like a half-working tree rather than an empty one. + * + * @return false if the node never subscribed + */ + bool publishRecordedTf(const Recording & recording) + { + staticBroadcaster_ = std::make_shared(*helper()); + for(const tf2_msgs::msg::TFMessage & message : recording.staticTransforms) + { + staticBroadcaster_->sendTransform(message.transforms); + } + tfPublisher_ = helper()->create_publisher( + "/tf", rclcpp::QoS(200)); + // Subscribe to the same topic, so delivery can be waited on rather than guessed + // at: a fixed pause is enough on an idle machine and not enough on a loaded one, + // and a cloud that arrives before the transforms is refused outright. + std::shared_ptr> echo = + collect("/tf", rclcpp::QoS(200)); + if(!waitForSubscriber(tfPublisher_, 2)) // the node's listener, and this echo + { + return false; + } + // In chunks, spinning in between, rather than all at once: tf2's listener reads + // /tf on its own thread with a bounded queue, and a burst of a hundred messages + // overflows it on a machine that cannot drain them -- dropping the oldest, which + // are exactly the ones covering the first cloud. The whole history still goes out + // before any cloud does. + size_t published = 0; + for(const tf2_msgs::msg::TFMessage & message : recording.transforms) + { + tfPublisher_->publish(message); + if(++published % 10 == 0) + { + spinFor(std::chrono::milliseconds(10)); + } + } + return spinUntil([&]() { return echo->size() >= recording.transforms.size(); }); + } + +private: + std::shared_ptr staticTf_; + std::shared_ptr staticBroadcaster_; + std::shared_ptr imuTf_; + rclcpp::Publisher::SharedPtr imuPublisher_; + rclcpp::Publisher::SharedPtr tfPublisher_; +}; + +/** + * The first scan initializes odometry rather than registering anything: the pose is the + * identity and the covariance is RTAB-Map's "not estimated" value, not a real one. + */ +/** + * The first scan initializes odometry rather than registering anything: the pose is the + * identity, and it is the frame every later pose is relative to. + */ +TEST_F(IcpOdometryTest, publishes_an_identity_pose_for_the_first_scan_cloud) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode(icpTestParameters()); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(makeXYZCloud("lidar", 1.0, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })); + + const nav_msgs::msg::Odometry & msg = odom->back(); + EXPECT_EQ("odom", msg.header.frame_id); + EXPECT_EQ("base_link", msg.child_frame_id); + EXPECT_NEAR(0.0, msg.pose.pose.position.x, 1e-6); + EXPECT_NEAR(0.0, msg.pose.pose.position.y, 1e-6); + EXPECT_NEAR(0.0, msg.pose.pose.position.z, 1e-6); +} + +/// A 2D lidar goes in on `scan` instead of `scan_cloud`, and reaches the same odometry. +TEST_F(IcpOdometryTest, accepts_a_laser_scan) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode(icpTestParameters()); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(makeLaserScan("lidar", 1.0)); + EXPECT_TRUE(spinUntil([&]() { return !odom->empty(); })); +} + +/** + * The point of the node: a second scan taken from a known offset comes back as that + * offset in the published odometry. + * + * The motion matches the one RTAB-Map's own Icp3DCornerRecoversMotionWithoutGuess uses -- + * about 12 cm spread over three axes. Size matters here: a step much larger than + * Icp/MaxCorrespondenceDistance (0.1 m by default) leaves ICP with nothing to associate + * and it recovers nothing at all, which is the behaviour described under "When it loses + * track" in doc/icp_odometry.md. + * + * The scene is a corner, so the motion is fully constrained -- see "Degenerate geometry" + * for the environments where it is not. + */ +TEST_F(IcpOdometryTest, recovers_a_known_motion_between_two_scans) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode(icpTestParameters()); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(makeXYZCloud("lidar", 1.0, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 1; })); + + // The robot moved by this much, so the corner is seen that much nearer. + const cv::Point3f motion(0.10f, 0.06f, 0.04f); + pub->publish(makeXYZCloud("lidar", 1.1, corner3D(motion))); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; })); + + const nav_msgs::msg::Odometry & msg = odom->back(); + EXPECT_NEAR(motion.x, msg.pose.pose.position.x, 0.01); + EXPECT_NEAR(motion.y, msg.pose.pose.position.y, 0.01); + EXPECT_NEAR(motion.z, msg.pose.pose.position.z, 0.01); +} + +/// Two steps in a row accumulate, rather than each being reported relative to the last. +TEST_F(IcpOdometryTest, integrates_successive_motions_into_a_pose) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode(icpTestParameters()); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + for(int i=0; i<3; ++i) + { + pub->publish(makeXYZCloud("lidar", 1.0 + 0.1*i, corner3D(cv::Point3f(0.05f*i, 0, 0)))); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); })); + } + + // Three frames at 0, 0.05 and 0.10 m: the pose is the total, not the last step. + EXPECT_NEAR(0.10, odom->back().pose.pose.position.x, 0.01); +} + +/** + * The scan filters default to RTAB-Map's own Icp parameter values rather than to the zeros the + * source's member initializers suggest. See "Where these defaults come from" in the doc. + */ +TEST_F(IcpOdometryTest, scan_filters_default_to_the_icp_parameter_values) +{ + publishSensorTf(); + std::shared_ptr node = makeNode(); + + EXPECT_NEAR(0.05, node->get_parameter("scan_voxel_size").as_double(), 1e-6); + EXPECT_EQ(5, node->get_parameter("scan_normal_k").as_int()); +} + +/// Setting the ROS parameter explicitly takes precedence over the Icp/* value. +TEST_F(IcpOdometryTest, an_explicit_scan_voxel_size_wins_over_the_icp_parameter) +{ + publishSensorTf(); + std::shared_ptr node = + makeNode({rclcpp::Parameter("scan_voxel_size", 0.25)}); + + EXPECT_NEAR(0.25, node->get_parameter("scan_voxel_size").as_double(), 1e-6); +} + +/// odom_info carries the registration result, and is only built when something subscribes. +TEST_F(IcpOdometryTest, publishes_odom_info_describing_the_registration) +{ + publishSensorTf(); + std::shared_ptr> info = + collect("odom_info"); + makeNode(icpTestParameters()); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(info->subscription)); + + pub->publish(makeXYZCloud("lidar", 1.0, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return !info->empty(); })); + pub->publish(makeXYZCloud("lidar", 1.1, corner3D(cv::Point3f(0.1f, 0.0f, 0.0f)))); + ASSERT_TRUE(spinUntil([&]() { return info->size() >= 2; })); + + // The second frame registered against the first, so the scan map is populated and + // correspondences were found. + const rtabmap_msgs::msg::OdomInfo & msg = *info->messages[1]; + EXPECT_FALSE(msg.lost); + EXPECT_GT(msg.local_scan_map_size, 0); + EXPECT_GT(msg.icp_correspondences, 0); +} + + +// --------------------------------------------------------------------------- +// Deskewing, against a real rotating lidar: test/data/lidar/ouster_pair. +// +// The Ouster sits on a mast that turns on a dynamixel joint while the base stays put, so +// the sensor moves through its own 0.1 s sweep and the cloud comes off the driver skewed +// -- points recorded early in the sweep are expressed in a pose the sensor has already +// left. Deskewing undoes that from TF, and doing it wrong is invisible in a synthetic +// scene where the sensor is motionless within a sweep. +// +// The recording carries the per-point `t` field deskewing needs, plus TF from 0.1 s +// before the first sweep to past the end of the last one. +// --------------------------------------------------------------------------- + +/// The fixture is worthless if the recording is not there, so say so plainly. +TEST_F(IcpOdometryTest, the_recorded_lidar_pair_is_readable) +{ + const std::vector clouds = + readBagMessages( + ousterHalfTurnBag(), "/os_cloud_node/points"); + + ASSERT_EQ(2u, clouds.size()) << "expected two clouds in " << ousterHalfTurnBag(); + EXPECT_EQ("os_sensor", clouds[0].header.frame_id); + EXPECT_EQ(1024u, clouds[0].width); + EXPECT_EQ(32u, clouds[0].height); + + // Deskewing needs a per-point time offset; without this field it refuses the cloud. + bool hasTime = false; + for(const sensor_msgs::msg::PointField & field : clouds[0].fields) + { + hasTime = hasTime || field.name == "t"; + } + EXPECT_TRUE(hasTime) << "the cloud has no per-point t field to deskew with"; +} + +/** + * deskewing:=true with a fixed frame: the node corrects each sweep against TF before + * registering it, and both clouds come back as odometry in base_link. + */ +TEST_F(IcpOdometryTest, deskews_a_rotating_lidar_sweep_against_tf) +{ + std::shared_ptr> odom = + collect("odom_deskewed"); + const Recording recording = readOusterRecording(); + ASSERT_TRUE(recording.valid()) << "could not read " << ousterHalfTurnBag(); + + // guess_frame_id is what selects the TF path: with only two clouds there is no + // velocity estimate yet, so the constant-velocity fallback would never run. base_link + // is the rig's root -- the mast turns relative to it, which is the motion to undo. + makeNode({rclcpp::Parameter("deskewing", true), + rclcpp::Parameter("guess_frame_id", "base_link"), + rclcpp::Parameter("scan_cloud_max_points", 65536), + // The room is metres across and the sweeps start half a turn apart. + rclcpp::Parameter("scan_voxel_size", 0.2), + rclcpp::Parameter("Icp/MaxCorrespondenceDistance", "2.0"), + // Pinned: a build without libpointmatcher defaults it to false. + rclcpp::Parameter("Icp/PointToPlane", "true"), + // Raised from 0.2 m, which the uncorrected run walks past and is refused for. + rclcpp::Parameter("Icp/MaxTranslation", "0.5"), + // The recorded transforms may still be arriving when the first cloud lands. + rclcpp::Parameter("wait_for_transform", 2.0)}, + "deskewed"); + ASSERT_TRUE(publishRecordedTf(recording)); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(recording.clouds[0]); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })) + << "the first deskewed sweep produced no odometry"; + pub->publish(recording.clouds[1]); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; })) + << "the second deskewed sweep produced no odometry"; + + EXPECT_EQ("odom", odom->back().header.frame_id); + EXPECT_EQ("base_link", odom->back().child_frame_id); + ASSERT_FALSE(isLost(odom->back())) << "lost tracking between the two sweeps"; + + // The ground truth is known: the platform never moves in this recording, only the + // mast turns, so base_link is where it started and the pose should be the identity. + // What is left after deskewing half a turn of rotation out of two sweeps is 0.013 m + // and 0.012 rad with libpointmatcher, 0.09 m and 0.015 rad with PCL -- point to + // plane either way, which is why the parameter above is not left to its default. + EXPECT_LT(translationNorm(odom->back()), 0.15) + << "the platform never moved; this is too far from the origin"; + EXPECT_LT(rotationAngle(odom->back()), 0.05) + << "the platform never turned; this is too far from the origin"; +} + +/** + * The same two sweeps with and without deskewing, registered side by side. + * + * The platform never moved, so the answer is known: the identity. Corrected, the pair + * lands 0.013 m and 0.012 rad from it with libpointmatcher and 0.09 m with PCL. + * Uncorrected, it still registers -- on nearly as many points -- but arrives several + * times further out. That is the shape of a deskewing bug in the field: not a failure, a + * quietly worse answer. + * + * The correction itself is measured too, on the scan each node republishes on + * odom_filtered_input_scan. That one does not depend on the backend at all, so it is the + * assertion that catches a deskewing step that quietly stopped working. + * + * At RTAB-Map's default Icp/MaxTranslation of 0.2 m the uncorrected run does not even get + * that far: libpointmatcher aborts with "limit out of bounds: tr 0.214016/0.2" and the + * pose comes back unusable. The limit is raised here so that both runs produce a number + * to compare, which says more than one of them failing. + */ +TEST_F(IcpOdometryTest, deskewing_is_what_lets_a_half_turn_pair_register) +{ + std::shared_ptr> deskewed = + collect("odom_deskewed"); + std::shared_ptr> asRecorded = + collect("odom_as_recorded"); + std::shared_ptr> deskewedInfo = + collect("odom_info_deskewed"); + std::shared_ptr> asRecordedInfo = + collect("odom_info_as_recorded"); + std::shared_ptr> deskewedScan = + collect("odom_filtered_input_scan_deskewed"); + std::shared_ptr> asRecordedScan = + collect("odom_filtered_input_scan_as_recorded"); + const Recording recording = readOusterRecording(); + ASSERT_TRUE(recording.valid()) << "could not read " << ousterHalfTurnBag(); + + // Identical in every respect but the one parameter. + makeNode({rclcpp::Parameter("deskewing", true), + rclcpp::Parameter("guess_frame_id", "base_link"), + rclcpp::Parameter("scan_cloud_max_points", 65536), + // The room is metres across and the sweeps start half a turn apart. + rclcpp::Parameter("scan_voxel_size", 0.2), + rclcpp::Parameter("Icp/MaxCorrespondenceDistance", "2.0"), + // Pinned: a build without libpointmatcher defaults it to false. + rclcpp::Parameter("Icp/PointToPlane", "true"), + // Raised from 0.2 m, which the uncorrected run walks past and is refused for. + rclcpp::Parameter("Icp/MaxTranslation", "0.5"), + // The recorded transforms may still be arriving when the first cloud lands. + rclcpp::Parameter("wait_for_transform", 2.0)}, + "deskewed"); + makeNode({rclcpp::Parameter("deskewing", false), + rclcpp::Parameter("guess_frame_id", "base_link"), + rclcpp::Parameter("scan_cloud_max_points", 65536), + rclcpp::Parameter("scan_voxel_size", 0.2), + rclcpp::Parameter("Icp/MaxCorrespondenceDistance", "2.0"), + rclcpp::Parameter("Icp/PointToPlane", "true"), + rclcpp::Parameter("Icp/MaxTranslation", "0.5"), + rclcpp::Parameter("wait_for_transform", 2.0)}, + "as_recorded"); + ASSERT_TRUE(publishRecordedTf(recording)); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub, 2)); + ASSERT_TRUE(waitForPublisher(deskewedScan->subscription)); + ASSERT_TRUE(waitForPublisher(asRecordedScan->subscription)); + + pub->publish(recording.clouds[0]); + ASSERT_TRUE(spinUntil([&]() { return !deskewed->empty() && !asRecorded->empty(); })); + pub->publish(recording.clouds[1]); + ASSERT_TRUE(spinUntil([&]() { + return deskewed->size() >= 2 && asRecorded->size() >= 2 && + deskewedInfo->size() >= 2 && asRecordedInfo->size() >= 2; })); + + ASSERT_FALSE(isLost(deskewed->back())) << "the deskewed pair failed to register"; + EXPECT_GT(deskewedInfo->back().icp_inliers_ratio, 0.1f) + << "the deskewed pair barely matched itself, so something else is wrong"; + + // The correction itself: the same sweep, as the two nodes handed it to ICP. Half a + // turn of mast rotation across one sweep moves the far end of it by a good fraction + // of a metre, and a deskewing step that stopped working would leave these two clouds + // on top of each other. + ASSERT_FALSE(deskewedScan->empty()) << "the deskewed run republished no scan"; + ASSERT_FALSE(asRecordedScan->empty()) << "the uncorrected run republished no scan"; + // The control: the measure has to say every point of a cloud corresponds to itself, + // so a number well below that is the two clouds genuinely having moved apart rather + // than the measurement being broken. + EXPECT_NEAR(1.0, correspondenceRatio(deskewedScan->back(), deskewedScan->back(), 0.01), + 1e-6) << "the correspondence measure does not even match a cloud to itself"; + + // How far apart the two are depends on PCL's correspondence estimation, which is not + // the same from one version to the next -- 0.12 with 1.12 against 0.07 with 1.15 -- + // so the bound is loose. A deskewing step that stopped working would leave the two + // clouds identical and put this back at 1.0, which is what it is here to catch. + const double stillTogether = + correspondenceRatio(deskewedScan->back(), asRecordedScan->back(), 0.01); + EXPECT_LT(stillTogether, 0.5) + << stillTogether*100.0 << "% of the deskewed scan is still within a centimetre " + "of the uncorrected one, so the correction never reached the cloud"; + + // Both runs register, so this part of the claim is about accuracy rather than + // survival. + EXPECT_LT(translationNorm(deskewed->back()), 0.15) + << "the platform never moved; the deskewed estimate should say so"; + EXPECT_LT(rotationAngle(deskewed->back()), 0.05) + << "the platform never turned; the deskewed estimate should say so"; + if(isLost(asRecorded->back())) + { + // It failed outright instead -- an even stronger version of the same claim. + SUCCEED() << "the uncorrected pair could not be registered at all"; + } + else + { + EXPECT_GT(translationNorm(asRecorded->back()), translationNorm(deskewed->back()) * 1.1) + << "deskewing did not improve the translation error (deskewed=" + << translationNorm(deskewed->back()) << " m, as recorded=" + << translationNorm(asRecorded->back()) << " m)"; + EXPECT_GT(rotationAngle(asRecorded->back()), rotationAngle(deskewed->back()) * 1.1) + << "deskewing did not improve the rotation error (deskewed=" + << rotationAngle(deskewed->back()) << " rad, as recorded=" + << rotationAngle(asRecorded->back()) << " rad)"; + } +} + + + +/** + * The same turn again, predicted from TF instead of an IMU: guess_frame_id names a pose + * source, the node differences it between the two scan stamps, and ICP gets the same + * 0.35 rad guess it got from the IMU. Different input, same prediction. + */ +TEST_F(IcpOdometryTest, takes_the_rotation_of_its_guess_from_the_guess_frame) +{ + publishSensorTf(); + std::shared_ptr> info = + collect("odom_info"); + std::shared_ptr> odom = + collect("odom"); + std::vector params = icpTestParameters(); + params.push_back(rclcpp::Parameter("guess_frame_id", "wheel_odom")); + params.push_back(rclcpp::Parameter("wait_for_transform", 2.0)); + makeNode(params); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(publishGuessTurn(1.0)); + + pub->publish(makeXYZCloud("lidar", 1.0, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })) << "no odometry for the first scan"; + pub->publish(makeXYZCloud("lidar", 1.1, corner3DTurned(kPredictedTurn))); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; })) + << "no odometry for the second scan"; + + // The same guess the IMU produced: 0.35 rad of rotation, no translation. + EXPECT_NEAR(kPredictedTurn, rotationAngleOf(info->back().guess.rotation), 0.01) + << "the guess handed to ICP did not come from the guess frame"; + EXPECT_NEAR(0.0, info->back().guess.translation.x, 1e-6); + EXPECT_NEAR(0.0, info->back().guess.translation.y, 1e-6); + EXPECT_NEAR(0.0, info->back().guess.translation.z, 1e-6); + + ASSERT_FALSE(isLost(odom->back())) << "the registration did not converge"; + // The pose is the turn itself, where the IMU version also carries kImuHeading: both + // seed the first pose from their prediction, and this one starts at the identity. + EXPECT_NEAR(kPredictedTurn, rotationAngle(odom->back()), 0.02); +} + + +/** + * A frame stamped ahead of every IMU sample in the buffer is held, not processed: the + * odometry would otherwise register it without the orientation that belongs to it. It + * comes out as soon as an IMU sample reaches its stamp. + */ +TEST_F(IcpOdometryTest, holds_a_frame_until_an_imu_covers_its_stamp) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::vector params = icpTestParameters(); + params.push_back(rclcpp::Parameter("wait_imu_to_init", true)); + makeNode(params); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + rclcpp::Publisher::SharedPtr imu = + helper()->create_publisher("imu", rclcpp::QoS(200)); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForSubscriber(imu)); + publishImuTf(); + + // IMU up to 1.0 only... + for(double t = 0.9; t <= 1.0; t += 0.01) + { + imu->publish(imuSample(t, kImuHeading)); + } + spinFor(std::chrono::milliseconds(200)); + + // ...and a frame stamped after all of it. + pub->publish(makeXYZCloud("lidar", 1.05, corner3D())); + spinFor(std::chrono::milliseconds(500)); + EXPECT_TRUE(odom->empty()) + << "the frame was registered before any IMU covered its stamp"; + + // One sample at or past the frame's stamp releases it. + imu->publish(imuSample(1.06, kImuHeading)); + EXPECT_TRUE(spinUntil([&]() { return !odom->empty(); })) + << "the held frame was never processed once the IMU caught up"; +} + +/** + * The orientation handed to the odometry is the one belonging to the frame's stamp, not + * whatever the IMU has reached by the time the frame is processed. Here the IMU keeps + * turning well past the second frame -- to 2.0 rad, twice the turn between the scans -- + * and the guess still comes out at the turn the frame saw. + */ +TEST_F(IcpOdometryTest, uses_the_orientation_at_the_frame_stamp_not_the_newest_one) +{ + publishSensorTf(); + std::shared_ptr> info = + collect("odom_info"); + std::shared_ptr> odom = + collect("odom"); + std::vector params = icpTestParameters(); + params.push_back(rclcpp::Parameter("wait_imu_to_init", true)); + makeNode(params); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(publishImuTurn(1.0, 2.0)); + + pub->publish(makeXYZCloud("lidar", 1.0, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })); + pub->publish(makeXYZCloud("lidar", 1.1, corner3DTurned(kPredictedTurn))); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; })); + + // 1.05, the turn between the two stamps -- not the 2.0 the IMU has reached by then. + EXPECT_NEAR(kPredictedTurn, rotationAngleOf(info->back().guess.rotation), 0.02) + << "the guess did not correspond to the frame's own stamp"; + ASSERT_FALSE(isLost(odom->back())) << "the registration did not converge"; + EXPECT_NEAR(kImuHeading + kPredictedTurn, rotationAngle(odom->back()), 0.02); +} + +// --------------------------------------------------------------------------- +// 2D scan deskewing: a lidar on a robot driving at a corner. +// --------------------------------------------------------------------------- + +/** + * The LaserScan path through icp_odometry, with deskewing against a fixed frame. + * + * The robot drives at the corner at 1 m/s and stops just before the second scan, so the + * first sweep is bent and the second is straight. Deskewed, both describe the same corner + * and the registration returns the distance actually travelled between the two stamps. + */ +TEST_F(IcpOdometryTest, deskews_a_2d_scan_against_a_fixed_frame) +{ + std::shared_ptr> odom = + collect("odom_deskewed"); + makeNode({rclcpp::Parameter("deskewing", true), + rclcpp::Parameter("guess_frame_id", "odom"), + rclcpp::Parameter("Icp/PointToPlane", "false"), + rclcpp::Parameter("Icp/CorrespondenceRatio", "0.1"), + rclcpp::Parameter("Icp/MaxTranslation", "0.0"), + rclcpp::Parameter("Reg/Force3DoF", "true"), + rclcpp::Parameter("scan_voxel_size", 0.0), + rclcpp::Parameter("wait_for_transform", 2.0)}, + "deskewed"); + ASSERT_TRUE(publishRobotTrajectory()); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(makeSkewedCornerScan(kFirstScan)); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })) << "the first scan produced nothing"; + pub->publish(makeSkewedCornerScan(kSecondScan)); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; })) << "the second scan produced nothing"; + + ASSERT_FALSE(isLost(odom->back())) << "the deskewed pair failed to register"; + // 0.399909 m against a truth of 0.4, and 5e-5 m sideways, repeatable to the last + // digit. A centimetre of tolerance leaves room for a different ICP backend without + // leaving room for the 5 cm error an unskewed registration makes on this scene. + EXPECT_NEAR(trueDisplacement(), odom->back().pose.pose.position.x, 0.01) + << "the robot drove " << trueDisplacement() << " m between the two scans"; + EXPECT_NEAR(0.0, odom->back().pose.pose.position.y, 0.01) + << "it drove straight at the corner, so there is no sideways motion to find"; +} + + +/** + * The same two scans with deskewing off, side by side with a node that has it on. + * + * The first sweep is bent by the motion and the second is not, so an uncorrected + * registration is matching two different shapes and pays for it in the estimate. + */ +TEST_F(IcpOdometryTest, deskewing_a_2d_scan_changes_what_is_registered) +{ + std::shared_ptr> deskewed = + collect("odom_deskewed"); + std::shared_ptr> asScanned = + collect("odom_as_scanned"); + const std::vector icp = { + rclcpp::Parameter("guess_frame_id", "odom"), + rclcpp::Parameter("Icp/PointToPlane", "false"), + rclcpp::Parameter("Icp/CorrespondenceRatio", "0.1"), + rclcpp::Parameter("Icp/MaxTranslation", "0.0"), + rclcpp::Parameter("Reg/Force3DoF", "true"), + rclcpp::Parameter("scan_voxel_size", 0.0), + rclcpp::Parameter("wait_for_transform", 2.0)}; + std::vector on = icp, off = icp; + on.push_back(rclcpp::Parameter("deskewing", true)); + off.push_back(rclcpp::Parameter("deskewing", false)); + makeNode(on, "deskewed"); + makeNode(off, "as_scanned"); + ASSERT_TRUE(publishRobotTrajectory()); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(pub, 2)); + + pub->publish(makeSkewedCornerScan(kFirstScan)); + ASSERT_TRUE(spinUntil([&]() { return !deskewed->empty() && !asScanned->empty(); })); + pub->publish(makeSkewedCornerScan(kSecondScan)); + ASSERT_TRUE(spinUntil([&]() { + return deskewed->size() >= 2 && asScanned->size() >= 2; })); + + ASSERT_FALSE(isLost(deskewed->back())); + ASSERT_FALSE(isLost(asScanned->back())) + << "the uncorrected pair was expected to register, just badly"; + + const double deskewedError = + std::fabs(deskewed->back().pose.pose.position.x - trueDisplacement()); + const double skewedError = + std::fabs(asScanned->back().pose.pose.position.x - trueDisplacement()); + + // Measured: 0.0001 m corrected against 0.0499 m uncorrected. That 5 cm is half the + // 0.1 m the robot covered during the bent sweep, which is what it costs to match a + // bent corner against a straight one. + EXPECT_LT(deskewedError, 0.01) << "deskewed estimate is " << deskewed->back().pose.pose.position.x; + EXPECT_GT(skewedError, 0.02) + << "the uncorrected registration was as good as the corrected one, so " + "deskewing never reached the scan (deskewed error=" << deskewedError + << " m, uncorrected error=" << skewedError << " m)"; + EXPECT_GT(skewedError, deskewedError * 5.0) << "deskewing barely improved the estimate"; +} + + +// --------------------------------------------------------------------------- +// IMU: the orientation replaces the rotation of the odometry's prediction. +// +// RTAB-Map builds a guess for ICP from a constant-velocity model, and when an IMU with a +// valid orientation is available it keeps that model's translation but takes the rotation +// from the IMU ("replace orientation guess with IMU" in Odometry::process). On the second +// frame there is no velocity yet, so the guess is the IMU's rotation and nothing else -- +// which is exactly what odom_info reports. +// +// The IMU topic exists only when wait_imu_to_init is set; without it the node never +// subscribes and every scan is registered from an identity guess. +// --------------------------------------------------------------------------- + +/** + * The sensor turns 0.35 rad between two scans and an IMU says so: the guess handed to ICP + * carries that rotation and no translation, and the registration lands on it. + */ +TEST_F(IcpOdometryTest, takes_the_rotation_of_its_guess_from_the_imu) +{ + publishSensorTf(); + std::shared_ptr> info = + collect("odom_info"); + std::shared_ptr> odom = + collect("odom"); + std::vector params = icpTestParameters(); + params.push_back(rclcpp::Parameter("wait_imu_to_init", true)); + makeNode(params); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(publishImuTurn(1.0)); + + pub->publish(makeXYZCloud("lidar", 1.0, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })) << "no odometry for the first scan"; + pub->publish(makeXYZCloud("lidar", 1.1, corner3DTurned(kPredictedTurn))); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; })) + << "no odometry for the second scan"; + + // The guess is the IMU's change of orientation, exactly: 0.35 rad. + EXPECT_NEAR(kPredictedTurn, rotationAngleOf(info->back().guess.rotation), 0.01) + << "the guess handed to ICP did not come from the IMU"; + // ...and the constant-velocity model contributes nothing to it yet, there being no + // velocity to speak of after a single frame. + EXPECT_NEAR(0.0, info->back().guess.translation.x, 1e-6); + EXPECT_NEAR(0.0, info->back().guess.translation.y, 1e-6); + EXPECT_NEAR(0.0, info->back().guess.translation.z, 1e-6); + + ASSERT_FALSE(isLost(odom->back())) << "the registration did not converge"; + // The pose also carries the heading the IMU started from: RTAB-Map seeds the first + // pose with the IMU orientation, so this is kImuHeading + kPredictedTurn, not kPredictedTurn. + EXPECT_NEAR(kImuHeading + kPredictedTurn, rotationAngle(odom->back()), 0.02); +} + +/** + * The same two scans with no IMU: the guess carries no rotation at all. + * + * ICP then fails to find the turn, on either backend: it settles about 0.01 rad from no + * motion at all and reports that as a successful registration. How it fails does vary -- + * at smaller turns libpointmatcher trips Icp/MaxTranslation and returns an unusable pose + * while PCL converges correctly -- so the test asserts only that the turn was not + * recovered, not the manner of it. + */ +TEST_F(IcpOdometryTest, without_an_imu_the_guess_carries_no_rotation) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr> info = + collect("odom_info"); + makeNode(icpTestParameters()); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(makeXYZCloud("lidar", 1.0, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })); + pub->publish(makeXYZCloud("lidar", 1.1, corner3DTurned(kPredictedTurn))); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; })); + + // Null or identity, but in no case the turn: with no IMU there is nothing to predict + // a rotation from, and no velocity yet either. + EXPECT_GT(std::fabs(rotationAngleOf(info->back().guess.rotation) - kPredictedTurn), 0.1) + << "the guess carried the turn with no IMU to supply it"; + + // And without it the registration does not find the turn: measured at 0.009 rad on + // PCL and 0.010 on libpointmatcher, against a real 1.05. Neither reports failure -- + // they settle on "barely moved", which is the quiet way this goes wrong in the field. + if(!isLost(odom->back())) + { + EXPECT_GT(std::fabs(rotationAngle(odom->back()) - kPredictedTurn), 0.5) + << "ICP recovered the turn from an identity guess, which would make the " + "prediction tested above unnecessary"; + } +} + + +// --------------------------------------------------------------------------- +// What the node hands downstream, and two parameters that change it. +// --------------------------------------------------------------------------- + +/** + * The scan republished on odom_sensor_data is the one ICP registered -- after + * voxelization -- not the sweep the lidar published. See "Reusing the filtered scan + * downstream" in doc/icp_odometry.md. + */ +TEST_F(IcpOdometryTest, republishes_the_filtered_scan_rather_than_the_input) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr> data = + collect("odom_sensor_data/raw"); + std::vector params = icpTestParameters(); + params.push_back(rclcpp::Parameter("scan_voxel_size", 0.5)); // coarse, on a 4 m corner + makeNode(params); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + const std::vector points = corner3D(); + pub->publish(makeXYZCloud("lidar", 1.0, points)); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty() && !data->empty(); })); + + // 1200 points in, 243 out at a 0.5 m voxel on a 4 m corner. + EXPECT_GT(data->back().laser_scan.width, 0u) << "no scan was republished at all"; + EXPECT_LT(data->back().laser_scan.width, points.size() / 2) + << "the republished scan still has the input's density, so it is the input"; +} + +/** + * scan_cloud_is_2d says a cloud carrying a z field is a planar scan after all, so it is + * registered -- and republished -- as 2D. The scan format says which it was: 3 + * (kXYNormal) against 8 (3D with normals) for the same cloud. + */ +TEST_F(IcpOdometryTest, scan_cloud_is_2d_registers_a_cloud_as_a_planar_scan) +{ + publishSensorTf(); + std::shared_ptr> planar = + collect("odom_sensor_data_planar/raw"); + std::shared_ptr> volume = + collect("odom_sensor_data_volume/raw"); + std::vector flat = icpTestParameters(); + std::vector spatial = icpTestParameters(); + flat.push_back(rclcpp::Parameter("scan_cloud_is_2d", true)); + spatial.push_back(rclcpp::Parameter("scan_cloud_is_2d", false)); + makeNode(flat, "planar"); + makeNode(spatial, "volume"); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub, 2)); + + // A flat corner: two walls, every point at z = 0, but the cloud still carries a z field. + std::vector points; + for(int i=0; i<200; ++i) + { + points.push_back(cv::Point3f(-2.0f + 0.02f*i, -2.0f, 0.0f)); + points.push_back(cv::Point3f(-2.0f, -2.0f + 0.02f*i, 0.0f)); + } + pub->publish(makeXYZCloud("lidar", 1.0, points)); + ASSERT_TRUE(spinUntil([&]() { return !planar->empty() && !volume->empty(); })); + + // LaserScan::Format numbers the 2D layouts 1 to 4 and the 3D ones from 5 up. + EXPECT_LT(planar->back().laser_scan_format, 5) + << "the cloud was registered as 3D despite scan_cloud_is_2d"; + EXPECT_GE(volume->back().laser_scan_format, 5) + << "the same cloud should be 3D without the parameter"; +} + +/** + * deskewing_slerp interpolates the correction between the ends of the sweep instead of + * looking TF up for every point -- cheaper, and per the documentation slightly less + * accurate. It has to land in the same place. + */ +TEST_F(IcpOdometryTest, deskewing_slerp_gives_the_same_answer_as_the_per_point_lookup) +{ + std::shared_ptr> slerp = + collect("odom_slerp"); + std::shared_ptr> perPoint = + collect("odom_perpoint"); + const Recording recording = readOusterRecording(); + ASSERT_TRUE(recording.valid()) << "could not read " << ousterHalfTurnBag(); + + const std::vector common = { + rclcpp::Parameter("deskewing", true), + rclcpp::Parameter("guess_frame_id", "base_link"), + rclcpp::Parameter("scan_cloud_max_points", 65536), + rclcpp::Parameter("scan_voxel_size", 0.2), + rclcpp::Parameter("Icp/MaxCorrespondenceDistance", "2.0"), + rclcpp::Parameter("Icp/MaxTranslation", "0.5"), + rclcpp::Parameter("wait_for_transform", 2.0)}; + std::vector a = common, b = common; + a.push_back(rclcpp::Parameter("deskewing_slerp", true)); + b.push_back(rclcpp::Parameter("deskewing_slerp", false)); + makeNode(a, "slerp"); + makeNode(b, "perpoint"); + ASSERT_TRUE(publishRecordedTf(recording)); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub, 2)); + + pub->publish(recording.clouds[0]); + ASSERT_TRUE(spinUntil([&]() { return !slerp->empty() && !perPoint->empty(); })); + pub->publish(recording.clouds[1]); + ASSERT_TRUE(spinUntil([&]() { + return slerp->size() >= 2 && perPoint->size() >= 2; })); + + ASSERT_FALSE(isLost(slerp->back())) << "the interpolated deskew failed to register"; + ASSERT_FALSE(isLost(perPoint->back())); + // 0.0117 m against 0.0131 m, and the rotations agree to four decimals: "slightly less + // accurate", as documented, and nowhere near a different answer. + EXPECT_NEAR(translationNorm(perPoint->back()), translationNorm(slerp->back()), 0.01) + << "interpolating the correction moved the estimate"; + EXPECT_NEAR(rotationAngle(perPoint->back()), rotationAngle(slerp->back()), 0.01); +} + + +// --------------------------------------------------------------------------- +// The fields a driver puts in its cloud, and the filters the node runs on them. +// --------------------------------------------------------------------------- + +/** + * @brief A cloud carrying intensity, normals, both, or neither. + * + * Each combination is a different PCL point type inside the node, so a driver that + * publishes everything its lidar measured is handled by different code from one that + * publishes bare XYZ. All four describe the same corner, so all four have to end at the + * same pose. scan_downsampling_step is on throughout: the step is applied in each of the + * four, and halving the cloud is visible in what comes back out. + */ +class IcpOdometryCloudFieldsTest : + public IcpOdometryTest, + public ::testing::WithParamInterface> +{ +}; + +TEST_P(IcpOdometryCloudFieldsTest, recovers_the_motion_whatever_fields_the_cloud_carries) +{ + const bool intensity = std::get<0>(GetParam()); + const bool normals = std::get<1>(GetParam()); + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr> data = + collect("odom_sensor_data/raw"); + std::vector params = icpTestParameters(); + params.push_back(rclcpp::Parameter("scan_downsampling_step", 2)); + makeNode(params); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + const std::vector points = corner3D(); + pub->publish(makeCloudWithFields("lidar", 1.0, points, intensity, normals)); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 1 && data->size() >= 1; })); + + const cv::Point3f motion(0.10f, 0.06f, 0.04f); + pub->publish(makeCloudWithFields("lidar", 1.1, corner3D(motion), intensity, normals)); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; })); + + const nav_msgs::msg::Odometry & msg = odom->back(); + EXPECT_NEAR(motion.x, msg.pose.pose.position.x, 0.015); + EXPECT_NEAR(motion.y, msg.pose.pose.position.y, 0.015); + EXPECT_NEAR(motion.z, msg.pose.pose.position.z, 0.015); + EXPECT_EQ(points.size()/2, data->front().laser_scan.width) + << "scan_downsampling_step:=2 should have kept every second point"; +} + +INSTANTIATE_TEST_SUITE_P( + OptionalFields, + IcpOdometryCloudFieldsTest, + ::testing::Combine(::testing::Bool(), ::testing::Bool()), + [](const ::testing::TestParamInfo> & info) { + return std::string(std::get<0>(info.param) ? "intensity" : "no_intensity") + + (std::get<1>(info.param) ? "_normals" : "_no_normals"); + }); + +/** + * @brief A cloud that says it is not dense, with and without intensity. + * + * is_dense:=false is a driver saying some of these points are NaN -- a ray that hit + * nothing. The node drops them rather than handing NaNs to ICP. + */ +class IcpOdometryDenseTest : + public IcpOdometryTest, + public ::testing::WithParamInterface +{ +}; + +TEST_P(IcpOdometryDenseTest, drops_the_invalid_points_of_a_cloud_that_is_not_dense) +{ + publishSensorTf(); + std::shared_ptr> data = + collect("odom_sensor_data/raw"); + makeNode(icpTestParameters()); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + std::vector points = corner3D(); + const size_t valid = points.size(); + const float nan = std::numeric_limits::quiet_NaN(); + points.insert(points.end(), 100, cv::Point3f(nan, nan, nan)); + sensor_msgs::msg::PointCloud2 cloud = + makeCloudWithFields("lidar", 1.0, points, GetParam(), false); + cloud.is_dense = false; + pub->publish(cloud); + ASSERT_TRUE(spinUntil([&]() { return !data->empty(); })); + + EXPECT_EQ(valid, data->back().laser_scan.width) + << "the 100 NaN points were registered along with the real ones"; +} + +INSTANTIATE_TEST_SUITE_P( + OptionalFields, + IcpOdometryDenseTest, + ::testing::Bool(), + [](const ::testing::TestParamInfo & info) { + return info.param ? "intensity" : "no_intensity"; + }); + +/** + * An organized cloud -- one row per laser ring -- tells the node how many points a full + * sweep has, so scan_cloud_max_points does not have to be given at all. + */ +TEST_F(IcpOdometryTest, takes_scan_cloud_max_points_from_an_organized_cloud) +{ + publishSensorTf(); + std::shared_ptr> data = + collect("odom_sensor_data/raw"); + makeNode(icpTestParameters()); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + const std::vector points = corner3D(); + pub->publish(organized(makeXYZCloud("lidar", 1.0, points), 80)); + ASSERT_TRUE(spinUntil([&]() { return !data->empty(); })); + + EXPECT_EQ(int(points.size()), data->back().laser_scan_max_pts); +} + +/// A value too small for the cloud that arrived is raised to it rather than believed. +TEST_F(IcpOdometryTest, raises_scan_cloud_max_points_to_the_size_of_an_organized_cloud) +{ + publishSensorTf(); + std::shared_ptr> data = + collect("odom_sensor_data/raw"); + std::vector params = icpTestParameters(); + params.push_back(rclcpp::Parameter("scan_cloud_max_points", 10)); + makeNode(params); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + const std::vector points = corner3D(); + pub->publish(organized(makeXYZCloud("lidar", 1.0, points), 80)); + ASSERT_TRUE(spinUntil([&]() { return !data->empty(); })); + + EXPECT_EQ(int(points.size()), data->back().laser_scan_max_pts); +} + +/** + * scan_range_min and scan_range_max cut the cloud down to a shell around the sensor. The + * corner spans 2.0 m to 3.5 m from the origin, so a 2.6-3.0 m window keeps part of it. + */ +TEST_F(IcpOdometryTest, keeps_only_the_cloud_points_within_the_range_limits) +{ + publishSensorTf(); + std::shared_ptr> data = + collect("odom_sensor_data/raw"); + std::vector params = icpTestParameters(); + params.push_back(rclcpp::Parameter("scan_range_min", 2.6)); + params.push_back(rclcpp::Parameter("scan_range_max", 3.0)); + makeNode(params); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + const std::vector points = corner3D(); + pub->publish(makeXYZCloud("lidar", 1.0, points)); + ASSERT_TRUE(spinUntil([&]() { return !data->empty(); })); + + EXPECT_GT(data->back().laser_scan.width, 0u) << "the whole cloud was filtered away"; + EXPECT_LT(data->back().laser_scan.width, points.size()) + << "nothing outside 2.6-3.0 m was dropped"; +} + +/** + * scan_normal_ground_up turns the normals a driver sent toward a viewpoint 10 m above the + * sensor, so the ground's point up the way ICP expects rather than into the road. Here + * every normal arrives pointing down, and the scan republished on + * odom_filtered_input_scan is the one the node registered -- normals included, so it says + * which way they ended up. The second node is the control: without the parameter the node + * does not touch them at all. + */ +TEST_F(IcpOdometryTest, turns_the_normals_of_a_cloud_toward_a_viewpoint_above) +{ + publishSensorTf(); + std::shared_ptr> aligned = + collect("odom_filtered_input_scan_aligned"); + std::shared_ptr> asIs = + collect("odom_filtered_input_scan_as_is"); + std::vector up = icpTestParameters(); + up.push_back(rclcpp::Parameter("scan_normal_ground_up", 0.8)); + makeNode(up, "aligned"); + makeNode(icpTestParameters(), "as_is"); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub, 2)); + ASSERT_TRUE(waitForPublisher(aligned->subscription)); + ASSERT_TRUE(waitForPublisher(asIs->subscription)); + + pub->publish(makeCloudWithFields("lidar", 1.0, corner3D(), false, true, + cv::Point3f(0, 0, -1))); + ASSERT_TRUE(spinUntil([&]() { return !aligned->empty() && !asIs->empty(); })); + + const std::vector turned = fieldValues(aligned->back(), "normal_z"); + ASSERT_FALSE(turned.empty()) << "the republished scan carries no normals"; + EXPECT_EQ(turned.size(), countPositive(turned)) << "some normals still point down"; + + const std::vector untouched = fieldValues(asIs->back(), "normal_z"); + ASSERT_EQ(turned.size(), untouched.size()); + EXPECT_EQ(0u, countPositive(untouched)) + << "the normals were turned over without scan_normal_ground_up being set"; +} + +/// What goes out on odom_filtered_input_scan is the filtered scan, not the input. +TEST_F(IcpOdometryTest, publishes_the_scan_it_registered_on_odom_filtered_input_scan) +{ + publishSensorTf(); + std::shared_ptr> filtered = + collect("odom_filtered_input_scan"); + std::vector params = icpTestParameters(); + params.push_back(rclcpp::Parameter("scan_voxel_size", 0.5)); // coarse, on a 4 m corner + makeNode(params); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(filtered->subscription)); + + const std::vector points = corner3D(); + pub->publish(makeXYZCloud("lidar", 1.0, points)); + ASSERT_TRUE(spinUntil([&]() { return !filtered->empty(); })); + + EXPECT_EQ("lidar", filtered->back().header.frame_id); + EXPECT_GT(filtered->back().width, 0u) << "nothing was republished at all"; + EXPECT_LT(filtered->back().width, points.size()/2) + << "the republished scan still has the input's density, so it is the input"; +} + +/** + * @brief Deskewing without a guess frame, on a cloud already in frame_id and on one that + * is not. + * + * With no fixed frame to look the motion up in, the node deskews against its own constant + * velocity model, which lives in frame_id: a cloud published in the sensor's frame has to + * be carried into frame_id and back, and one already there is deskewed where it lies. The + * velocity only exists once two frames have registered, so the correction starts on the + * third. + */ +class IcpOdometryDeskewFrameTest : + public IcpOdometryTest, + public ::testing::WithParamInterface +{ +}; + +TEST_P(IcpOdometryDeskewFrameTest, deskews_against_its_own_velocity) +{ + const std::string frame = GetParam(); + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::vector params = icpTestParameters(); + params.push_back(rclcpp::Parameter("deskewing", true)); + makeNode(params); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + for(int i=0; i<4; ++i) + { + pub->publish(makeCloudWithFields(frame, 1.0 + 0.1*i, + corner3D(cv::Point3f(0.05f*i, 0, 0)), false, false, + cv::Point3f(0, 0, 1), true)); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); })) + << "no odometry for the cloud at " << (1.0 + 0.1*i) << "s"; + } + + // Four frames 0.05 m apart, and a sweep short enough that deskewing them barely moves + // anything: the point is that the correction ran, not that it changed the answer. + EXPECT_NEAR(0.15, odom->back().pose.pose.position.x, 0.02); +} + +INSTANTIATE_TEST_SUITE_P( + CloudFrame, + IcpOdometryDeskewFrameTest, + ::testing::Values("base_link", "lidar"), + [](const ::testing::TestParamInfo & info) { + return "in_" + info.param; + }); + +// --------------------------------------------------------------------------- +// The same filters, on the 2D scan topic. +// --------------------------------------------------------------------------- + +/** + * @brief A LaserScan with and without the intensity channel. + * + * laser_geometry carries the channel into the projected cloud when the scan has one, and + * from there the node takes the PointXYZI path rather than the PointXYZ one -- separate + * code for every filter below. + */ +class IcpOdometryScanChannelTest : + public IcpOdometryTest, + public ::testing::WithParamInterface +{ +}; + +TEST_P(IcpOdometryScanChannelTest, recovers_the_motion_whatever_channels_the_scan_carries) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode(icpTestParameters()); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(cornerScan(1.0, 0.0, 0.0, GetParam())); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 1; })); + + pub->publish(cornerScan(1.1, 0.05, 0.0, GetParam())); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; })); + + EXPECT_NEAR(0.05, odom->back().pose.pose.position.x, 0.02); +} + +/// scan_downsampling_step and scan_voxel_size thin the scan before ICP sees it. +TEST_P(IcpOdometryScanChannelTest, downsamples_and_voxelizes_a_laser_scan) +{ + publishSensorTf(); + std::shared_ptr> data = + collect("odom_sensor_data/raw"); + std::vector params = icpTestParameters(); + params.push_back(rclcpp::Parameter("scan_downsampling_step", 2)); + params.push_back(rclcpp::Parameter("scan_voxel_size", 0.05)); + makeNode(params); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + const sensor_msgs::msg::LaserScan scan = cornerScan(1.0, 0.0, 0.0, GetParam()); + pub->publish(scan); + ASSERT_TRUE(spinUntil([&]() { return !data->empty(); })); + + EXPECT_GT(data->back().laser_scan.width, 0u) << "the whole scan was filtered away"; + EXPECT_LE(data->back().laser_scan.width, scan.ranges.size()/2 + 1) + << "scan_downsampling_step:=2 alone should have halved it"; +} + +/// With both scan_normal_k and scan_normal_radius off, the scan goes to ICP as bare points. +TEST_P(IcpOdometryScanChannelTest, registers_a_laser_scan_without_computing_normals) +{ + publishSensorTf(); + std::shared_ptr> data = + collect("odom_sensor_data/raw"); + std::vector params = icpTestParameters(); + params.push_back(rclcpp::Parameter("scan_normal_k", 0)); + params.push_back(rclcpp::Parameter("scan_normal_radius", 0.0)); + makeNode(params); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(cornerScan(1.0, 0.0, 0.0, GetParam())); + ASSERT_TRUE(spinUntil([&]() { return !data->empty(); })); + + // LaserScan::Format 1 is kXY and 2 is kXYI; the layouts with normals are 3 and 4. + EXPECT_LT(data->back().laser_scan_format, 3) + << "normals were computed although both scan_normal_* are off"; +} + +/// The range limits apply to a 2D scan too: the corner is 3 m away at its nearest. +TEST_P(IcpOdometryScanChannelTest, keeps_only_the_scan_points_within_the_range_limits) +{ + publishSensorTf(); + std::shared_ptr> data = + collect("odom_sensor_data/raw"); + std::vector params = icpTestParameters(); + params.push_back(rclcpp::Parameter("scan_range_min", 3.5)); + params.push_back(rclcpp::Parameter("scan_range_max", 6.0)); + makeNode(params); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + const sensor_msgs::msg::LaserScan scan = cornerScan(1.0, 0.0, 0.0, GetParam()); + pub->publish(scan); + ASSERT_TRUE(spinUntil([&]() { return !data->empty(); })); + + EXPECT_GT(data->back().laser_scan.width, 0u) << "the whole scan was filtered away"; + EXPECT_LT(data->back().laser_scan.width, scan.ranges.size()) + << "nothing nearer than 3.5 m was dropped"; + EXPECT_NEAR(6.0, data->back().laser_scan_max_range, 1e-3) + << "scan_range_max should replace the scan's own 30 m range_max"; +} + +INSTANTIATE_TEST_SUITE_P( + Channels, + IcpOdometryScanChannelTest, + ::testing::Bool(), + [](const ::testing::TestParamInfo & info) { + return info.param ? "with_intensities" : "without_intensities"; + }); + +// --------------------------------------------------------------------------- +// One lidar at a time, and the parameters that come from RTAB-Map's own names. +// --------------------------------------------------------------------------- + +/** + * The node subscribes to both `scan` and `scan_cloud`, but registering a 2D scan against + * a 3D cloud is meaningless, so whichever topic speaks second is dropped for good. + */ +TEST_F(IcpOdometryTest, stops_listening_for_scans_once_clouds_are_arriving) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode(icpTestParameters()); + + rclcpp::Publisher::SharedPtr cloud = + helper()->create_publisher("scan_cloud", 10); + rclcpp::Publisher::SharedPtr scan = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(cloud)); + ASSERT_TRUE(waitForSubscriber(scan)); + + cloud->publish(makeXYZCloud("lidar", 1.0, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 1; })); + + scan->publish(cornerScan(1.1)); + spinFor(std::chrono::milliseconds(300)); + EXPECT_EQ(1u, odom->size()) << "the scan was registered against the cloud"; + EXPECT_EQ(0u, scan->get_subscription_count()) << "the scan subscriber is still up"; +} + +/// And the other way round: a cloud arriving after scans is dropped instead. +TEST_F(IcpOdometryTest, stops_listening_for_clouds_once_scans_are_arriving) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode(icpTestParameters()); + + rclcpp::Publisher::SharedPtr cloud = + helper()->create_publisher("scan_cloud", 10); + rclcpp::Publisher::SharedPtr scan = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(cloud)); + ASSERT_TRUE(waitForSubscriber(scan)); + + scan->publish(cornerScan(1.0)); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 1; })); + + cloud->publish(makeXYZCloud("lidar", 1.1, corner3D())); + spinFor(std::chrono::milliseconds(300)); + EXPECT_EQ(1u, odom->size()) << "the cloud was registered against the scan"; + EXPECT_EQ(0u, cloud->get_subscription_count()) << "the cloud subscriber is still up"; +} + +/** + * Several of the Icp parameters name a filter the node runs itself, before ICP ever sees the + * scan. Setting one of those is taken as asking for the node's filter, so the value moves + * to the matching ros parameter -- see "Where these defaults come from" in the doc. + */ +TEST_F(IcpOdometryTest, takes_the_icp_filter_values_as_its_own_scan_parameters) +{ + publishSensorTf(); + std::shared_ptr node = makeNode({ + rclcpp::Parameter("Icp/DownsamplingStep", "2"), + rclcpp::Parameter("Icp/RangeMin", "0.5"), + rclcpp::Parameter("Icp/RangeMax", "20.0"), + rclcpp::Parameter("Icp/PointToPlaneRadius", "0.3"), + rclcpp::Parameter("Icp/PointToPlaneGroundNormalsUp", "0.8")}); + + EXPECT_EQ(2, node->get_parameter("scan_downsampling_step").as_int()); + EXPECT_NEAR(0.5, node->get_parameter("scan_range_min").as_double(), 1e-6); + EXPECT_NEAR(20.0, node->get_parameter("scan_range_max").as_double(), 1e-6); + EXPECT_NEAR(0.3, node->get_parameter("scan_normal_radius").as_double(), 1e-6); + EXPECT_NEAR(0.8, node->get_parameter("scan_normal_ground_up").as_double(), 1e-6); +} + +/** + * Reg/Strategy picks between visual and ICP registration, and this node only has ICP. A + * value asking for anything else is overruled rather than obeyed. + */ +TEST_F(IcpOdometryTest, registers_with_icp_whatever_reg_strategy_asks_for) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::vector params = icpTestParameters(); + params.push_back(rclcpp::Parameter("Reg/Strategy", "0")); // visual only + makeNode(params); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(makeXYZCloud("lidar", 1.0, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 1; })); + + const cv::Point3f motion(0.10f, 0.06f, 0.04f); + pub->publish(makeXYZCloud("lidar", 1.1, corner3D(motion))); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; })); + + // Nothing but ICP could have recovered this: there is no image to register. + EXPECT_NEAR(motion.x, odom->back().pose.pose.position.x, 0.01); +} + + +/** + * @brief The four field combinations again, organized and downsampled. + * + * An organized cloud keeps its rows through the step, so the node counts what a full + * sweep holds from the cloud's own dimensions instead of dividing by the step. + */ +class IcpOdometryOrganizedFieldsTest : + public IcpOdometryTest, + public ::testing::WithParamInterface> +{ +}; + +TEST_P(IcpOdometryOrganizedFieldsTest, sizes_a_downsampled_organized_cloud_from_its_rows) +{ + const bool intensity = std::get<0>(GetParam()); + const bool normals = std::get<1>(GetParam()); + publishSensorTf(); + std::shared_ptr> data = + collect("odom_sensor_data/raw"); + std::vector params = icpTestParameters(); + params.push_back(rclcpp::Parameter("scan_downsampling_step", 2)); + makeNode(params); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + const std::vector points = corner3D(); + pub->publish(organized( + makeCloudWithFields("lidar", 1.0, points, intensity, normals), 80)); + ASSERT_TRUE(spinUntil([&]() { return !data->empty(); })); + + // 1200 points as 15 rings of 80, every second point of each ring kept: 15 x 40. + EXPECT_EQ(int(points.size()/2), data->back().laser_scan_max_pts); +} + +INSTANTIATE_TEST_SUITE_P( + OptionalFields, + IcpOdometryOrganizedFieldsTest, + ::testing::Combine(::testing::Bool(), ::testing::Bool()), + [](const ::testing::TestParamInfo> & info) { + return std::string(std::get<0>(info.param) ? "intensity" : "no_intensity") + + (std::get<1>(info.param) ? "_normals" : "_no_normals"); + }); + +/// With both scan_normal_* off, a cloud reaches ICP as bare points, intensity or not. +TEST_P(IcpOdometryDenseTest, registers_a_cloud_without_computing_normals) +{ + publishSensorTf(); + std::shared_ptr> data = + collect("odom_sensor_data/raw"); + std::vector params = icpTestParameters(); + params.push_back(rclcpp::Parameter("scan_normal_k", 0)); + params.push_back(rclcpp::Parameter("scan_normal_radius", 0.0)); + makeNode(params); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(makeCloudWithFields("lidar", 1.0, corner3D(), GetParam(), false)); + ASSERT_TRUE(spinUntil([&]() { return !data->empty(); })); + + // LaserScan::Format kXYZ and kXYZI; the layouts with normals are 8 and 9. + EXPECT_EQ(GetParam() ? 6 : 5, data->back().laser_scan_format) + << "normals were computed although both scan_normal_* are off"; +} + +/** + * An intensity channel the node cannot read -- anything but float32 -- is dropped rather + * than misread, and the cloud is registered without it. + */ +TEST_F(IcpOdometryTest, ignores_an_intensity_field_it_cannot_read) +{ + publishSensorTf(); + std::shared_ptr> data = + collect("odom_sensor_data/raw"); + makeNode(icpTestParameters()); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + sensor_msgs::msg::PointCloud2 cloud = + makeCloudWithFields("lidar", 1.0, corner3D(), true, false); + for(sensor_msgs::msg::PointField & field : cloud.fields) + { + if(field.name == "intensity") + { + field.datatype = sensor_msgs::msg::PointField::UINT8; + } + } + pub->publish(cloud); + ASSERT_TRUE(spinUntil([&]() { return !data->empty(); })); + + // kXYZNormal rather than kXYZINormal: the channel was left out. + EXPECT_EQ(8, data->back().laser_scan_format) + << "an intensity channel that is not float32 was carried through anyway"; +} + +/// A frame TF knows nothing about is an error, not a pose: the frame is dropped. +TEST_F(IcpOdometryTest, refuses_a_cloud_whose_frame_is_not_in_tf) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode(icpTestParameters()); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(makeXYZCloud("unmounted_lidar", 1.0, corner3D())); + spinFor(std::chrono::milliseconds(500)); + EXPECT_TRUE(odom->empty()) << "a cloud from an unknown frame was registered anyway"; +} + +/// The same for a 2D scan. +TEST_F(IcpOdometryTest, refuses_a_scan_whose_frame_is_not_in_tf) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode(icpTestParameters()); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + sensor_msgs::msg::LaserScan scan = cornerScan(1.0); + scan.header.frame_id = "unmounted_lidar"; + pub->publish(scan); + spinFor(std::chrono::milliseconds(500)); + EXPECT_TRUE(odom->empty()) << "a scan from an unknown frame was registered anyway"; +} + +/** + * @brief Deskewing a 2D scan without a guess frame, from the sensor's frame and from + * frame_id. + * + * The cloud version of this is IcpOdometryDeskewFrameTest above; a scan takes a different + * route to the same place, because laser_geometry projects it into frame_id first when + * the sensor is mounted somewhere else, and leaves it alone when it is not. + */ +class IcpOdometryScanDeskewFrameTest : + public IcpOdometryTest, + public ::testing::WithParamInterface +{ +}; + +TEST_P(IcpOdometryScanDeskewFrameTest, deskews_a_scan_against_its_own_velocity) +{ + const std::string frame = GetParam(); + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::vector params = icpTestParameters(); + params.push_back(rclcpp::Parameter("deskewing", true)); + makeNode(params); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + for(int i=0; i<4; ++i) + { + sensor_msgs::msg::LaserScan scan = cornerScan(1.0 + 0.1*i, 0.05*i, 0.0, false, 0.01f); + scan.header.frame_id = frame; + pub->publish(scan); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); })) + << "no odometry for the scan at " << (1.0 + 0.1*i) << "s"; + } + + EXPECT_NEAR(0.15, odom->back().pose.pose.position.x, 0.02); +} + +INSTANTIATE_TEST_SUITE_P( + ScanFrame, + IcpOdometryScanDeskewFrameTest, + ::testing::Values("base_link", "lidar"), + [](const ::testing::TestParamInfo & info) { + return "in_" + info.param; + }); + + +/** + * pause_odom stops the scan topic as well as the cloud one -- the scans keep arriving, + * and none of them is registered until resume_odom. + */ +TEST_F(IcpOdometryTest, pause_and_resume_stop_and_restart_processing_scans) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr node = makeNode(icpTestParameters()); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + pub->publish(cornerScan(1.0)); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })); + + ASSERT_TRUE(callEmptyService("pause_odom")); + const size_t whilePaused = odom->size(); + pub->publish(cornerScan(1.1, 0.05)); + spinFor(std::chrono::milliseconds(500)); + EXPECT_EQ(whilePaused, odom->size()) << "a scan was registered while paused"; + + ASSERT_TRUE(callEmptyService("resume_odom")); + pub->publish(cornerScan(1.2, 0.10)); + EXPECT_TRUE(spinUntil([&]() { return odom->size() > whilePaused; })); +} + + + +} // namespace +} // namespace rtabmap_odom_test diff --git a/rtabmap_odom/test/test_odometry_ros.cpp b/rtabmap_odom/test/test_odometry_ros.cpp new file mode 100644 index 00000000..6694f994 --- /dev/null +++ b/rtabmap_odom/test/test_odometry_ros.cpp @@ -0,0 +1,1350 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include + +#include + +#include +#include + +#include + +#include + +#include + +#include "msg_builders.hpp" +#include "node_test_utils.hpp" +#include "scan_scenes.hpp" + +namespace rtabmap_odom_test { + +namespace { + +::testing::Environment * const kRclcppEnv = registerRclcppEnvironment(); + +/** + * OdometryROS is abstract, so these drive it through ICPOdometry -- the cheapest of the + * three to feed, since a synthetic point cloud needs no camera calibration. Everything + * asserted here lives in the base class and behaves identically on all three nodes. + */ +class OdometryRosTest : public NodeTest +{ +protected: + void publishSensorTf() + { + staticTf_ = std::make_shared(*helper()); + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = helper()->now(); + tf.header.frame_id = "base_link"; + tf.child_frame_id = "lidar"; + tf.transform.rotation.w = 1.0; + staticTf_->sendTransform(tf); + } + + std::shared_ptr makeNode( + std::vector params = {}) + { + std::vector all = icpTestParameters(); + all.push_back(rclcpp::Parameter("frame_id", "base_link")); + // What a sweep holds, as a real lidar reports it. ICP measures its correspondence + // ratio against this, and without it the ratio falls back to the scan's own size + // -- which lets the three points of degenerateCloud() match themselves perfectly. + all.push_back(rclcpp::Parameter("scan_cloud_max_points", int(corner3D().size()))); + // See the note in test_icp_odometry.cpp: without this the node drops frames that + // arrive closer together than their stamps claim, which is what a loaded runner + // does to a sequence published back to back. + all.push_back(rclcpp::Parameter("always_process_most_recent_frame", false)); + for(const rclcpp::Parameter & p : params) + { + all.push_back(p); + } + rclcpp::NodeOptions options; + options.parameter_overrides(all); + return addNode(std::make_shared(options)); + } + + rclcpp::Publisher::SharedPtr scanPublisher() + { + return helper()->create_publisher("scan_cloud", 10); + } + + /** + * @brief Drives `count` frames through the node, each `step` metres further along x. + * + * `unpublished` is how many of them are expected to produce no message, so the wait + * still knows when each frame is done. It is 1 with `publish_null_when_lost` off, + * where the first frame initialises the odometry and is held back for having no + * velocity behind it, and 0 everywhere else. + */ + void feedFrames( + const rclcpp::Publisher::SharedPtr & pub, + const std::shared_ptr> & odom, + int count, float step = 0.05f, int unpublished = 0) + { + for(int i=0; ipublish(makeXYZCloud("lidar", 1.0 + 0.1*i, corner3D(cv::Point3f(step*i, 0, 0)))); + const int expected = i+1 - unpublished; + if(expected > 0) + { + spinUntil([&]() { return int(odom->size()) >= expected; }); + } + else + { + spinFor(std::chrono::milliseconds(200)); + } + } + } + + /// A scan with nothing in it to register against: three points on a line. + sensor_msgs::msg::PointCloud2 degenerateCloud(double stamp) + { + // A short line off in free space: enough points for the scan_normal_k neighbours + // the node asks for, and jittered, because a perfectly straight one has no + // second axis for the normals to be fitted against and some PCL versions return + // them as NaN -- which empties the scan and has the odometry refuse it for that + // rather than for its shape. Its structural complexity stays near 0.016, under + // the 0.02 that Icp/PointToPlaneMinComplexity asks of a frame to start a map on. + cv::RNG rng(0xDECAF); + std::vector line; + for(int i=0; i<20; ++i) + { + line.push_back(cv::Point3f(1.0f + 0.05f*float(i), + float(rng.gaussian(0.002)), float(rng.gaussian(0.002)))); + } + return makeXYZCloud("lidar", stamp, line); + } + + /// Counts the transforms matching @p parent -> @p child seen so far. + static size_t countTransforms( + const std::shared_ptr> & tf, + const std::string & parent, const std::string & child) + { + size_t found = 0; + for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages) + { + for(const geometry_msgs::msg::TransformStamped & t : msg->transforms) + { + found += (t.header.frame_id == parent && t.child_frame_id == child) ? 1 : 0; + } + } + return found; + } + + /** + * @brief Publishes wheel_odom -> base_link driving straight at @p speed m/s. + * + * Sampled at 100 Hz across the whole window and sent in chunks: tf2's listener reads + * /tf on its own thread with a bounded queue, and a burst large enough to overflow it + * drops the oldest transforms -- the ones the first frames need. + */ + bool publishGuessMotion(double from, double to, double speed) + { + rclcpp::Publisher::SharedPtr tf = + helper()->create_publisher("/tf", rclcpp::QoS(200)); + std::shared_ptr> echo = + collect("/tf", rclcpp::QoS(200)); + if(!waitForSubscriber(tf, 2)) + { + return false; + } + size_t published = 0; + for(double t = from; t <= to; t += 0.01) + { + geometry_msgs::msg::TransformStamped pose; + pose.header.stamp = stampOf(t); + pose.header.frame_id = "wheel_odom"; + pose.child_frame_id = "base_link"; + pose.transform.translation.x = speed * (t - from); + pose.transform.rotation.w = 1.0; + tf2_msgs::msg::TFMessage message; + message.transforms.push_back(pose); + tf->publish(message); + if(++published % 10 == 0) + { + spinFor(std::chrono::milliseconds(5)); + } + } + guessMotionTf_ = tf; + return spinUntil([&]() { return echo->size() >= published; }); + } + + /** + * @brief Publishes odom -> base_link as another node would: a fused estimate. + * + * This is the "sensor fusion is used" case the reset paths look for -- the output of + * robot_localization, say, publishing the same odom frame this node reports in. Static + * here so that the lookup succeeds whatever stamp a frame carries. + */ + void publishFusedPose(double x) + { + fusedTf_ = std::make_shared(*helper()); + geometry_msgs::msg::TransformStamped fused; + fused.header.stamp = helper()->now(); + fused.header.frame_id = "odom"; + fused.child_frame_id = "base_link"; + fused.transform.translation.x = x; + fused.transform.rotation.w = 1.0; + fusedTf_->sendTransform(fused); + spinFor(std::chrono::milliseconds(200)); + } + + /// Calls an Empty service on the node and waits for it to return. + /// + /// The node advertises these under its own name -- /icp_odometry/reset_odom, not + /// /reset_odom -- so the bare name a client would resolve against the namespace is + /// not the right one. + bool callEmptyService(const std::string & name, const std::string & node = "icp_odometry") + { + rclcpp::Client::SharedPtr client = + helper()->create_client("/" + node + "/" + name); + if(!spinUntil([&]() { return client->service_is_ready(); })) + { + return false; + } + std::shared_future future = + client->async_send_request( + std::make_shared()).future.share(); + return spinUntil([&]() { + return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; }); + } + +protected: + std::shared_ptr staticTf_; + std::shared_ptr guessTf_; + rclcpp::Publisher::SharedPtr guessMotionTf_; + std::shared_ptr fusedTf_; +}; + +/** + * always_process_most_recent_frame is a "skip the backlog" policy, and it is on by + * default: a frame that arrives while the previous one is still being registered is + * dropped rather than queued, so the odometry stays on the newest data instead of falling + * further and further behind a sensor it cannot keep up with. Registration holds the data + * mutex for its whole duration, and a frame that cannot take that mutex is the one that + * gets dropped. + * + * Every other test in these suites turns the policy off, to be able to account for each + * frame; this pair is where the default itself is covered. + * + * topic_queue_size is raised because icp_odometry defaults to 1: with a queue that deep + * the middleware would drop the burst before the node ever saw it, and the test would + * pass without exercising anything. + */ +TEST_F(OdometryRosTest, drops_frames_that_arrive_while_the_previous_one_is_registering) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("always_process_most_recent_frame", true), + rclcpp::Parameter("topic_queue_size", 20)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + + // A burst with no spin in between, so the executor hands the node its second frame + // while the worker thread is still inside the first registration. + const size_t burst = 10; + for(size_t i=0; ipublish(makeXYZCloud("lidar", 1.0 + 0.1*i, corner3D(cv::Point3f(0.05f*i, 0, 0)))); + } + spinFor(std::chrono::milliseconds(2000)); + + EXPECT_GE(odom->size(), 1u) << "the burst produced no odometry at all"; + EXPECT_LT(odom->size(), burst) + << "every frame of the burst came back out, so nothing was skipped"; +} + +/// With the policy off, the same burst is registered whole, one pose per frame. +TEST_F(OdometryRosTest, processes_every_frame_of_a_burst_when_the_policy_is_off) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + // always_process_most_recent_frame:=false comes from the fixture. + makeNode({rclcpp::Parameter("topic_queue_size", 20)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + + const size_t burst = 10; + for(size_t i=0; ipublish(makeXYZCloud("lidar", 1.0 + 0.1*i, corner3D(cv::Point3f(0.05f*i, 0, 0)))); + } + + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= burst; })) + << "only " << odom->size() << " of " << burst << " frames came back out"; + spinFor(std::chrono::milliseconds(200)); + EXPECT_EQ(burst, odom->size()) << "more poses than frames"; +} + +/// The frames the pose is published in are both configurable. +TEST_F(OdometryRosTest, publishes_in_the_configured_frames) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("odom_frame_id", "custom_odom")}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 1); + + ASSERT_FALSE(odom->empty()); + EXPECT_EQ("custom_odom", odom->back().header.frame_id); + EXPECT_EQ("base_link", odom->back().child_frame_id); +} + +/// publish_tf broadcasts odom -> base_link, which is how the rest of the system sees the pose. +TEST_F(OdometryRosTest, broadcasts_the_odom_to_base_transform) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr> tf = + collect("/tf"); + makeNode({rclcpp::Parameter("publish_tf", true)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 2); + + ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); })); + bool found = false; + for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages) + { + for(const geometry_msgs::msg::TransformStamped & t : msg->transforms) + { + found = found || (t.header.frame_id == "odom" && t.child_frame_id == "base_link"); + } + } + EXPECT_TRUE(found); +} + +/** + * With publish_tf off nothing is broadcast, which is what you want when another node -- + * robot_localization, typically -- already owns odom -> base_link. + */ +TEST_F(OdometryRosTest, publishes_no_transform_when_publish_tf_is_off) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr> tf = + collect("/tf"); + makeNode({rclcpp::Parameter("publish_tf", false)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 2); + ASSERT_FALSE(odom->empty()) << "the node must still publish odometry"; + + for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages) + { + for(const geometry_msgs::msg::TransformStamped & t : msg->transforms) + { + EXPECT_FALSE(t.header.frame_id == "odom" && t.child_frame_id == "base_link"); + } + } +} + +/** + * With a guess frame configured the node publishes a correction rather than the pose: + * odom -> guess_frame_id, leaving guess_frame_id -> base_link to the guess source. This + * is what keeps the TF tree connected while registration is lost -- see "It also keeps TF + * alive through a failure" in the README. + */ +TEST_F(OdometryRosTest, broadcasts_a_correction_to_the_guess_frame_when_one_is_configured) +{ + publishSensorTf(); + // The guess source owns this half of the chain. + geometry_msgs::msg::TransformStamped guess; + guess.header.stamp = helper()->now(); + guess.header.frame_id = "odom"; + guess.child_frame_id = "base_link"; + guess.transform.rotation.w = 1.0; + guessTf_ = std::make_shared(*helper()); + guessTf_->sendTransform(guess); + + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr> tf = + collect("/tf"); + // As the launch files in this repository do it: the guess source keeps the + // conventional /odom frame, and this node takes a distinct one. + makeNode({rclcpp::Parameter("publish_tf", true), + rclcpp::Parameter("odom_frame_id", "icp_odom"), + rclcpp::Parameter("guess_frame_id", "odom")}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 2); + ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); })); + + bool correction = false; + bool directPose = false; + for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages) + { + for(const geometry_msgs::msg::TransformStamped & t : msg->transforms) + { + correction = correction || (t.header.frame_id == "icp_odom" && t.child_frame_id == "odom"); + directPose = directPose || (t.header.frame_id == "icp_odom" && t.child_frame_id == "base_link"); + } + } + EXPECT_TRUE(correction) << "expected the icp_odom -> odom correction"; + EXPECT_FALSE(directPose) << "the link to base_link belongs to the guess source, not this node"; +} + +/** + * Naming the guess frame the same as odom_frame_id would publish a frame as its own + * parent, so the node drops the guess and warns rather than doing it. The pose then goes + * out the ordinary way, odom -> base_link. + */ +TEST_F(OdometryRosTest, disables_the_guess_when_its_frame_matches_odom_frame_id) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr> tf = + collect("/tf"); + std::shared_ptr node = + makeNode({rclcpp::Parameter("publish_tf", true), + rclcpp::Parameter("odom_frame_id", "odom"), + rclcpp::Parameter("guess_frame_id", "odom")}); + + EXPECT_TRUE(node->guessFrameId().empty()) << "the guess should have been disabled"; + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 2); + ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); })); + + bool directPose = false; + for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages) + { + for(const geometry_msgs::msg::TransformStamped & t : msg->transforms) + { + directPose = directPose || (t.header.frame_id == "odom" && t.child_frame_id == "base_link"); + } + } + EXPECT_TRUE(directPose) << "with the guess disabled the pose is published directly"; +} + +/// initial_pose starts the trajectory somewhere other than the origin. +TEST_F(OdometryRosTest, starts_from_the_configured_initial_pose) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("initial_pose", "1 2 3 0 0 0")}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 1); + + ASSERT_FALSE(odom->empty()); + EXPECT_NEAR(1.0, odom->back().pose.pose.position.x, 1e-3); + EXPECT_NEAR(2.0, odom->back().pose.pose.position.y, 1e-3); + EXPECT_NEAR(3.0, odom->back().pose.pose.position.z, 1e-3); +} + +/// reset_odom puts the pose back to the identity and starts the map again. +TEST_F(OdometryRosTest, reset_odom_returns_the_pose_to_the_origin) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode(); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 3); + ASSERT_GE(odom->size(), 3u); + ASSERT_GT(odom->back().pose.pose.position.x, 0.01) << "should have travelled before reset"; + + ASSERT_TRUE(callEmptyService("reset_odom")); + + const size_t before = odom->size(); + pub->publish(makeXYZCloud("lidar", 2.0, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; })); + EXPECT_NEAR(0.0, odom->back().pose.pose.position.x, 1e-6); +} + +/// reset_odom_to_pose does the same, to a pose of your choosing. +TEST_F(OdometryRosTest, reset_odom_to_pose_sets_the_given_pose) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode(); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 2); + + rclcpp::Client::SharedPtr client = + helper()->create_client("/icp_odometry/reset_odom_to_pose"); + ASSERT_TRUE(spinUntil([&]() { return client->service_is_ready(); })); + std::shared_ptr request = + std::make_shared(); + request->x = 5.0; + request->y = -2.0; + std::shared_future future = + client->async_send_request(request).future.share(); + ASSERT_TRUE(spinUntil([&]() { + return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; })); + + const size_t before = odom->size(); + pub->publish(makeXYZCloud("lidar", 3.0, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; })); + EXPECT_NEAR(5.0, odom->back().pose.pose.position.x, 1e-3); + EXPECT_NEAR(-2.0, odom->back().pose.pose.position.y, 1e-3); +} + +/** + * A reset is announced to whatever consumes this topic by publishing both covariances + * bad: there is a pose, but it is an initialisation rather than a registration and there + * is no velocity behind it yet. The same pair of values goes out on the very first frame + * of a session, which is the same situation. + * + * They are bad for different reasons, and do not carry the same number. The pose + * covariance is RTAB-Map's registration covariance doubled, and registration reports + * BAD_COVARIANCE for a frame it could not link, so the pose comes out at 19998. The twist + * covariance is set to BAD_COVARIANCE directly, 9999, whenever no velocity is available. + * Tracking normally, both drop to the real values -- around 1e-8 on this synthetic scene. + * + * rtabmap_slam starts a new mapping session when it sees this, because the new pose + * cannot be linked to the previous one. + */ +TEST_F(OdometryRosTest, marks_both_covariances_bad_on_the_frame_after_a_reset) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode(); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 3); + ASSERT_GE(odom->size(), 3u); + // Tracking normally: a velocity is available, so the twist covariance is real. + EXPECT_LT(odom->back().twist.covariance[0], 9999.0); + + ASSERT_TRUE(callEmptyService("reset_odom")); + + const size_t before = odom->size(); + pub->publish(makeXYZCloud("lidar", 5.0, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; })); + + EXPECT_GE(odom->back().twist.covariance[0], 9999.0) + << "the first frame after a reset has no velocity, and must say so"; + EXPECT_GE(odom->back().pose.covariance[0], 9999.0) + << "and it is an initialization rather than a registration"; +} + +/** + * Same after reset_odom_to_pose, where the pose is non-identity and perfectly usable. + * rtabmap_slam relies on this: it starts a new mapping session when it sees an identity + * pose, or both covariances bad, so a reset to an arbitrary pose is still recognized as + * the discontinuity it is. + */ +TEST_F(OdometryRosTest, marks_both_covariances_bad_after_reset_to_pose) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode(); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 3); + + rclcpp::Client::SharedPtr client = + helper()->create_client("/icp_odometry/reset_odom_to_pose"); + ASSERT_TRUE(spinUntil([&]() { return client->service_is_ready(); })); + std::shared_ptr request = + std::make_shared(); + request->x = 3.0; + std::shared_future future = + client->async_send_request(request).future.share(); + ASSERT_TRUE(spinUntil([&]() { + return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; })); + + const size_t before = odom->size(); + pub->publish(makeXYZCloud("lidar", 5.0, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; })); + + // Both covariances are bad, even though the pose itself is perfectly usable: the frame + // after a reset is an initialization, not a registration. That pair is what + // rtabmap_slam tests for to start a new mapping session. + EXPECT_GE(odom->back().twist.covariance[0], 9999.0); + EXPECT_GE(odom->back().pose.covariance[0], 9999.0); + EXPECT_NEAR(3.0, odom->back().pose.pose.position.x, 1e-3); +} + +/// pause_odom stops processing entirely; resume_odom starts it again. +TEST_F(OdometryRosTest, pause_and_resume_stop_and_restart_processing) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr node = makeNode(); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 1); + ASSERT_FALSE(odom->empty()); + + ASSERT_TRUE(callEmptyService("pause_odom")); + EXPECT_TRUE(node->isPaused()); + + const size_t whilePaused = odom->size(); + pub->publish(makeXYZCloud("lidar", 2.0, corner3D())); + spinFor(std::chrono::milliseconds(500)); + EXPECT_EQ(whilePaused, odom->size()) << "nothing should be processed while paused"; + + ASSERT_TRUE(callEmptyService("resume_odom")); + EXPECT_FALSE(node->isPaused()); + + pub->publish(makeXYZCloud("lidar", 3.0, corner3D())); + EXPECT_TRUE(spinUntil([&]() { return odom->size() > whilePaused; })); +} + +/// The log-level services are advertised; they change RTAB-Map's own verbosity. +TEST_F(OdometryRosTest, advertises_the_log_level_services) +{ + publishSensorTf(); + makeNode(); + + for(const char * const name : {"log_debug", "log_info", "log_warning", "log_error"}) + { + EXPECT_TRUE(callEmptyService(name)) << name << " did not answer"; + } +} + +/** + * publish_null_when_lost:=false also suppresses the *successful* frame after a reset. + * + * That frame has a pose but no velocity, and the publish is gated on + * `setTwist || publish_null_when_lost` -- so with null publishing off, the one message + * carrying the reset's bad covariances never goes out. The next frame has a velocity + * again and is published with ordinary covariances. + * + * The consequence is worth knowing: rtabmap never sees the reset, and does not start a + * new mapping session -- so this parameter silently changes mapping behaviour as well as + * the lost signal it is named for. + */ +TEST_F(OdometryRosTest, suppresses_the_post_reset_frame_when_null_publishing_is_off) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("publish_null_when_lost", false)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 4); + ASSERT_FALSE(odom->empty()); + + ASSERT_TRUE(callEmptyService("reset_odom")); + + // The frame that would have carried the reset signal is not published at all. + size_t before = odom->size(); + pub->publish(makeXYZCloud("lidar", 6.0, corner3D())); + spinFor(std::chrono::milliseconds(1000)); + EXPECT_EQ(before, odom->size()) + << "the post-reset frame has no velocity, so it is gated away"; + + // The frame after that has a velocity again, and looks entirely ordinary. + before = odom->size(); + pub->publish(makeXYZCloud("lidar", 6.1, corner3D(cv::Point3f(0.05f, 0, 0)))); + ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; })); + EXPECT_LT(odom->back().pose.covariance[0], 9999.0); + EXPECT_LT(odom->back().twist.covariance[0], 9999.0); +} + + +// --------------------------------------------------------------------------- +// What happens while registration is lost, and the rate limits around it. +// --------------------------------------------------------------------------- + +/** + * A guess frame keeps TF alive through a failure: while registration is lost the node + * goes on publishing the correction, frozen at the last pose it computed and carrying + * whatever the guess source has moved since, so base_link keeps moving in TF instead of + * stalling. See "It also keeps TF alive through a failure" in the README. + */ +TEST_F(OdometryRosTest, keeps_broadcasting_the_correction_while_registration_is_lost) +{ + publishSensorTf(); + geometry_msgs::msg::TransformStamped guess; + guess.header.stamp = helper()->now(); + guess.header.frame_id = "odom"; + guess.child_frame_id = "base_link"; + guess.transform.rotation.w = 1.0; + guessTf_ = std::make_shared(*helper()); + guessTf_->sendTransform(guess); + + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr> tf = + collect("/tf"); + makeNode({rclcpp::Parameter("publish_tf", true), + rclcpp::Parameter("odom_frame_id", "icp_odom"), + rclcpp::Parameter("guess_frame_id", "odom")}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 2); + ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); })); + spinFor(std::chrono::milliseconds(300)); // let every transform of the good frames land + const size_t beforeLoss = countTransforms(tf, "icp_odom", "odom"); + + // Nothing registrable: the pose is lost, but the chain must not go quiet. + const size_t odomBefore = odom->size(); + pub->publish(degenerateCloud(5.0)); + ASSERT_TRUE(spinUntil([&]() { return odom->size() > odomBefore; })); + spinFor(std::chrono::milliseconds(300)); + + EXPECT_GE(odom->back().pose.covariance[0], 9999.0) << "the frame was expected to be lost"; + EXPECT_GT(countTransforms(tf, "icp_odom", "odom"), beforeLoss) + << "the correction stopped while lost, which breaks the TF tree downstream"; +} + +/// Without a guess frame there is no correction to publish, so TF stops while lost. +TEST_F(OdometryRosTest, broadcasts_no_transform_while_lost_without_a_guess_frame) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr> tf = + collect("/tf"); + makeNode({rclcpp::Parameter("publish_tf", true)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 2); + ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); })); + spinFor(std::chrono::milliseconds(300)); + const size_t beforeLoss = countTransforms(tf, "odom", "base_link"); + + const size_t odomBefore = odom->size(); + pub->publish(degenerateCloud(5.0)); + ASSERT_TRUE(spinUntil([&]() { return odom->size() > odomBefore; })); + spinFor(std::chrono::milliseconds(300)); + + EXPECT_GE(odom->back().pose.covariance[0], 9999.0) << "the frame was expected to be lost"; + EXPECT_EQ(beforeLoss, countTransforms(tf, "odom", "base_link")) + << "a pose was broadcast for a frame that did not register"; +} + +/// max_update_rate throttles registration, dropping the frames that arrive too soon. +TEST_F(OdometryRosTest, max_update_rate_skips_frames_that_arrive_too_soon) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("max_update_rate", 5.0), // one frame per 0.2 s + rclcpp::Parameter("topic_queue_size", 20)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + + // Ten frames a tenth of a second apart: twice the rate the node will accept. + for(int i=0; i<10; ++i) + { + pub->publish(makeXYZCloud("lidar", 1.0 + 0.1*i, corner3D(cv::Point3f(0.02f*i, 0, 0)))); + spinFor(std::chrono::milliseconds(50)); + } + spinFor(std::chrono::milliseconds(300)); + + // Four of the ten got through on this machine; the rate allows about five. + EXPECT_GE(odom->size(), 2u) << "the throttle swallowed everything"; + EXPECT_LE(odom->size(), 7u) << "ten frames at twice the rate should not all register"; +} + +/** + * min_update_rate is the other end: when the gap between frames grows beyond it the + * motion assumption no longer holds, so the odometry is reset rather than continued. + */ +TEST_F(OdometryRosTest, min_update_rate_resets_when_a_frame_arrives_too_late) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("min_update_rate", 2.0)}); // half a second + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 3); + ASSERT_GE(odom->size(), 3u); + + // Two seconds later, well past 1/min_update_rate. + const double previous = odom->back().pose.pose.position.x; + const size_t before = odom->size(); + pub->publish(makeXYZCloud("lidar", 5.0, corner3D(cv::Point3f(0.1f, 0, 0)))); + ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; })); + // Registered as an initialisation rather than a continuation: both covariances bad, + // exactly as after an explicit reset. + EXPECT_GE(odom->back().pose.covariance[0], 9999.0); + EXPECT_GE(odom->back().twist.covariance[0], 9999.0); + // The pose itself carries on from where it was -- the reset drops the map, not the + // place the robot had reached. + EXPECT_NEAR(previous, odom->back().pose.pose.position.x, 1e-3); +} + +/** + * Below guess_min_translation the node does not register at all: it forwards the guess as + * the pose and labels it with guess_linear_variance, which is how a consumer can tell the + * difference from a real registration. + */ +TEST_F(OdometryRosTest, skips_registration_when_the_guess_says_it_barely_moved) +{ + publishSensorTf(); + geometry_msgs::msg::TransformStamped guess; + guess.header.stamp = helper()->now(); + guess.header.frame_id = "wheel_odom"; + guess.child_frame_id = "base_link"; + guess.transform.rotation.w = 1.0; + guessTf_ = std::make_shared(*helper()); + guessTf_->sendTransform(guess); + + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("guess_frame_id", "wheel_odom"), + rclcpp::Parameter("guess_min_translation", 1.0), // a metre, never reached + rclcpp::Parameter("guess_linear_variance", 0.042)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 3); + ASSERT_GE(odom->size(), 2u); + // The published covariance is guess_linear_variance, doubled on the way out as every + // pose covariance is -- 0.084 for the 0.042 configured here. A registration would + // report its own, far smaller value, so this is what marks the frame as guess-only. + EXPECT_NEAR(2.0 * 0.042, odom->back().pose.covariance[0], 1e-6) + << "the frame was registered rather than taken from the guess"; +} + +/// Odom/ResetCountdown resets the odometry after that many consecutive lost frames. +TEST_F(OdometryRosTest, resets_itself_after_the_configured_number_of_lost_frames) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("Odom/ResetCountdown", "2")}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 3); + const double moved = odom->back().pose.pose.position.x; + + for(int i=0; i<2; ++i) + { + const size_t before = odom->size(); + pub->publish(degenerateCloud(5.0 + i)); + ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; })); + } + // A registrable frame again, after the countdown has run out. + const size_t before = odom->size(); + pub->publish(makeXYZCloud("lidar", 8.0, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; })); + // The countdown reset the odometry, so this frame initialises a new map from where + // the robot had got to -- 0.0999975 m along, with an initialisation's covariance. + // With the countdown disabled the same frame comes back null, at the origin: the + // stale map is still there and still does not match. + EXPECT_NEAR(moved, odom->back().pose.pose.position.x, 1e-3) + << "the recovered pose should continue from the last good one"; + EXPECT_GT(odom->back().pose.pose.position.x, 0.05) + << "a null pose at the origin means the odometry never recovered"; +} + + +/** + * The frame the odometry registered, republished on the auxiliary topics. + * + * These are not copies of the input: they carry what the registration worked on and what + * it produced. `odom_sensor_data/features` is the same message with the images and the + * scan stripped out, leaving the features alone. See "Outputting filtered scans and + * features" in the README. + * + * `odom_local_map` and `odom_last_frame` are not among them: both are built from the + * frame's visual words, so a lidar-only run publishes neither. `odom_local_scan_map` is + * the ICP path's equivalent, and the visual pair is covered in test_rgbd_odometry.cpp. + */ +TEST_F(OdometryRosTest, republishes_the_registered_frame_on_the_auxiliary_topics) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr> lite = + collect("odom_info_lite"); + std::shared_ptr> scanMap = + collect("odom_local_scan_map"); + std::shared_ptr> raw = + collect("odom_sensor_data/raw"); + std::shared_ptr> features = + collect("odom_sensor_data/features"); + makeNode(); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 3); + ASSERT_TRUE(spinUntil([&]() { + return !lite->empty() && !scanMap->empty() && !raw->empty() && !features->empty(); })) + << "lite=" << lite->size() << " scan_map=" << scanMap->size() + << " raw=" << raw->size() << " features=" << features->size(); + + EXPECT_GT(raw->back().laser_scan.data.size(), 0u) + << "the raw topic should carry the scan the odometry registered"; + EXPECT_EQ(0u, features->back().laser_scan.data.size()) + << "the features topic is the same message with the scan and images removed"; + EXPECT_GT(scanMap->back().width, 0u) << "the scan map the frame was registered against"; +} + +/// publish_compressed_sensor_data swaps the raw images and scan for compressed ones. +TEST_F(OdometryRosTest, publishes_compressed_sensor_data_when_asked) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr> compressed = + collect("odom_sensor_data/compressed"); + makeNode({rclcpp::Parameter("publish_compressed_sensor_data", true)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 2); + ASSERT_TRUE(spinUntil([&]() { return !compressed->empty(); })) + << "nothing was published on odom_sensor_data/compressed"; + // The scan travels compressed instead of raw: ~25 kB against an empty raw field. + EXPECT_GT(compressed->back().laser_scan_compressed.size(), 0u) + << "nothing was compressed into the message"; + EXPECT_EQ(0u, compressed->back().laser_scan.data.size()) + << "the raw scan should have been replaced by the compressed one"; +} + + +/** + * The whole recovery story, as a wheeled robot would live it: a guess frame it trusts, + * silence while lost, and a pose that is still right when registration comes back. + * + * The robot drives at 1 m/s. Registration fails on the first degenerate scan and, with + * Odom/ResetCountdown at 1, the odometry resets immediately onto the guess. It stays lost + * for a second -- ten frames at 10 Hz, a metre of travel -- publishing nothing at all, + * because publish_null_when_lost is off. When a registrable scan arrives again the node + * reports a usable pose, and that pose is where the wheels say the robot is: a metre on. + */ +TEST_F(OdometryRosTest, recovers_on_the_guess_after_a_metre_of_being_lost) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("guess_frame_id", "wheel_odom"), + rclcpp::Parameter("publish_null_when_lost", false), + rclcpp::Parameter("Odom/ResetCountdown", "1"), + rclcpp::Parameter("wait_for_transform", 2.0)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + // 1 m/s from t=1.0 to t=2.5, which covers every frame below: x(t) = t - 1.0. + const double speed = 1.0; // m/s, and the frames below are 10 Hz + ASSERT_TRUE(publishGuessMotion(1.0, 2.5, speed)); + + // Tracking normally at the start. The first frame has no velocity behind it yet, so + // with null publishing off it is not published either -- the second one is. + pub->publish(makeXYZCloud("lidar", 1.0, corner3D())); + spinFor(std::chrono::milliseconds(200)); + pub->publish(makeXYZCloud("lidar", 1.1, corner3D(cv::Point3f(float(0.1*speed), 0, 0)))); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })) << "never started tracking"; + const size_t whileTracking = odom->size(); + + // A second of nothing to register against, at 10 Hz, while the wheels carry on. + for(int i=2; i<=11; ++i) + { + pub->publish(degenerateCloud(1.0 + 0.1*i)); + spinFor(std::chrono::milliseconds(60)); + } + const size_t whileLost = odom->size(); + + // The scene comes back, seen from where the wheels say the robot now is. The frame + // that restarts the map is published as a continuation of the old one (see + // publishes_the_restarting_frame_as_a_continuation_of_the_guess below); a second + // frame follows so that what is asserted here is a real registration. + pub->publish(makeXYZCloud("lidar", 2.2, corner3D(cv::Point3f(float(1.2*speed), 0, 0)))); + spinFor(std::chrono::milliseconds(200)); + pub->publish(makeXYZCloud("lidar", 2.3, corner3D(cv::Point3f(float(1.3*speed), 0, 0)))); + ASSERT_TRUE(spinUntil([&]() { return odom->size() > whileLost; })) + << "never recovered once there was something to register again"; + spinFor(std::chrono::milliseconds(500)); // give any second message time to arrive + + // Nothing goes missing while lost either. With a guess to fall back on, a frame that + // re-initialises the map is published rather than dropped, carrying the guess's pose + // and its confidence. Whether any of the degenerate frames manages to initialise a map + // at all differs between builds, so what is checked is what such a frame carries + // rather than how many of them there are. + for(size_t i=whileTracking; imessages[i]->pose.covariance[0]) + << "a pose published while lost should carry the guess's confidence, " + << "message " << i << " of " << whileLost; + } + + // Tracking again, on a real registration rather than the guess. + EXPECT_LT(odom->back().pose.covariance[0], 9999.0) << "still lost after recovering"; + + // And the pose is where the wheels say the robot is: 1.3 m, to the millimetre. + // + // This used to come back 0.1 m short, and the shortfall accumulated -- 0.1, 0.2, 0.3 m + // after one, two and three loss-and-recovery cycles. The guess for the frame that + // re-initialises the map is now folded into the pose before that frame is processed, + // so the new map is anchored where the robot actually is. + const double truth = 1.3 * speed; + EXPECT_NEAR(truth, odom->back().pose.pose.position.x, 0.02) + << "the recovered pose does not match where the guess says the robot is"; + EXPECT_NEAR(0.0, odom->back().pose.pose.position.y, 0.02) + << "the robot drove straight"; +} + + +/** + * The same recovery contract on the other reset path: min_update_rate. + * + * The robot drives at 1 m/s and a second passes without a frame -- twice the configured + * limit -- so the odometry is reset. A guess frame is available throughout, and the pose + * after the gap should be where the wheels say the robot is. + * + * This used to report 0.2 m against a true 1.2, losing the whole gap rather than one + * frame of it: tooOldPreviousData was handled before the guess for the frame was worked + * out, so guess_ was still null from the last successful frame, the "reset based on + * latest guess available from TF" branch never ran, and the node carried on from where + * it was when it went quiet. The reset now happens after the guess is computed. + */ +TEST_F(OdometryRosTest, recovers_on_the_guess_after_min_update_rate_resets_it) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("guess_frame_id", "wheel_odom"), + rclcpp::Parameter("min_update_rate", 2.0), // half a second + rclcpp::Parameter("wait_for_transform", 2.0)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(publishGuessMotion(1.0, 2.5, 1.0)); // 1 m/s, x(t) = t - 1.0 + + const auto drive = [&](double stamp) { + pub->publish(makeXYZCloud("lidar", stamp, + corner3D(cv::Point3f(float(stamp - 1.0), 0, 0)))); + spinFor(std::chrono::milliseconds(120)); + }; + + drive(1.0); + drive(1.1); // tracking, a tenth of a metre along + drive(2.1); // a second later: the gap resets the odometry + drive(2.2); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; })); + spinFor(std::chrono::milliseconds(200)); + + EXPECT_LT(odom->back().pose.covariance[0], 9999.0) << "still lost after the gap"; + EXPECT_NEAR(1.2, odom->back().pose.pose.position.x, 0.02) + << "the pose after the gap does not match the guess"; +} + +/** + * @brief The same metre of being lost, in the default configuration. + * + * `publish_null_when_lost` on is what most robots run: every lost frame is announced with + * a null pose, and the frame that restarts the map says it cannot be linked to what came + * before. What must not differ is *where* the odometry restarts -- the guess is folded + * into the pose either way, so the trajectory has to come back where the wheels say it is. + */ +TEST_F(OdometryRosTest, recovers_on_the_guess_after_a_metre_of_being_lost_announcing_each_loss) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("guess_frame_id", "wheel_odom"), + rclcpp::Parameter("Odom/ResetCountdown", "1"), + // Point to plane, so the scan's own shape is what decides whether a frame + // is good enough to start a map on, rather than how ICP happened to fail. + // The threshold sits between the two scenes this test feeds it: the corner + // measures 0.016 and the line of degenerateCloud() 0.0001, and the 0.02 + // default would turn away both. + rclcpp::Parameter("Icp/PointToPlane", "true"), + rclcpp::Parameter("Icp/PointToPlaneMinComplexity", "0.005"), + rclcpp::Parameter("wait_for_transform", 2.0)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + // 1 m/s from t=1.0 to t=2.5, which covers every frame below: x(t) = t - 1.0. + const double speed = 1.0; + ASSERT_TRUE(publishGuessMotion(1.0, 2.5, speed)); + + pub->publish(makeXYZCloud("lidar", 1.0, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })) << "never started tracking"; + pub->publish(makeXYZCloud("lidar", 1.1, corner3D(cv::Point3f(float(0.1*speed), 0, 0)))); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; })); + const size_t whileTracking = odom->size(); + + // A second of nothing to register against, at 10 Hz, while the wheels carry on. + for(int i=2; i<=11; ++i) + { + pub->publish(degenerateCloud(1.0 + 0.1*i)); + spinFor(std::chrono::milliseconds(60)); + } + + // Every one of them is announced, which is the whole point of the default. + ASSERT_GT(odom->size(), whileTracking) << "being lost was never announced"; + EXPECT_GE(odom->back().pose.covariance[0], 9999.0) + << "a usable pose while there was nothing to register"; + + // The scene comes back, seen from where the wheels say the robot now is. Two frames: + // the line was never good enough to start a map on, so the first corner does that + // and the second is the first that has something to be registered against. + pub->publish(makeXYZCloud("lidar", 2.2, corner3D(cv::Point3f(float(1.2*speed), 0, 0)))); + spinFor(std::chrono::milliseconds(200)); + pub->publish(makeXYZCloud("lidar", 2.3, corner3D(cv::Point3f(float(1.3*speed), 0, 0)))); + ASSERT_TRUE(spinUntil([&]() { return odom->back().pose.covariance[0] < 9999.0; })) + << "never recovered once there was something to register again"; + + // And the trajectory resumes where the wheels say, exactly as with null publishing off. + EXPECT_NEAR(1.3*speed, odom->back().pose.pose.position.x, 0.02) + << "the recovered pose does not match where the guess says the robot is"; + EXPECT_NEAR(0.0, odom->back().pose.pose.position.y, 0.02) << "the robot drove straight"; +} + + +/** + * The same service with a guess frame configured restarts the pose there instead. + * + * Resetting to the identity would put the odometry somewhere the robot has not been, and + * the guess source knows better: it has been tracking the whole time. So the pose picks up + * the guess frame's current pose and the map is rebuilt around it. + * + * reset_odom_to_pose is not affected -- a pose asked for explicitly is left alone. + */ +TEST_F(OdometryRosTest, reset_odom_restarts_from_the_guess_frame_when_one_is_configured) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("guess_frame_id", "wheel_odom"), + rclcpp::Parameter("wait_for_transform", 2.0)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(publishGuessMotion(1.0, 3.0, 1.0)); // 1 m/s, so the frame is at x = t - 1.0 + + const auto drive = [&](double stamp) { + pub->publish(makeXYZCloud("lidar", stamp, + corner3D(cv::Point3f(float(stamp - 1.0), 0, 0)))); + spinFor(std::chrono::milliseconds(120)); + }; + + drive(1.0); + drive(1.1); + drive(1.2); + ASSERT_FALSE(odom->empty()); + EXPECT_NEAR(0.2, odom->back().pose.pose.position.x, 0.02) << "not tracking before the reset"; + + ASSERT_TRUE(callEmptyService("reset_odom")); + + const size_t before = odom->size(); + drive(1.3); // seeds the pose from the guess frame and initialises the map there + drive(1.4); // registers against it + ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; })); + spinFor(std::chrono::milliseconds(200)); + + // 0.4 m: where the guess frame is at that stamp, not the origin the service name + // suggests. Without a guess frame the same call does return to the origin, which is + // what reset_odom_returns_the_pose_to_the_origin covers. + EXPECT_NEAR(0.4, odom->back().pose.pose.position.x, 0.02) + << "the reset did not restart from the guess frame"; +} + + +/** + * With no guess frame, a reset looks for a fused pose in TF before falling back. + * + * If something else publishes odom -> base_link -- an EKF fusing wheels and IMU, say -- + * then that is a better answer than the pose this node had when it lost tracking, and the + * automatic reset adopts it. Without it, the reset keeps the latest computed pose, which + * the other reset tests cover. + */ +TEST_F(OdometryRosTest, reset_countdown_adopts_a_fused_pose_from_tf_when_there_is_one) +{ + publishSensorTf(); + publishFusedPose(3.0); + std::shared_ptr> odom = + collect("odom"); + // The configuration this branch is written for: another node owns odom -> base_link, + // so this one does not publish it, and a filter is consuming the odom topic, so null + // poses are suppressed rather than fed to it. + makeNode({rclcpp::Parameter("Odom/ResetCountdown", "1"), + rclcpp::Parameter("publish_tf", false), + rclcpp::Parameter("publish_null_when_lost", false)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + // One less message than frames: the first one initialises the odometry. + feedFrames(pub, odom, 3, 0.05f, 1); + ASSERT_GE(odom->size(), 2u); + + // Lose it: the countdown fires and goes looking for a pose in TF. Nothing is published + // for this frame, nor for the one that initialises the map after it -- that one has no + // velocity behind it yet. + const size_t whileTracking = odom->size(); + pub->publish(degenerateCloud(5.0)); + spinFor(std::chrono::milliseconds(300)); + EXPECT_EQ(whileTracking, odom->size()) << "a null pose went out with null publishing off"; + + // Something to register against again: the first initialises the map at the adopted + // pose, the second registers against it and is published. + pub->publish(makeXYZCloud("lidar", 6.0, corner3D())); + spinFor(std::chrono::milliseconds(300)); + pub->publish(makeXYZCloud("lidar", 6.1, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return odom->size() > whileTracking; })) + << "nothing came back after the odometry recovered"; + + // The robot has not moved since, so the pose is the one the reset adopted. + EXPECT_NEAR(3.0, odom->back().pose.pose.position.x, 0.02) + << "the reset did not adopt the fused pose published on odom -> base_link"; +} + +/// The same fallback on the other reset path, the one min_update_rate triggers. +TEST_F(OdometryRosTest, min_update_rate_adopts_a_fused_pose_from_tf_when_there_is_one) +{ + publishSensorTf(); + publishFusedPose(2.0); + std::shared_ptr> odom = + collect("odom"); + // Same configuration as above: the fused source owns odom -> base_link, and a filter + // downstream means null poses are suppressed. + makeNode({rclcpp::Parameter("min_update_rate", 2.0), // half a second + rclcpp::Parameter("publish_tf", false), + rclcpp::Parameter("publish_null_when_lost", false)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + // One less message than frames: the first one initialises the odometry. + feedFrames(pub, odom, 3, 0.05f, 1); + ASSERT_GE(odom->size(), 2u); + + // A frame well past the limit: the odometry is reset, and TF has a better pose. That + // frame initialises the map and is not published, having no velocity behind it; the + // next one registers against it and is. + const size_t before = odom->size(); + pub->publish(makeXYZCloud("lidar", 5.0, corner3D())); + spinFor(std::chrono::milliseconds(300)); + pub->publish(makeXYZCloud("lidar", 5.1, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; })) + << "nothing came back after the gap"; + + EXPECT_NEAR(2.0, odom->back().pose.pose.position.x, 0.02) + << "the reset did not adopt the fused pose published on odom -> base_link"; +} + + +/** + * @brief With the guess frame vouching for it, a reset is published rather than swallowed. + * + * The frame that restarts the map has nothing to measure against, so it normally carries + * 9999 on both covariances -- rtabmap's signal to start a new map rather than link across + * a jump. With `publish_null_when_lost:=false` that frame was not published at all: the + * map stayed in one piece, but its consumer lost a pose. It now goes out carrying the + * guess frame's own confidence and velocity, which is what makes the poses *and* the + * covariances on this topic continuous for as long as the guess is there. + */ +TEST_F(OdometryRosTest, publishes_the_restarting_frame_as_a_continuation_of_the_guess) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("guess_frame_id", "wheel_odom"), + rclcpp::Parameter("publish_null_when_lost", false), + rclcpp::Parameter("Odom/ResetCountdown", "1"), + rclcpp::Parameter("wait_for_transform", 2.0)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + const double speed = 1.0; // m/s, frames at 10 Hz: x(t) = t - 1.0 + ASSERT_TRUE(publishGuessMotion(1.0, 2.0, speed)); + + // Tracking, then one frame with nothing in it, which the countdown turns into a reset. + pub->publish(makeXYZCloud("lidar", 1.0, corner3D())); + spinFor(std::chrono::milliseconds(200)); + pub->publish(makeXYZCloud("lidar", 1.1, corner3D(cv::Point3f(0.1f, 0, 0)))); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })) << "never started tracking"; + pub->publish(degenerateCloud(1.2)); + spinFor(std::chrono::milliseconds(200)); + const size_t beforeRestart = odom->size(); + + // The scene comes back: this is the frame that restarts the map. + pub->publish(makeXYZCloud("lidar", 1.3, corner3D(cv::Point3f(0.3f, 0, 0)))); + ASSERT_TRUE(spinUntil([&]() { return odom->size() > beforeRestart; })) + << "the frame that restarts the map was not published"; + + const nav_msgs::msg::Odometry restart = odom->back(); + EXPECT_LT(restart.pose.covariance[0], 9999.0) + << "the restarting frame still says it cannot be linked to what came before"; + EXPECT_LT(restart.twist.covariance[0], 9999.0) + << "rtabmap starts a new map when both covariances say 9999"; + // The guess's own confidence, doubled into the pose as everywhere else on this topic. + EXPECT_DOUBLE_EQ(0.002, restart.pose.covariance[0]); + EXPECT_DOUBLE_EQ(0.001, restart.twist.covariance[0]); + EXPECT_DOUBLE_EQ(0.001, restart.twist.covariance[21]); + // And the guess's own velocity, there being no registration to take one from. + EXPECT_NEAR(speed, restart.twist.twist.linear.x, 0.05) + << "the twist of the restarting frame does not match the guess it came from"; + EXPECT_NEAR(0.3*speed, restart.pose.pose.position.x, 0.02) + << "the map restarted somewhere other than where the guess says the robot is"; + + // From the next frame on it is an ordinary registration again, with its own covariance. + pub->publish(makeXYZCloud("lidar", 1.4, corner3D(cv::Point3f(0.4f, 0, 0)))); + ASSERT_TRUE(spinUntil([&]() { return odom->size() > beforeRestart + 1; })); + EXPECT_NE(0.002, odom->back().pose.covariance[0]) + << "still reporting the guess's confidence after the registration resumed"; + EXPECT_LT(odom->back().pose.covariance[0], 9999.0); +} + +/// Unchanged by default: the restart still announces itself, which is what starts a new map. +TEST_F(OdometryRosTest, marks_the_restarting_frame_as_unlinked_when_null_publishing_is_on) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("guess_frame_id", "wheel_odom"), + rclcpp::Parameter("Odom/ResetCountdown", "1"), + rclcpp::Parameter("wait_for_transform", 2.0)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(publishGuessMotion(1.0, 2.0, 1.0)); + + pub->publish(makeXYZCloud("lidar", 1.0, corner3D())); + spinFor(std::chrono::milliseconds(200)); + pub->publish(makeXYZCloud("lidar", 1.1, corner3D(cv::Point3f(0.1f, 0, 0)))); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })); + pub->publish(degenerateCloud(1.2)); + spinFor(std::chrono::milliseconds(200)); + const size_t beforeRestart = odom->size(); + + pub->publish(makeXYZCloud("lidar", 1.3, corner3D(cv::Point3f(0.3f, 0, 0)))); + ASSERT_TRUE(spinUntil([&]() { return odom->size() > beforeRestart; })); + + // The pose is the recovered one, but both covariances say it is not linked to the + // trajectory before it, which is how rtabmap knows to start a new map. + EXPECT_NEAR(0.3, odom->back().pose.pose.position.x, 0.02); + EXPECT_GE(odom->back().pose.covariance[0], 9999.0); + EXPECT_GE(odom->back().twist.covariance[0], 9999.0); +} + +} // namespace +} // namespace rtabmap_odom_test diff --git a/rtabmap_odom/test/test_rgbd_odometry.cpp b/rtabmap_odom/test/test_rgbd_odometry.cpp new file mode 100644 index 00000000..d3f7fb00 --- /dev/null +++ b/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 + +#include + +#include + +#include +#include + +#include + +#include + +#include + +#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(*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 makeNode( + std::vector 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 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 node = + addNode(std::make_shared(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::SharedPtr client = + helper()->create_client("/rgbd_odometry/" + name); + if(!spinUntil([&]() { return client->service_is_ready(); })) + { + return false; + } + std::shared_future future = + client->async_send_request( + std::make_shared()).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(*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 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::SharedPtr rgb = + helper()->create_publisher("rgb/image", 10); + rclcpp::Publisher::SharedPtr depth = + helper()->create_publisher("depth/image", 10); + rclcpp::Publisher::SharedPtr info = + helper()->create_publisher("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> odom = + collect("odom"); + makeNode(); + + rclcpp::Publisher::SharedPtr rgb = + helper()->create_publisher("rgb/image", 10); + rclcpp::Publisher::SharedPtr depth = + helper()->create_publisher("depth/image", 10); + rclcpp::Publisher::SharedPtr info = + helper()->create_publisher("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> odom = + collect("odom"); + makeNode({rclcpp::Parameter("subscribe_rgbd", true)}); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("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> odom = + collect("odom"); + std::shared_ptr> info = + collect("odom_info"); + makeNode({rclcpp::Parameter("subscribe_rgbd", true)}); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("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> odom = + collect("odom"); + makeNode({rclcpp::Parameter("subscribe_rgbd", true)}); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("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 +{ +}; + +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::SharedPtr> publishers; + for(int i=0; icreate_publisher( + "rgbd_image" + std::to_string(i), 10)); + } + + for(int i=0; i & 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::SharedPtr pub = + helper()->create_publisher("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 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> odom = + collect("odom"); + std::shared_ptr> info = + collect("odom_info"); + makeNode({rclcpp::Parameter("subscribe_rgbd", true)}); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("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> odom = + collect("odom"); + makeNode({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("publish_null_when_lost", false)}); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("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> frames = + collect("odom_rgbd_image"); + makeNode({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("keep_color", true)}); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("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> frames = + collect("odom_rgbd_image"); + std::shared_ptr node = + makeNode({rclcpp::Parameter("subscribe_rgbd", true)}); + EXPECT_FALSE(node->get_parameter("keep_color").as_bool()); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("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> odom = + collect("odom"); + std::shared_ptr> localMap = + collect("odom_local_map"); + std::shared_ptr> lastFrame = + collect("odom_last_frame"); + makeNode({rclcpp::Parameter("subscribe_rgbd", true)}); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("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 +{ +}; + +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> odom = + collect("odom"); + std::shared_ptr> info = + collect("odom_info"); + makeNode({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("rgbd_cameras", 0), + rclcpp::Parameter("Vis/EstimationType", std::to_string(estimationType))}); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("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; ipublish(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 & 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> odom = + collect("odom"); + std::shared_ptr> info = + collect("odom_info"); + makeNode({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("rgbd_cameras", 0)}); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("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; cpublish(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> odom = + collect("odom"); + makeNode({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("rgbd_cameras", 0), + rclcpp::Parameter("Odom/ImageDecimation", "2")}); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_images", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + const int frames = 11; + for(int i=0; ipublish(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> +{ +}; + +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> odom = + collect("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::SharedPtr> publishers; + for(int i=0; icreate_publisher( + cameras == 1 ? "rgbd_image" : "rgbd_image" + std::to_string(i), 10)); + } + for(int i=0; ipublish(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; cpublish(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> & 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 diff --git a/rtabmap_odom/test/test_stereo_odometry.cpp b/rtabmap_odom/test/test_stereo_odometry.cpp new file mode 100644 index 00000000..d400cb21 --- /dev/null +++ b/rtabmap_odom/test/test_stereo_odometry.cpp @@ -0,0 +1,1272 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +#include + +#include + +#include + +#include +#include + +#include + +#include + +#include + +#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(); + +/** + * These assert the ROS-level contract -- topics, parameters, what gets published -- on + * real input: the pairs in test/data/stereo, rectified ones from the set RTAB-Map + * registers in corelib/test/test_odometry.cpp and unrectified ones for the paths the + * documentation describes around Rtabmap/ImagesAlreadyRectified. The accuracy of the + * registration is that test's business; what is checked here is that a frame published on + * the four raw topics comes out of this node as a plausible, non-degenerate odometry + * message. + */ +const char * const kFirstFrame = "50"; +const char * const kSecondFrame = "60"; // ~15 cm of motion from the first + +/// Straight off the camera, still distorted: test/data/stereo/raw, 21.00 s and 21.25 s. +const char * const kFirstRawFrame = "420"; +const char * const kSecondRawFrame = "425"; + +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 StereoOdometryTest : public NodeTest +{ +protected: + void publishSensorTf() + { + staticTf_ = std::make_shared(*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; }); + } + + /// Calls one of the node's std_srvs/Empty services and waits for the answer. + bool callEmptyService(const std::string & name) + { + rclcpp::Client::SharedPtr client = + helper()->create_client("/stereo_odometry/" + name); + if(!spinUntil([&]() { return client->service_is_ready(); })) + { + return false; + } + std::shared_future future = + client->async_send_request( + std::make_shared()).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(*helper()); + staticTf_->sendTransform(cameraRigTransforms(rig, helper()->now())); + } + + std::shared_ptr makeNode( + std::vector 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 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 node = + addNode(std::make_shared(options)); + waitForTfListener(); + return node; + } + + /// The four raw topics a stereo driver publishes, which this node takes by default. + struct StereoPublishers + { + rclcpp::Publisher::SharedPtr left; + rclcpp::Publisher::SharedPtr right; + rclcpp::Publisher::SharedPtr leftInfo; + rclcpp::Publisher::SharedPtr rightInfo; + }; + + StereoPublishers makeStereoPublishers() + { + StereoPublishers pubs; + pubs.left = helper()->create_publisher("left/image_rect", 10); + pubs.right = helper()->create_publisher("right/image_rect", 10); + pubs.leftInfo = helper()->create_publisher("left/camera_info", 10); + pubs.rightInfo = helper()->create_publisher("right/camera_info", 10); + return pubs; + } + + bool waitForStereoSubscribers(const StereoPublishers & pubs) + { + return waitForSubscriber(pubs.left) && waitForSubscriber(pubs.right) && + waitForSubscriber(pubs.leftInfo) && waitForSubscriber(pubs.rightInfo); + } + + /** + * @brief Publishes one pair from test/data/stereo as the four raw topics. + * @param rightOffset added to the right camera's stamp, to break an exact pairing. + * + * Both cameras report the same frame_id, as a rectified rig does: the pair is already + * in a common frame, so there is nothing for TF to say about the two of them. + */ + void publishStereoFrame( + const StereoPublishers & pubs, const std::string & name, double stamp, + double rightOffset = 0.0, StereoSet set = kRectified) + { + const cv::Mat left = stereoLeftImage(name, set); + const cv::Mat right = stereoRightImage(name, set); + const std::string dir = set == kRaw ? "raw" : "rect"; + ASSERT_FALSE(left.empty()) << "test/data/stereo/" << dir << "/left/" << name << ".jpg missing"; + ASSERT_FALSE(right.empty()) << "test/data/stereo/" << dir << "/right/" << name << ".jpg missing"; + + pubs.left->publish(makeImage("camera", stamp, left, "bgr8")); + pubs.right->publish(makeImage("camera", stamp + rightOffset, right, "mono8")); + pubs.leftInfo->publish(stereoLeftInfo("camera", stamp, set)); + pubs.rightInfo->publish(stereoRightInfo("camera", stamp + rightOffset, set)); + } + + /** + * @brief Publishes an unrectified pair with one frame_id per camera. + * + * Which is what an unrectified rig looks like: the two images are in different frames, + * and Rtabmap/ImagesAlreadyRectified:=false makes the node ask TF for the transform + * between them rather than reading the baseline out of the right camera's P. + */ + void publishRawStereoFrameInSplitFrames( + const StereoPublishers & pubs, const std::string & name, double stamp) + { + const cv::Mat left = stereoLeftImage(name, kRaw); + const cv::Mat right = stereoRightImage(name, kRaw); + ASSERT_FALSE(left.empty()) << "test/data/stereo/raw/left/" << name << ".jpg missing"; + ASSERT_FALSE(right.empty()) << "test/data/stereo/raw/right/" << name << ".jpg missing"; + + pubs.left->publish(makeImage("camera_left", stamp, left, "bgr8")); + pubs.right->publish(makeImage("camera_right", stamp, right, "mono8")); + pubs.leftInfo->publish(stereoLeftInfo("camera_left", stamp, kRaw)); + pubs.rightInfo->publish(stereoRightInfo("camera_right", stamp, kRaw)); + } + + /** + * @brief One pair packed the way stereo_sync publishes it. + * + * The RGB-D message is reused for stereo: the left image and its calibration travel in + * the rgb fields, the right ones in the depth fields. See stereo_sync.cpp. + */ + rtabmap_msgs::msg::RGBDImage makeStereoRGBDImage(const std::string & name, double stamp) + { + const cv::Mat left = stereoLeftImage(name); + const cv::Mat right = stereoRightImage(name); + EXPECT_FALSE(left.empty()) << "test/data/stereo/rect/left/" << name << ".jpg missing"; + EXPECT_FALSE(right.empty()) << "test/data/stereo/rect/right/" << name << ".jpg missing"; + + rtabmap_msgs::msg::RGBDImage msg; + msg.header.frame_id = "camera"; + msg.header.stamp = stampOf(stamp); + msg.rgb = makeImage("camera", stamp, left, "bgr8"); + msg.depth = makeImage("camera", stamp, right, "mono8"); + msg.rgb_camera_info = stereoLeftInfo("camera", stamp); + msg.depth_camera_info = stereoRightInfo("camera", stamp); + return msg; + } + + /** + * @brief Publishes an unrectified pair whose camera_info messages carry no frame_id. + * + * A driver that leaves frame_id empty gives the node nothing to ask TF about. The + * baseline in the right camera's P is then the only thing left to work from, which the + * node falls back to -- unless @p zeroBaseline strips that too, and nothing remains. + */ + void publishRawStereoFrameWithUnnamedCameras( + const StereoPublishers & pubs, const std::string & name, double stamp, + bool zeroBaseline = false) + { + const cv::Mat left = stereoLeftImage(name, kRaw); + const cv::Mat right = stereoRightImage(name, kRaw); + ASSERT_FALSE(left.empty()) << "test/data/stereo/raw/left/" << name << ".jpg missing"; + ASSERT_FALSE(right.empty()) << "test/data/stereo/raw/right/" << name << ".jpg missing"; + + // The images keep their frame: base_link -> camera is what localTransform needs, + // and it is a different lookup from the one between the two cameras. + sensor_msgs::msg::CameraInfo leftInfo = stereoLeftInfo("", stamp, kRaw); + sensor_msgs::msg::CameraInfo rightInfo = stereoRightInfo("", stamp, kRaw); + if(zeroBaseline) + { + rightInfo.p[3] = 0.0; + } + + pubs.left->publish(makeImage("camera", stamp, left, "bgr8")); + pubs.right->publish(makeImage("camera", stamp, right, "mono8")); + pubs.leftInfo->publish(leftInfo); + pubs.rightInfo->publish(rightInfo); + } + + /** + * @brief base_link -> camera_left -> camera_right, the rig an unrectified pair needs in TF. + * + * The second link is the rig's measured extrinsics rather than an assumed baseline: + * ~12 cm along x, plus the few milliradians the two cameras are really off by. That is + * the transform the node looks up to rectify the pair for itself. + */ + void publishSplitSensorTf() + { + staticTf_ = std::make_shared(*helper()); + std::vector transforms(2); + transforms[0].header.stamp = helper()->now(); + transforms[0].header.frame_id = "base_link"; + transforms[0].child_frame_id = "camera_left"; + transforms[0].transform.rotation.w = 1.0; + transforms[1] = transforms[0]; + transforms[1].header.frame_id = "camera_left"; + transforms[1].child_frame_id = "camera_right"; + transforms[1].transform = stereoRightInLeftFrame(); + staticTf_->sendTransform(transforms); + } + + std::shared_ptr staticTf_; +}; + +/// The vendored calibration has to survive the trip through CameraInfo, or nothing below means anything. +TEST_F(StereoOdometryTest, the_test_calibration_describes_the_stereo_rig) +{ + const sensor_msgs::msg::CameraInfo left = stereoLeftInfo("camera", 1.0); + const sensor_msgs::msg::CameraInfo right = stereoRightInfo("camera", 1.0); + + ASSERT_EQ(640u, left.width); + ASSERT_EQ(480u, left.height); + EXPECT_NEAR(487.6, left.k[0], 0.1); + EXPECT_DOUBLE_EQ(0.0, left.p[3]); + + // P(0,3) = -fx * baseline: ~12 cm, the scale every stereo estimate below rests on. + const double baseline = -right.p[3] / right.p[0]; + EXPECT_NEAR(0.1197, baseline, 0.001); + + // Rectified: nothing left to undistort, and no rotation left to apply. + EXPECT_DOUBLE_EQ(0.0, left.d[0]); + EXPECT_DOUBLE_EQ(1.0, left.r[0]); +} + +/** + * The raw set is a different calibration of the same rig, and must not be confused with + * the rectified one: it carries the lens's distortion and the rotation into the rectified + * frame, which is what Rtabmap/ImagesAlreadyRectified:=false needs to do the rectification + * the pipeline has not done. + */ +TEST_F(StereoOdometryTest, the_raw_calibration_carries_distortion_and_rectification) +{ + const sensor_msgs::msg::CameraInfo left = stereoLeftInfo("camera", 1.0, kRaw); + const sensor_msgs::msg::CameraInfo right = stereoRightInfo("camera", 1.0, kRaw); + + ASSERT_EQ(640u, left.width); + ASSERT_EQ(480u, left.height); + EXPECT_LT(left.d[0], -0.1) << "a raw pair needs real distortion coefficients"; + EXPECT_NE(1.0, left.r[0]) << "a raw pair needs the rotation into the rectified frame"; + + // The same physical rig, so the same ~12 cm baseline as the rectified calibration. + EXPECT_NEAR(0.1197, -right.p[3] / right.p[0], 0.001); +} + +/** + * The rig's extrinsics, which the unrectified path takes from TF rather than from P. + * Stereo calibration stores left-to-right; TF publishes right-in-left, so the sign of the + * baseline flips on the way through, and getting that backwards would put the right camera + * on the wrong side of the left one. + */ +TEST_F(StereoOdometryTest, the_stereo_pose_puts_the_right_camera_beside_the_left_one) +{ + const geometry_msgs::msg::Transform transform = stereoRightInLeftFrame(); + + // In the optical frame x points right, so the right camera sits at +baseline. + EXPECT_NEAR(0.1194, transform.translation.x, 0.001); + EXPECT_NEAR(0.0, transform.translation.y, 0.01); + EXPECT_NEAR(0.0, transform.translation.z, 0.01); + // The two cameras are nearly parallel: a few milliradians, not a few degrees. + EXPECT_NEAR(1.0, std::fabs(transform.rotation.w), 0.001); +} + +/// By default the node takes the four raw stereo topics. +TEST_F(StereoOdometryTest, subscribes_to_the_raw_stereo_topics_by_default) +{ + publishSensorTf(); + makeNode(); + + StereoPublishers pubs = makeStereoPublishers(); + + EXPECT_TRUE(waitForSubscriber(pubs.left)); + EXPECT_TRUE(waitForSubscriber(pubs.right)); + EXPECT_TRUE(waitForSubscriber(pubs.leftInfo)); + EXPECT_TRUE(waitForSubscriber(pubs.rightInfo)); +} + +/// A synchronized set of the four topics produces one odometry message, at the origin. +TEST_F(StereoOdometryTest, publishes_odom_for_a_synchronized_stereo_frame) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode(); + + StereoPublishers pubs = makeStereoPublishers(); + ASSERT_TRUE(waitForStereoSubscribers(pubs)); + + // Identical stamps, which is what this node's exact-by-default policy requires. + publishStereoFrame(pubs, kFirstFrame, 1.0); + + 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); +} + +/** + * The point of feeding real pairs: a second frame is registered against the first and a + * motion comes out. The bounds are loose on purpose -- the claim is "a plausible, + * non-degenerate transform from real correspondences", not a specific value, which + * depends on the odometry strategy RTAB-Map was built with. + */ +TEST_F(StereoOdometryTest, recovers_motion_between_two_real_stereo_pairs) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr> info = + collect("odom_info"); + makeNode(); + + StereoPublishers pubs = makeStereoPublishers(); + ASSERT_TRUE(waitForStereoSubscribers(pubs)); + ASSERT_TRUE(waitForPublisher(info->subscription)); + + publishStereoFrame(pubs, kFirstFrame, 1.0); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })); + publishStereoFrame(pubs, kSecondFrame, 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 between frames " + << kFirstFrame << " and " << kSecondFrame; + EXPECT_GT(info->back().inliers, 20) + << "too few inliers (matches=" << info->back().matches << ")"; + EXPECT_LE(info->back().inliers, info->back().matches); + + // 0.1710 to 0.1740 m and 0.1312 to 0.1315 rad over ten runs, on this build. + EXPECT_NEAR(0.172, translationNorm(second), 0.035) + << "the pair is ~17 cm apart; this estimate is not that"; + EXPECT_NEAR(0.131, rotationAngle(second), 0.030) + << "the pair turns ~0.13 rad; this estimate is not that"; +} + +/** + * Unlike rgbd_odometry, this node requires identical stamps by default, because a stereo + * pair is normally hardware-triggered. See "Synchronization" in doc/stereo_odometry.md. + */ +TEST_F(StereoOdometryTest, approx_sync_is_off_by_default) +{ + publishSensorTf(); + std::shared_ptr node = makeNode(); + + EXPECT_FALSE(node->get_parameter("approx_sync").as_bool()); +} + +/// With the exact policy, stamps that differ never pair and nothing is published at all. +TEST_F(StereoOdometryTest, publishes_nothing_when_stamps_differ_under_exact_sync) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("approx_sync", false)}); + + StereoPublishers pubs = makeStereoPublishers(); + ASSERT_TRUE(waitForStereoSubscribers(pubs)); + + publishStereoFrame(pubs, kFirstFrame, 1.0, /*rightOffset=*/0.001); // a millisecond apart + spinFor(std::chrono::milliseconds(1500)); + + EXPECT_TRUE(odom->empty()) + << "the exact policy must not pair frames whose stamps differ"; +} + +/// Approximate matching pairs them anyway, which is the fix when a rig is not triggered. +TEST_F(StereoOdometryTest, approx_sync_pairs_frames_whose_stamps_differ) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("approx_sync", true)}); + + StereoPublishers pubs = makeStereoPublishers(); + ASSERT_TRUE(waitForStereoSubscribers(pubs)); + + // Two frames: the approximate policy needs a following message before it can settle + // on the best pairing for the first one. + publishStereoFrame(pubs, kFirstFrame, 1.0, /*rightOffset=*/0.001); + spinFor(std::chrono::milliseconds(100)); + publishStereoFrame(pubs, kSecondFrame, 1.1, /*rightOffset=*/0.001); + + EXPECT_TRUE(spinUntil([&]() { return !odom->empty(); })); +} + +/** + * keep_color decides what reaches RTAB-Map from a color left image: the matcher only ever + * works in grayscale, so the color is dropped by default and kept only when asked for, + * which is what a downstream consumer of odom_rgbd_image or odom_sensor_data needs. + */ +TEST_F(StereoOdometryTest, keeps_the_left_image_in_color_when_asked) +{ + publishSensorTf(); + std::shared_ptr> frames = + collect("odom_rgbd_image"); + makeNode({rclcpp::Parameter("keep_color", true)}); + + StereoPublishers pubs = makeStereoPublishers(); + ASSERT_TRUE(waitForStereoSubscribers(pubs)); + // The node republishes the frame only when something is listening for it. + ASSERT_TRUE(waitForPublisher(frames->subscription)); + + publishStereoFrame(pubs, kFirstFrame, 1.0); + + ASSERT_TRUE(spinUntil([&]() { return !frames->empty(); })); + EXPECT_EQ("bgr8", frames->back().rgb.encoding); + // The right image is the matcher's other input and is grayscale either way. + EXPECT_EQ("mono8", frames->back().depth.encoding); +} + +/// Off by default: RTAB-Map converts the pair to grayscale on the way in. +TEST_F(StereoOdometryTest, converts_the_left_image_to_grayscale_by_default) +{ + publishSensorTf(); + std::shared_ptr> frames = + collect("odom_rgbd_image"); + std::shared_ptr node = makeNode(); + EXPECT_FALSE(node->get_parameter("keep_color").as_bool()); + + StereoPublishers pubs = makeStereoPublishers(); + ASSERT_TRUE(waitForStereoSubscribers(pubs)); + ASSERT_TRUE(waitForPublisher(frames->subscription)); + + // The same color pair as above: what differs is only what the node does with it. + publishStereoFrame(pubs, kFirstFrame, 1.0); + + ASSERT_TRUE(spinUntil([&]() { return !frames->empty(); })); + EXPECT_EQ("mono8", frames->back().rgb.encoding); +} + +/** + * The unrectified path the documentation offers as the alternative to stereo_image_proc: + * Rtabmap/ImagesAlreadyRectified:=false, raw images, and the transform between the two + * cameras taken from TF. See "The images are normally rectified" in doc/stereo_odometry.md. + */ +TEST_F(StereoOdometryTest, rectifies_a_raw_pair_itself_when_told_the_images_are_not_rectified) +{ + publishSplitSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("Rtabmap/ImagesAlreadyRectified", "false")}); + + StereoPublishers pubs = makeStereoPublishers(); + ASSERT_TRUE(waitForStereoSubscribers(pubs)); + + publishRawStereoFrameInSplitFrames(pubs, kFirstRawFrame, 1.0); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })); + EXPECT_EQ("odom", odom->back().header.frame_id); + EXPECT_NEAR(0.0, translationNorm(odom->back()), 1e-9) << "the first frame is the origin"; + + publishRawStereoFrameInSplitFrames(pubs, kSecondRawFrame, 1.25); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; })); + + // The rectification RTAB-Map did for itself has to be good enough to register the two + // frames, with the scale taken from TF. A quarter second of walking forward, which is + // 0.2329 to 0.2351 m and 0.0298 to 0.0303 rad over ten runs on this build. + const nav_msgs::msg::Odometry & second = odom->back(); + ASSERT_FALSE(isLost(second)) << "lost tracking on a pair it rectified itself"; + EXPECT_NEAR(0.234, translationNorm(second), 0.047) + << "a quarter second of walking is ~0.23 m; this estimate is not that"; + EXPECT_NEAR(0.030, rotationAngle(second), 0.015) + << "the pair turns ~0.03 rad; this estimate is not that"; +} + +/** + * Same parameter, but both camera_info messages name the same frame: TF then answers with + * the identity, which is no baseline at all. The node refuses the frame rather than + * estimating a trajectory at an arbitrary scale. + */ +TEST_F(StereoOdometryTest, publishes_nothing_for_a_raw_pair_when_the_cameras_share_a_frame) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("Rtabmap/ImagesAlreadyRectified", "false")}); + + StereoPublishers pubs = makeStereoPublishers(); + ASSERT_TRUE(waitForStereoSubscribers(pubs)); + + // Both images on "camera", which is right for a rectified pair and wrong for this one. + publishStereoFrame(pubs, kFirstRawFrame, 1.0, /*rightOffset=*/0.0, kRaw); + spinFor(std::chrono::milliseconds(1500)); + + EXPECT_TRUE(odom->empty()) + << "an identity transform between the cameras cannot give a baseline"; +} + +/** + * Unrectified images from a driver that leaves camera_info's frame_id empty: there is no + * TF query to make, so the node falls back to the baseline in the right camera's P rather + * than refusing the frame. + */ +TEST_F(StereoOdometryTest, uses_the_calibration_baseline_for_a_raw_pair_with_unnamed_cameras) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("Rtabmap/ImagesAlreadyRectified", "false")}); + + StereoPublishers pubs = makeStereoPublishers(); + ASSERT_TRUE(waitForStereoSubscribers(pubs)); + + publishRawStereoFrameWithUnnamedCameras(pubs, kFirstRawFrame, 1.0); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })); + EXPECT_NEAR(0.0, translationNorm(odom->back()), 1e-9) << "the first frame is the origin"; + + publishRawStereoFrameWithUnnamedCameras(pubs, kSecondRawFrame, 1.25); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; })); + + // The same motion as above, held to the same tolerance: the baseline came from the file + // instead of TF, and the estimate has to come out the same size either way. + const nav_msgs::msg::Odometry & second = odom->back(); + ASSERT_FALSE(isLost(second)) << "lost tracking on a pair rectified from the calibration alone"; + EXPECT_NEAR(0.234, translationNorm(second), 0.047) + << "a quarter second of walking is ~0.23 m; this estimate is not that"; + EXPECT_NEAR(0.030, rotationAngle(second), 0.015) + << "the pair turns ~0.03 rad; this estimate is not that"; +} + +/// No frame_id and no baseline in P either: nothing left to derive the scale from. +TEST_F(StereoOdometryTest, publishes_nothing_for_a_raw_pair_with_neither_frame_nor_baseline) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("Rtabmap/ImagesAlreadyRectified", "false")}); + + StereoPublishers pubs = makeStereoPublishers(); + ASSERT_TRUE(waitForStereoSubscribers(pubs)); + + publishRawStereoFrameWithUnnamedCameras(pubs, kFirstRawFrame, 1.0, /*zeroBaseline=*/true); + spinFor(std::chrono::milliseconds(1500)); + + EXPECT_TRUE(odom->empty()) + << "with no TF and no Tx there is no baseline, so no estimate to publish"; +} + +/** + * The silent middle case doc/stereo_odometry.md warns about: an unrectified pair while the + * node is left believing it is rectified. Nothing fails -- odometry is published, and its + * covariance says the node stands behind it -- which is exactly why the warning is there + * and why this test pins the behaviour rather than an accuracy bound. + */ +TEST_F(StereoOdometryTest, accepts_a_raw_pair_silently_when_it_believes_it_is_rectified) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + makeNode(); + + StereoPublishers pubs = makeStereoPublishers(); + ASSERT_TRUE(waitForStereoSubscribers(pubs)); + + publishStereoFrame(pubs, kFirstRawFrame, 1.0, /*rightOffset=*/0.0, kRaw); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })); + publishStereoFrame(pubs, kSecondRawFrame, 1.25, /*rightOffset=*/0.0, kRaw); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; })); + + EXPECT_FALSE(isLost(odom->back())) + << "the node has no way to notice the images are distorted"; +} + +/// subscribe_rgbd swaps the four topics for one pre-synchronized message from stereo_sync. +TEST_F(StereoOdometryTest, subscribe_rgbd_takes_a_single_rgbd_image_topic) +{ + publishSensorTf(); + makeNode({rclcpp::Parameter("subscribe_rgbd", true)}); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + + EXPECT_TRUE(waitForSubscriber(pub)); +} + +/** + * The same pairs through the other entry point. Both callbacks hand the same four vectors + * to commonCallback, so what this covers is the unpacking on the way in: left out of rgb, + * right out of depth, and the baseline out of depth_camera_info -- swap the two infos and + * the scale of the whole trajectory goes with them. + */ +TEST_F(StereoOdometryTest, recovers_motion_from_a_single_rgbd_image_topic) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr> info = + collect("odom_info"); + makeNode({rclcpp::Parameter("subscribe_rgbd", true)}); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(info->subscription)); + + pub->publish(makeStereoRGBDImage(kFirstFrame, 1.0)); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })); + EXPECT_NEAR(0.0, translationNorm(odom->back()), 1e-9) << "the first frame is the origin"; + + pub->publish(makeStereoRGBDImage(kSecondFrame, 1.1)); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; })); + + const nav_msgs::msg::Odometry & second = odom->back(); + ASSERT_FALSE(isLost(second)) << "lost tracking between frames " + << kFirstFrame << " and " << kSecondFrame; + EXPECT_GT(info->back().inliers, 20) + << "too few inliers (matches=" << info->back().matches << ")"; + + // The same motion the four-topic test sees, held to the same tolerance: the two paths + // differ only in packaging, so an estimate that differs is a packing bug. + EXPECT_NEAR(0.172, translationNorm(second), 0.035) + << "the pair is ~17 cm apart; this estimate is not that"; + EXPECT_NEAR(0.131, rotationAngle(second), 0.030) + << "the pair turns ~0.13 rad; this estimate is not that"; +} + +/** + * @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 StereoOdometryCamerasTest : + public StereoOdometryTest, + public ::testing::WithParamInterface +{ +}; + +TEST_P(StereoOdometryCamerasTest, 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::SharedPtr> publishers; + for(int i=0; icreate_publisher( + "rgbd_image" + std::to_string(i), 10)); + } + + for(int i=0; i & info) { + return std::to_string(info.param) + "_cameras"; + }); + +/// rgbd_cameras:=0 takes any number of cameras in one RGBDImages message. +TEST_F(StereoOdometryTest, rgbd_cameras_zero_takes_an_rgbd_images_topic) +{ + publishSensorTf(); + makeNode({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("rgbd_cameras", 0)}); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_images", 10); + + EXPECT_TRUE(waitForSubscriber(pub)); +} + +/** + * @brief Four stereo pairs on one 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. What differs from the + * RGB-D rig is the second calibration of each camera: the node has to read the pairs as + * stereo, and the features still belong to the left image of each one. + * + * 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 StereoOdometryRigTest : + public StereoOdometryTest, + public ::testing::WithParamInterface +{ +}; + +TEST_P(StereoOdometryRigTest, 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> odom = + collect("odom"); + std::shared_ptr> info = + collect("odom_info"); + makeNode({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("rgbd_cameras", 0), + rclcpp::Parameter("Vis/EstimationType", std::to_string(estimationType))}); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("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; ipublish(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. + 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, + StereoOdometryRigTest, + ::testing::Values(0, 1), + [](const ::testing::TestParamInfo & 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(StereoOdometryRigTest, the_same_frames_without_their_features_have_nothing_to_track) +{ + const CameraRig rig = makeCameraRig(); + publishRigTf(rig); + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr> info = + collect("odom_info"); + makeNode({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("rgbd_cameras", 0)}); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_images", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(info->subscription)); + + for(int i=0; i<2; ++i) + { + rtabmap_msgs::msg::RGBDImages frame = + cameraRigStereoFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i); + for(size_t c=0; cpublish(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())); +} + +/** + * @brief The same rig on the numbered topics, one to six stereo pairs. + * + * `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 StereoOdometryRigCamerasTest : + public StereoOdometryTest, + public ::testing::WithParamInterface> +{ +}; + +TEST_P(StereoOdometryRigCamerasTest, 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> odom = + collect("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::SharedPtr> publishers; + for(int i=0; icreate_publisher( + cameras == 1 ? "rgbd_image" : "rgbd_image" + std::to_string(i), 10)); + } + for(int i=0; ipublish(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 = + cameraRigStereoFrame(rig, rtabmap::Transform(1.1f, 0, 0, 0, 0, 0), 2.1); + for(int c=0; cpublish(frame.rgbd_images[c]); + } + EXPECT_TRUE(spinUntil([&]() { return odom->size() > beforeReset; })) + << "nothing came back after the reset rebuilt the synchronizer"; +} + +INSTANTIATE_TEST_SUITE_P( + StereoCameras, + StereoOdometryRigCamerasTest, + ::testing::Combine(::testing::Range(1, 7), ::testing::Bool(), ::testing::Values(0, 1)), + [](const ::testing::TestParamInfo> & 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"); + }); + +/** + * A frame whose cameras disagree about carrying images is refused rather than + * half-processed: the images are stitched side by side and the keypoints indexed into + * that strip, so one camera short of images would put everything after it in the wrong + * place. + */ +TEST_F(StereoOdometryTest, refuses_a_frame_whose_cameras_disagree_about_carrying_images) +{ + const CameraRig rig = makeCameraRig(2); + publishRigTf(rig); + std::shared_ptr> odom = + collect("odom"); + makeNode({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("rgbd_cameras", 2)}); + + std::vector::SharedPtr> publishers; + for(int i=0; i<2; ++i) + { + publishers.push_back(helper()->create_publisher( + "rgbd_image" + std::to_string(i), 10)); + } + ASSERT_TRUE(waitForSubscriber(publishers[0])); + ASSERT_TRUE(waitForSubscriber(publishers[1])); + + rtabmap_msgs::msg::RGBDImages frame = + cameraRigStereoFrame(rig, rtabmap::Transform(0, 0, 0, 0, 0, 0), 1.0, 0.12, /*withImages=*/true); + // The second camera sends its features and its calibration, but no images. + frame.rgbd_images[1].rgb = sensor_msgs::msg::Image(); + frame.rgbd_images[1].depth = sensor_msgs::msg::Image(); + publishers[0]->publish(frame.rgbd_images[0]); + publishers[1]->publish(frame.rgbd_images[1]); + spinFor(std::chrono::milliseconds(500)); + + EXPECT_TRUE(odom->empty()) << "a frame with images on one camera only was processed anyway"; +} + +/** + * Features whose three parts disagree are dropped rather than used out of step: a + * keypoint read against the wrong descriptor, or given another keypoint's 3D point, + * would register the frame confidently and wrongly. + */ +TEST_F(StereoOdometryTest, ignores_features_whose_counts_disagree) +{ + const CameraRig rig = makeCameraRig(1); + publishRigTf(rig); + std::shared_ptr> info = + collect("odom_info"); + makeNode({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("rgbd_cameras", 1)}); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(info->subscription)); + + rtabmap_msgs::msg::RGBDImages frame = cameraRigStereoFrame(rig, rtabmap::Transform(0, 0, 0, 0, 0, 0), 1.0); + ASSERT_GT(frame.rgbd_images[0].key_points.size(), 1u); + // One keypoint fewer than there are 3D points and descriptor rows. + frame.rgbd_images[0].key_points.pop_back(); + pub->publish(frame.rgbd_images[0]); + ASSERT_TRUE(spinUntil([&]() { return !info->empty(); })); + + EXPECT_EQ(0, info->back().features) + << "features that do not line up with each other were used anyway"; +} + +/** + * @brief The calibration paths around `Rtabmap/ImagesAlreadyRectified`, on synthetic pairs. + * + * A stereo pair is only usable if the node can work out how far apart the two cameras + * are. It has two ways -- `P(0,3)` in the right `camera_info`, or the transform between + * the two camera frames in TF -- and refuses the frame rather than guessing when neither + * answers. These drive each of those outcomes. + */ +class StereoOdometryCalibrationTest : public StereoOdometryTest +{ +protected: + /// A one-camera rig and its publisher, with the node already listening. + rclcpp::Publisher::SharedPtr start( + const CameraRig & rig, std::vector params = {}) + { + publishRigTf(rig); + params.push_back(rclcpp::Parameter("subscribe_rgbd", true)); + params.push_back(rclcpp::Parameter("rgbd_cameras", 1)); + makeNode(params); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + EXPECT_TRUE(waitForSubscriber(pub)); + return pub; + } +}; + +/// No `P(0,3)` and nothing in TF to make up for it: there is no scale, so no pose. +TEST_F(StereoOdometryCalibrationTest, refuses_a_pair_whose_calibration_has_no_baseline) +{ + const CameraRig rig = makeCameraRig(1); + std::shared_ptr> odom = + collect("odom"); + rclcpp::Publisher::SharedPtr pub = start(rig); + + // Both calibrations describe the same camera, so TF between them is the identity. + rtabmap_msgs::msg::RGBDImages frame = cameraRigStereoFrame( + rig, rtabmap::Transform(0, 0, 0, 0, 0, 0), 1.0, /*baseline=*/0.0, /*withImages=*/true); + pub->publish(frame.rgbd_images[0]); + spinFor(std::chrono::milliseconds(500)); + + EXPECT_TRUE(odom->empty()) << "a pair with no baseline was registered anyway"; +} + +/// The D400 case the node warns about: no `P(0,3)`, but the two frames are in TF. +TEST_F(StereoOdometryCalibrationTest, takes_the_baseline_from_tf_when_the_calibration_has_none) +{ + const CameraRig rig = makeCameraRig(1); + const double baseline = 0.12; + std::shared_ptr> odom = + collect("odom"); + + // The rig's TF, plus the right camera beside the left one. + staticTf_ = std::make_shared(*helper()); + std::vector transforms = + cameraRigTransforms(rig, helper()->now()); + geometry_msgs::msg::TransformStamped right; + right.header.stamp = helper()->now(); + right.header.frame_id = rig.frameIds[0]; + right.child_frame_id = rig.frameIds[0] + "_right"; + right.transform.translation.x = baseline; + right.transform.rotation.w = 1.0; + transforms.push_back(right); + staticTf_->sendTransform(transforms); + + // makeNode() waits for the node's TF listener to pick the static transforms up. + makeNode({rclcpp::Parameter("subscribe_rgbd", true), rclcpp::Parameter("rgbd_cameras", 1)}); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + + // A metre forward, with the baseline reachable only through TF. + for(int i=0; i<11; ++i) + { + rtabmap_msgs::msg::RGBDImages frame = cameraRigStereoFrame( + rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i, + /*baseline=*/0.0, /*withImages=*/true); + frame.rgbd_images[0].depth_camera_info.header.frame_id = rig.frameIds[0] + "_right"; + pub->publish(frame.rgbd_images[0]); + ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); })) + << "nothing came back for frame " << i; + } + + EXPECT_FALSE(isLost(odom->back())) << "the baseline from TF did not make the pair usable"; + EXPECT_NEAR(1.0, odom->back().pose.pose.position.x, 0.05) + << "the rig travelled a metre along x"; +} + +/// Unrectified images the node is asked to rectify, with no transform between the cameras. +TEST_F(StereoOdometryCalibrationTest, refuses_unrectified_images_when_the_cameras_are_not_in_tf) +{ + const CameraRig rig = makeCameraRig(1); + std::shared_ptr> odom = + collect("odom"); + rclcpp::Publisher::SharedPtr pub = + start(rig, {rclcpp::Parameter("Rtabmap/ImagesAlreadyRectified", "false")}); + + rtabmap_msgs::msg::RGBDImages frame = cameraRigStereoFrame( + rig, rtabmap::Transform(0, 0, 0, 0, 0, 0), 1.0, 0.12, /*withImages=*/true); + // A right camera whose frame nothing in TF knows about. + frame.rgbd_images[0].depth_camera_info.header.frame_id = "right_camera_nobody_publishes"; + pub->publish(frame.rgbd_images[0]); + spinFor(std::chrono::milliseconds(500)); + + EXPECT_TRUE(odom->empty()) + << "rectification was attempted without knowing where the two cameras are"; +} + +/// An encoding the node cannot read is refused rather than reinterpreted. +TEST_F(StereoOdometryCalibrationTest, refuses_an_image_encoding_it_cannot_use) +{ + const CameraRig rig = makeCameraRig(1); + std::shared_ptr> odom = + collect("odom"); + rclcpp::Publisher::SharedPtr pub = start(rig); + + rtabmap_msgs::msg::RGBDImages frame = cameraRigStereoFrame( + rig, rtabmap::Transform(0, 0, 0, 0, 0, 0), 1.0, 0.12, /*withImages=*/true); + frame.rgbd_images[0].rgb = makeImage(rig.frameIds[0], 1.0, + cv::Mat::zeros(rig.height, rig.width, CV_32FC1), "32FC1"); + pub->publish(frame.rgbd_images[0]); + spinFor(std::chrono::milliseconds(500)); + + EXPECT_TRUE(odom->empty()) << "a 32FC1 left image was taken as a stereo frame"; +} + +/// The images of every camera are stitched into one strip, so they have to share a type. +TEST_F(StereoOdometryTest, refuses_cameras_whose_images_are_of_different_types) +{ + const CameraRig rig = makeCameraRig(2); + publishRigTf(rig); + std::shared_ptr> odom = + collect("odom"); + // keep_color leaves a color image in color, so the two cameras below stay different; + // converted to grayscale they would both end up 8UC1 and agree. + makeNode({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("rgbd_cameras", 2), + rclcpp::Parameter("keep_color", true)}); + + std::vector::SharedPtr> publishers; + for(int i=0; i<2; ++i) + { + publishers.push_back(helper()->create_publisher( + "rgbd_image" + std::to_string(i), 10)); + } + ASSERT_TRUE(waitForSubscriber(publishers[0])); + ASSERT_TRUE(waitForSubscriber(publishers[1])); + + rtabmap_msgs::msg::RGBDImages frame = cameraRigStereoFrame( + rig, rtabmap::Transform(0, 0, 0, 0, 0, 0), 1.0, 0.12, /*withImages=*/true); + // The rig sends mono8; this camera sends color. + frame.rgbd_images[1].rgb = makeImage(rig.frameIds[1], 1.0, + cv::Mat::zeros(rig.height, rig.width, CV_8UC3), "bgr8"); + publishers[0]->publish(frame.rgbd_images[0]); + publishers[1]->publish(frame.rgbd_images[1]); + spinFor(std::chrono::milliseconds(500)); + + EXPECT_TRUE(odom->empty()) << "images of two different types were stitched together"; +} + +/** + * Cameras of one rig are meant to fire together. When their stamps are far apart the node + * says so once and carries on -- the frame is still registered, since refusing it would + * be worse than registering a slightly stale one. + */ +TEST_F(StereoOdometryTest, warns_but_carries_on_when_the_cameras_are_far_apart_in_time) +{ + const CameraRig rig = makeCameraRig(2); + publishRigTf(rig); + std::shared_ptr> odom = + collect("odom"); + // Exact matching would never pair frames this far apart, so there would be nothing + // to warn about. + makeNode({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("rgbd_cameras", 2), + rclcpp::Parameter("approx_sync", true)}); + + std::vector::SharedPtr> publishers; + for(int i=0; i<2; ++i) + { + publishers.push_back(helper()->create_publisher( + "rgbd_image" + std::to_string(i), 10)); + } + ASSERT_TRUE(waitForSubscriber(publishers[0])); + ASSERT_TRUE(waitForSubscriber(publishers[1])); + + // Each camera 60 ms behind the other, against the 1/60 s the node considers high. + // Several pairs: the approximate policy needs more than one message per topic before + // it commits to a pairing. + for(int i=0; i<4; ++i) + { + const double stamp = 1.0 + 0.1*i; + const rtabmap::Transform pose(0.1f*i, 0, 0, 0, 0, 0); + publishers[0]->publish( + cameraRigStereoFrame(rig, pose, stamp, 0.12, true).rgbd_images[0]); + publishers[1]->publish( + cameraRigStereoFrame(rig, pose, stamp + 0.06, 0.12, true).rgbd_images[1]); + spinFor(std::chrono::milliseconds(100)); + } + + EXPECT_TRUE(spinUntil([&]() { return !odom->empty(); })) + << "a pair whose cameras disagree about the time was dropped, not warned about"; +} + +/// A baseline that cannot be real is called out, and the frame is registered regardless. +TEST_F(StereoOdometryCalibrationTest, warns_about_an_implausible_baseline) +{ + const CameraRig rig = makeCameraRig(1); + std::shared_ptr> odom = + collect("odom"); + rclcpp::Publisher::SharedPtr pub = start(rig); + + // 20 m between the two cameras of one rig: possible to write down, not to build. + const rtabmap_msgs::msg::RGBDImages frame = cameraRigStereoFrame( + rig, rtabmap::Transform(0, 0, 0, 0, 0, 0), 1.0, /*baseline=*/20.0, /*withImages=*/true); + pub->publish(frame.rgbd_images[0]); + + EXPECT_TRUE(spinUntil([&]() { return !odom->empty(); })) + << "the frame was dropped rather than registered with a warning"; +} + +} // namespace +} // namespace rtabmap_odom_test diff --git a/rtabmap_python/README.md b/rtabmap_python/README.md index a57dbf58..b29ef440 100644 --- a/rtabmap_python/README.md +++ b/rtabmap_python/README.md @@ -6,6 +6,12 @@ RTAB-Map is a C++ library, and the data it hands to ROS is not always plain ROS 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. @@ -34,16 +40,6 @@ The encoding is a zlib stream followed by a 12-byte trailer holding rows, cols a **The trailer is host-endian**, because the C++ side writes raw `int`s. A blob is not portable between machines of opposite endianness. -## Building and testing - -```bash -colcon build --packages-select rtabmap_python -colcon test --packages-select rtabmap_python -colcon test-result --verbose -``` - -The tests cover the round trip for every supported depth, the exact trailer bytes against the codes RTAB-Map's `serializeMatType()` produces, and each of the behaviours above. Alongside them, the standard `ament_copyright`, `ament_flake8` and `ament_pep257` linters run over the package. - ## License BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license). diff --git a/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp b/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp index b66b6b31..a68ccfff 100644 --- a/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp +++ b/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 #include "rtabmap_rviz_plugins/MapCloudDisplay.h" #include @@ -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); } } diff --git a/rtabmap_rviz_plugins/src/MapGraphDisplay.cpp b/rtabmap_rviz_plugins/src/MapGraphDisplay.cpp index 81d065c6..17463412 100644 --- a/rtabmap_rviz_plugins/src/MapGraphDisplay.cpp +++ b/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 +#include +#include +#include +#include +#include +#include +#include + #include #include "rviz_common/properties/color_property.hpp" #include "rviz_common/properties/float_property.hpp" diff --git a/rtabmap_sync/README.md b/rtabmap_sync/README.md index 04ddb644..7a46c4eb 100644 --- a/rtabmap_sync/README.md +++ b/rtabmap_sync/README.md @@ -6,6 +6,14 @@ A SLAM node needs a camera's color image, its depth image and its calibration as Every node is a [composable node](https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Composition.html) as well as a standalone executable. **Compose these into the camera driver's process where you can**: they copy every pixel of every frame, and across a process boundary that copy is a serialization plus a memcpy per image. +## Contents + +- [Nodes](#nodes) +- [Library](#library) +- [Conventions](#conventions) +- [Build options](#build-options) +- [License](#license) + ## Nodes One page per node. @@ -62,16 +70,6 @@ For several cameras, **prefer turning `RTABMAP_SYNC_MULTI_RGBD` on**. The consum Without the options, asking for either is refused rather than ignored quietly — `subscribe_user_data` is reset to false with an error, and `rgbd_cameras` > 1 leaves nothing subscribed and says so. -## Building and testing - -```bash -colcon build --packages-select rtabmap_sync -colcon test --packages-select rtabmap_sync -colcon test-result --verbose -``` - -The tests drive each node over real ROS topics inside the gtest binary — no launch files and no separate processes — so they also serve as worked examples of each node's topics and parameters. - ## License BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license). diff --git a/rtabmap_sync/doc/rgb_sync.md b/rtabmap_sync/doc/rgb_sync.md index da5c157f..54b48bdc 100644 --- a/rtabmap_sync/doc/rgb_sync.md +++ b/rtabmap_sync/doc/rgb_sync.md @@ -20,6 +20,16 @@ flowchart LR ODOM -->|odometry| MAP ``` +## Contents + +- [Usage](#usage) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Parameters](#parameters) +- [fill_empty_depth](#fill_empty_depth) +- [Synchronization](#synchronization) +- [Diagnostics](#diagnostics) + ## Usage ```bash diff --git a/rtabmap_sync/doc/rgbd_sync.md b/rtabmap_sync/doc/rgbd_sync.md index cf1f3260..f32e13fc 100644 --- a/rtabmap_sync/doc/rgbd_sync.md +++ b/rtabmap_sync/doc/rgbd_sync.md @@ -63,6 +63,18 @@ It can also decimate the images, rescale depth into the unit RTAB-Map expects, a For a monocular camera use [rgb_sync](rgb_sync.md); for a stereo pair, [stereo_sync](stereo_sync.md); for several RGB-D cameras, one of these per camera feeding [rgbdx_sync](rgbdx_sync.md). +## Contents + +- [Usage](#usage) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Parameters](#parameters) +- [Synchronization](#synchronization) +- [Depth units](#depth-units) +- [Compressing for a slow link](#compressing-for-a-slow-link) +- [Decimation](#decimation) +- [Diagnostics](#diagnostics) + ## Usage ```bash diff --git a/rtabmap_sync/doc/rgbdx_sync.md b/rtabmap_sync/doc/rgbdx_sync.md index e6db4c04..2335cd8f 100644 --- a/rtabmap_sync/doc/rgbdx_sync.md +++ b/rtabmap_sync/doc/rgbdx_sync.md @@ -54,6 +54,17 @@ flowchart LR ODOMT --> MAP & VIZ ``` +## Contents + +- [Usage](#usage) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Parameters](#parameters) +- [Order matters](#order-matters) +- [Synchronization](#synchronization) +- [Feeding it to rtabmap](#feeding-it-to-rtabmap) +- [Diagnostics](#diagnostics) + ## Usage ```bash diff --git a/rtabmap_sync/doc/stereo_sync.md b/rtabmap_sync/doc/stereo_sync.md index 971a34d3..8d790179 100644 --- a/rtabmap_sync/doc/stereo_sync.md +++ b/rtabmap_sync/doc/stereo_sync.md @@ -44,6 +44,17 @@ flowchart LR ODOMT --> MAP & VIZ ``` +## Contents + +- [Usage](#usage) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Parameters](#parameters) +- [How a stereo pair travels in an RGBDImage](#how-a-stereo-pair-travels-in-an-rgbdimage) +- [Synchronization](#synchronization) +- [Compressing for a slow link](#compressing-for-a-slow-link) +- [Diagnostics](#diagnostics) + ## Usage ```bash diff --git a/rtabmap_util/README.md b/rtabmap_util/README.md index 398c7bf5..4e86b566 100644 --- a/rtabmap_util/README.md +++ b/rtabmap_util/README.md @@ -4,6 +4,12 @@ Standalone utility nodes for [RTAB-Map](https://github.com/introlab/rtabmap) pip Every node is a [composable node](https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Composition.html) as well as a standalone executable. Composing them into one process with their producer avoids copying images and clouds between processes, which is worth doing for anything on the sensor path. +## Contents + +- [Nodes](#nodes) +- [Library](#library) +- [Conventions](#conventions) + ## Nodes One page per node. diff --git a/rtabmap_util/doc/db_player.md b/rtabmap_util/doc/db_player.md index 1dfae23d..23bd3e58 100644 --- a/rtabmap_util/doc/db_player.md +++ b/rtabmap_util/doc/db_player.md @@ -8,6 +8,16 @@ That makes it the tool for offline work: re-run SLAM with different parameters o > **The executable is named `data_player`**, not `db_player`. The composable node is `rtabmap_util::DbPlayer`. +## Contents + +- [Usage](#usage) +- [Published Topics](#published-topics) +- [Published Transforms](#published-transforms) +- [Services](#services) +- [Parameters](#parameters) +- [Simulated time](#simulated-time) +- [Notes](#notes) + ## Usage ```bash @@ -67,7 +77,7 @@ Broadcast on every frame unless `publish_tf` is false. | Service | Type | Description | |---|---|---| | `~/pause` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Pause playback. | -| `~/resume` | `std_srvs/srv/Empty` | Resume it. | +| `~/resume` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Resume it. | When run as the standalone executable, the **space bar** toggles pause as well. diff --git a/rtabmap_util/doc/disparity_to_depth.md b/rtabmap_util/doc/disparity_to_depth.md index f8cc9ea7..40942705 100644 --- a/rtabmap_util/doc/disparity_to_depth.md +++ b/rtabmap_util/doc/disparity_to_depth.md @@ -6,6 +6,14 @@ Most of ROS handles depth, while a stereo pipeline produces disparity. This node Pixels whose disparity falls outside the message's own `min_disparity`/`max_disparity` are written as zero, which is the ROS convention for "no reading". +## Contents + +- [Usage](#usage) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Parameters](#parameters) +- [Notes](#notes) + ## Usage ```bash diff --git a/rtabmap_util/doc/imu_to_tf.md b/rtabmap_util/doc/imu_to_tf.md index 6d2d69bc..3fecd7a4 100644 --- a/rtabmap_util/doc/imu_to_tf.md +++ b/rtabmap_util/doc/imu_to_tf.md @@ -2,10 +2,24 @@ Broadcasts the orientation of an IMU as a TF transform. -The node subscribes to a `sensor_msgs/msg/Imu` topic, takes the `orientation` field and broadcasts it on `/tf` as the rotation of `fixed_frame_id` → the IMU frame. Set `base_frame_id` and that frame becomes the child instead, with the orientation re-expressed in it from the IMU's mounting, so the transform says how the *robot* is oriented rather than how the sensor is. Either way `fixed_frame_id` is the parent, and nothing else of the message is used: the transform's translation is always zero, and the angular velocity and linear acceleration are ignored. +The node subscribes to a [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) topic, takes the `orientation` field and broadcasts it on `/tf` as the rotation of `fixed_frame_id` → the IMU frame. Set `base_frame_id` and that frame becomes the child instead, with the orientation re-expressed in it from the IMU's mounting, so the transform says how the *robot* is oriented rather than how the sensor is. Either way `fixed_frame_id` is the parent, and nothing else of the message is used: the transform's translation is always zero, and the angular velocity and linear acceleration are ignored. It exists so that a consumer that needs an oriented frame — a lidar deskewing node, a point cloud assembler, RViz — can get one from an IMU alone, without running odometry. +## Contents + +- [Usage](#usage) + - [When the IMU has no orientation](#when-the-imu-has-no-orientation) + - [A stabilized frame for lidar deskewing and odometry](#a-stabilized-frame-for-lidar-deskewing-and-odometry) + - [A rotation guess for visual odometry](#a-rotation-guess-for-visual-odometry) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Published Transforms](#published-transforms) +- [Required Transforms](#required-transforms) +- [Parameters](#parameters) +- [Mounting offset](#mounting-offset) +- [Notes](#notes) + ## Usage As a standalone node: diff --git a/rtabmap_util/doc/lidar_deskewing.md b/rtabmap_util/doc/lidar_deskewing.md index c2ecea94..64dafa66 100644 --- a/rtabmap_util/doc/lidar_deskewing.md +++ b/rtabmap_util/doc/lidar_deskewing.md @@ -6,6 +6,17 @@ A spinning lidar takes tens of milliseconds to complete a sweep, and on a moving This node uses TF to find where the sensor actually was when each point was taken, and moves every point into the pose at the start of the sweep. A straight wall comes back straight. +## Contents + +- [Usage](#usage) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Required Transforms](#required-transforms) +- [Parameters](#parameters) +- [Requirements](#requirements) +- [Behavior when TF is missing](#behavior-when-tf-is-missing) +- [Notes](#notes) + ## Usage ```bash diff --git a/rtabmap_util/doc/map_assembler.md b/rtabmap_util/doc/map_assembler.md index a6f5cbf7..ee7208a5 100644 --- a/rtabmap_util/doc/map_assembler.md +++ b/rtabmap_util/doc/map_assembler.md @@ -8,6 +8,17 @@ It also lets you produce maps RTAB-Map is not currently configured to publish, o The assembling itself is done by `MapsManager`, which is shared with `rtabmap_slam` — the outputs and every `Grid/*` parameter behave identically in both. +## Contents + +- [Usage](#usage) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Services](#services) +- [Parameters](#parameters) +- [Octomap tree type](#octomap-tree-type) +- [Start-up](#start-up) +- [Notes](#notes) + ## Usage ```bash diff --git a/rtabmap_util/doc/obstacles_detection.md b/rtabmap_util/doc/obstacles_detection.md index f98e95df..2c2be26f 100644 --- a/rtabmap_util/doc/obstacles_detection.md +++ b/rtabmap_util/doc/obstacles_detection.md @@ -6,6 +6,17 @@ The node takes a cloud, works out which points belong to the floor and which sti The segmentation is RTAB-Map's own [`LocalGridMaker`](https://introlab.github.io/rtabmap/api/latest/classrtabmap_1_1LocalGridMaker.html), so it is configured through the same `Grid/*` parameters as RTAB-Map itself and produces the same result the SLAM node would. +## Contents + +- [Usage](#usage) + - [Feeding a nav2 costmap](#feeding-a-nav2-costmap) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Required Transforms](#required-transforms) +- [Parameters](#parameters) +- [Levelling on a slope](#levelling-on-a-slope) +- [Notes](#notes) + ## Usage ```bash diff --git a/rtabmap_util/doc/point_cloud_aggregator.md b/rtabmap_util/doc/point_cloud_aggregator.md index 49995394..f13ba552 100644 --- a/rtabmap_util/doc/point_cloud_aggregator.md +++ b/rtabmap_util/doc/point_cloud_aggregator.md @@ -8,6 +8,17 @@ The sensors do not have to fire together: the clouds are matched by nearest stam It combines **several sensors into one frame**. To combine **one sensor over many frames**, use [point_cloud_assembler](point_cloud_assembler.md). +## Contents + +- [Usage](#usage) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Required Transforms](#required-transforms) +- [Parameters](#parameters) +- [Converting back to a LaserScan](#converting-back-to-a-laserscan) +- [Sensors that do not fire together](#sensors-that-do-not-fire-together) +- [Diagnostics](#diagnostics) + ## Usage ```bash diff --git a/rtabmap_util/doc/point_cloud_assembler.md b/rtabmap_util/doc/point_cloud_assembler.md index a960805a..bad5c617 100644 --- a/rtabmap_util/doc/point_cloud_assembler.md +++ b/rtabmap_util/doc/point_cloud_assembler.md @@ -8,6 +8,20 @@ That is used for two quite different things. With a **narrow field of view** — It combines **one sensor over many frames**. To combine **several sensors into one frame**, use [point_cloud_aggregator](point_cloud_aggregator.md) — or, if you want them merely accumulated rather than matched into sets, remap them all onto this node's `cloud` topic. Nothing stops several publishers sharing it, and each cloud is placed by its own stamp and frame like any other; the publish trigger then covers them together — `max_clouds` counts across all the sensors, and a given `assembling_time` gathers correspondingly more clouds. +## Contents + +- [Usage](#usage) + - [Denser clouds for SLAM, and keeping every point](#denser-clouds-for-slam-and-keeping-every-point) + - [Widening a narrow field of view](#widening-a-narrow-field-of-view) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Required Transforms](#required-transforms) +- [Parameters](#parameters) +- [Where the poses come from](#where-the-poses-come-from) +- [Following odometry's keyframes](#following-odometrys-keyframes) +- [Notes](#notes) +- [Diagnostics](#diagnostics) + ## Usage Assemble 10 sweeps, using TF for the poses: diff --git a/rtabmap_util/doc/point_cloud_xyz.md b/rtabmap_util/doc/point_cloud_xyz.md index 280d3d9c..9b88ce25 100644 --- a/rtabmap_util/doc/point_cloud_xyz.md +++ b/rtabmap_util/doc/point_cloud_xyz.md @@ -8,6 +8,15 @@ The node takes a depth image and its calibration and produces a [`sensor_msgs/ms See [point_cloud_xyzrgb](point_cloud_xyzrgb.md) for the colored equivalent. +## Contents + +- [Usage](#usage) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Parameters](#parameters) +- [Organized output](#organized-output) +- [Notes](#notes) + ## Usage ```bash diff --git a/rtabmap_util/doc/point_cloud_xyzrgb.md b/rtabmap_util/doc/point_cloud_xyzrgb.md index 158905fb..444d3418 100644 --- a/rtabmap_util/doc/point_cloud_xyzrgb.md +++ b/rtabmap_util/doc/point_cloud_xyzrgb.md @@ -4,6 +4,15 @@ Projects an RGB-D frame, a stereo pair or a disparity image into a colored point The colored counterpart of [point_cloud_xyz](point_cloud_xyz.md): same filtering, same parameters, but every point carries the color of the pixel it came from. It accepts four different input sets, so it can sit at the end of an RGB-D, stereo or disparity pipeline without anything in between. +## Contents + +- [Usage](#usage) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Parameters](#parameters) +- [Stereo matching](#stereo-matching) +- [Notes](#notes) + ## Usage ```bash diff --git a/rtabmap_util/doc/pointcloud_to_depthimage.md b/rtabmap_util/doc/pointcloud_to_depthimage.md index 329be95b..f1c0b5cc 100644 --- a/rtabmap_util/doc/pointcloud_to_depthimage.md +++ b/rtabmap_util/doc/pointcloud_to_depthimage.md @@ -9,6 +9,17 @@ Two typical setups: * **Lidar + one or more RGB cameras.** The natural way to feed a lidar into an RGB-D SLAM setup: the lidar supplies the geometry, the cameras the appearance. Run one instance per camera, each subscribing to the same cloud but to that camera's `camera_info`; a 3D lidar usually covers all of them at once. The resulting RGB-D streams can then be combined with [rtabmap_sync](https://docs.ros.org/en/jazzy/p/rtabmap_sync/)'s `rgbd_sync`/`rgbdx_sync` and given to RTAB-Map through its `rgbd_cameras` parameter. * **ToF camera + RGB camera, not synchronized.** Two separate sensors, each with its own clock and its own pose, so their frames line up neither in time nor in space. Projecting the ToF cloud into the RGB camera registers the depth to the color image, and setting `fixed_frame_id` to a high-rate odometry frame — VIO, or an IMU-driven odometry running well above the camera rate — compensates the motion between the two stamps at the same time. See [Motion compensation](#motion-compensation). +## Contents + +- [Usage](#usage) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Required Transforms](#required-transforms) +- [Parameters](#parameters) +- [Motion compensation](#motion-compensation) +- [Hole filling](#hole-filling) +- [Notes](#notes) + ## Usage ```bash diff --git a/rtabmap_util/doc/rgbd_relay.md b/rtabmap_util/doc/rgbd_relay.md index 35ed0cc0..f3574b7d 100644 --- a/rtabmap_util/doc/rgbd_relay.md +++ b/rtabmap_util/doc/rgbd_relay.md @@ -6,6 +6,15 @@ An `RGBDImage` can carry its images raw or compressed. This node converts betwee With both `compress` and `uncompress` left false the message is forwarded untouched, which makes the node a plain relay — useful to give a topic a second name, or to bridge two incompatible QoS profiles with `qos_sub` and `qos_pub`. See [Bridging QoS profiles](#bridging-qos-profiles). +## Contents + +- [Usage](#usage) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Parameters](#parameters) +- [Bridging QoS profiles](#bridging-qos-profiles) +- [Notes](#notes) + ## Usage Compress before sending over a slow link: diff --git a/rtabmap_util/doc/rgbd_split.md b/rtabmap_util/doc/rgbd_split.md index 1e8a8c0e..fac1b1b2 100644 --- a/rtabmap_util/doc/rgbd_split.md +++ b/rtabmap_util/doc/rgbd_split.md @@ -6,6 +6,15 @@ Splits an [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap It is the inverse of [rtabmap_sync](https://docs.ros.org/en/jazzy/p/rtabmap_sync/)'s `rgbd_sync`, and of its `stereo_sync` when `stereo` is set — those two are what produce an `RGBDImage` in the first place. +## Contents + +- [Usage](#usage) +- [Subscribed Topics](#subscribed-topics) +- [Published Topics](#published-topics) +- [Parameters](#parameters) +- [Stereo messages](#stereo-messages) +- [Notes](#notes) + ## Usage ```bash diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index b142d14f..a6fa55f2 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -24,6 +24,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include #include "rtabmap_util/MapsManager.h" #include @@ -1045,7 +1046,7 @@ void MapsManager::publishMaps( if(cloudGroundPub_->get_subscription_count()) { sensor_msgs::msg::PointCloud2::UniquePtr cloudMsg(new sensor_msgs::msg::PointCloud2); - pcl::toROSMsg(*assembledGround_, *cloudMsg); + rtabmap_conversions::toPointCloud2Msg(*assembledGround_, *cloudMsg); cloudMsg->header.stamp = stamp; cloudMsg->header.frame_id = mapFrameId; cloudGroundPub_->publish(std::move(cloudMsg)); @@ -1054,7 +1055,7 @@ void MapsManager::publishMaps( if(cloudObstaclesPub_->get_subscription_count()) { sensor_msgs::msg::PointCloud2::UniquePtr cloudMsg(new sensor_msgs::msg::PointCloud2); - pcl::toROSMsg(*assembledObstacles_, *cloudMsg); + rtabmap_conversions::toPointCloud2Msg(*assembledObstacles_, *cloudMsg); cloudMsg->header.stamp = stamp; cloudMsg->header.frame_id = mapFrameId; cloudObstaclesPub_->publish(std::move(cloudMsg)); @@ -1064,7 +1065,7 @@ void MapsManager::publishMaps( { pcl::PointCloud cloud = *assembledObstacles_ + *assembledGround_; sensor_msgs::msg::PointCloud2::UniquePtr cloudMsg(new sensor_msgs::msg::PointCloud2); - pcl::toROSMsg(cloud, *cloudMsg); + rtabmap_conversions::toPointCloud2Msg(cloud, *cloudMsg); cloudMsg->header.stamp = stamp; cloudMsg->header.frame_id = mapFrameId; @@ -1172,7 +1173,7 @@ void MapsManager::publishMaps( pcl::PointCloud cloudOccupiedSpace; pcl::IndicesPtr indices = util3d::concatenate(obstacleIndices, groundIndices); pcl::copyPointCloud(*cloud, *indices, cloudOccupiedSpace); - pcl::toROSMsg(cloudOccupiedSpace, msg); + rtabmap_conversions::toPointCloud2Msg(cloudOccupiedSpace, msg); msg.header.frame_id = mapFrameId; msg.header.stamp = stamp; octoMapCloud_->publish(msg); @@ -1182,7 +1183,7 @@ void MapsManager::publishMaps( { pcl::PointCloud cloudFrontier; pcl::copyPointCloud(*cloud, *frontierIndices, cloudFrontier); - pcl::toROSMsg(cloudFrontier, msg); + rtabmap_conversions::toPointCloud2Msg(cloudFrontier, msg); msg.header.frame_id = mapFrameId; msg.header.stamp = stamp; octoMapFrontierCloud_->publish(msg); @@ -1192,7 +1193,7 @@ void MapsManager::publishMaps( { pcl::PointCloud cloudObstacles; pcl::copyPointCloud(*cloud, *obstacleIndices, cloudObstacles); - pcl::toROSMsg(cloudObstacles, msg); + rtabmap_conversions::toPointCloud2Msg(cloudObstacles, msg); msg.header.frame_id = mapFrameId; msg.header.stamp = stamp; octoMapObstacleCloud_->publish(msg); @@ -1202,7 +1203,7 @@ void MapsManager::publishMaps( { pcl::PointCloud cloudGround; pcl::copyPointCloud(*cloud, *groundIndices, cloudGround); - pcl::toROSMsg(cloudGround, msg); + rtabmap_conversions::toPointCloud2Msg(cloudGround, msg); msg.header.frame_id = mapFrameId; msg.header.stamp = stamp; octoMapGroundCloud_->publish(msg); @@ -1212,7 +1213,7 @@ void MapsManager::publishMaps( { pcl::PointCloud cloudEmptySpace; pcl::copyPointCloud(*cloud, *emptyIndices, cloudEmptySpace); - pcl::toROSMsg(cloudEmptySpace, msg); + rtabmap_conversions::toPointCloud2Msg(cloudEmptySpace, msg); msg.header.frame_id = mapFrameId; msg.header.stamp = stamp; octoMapEmptySpace_->publish(msg); diff --git a/rtabmap_util/src/nodelets/obstacles_detection.cpp b/rtabmap_util/src/nodelets/obstacles_detection.cpp index 0f697caf..50cb8787 100644 --- a/rtabmap_util/src/nodelets/obstacles_detection.cpp +++ b/rtabmap_util/src/nodelets/obstacles_detection.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 #include #include @@ -159,7 +160,7 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str()); pcl::PointCloud::Ptr inputCloud(new pcl::PointCloud); - pcl::fromROSMsg(*cloudMsg, *inputCloud); + rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *inputCloud); if(inputCloud->isOrganized()) { std::vector indices; @@ -273,7 +274,7 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar if(groundPub_->get_subscription_count()) { sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2); - pcl::toROSMsg(*groundCloud, *rosCloud); + rtabmap_conversions::toPointCloud2Msg(*groundCloud, *rosCloud); rosCloud->header = cloudMsg->header; //publish the message @@ -283,7 +284,7 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar if(obstaclesPub_->get_subscription_count()) { sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2); - pcl::toROSMsg(*obstaclesCloud, *rosCloud); + rtabmap_conversions::toPointCloud2Msg(*obstaclesCloud, *rosCloud); rosCloud->header = cloudMsg->header; //publish the message @@ -293,7 +294,7 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar if(projObstaclesPub_->get_subscription_count()) { sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2); - pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, *rosCloud); + rtabmap_conversions::toPointCloud2Msg(*obstaclesCloudWithoutFlatSurfaces, *rosCloud); rosCloud->header.stamp = cloudMsg->header.stamp; rosCloud->header.frame_id = frameId_; diff --git a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp index 98532d7e..6c01e60a 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyz.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 #include #include @@ -345,7 +346,7 @@ if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_ { pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal); } - pcl::toROSMsg(*pclCloudNormal, *rosCloud); + rtabmap_conversions::toPointCloud2Msg(*pclCloudNormal, *rosCloud); } else { @@ -353,7 +354,7 @@ if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_ { pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud); } - pcl::toROSMsg(*pclCloud, *rosCloud); + rtabmap_conversions::toPointCloud2Msg(*pclCloud, *rosCloud); } rosCloud->header.stamp = header.stamp; rosCloud->header.frame_id = header.frame_id; diff --git a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp index f4dd9b39..16fac502 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.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 #include #include @@ -538,7 +539,7 @@ void PointCloudXYZRGB::processAndPublish( { pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal); } - pcl::toROSMsg(*pclCloudNormal, *rosCloud); + rtabmap_conversions::toPointCloud2Msg(*pclCloudNormal, *rosCloud); } else { @@ -546,7 +547,7 @@ void PointCloudXYZRGB::processAndPublish( { pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud); } - pcl::toROSMsg(*pclCloud, *rosCloud); + rtabmap_conversions::toPointCloud2Msg(*pclCloud, *rosCloud); } rosCloud->header.stamp = header.stamp; rosCloud->header.frame_id = header.frame_id;