i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

rtabmap_odom tests and doc (#1456)

Browse Source 
* rtabmap_odom tests and doc

* opengv note

* added ci checks or humble-latest flaky dep cmake errors

* Added real data tests for rgbd_odom and stereo_odom

* added real data for icp_odometry's deskewing test

* fixing json cmake error on lyrical/rolling

* test 2d icp odom deskewing branch

* first review of existing OdometryROS tests

* testing with imu used as guess

* tested imu arrivals sync

* Fixed odom reset on right pose when guess frame id is used

* fixing header errors in ci >=lyrical

* Added support for input rgbd_image topic with features for odom, added multicam rgbd_odometry test

* Added stereo odom support for features-only frames. Added multicam stereo tests.

* forcing latest rtabmap version

* updated OdometryROS API

* ci: dont build non-latest docker in pull requests

* splitting docker jobs

* doc edit

* Making publish_null_when_lost:=false continous when guess is provided (using guess covariance when we cannot register yet)

* updated stereo doc

* ficing rolling ci (rviz Ogre header)

* Added test coverage of alll rgbd_image callbacks

* fixing rolling ci

* making docker ci build/run the tests on pull requests

* fixing ros2 ci testing

* improved sync callback coverage

* improving stereo_odometry test coverage

* improved icp_odometry test coverage

* lyrical voxel_grid ptr error

* make multicam tests working as well without opengv

* removing deps of missing packages on rolling

* PCL empty cloud  conversion compiler errors fix

* fixing icp_odometry test failure on ci witohut libpointmatcher

* fixing nav2 costmap plugin build on lyrical

* joining thread when exiting

* updating icp test to work the same on pcl 1.15 (lyrical)

* Fix parallel tests seg fault

---------

Co-authored-by: mathieu86 <[email protected]>
This commit is contained in:
matlabbe
2026-09-21 17:02:45 -07:00
committed by GitHub
co-authored by mathieu86
parent 73c98f87a8
commit 11edc01d6a
91 changed files with 9736 additions and 350 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.github/workflows/coverage.yml
+2 -2
View File
@@ -60,7 +60,7 @@ jobs:
# tested package depends on them (--packages-up-to), just not measured.
- uses: ros-tooling/[email protected]
with:
package-name: rtabmap_conversions rtabmap_util rtabmap_sync rtabmap_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.
.github/workflows/docker-ros2.yml
+69 -23
View File
@@ -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/<distro>): they hold
# nothing from the tree under review, so building them on a pull request would only
# report that the release still installs. Left to the pushes that publish them.
docker-released:
if: ${{ github.event_name != 'pull_request' && (github.event_name != 'workflow_dispatch' || github.ref == 'refs/heads/ros2') }}
runs-on: ubuntu-latest
strategy:
fail-fast: false
matrix:
docker_tag: [humble, jazzy, kilted, lyrical]
include:
- docker_tag: humble
docker_path: 'humble'
docker_platforms: |
linux/amd64
- docker_tag: jazzy
docker_path: 'jazzy'
docker_platforms: |
linux/amd64
linux/arm64
- docker_tag: kilted
docker_path: 'kilted'
docker_platforms: |
linux/amd64
linux/arm64
- docker_tag: lyrical
docker_path: 'lyrical'
docker_platforms: |
linux/amd64
linux/arm64
steps:
-
name: Checkout
uses: actions/checkout@v4
-
name: Set up QEMU
uses: docker/setup-qemu-action@v3
with:
platforms: all
-
name: Set up Docker Buildx
uses: docker/setup-buildx-action@v3
-
name: Login to DockerHub
uses: docker/login-action@v3
with:
username: ${{ secrets.DOCKERHUB_USERNAME }}
password: ${{ secrets.DOCKERHUB_TOKEN }}
-
name: Build and push
uses: docker/build-push-action@v6
with:
context: .
push: true
platforms: ${{ matrix.docker_platforms }}
file: ./docker/${{ matrix.docker_path }}/Dockerfile
tags: introlab3it/rtabmap_ros:${{ matrix.docker_tag }}
no-cache: true
cache-to: type=inline
.github/workflows/ros2.yml
+13 -11
View File
@@ -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
README.md
+10 -1
View File
@@ -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.
codecov.yml
+6 -3
View File
@@ -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/**"
docker/humble/latest/Dockerfile
+21 -3
View File
@@ -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
docker/iron/latest/Dockerfile
+9 -3
View File
@@ -6,15 +6,21 @@ RUN source /ros_entrypoint.sh && \
COPY . ros2_ws/src/rtabmap_ros
# No rosdep "-r": a bad mirror must fail here.
RUN source /ros_entrypoint.sh && \
cd ros2_ws && \
export MAKEFLAGS="-j1" && \
echo 'Acquire::Retries "5";' > /etc/apt/apt.conf.d/80-retries && \
rosdep init && \
rosdep update && \
apt-get update && \
rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool && \
rosdep install --from-paths src --ignore-src -y -t build_export -t test -t build -t buildtool_export -t buildtool && \
apt remove ros-$ROS_DISTRO-rtabmap* -y && \
apt-get clean && rm -rf /var/lib/apt/lists/ && \
bash src/rtabmap_ros/docker/verify_deps.sh && \
apt-get clean && rm -rf /var/lib/apt/lists/
RUN source /ros_entrypoint.sh && \
cd ros2_ws && \
export MAKEFLAGS="-j1" && \
colcon build --event-handlers console_direct+ --install-base /opt/ros/iron --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
cd && \
rm -rf ros2_ws
docker/jazzy/latest/Dockerfile
+21 -3
View File
@@ -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
docker/kilted/latest/Dockerfile
+21 -3
View File
@@ -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
docker/lyrical/latest/Dockerfile
+22 -4
View File
@@ -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
docker/verify_deps.sh
Executable
+63
View File
@@ -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-<config>.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"
rtabmap_conversions/CMakeLists.txt
+1 -1
View File
@@ -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
rtabmap_conversions/README.md
+7 -8
View File
@@ -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).
rtabmap_conversions/include/rtabmap_conversions/PointCloudConversion.h
+75
View File
@@ -0,0 +1,75 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#ifndef RTABMAP_CONVERSIONS_POINTCLOUDCONVERSION_H_
#define RTABMAP_CONVERSIONS_POINTCLOUDCONVERSION_H_
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
/**
* @file
* @brief pcl::toROSMsg and pcl::fromROSMsg, minus their empty-cloud crash.
*
* Both take the address of the first point, and of the first byte of the output, before
* checking that there is one (see pcl/conversions.h): for an empty cloud that indexes
* past the end of an empty vector. Nothing notices while the standard library does not
* check, which is why it went unseen for years -- Ubuntu enables those checks from
* resolute on, and then the process aborts outright.
*
* An empty cloud is ordinary here rather than exceptional: a scan whose points were all
* filtered out, a frame with no obstacles in it, an occupancy grid with nothing new. Each
* of those still has to be published, so the conversions are used through this.
*/
namespace rtabmap_conversions {
/**
* @brief @p cloud as a PointCloud2 message.
*
* An empty cloud is converted as a single point and emptied afterwards, so the message
* still carries the field layout the installed PCL would have given it.
*/
template<typename PointT>
void toPointCloud2Msg(
const pcl::PointCloud<PointT> & cloud, sensor_msgs::msg::PointCloud2 & msg)
{
if(!cloud.empty())
{
pcl::toROSMsg(cloud, msg);
return;
}
pcl::PointCloud<PointT> onePoint;
onePoint.header = cloud.header;
onePoint.is_dense = cloud.is_dense;
onePoint.push_back(PointT());
pcl::toROSMsg(onePoint, msg);
msg.width = 0;
msg.height = 1;
msg.row_step = 0;
msg.data.clear();
}
/// @brief @p msg as a point cloud, an empty message included.
template<typename PointT>
void fromPointCloud2Msg(
const sensor_msgs::msg::PointCloud2 & msg, pcl::PointCloud<PointT> & cloud)
{
if(msg.data.empty())
{
cloud.clear();
cloud.is_dense = msg.is_dense;
pcl_conversions::toPCL(msg.header, cloud.header);
return;
}
pcl::fromROSMsg(msg, cloud);
}
} // namespace rtabmap_conversions
#endif /* RTABMAP_CONVERSIONS_POINTCLOUDCONVERSION_H_ */
rtabmap_conversions/src/MsgConversion.cpp
+3 -2
View File
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap_conversions/PointCloudConversion.h>
#include "rtabmap_conversions/MsgConversion.h"
#include <cmath>
@@ -2808,7 +2809,7 @@ bool convertScanMsg(
if(hasIntensity)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(scanOut, *pclScan);
rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScan);
pclScan->is_dense = true;
data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom).data(); // put back in laser frame
format = rtabmap::LaserScan::kXYI;
@@ -2816,7 +2817,7 @@ bool convertScanMsg(
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan);
rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScan);
pclScan->is_dense = true;
data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom).data(); // put back in laser frame
format = rtabmap::LaserScan::kXY;
rtabmap_costmap_plugins/CMakeLists.txt
+1
View File
@@ -45,6 +45,7 @@ IF("$ENV{ROS_DISTRO}" STRLESS "lyrical")
IF("$ENV{ROS_DISTRO}" STRLESS "jazzy")
target_compile_definitions(rtabmap_costmap_plugins PRIVATE -DPRE_ROS_JAZZY)
ENDIF()
target_compile_definitions(rtabmap_costmap_plugins PRIVATE -DPRE_ROS_LYRICAL)
ament_target_dependencies(rtabmap_costmap_plugins ${AmentLibraries})
ELSE()
target_link_libraries(rtabmap_costmap_plugins PRIVATE ${Libraries})
rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp
+9
View File
@@ -284,6 +284,15 @@ protected:
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
/**
* @brief Declares one of this layer's parameters and returns its value
* @param node the node the layer runs in
* @param name the parameter's name, without the layer prefix
* @param defaultValue the value to use when nothing else sets it
*/
template<typename T, typename NodeT>
T declareOrGetParameter(NodeT & node, const std::string & name, const T & defaultValue);
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
};
rtabmap_costmap_plugins/src/voxel_layer.cpp
+77 -38
View File
@@ -58,42 +58,76 @@ using rcl_interfaces::msg::ParameterType;
namespace rtabmap_costmap_plugins
{
namespace
{
/// nav2's Observation::cloud_ used to be a raw pointer and is now the cloud itself, so
/// it is reached through this rather than dereferenced directly.
inline const sensor_msgs::msg::PointCloud2 & cloudOf(
const sensor_msgs::msg::PointCloud2 & cloud)
{
return cloud;
}
inline const sensor_msgs::msg::PointCloud2 & cloudOf(
const sensor_msgs::msg::PointCloud2 * cloud)
{
return *cloud;
}
/// nav2 hands out observations by value up to kilted and by shared pointer after it.
inline const nav2_costmap_2d::Observation & obsOf(
const nav2_costmap_2d::Observation & observation)
{
return observation;
}
inline const nav2_costmap_2d::Observation & obsOf(
const std::shared_ptr<const nav2_costmap_2d::Observation> & observation)
{
return *observation;
}
} // namespace
/// nav2 declared a layer's parameters through Layer::declareParameter up to kilted and
/// through the node itself after it.
template<typename T, typename NodeT>
T VoxelLayer::declareOrGetParameter(
NodeT & node, const std::string & name, const T & defaultValue)
{
#ifdef PRE_ROS_LYRICAL
declareParameter(name, rclcpp::ParameterValue(defaultValue));
T value = defaultValue;
node->get_parameter(name_ + "." + name, value);
return value;
#else
return node->declare_or_get_parameter(name_ + "." + name, defaultValue);
#endif
}
void VoxelLayer::onInitialize()
{
nav2_costmap_2d::ObstacleLayer::onInitialize();
declareParameter("enabled", rclcpp::ParameterValue(true));
declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true));
declareParameter("min_obstacle_height", rclcpp::ParameterValue(0.0));
declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0));
declareParameter("z_voxels", rclcpp::ParameterValue(10));
declareParameter("origin_z", rclcpp::ParameterValue(0.0));
declareParameter("z_resolution", rclcpp::ParameterValue(0.2));
declareParameter("unknown_threshold", rclcpp::ParameterValue(15));
declareParameter("mark_threshold", rclcpp::ParameterValue(0));
declareParameter("combination_method", rclcpp::ParameterValue(1));
declareParameter("publish_voxel_map", rclcpp::ParameterValue(false));
declareParameter("robot_base_frame", rclcpp::ParameterValue("base_link"));
auto node = node_.lock();
if (!node) {
throw std::runtime_error{"Failed to lock node"};
}
node->get_parameter(name_ + "." + "enabled", enabled_);
node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_);
node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_);
node->get_parameter(name_ + "." + "z_voxels", size_z_);
node->get_parameter(name_ + "." + "origin_z", origin_z_);
node->get_parameter(name_ + "." + "z_resolution", z_resolution_);
node->get_parameter(name_ + "." + "unknown_threshold", unknown_threshold_);
node->get_parameter(name_ + "." + "mark_threshold", mark_threshold_);
node->get_parameter(name_ + "." + "publish_voxel_map", publish_voxel_);
node->get_parameter(name_ + "." + "robot_base_frame", robot_base_frame_);
enabled_ = declareOrGetParameter(node, "enabled", true);
footprint_clearing_enabled_ = declareOrGetParameter(node, "footprint_clearing_enabled", true);
min_obstacle_height_ = declareOrGetParameter(node, "min_obstacle_height", 0.0);
max_obstacle_height_ = declareOrGetParameter(node, "max_obstacle_height", 2.0);
size_z_ = declareOrGetParameter(node, "z_voxels", 10);
origin_z_ = declareOrGetParameter(node, "origin_z", 0.0);
z_resolution_ = declareOrGetParameter(node, "z_resolution", 0.2);
unknown_threshold_ = declareOrGetParameter(node, "unknown_threshold", 15);
mark_threshold_ = declareOrGetParameter(node, "mark_threshold", 0);
publish_voxel_ = declareOrGetParameter(node, "publish_voxel_map", false);
robot_base_frame_ = declareOrGetParameter(node, "robot_base_frame", std::string("base_link"));
int combination_method_param{};
node->get_parameter(name_ + "." + "combination_method", combination_method_param);
const int combination_method_param = declareOrGetParameter(node, "combination_method", 1);
#ifdef PRE_ROS_JAZZY
combination_method_ = combination_method_param;
#else
@@ -169,7 +203,11 @@ void VoxelLayer::updateBounds(
useExtraBounds(min_x, min_y, max_x, max_y);
bool current = true;
#ifdef PRE_ROS_LYRICAL
std::vector<nav2_costmap_2d::Observation> observations, clearing_observations;
#else
std::vector<nav2_costmap_2d::Observation::ConstSharedPtr> observations, clearing_observations;
#endif
// get the marking observations
current = getMarkingObservations(observations) && current;
@@ -182,16 +220,15 @@ void VoxelLayer::updateBounds(
// raytrace freespace
for (unsigned int i = 0; i < clearing_observations.size(); ++i) {
raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
raytraceFreespace(obsOf(clearing_observations[i]), min_x, min_y, max_x, max_y);
}
// place the new obstacles into a priority queue... each with a priority of zero to begin with
for (std::vector<nav2_costmap_2d::Observation>::const_iterator it = observations.begin(); it != observations.end();
++it)
for (auto it = observations.begin(); it != observations.end(); ++it)
{
const nav2_costmap_2d::Observation & obs = *it;
const nav2_costmap_2d::Observation & obs = obsOf(*it);
const sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_);
const sensor_msgs::msg::PointCloud2 & cloud = cloudOf(obs.cloud_);
double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_;
double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_;
@@ -277,7 +314,9 @@ void VoxelLayer::raytraceFreespace(
{
auto clearing_endpoints_ = std::make_unique<sensor_msgs::msg::PointCloud2>();
if (clearing_observation.cloud_->height == 0 || clearing_observation.cloud_->width == 0) {
const sensor_msgs::msg::PointCloud2 & clearing_cloud = cloudOf(clearing_observation.cloud_);
if (clearing_cloud.height == 0 || clearing_cloud.width == 0) {
return;
}
@@ -311,8 +350,8 @@ void VoxelLayer::raytraceFreespace(
}
clearing_endpoints_->data.clear();
clearing_endpoints_->width = clearing_observation.cloud_->width;
clearing_endpoints_->height = clearing_observation.cloud_->height;
clearing_endpoints_->width = clearing_cloud.width;
clearing_endpoints_->height = clearing_cloud.height;
clearing_endpoints_->is_dense = true;
clearing_endpoints_->is_bigendian = false;
@@ -331,9 +370,9 @@ void VoxelLayer::raytraceFreespace(
double map_end_y = origin_y_ + getSizeInMetersY();
double map_end_z = origin_z_ + getSizeInMetersZ();
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
sensor_msgs::PointCloud2ConstIterator<float> iter_x(clearing_cloud, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(clearing_cloud, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(clearing_cloud, "z");
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
double wpx = *iter_x;
@@ -431,7 +470,7 @@ void VoxelLayer::raytraceFreespace(
if (publish_clearing_points) {
clearing_endpoints_->header.frame_id = global_frame_;
clearing_endpoints_->header.stamp = clearing_observation.cloud_->header.stamp;
clearing_endpoints_->header.stamp = clearing_cloud.header.stamp;
clearing_endpoints_pub_->publish(std::move(clearing_endpoints_));
}
rtabmap_odom/CMakeLists.txt
+48
View File
@@ -157,4 +157,52 @@ install(DIRECTORY include/
FILES_MATCHING PATTERN "*.h"
)
#############
## Testing ##
#############
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(OpenCV REQUIRED COMPONENTS core imgcodecs)
# Recorded sensor input is replayed by the tests themselves, not by "ros2 bag play".
find_package(rosbag2_cpp REQUIRED)
# Real frames the visual odometry tests register against, read from the source tree:
# these binaries are never installed, and the fixtures are not either. See
# test/data/README.md for where they come from.
set(rtabmap_odom_test_data_root "${CMAKE_CURRENT_SOURCE_DIR}/test/data")
# Each node gets its own test binary: a crash or a stuck executor in one node cannot
# take the others down, and every binary starts with a clean DDS graph.
#
# Each binary also gets its own DDS domain. colcon tests packages in parallel and ctest
# can run these binaries in parallel, while these suites share topic names -- odom,
# rgbd_image, scan_cloud -- with rtabmap_sync's and rtabmap_util's. On a shared domain
# they discover each other's publishers and assertions then see traffic the test never
# sent. rtabmap_util numbers from 30 and rtabmap_sync from 50; keep the ranges apart.
set(rtabmap_odom_test_domain_id 70)
macro(rtabmap_odom_add_node_test test_name)
ament_add_gtest(${test_name} test/${test_name}.cpp
ENV ROS_DOMAIN_ID=${rtabmap_odom_test_domain_id}
TIMEOUT 300)
math(EXPR rtabmap_odom_test_domain_id "${rtabmap_odom_test_domain_id} + 1")
if(TARGET ${test_name})
target_include_directories(${test_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test)
target_compile_definitions(${test_name} PRIVATE
RTABMAP_ODOM_TEST_DATA_ROOT="${rtabmap_odom_test_data_root}")
target_link_libraries(${test_name} rtabmap_odom_plugins rtabmap_odom
opencv_core opencv_imgcodecs rosbag2_cpp::rosbag2_cpp rtabmap::core)
if("$ENV{ROS_DISTRO}" STRLESS "lyrical")
ament_target_dependencies(${test_name} ${AmentLibraries})
else()
target_link_libraries(${test_name} ${Libraries} ${PublicLibraries})
endif()
endif()
endmacro()
rtabmap_odom_add_node_test(test_odometry_ros)
rtabmap_odom_add_node_test(test_rgbd_odometry)
rtabmap_odom_add_node_test(test_stereo_odometry)
rtabmap_odom_add_node_test(test_icp_odometry)
endif()
ament_package()
rtabmap_odom/README.md
+273
View File
@@ -0,0 +1,273 @@
# rtabmap_odom
Odometry for [RTAB-Map](https://github.com/introlab/rtabmap): where the robot is relative to a local fixed frame, estimated from its own sensors — a pose that moves continuously and never jumps, but drifts over time.
SLAM needs a pose for every measurement it maps. These nodes produce one by registering each new frame against the last — visually from an RGB-D or stereo camera, or geometrically from a lidar — and integrating the result into a [`nav_msgs/msg/Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) and a TF.
## Contents
- [Nodes](#nodes)
- [Choosing a sensor modality for the environment](#choosing-a-sensor-modality-for-the-environment)
- [Library](#library)
- [Conventions](#conventions)
- [Frames and TF](#frames-and-tf)
- [RTAB-Map's own parameters](#rtab-maps-own-parameters)
- [Feeding in an external guess](#feeding-in-an-external-guess)
- [IMU](#imu)
- [Update rates and dropped frames](#update-rates-and-dropped-frames)
- [Lost frames, resets and new maps](#lost-frames-resets-and-new-maps)
- [Services](#services)
- [Published topics](#published-topics)
- [Outputting filtered scans and features](#outputting-filtered-scans-and-features)
- [Diagnostics](#diagnostics)
- [License](#license)
## Nodes
| Node | Description |
|---|---|
| [rgbd_odometry](doc/rgbd_odometry.md) | Visual odometry from a color image and a depth image registered to it. |
| [stereo_odometry](doc/stereo_odometry.md) | Visual odometry from a stereo pair. |
| [icp_odometry](doc/icp_odometry.md) | Geometric odometry from a 2D or 3D lidar. |
Every node is a [composable node](https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Composition.html) as well as a standalone executable.
### Choosing a sensor modality for the environment
Which to use is a question about the **environment**, not about which sensor is better. A camera tracks visual texture; a lidar tracks geometry. Each fails where its own cue is missing, and the two failures do not overlap much.
| Environment | Use | Why |
|---|---|---|
| Visually textured and well lit — offices, cluttered rooms, daylight outdoors | **Camera** | Plenty of features to match, and appearance gives loop closure for free. |
| Textureless but geometrically rich — bare corridors with doorways and furniture, warehouse aisles | **Lidar** | Blank walls give a camera nothing; the shape of the space still constrains ICP. |
| Dark, or lighting that changes abruptly | **Lidar** | A camera is simply blind. Lidar does not care. |
| Geometrically plain but visually rich — a large open hall with a patterned floor, textured flat walls | **Camera** | [Degenerate geometry](doc/icp_odometry.md#degenerate-geometry) defeats ICP here, while the texture is exactly what a camera needs. |
| Both plain and textureless — an empty warehouse, a long featureless tunnel | **Wheel odometry**, with either as a corrector | Neither cue is present. This is the case where wheel odometry carries the pose. |
| Repetitive and self-similar — tiled floors, rows of racking, a long colonnade | **Wheel odometry as the guess**, with either on top | Both cues are present but *ambiguous*: a camera matches the wrong copy of a feature, a lidar the wrong bay of shelving. See [Repetitive patterns](doc/rgbd_odometry.md#repetitive-patterns). |
| Outdoors at range | **Stereo camera or 3D lidar** | RGB-D depth stops working outdoors; both of these keep going. |
**Do not underestimate wheel odometry.** On a wheeled robot it is locally excellent and only drifts over distance — the opposite failure from both of the above, which are locally noisy but not systematically biased. It is also the only one of the three that keeps working when the environment offers no cue at all — and, because it is indifferent to what the scene *looks* like, the only one that is not fooled when the scene repeats itself.
**Fuse the wheels with an IMU before feeding them in.** [`robot_localization`](https://docs.ros.org/en/jazzy/p/robot_localization/) is the standard way: its EKF combines wheel odometry with IMU orientation and angular rates into one filtered `odom` topic, which is a markedly better guess than the wheels alone. [FusionCore](https://github.com/manankharwar/fusioncore) is another EKF that does the same job. The IMU fixes exactly what encoders are worst at — yaw through a turn, and wheel slip, which encoders report as motion that never happened. Where the camera or lidar fails outright, that filtered estimate is what carries the robot through, and a pipeline built this way degrades instead of breaking.
Which is why the robust arrangement is rarely one of them alone: feed wheel odometry in as `guess_frame_id` and the registration starts near the answer every frame. That covers the camera's fast-motion and blank-wall failures and the lidar's degenerate-corridor failure, while the camera or lidar in turn corrects the wheels' drift. See [Feeding in an external guess](#feeding-in-an-external-guess).
**For 2D indoor odometry a lidar usually costs less computation.** Registering a few hundred scan points is far less CPU than detecting, describing and matching visual features on every frame, and it needs no GPU — which is what decides whether odometry keeps up on the small onboard computers these robots carry.
With both a camera and a lidar, the usual arrangement is `icp_odometry` for the pose and the camera for appearance — see [Combining a camera and a lidar](doc/icp_odometry.md#combining-a-camera-and-a-lidar).
## Library
The package installs a C++ library, documented in the [C++ API reference](https://docs.ros.org/en/jazzy/p/rtabmap_odom/generated/index.html) generated from the headers.
**`OdometryROS`** is the base class all three nodes derive from, and it is where most of this package's behaviour actually lives. It owns the RTAB-Map `Odometry` object, the pose integration, the TF broadcast, the IMU intake, the services and the diagnostics. Each node subclasses it to do one thing: turn its own topics into a `rtabmap::SensorData` and hand it over. That is why the three nodes share nearly all of their parameters and publish exactly the same topics.
It also runs the registration on **its own thread**. A frame arriving while the previous one is still being processed does not block the subscription callback; see [Update rates and dropped frames](#update-rates-and-dropped-frames).
## Conventions
These apply to all three nodes.
### Frames and TF
| Parameter | Type | Default | Description |
|---|---|---|---|
| `frame_id` | `string` | `"base_link"` | The robot frame being tracked. The pose published is this frame's, not the sensor's — the sensor-to-robot transform is read from TF. |
| `odom_frame_id` | `string` | `"odom"` | The fixed frame the pose is expressed in. |
| `publish_tf` | `bool` | `true` | Broadcast the pose on TF. **Turn this off if something else already publishes that transform**, or the two fight and TF alternates between them. What exactly is broadcast depends on `guess_frame_id`: without it, `odom_frame_id` → `frame_id`; with it, a correction `odom_frame_id` → `guess_frame_id` ([why](#it-also-keeps-tf-alive-through-a-failure)). |
| `wait_for_transform` | `double` | `0.1` | Seconds to wait for a needed transform before giving up on the frame. |
| `initial_pose` | `string` | `""` | Starting pose, `"x y z roll pitch yaw"`. Also settable at runtime through `reset_odom_to_pose`. |
| `ground_truth_frame_id` | `string` | `""` | When set, the pose is taken from this TF instead of being computed — for replaying a dataset with a known trajectory. |
| `ground_truth_base_frame_id` | `string` | value of `frame_id` | The robot frame within the ground truth TF tree. |
| `guess_frame_id` | `string` | `""` | A frame carrying another odometry source, used as the initial guess for each registration. Documented with its companions under [Feeding in an external guess](#feeding-in-an-external-guess) -- **the highest-value parameter here for a wheeled robot**. |
The sensor must be connected to `frame_id` in TF **before the first frame arrives**, or that frame is dropped with a warning. A static publisher is the usual answer.
### RTAB-Map's own parameters
Everything in RTAB-Map's odometry parameter set is exposed as a ROS parameter **under its RTAB-Map name**, so tuning is done directly:
```bash
ros2 run rtabmap_odom rgbd_odometry --ros-args \
-p "Odom/Strategy:='1'" \
-p "Vis/MinInliers:='15'" \
-p "Odom/ResetCountdown:='1'"
```
**Note the quoting.** Every RTAB-Map parameter is declared as a **string**, whatever it looks like, because that is how RTAB-Map's own parameter map stores them. Writing `-p Odom/Strategy:=1` makes ROS infer an integer, and the node throws on startup rather than starting with the wrong value:
```
parameter 'Odom/Strategy' has invalid type: Wrong parameter type,
parameter {Odom/Strategy} is of type {string}, setting it to {integer} is not allowed.
```
The inner quotes are what keeps it a string. Shell quotes alone do not help, since the value is parsed as YAML after the shell is done with it. In a launch file the same rule reads naturally: `{'Odom/Strategy': '1'}`.
This applies **only** to RTAB-Map's own parameters. The nodes' ROS parameters -- `frame_id`, `publish_tf`, `scan_voxel_size`, `approx_sync` -- are declared with their real types and take plain values.
Which parameters exist depends on the node: each declares the set matching its sensor, so `Vis/*` appears on the visual nodes and `Icp/*` only on `icp_odometry`. `ros2 param list` on a running node is the authoritative list; the meaning of each is in [RTAB-Map's parameter reference](https://github.com/introlab/rtabmap/blob/master/corelib/include/rtabmap/core/Parameters.h).
The two worth knowing before anything else:
- **`Odom/Strategy`** selects the registration algorithm — `0` frame-to-map (default, more accurate), `1` frame-to-frame (cheaper), and others for the external VO libraries RTAB-Map can be built against.
- **`Odom/ResetCountdown`** automatically resets odometry after this many consecutive lost frames instead of staying lost forever. `0` disables it, which is the default.
`config_path` loads the same parameters from an INI file; only the odometry ones are taken from it.
### Feeding in an external guess
Registration works far better when it starts near the answer. Two ways to supply one:
| Parameter | Type | Default | Description |
|---|---|---|---|
| `guess_frame_id` | `string` | `""` | A TF frame carrying another odometry source — wheels, IMU-integrated, a base driver. Its motion between frames becomes the initial guess. |
| `guess_min_translation` | `double` | `0.0` | Skip frames whose guessed motion is below this, in meters. `0` disables. |
| `guess_min_rotation` | `double` | `0.0` | Same, in radians. |
| `guess_min_time` | `double` | `0.0` | Same, in seconds. |
| `guess_linear_variance` | `double` | `0.001` | Covariance of the published pose when the guess is used directly. |
| `guess_angular_variance` | `double` | `0.001` | Same, rotational. |
**`guess_frame_id` is the single biggest improvement available to a wheeled robot.** Wheel odometry is locally excellent and globally hopeless; visual and ICP registration is the reverse. Giving the registration a wheel-odometry guess makes it converge more often, faster, and survive the frames where the camera sees nothing.
The `guess_min_*` parameters additionally suppress processing while the robot is stationary, which stops a static scene from accumulating drift and saves the CPU.
#### It also keeps TF alive through a failure
Setting `guess_frame_id` also changes *how* the pose is broadcast. This is worth understanding before odometry fails on a real robot, because it decides what the rest of the system sees while it is lost.
With a guess frame configured, the node no longer publishes `odom_frame_id` → `frame_id` directly. It publishes a **correction** instead, `odom_frame_id` → `guess_frame_id`.
So the guess source keeps the conventional `odom` frame -- whatever produces it, the robot's driver or [`robot_localization`](https://docs.ros.org/en/jazzy/p/robot_localization/) -- and **this node takes a different name for its own `odom_frame_id`**. The examples in this repository use `vo` for the visual nodes and `icp_odom` for the lidar one:
```mermaid
flowchart TD
ODOM(["/vo<br><i>odom_frame_id</i>"])
GUESS(["/odom<br><i>guess_frame_id</i>"])
BASE(["/base_link<br><i>frame_id</i>"])
SENSOR(["/camera or /lidar<br><i>the sensor's header.frame_id</i>"])
ODOM -->|correction, this node<br>e.g. ~10 Hz, ~50 ms delay| GUESS
GUESS -->|robot driver or robot_localization<br>e.g. ~50 Hz, ~1 ms delay| BASE
BASE -->|static| SENSOR
```
*The rates and delays above are examples only — yours depend on the sensor, the base driver and the computer.*
**That chain keeps being published while registration is lost.** The correction freezes at the last successfully computed pose composed with the motion the guess has accumulated since, so `base_link` keeps moving in TF at the guess source's rate, driven entirely by the guess. Nothing downstream stalls or jumps; the pose just accumulates that source's drift until registration recovers. Without `guess_frame_id` there is no correction to publish and **no TF at all is broadcast while lost**, which is what breaks the tree.
Pair it with `Odom/ResetCountdown` and the recovery is complete. Take a robot turning to face a white wall: visual odometry loses tracking, TF keeps flowing from the wheels, and after the configured number of failed frames the odometry resets — not to where it was when it got lost, but to `last computed pose × guess motion`, which is where the wheels say the robot has got to in the meantime. Registration restarts from there and the trajectory carries on with only the drift the wheels accumulated.
What it resets to depends on what is available, in this order:
1. **A guess** — resets to the last pose composed with the guess motion, as above.
2. **No guess, but `odom_frame_id` → `frame_id` exists in TF at the sensor frame's stamp** — resets to that pose. This is the `publish_tf:=false` arrangement: this node publishes only its odometry topic, [`robot_localization`](https://docs.ros.org/en/jazzy/p/robot_localization/) fuses that topic with the wheels and the IMU, and the filter owns the transform. The reset therefore lands on the filter's current estimate — the odometry gets restarted from where the fused solution says the robot is, having contributed to that solution itself while it was working. `publish_tf` has to be off for this to mean anything, and **`publish_null_when_lost` should be off too**: the null pose is a signal for consumers that read it as one, and a filter fusing this topic is not — it would be handed an invalid pose to fuse. With it off the node simply stops publishing while lost, and the filter carries on from its other inputs until registration recovers.
3. **Neither** — resets to the last computed pose, so the robot resumes believing it never moved while lost.
After an automatic reset the countdown is left armed, so if odometry still cannot initialize on the next frames it keeps re-resetting to the latest guess rather than getting stuck.
### IMU
| Parameter | Type | Default | Description |
|---|---|---|---|
| `wait_imu_to_init` | `bool` | `false` | Hold off until an IMU message has arrived, so gravity is known from the first frame. |
| `imu_queue_size` | `int` | `200` | Depth of the IMU buffer. IMUs run far faster than cameras; this is why it is large. |
| `qos_imu` | `int` | value of `qos` | Reliability of the `imu` subscription. |
| `always_check_imu_tf` | `bool` | `false` | Re-read the IMU-to-robot transform every message rather than caching it. |
The `imu` topic is optional on all three nodes. Supplying it lets odometry know which way is down, which constrains roll and pitch — worth doing on any robot that has an IMU, and close to mandatory for a handheld or aerial one.
**With no `guess_frame_id`, the IMU also supplies the rotation half of each frame's guess.** The rotation measured between the previous frame and this one becomes the guess's orientation, leaving the translation to the motion model. That is often the difference between tracking a fast turn and losing it, since rotation is what breaks feature matching first. An external guess takes precedence when there is one: `guess_frame_id` is used whole, and the IMU is not consulted for the guess at all.
### Update rates and dropped frames
Registration runs on its own thread, so a slow frame does not block the subscription. What happens to the frames arriving meanwhile is a choice:
| Parameter | Type | Default | Description |
|---|---|---|---|
| `always_process_most_recent_frame` | `bool` | `true` | Drop frames that arrive while registration is still running and take the newest. `false` registers every frame in order, on the subscription thread. |
| `expected_update_rate` | `double` | `0.0` | The rate frames are expected at, in Hz. Used only when `max_update_rate` is unset, and it is a **ceiling**: a frame arriving sooner than `1/expected_update_rate` after the last one is skipped, with a warning that the input is faster than expected. `0` disables. |
| `max_update_rate` | `double` | `0.0` | Throttle registration to at most this rate, skipping frames silently. Takes precedence over `expected_update_rate`. `0` disables. |
| `min_update_rate` | `double` | `0.0` | Treat odometry as **lost and reset it** when more than `1/min_update_rate` passes between updates — the motion assumption no longer holds across a gap that long. `0` disables. |
**`always_process_most_recent_frame` already bounds the delay.** A frame arriving while registration is still running is dropped on the spot rather than queued, so the worker always picks up the newest frame and the published pose is at most one registration behind the sensor. No backlog ever forms. `/diagnostics` reports how many frames went this way.
That is why **`max_update_rate` is about CPU, not latency**: given the skipping above, the worst-case delay is roughly the same whether it is set or not. What it changes is how many frames get registered at all. Set it to give the rest of the robot its cores back — not to make the pose fresher, which it will not do.
Setting `always_process_most_recent_frame:=false` is the opposite trade: every frame is registered, in order, on the subscription thread. That is what you want when replaying a bag, where dropping frames loses data you meant to process.
### Lost frames, resets and new maps
A frame that cannot be registered is *lost*: the node publishes an all-zero pose with `9999` down the diagonal of both covariance matrices, which says there is no pose here to use.
The first frame after a reset — from `reset_odom`, `reset_odom_to_pose` or `Odom/ResetCountdown` — carries the same `9999` for a different reason. It is an *initialization* rather than a registration: nothing to measure against, no velocity to carry over. Its pose is real and meant to be used; what the covariance says is that it does not continue the last valid one.
| Parameter | Type | Default | Description |
|---|---|---|---|
| `publish_null_when_lost` | `bool` | `true` | Publish a null pose, with `9999` on the covariance diagonals, when a frame cannot be registered. `false` publishes nothing. |
**Leave it on, unless a filter is consuming this topic.** A consumer that sees the null message knows odometry is lost; one that sees nothing cannot tell that apart from a node that died or a topic that was never connected. `rtabmap` relies on it to know the frame should not be mapped.
`rtabmap` reads an identity pose, or both covariances at `9999`, as a discontinuity, and starts a **new map** rather than deforming the graph across a jump the robot never made:
```
Odometry is reset (identity pose or high variance detected). Increment map id!
```
While it is lost, `publish_null_when_lost:=true` publishes a null pose and no velocity for every frame, both marked `9999`. With `:=false` it publishes nothing — except that a guess frame keeps it going: every frame that re-initialises the map, which `Odom/ResetCountdown` makes frequent, is published with the guess's pose and confidence, so the topic has no gap for as long as the guess is there. What differs between configurations is the first frame after the reset, and where it restarts from:
| | First frame after the reset | Second frame | TF while lost | `rtabmap` |
|---|---|---|---|---|
| `publish_null_when_lost:=true` (default), with or without a guess | recovered pose, `9999` on both pose and velocity | registered, from the recovered pose | unbroken with a guess, absent without one | new map |
| `publish_null_when_lost:=false` with `guess_frame_id` | recovered pose and the guess's velocity, both with the guess's covariance | registered, from the recovered pose | unbroken | one session |
| `publish_null_when_lost:=false`, `publish_tf:=false`, another node publishing `odom` → `base_link` | not published | registered, from the recovered pose | unbroken, published by the other node | one session |
| `publish_null_when_lost:=false` with neither | not published | registered, from the pose held before the loss | absent until it recovers | one session, across the gap |
*Registered* is the ordinary case: a pose and a velocity measured against the previous valid frame, with the covariance the registration computed.
The two middle rows are the ones to build on. Either an external source is named through `guess_frame_id`, and the restarting frame is published as a continuation of the trajectory — the poses *and* the covariances staying continuous for as long as that guess is published — or a filter such as `robot_localization` owns `odom` → `base_link`, and the reset adopts whatever pose it holds.
**The last row is a trap.** With nothing to say where the robot went while the odometry was lost, the reset resumes at the pose from before it, that motion is dropped from the trajectory, and since no `9999` ever reaches `rtabmap` the map is deformed across the gap rather than split at it. The node reports that combination as an error at startup.
For finer control, put an intermediate node between this one and `rtabmap` and let it set the covariances itself. It decides what counts as a discontinuity, instead of that being inferred from the reset alone — starting a new map when the guess frame has gone quiet, say, and the newly computed pose may be wrong even though registration reported success.
Recovering from lost is what `Odom/ResetCountdown` is for, or the `reset_odom` service. Combined with `guess_frame_id` it also keeps the TF tree intact throughout — see [It also keeps TF alive through a failure](#it-also-keeps-tf-alive-through-a-failure).
## Services
| Service | Type | Description |
|---|---|---|
| `reset_odom` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Drop the internal local map and restart the pose — at the identity, or, with `guess_frame_id` configured, at whatever pose the guess frame currently holds, so that odometry restarts where the other source says the robot is. |
| `reset_odom_to_pose` | [`rtabmap_msgs/srv/ResetPose`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/ResetPose.html) | Reset to a given `x y z roll pitch yaw`. |
| `pause_odom` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Stop processing incoming frames. |
| `resume_odom` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Resume. |
| `log_debug`, `log_info`, `log_warning`, `log_error` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Change RTAB-Map's own log level at runtime. |
## Published topics
Common to all three nodes. **Every one of them, `odom` included, is published only when something is subscribed** -- the work of building each message is skipped otherwise. The TF broadcast is not gated this way and happens whenever `publish_tf` is on -- except while registration is lost with no guess frame configured, when there is nothing to broadcast.
| Topic | Type | Description |
|---|---|---|
| `odom` | [`nav_msgs/msg/Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) | The pose and velocity. Covariance is meaningful: it grows with registration uncertainty, and is `9999` on the diagonal when lost. |
| `odom_info` | [`rtabmap_msgs/msg/OdomInfo`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/OdomInfo.html) | Everything about how the frame was registered — inlier count, matches, features, timings. The first thing to look at when odometry misbehaves. |
| `odom_info_lite` | [`rtabmap_msgs/msg/OdomInfo`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/OdomInfo.html) | The same without the per-feature arrays, for logging or a slow link. |
| `odom_local_map` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The feature map the current frame was registered against. Visual paths only — built from the frame's visual words, so `icp_odometry` never fills it. |
| `odom_local_scan_map` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The scan map, the ICP path's equivalent of `odom_local_map`. |
| `odom_last_frame` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The current frame's **features**, in the odom frame — not its scan or its pixels. Visual paths only, for the same reason as `odom_local_map`; for the filtered scan see `odom_sensor_data/*`. |
| `odom_rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The frame **as odometry processed it**, not the input as it arrived. See [Outputting filtered scans and features](#outputting-filtered-scans-and-features). |
| `odom_sensor_data/raw`, `/features`, `/compressed` | [`rtabmap_msgs/msg/SensorData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/SensorData.html) | The same frame as `SensorData`. `/features` strips the images and scan and keeps only the extracted features; `/compressed` carries JPEG/PNG images instead of raw. |
### Outputting filtered scans and features
`odom_rgbd_image` and `odom_sensor_data/*` republish the frame **after** odometry has worked on it, which is the point of them — they are what odometry actually registered, not a copy of the input:
- **Features are included.** Registration writes the keypoints, their 3D positions and their descriptors back into the frame, so these topics carry them. `odom_sensor_data/features` is that alone, with the images and scan removed.
- **The scan is the filtered one.** `icp_odometry` builds the frame after deskewing, voxelization, range filtering and normal estimation, so what comes out here is the decimated cloud ICP saw — not the raw sweep the lidar published. Subscribe to the driver's topic if you want the original.
- **Images are converted.** `rgbd_odometry` hands over grayscale unless `keep_color` is set, so that is what these carry too.
## Diagnostics
All three publish to `/diagnostics`: the input rate, the output rate, and how many frames were processed versus dropped. A healthy input rate with a low output rate means frames are arriving but not registering — check `odom_info` before touching anything else.
## License
BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license).
rtabmap_odom/doc/icp_odometry.md
+238
View File
@@ -0,0 +1,238 @@
# icp_odometry
Odometry from a 2D or 3D lidar, by registering each scan against the previous one with ICP.
No features and no appearance: the motion is whatever transform best aligns this scan's points with the last. That makes it indifferent to lighting and texture — it works in the dark, and on the blank white corridor where [rgbd_odometry](rgbd_odometry.md) has nothing to track.
What it is sensitive to instead is **geometry**. ICP can only recover motion that the scene's shape constrains, and a scene can fail to constrain it: see [Degenerate geometry](#degenerate-geometry), which is the failure mode worth understanding before deploying this.
The shared parameters — frames, TF, guesses, the IMU, RTAB-Map's own, the services — are in the [package README](../README.md#conventions). This page covers what is specific to this node.
## Contents
- [Usage](#usage)
- [Subscribed Topics](#subscribed-topics)
- [Published Topics](#published-topics)
- [Reusing the filtered scan downstream](#reusing-the-filtered-scan-downstream)
- [Parameters](#parameters)
- [Where these defaults come from](#where-these-defaults-come-from)
- [Making the correspondence ratio mean something](#making-the-correspondence-ratio-mean-something)
- [Preparing the scan](#preparing-the-scan)
- [Deskewing](#deskewing)
- [Degenerate geometry](#degenerate-geometry)
- [Combining a camera and a lidar](#combining-a-camera-and-a-lidar)
- [When it loses track](#when-it-loses-track)
## Usage
2D lidar:
```bash
ros2 run rtabmap_odom icp_odometry --ros-args \
-r scan:=/scan \
-p frame_id:=base_link
```
3D lidar:
```bash
ros2 run rtabmap_odom icp_odometry --ros-args \
-r scan_cloud:=/velodyne_points \
-p frame_id:=base_link \
-p "Icp/PointToPlane:='true'" \
-p scan_normal_k:=10 \
-p scan_voxel_size:=0.1
```
```python
ComposableNode(
package='rtabmap_odom',
plugin='rtabmap_odom::ICPOdometry',
name='icp_odometry',
parameters=[{'frame_id': 'base_link',
'scan_voxel_size': 0.1,
'scan_normal_k': 10,
'Icp/PointToPlane': 'true'}],
remappings=[('scan_cloud', '/velodyne_points')])
```
## Subscribed Topics
One of the two scan topics, not both.
| Topic | Type | Description |
|---|---|---|
| `scan` | [`sensor_msgs/msg/LaserScan`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/LaserScan.html) | A 2D lidar. |
| `scan_cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | A 3D lidar, or a 2D one already converted to a cloud. |
| `imu` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Optional, and more useful here than elsewhere — it pins roll and pitch, which a lidar alone constrains poorly. |
## Published Topics
Most are common to all three nodes; see [the README](../README.md#published-topics). Two belong to this path:
| Topic | Type | Description |
|---|---|---|
| `odom_local_scan_map` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The accumulated scan map the current scan was registered against. |
| `odom_filtered_input_scan` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The input scan **after** deskewing, voxelization, range filtering and normal estimation — exactly what ICP registered, carrying the original header. |
### Reusing the filtered scan downstream
Remap `odom_filtered_input_scan` onto `rtabmap`'s `scan_cloud` and the map is built from the scan this node already prepared, rather than from the raw sweep.
```mermaid
flowchart LR
LIDAR["lidar"]
ICP["icp_odometry"]
MAP["rtabmap<br>subscribe_scan_cloud:=true"]
LIDAR -->|scan_cloud| ICP
ICP -->|odom_filtered_input_scan| MAP
ICP -->|odom + TF| MAP
```
That skips the expensive half twice over. Voxelization and normal estimation are not repeated, since the cloud arrives already decimated and carrying `normal_*` fields, and the deskewing this node did is carried along with it.
The alternative for deskewing is to do it **before** odometry, with [`lidar_deskewing`](../../rtabmap_util/doc/lidar_deskewing.md), and fan the corrected cloud out to both nodes:
```mermaid
flowchart LR
LIDAR["lidar"]
DESKEW["lidar_deskewing"]
ICP["icp_odometry"]
MAP["rtabmap<br>subscribe_scan_cloud:=true"]
LIDAR -->|scan_cloud| DESKEW
DESKEW -->|deskewed cloud| ICP & MAP
ICP -->|odom + TF| MAP
```
That is the only way to give `rtabmap` **every point of the sweep**. `odom_filtered_input_scan` carries the decimated cloud ICP registered, so a map built from it inherits whatever voxelization odometry applied — and outdoors that is 30 to 50 cm. Deskewing upstream separates the two: odometry can filter as hard as it likes while the map is built from the full-resolution cloud, at the cost of an extra node and an extra copy of every sweep.
## Parameters
Specific to this node: the scan is filtered **before** ICP sees it, and these control that. The registration itself is tuned through RTAB-Map's `Icp/*` parameters.
These two sets overlap, and the node resolves the overlap for you — see [Where these defaults come from](#where-these-defaults-come-from), because the defaults below are not what the source's initializers suggest.
| Parameter | Type | Default | Description |
|---|---|---|---|
| `scan_voxel_size` | `double` | `0.05` | Downsample to one point per voxel of this size, in meters. From `Icp/VoxelSize`. `0` disables it here. |
| `scan_downsampling_step` | `int` | `1` | Keep every Nth point. From `Icp/DownsamplingStep`. Cheaper than voxelization but density-dependent; prefer `scan_voxel_size`. |
| `scan_range_min` | `double` | `0.0` | Drop points closer than this, in meters. From `Icp/RangeMin`. `0` disables. Useful against the robot's own body. |
| `scan_range_max` | `double` | `0.0` | Drop points farther than this. From `Icp/RangeMax`. `0` disables. |
| `scan_normal_k` | `int` | `5` | Estimate each point's normal from this many neighbours. From `Icp/PointToPlaneK`. **Point-to-plane ICP needs normals**; without them it has nothing to work with. |
| `scan_normal_radius` | `double` | `0.0` | Estimate normals from neighbours within this radius instead. From `Icp/PointToPlaneRadius`. `0` disables. |
| `scan_normal_ground_up` | `double` | `0.0` | Force normals to point upward when within this dot-product threshold of vertical. From `Icp/PointToPlaneGroundNormalsUp`. Helps on ground planes. |
| `scan_cloud_max_points` | `int` | `-1` | How many points a full sweep of the lidar holds. It is the **denominator of the correspondence ratio** — see [Making the correspondence ratio mean something](#making-the-correspondence-ratio-mean-something). `-1` leaves it unset; an organized cloud fills it in automatically. |
| `scan_cloud_is_2d` | `bool` | `false` | Treat `scan_cloud` as a planar scan even though it carries a `z` field, so it is registered as a 2D scan rather than a 3D one. For a 2D lidar already converted to a cloud. |
| `deskewing` | `bool` | `false` | Correct for motion during the sweep. See [Deskewing](#deskewing). |
| `deskewing_slerp` | `bool` | `false` | Interpolate the deskewing transform rather than looking up TF per point. Faster, slightly less accurate. |
| `topic_queue_size` | `int` | `1` | Queue depth of the scan subscription. Deliberately small: a stale scan is worse than a dropped one. |
### Where these defaults come from
Filtering a scan and filtering it again inside ICP would be wasted work, so at startup the node moves each filter from RTAB-Map's parameter to its own:
> `IcpOdometry: Transferring value 5 of "Icp/PointToPlaneK" to ros parameter "scan_normal_k" for convenience.`
That log line is normal, not a warning about your configuration. Each `scan_*` parameter above **takes its default from the matching `Icp/*` parameter**, and the `Icp/*` one is then set to `0` so the filter runs once, here, rather than twice. This is why `scan_voxel_size` is `0.05` and `scan_normal_k` is `5` out of the box rather than disabled.
Setting the ROS parameter explicitly wins: the transfer is skipped and the `Icp/*` value is zeroed instead. Setting **both** is the case to avoid — the node warns that both are set, and the scan is then filtered twice:
```
IcpOdometry: Both parameter "Icp/VoxelSize" and ros parameter "scan_voxel_size" are set.
```
So tune through `scan_*` **or** through `Icp/*`, not both.
### Making the correspondence ratio mean something
`Icp/CorrespondenceRatio` decides whether a registration is trustworthy: the points ICP managed to pair, over the points it could have paired. `scan_cloud_max_points` is what sets that second number. Set it to the theoretical maximum points per sweep. No need to set it explicitly for organized clouds though, `width × height` will be used as maximum points.
Left at `-1`, the denominator becomes the size of the larger of the two scans being matched (for dense clouds). Take two scans that came back with 30 and 50 points — a lidar staring at open space, where most rays returned nothing. Dividing by 50 says "we matched most of what we saw", and the ratio looks healthy. But the sensor emits 10000 rays a sweep, so 50 returns means almost nothing was in range, and the registration is resting on nearly no evidence. Told that a full sweep is 10000 points, ICP divides by that instead and the ratio collapses to what the overlap actually was, so the threshold rejects the frame.
## Preparing the scan
ICP cost grows with the number of points, and a 3D lidar produces far more than registration needs — a 64-beam sensor is a hundred thousand points per sweep, and aligning them all is both slow and *no more accurate* than aligning a well-spread subset. Voxelization is therefore on by default at 5 cm.
What it buys beyond speed is even density, which matters more than the point count: a raw lidar sweep is dense near the sensor and sparse far away, so an unvoxelized ICP is dominated by whatever is closest — often the robot itself or the ground right under it. Size `scan_voxel_size` to the environment: **0.05 to 0.2 m indoors**, and **0.3 to 0.5 m outdoors**, where the scene is far larger and the extra resolution buys nothing but CPU.
**Move `Icp/MaxCorrespondenceDistance` with it — a good rule of thumb is ten times the voxel size.** The two are coupled: voxelizing at 0.3 m leaves neighbouring points that far apart, so a correspondence distance of 0.1 m cannot pair anything and ICP returns nothing at all.
**Point-to-plane ICP converges better than point-to-point** on the flat surfaces that dominate most environments, and it is what the default `scan_normal_k` of 5 is there to support:
```bash
-p "Icp/PointToPlane:='true'" -p scan_normal_k:=10
```
The quoting is not optional: RTAB-Map parameters are strings, and an unquoted `true` makes the node throw on startup ([why](../README.md#rtab-maps-own-parameters)).
Whether it is on by default depends on how RTAB-Map was built — `Icp/PointToPlane` defaults to `true` only with libpointmatcher available, and `false` otherwise — so set it explicitly if you care.
`scan_range_min` is worth setting on any robot whose lidar can see parts of itself. Those points are perfectly self-consistent between scans, so they pull the alignment toward "no motion" — a bias that looks like the robot under-travelling rather than like an error.
## Deskewing
A spinning lidar measures its points over a whole revolution, not at an instant. If the robot moves during that revolution, the scan is a smear — the points are in a frame that no longer exists by the time the sweep ends. At walking pace with a 10 Hz lidar this is centimeters; on a fast vehicle it dominates the error budget.
`deskewing:=true` corrects it. It needs the cloud to carry **per-point timestamps**, in a field named `t`, `time`, `stamps` or `timestamp`; without one the node logs an error and drops the frame rather than guessing.
There are two ways it gets the motion to correct with, and which one is used depends on whether you gave it an external guess:
- **With `guess_frame_id` set** — the motion comes from that TF. This is the accurate route, and the reason to pair deskewing with wheel odometry or an IMU-integrated frame.
- **Without it** — a constant-velocity model from the previous frame's estimate. It cannot deskew the very first frame, and it degrades exactly when velocity changes fastest, which is when deskewing matters most.
`deskewing_slerp` interpolates between the sweep's endpoints rather than looking up a transform per point. Much cheaper, and accurate enough unless the motion within one sweep is strongly non-linear.
## Degenerate geometry
This is the failure that matters, and it is not a bug. ICP recovers only the motion the scene constrains, and some scenes do not constrain all of it:
- **A long featureless corridor** does not constrain motion *along* the corridor. The walls look identical a meter forward, so ICP happily reports no motion while the robot drives. The map then folds the corridor up into a fraction of its length.
- **A large open space** with everything out of range constrains nothing at all.
- **A flat plane** — a warehouse floor to a horizontal 2D lidar — constrains height and tilt but not translation.
RTAB-Map detects this rather than walking into it, but only on the point-to-plane path. The defences, in order of effectiveness:
1. **`guess_frame_id` with wheel odometry.** The guess supplies the motion ICP cannot see, and ICP corrects the part it can. This turns the corridor case from a failure into a non-issue, and it is why lidar odometry on a wheeled robot should essentially always have it.
2. **The structural complexity check**, which is the built-in one. With `Icp/PointToPlane` on, a scan whose normals fail to span the space — the definition of a corridor — scores below `Icp/PointToPlaneMinComplexity` (`0.02`) and is handled by `Icp/PointToPlaneLowComplexityStrategy` instead of being trusted:
| Value | Behaviour |
|---|---|
| `0` | Reject the transform outright: the frame is reported lost. |
| `1` *(default)* | Recompute with point-to-point and constrain the correction to the axes that *are* observable — in a corridor, y and yaw are kept and **x is taken from the guess**. |
| `2` | Recompute with point-to-point and accept the result as is. |
| `3` | Keep the point-to-plane transform, with the same axis-constrained projection as `1`. |
The default pairs with defence 1: it detects the unobservable axis and hands that axis to the guess. Without a `guess_frame_id` there is nothing to hand it to, which is why the two belong together.
3. **A 3D lidar instead of a 2D one**, which sees ceiling, floor and doorways that a horizontal slice misses.
4. **`Icp/CorrespondenceRatio`** to reject registrations supported by too few correspondences, so a bad frame is reported lost rather than silently accepted.
With `Icp/PointToPlane` off, none of the complexity machinery runs: there are no normals to measure, so a degenerate scan is registered and trusted like any other.
## Combining a camera and a lidar
With both sensors, the usual arrangement is `icp_odometry` for the pose and the camera for appearance:
```mermaid
flowchart LR
LIDAR["lidar"]
CAM["RGB-D camera"]
ICP["icp_odometry"]
SYNC["rgbd_sync"]
ODOM(["odom + TF"])
MAP["rtabmap<br>subscribe_rgbd + subscribe_scan_cloud"]
LIDAR --> ICP --> ODOM --> MAP
LIDAR --> MAP
CAM --> SYNC --> MAP
```
Lidar geometry is the more reliable pose source, while loop closure detection is appearance-based and wants the images. `rtabmap` then subscribes to the camera, the scan and this node's odometry together.
## When it loses track
`odom_info` carries the ICP result. The numbers to look at are the correspondence count and ratio: too few correspondences means the scans do not overlap enough, whether because the robot moved too far between them, the range filters are too aggressive, or the scene genuinely changed.
- **Scans too far apart** — the lidar rate is too low for the speed, or `max_update_rate` is throttling too hard.
- **`Icp/MaxCorrespondenceDistance` too small** — ICP never associates the points at all. It has to be larger than the motion between scans; too large and it associates the wrong things.
- **Everything filtered away** — check `scan_range_min`/`scan_range_max` and `scan_voxel_size` against the actual scale of the environment.
As everywhere else in this package, `Odom/ResetCountdown` recovers automatically from a lost state instead of staying lost.
rtabmap_odom/doc/rgbd_odometry.md
+211
View File
@@ -0,0 +1,211 @@
# rgbd_odometry
Visual odometry from a color image, a registered depth image and a calibration.
Each frame's visual features are matched against the previous frame — or against a small local map of recent features — and the camera motion that best explains the matches becomes the pose. Depth turns the 2D feature matches into 3D correspondences, which is what makes the scale real rather than arbitrary.
Use it when an RGB-D camera is the main sensor. For a stereo pair use [stereo_odometry](stereo_odometry.md); for a lidar, [icp_odometry](icp_odometry.md). All three publish the same topics and share the parameters in the [package README](../README.md#conventions), which covers frames, TF, the RTAB-Map parameters, guesses, the IMU and the services. This page covers what is specific to this node.
## Contents
- [Pipeline arrangements](#pipeline-arrangements)
- [Usage](#usage)
- [Subscribed Topics](#subscribed-topics)
- [Published Topics](#published-topics)
- [Parameters](#parameters)
- [Synchronization](#synchronization)
- [Several cameras](#several-cameras)
- [Repetitive patterns](#repetitive-patterns)
- [When it loses track](#when-it-loses-track)
## Pipeline arrangements
A camera-only pipeline, with the camera synchronized once and fanned out -- the arrangement [Synchronization](#synchronization) recommends:
```mermaid
flowchart LR
CAM["RGB-D camera"]
SYNC["rgbd_sync"]
ODOM["rgbd_odometry<br>subscribe_rgbd:=true"]
MAP["rtabmap<br>subscribe_rgbd:=true"]
CAM -->|rgb/image<br>depth/image<br>rgb/camera_info| SYNC
SYNC -->|rgbd_image| ODOM & MAP
ODOM -->|odom + TF| MAP
```
Without `rgbd_sync`, both nodes subscribe to the three raw topics and each synchronizes them independently -- which works, but lets the two settle on different pairings.
Another arrangement drops `rgbd_sync` altogether and feeds `rtabmap` from **this node's own output** instead:
```mermaid
flowchart LR
CAM["RGB-D camera"]
ODOM["rgbd_odometry"]
MAP["rtabmap<br>subscribe_rgbd or subscribe_sensor_data"]
CAM -->|rgb/image<br>depth/image<br>rgb/camera_info| ODOM
ODOM -->|odom_rgbd_image<br>or odom_sensor_data| MAP
ODOM -->|odom + TF| MAP
```
Remap `rtabmap`'s `rgbd_image` to `odom_rgbd_image`, or set `subscribe_sensor_data` and remap to `odom_sensor_data/raw`. Two things come for free:
- **No separate synchronization.** This node already matched the three topics to register the frame, and republishes the result, so there is no `rgbd_sync` to run and no second synchronizer to agree with.
- **The features are reused.** `odom_sensor_data` carries the keypoints, their 3D positions and their descriptors that odometry extracted; they survive the conversion back into RTAB-Map on the other side, so `rtabmap` does not redo feature detection and descriptor extraction.
## Usage
Against a camera's raw topics:
```bash
ros2 run rtabmap_odom rgbd_odometry --ros-args \
-r rgb/image:=/camera/color/image_raw \
-r depth/image:=/camera/depth/image_rect_raw \
-r rgb/camera_info:=/camera/color/camera_info \
-p frame_id:=base_link
```
Against an [`rgbd_sync`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_sync) output, which is the better arrangement when anything else consumes the same camera:
```bash
ros2 run rtabmap_odom rgbd_odometry --ros-args \
-p subscribe_rgbd:=true \
-r rgbd_image:=/camera/rgbd_image \
-p frame_id:=base_link
```
```python
ComposableNode(
package='rtabmap_odom',
plugin='rtabmap_odom::RGBDOdometry',
name='rgbd_odometry',
parameters=[{'frame_id': 'base_link', 'subscribe_rgbd': True}],
remappings=[('rgbd_image', '/camera/rgbd_image')])
```
## Subscribed Topics
Which topics are used depends on `subscribe_rgbd` and `rgbd_cameras`.
**Default** — `subscribe_rgbd:=false`:
| Topic | Type | Description |
|---|---|---|
| `rgb/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Color or mono image. Goes through `image_transport`. |
| `depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Depth registered to the color camera. `16UC1` in millimeters or `32FC1` in meters. |
| `rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Calibration of the color camera. |
**With `subscribe_rgbd:=true`**, one pre-synchronized message instead of three topics:
| `rgbd_cameras` | Topic | Type |
|---|---|---|
| `1` (default) | `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) |
| `2`–`6` | `rgbd_image0` … `rgbd_image5` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) |
| `0` | `rgbd_images` | [`rtabmap_msgs/msg/RGBDImages`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImages.html) |
`rgbd_cameras:=0` takes any number of cameras in a single message, which is what [`rgbdx_sync`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_sync) produces — the route that needs no rebuild and the only one that goes past six.
| Topic | Type | Description |
|---|---|---|
| `imu` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Optional. Constrains roll and pitch; see [the README](../README.md#imu). |
## Published Topics
`odom`, `odom_info`, `odom_local_map`, `odom_last_frame`, `odom_rgbd_image` and the rest are common to all three nodes and documented in [the README](../README.md#published-topics).
## Parameters
Specific to this node. The shared ones — `frame_id`, `publish_tf`, `guess_frame_id`, `max_update_rate`, all of RTAB-Map's own — are in [the README](../README.md#conventions).
| Parameter | Type | Default | Description |
|---|---|---|---|
| `subscribe_rgbd` | `bool` | `false` | Take a pre-synchronized `RGBDImage` instead of three raw topics. |
| `rgbd_cameras` | `int` | `1` | Number of `RGBDImage` topics. `0` means one `RGBDImages` topic carrying any number. Only with `subscribe_rgbd:=true`. |
| `approx_sync` | `bool` | `true` | Match the raw topics by nearest stamp. See [Synchronization](#synchronization). |
| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. |
| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. |
| `sync_queue_size` | `int` | `5` | Queue depth of the synchronizer. |
| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. Copied to it with a warning. |
| `qos_camera_info` | `int` | value of `qos` | Reliability of the `rgb/camera_info` subscription alone. |
| `keep_color` | `bool` | `false` | Keep the color image in the data handed to the odometry instead of converting to grayscale. Registration is grayscale either way; this only matters for what downstream consumers of `odom_rgbd_image` receive. |
| `image_transport` | `string` | `"raw"` | Transport for `rgb/image`, e.g. `compressed`. |
| `depth_transport` | `string` | `"raw"` | Transport for `depth/image`, e.g. `compressedDepth`. |
| `rgb_transport` | `string` | — | **Deprecated**, renamed to `image_transport`. |
## Synchronization
With `subscribe_rgbd:=false` this node runs its own synchronizer over the three raw topics, and the same trade-off applies as everywhere else in this stack: **exact matching is cheaper and cannot mismatch, but publishes nothing at all if the stamps differ by a nanosecond**. The default here is approximate because many RGB-D cameras do not stamp color and depth identically.
When several nodes consume the same camera — odometry and `rtabmap`, usually — **synchronize once with `rgbd_sync` and set `subscribe_rgbd:=true` on both**. Two independent approximate synchronizers over the same three topics can settle on different pairings, and then `rtabmap` maps a frame at a pose computed from a different one. Feeding both from one `RGBDImage` removes the possibility.
`approx_sync_max_interval` is worth setting whenever approximate matching stays on: without it, a camera that stalls and resumes silently pairs a fresh color frame with a stale depth frame. A tenth of the frame period is a reasonable start.
## Several cameras
More cameras means more of the scene is textured enough to track, which is the usual reason visual odometry fails indoors. Point them in different directions rather than overlapping.
Two routes, both requiring the frames to be synchronized and each camera to be in TF:
- **`rgbd_cameras:=2..6`** subscribes to `rgbd_image0`…`rgbd_imageN` and synchronizes them here.
- **`rgbd_cameras:=0`** takes one `rgbd_images` topic from `rgbdx_sync`, which has no upper limit and needs no rebuild.
**RTAB-Map has to be built with OpenGV for this.** The default motion estimation is PnP (`Vis/EstimationType=1`, 3D→2D), and the multi-camera version of it lives in OpenGV. Without that dependency the registration refuses to run and says so:
```
Multi-camera 2D-3D PnP registration is only available if rtabmap is built with
OpenGV dependency. Use 3D-3D registration approach instead for multi-camera.
```
Check with `rtabmap --version`, which prints a `With OpenGV:` line. If it says `false`, either rebuild RTAB-Map against OpenGV or switch to `Vis/EstimationType:='0'` (3D→3D), which needs no extra dependency but registers point cloud to point cloud rather than reprojecting, and is the weaker estimator when depth is noisy.
**Hardware-synchronize the cameras if you can.** The node treats the set as one rigid observation at one timestamp: features from every camera are registered together, with the extrinsics from TF held fixed. There is no equivalent of lidar deskewing here — it cannot estimate the motion that happened *within* the rig between one camera's exposure and the next. If the cameras fire at different instants while the robot moves, that motion is absorbed as though the rig had flexed, and the registration is pulled off by however far the robot travelled in between.
Synchronizing the topics is not the same thing: `approx_sync` only decides which frames are grouped, it cannot undo an exposure that happened 20 ms later than its neighbour's.
Calibration matters more with several cameras than with one for the same reason: the extrinsics between them come from TF, and an error there shows up as a constant bias in the estimated motion rather than as an obvious failure.
## Features computed elsewhere
`RGBDImage` has fields for local features — `key_points`, `points` and `descriptors` — and when a frame arrives with them filled, this node hands them to the odometry as they are instead of detecting and describing anything. RTAB-Map extracts features only from a frame that brought none, so nothing is recomputed.
This is for a camera, or a driver, that already does the extraction: on a multi-camera rig it is most of the per-frame work, and it can be done once and shared with `rtabmap` rather than repeated in each node.
What a publisher has to get right:
- **One entry per camera**, in the same order as the images, for `rgbd_cameras:=0` as well as the numbered topics.
- **Keypoints in their own camera's image coordinates.** The node stitches the images side by side and shifts each camera's keypoints by the images that precede it.
- **3D points in that camera's optical frame.** They are brought into `frame_id` with the camera's transform from TF, the same one used for the calibration.
- **Descriptors compressed** with `rtabmap::compressData()`, one row per keypoint, the same type for every camera.
- **Equal counts.** `points` and `descriptors` may be left empty, but if they are there, they must have as many entries as there are keypoints. A frame whose three disagree has its features dropped with an error rather than used out of step.
Both images may be left out entirely: the depth image's job was to give the keypoints their depth and they arrive with it, and the color image's was to have features found in it. A frame is then its calibration and its features, which is the whole point — the images are nearly all of the bandwidth. The `camera_info` of each camera has to be there either way, as it is what says how big the image would have been and, through its `frame_id`, where the camera is.
What stops applying, since nothing is extracted: `Vis/MaxFeatures`, `Vis/DepthAsMask`, the detector chosen with `Kp/DetectorStrategy`, and the depth bounds `Vis/MinDepth` and `Vis/MaxDepth`. Whatever is published is what gets registered, so the publisher owns those decisions. `Vis/CorType` must stay at `0` (feature matching); optical flow (`1`) reads the images themselves and has nothing to work with here.
## Repetitive patterns
Not every failure announces itself. A scene full of identical detail — a tiled floor, rows of identical shelving, a patterned carpet, a brick wall — hands the matcher plenty of features and plenty of confident matches, just not always the *right* ones. One tile matched to its neighbour looks like a perfectly good inlier, and the pose comes out shifted by exactly one tile. Inlier counts stay healthy, nothing is reported lost, and the trajectory drifts in steps.
The defence is to constrain **where** a match is allowed to come from:
- **`guess_frame_id`** gives each feature a predicted image position, from wheel odometry or another external source.
- **`Vis/CorGuessWinSize`** bounds the search around that prediction — 40 pixels by default. Reducing it, to 10 or 20, means a feature can only match something close to where the guess says it should be, so the identical neighbour one tile away is never a candidate.
## When it loses track
Visual odometry fails when there is nothing to match: a blank wall, a dark room, motion blur, or a scene where everything moved. The node then publishes a null pose (see [the README](../README.md#lost-frames-resets-and-new-maps)) and `odom_info` says why.
Look at `odom_info` first — `inliers` is the number that matters:
```bash
ros2 topic echo /odom_info --field inliers
```
Inliers falling below `Vis/MinInliers` (default 20) is the definition of a lost frame. Whether the fix is more features, a better guess or a different strategy depends on which part is short:
- **Few features detected at all** — the scene is untextured or too dark. Lowering `Vis/MinInliers` lets frames register on fewer matches, but a pose resting on a handful of inliers is poorly constrained and drifts badly — it buys continuity at the price of accuracy. The real fixes are physical. If it is dark, add a light — a spotlight on the robot restores texture the camera can track, and costs far less than changing sensor. Otherwise it is **more field of view**: a wider lens, or [several cameras](#several-cameras) pointed in different directions. It only takes one textured patch somewhere in view to track, so a blank wall filling a narrow FOV stops being a problem the moment the rig can also see the ceiling or a doorway. Failing that, the camera is the wrong sensor here — a lidar if the scene has geometry, wheel odometry if it has neither. See [Choosing a sensor modality for the environment](../README.md#choosing-a-sensor-modality-for-the-environment).
- **Features detected but few matched** — motion is too fast for the search window, or the frame rate is too low. A `guess_frame_id` from wheel odometry is what helps most here.
- **Matched but rejected as outliers** — usually a moving scene, or depth that does not agree with the color image. Check that depth really is registered to color.
`Odom/ResetCountdown` gets the node out of a lost state automatically instead of leaving it lost until something calls `reset_odom`.
**A camera alone is not a great odometry source on a wheeled robot.** If the base publishes wheel odometry, feeding it in through `guess_frame_id` is worth more than any amount of tuning here.
rtabmap_odom/doc/stereo_odometry.md
+219
View File
@@ -0,0 +1,219 @@
# stereo_odometry
Visual odometry from a stereo pair.
Features are found in the left image and matched into the right one to get their depth by disparity, then matched against the previous frame to get the motion. It is the same registration as [rgbd_odometry](rgbd_odometry.md); only the source of depth differs — computed here from the pair rather than measured by the sensor.
That difference is the reason to choose it. A stereo pair works outdoors and at range, where the projected-pattern depth of an RGB-D camera returns nothing, and its accuracy degrades gracefully with distance instead of cutting off. The cost is that depth is only available where there is texture to match, and that it depends on a good stereo calibration.
The shared parameters — frames, TF, guesses, the IMU, RTAB-Map's own parameters, the services — are in the [package README](../README.md#conventions). This page covers what is specific to this node.
## Contents
- [Pipeline arrangements](#pipeline-arrangements)
- [Usage](#usage)
- [Subscribed Topics](#subscribed-topics)
- [Published Topics](#published-topics)
- [Parameters](#parameters)
- [Synchronization](#synchronization)
- [Getting the scale right](#getting-the-scale-right)
- [Features computed elsewhere](#features-computed-elsewhere)
- [Repetitive patterns](#repetitive-patterns)
- [When it loses track](#when-it-loses-track)
## Pipeline arrangements
A typical stereo pipeline, rectification included:
```mermaid
flowchart LR
CAM["stereo driver"]
PROC["stereo_image_proc"]
SYNC["stereo_sync"]
ODOM["stereo_odometry"]
MAP["rtabmap"]
CAM -->|left/image_raw<br>right/image_raw<br>camera_info x2| PROC
PROC -->|left/image_rect<br>right/image_rect<br>camera_info x2| SYNC
SYNC -->|rgbd_image| ODOM & MAP
ODOM -->|odom + TF| MAP
```
`stereo_image_proc` can be dropped when the driver already publishes rectified images:
```mermaid
flowchart LR
CAM["stereo driver<br>publishing rectified images"]
SYNC["stereo_sync"]
ODOM["stereo_odometry"]
MAP["rtabmap"]
CAM -->|left/image_rect<br>right/image_rect<br>camera_info x2| SYNC
SYNC -->|rgbd_image| ODOM & MAP
ODOM -->|odom + TF| MAP
```
The same shortcut as on the RGB-D side is available here: drop `stereo_sync` and feed `rtabmap` from **this node's own output**.
```mermaid
flowchart LR
CAM["stereo driver<br>publishing rectified images"]
ODOM["stereo_odometry"]
MAP["rtabmap<br>subscribe_rgbd or subscribe_sensor_data"]
CAM -->|left/image_rect<br>right/image_rect<br>camera_info x2| ODOM
ODOM -->|odom_rgbd_image<br>or odom_sensor_data| MAP
ODOM -->|odom + TF| MAP
```
Remap `rtabmap`'s `rgbd_image` to `odom_rgbd_image`, or set `subscribe_sensor_data` and remap to `odom_sensor_data/raw`. The stereo pair survives the trip intact -- the left image, the right image and both calibrations travel in the one message, exactly as `stereo_sync` would have packed them -- and the features this node extracted come with it, so `rtabmap` does not redo feature detection and descriptor extraction.
Everything above hands the node rectified images. It can also take the raw pair, straight from the driver:
```mermaid
flowchart LR
CAM["stereo driver"]
ODOM["stereo_odometry<br>Rtabmap/ImagesAlreadyRectified:=false"]
MAP["rtabmap<br>subscribe_rgbd or subscribe_sensor_data"]
CAM -->|left/image_raw<br>right/image_raw<br>camera_info x2| ODOM
ODOM -->|odom_rgbd_image<br>or odom_sensor_data| MAP
ODOM -->|odom + TF| MAP
```
Two different things can make that work:
- **The odometry rectifies the pair itself.** With `Rtabmap/ImagesAlreadyRectified:=false` it builds a rectification map from the calibration and applies it to every frame, saying so once:
```
Rtabmap/ImagesAlreadyRectified parameter is set to false but the selected odometry
approach cannot process raw stereo images. We will rectify them for convenience.
```
It needs the geometry between the two cameras to do that — the right `camera_info` carrying `P(0,3)`, or TF between the two camera frames. If a rectification map cannot be built from what the calibration says, the frame is refused rather than registered wrong.
- **The odometry takes them raw.** A few approaches do their own undistortion and want the unrectified images: `Odom/Strategy` `6` (OKVIS), `8` (MSCKF), `9` (VINS-Fusion) and `10` (OpenVINS), each available only if RTAB-Map was built against that library. Nothing rectifies anything then, and `Rtabmap/ImagesAlreadyRectified:=false` simply tells the pipeline to leave the images alone.
Which of the two applies decides what `rtabmap` needs when it is fed from this node's output. If the odometry rectified the pair, the rectified images are what travels on -- they replace the raw ones in the frame -- and `rtabmap` keeps `Rtabmap/ImagesAlreadyRectified` at its default `true`. If the odometry took them raw, they arrive raw, and `rtabmap` needs `Rtabmap/ImagesAlreadyRectified:=false` of its own to rectify them again on its side. Set `Mem/UseOdomFeatures:=false` along with it, since it defaults to `true`: `rtabmap` rectifies a stereo pair but does not currently map features that travelled with the frame into the rectified image, so any it reused would be read against the wrong one.
Rectifying here costs what `stereo_image_proc` would have cost, but only on the frames the odometry actually registers. When it runs slower than the camera — throttled by `max_update_rate`, or dropping frames that arrive while a registration is still running — the rectification happens at the odometry's rate instead of the camera's, and every frame `stereo_image_proc` would have rectified for nothing is saved. Where something else needs the whole stream rectified, the first arrangement is still the one to use.
## Usage
```bash
ros2 run rtabmap_odom stereo_odometry --ros-args \
-r left/image_rect:=/stereo/left/image_rect \
-r right/image_rect:=/stereo/right/image_rect \
-r left/camera_info:=/stereo/left/camera_info \
-r right/camera_info:=/stereo/right/camera_info \
-p frame_id:=base_link
```
```python
ComposableNode(
package='rtabmap_odom',
plugin='rtabmap_odom::StereoOdometry',
name='stereo_odometry',
parameters=[{'frame_id': 'base_link'}],
remappings=[('left/image_rect', '/stereo/left/image_rect'),
('right/image_rect', '/stereo/right/image_rect'),
('left/camera_info', '/stereo/left/camera_info'),
('right/camera_info', '/stereo/right/camera_info')])
```
**The images are normally rectified**, which is what the `image_rect` topic names assume, and what [`stereo_image_proc`](https://docs.ros.org/en/jazzy/p/stereo_image_proc/) produces when the driver does not.
They do not have to be. RTAB-Map can rectify them itself from the calibration -- set `Rtabmap/ImagesAlreadyRectified:=false` and feed it the raw pair with distortion coefficients in the `camera_info`. What does not work is the silent middle case: **unrectified images with that parameter left at its default of `true`**. Nothing fails loudly; disparity is computed across rows that no longer correspond, giving depths that are wrong in a smoothly varying way and a trajectory that is wrong without looking broken.
## Subscribed Topics
**Default** — `subscribe_rgbd:=false`:
| Topic | Type | Description |
|---|---|---|
| `left/image_rect` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified left image, color or mono. |
| `right/image_rect` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified right image, color or mono. |
| `left/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Left calibration. |
| `right/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Right calibration. Its `P` matrix carries the baseline, which sets the scale of the whole trajectory. |
**With `subscribe_rgbd:=true`**, one pre-synchronized message from [`stereo_sync`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_sync):
| `rgbd_cameras` | Topic | Type |
|---|---|---|
| `1` (default) | `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) |
| `2`–`6` | `rgbd_image0` … `rgbd_image5` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) |
| `0` | `rgbd_images` | [`rtabmap_msgs/msg/RGBDImages`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImages.html) |
| Topic | Type | Description |
|---|---|---|
| `imu` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Optional. See [the README](../README.md#imu). |
## Published Topics
Common to all three nodes; see [the README](../README.md#published-topics).
## Parameters
| Parameter | Type | Default | Description |
|---|---|---|---|
| `subscribe_rgbd` | `bool` | `false` | Take a pre-synchronized `RGBDImage` from `stereo_sync` instead of four raw topics. |
| `rgbd_cameras` | `int` | `1` | Number of `RGBDImage` topics. `0` means one `RGBDImages` topic. Only with `subscribe_rgbd:=true`. More than one needs RTAB-Map built with OpenGV, and the cameras hardware-synchronized — see [Several cameras](rgbd_odometry.md#several-cameras). |
| `approx_sync` | `bool` | `false` | Match the raw topics by nearest stamp. **Defaults to exact**, unlike `rgbd_odometry` — see [Synchronization](#synchronization). |
| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. Only used when `approx_sync` is on. |
| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. |
| `sync_queue_size` | `int` | `5` | Queue depth of the synchronizer. |
| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. |
| `qos_camera_info` | `int` | value of `qos` | Reliability of the two `camera_info` subscriptions. |
| `keep_color` | `bool` | `false` | Keep the left image in color in the data passed on, rather than converting to grayscale. Registration is grayscale either way. |
| `image_transport` | `string` | `"raw"` | Transport for both image topics. |
## Synchronization
**This node defaults to exact matching**, because a stereo pair is normally hardware-triggered and the two images therefore carry identical stamps. That is the right default: exact matching is cheaper and cannot pair the left image with the wrong right one — and a mismatched stereo pair does not produce an error, it produces wrong disparities and a wrong trajectory.
The failure mode to recognize is the other one: if the stamps are *not* identical, **nothing is ever published and nothing says why**. Check before assuming the node is broken:
```bash
ros2 topic echo --once /stereo/left/image_rect --field header.stamp
ros2 topic echo --once /stereo/right/image_rect --field header.stamp
```
If they differ, set `approx_sync:=true` and set `approx_sync_max_interval` to something tight — a stereo pair whose images are more than a fraction of a frame apart is not usable for disparity regardless.
## Getting the scale right
Everything about a stereo trajectory's scale comes from the **baseline**, which this node reads from the right camera's `P` matrix (`P[3] = -fx * baseline`). Two consequences:
- A `right/camera_info` whose `P` matrix is all zeros — which some drivers publish before calibration is loaded — gives a zero baseline and no usable depth at all.
- A calibration whose baseline is off by a few percent produces a trajectory off by the same few percent, consistently, with nothing else looking wrong.
If the map comes out uniformly too large or too small, check the baseline before anything else.
## Features computed elsewhere
`RGBDImage` has fields for local features — `key_points`, `points` and `descriptors` — and when a frame arrives with them filled, this node hands them to the odometry as they are instead of detecting and describing anything. RTAB-Map extracts features only from a frame that brought none, so nothing is recomputed, and neither is the disparity search that would otherwise give each feature its depth.
This is for a camera, or a driver, that already does the extraction: on a multi-camera rig it is most of the per-frame work, and it can be done once and shared with `rtabmap` rather than repeated in each node.
What a publisher has to get right:
- **One entry per camera**, in the same order as the images, for `rgbd_cameras:=0` as well as the numbered topics.
- **Keypoints in their own camera's left image.** The node stitches the left images side by side and shifts each camera's keypoints by the images that precede it. A stereo pair's features belong to the left image; nothing is expected in the right one.
- **3D points in that camera's left optical frame.** They are brought into `frame_id` with the camera's transform from TF, the same one used for the calibration.
- **Descriptors compressed** with `rtabmap::compressData()`, one row per keypoint, the same type for every camera.
- **Equal counts.** `points` and `descriptors` may be left empty, but if they are there, they must have as many entries as there are keypoints. A frame whose three disagree has its features dropped with an error rather than used out of step.
Both images may be left out entirely — the left image's job was to have features found in it, the right one's to give them their disparity, and they arrive with their 3D positions already. A frame is then its two calibrations and its features, which is the whole point: the images are nearly all of the bandwidth. Both `camera_info` still have to be there, the left one saying how big the image would have been and where the camera is, the right one carrying the baseline in `P(0,3)` — without it there is no scale, features or not.
What stops applying, since nothing is extracted: `Vis/MaxFeatures`, the detector chosen with `Kp/DetectorStrategy`, and the depth bounds `Vis/MinDepth` and `Vis/MaxDepth`. Whatever is published is what gets registered, so the publisher owns those decisions. `Vis/CorType` must stay at `0` (feature matching); optical flow (`1`) reads the images themselves and has nothing to work with here.
## Repetitive patterns
Identical detail repeated across the scene — a tiled floor, rows of shelving, a brick wall — lets the matcher pair a feature with the wrong copy of itself, which drifts the trajectory by exactly one repeat while the inlier count stays healthy and nothing is reported lost. It bites stereo twice over, since the same ambiguity also misplaces the left/right match that sets the depth.
The fix is the same as for [rgbd_odometry](rgbd_odometry.md#repetitive-patterns): an external guess through `guess_frame_id`, with `Vis/CorGuessWinSize` reduced so a match has to come from close to where the guess predicts. `Stereo/WinWidth` and `Stereo/WinHeight` matter here too — a correlation window smaller than the repeating pattern has nothing unique to lock onto.
## When it loses track
The same diagnosis as [rgbd_odometry](rgbd_odometry.md#when-it-loses-track) — `odom_info`'s `inliers` is the number to watch — with two failure modes specific to stereo:
- **Poor rectification.** Matched features should lie on the same image row. If they do not, either the pair is unrectified while `Rtabmap/ImagesAlreadyRectified` is `true`, or the calibration itself is off.
- **Untextured scene.** With no texture there is nothing to match *between* left and right either, so there is no depth at all — worse than the RGB-D case, where the sensor still measures wrong depth on a blank wall.
`Stereo/*` parameters tune the disparity matching itself: `Stereo/MaxDisparity` bounds how close a point can be, `Stereo/WinWidth` and `Stereo/WinHeight` the correlation window. They are listed by `ros2 param list` like every other RTAB-Map parameter.
rtabmap_odom/include/rtabmap_odom/OdometryROS.h
+87
View File
@@ -58,57 +58,144 @@ namespace rtabmap {
class Odometry;
}
/**
* @file
* @brief The node the three odometry nodes of this package are built on.
*/
namespace rtabmap_odom {
/**
* @brief Runs RTAB-Map's odometry as a ROS node: everything but the subscriptions.
*
* `rgbd_odometry`, `stereo_odometry` and `icp_odometry` differ only in what they listen
* to. Each turns its own topics into a rtabmap::SensorData and hands it to processData();
* from there on this class does the work -- registration, pose integration, the `odom`
* topic and its TF, the IMU intake, the services, the diagnostics, the reset policy when
* tracking is lost. That is why the three nodes share nearly all of their parameters and
* publish the same topics.
*
* A subclass is expected to:
* - call init() from its constructor, saying which families of RTAB-Map parameters it
* accepts, which decides both the defaults and what the node will accept being set;
* - create its subscriptions in onOdomInit() and describe them with initDiagnosticMsg();
* - call tick() when a message arrives and processData() once a frame is complete;
* - implement flushCallbacks(), so that a reset can drop whatever its synchronizer holds.
*
* The class is also a UThread. By default the frame handed to processData() is passed to
* that thread and the callback returns at once, so a slow registration cannot block the
* executor; a frame arriving while the thread is busy is dropped rather than queued. With
* `always_process_most_recent_frame:=false` it is registered on the calling thread
* instead, which keeps every frame at the cost of holding up the executor.
*
* @see the package README for the parameters and topics these nodes have in common.
*/
class OdometryROS : public rclcpp::Node, public UThread
{
public:
/// Constructs the node under its default name.
explicit OdometryROS(const rclcpp::NodeOptions & options);
/// Constructs the node under @p name, which is what the three nodes use.
explicit OdometryROS(const std::string & name, const rclcpp::NodeOptions & options);
virtual ~OdometryROS();
/**
* @brief Hands a complete frame to the odometry; called by a subclass's callback.
* @param[in,out] data the frame to register, which comes back carrying the
* features the odometry ended up using
* @param[in] header stamp and frame of the data, used to publish the result
*
* The frame is either queued for the worker thread or registered right here,
* depending on `always_process_most_recent_frame`. Either way, a frame that arrives
* while the previous one is still being registered is dropped: the odometry stays on
* the newest data rather than falling behind.
*/
void processData(rtabmap::SensorData & data, const std_msgs::msg::Header & header);
/// `reset_odom` service: starts a new map at the origin, or at the guess frame's pose.
void resetOdom(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
/// `reset_odom_to_pose` service: starts a new map at the pose given in the request.
void resetToPose(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::ResetPose::Request>, std::shared_ptr<rtabmap_msgs::srv::ResetPose::Response>);
/// `pause_odom` service: keeps the subscriptions but stops registering what arrives.
void pause(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
/// `resume_odom` service: registers again, starting from the next frame.
void resume(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
/// `log_debug` service: raises RTAB-Map's own log level to debug at runtime.
void setLogDebug(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
/// `log_info` service; see setLogDebug().
void setLogInfo(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
/// `log_warning` service; see setLogDebug().
void setLogWarn(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
/// `log_error` service; see setLogDebug().
void setLogError(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
/// The robot frame the odometry is computed for, `frame_id`.
const std::string & frameId() const {return frameId_;}
/// The frame the estimated poses are expressed in, `odom_frame_id`.
const std::string & odomFrameId() const {return odomFrameId_;}
/// The frame an external motion guess is read from, `guess_frame_id`; empty if unused.
const std::string & guessFrameId() const {return guessFrameId_;}
/// The RTAB-Map parameters this node was configured with, defaults included.
const rtabmap::ParametersMap & parameters() const {return parameters_;}
/// Whether the `pause_odom` service has been called and not resumed since.
bool isPaused() const {return paused_;}
protected:
/**
* @brief Declares the node's parameters and creates the odometry; call it last in the
* subclass constructor.
* @param[in] stereoParams true if the node takes RTAB-Map's stereo parameters
* @param[in] visParams true if it takes the visual registration ones
* @param[in] icpParams true if it takes the scan matching ones
*
* The three flags decide which RTAB-Map parameters the node declares, and so which
* ones it accepts being set: `icp_odometry` refuses a `Vis/` parameter and the other
* two refuse an `Icp/` one. onOdomInit() is called at the end, for the subclass to
* create its subscriptions.
*/
void init(bool stereoParams, bool visParams, bool icpParams);
/// The reliability the subclass should give its own subscriptions, from `qos`.
rmw_qos_reliability_policy_t qos() const {return qos_;}
/**
* @brief Starts the diagnostics, once the subclass knows what it subscribed to.
* @param[in] subscribedTopicsMsg the human readable list logged at startup and
* repeated in the "no data received" warning
* @param[in] approxSync whether the subclass matches stamps approximately,
* which that warning mentions as a likely cause
* @param[in] subscribedTopic the one topic whose rate is watched, if any
*/
void initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic = "");
/// Drops whatever the subclass's synchronizer holds; called when the odometry resets.
virtual void flushCallbacks() {};
/// The node's TF buffer, for the subclass to look up its sensors' frames.
tf2_ros::Buffer & tfBuffer() {return *tfBuffer_;}
/// How long a TF lookup may block, from `wait_for_transform`.
const double & waitForTransform() const {return waitForTransform_;}
/// The velocity of the last registered frame, null when there is no estimate yet.
rtabmap::Transform velocityGuess() const;
/// Stamp of the last registered frame, 0 before the first one.
double previousStamp() const {return previousStamp_;}
/// Called after a frame has been registered and published, for a subclass to add to it.
virtual void postProcessData(const rtabmap::SensorData & /*data*/, const std_msgs::msg::Header & /*header*/) const {}
private:
void processData();
virtual void mainLoop();
virtual void mainLoopKill();
/// Lets a subclass adjust the RTAB-Map parameters before the odometry is created.
virtual void updateParameters(rtabmap::ParametersMap &) {}
/// Called at the end of init(), where a subclass creates its subscriptions.
virtual void onOdomInit() {}
void callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg);
void reset(const rtabmap::Transform & pose = rtabmap::Transform::getIdentity());
protected:
/// The callback group the subclass's sensor subscriptions belong to.
rclcpp::CallbackGroup::SharedPtr dataCallbackGroup_;
/// Reports the arrival of an input message to the diagnostics, before anything else.
void tick(const rclcpp::Time & stamp);
private:
rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp
+11
View File
@@ -44,6 +44,17 @@ using namespace rtabmap;
namespace rtabmap_odom
{
/**
* @brief Odometry from a laser scanner, 2D or 3D, by scan matching.
*
* Takes a sensor_msgs::msg::LaserScan or a sensor_msgs::msg::PointCloud2 and registers
* each scan against the previous ones with ICP. A cloud whose points carry their own
* timestamps is deskewed first, using TF or the last known velocity, since a scan taken
* while the robot moves is not one rigid observation.
*
* @see doc/icp_odometry.md for the topics, the parameters and the shapes it can and
* cannot constrain.
*/
class ICPOdometry : public rtabmap_odom::OdometryROS
{
public:
rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp
+24 -1
View File
@@ -38,6 +38,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <image_transport/subscriber_filter.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <rtabmap_msgs/msg/key_point.hpp>
#include <rtabmap_msgs/msg/point3f.hpp>
#include <rtabmap_msgs/msg/rgbd_image.hpp>
#include <rtabmap_msgs/msg/rgbd_images.hpp>
@@ -50,6 +52,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_odom
{
/**
* @brief Odometry from an RGB-D camera, or from several on one rig.
*
* Takes either the three raw topics of a camera (`rgb/image`, `depth/image`,
* `rgb/camera_info`) or pre-synchronized rtabmap_msgs::msg::RGBDImage messages, one per
* camera, and registers each frame's visual features against a local feature map. A
* frame that arrives with its own keypoints, 3D points and descriptors is registered
* with those rather than having them extracted again.
*
* @see doc/rgbd_odometry.md for the topics, the parameters and what to do when it loses
* tracking.
*/
class RGBDOdometry : public rtabmap_odom::OdometryROS
{
public:
@@ -61,10 +75,19 @@ private:
virtual void updateParameters(rtabmap::ParametersMap & parameters);
virtual void onOdomInit();
/**
* Local features, when the input topic carries them, are indexed per camera like
* the images are: one entry per camera, in the same order. They are optional, and
* a frame that comes without them is processed exactly as before, the features
* being extracted from the images downstream.
*/
void commonCallback(
const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos);
const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos,
const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs = {},
const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs = {},
const std::vector<cv::Mat> & localDescriptorsMsgs = {});
void callback(
const sensor_msgs::msg::Image::ConstSharedPtr image,
rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp
+23 -1
View File
@@ -43,12 +43,25 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <sensor_msgs/msg/image.hpp>
#include <rtabmap_msgs/msg/key_point.hpp>
#include <rtabmap_msgs/msg/point3f.hpp>
#include <rtabmap_msgs/msg/rgbd_image.hpp>
#include <rtabmap_msgs/msg/rgbd_images.hpp>
namespace rtabmap_odom
{
/**
* @brief Odometry from a stereo pair, or from several on one rig.
*
* Takes either the four raw topics of a stereo camera (left and right image plus their
* calibrations) or pre-synchronized rtabmap_msgs::msg::RGBDImage messages carrying the
* pair, one per camera. The right camera's `P(0,3)` is what gives the trajectory its
* scale. A frame that arrives with its own keypoints, 3D points and descriptors is
* registered with those rather than having them extracted again.
*
* @see doc/stereo_odometry.md for the topics, the parameters and the scale it depends on.
*/
class StereoOdometry : public rtabmap_odom::OdometryROS
{
public:
@@ -60,11 +73,20 @@ private:
virtual void updateParameters(rtabmap::ParametersMap & parameters);
virtual void onOdomInit();
/**
* Local features, when the input topic carries them, are indexed per camera like
* the images are: one entry per camera, in the same order, and placed in that
* camera's left image. They are optional, and a frame that comes without them is
* processed exactly as before, the features being extracted downstream.
*/
void commonCallback(
const std::vector<cv_bridge::CvImageConstPtr> & leftImages,
const std::vector<cv_bridge::CvImageConstPtr> & rightImages,
const std::vector<sensor_msgs::msg::CameraInfo>& leftCameraInfos,
const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos);
const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos,
const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs = {},
const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs = {},
const std::vector<cv::Mat> & localDescriptorsMsgs = {});
void callback(
const sensor_msgs::msg::Image::ConstSharedPtr imageRectLeft,
rtabmap_odom/package.xml
+6
View File
@@ -30,8 +30,14 @@
<depend>rtabmap_util</depend>
<depend>rtabmap_sync</depend>
<test_depend>ament_cmake_gtest</test_depend>
<!-- The odometry tests replay recorded sensor data from test/data/lidar. -->
<test_depend>rosbag2_cpp</test_depend>
<test_depend>rosbag2_storage_mcap</test_depend>
<export>
<build_type>ament_cmake</build_type>
<rosdoc2>rosdoc2.yaml</rosdoc2>
</export>
</package>
rtabmap_odom/rosdoc2.yaml
+35
View File
@@ -0,0 +1,35 @@
## Configuration for rosdoc2, the documentation generator used by docs.ros.org.
## Regenerate the annotated default with:
## rosdoc2 default_config --package-path rtabmap_odom
## Build the docs locally with:
## rosdoc2 build --package-path rtabmap_odom --output-directory doc_output
## This 'attic section' self-documents this file's type and version.
type: 'rosdoc2 config'
version: 1
---
settings:
## Generate the standard index page from package.xml (description, maintainer,
## license, links) and a table of contents for the builders below.
generate_package_index: true
## This is an ament_cmake package, so doxygen runs on the public headers by
## default and there are no Python modules to document.
always_run_doxygen: false
always_run_sphinx_apidoc: false
builders:
## Doxygen parses the public C++ API out of include/.
- doxygen: {
name: 'rtabmap_odom Public C/C++ API',
output_dir: 'generated/doxygen'
}
## Sphinx renders the landing page and pulls the Doxygen XML in through
## breathe/exhale so the API is browsable alongside the narrative docs.
- sphinx: {
name: 'rtabmap_odom',
doxygen_xml_directory: 'generated/doxygen/xml',
output_dir: ''
}
rtabmap_odom/src/OdometryROS.cpp
+212 -66
View File
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap_conversions/PointCloudConversion.h>
#include "rtabmap_odom/OdometryROS.h"
#include <sensor_msgs/msg/image.hpp>
@@ -61,6 +62,44 @@ using namespace rtabmap;
namespace rtabmap_odom {
namespace {
/**
* @brief The covariance of a pose that came from the guess frame instead of registration.
*
* Used wherever the guess is what the published pose rests on: a frame the odometry did
* not update because it had not moved enough, and the frame that restarts the map after
* a reset. Nothing was measured in either case, so the confidence is the one the guess
* was declared to have rather than anything the registration computed.
*/
cv::Mat guessCovariance(double linearVariance, double angularVariance)
{
cv::Mat covariance = cv::Mat::zeros(6,6,CV_64FC1);
covariance.at<double>(0,0) = linearVariance; // xx
covariance.at<double>(1,1) = linearVariance; // yy
covariance.at<double>(2,2) = linearVariance; // zz
covariance.at<double>(3,3) = angularVariance; // rr
covariance.at<double>(4,4) = angularVariance; // pp
covariance.at<double>(5,5) = angularVariance; // yawyaw
return covariance;
}
/**
* @brief The velocity a motion implies, for a frame with no registration to measure one.
*
* Named apart from the guess itself so that it can be called where a `guessVelocity`
* variable is in scope.
*/
rtabmap::Transform velocityFrom(const rtabmap::Transform & motion, double dt)
{
UASSERT(dt > 0.0);
float x,y,z,roll,pitch,yaw;
motion.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
return rtabmap::Transform(x/dt, y/dt, z/dt, roll/dt, pitch/dt, yaw/dt);
}
} // namespace
OdometryROS::OdometryROS(const rclcpp::NodeOptions & options) :
OdometryROS("odometry", options)
{}
@@ -83,6 +122,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
publishNullWhenLost_(true),
publishCompressedSensorData_(false),
qos_(RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT),
bufferedDataToProcess_(false),
paused_(false),
resetCountdown_(0),
resetCurrentCount_(0),
@@ -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<double>(0,0) = guessLinearVariance_; // xx
info.reg.covariance.at<double>(1,1) = guessLinearVariance_; // yy
info.reg.covariance.at<double>(2,2) = guessLinearVariance_; // zz
info.reg.covariance.at<double>(3,3) = guessAngularVariance_; // rr
info.reg.covariance.at<double>(4,4) = guessAngularVariance_; // pp
info.reg.covariance.at<double>(5,5) = guessAngularVariance_; // yawyaw
//set velocity
double dt = rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_;
UASSERT(dt>0.0);
// use part of guess matching dt
(previousPose.inverse() * guessCurrentPose).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
guessVelocity = rtabmap::Transform(x/dt, y/dt, z/dt, roll/dt, pitch/dt, yaw/dt);
skipOdometryUpdate = true;
}
}
@@ -817,16 +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<double>(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<pcl::PointXYZINormal>::Ptr cloud = util3d::laserScanToPointCloudINormal(info.localScanMap, info.localScanMap.localTransform());
pcl::toROSMsg(*cloud, cloudMsg);
rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg);
}
else if(info.localScanMap.hasNormals())
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(info.localScanMap, info.localScanMap.localTransform());
pcl::toROSMsg(*cloud, cloudMsg);
rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg);
}
else if(info.localScanMap.hasIntensity())
{
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = util3d::laserScanToPointCloudI(info.localScanMap, info.localScanMap.localTransform());
pcl::toROSMsg(*cloud, cloudMsg);
rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(info.localScanMap, info.localScanMap.localTransform());
pcl::toROSMsg(*cloud, cloudMsg);
rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg);
}
cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
@@ -1106,6 +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;
rtabmap_odom/src/nodelets/icp_odometry.cpp
+9 -7
View File
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap_conversions/PointCloudConversion.h>
#include <rtabmap_odom/icp_odometry.hpp>
#include <laser_geometry/laser_geometry.hpp>
@@ -69,6 +70,7 @@ ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) :
ICPOdometry::~ICPOdometry()
{
this->join(true);
}
void ICPOdometry::onOdomInit()
@@ -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<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
pcl::fromROSMsg(*cloudMsg, *pclScan);
rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan);
if(pclScan->size() && scanDownsamplingStep_ > 1)
{
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -686,7 +688,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
else if(hasNormals)
{
pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*cloudMsg, *pclScan);
rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan);
if(pclScan->size() && scanDownsamplingStep_ > 1)
{
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -704,7 +706,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
else if(hasIntensity)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(*cloudMsg, *pclScan);
rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan);
if(pclScan->size() && scanDownsamplingStep_ > 1)
{
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -750,7 +752,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloudMsg, *pclScan);
rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan);
if(pclScan->size() && scanDownsamplingStep_ > 1)
{
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
rtabmap_odom/src/nodelets/rgbd_odometry.cpp
+214 -67
View File
@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap_conversions/MsgConversion.h"
#include <rtabmap_msgs/msg/rgbd_images.hpp>
#include <rtabmap/core/Compression.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/utilite/ULogger.h>
@@ -73,6 +74,8 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
RGBDOdometry::~RGBDOdometry()
{
this->join(true);
delete approxSync_;
delete exactSync_;
delete approxSync2_;
@@ -435,40 +438,58 @@ void RGBDOdometry::updateParameters(ParametersMap & parameters)
void RGBDOdometry::commonCallback(
const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos)
const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos,
const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs,
const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs,
const std::vector<cv::Mat> & localDescriptorsMsgs)
{
UASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size());
rclcpp::Time higherStamp;
UASSERT_MSG(rgbImages[0], "RGB image is null!");
int imageWidth = rgbImages[0]->image.cols;
int imageHeight = rgbImages[0]->image.rows;
UASSERT_MSG(depthImages[0], "Depth image is null!");
// The images are what the local features would otherwise be extracted from, so a frame
// that brings its own can leave them out -- which is nearly all of the bandwidth. It
// then describes itself with its calibration alone: how big the image would have been,
// where the camera is, what it sees. (An RGB-D message with no depth image at all also
// used to divide by zero below.)
const bool hasRgb = !rgbImages[0]->image.empty();
const bool hasDepth = !depthImages[0]->image.empty();
int imageWidth = hasRgb?rgbImages[0]->image.cols:(int)cameraInfos[0].width;
int imageHeight = hasRgb?rgbImages[0]->image.rows:(int)cameraInfos[0].height;
int depthWidth = depthImages[0]->image.cols;
int depthHeight = depthImages[0]->image.rows;
UASSERT_MSG(
imageWidth/depthWidth == imageHeight/depthHeight,
uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
if(hasDepth)
{
UASSERT_MSG(
imageWidth/depthWidth == imageHeight/depthHeight,
uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
}
int cameraCount = rgbImages.size();
cv::Mat rgb;
cv::Mat depth;
std::vector<rtabmap::CameraModel> cameraModels;
std::vector<cv::KeyPoint> keypoints;
std::vector<cv::Point3f> points3d;
cv::Mat descriptors;
for(unsigned int i=0; i<rgbImages.size(); ++i)
{
UASSERT_MSG(rgbImages[i], uFormat("RGB image is null for camera %d", i).c_str());
UASSERT_MSG(depthImages[i], uFormat("Depth image is null for camera %d", i).c_str());
if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
if((hasRgb && !(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
!(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0)) ||
(hasDepth && !(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)))
{
RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and "
"image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s",
@@ -476,20 +497,33 @@ void RGBDOdometry::commonCallback(
depthImages[i]->encoding.c_str());
return;
}
UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
imageWidth,
rgbImages[i]->image.cols,
imageHeight,
rgbImages[i]->image.rows).c_str());
UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight,
uFormat("depthWidth=%d vs %d depthHeight=%d vs %d",
depthWidth,
depthImages[i]->image.cols,
depthHeight,
depthImages[i]->image.rows).c_str());
if(hasRgb)
{
UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
imageWidth,
rgbImages[i]->image.cols,
imageHeight,
rgbImages[i]->image.rows).c_str());
}
if(hasDepth)
{
UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight,
uFormat("depthWidth=%d vs %d depthHeight=%d vs %d",
depthWidth,
depthImages[i]->image.cols,
depthHeight,
depthImages[i]->image.rows).c_str());
}
rclcpp::Time stamp = rtabmap_conversions::timestampFromROS(rgbImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(depthImages[i]->header.stamp)?rgbImages[i]->header.stamp:depthImages[i]->header.stamp;
// An image that is not there carries no header either, so a frame that has none is
// stamped and placed by its calibration, which is all it has.
const std::string & cameraFrameId = hasRgb?rgbImages[i]->header.frame_id:cameraInfos[i].header.frame_id;
rclcpp::Time stamp = cameraInfos[i].header.stamp;
if(hasRgb || hasDepth)
{
stamp = rtabmap_conversions::timestampFromROS(rgbImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(depthImages[i]->header.stamp)?rgbImages[i]->header.stamp:depthImages[i]->header.stamp;
}
if(i == 0)
{
@@ -500,7 +534,7 @@ void RGBDOdometry::commonCallback(
higherStamp = stamp;
}
Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp, tfBuffer(), waitForTransform());
Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), cameraFrameId, stamp, tfBuffer(), waitForTransform());
if(localTransform.isNull())
{
return;
@@ -528,53 +562,75 @@ void RGBDOdometry::commonCallback(
}
}
cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
if(hasRgb)
{
if(keepColor_ && rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
{
ptrImage = cv_bridge::cvtColor(rgbImages[i], "bgr8");
if(keepColor_ && rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
{
ptrImage = cv_bridge::cvtColor(rgbImages[i], "bgr8");
}
else
{
ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
}
}
// initialize
if(rgb.empty())
{
rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
}
if(ptrImage->image.type() == rgb.type())
{
ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
}
else
{
ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
RCLCPP_ERROR(this->get_logger(), "Some RGB images are not the same type! %d vs %d", ptrImage->image.type(), rgb.type());
return;
}
}
cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
if(hasDepth)
{
cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
if(depth.empty())
{
depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
}
// initialize
if(rgb.empty())
{
rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
}
if(depth.empty())
{
depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
}
if(ptrImage->image.type() == rgb.type())
{
ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
}
else
{
RCLCPP_ERROR(this->get_logger(), "Some RGB images are not the same type! %d vs %d", ptrImage->image.type(), rgb.type());
return;
}
if(ptrDepth->image.type() == depth.type())
{
ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
}
else
{
RCLCPP_ERROR(this->get_logger(), "Some Depth images are not the same type! %d vs %d", ptrDepth->image.type(), depth.type());
return;
if(ptrDepth->image.type() == depth.type())
{
ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
}
else
{
RCLCPP_ERROR(this->get_logger(), "Some Depth images are not the same type! %d vs %d", ptrDepth->image.type(), depth.type());
return;
}
}
cameraModels.push_back(rtabmap_conversions::cameraModelFromROS(cameraInfos[i], localTransform));
// The images of all cameras are stitched side by side above, so the keypoints of
// camera i are shifted by as many images as come before it, and their 3D points,
// which arrive in that camera's optical frame, are brought back to the base frame.
if(localKeyPointsMsgs.size() == rgbImages.size())
{
rtabmap_conversions::keypointsFromROS(localKeyPointsMsgs[i], keypoints, imageWidth*i);
}
if(localPoints3dMsgs.size() == rgbImages.size())
{
rtabmap_conversions::points3fFromROS(localPoints3dMsgs[i], points3d, localTransform);
}
if(localDescriptorsMsgs.size() == rgbImages.size())
{
descriptors.push_back(localDescriptorsMsgs[i]);
}
}
rtabmap::SensorData data(
@@ -584,9 +640,30 @@ void RGBDOdometry::commonCallback(
0,
rtabmap_conversions::timestampFromROS(higherStamp));
// Features that came with the frame are used as they are: the odometry then skips
// detection, description and the depth lookup that would otherwise rebuild them
// (see RegistrationVis, which extracts only when the frame carries no keypoints).
// They are dropped rather than trusted if the three of them disagree, as using them
// out of step would silently mismatch keypoints with their descriptors or 3D points.
if(!keypoints.empty())
{
if((!points3d.empty() && points3d.size() != keypoints.size()) ||
(!descriptors.empty() && descriptors.rows != (int)keypoints.size()))
{
RCLCPP_ERROR(this->get_logger(), "Ignoring the local features received with this frame: "
"%d keypoints, %d 3D points and %d descriptors, which should be the same count "
"(or none at all for the 3D points and the descriptors).",
(int)keypoints.size(), (int)points3d.size(), descriptors.rows);
}
else
{
data.setFeatures(keypoints, points3d, descriptors);
}
}
std_msgs::msg::Header header;
header.stamp = higherStamp;
header.frame_id = rgbImages.size()==1?rgbImages[0]->header.frame_id:"";
header.frame_id = rgbImages.size()==1?(hasRgb?rgbImages[0]->header.frame_id:cameraInfos[0].header.frame_id):"";
this->processData(data, header);
}
@@ -627,6 +704,27 @@ void RGBDOdometry::callback(
}
}
namespace {
/**
* @brief Collects the local features one camera's image carries, cameras in order.
*
* An image that carries none pushes empty entries rather than nothing, so that the
* per-camera indexing still lines up with the images.
*/
void appendLocalFeatures(
const rtabmap_msgs::msg::RGBDImage & image,
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & keyPoints,
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & points3d,
std::vector<cv::Mat> & descriptors)
{
keyPoints.push_back(image.key_points);
points3d.push_back(image.points);
descriptors.push_back(rtabmap::uncompressData(image.descriptors));
}
} // namespace
void RGBDOdometry::callbackRGBDX(
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images)
{
@@ -642,13 +740,17 @@ void RGBDOdometry::callbackRGBDX(
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(images->rgbd_images.size());
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(images->rgbd_images.size());
std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
std::vector<cv::Mat> localDescriptors;
for(size_t i=0; i<images->rgbd_images.size(); ++i)
{
rtabmap_conversions::toCvShare(images->rgbd_images[i], images, imageMsgs[i], depthMsgs[i]);
infoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
appendLocalFeatures(images->rgbd_images[i], localKeyPoints, localPoints3d, localDescriptors);
}
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
}
@@ -665,7 +767,12 @@ void RGBDOdometry::callbackRGBD(
rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
infoMsgs.push_back(image->rgb_camera_info);
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
std::vector<cv::Mat> localDescriptors;
appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
}
@@ -685,7 +792,13 @@ void RGBDOdometry::callbackRGBD2(
infoMsgs.push_back(image->rgb_camera_info);
infoMsgs.push_back(image2->rgb_camera_info);
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
std::vector<cv::Mat> localDescriptors;
appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
}
@@ -708,7 +821,14 @@ void RGBDOdometry::callbackRGBD3(
infoMsgs.push_back(image2->rgb_camera_info);
infoMsgs.push_back(image3->rgb_camera_info);
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
std::vector<cv::Mat> localDescriptors;
appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
}
@@ -734,7 +854,15 @@ void RGBDOdometry::callbackRGBD4(
infoMsgs.push_back(image3->rgb_camera_info);
infoMsgs.push_back(image4->rgb_camera_info);
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
std::vector<cv::Mat> localDescriptors;
appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
}
@@ -763,7 +891,16 @@ void RGBDOdometry::callbackRGBD5(
infoMsgs.push_back(image4->rgb_camera_info);
infoMsgs.push_back(image5->rgb_camera_info);
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
std::vector<cv::Mat> localDescriptors;
appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors);
this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
}
@@ -795,7 +932,17 @@ void RGBDOdometry::callbackRGBD6(
infoMsgs.push_back(image5->rgb_camera_info);
infoMsgs.push_back(image6->rgb_camera_info);
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
std::vector<cv::Mat> localDescriptors;
appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image6, localKeyPoints, localPoints3d, localDescriptors);
this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
}
rtabmap_odom/src/nodelets/stereo_odometry.cpp
+201 -63
View File
@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap/core/Odometry.h>
using namespace rtabmap;
@@ -73,6 +74,8 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
StereoOdometry::~StereoOdometry()
{
this->join(true);
delete approxSync_;
delete exactSync_;
delete approxSync2_;
@@ -407,29 +410,46 @@ void StereoOdometry::commonCallback(
const std::vector<cv_bridge::CvImageConstPtr> & leftImages,
const std::vector<cv_bridge::CvImageConstPtr> & rightImages,
const std::vector<sensor_msgs::msg::CameraInfo>& leftCameraInfos,
const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos)
const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos,
const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs,
const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs,
const std::vector<cv::Mat> & localDescriptorsMsgs)
{
UASSERT(leftImages.size() > 0 &&
leftImages.size() == rightImages.size() &&
leftImages.size() == leftCameraInfos.size() &&
rightImages.size() == rightCameraInfos.size());
rclcpp::Time higherStamp;
int leftWidth = leftImages[0]->image.cols;
int leftHeight = leftImages[0]->image.rows;
// The images are what the local features would otherwise be extracted from, so a frame
// that brings its own can leave them out -- which is nearly all of the bandwidth. It
// then describes itself with its calibration alone: how big the left image would have
// been, where the rig is, how far apart the two cameras are.
const bool hasImages = !leftImages[0]->image.empty() && !rightImages[0]->image.empty();
int leftWidth = hasImages?leftImages[0]->image.cols:(int)leftCameraInfos[0].width;
int leftHeight = hasImages?leftImages[0]->image.rows:(int)leftCameraInfos[0].height;
int rightWidth = rightImages[0]->image.cols;
int rightHeight = rightImages[0]->image.rows;
UASSERT_MSG(
leftWidth == rightWidth && leftHeight == rightHeight,
uFormat("left=%dx%d right=%dx%d", leftWidth, leftHeight, rightWidth, rightHeight).c_str());
if(hasImages)
{
UASSERT_MSG(
leftWidth == rightWidth && leftHeight == rightHeight,
uFormat("left=%dx%d right=%dx%d", leftWidth, leftHeight, rightWidth, rightHeight).c_str());
}
int cameraCount = leftImages.size();
cv::Mat left;
cv::Mat right;
std::vector<rtabmap::StereoCameraModel> cameraModels;
std::vector<cv::KeyPoint> keypoints;
std::vector<cv::Point3f> points3d;
cv::Mat descriptors;
for(unsigned int i=0; i<leftImages.size(); ++i)
{
if(!(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
if(hasImages &&
(!(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
@@ -442,14 +462,21 @@ void StereoOdometry::commonCallback(
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0)))
{
RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
leftImages[i]->encoding.c_str(), rightImages[i]->encoding.c_str());
return;
}
rclcpp::Time stamp = rtabmap_conversions::timestampFromROS(leftImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(rightImages[i]->header.stamp)?leftImages[i]->header.stamp:rightImages[i]->header.stamp;
// An image that is not there carries no header either, so a frame that has none is
// stamped and placed by its calibration, which is all it has.
const std::string & cameraFrameId = hasImages?leftImages[i]->header.frame_id:leftCameraInfos[i].header.frame_id;
rclcpp::Time stamp = leftCameraInfos[i].header.stamp;
if(hasImages)
{
stamp = rtabmap_conversions::timestampFromROS(leftImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(rightImages[i]->header.stamp)?leftImages[i]->header.stamp:rightImages[i]->header.stamp;
}
if(i == 0)
{
@@ -460,7 +487,7 @@ void StereoOdometry::commonCallback(
higherStamp = stamp;
}
Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), leftImages[i]->header.frame_id, stamp, tfBuffer(), waitForTransform());
Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), cameraFrameId, stamp, tfBuffer(), waitForTransform());
if(localTransform.isNull())
{
return;
@@ -488,7 +515,13 @@ void StereoOdometry::commonCallback(
}
}
if(!leftImages[i]->image.empty() && !rightImages[i]->image.empty())
if(hasImages != (!leftImages[i]->image.empty() && !rightImages[i]->image.empty()))
{
RCLCPP_ERROR(this->get_logger(), "Odom: camera %d of this frame has images while "
"another one doesn't (or the other way around)?!?", i);
return;
}
{
bool alreadyRectified = true;
Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
@@ -602,62 +635,76 @@ void StereoOdometry::commonCallback(
shown = true;
}
}
cv_bridge::CvImageConstPtr ptrLeft = leftImages[i];
if(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
if(hasImages)
{
if(keepColor_ && leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
cv_bridge::CvImageConstPtr ptrLeft = leftImages[i];
if(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
{
ptrLeft = cv_bridge::cvtColor(leftImages[i], "bgr8");
if(keepColor_ && leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
{
ptrLeft = cv_bridge::cvtColor(leftImages[i], "bgr8");
}
else
{
ptrLeft = cv_bridge::cvtColor(leftImages[i], "mono8");
}
}
cv_bridge::CvImageConstPtr ptrRight = rightImages[i];
if(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
{
ptrRight = cv_bridge::cvtColor(rightImages[i], "mono8");
}
// initialize
if(left.empty())
{
left = cv::Mat(leftHeight, leftWidth*cameraCount, ptrLeft->image.type());
}
if(right.empty())
{
right = cv::Mat(rightHeight, rightWidth*cameraCount, ptrRight->image.type());
}
if(ptrLeft->image.type() == left.type())
{
ptrLeft->image.copyTo(cv::Mat(left, cv::Rect(i*leftWidth, 0, leftWidth, leftHeight)));
}
else
{
ptrLeft = cv_bridge::cvtColor(leftImages[i], "mono8");
RCLCPP_ERROR(this->get_logger(), "Some left images are not the same type! %d vs %d", ptrLeft->image.type(), left.type());
return;
}
}
cv_bridge::CvImageConstPtr ptrRight = rightImages[i];
if(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
{
ptrRight = cv_bridge::cvtColor(rightImages[i], "mono8");
}
// initialize
if(left.empty())
{
left = cv::Mat(leftHeight, leftWidth*cameraCount, ptrLeft->image.type());
}
if(right.empty())
{
right = cv::Mat(rightHeight, rightWidth*cameraCount, ptrRight->image.type());
}
if(ptrLeft->image.type() == left.type())
{
ptrLeft->image.copyTo(cv::Mat(left, cv::Rect(i*leftWidth, 0, leftWidth, leftHeight)));
}
else
{
RCLCPP_ERROR(this->get_logger(), "Some left images are not the same type! %d vs %d", ptrLeft->image.type(), left.type());
return;
}
if(ptrRight->image.type() == right.type())
{
ptrRight->image.copyTo(cv::Mat(right, cv::Rect(i*rightWidth, 0, rightWidth, rightHeight)));
}
else
{
RCLCPP_ERROR(this->get_logger(), "Some right images are not the same type! %d vs %d", ptrRight->image.type(), right.type());
return;
if(ptrRight->image.type() == right.type())
{
ptrRight->image.copyTo(cv::Mat(right, cv::Rect(i*rightWidth, 0, rightWidth, rightHeight)));
}
else
{
RCLCPP_ERROR(this->get_logger(), "Some right images are not the same type! %d vs %d", ptrRight->image.type(), right.type());
return;
}
}
cameraModels.push_back(stereoModel);
}
else
// The left images of all cameras are stitched side by side above, so the keypoints
// of camera i are shifted by as many images as come before it, and their 3D points,
// which arrive in that camera's optical frame, are brought back to the base frame.
if(localKeyPointsMsgs.size() == leftImages.size())
{
RCLCPP_ERROR(this->get_logger(), "Odom: input images empty?!?");
return;
rtabmap_conversions::keypointsFromROS(localKeyPointsMsgs[i], keypoints, leftWidth*i);
}
if(localPoints3dMsgs.size() == leftImages.size())
{
rtabmap_conversions::points3fFromROS(localPoints3dMsgs[i], points3d, localTransform);
}
if(localDescriptorsMsgs.size() == leftImages.size())
{
descriptors.push_back(localDescriptorsMsgs[i]);
}
}
@@ -669,9 +716,30 @@ void StereoOdometry::commonCallback(
0,
rtabmap_conversions::timestampFromROS(higherStamp));
// Features that came with the frame are used as they are: the odometry then skips
// detection, description and the disparity search that would otherwise rebuild them
// (see RegistrationVis, which extracts only when the frame carries no keypoints).
// They are dropped rather than trusted if the three of them disagree, as using them
// out of step would silently mismatch keypoints with their descriptors or 3D points.
if(!keypoints.empty())
{
if((!points3d.empty() && points3d.size() != keypoints.size()) ||
(!descriptors.empty() && descriptors.rows != (int)keypoints.size()))
{
RCLCPP_ERROR(this->get_logger(), "Ignoring the local features received with this frame: "
"%d keypoints, %d 3D points and %d descriptors, which should be the same count "
"(or none at all for the 3D points and the descriptors).",
(int)keypoints.size(), (int)points3d.size(), descriptors.rows);
}
else
{
data.setFeatures(keypoints, points3d, descriptors);
}
}
std_msgs::msg::Header header;
header.stamp = higherStamp;
header.frame_id = leftImages.size()==1?leftImages[0]->header.frame_id:"";
header.frame_id = leftImages.size()==1?(hasImages?leftImages[0]->header.frame_id:leftCameraInfos[0].header.frame_id):"";
this->processData(data, header);
}
@@ -715,6 +783,27 @@ void StereoOdometry::callback(
}
}
namespace {
/**
* @brief Collects the local features one camera's frame carries, cameras in order.
*
* A frame that carries none pushes empty entries rather than nothing, so that the
* per-camera indexing still lines up with the images.
*/
void appendLocalFeatures(
const rtabmap_msgs::msg::RGBDImage & image,
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & keyPoints,
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & points3d,
std::vector<cv::Mat> & descriptors)
{
keyPoints.push_back(image.key_points);
points3d.push_back(image.points);
descriptors.push_back(rtabmap::uncompressData(image.descriptors));
}
} // namespace
void StereoOdometry::callbackRGBD(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
{
@@ -730,7 +819,12 @@ void StereoOdometry::callbackRGBD(
leftInfoMsgs.push_back(image->rgb_camera_info);
rightInfoMsgs.push_back(image->depth_camera_info);
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
std::vector<cv::Mat> localDescriptors;
appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
}
@@ -750,14 +844,18 @@ void StereoOdometry::callbackRGBDX(
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(images->rgbd_images.size());
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
std::vector<cv::Mat> localDescriptors;
for(size_t i=0; i<images->rgbd_images.size(); ++i)
{
rtabmap_conversions::toCvShare(images->rgbd_images[i], images, leftMsgs[i], rightMsgs[i]);
leftInfoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
rightInfoMsgs.push_back(images->rgbd_images[i].depth_camera_info);
appendLocalFeatures(images->rgbd_images[i], localKeyPoints, localPoints3d, localDescriptors);
}
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
}
@@ -780,7 +878,13 @@ void StereoOdometry::callbackRGBD2(
rightInfoMsgs.push_back(image->depth_camera_info);
rightInfoMsgs.push_back(image2->depth_camera_info);
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
std::vector<cv::Mat> localDescriptors;
appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
}
@@ -807,7 +911,14 @@ void StereoOdometry::callbackRGBD3(
rightInfoMsgs.push_back(image2->depth_camera_info);
rightInfoMsgs.push_back(image3->depth_camera_info);
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
std::vector<cv::Mat> localDescriptors;
appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
}
@@ -838,7 +949,15 @@ void StereoOdometry::callbackRGBD4(
rightInfoMsgs.push_back(image3->depth_camera_info);
rightInfoMsgs.push_back(image4->depth_camera_info);
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
std::vector<cv::Mat> localDescriptors;
appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
}
@@ -873,7 +992,16 @@ void StereoOdometry::callbackRGBD5(
rightInfoMsgs.push_back(image4->depth_camera_info);
rightInfoMsgs.push_back(image5->depth_camera_info);
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
std::vector<cv::Mat> localDescriptors;
appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors);
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
}
@@ -912,7 +1040,17 @@ void StereoOdometry::callbackRGBD6(
rightInfoMsgs.push_back(image5->depth_camera_info);
rightInfoMsgs.push_back(image6->depth_camera_info);
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
std::vector<cv::Mat> localDescriptors;
appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors);
appendLocalFeatures(*image6, localKeyPoints, localPoints3d, localDescriptors);
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
}
rtabmap_odom/test/bag_playback.hpp
+79
View File
@@ -0,0 +1,79 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#ifndef RTABMAP_ODOM_BAG_PLAYBACK_HPP_
#define RTABMAP_ODOM_BAG_PLAYBACK_HPP_
#include <rclcpp/serialization.hpp>
#include <rosbag2_cpp/reader.hpp>
#include <string>
#include <vector>
#include "test_data.hpp"
/**
* @file
* @brief Reads recorded messages out of a bag, for tests that need real sensor input.
*
* The messages are read and replayed by the test itself rather than by `ros2 bag play`:
* no second process, no wall-clock pacing, and the test controls exactly when each
* message reaches the node. What the recording provides is the part that cannot be
* written by hand -- a real driver's cloud layout and a dense TF history around it.
*/
namespace rtabmap_odom_test {
/**
* @brief The Ouster recording in test/data/lidar; see that directory's README.
*
* Two sweeps half a mast turn apart, from a platform that never moves: the only thing
* between them is the mast's rotation, which TF describes in full.
*/
inline std::string ousterHalfTurnBag()
{
return testDataRoot() + "/lidar/ouster_half_turn";
}
/**
* @brief Every message recorded on @p topic, deserialized.
*
* Returns an empty vector if the bag or the topic is missing, which the caller is
* expected to assert on -- a silently empty fixture would make a test pass for the wrong
* reason.
*/
template <typename MsgT>
std::vector<MsgT> readBagMessages(const std::string & bagPath, const std::string & topic)
{
std::vector<MsgT> messages;
rosbag2_cpp::Reader reader;
try
{
reader.open(bagPath);
}
catch(const std::exception & e)
{
return messages;
}
rclcpp::Serialization<MsgT> serialization;
while(reader.has_next())
{
const std::shared_ptr<rosbag2_storage::SerializedBagMessage> message = reader.read_next();
if(message->topic_name != topic)
{
continue;
}
rclcpp::SerializedMessage serialized(*message->serialized_data);
MsgT deserialized;
serialization.deserialize_message(&serialized, &deserialized);
messages.push_back(deserialized);
}
return messages;
}
} // namespace rtabmap_odom_test
#endif /* RTABMAP_ODOM_BAG_PLAYBACK_HPP_ */
rtabmap_odom/test/camera_rig.hpp
+319
View File
@@ -0,0 +1,319 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#ifndef RTABMAP_ODOM_CAMERA_RIG_HPP_
#define RTABMAP_ODOM_CAMERA_RIG_HPP_
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rtabmap_msgs/msg/rgbd_images.hpp>
#include <rtabmap_conversions/MsgConversion.h>
#include <rtabmap/core/CameraModel.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap/core/util3d_transforms.h>
#include <opencv2/core/core.hpp>
#include <algorithm>
#include <cmath>
#include <random>
#include <string>
#include <vector>
#include "msg_builders.hpp"
/**
* @file
* @brief A camera rig in a world of points, for the multi-camera odometry tests.
*
* Several cameras looking outward from one body is the case that cannot be covered with
* recorded frames: it needs a calibrated rig, a scene all of them can see, and ground
* truth for where the body went. So the scene is made rather than recorded -- points
* scattered around the rig, projected into each camera at each pose along a known
* trajectory, which is the approach of RTAB-Map's own multi-camera tests
* (`EstimateMotion3DTo2DMultiCam*` in corelib/test/test_util3d_motion_estimation.cpp and
* the four-camera rig of test_optimizer.cpp).
*
* What comes out is the *features*, not imagery: keypoints, their 3D positions and a
* descriptor per point, which is what a camera driver doing its own feature extraction
* would publish on `rgbd_images`, and all it needs to publish. The frames carry no image
* at all, so a run that recovers the trajectory can only have done it from them.
*/
namespace rtabmap_odom_test {
/// Descriptors are matched between frames, so the same point has to keep the same one.
const int kRigDescriptorSize = 32;
/**
* @brief Cameras looking outward from one body, and the points they see.
*
* The cameras are spread evenly around the body and mounted on its rim, so opposite
* cameras are a real distance apart rather than sharing an optical centre.
*/
struct CameraRig
{
int width = 0;
int height = 0;
double fx = 0.0;
/// base_link -> camera i, optical frame, in the order the cameras are published.
std::vector<rtabmap::Transform> localTransforms;
std::vector<std::string> frameIds;
/// The world: points in the odometry frame, and one descriptor row per point.
std::vector<cv::Point3f> points;
cv::Mat descriptors;
size_t cameras() const { return localTransforms.size(); }
/// Camera @p i as RTAB-Map sees it, intrinsics and mounting together.
rtabmap::CameraModel model(size_t i) const
{
return rtabmap::CameraModel(fx, fx, width/2.0, height/2.0,
localTransforms[i], 0, cv::Size(width, height));
}
};
/**
* @brief A rig of @p cameras cameras in a world of @p numPoints points.
*
* The horizontal field of view is 360/cameras degrees, up to 90, so the cameras tile as
* much of the circle as they can without overlapping: a point is then seen by at most one
* of them, which keeps every descriptor unique within a frame. Two cameras seeing the same
* point would put two identical descriptors in the same frame, and the ratio test that
* accepts a match only when the best candidate is clearly better than the second would
* throw both away.
*
* The points sit in a box wider than the trajectory, minus a hole around it: something
* closer than @p minRange would swing through a camera's field of view, or behind it,
* over the course of the run.
*/
inline CameraRig makeCameraRig(
int cameras = 4,
int numPoints = 300,
float boxXY = 8.0f,
float boxZ = 1.5f,
float minRange = 2.5f,
int width = 160,
int height = 120,
float rimRadius = 0.175f,
float rimHeight = 0.05f,
uint32_t seed = 7)
{
CameraRig rig;
rig.width = width;
rig.height = height;
// Half the horizontal field of view spans half the angle between two cameras, capped
// at 45 degrees: one or two cameras would otherwise be asked for a 360 or 180 degree
// view, which no pinhole model has. The cap only widens the gaps between cameras, so
// a point is still seen by at most one of them.
rig.fx = (width/2.0) / std::tan(std::min(M_PI/double(cameras), M_PI/4.0));
for(int i=0; i<cameras; ++i)
{
const float yaw = 2.0f*float(M_PI)*float(i)/float(cameras);
rig.localTransforms.push_back(
rtabmap::Transform(rimRadius*std::cos(yaw), rimRadius*std::sin(yaw), rimHeight,
0.0f, 0.0f, yaw)
* rtabmap::CameraModel::opticalRotation());
rig.frameIds.push_back("camera" + std::to_string(i));
}
std::mt19937 rng(seed);
std::uniform_real_distribution<float> distXY(-boxXY, boxXY);
std::uniform_real_distribution<float> distZ(-boxZ, boxZ);
std::normal_distribution<float> distDescriptor(0.0f, 1.0f);
rig.descriptors = cv::Mat(numPoints, kRigDescriptorSize, CV_32FC1);
for(int i=0; i<numPoints; )
{
const cv::Point3f point(distXY(rng), distXY(rng), distZ(rng));
if(std::sqrt(point.x*point.x + point.y*point.y) < minRange)
{
continue;
}
rig.points.push_back(point);
for(int c=0; c<kRigDescriptorSize; ++c)
{
rig.descriptors.at<float>(i, c) = distDescriptor(rng);
}
++i;
}
return rig;
}
/// The mounting of each camera, as the rig's driver would publish it on /tf_static.
inline std::vector<geometry_msgs::msg::TransformStamped> cameraRigTransforms(
const CameraRig & rig, const rclcpp::Time & stamp,
const std::string & baseFrame = "base_link")
{
std::vector<geometry_msgs::msg::TransformStamped> transforms;
for(size_t i=0; i<rig.cameras(); ++i)
{
geometry_msgs::msg::TransformStamped tf;
tf.header.stamp = stamp;
tf.header.frame_id = baseFrame;
tf.child_frame_id = rig.frameIds[i];
rtabmap_conversions::transformToGeometryMsg(rig.localTransforms[i], tf.transform);
transforms.push_back(tf);
}
return transforms;
}
/// What one camera of the rig saw: its keypoints, their 3D points and their descriptors.
struct RigObservations
{
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > keyPoints;
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > points;
std::vector<cv::Mat> descriptors;
};
/**
* @brief What the rig sees from @p pose, one entry per camera.
*
* Each point is given to the first camera that has it in view, so no point is reported
* twice. The keypoints are in their own camera's image, the 3D points in their own
* camera's optical frame, and the descriptors in the order of the keypoints -- which is
* how a driver publishes them, and what the node has to reassemble.
*/
inline RigObservations observeCameraRig(const CameraRig & rig, const rtabmap::Transform & pose)
{
const size_t cameras = rig.cameras();
std::vector<rtabmap::CameraModel> models;
std::vector<rtabmap::Transform> worldToCamera;
for(size_t i=0; i<cameras; ++i)
{
models.push_back(rig.model(i));
worldToCamera.push_back((pose * rig.localTransforms[i]).inverse());
}
RigObservations seen;
seen.keyPoints.resize(cameras);
seen.points.resize(cameras);
seen.descriptors.resize(cameras);
for(size_t p=0; p<rig.points.size(); ++p)
{
for(size_t i=0; i<cameras; ++i)
{
const cv::Point3f inCamera =
rtabmap::util3d::transformPoint(rig.points[p], worldToCamera[i]);
if(inCamera.z <= 0.1f)
{
continue;
}
float u = 0.0f;
float v = 0.0f;
models[i].reproject(inCamera.x, inCamera.y, inCamera.z, u, v);
// Truncation would let a point just off the left or top edge through, at a
// negative pixel the odometry drops later, so the bounds are checked directly.
if(u < 0.0f || v < 0.0f || u >= float(rig.width) || v >= float(rig.height))
{
continue;
}
rtabmap_msgs::msg::KeyPoint keyPoint;
keyPoint.pt.x = u;
keyPoint.pt.y = v;
keyPoint.size = 3;
keyPoint.response = 1.0f;
seen.keyPoints[i].push_back(keyPoint);
rtabmap_msgs::msg::Point3f point;
point.x = inCamera.x;
point.y = inCamera.y;
point.z = inCamera.z;
seen.points[i].push_back(point);
seen.descriptors[i].push_back(rig.descriptors.row(int(p)));
break;
}
}
return seen;
}
/**
* @brief The rig's observations from @p pose as RGB-D frames, one per camera.
*
* @p withImages attaches a blank image to each camera. There is nothing to find in it,
* but the odometry only takes the paths that touch images when one is there.
*/
inline rtabmap_msgs::msg::RGBDImages cameraRigFrame(
const CameraRig & rig, const rtabmap::Transform & pose, double stamp,
bool withImages = false)
{
const RigObservations seen = observeCameraRig(rig, pose);
rtabmap_msgs::msg::RGBDImages msg;
msg.header.stamp = stampOf(stamp);
msg.header.frame_id = rig.frameIds[0];
for(size_t i=0; i<rig.cameras(); ++i)
{
rtabmap_msgs::msg::RGBDImage image;
image.header.stamp = msg.header.stamp;
image.header.frame_id = rig.frameIds[i];
// No image at all by default, neither color nor depth: this frame is its
// calibration and its features, which is everything a camera doing its own
// extraction has to send.
if(withImages)
{
image.rgb = makeImage(rig.frameIds[i], stamp,
cv::Mat::zeros(rig.height, rig.width, CV_8UC1), "mono8");
}
image.rgb_camera_info = makeCameraInfo(
rig.frameIds[i], stamp, rig.width, rig.height, 0.0, rig.fx);
image.depth_camera_info = image.rgb_camera_info;
image.key_points = seen.keyPoints[i];
image.points = seen.points[i];
image.descriptors = rtabmap::compressData(seen.descriptors[i]);
msg.rgbd_images.push_back(image);
}
return msg;
}
/**
* @brief The same observations as stereo frames: left camera plus a right one @p baseline
* to its side.
*
* `RGBDImage` carries a stereo pair as its color and depth fields, so this differs from
* the RGB-D frames above only in the second calibration, whose `P(0,3)` is what tells the
* node how far apart the two cameras are. The features belong to the left image either
* way, which is where a stereo pipeline finds them.
*/
inline rtabmap_msgs::msg::RGBDImages cameraRigStereoFrame(
const CameraRig & rig, const rtabmap::Transform & pose, double stamp,
double baseline = 0.12, bool withImages = false)
{
rtabmap_msgs::msg::RGBDImages msg = cameraRigFrame(rig, pose, stamp, withImages);
for(size_t i=0; i<msg.rgbd_images.size(); ++i)
{
msg.rgbd_images[i].depth_camera_info = makeCameraInfo(
rig.frameIds[i], stamp, rig.width, rig.height, -baseline*rig.fx, rig.fx);
if(withImages)
{
msg.rgbd_images[i].depth = makeImage(rig.frameIds[i], stamp,
cv::Mat::zeros(rig.height, rig.width, CV_8UC1), "mono8");
}
}
return msg;
}
/// How many features @p frame carries, all cameras together.
inline size_t cameraRigFeatureCount(const rtabmap_msgs::msg::RGBDImages & frame)
{
size_t count = 0;
for(size_t i=0; i<frame.rgbd_images.size(); ++i)
{
count += frame.rgbd_images[i].key_points.size();
}
return count;
}
} // namespace rtabmap_odom_test
#endif /* RTABMAP_ODOM_CAMERA_RIG_HPP_ */
rtabmap_odom/test/data/README.md
+103
View File
@@ -0,0 +1,103 @@
# Test data
Real frames for the odometry node tests, so that they register actual imagery instead of
synthetic noise. Synthetic textures give a detector corners that match nothing between
frames, which makes a "motion" that only proves the node did not crash.
Everything here is BSD-3-Clause, same authors and same terms as the rest of this
repository.
| Path | Origin | Used by |
| --- | --- | --- |
| `stereo/rect/{left,right}/{50,60}.jpg` | [RTAB-Map `data/stereo_rect`](https://github.com/introlab/rtabmap/tree/master/data/stereo_rect) | `test_stereo_odometry.cpp` |
| `stereo/rect/stereo_{left,right}.yaml` | [RTAB-Map `data/stereo_rect`](https://github.com/introlab/rtabmap/tree/master/data/stereo_rect) | `test_stereo_odometry.cpp` |
| `stereo/raw/{left,right}/{420,425}.jpg` | frames 420 and 425 (21.00 s and 21.25 s at 20 Hz) of the [stereo indoor tutorial](https://github.com/introlab/rtabmap/wiki/Stereo-mapping) test sequence | `test_stereo_odometry.cpp` |
| `stereo/raw/stereo_{left,right}.yaml`, `stereo/raw/stereo_pose.yaml` | that rig's own calibration (`stereo_tutorial_*`) | `test_stereo_odometry.cpp` |
| `lidar/ouster_half_turn/` | frames 1613418430.682 and 1613418435.082 of an Ouster recording on a rotating mast | `test_icp_odometry.cpp` |
| `rgbd/rgb/{17,154}.jpg`, `rgbd/depth/{17,154}.png` | [RTAB-Map `data/rgbd`](https://github.com/introlab/rtabmap/tree/master/data/rgbd) | `test_rgbd_odometry.cpp` |
| `rgbd/calib/{17,154}.yaml` | [RTAB-Map `data/rgbd`](https://github.com/introlab/rtabmap/tree/master/data/rgbd) | `test_rgbd_odometry.cpp` |
They are vendored rather than read from an RTAB-Map checkout because RTAB-Map reaches us
as an installed library: the `introlab3it/rtabmap` images used by CI delete the source tree
after `make install`, and the ROS buildfarm has no network during a build. A copy here is
what makes these tests run everywhere rather than skip.
## What the frames are
Two frames of each kind is the minimum that says anything: the first initialises the
odometry at the origin, the second has to be registered against it.
- **`stereo/rect`** -- `50` and `60`, two rectified pairs of the same scene a short motion
apart. The estimate comes out at 0.171 m, steady to well under a centimetre across runs.
These back `corelib/test/test_odometry.cpp` upstream, so a failure here that also fails
there is an RTAB-Map issue rather than a ROS one.
- **`stereo/raw`** -- `420` and `425`, an unrectified pair a quarter second apart, for the
`Rtabmap/ImagesAlreadyRectified:=false` path described in `doc/stereo_odometry.md`, and
for pinning what happens when that parameter is left at its default on distorted images.
A quarter second of walking forward, ~0.233 m, reproduced to a fraction of a percent
across runs. Wider gaps in the same window register too, but not reliably: 21.00 s to
22.00 s is ~1.04 m at around 45 inliers and lost tracking outright in one run out of
eight, where this pair holds 210 or more.
- **`lidar/ouster_half_turn`** -- two Ouster sweeps 4.40 s apart, recorded as a ROS 2 mcap
bag (`/tf`, `/tf_static`, `/os_cloud_node/points`) rather than as loose files, because
what makes them worth keeping is the TF history around them.
The sensor sits on a mast turning at ~42 deg/s while the platform stays put, so it moves
through its own 0.1 s sweep and the cloud comes off the driver skewed. The two sweeps are
half a turn apart (184.5 deg), which puts the skew in opposite directions and makes a
missing correction obvious. And because the platform never moves -- `base_link` ->
`box_link` does not translate by a single millimetre over the whole recording -- the
answer is known: the transform between the two is the identity. That is what the test
measures against, rather than a value taken from a previous run.
TF runs from 0.1 s before each sweep to 0.1 s past its end, with nothing in between: the
gap holds transforms nobody looks up, and keeping them would have tripled the file.
- **`rgbd`** -- `17` and `154`, two frames of a hand-held Kinect sequence, far enough apart
that losing tracking between them is a legitimate outcome.
In every set the left image is color and the right one grayscale, as the cameras recorded
them.
Both stereo sets come from the same 640x480 rig, but **not** from the same calibration, and
the two are not interchangeable:
| | `stereo/rect` | `stereo/raw` |
| --- | --- | --- |
| `distortion_coefficients` | zeros | the lens's real plumb_bob values (~-0.34) |
| `rectification_matrix` | identity | the rotation into the rectified frame |
| `projection_matrix` fx | 487.61 | 500.22 |
| baseline (`-Tx/fx`) | 0.1197 m | 0.1197 m |
Rectification is what leaves a calibration with no distortion and an identity rotation, so
the rectified file describes the output of `stereo_image_proc`, not what the camera
produced. Handing it to a raw pair claims a distortion-free lens the images do not have:
doing that costs roughly a third of the inliers (110 against 214) and triples the reported
standard deviation. `stereo_pose.yaml` holds the rig's measured extrinsics -- ~12 cm along
x plus a few milliradians of rotation -- which the tests publish as the TF between
`camera_left` and `camera_right`, the transform the node looks up when it has to rectify
the pair itself.
## File formats
The images are copied as they are. The calibration files differ from their originals by one
line: the format directive is commented out (`#%YAML:1.0`, with no `---`), which is the ROS
flavour of the same file. `%YAML:1.0` is not a valid YAML directive, so plain YAML parsers
-- `camera_info_manager`, `rosparam`, PyYAML -- reject the original form.
`test/test_data.hpp` puts the directive back in memory before handing the text to
`cv::FileStorage`, so the files stay readable by both.
Note that they carry no OpenCV `dt` field either, so `>> cv::Mat` cannot read them;
`rows`/`cols`/`data` are read element by element, as RTAB-Map's own `CameraModel::load`
does.
The depth images are 16-bit millimetres. `rgbd/calib/*.yaml` also carries a
`local_transform` (the optical-frame-to-robot transform RTAB-Map stores with the camera
model); the ROS nodes take that from TF instead, so the tests publish it as a `base_link`
-> `camera` static transform rather than reading it here.
## Using them
`test/test_data.hpp` loads these into `cv::Mat`, `sensor_msgs/CameraInfo` and
`geometry_msgs/Transform`. CMake passes the directory as `RTABMAP_ODOM_TEST_DATA_ROOT`,
pointing into the source tree: the test binaries are not installed, and neither are these
files.
rtabmap_odom/test/data/lidar/ouster_half_turn/metadata.yaml
+52
View File
@@ -0,0 +1,52 @@
rosbag2_bagfile_information:
compression_format: ''
compression_mode: ''
custom_data: null
duration:
nanoseconds: 5066021600
files:
- duration:
nanoseconds: 5066021600
message_count: 120
path: ouster_half_turn.mcap
starting_time:
nanoseconds_since_epoch: 1613418430206475184
message_count: 120
relative_file_paths:
- ouster_half_turn.mcap
ros_distro: rosbags
starting_time:
nanoseconds_since_epoch: 1613418430206475184
storage_identifier: mcap
topics_with_message_count:
- message_count: 2
topic_metadata:
name: /tf_static
offered_qos_profiles: "- avoid_ros_namespace_conventions: false\n deadline:
{nsec: 0, sec: 0}\n depth: 10\n durability: 1\n history: 1\n lifespan:
{nsec: 0, sec: 0}\n liveliness: 1\n liveliness_lease_duration: {nsec: 0,
sec: 0}\n reliability: 1"
serialization_format: cdr
type: tf2_msgs/msg/TFMessage
type_description_hash: RIHS01_e369d0f05a23ae52508854b66f6aa0437f3449d652e8cbf22d5abe85d020f087
- message_count: 116
topic_metadata:
name: /tf
offered_qos_profiles: "- avoid_ros_namespace_conventions: false\n deadline:
{nsec: 0, sec: 0}\n depth: 10\n durability: 2\n history: 1\n lifespan:
{nsec: 0, sec: 0}\n liveliness: 1\n liveliness_lease_duration: {nsec: 0,
sec: 0}\n reliability: 1"
serialization_format: cdr
type: tf2_msgs/msg/TFMessage
type_description_hash: RIHS01_e369d0f05a23ae52508854b66f6aa0437f3449d652e8cbf22d5abe85d020f087
- message_count: 2
topic_metadata:
name: /os_cloud_node/points
offered_qos_profiles: "- avoid_ros_namespace_conventions: false\n deadline:
{nsec: 0, sec: 0}\n depth: 10\n durability: 2\n history: 1\n lifespan:
{nsec: 0, sec: 0}\n liveliness: 1\n liveliness_lease_duration: {nsec: 0,
sec: 0}\n reliability: 1"
serialization_format: cdr
type: sensor_msgs/msg/PointCloud2
type_description_hash: RIHS01_9198cabf7da3796ae6fe19c4cb3bdd3525492988c70522628af5daa124bae2b5
version: 8
rtabmap_odom/test/data/lidar/ouster_half_turn/ouster_half_turn.mcap
BIN
View File
Binary file not shown.
rtabmap_odom/test/data/rgbd/calib/154.yaml
+16
View File
@@ -0,0 +1,16 @@
#%YAML:1.0
camera_name: "154"
image_width: 640
image_height: 480
camera_matrix:
rows: 3
cols: 3
data: [ 525., 0., 3.1950000000000000e+02, 0., 525.,
2.3950000000000000e+02, 0., 0., 1. ]
local_transform:
rows: 3
cols: 4
data: [ -1.09767914e-03, 5.99460304e-02, 9.98201132e-01,
4.20193411e-02, -9.99999523e-01, -6.60419464e-05, -1.09562278e-03,
-8.86659764e-05, 8.94069672e-08, -9.98201728e-01, 5.99459410e-02,
4.28920656e-01 ]
rtabmap_odom/test/data/rgbd/calib/17.yaml
+16
View File
@@ -0,0 +1,16 @@
#%YAML:1.0
camera_name: "17"
image_width: 640
image_height: 480
camera_matrix:
rows: 3
cols: 3
data: [ 525., 0., 3.1950000000000000e+02, 0., 525.,
2.3950000000000000e+02, 0., 0., 1. ]
local_transform:
rows: 3
cols: 4
data: [ -1.46162510e-03, 5.99460006e-02, 9.98200655e-01,
6.20153509e-02, -9.99999106e-01, -8.77380371e-05, -1.45888329e-03,
-1.63501027e-04, -2.98023224e-08, -9.98201728e-01, 5.99459410e-02,
4.28920656e-01 ]
rtabmap_odom/test/data/rgbd/depth/154.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 120 KiB

rtabmap_odom/test/data/rgbd/depth/17.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 114 KiB

rtabmap_odom/test/data/rgbd/rgb/154.jpg
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 77 KiB

rtabmap_odom/test/data/rgbd/rgb/17.jpg
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 80 KiB

rtabmap_odom/test/data/stereo/raw/left/420.jpg
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 58 KiB

rtabmap_odom/test/data/stereo/raw/left/425.jpg
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 59 KiB

rtabmap_odom/test/data/stereo/raw/right/420.jpg
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 54 KiB

rtabmap_odom/test/data/stereo/raw/right/425.jpg
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 54 KiB

rtabmap_odom/test/data/stereo/raw/stereo_left.yaml
+34
View File
@@ -0,0 +1,34 @@
#%YAML:1.0
camera_name: stereo_tutorial_left
image_width: 640
image_height: 480
camera_matrix:
rows: 3
cols: 3
data: [ 5.2500741669069100e+02, 0., 3.1964931134995413e+02, 0.,
5.2447137699104690e+02, 2.4893842459131687e+02, 0., 0., 1. ]
distortion_coefficients:
rows: 1
cols: 5
data: [ -3.4256158963391159e-01, 1.5067561187251743e-01,
-1.1171618396794343e-03, -1.1496904882258663e-03,
-3.4792309849001175e-02 ]
distortion_model: plumb_bob
rectification_matrix:
rows: 3
cols: 3
data: [ 9.9847710457215999e-01, -7.5204006718293083e-03,
-5.4652678058179430e-02, 7.6102614351676607e-03,
9.9997001005750097e-01, 1.4362821762221084e-03,
5.4640237610064049e-02, -1.8500160368176528e-03,
9.9850439251641721e-01 ]
projection_matrix:
rows: 3
cols: 4
data: [ 5.0021545003868783e+02, 0., 3.5945997238159180e+02, 0., 0.,
5.0021545003868783e+02, 2.4750019836425781e+02, 0., 0., 0., 1.,
0. ]
local_transform:
rows: 3
cols: 4
data: [ 0., 0., 1., 0., -1., 0., 0., 0., 0., -1., 0., 0. ]
rtabmap_odom/test/data/stereo/raw/stereo_pose.yaml
+30
View File
@@ -0,0 +1,30 @@
#%YAML:1.0
camera_name: stereo_tutorial
rotation_matrix:
rows: 3
cols: 3
data: [ 9.9998244174079198e-01, -9.1486052999498408e-04,
5.8548475927491656e-03, 8.9587501387632107e-04,
9.9999433529188531e-01, 3.2445018261516327e-03,
-5.8577826934067389e-03, -3.2391996466791719e-03,
9.9997759673282971e-01 ]
translation_matrix:
rows: 3
cols: 1
data: [ -1.1944376542810328e-01, 8.1410498203477041e-04,
7.2368896323297812e-03 ]
essential_matrix:
rows: 3
cols: 3
data: [ -1.1252198674164329e-05, -7.2394856860325228e-03,
7.9060664179560138e-04, 6.5370869431856798e-03,
-3.9352294745729046e-04, 1.1948346038335741e-01,
-9.2109737277881536e-04, -1.1944234402152067e-01,
-3.9230197564821978e-04 ]
fundamental_matrix:
rows: 3
cols: 3
data: [ -2.5309754099766013e-08, -1.6300536361959590e-05,
4.9995535977749410e-03, 1.4720205166478256e-05,
-8.8704021902692202e-07, 1.3677019067204871e-01,
-4.6630670977502930e-03, -1.3776019369349130e-01, 1. ]
rtabmap_odom/test/data/stereo/raw/stereo_right.yaml
+34
View File
@@ -0,0 +1,34 @@
#%YAML:1.0
camera_name: stereo_tutorial_right
image_width: 640
image_height: 480
camera_matrix:
rows: 3
cols: 3
data: [ 5.3173015723615038e+02, 0., 3.0841183762414585e+02, 0.,
5.3114393189729469e+02, 2.4247036103612575e+02, 0., 0., 1. ]
distortion_coefficients:
rows: 1
cols: 5
data: [ -3.2382605112765667e-01, 4.3067676369775459e-02,
-1.0212741406694578e-03, 5.1816193644504472e-04,
9.9965184406768520e-02 ]
distortion_model: plumb_bob
rectification_matrix:
rows: 3
cols: 3
data: [ 9.9814647006952373e-01, -6.8031680948065307e-03,
-6.0475955483342586e-02, 6.7037039320888168e-03,
9.9997582338248303e-01, -1.8474317621778344e-03,
6.0487061768119681e-02, 1.4385945915416094e-03,
9.9816794468879888e-01 ]
projection_matrix:
rows: 3
cols: 4
data: [ 5.0021545003868783e+02, 0., 3.5945997238159180e+02,
-5.9858566522579160e+01, 0., 5.0021545003868783e+02,
2.4750019836425781e+02, 0., 0., 0., 1., 0. ]
local_transform:
rows: 3
cols: 4
data: [ 0., 0., 1., 0., -1., 0., 0., 0., 0., -1., 0., 0. ]
rtabmap_odom/test/data/stereo/rect/left/50.jpg
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 67 KiB

rtabmap_odom/test/data/stereo/rect/left/60.jpg
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 66 KiB

rtabmap_odom/test/data/stereo/rect/right/50.jpg
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 65 KiB

rtabmap_odom/test/data/stereo/rect/right/60.jpg
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 64 KiB

rtabmap_odom/test/data/stereo/rect/stereo_left.yaml
+24
View File
@@ -0,0 +1,24 @@
#%YAML:1.0
camera_name: stereo_left
image_width: 640
image_height: 480
camera_matrix:
rows: 3
cols: 3
data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02, 0.,
4.8760873413085938e+02, 2.4944424438476562e+02, 0., 0., 1. ]
distortion_coefficients:
rows: 1
cols: 5
data: [ 0., 0., 0., 0., 0. ]
distortion_model: plumb_bob
rectification_matrix:
rows: 3
cols: 3
data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
projection_matrix:
rows: 3
cols: 4
data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02, 0., 0.,
4.8760873413085938e+02, 2.4944424438476562e+02, 0., 0., 0., 1.,
0. ]
rtabmap_odom/test/data/stereo/rect/stereo_right.yaml
+24
View File
@@ -0,0 +1,24 @@
#%YAML:1.0
camera_name: stereo_right
image_width: 640
image_height: 480
camera_matrix:
rows: 3
cols: 3
data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02, 0.,
4.8760873413085938e+02, 2.4944424438476562e+02, 0., 0., 1. ]
distortion_coefficients:
rows: 1
cols: 5
data: [ 0., 0., 0., 0., 0. ]
distortion_model: plumb_bob
rectification_matrix:
rows: 3
cols: 3
data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
projection_matrix:
rows: 3
cols: 4
data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02,
-5.8362700032946350e+01, 0., 4.8760873413085938e+02,
2.4944424438476562e+02, 0., 0., 0., 1., 0. ]
rtabmap_odom/test/msg_builders.hpp
+362
View File
@@ -0,0 +1,362 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#ifndef RTABMAP_ODOM_MSG_BUILDERS_HPP_
#define RTABMAP_ODOM_MSG_BUILDERS_HPP_
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rtabmap_msgs/msg/odom_info.hpp>
#include <rtabmap_msgs/msg/rgbd_image.hpp>
#include <rtabmap_msgs/msg/scan_descriptor.hpp>
#include <rtabmap_msgs/msg/sensor_data.hpp>
#include <rtabmap_msgs/msg/user_data.hpp>
#include <opencv2/core/core.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <string>
#include <vector>
namespace rtabmap_odom_test {
/// A ROS time from a double, the way sensor stamps are written throughout these tests.
inline rclcpp::Time stampOf(double seconds)
{
return rclcpp::Time(
int32_t(seconds), uint32_t((seconds - int32_t(seconds)) * 1e9), RCL_ROS_TIME);
}
/// A rectified pinhole CameraInfo; @p tx is P(0,3), non-zero for a stereo right camera.
inline sensor_msgs::msg::CameraInfo makeCameraInfo(
const std::string & frameId, double stamp, int width = 8, int height = 8,
double tx = 0.0, double fx = 100.0)
{
sensor_msgs::msg::CameraInfo info;
info.header.frame_id = frameId;
info.header.stamp = stampOf(stamp);
info.width = width;
info.height = height;
info.distortion_model = "plumb_bob";
info.d = {0.0, 0.0, 0.0, 0.0, 0.0};
info.k = {fx, 0.0, width/2.0, 0.0, fx, height/2.0, 0.0, 0.0, 1.0};
info.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
info.p = {fx, 0.0, width/2.0, tx, 0.0, fx, height/2.0, 0.0, 0.0, 0.0, 1.0, 0.0};
return info;
}
inline sensor_msgs::msg::Image makeImage(
const std::string & frameId, double stamp,
const cv::Mat & image, const std::string & encoding)
{
std_msgs::msg::Header header;
header.frame_id = frameId;
header.stamp = stampOf(stamp);
sensor_msgs::msg::Image msg;
cv_bridge::CvImage(header, encoding, image).toImageMsg(msg);
return msg;
}
/// A bgr8 color image of a single flat color.
inline sensor_msgs::msg::Image makeRgbImage(
const std::string & frameId, double stamp, int width = 8, int height = 8,
const cv::Scalar & color = cv::Scalar(10, 20, 30))
{
return makeImage(frameId, stamp, cv::Mat(height, width, CV_8UC3, color), "bgr8");
}
/// A 16UC1 depth image in millimeters, the encoding the RGB-D drivers publish.
inline sensor_msgs::msg::Image makeDepthImage(
const std::string & frameId, double stamp, int width = 8, int height = 8,
uint16_t millimeters = 1500)
{
return makeImage(frameId, stamp,
cv::Mat(height, width, CV_16UC1, cv::Scalar(millimeters)), "16UC1");
}
/// A mono8 image, used as a stereo left or right frame.
inline sensor_msgs::msg::Image makeMonoImage(
const std::string & frameId, double stamp, int width = 8, int height = 8,
uint8_t value = 60)
{
return makeImage(frameId, stamp,
cv::Mat(height, width, CV_8UC1, cv::Scalar(value)), "mono8");
}
/// An RGB-D message with raw bgr8 color and 16UC1 depth, as rgbd_sync publishes it.
inline rtabmap_msgs::msg::RGBDImage makeRGBDImage(
const std::string & frameId, double stamp, int width = 8, int height = 8,
const cv::Scalar & rgbColor = cv::Scalar(10, 20, 30), uint16_t depthValue = 1500)
{
rtabmap_msgs::msg::RGBDImage msg;
msg.header.frame_id = frameId;
msg.header.stamp = stampOf(stamp);
msg.rgb = makeRgbImage(frameId, stamp, width, height, rgbColor);
msg.depth = makeDepthImage(frameId, stamp, width, height, depthValue);
msg.rgb_camera_info = makeCameraInfo(frameId, stamp, width, height);
msg.depth_camera_info = makeCameraInfo(frameId, stamp, width, height);
return msg;
}
/// A flat LaserScan of @p count equal ranges over 180 degrees.
inline sensor_msgs::msg::LaserScan makeLaserScan(
const std::string & frameId, double stamp, size_t count = 10, float range = 2.0f)
{
sensor_msgs::msg::LaserScan scan;
scan.header.frame_id = frameId;
scan.header.stamp = stampOf(stamp);
scan.angle_min = -M_PI_2;
scan.angle_max = M_PI_2;
scan.angle_increment = count > 1 ? float(M_PI / double(count - 1)) : float(M_PI);
scan.time_increment = 0.0f;
scan.scan_time = 0.1f;
scan.range_min = 0.1f;
scan.range_max = 10.0f;
scan.ranges.assign(count, range);
return scan;
}
/// A dense unorganized XYZ float cloud, the shape a 3D lidar driver publishes.
inline sensor_msgs::msg::PointCloud2 makeXYZCloud(
const std::string & frameId, double stamp,
const std::vector<cv::Point3f> & points)
{
sensor_msgs::msg::PointCloud2 cloud;
cloud.header.frame_id = frameId;
cloud.header.stamp = stampOf(stamp);
cloud.height = 1;
cloud.width = points.size();
cloud.is_bigendian = false;
cloud.is_dense = true;
cloud.fields.resize(3);
const char * names[3] = {"x", "y", "z"};
for(int i=0; i<3; ++i)
{
cloud.fields[i].name = names[i];
cloud.fields[i].offset = 4 * i;
cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
cloud.fields[i].count = 1;
}
cloud.point_step = 12;
cloud.row_step = cloud.point_step * cloud.width;
cloud.data.resize(cloud.row_step * cloud.height);
for(size_t i=0; i<points.size(); ++i)
{
float * p = reinterpret_cast<float *>(&cloud.data[i * cloud.point_step]);
p[0] = points[i].x;
p[1] = points[i].y;
p[2] = points[i].z;
}
return cloud;
}
/**
* @brief An XYZ cloud carrying the optional fields a 3D lidar may add.
*
* `intensity` and the `normal_*`/`curvature` group each change which PCL point type the
* odometry converts the cloud into, so a driver that sends them takes a different path
* through the node than one that sends plain XYZ. `t` is the per-point offset from the
* header stamp that deskewing needs, spread evenly over @p sweep seconds.
*/
inline sensor_msgs::msg::PointCloud2 makeCloudWithFields(
const std::string & frameId, double stamp,
const std::vector<cv::Point3f> & points,
bool withIntensity, bool withNormals,
const cv::Point3f & normal = cv::Point3f(0, 0, 1),
bool withTime = false, float sweep = 0.01f)
{
sensor_msgs::msg::PointCloud2 cloud;
cloud.header.frame_id = frameId;
cloud.header.stamp = stampOf(stamp);
cloud.height = 1;
cloud.width = points.size();
cloud.is_bigendian = false;
cloud.is_dense = true;
std::vector<std::string> names = {"x", "y", "z"};
if(withIntensity)
{
names.push_back("intensity");
}
if(withNormals)
{
names.push_back("normal_x");
names.push_back("normal_y");
names.push_back("normal_z");
names.push_back("curvature");
}
if(withTime)
{
names.push_back("t");
}
cloud.fields.resize(names.size());
for(size_t i=0; i<names.size(); ++i)
{
cloud.fields[i].name = names[i];
cloud.fields[i].offset = uint32_t(4 * i);
cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
cloud.fields[i].count = 1;
}
cloud.point_step = uint32_t(4 * names.size());
cloud.row_step = cloud.point_step * cloud.width;
cloud.data.resize(size_t(cloud.row_step) * cloud.height);
for(size_t i=0; i<points.size(); ++i)
{
float * p = reinterpret_cast<float *>(&cloud.data[i * cloud.point_step]);
size_t f = 0;
p[f++] = points[i].x;
p[f++] = points[i].y;
p[f++] = points[i].z;
if(withIntensity)
{
p[f++] = float(i % 256);
}
if(withNormals)
{
p[f++] = normal.x;
p[f++] = normal.y;
p[f++] = normal.z;
p[f++] = 0.0f; // curvature
}
if(withTime)
{
p[f++] = points.size() > 1 ?
sweep * float(i) / float(points.size() - 1) : 0.0f;
}
}
return cloud;
}
/// A small cloud on a line, enough to tell one scan from another.
inline sensor_msgs::msg::PointCloud2 makeScanCloud(
const std::string & frameId, double stamp, size_t count = 4)
{
std::vector<cv::Point3f> points;
points.reserve(count);
for(size_t i=0; i<count; ++i)
{
points.push_back(cv::Point3f(1.0f + float(i), 0.0f, 0.0f));
}
return makeXYZCloud(frameId, stamp, points);
}
/// A ScanDescriptor carrying a 2D scan, a 3D scan, or both, and optionally a descriptor.
inline rtabmap_msgs::msg::ScanDescriptor makeScanDescriptor(
const std::string & frameId, double stamp,
bool with2d = true, bool with3d = false, bool withGlobalDescriptor = false)
{
rtabmap_msgs::msg::ScanDescriptor msg;
msg.header.frame_id = frameId;
msg.header.stamp = stampOf(stamp);
if(with2d)
{
msg.scan = makeLaserScan(frameId, stamp);
}
if(with3d)
{
msg.scan_cloud = makeScanCloud(frameId, stamp);
}
if(withGlobalDescriptor)
{
// Only "not empty" matters here: consumers pass the payload straight to
// RTAB-Map, which is what knows how to decode it.
msg.global_descriptor.header = msg.header;
msg.global_descriptor.data = {1, 2, 3, 4};
}
return msg;
}
/// An identity-pose odometry message at @p x meters along the x axis.
inline nav_msgs::msg::Odometry makeOdometry(
const std::string & frameId, double stamp, double x = 0.0,
const std::string & childFrameId = "base_link")
{
nav_msgs::msg::Odometry msg;
msg.header.frame_id = frameId;
msg.header.stamp = stampOf(stamp);
msg.child_frame_id = childFrameId;
msg.pose.pose.position.x = x;
msg.pose.pose.orientation.w = 1.0;
return msg;
}
inline rtabmap_msgs::msg::OdomInfo makeOdomInfo(
const std::string & frameId, double stamp, int inliers = 50)
{
rtabmap_msgs::msg::OdomInfo msg;
msg.header.frame_id = frameId;
msg.header.stamp = stampOf(stamp);
msg.inliers = inliers;
msg.matches = inliers;
return msg;
}
/// A SensorData carrying one RGB-D camera, as rtabmap_odom republishes it.
inline rtabmap_msgs::msg::SensorData makeSensorData(
const std::string & frameId, double stamp, int width = 8, int height = 8)
{
rtabmap_msgs::msg::SensorData msg;
msg.header.frame_id = frameId;
msg.header.stamp = stampOf(stamp);
msg.left = makeRgbImage(frameId, stamp, width, height);
msg.right = makeDepthImage(frameId, stamp, width, height);
msg.left_camera_info.push_back(makeCameraInfo(frameId, stamp, width, height));
msg.right_camera_info.push_back(makeCameraInfo(frameId, stamp, width, height));
geometry_msgs::msg::Transform localTransform;
localTransform.rotation.w = 1.0;
msg.local_transform.push_back(localTransform);
return msg;
}
/// An uncompressed user data matrix (several rows, so it is not taken as compressed).
inline rtabmap_msgs::msg::UserData makeUserData(
const std::string & frameId, double stamp)
{
rtabmap_msgs::msg::UserData msg;
msg.header.frame_id = frameId;
msg.header.stamp = stampOf(stamp);
msg.rows = 2;
msg.cols = 2;
msg.type = CV_8UC1;
msg.data = {1, 2, 3, 4};
return msg;
}
/**
* @brief Reads the x/y/z of a point from any FLOAT32 xyz cloud.
*
* Looks the offsets up in the field list rather than assuming they are 0/4/8.
*/
inline cv::Point3f readXYZ(const sensor_msgs::msg::PointCloud2 & cloud, size_t index)
{
uint32_t xOffset = 0, yOffset = 4, zOffset = 8;
for(size_t i=0; i<cloud.fields.size(); ++i)
{
if(cloud.fields[i].name == "x") { xOffset = cloud.fields[i].offset; }
else if(cloud.fields[i].name == "y") { yOffset = cloud.fields[i].offset; }
else if(cloud.fields[i].name == "z") { zOffset = cloud.fields[i].offset; }
}
const unsigned char * base = &cloud.data[index * cloud.point_step];
return cv::Point3f(
*reinterpret_cast<const float *>(base + xOffset),
*reinterpret_cast<const float *>(base + yOffset),
*reinterpret_cast<const float *>(base + zOffset));
}
} // namespace rtabmap_odom_test
#endif /* RTABMAP_ODOM_MSG_BUILDERS_HPP_ */
rtabmap_odom/test/node_test_utils.hpp
+256
View File
@@ -0,0 +1,256 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef RTABMAP_ODOM_NODE_TEST_UTILS_HPP_
#define RTABMAP_ODOM_NODE_TEST_UTILS_HPP_
#include <gtest/gtest.h>
#include <rclcpp/rclcpp.hpp>
#include <chrono>
#include <functional>
#include <memory>
#include <stdexcept>
#include <string>
#include <vector>
namespace rtabmap_odom_test {
/**
* @brief Brings rclcpp up once for the whole test binary.
*
* Registered as a gtest global environment so it runs before the first test and shuts
* down after the last one, which keeps gtest_main usable.
*/
class RclcppEnvironment : public ::testing::Environment
{
public:
void SetUp() override
{
if(!rclcpp::ok())
{
rclcpp::init(0, nullptr);
}
}
void TearDown() override
{
if(rclcpp::ok())
{
rclcpp::shutdown();
}
}
};
/// Registers RclcppEnvironment. Call once at file scope in each test binary.
inline ::testing::Environment * registerRclcppEnvironment()
{
static ::testing::Environment * const env =
::testing::AddGlobalTestEnvironment(new RclcppEnvironment);
return env;
}
/**
* @brief Base fixture for driving a node under test over real ROS topics.
*
* The node under test and a helper node share one single-threaded executor, so
* publishing, the node's callback and the assertion all happen on the same thread and
* the tests stay deterministic. No launch files and no separate processes are involved:
* everything runs in the gtest binary.
*/
class NodeTest : public ::testing::Test
{
protected:
void SetUp() override
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
helper_ = std::make_shared<rclcpp::Node>("rtabmap_odom_test_helper");
executor_->add_node(helper_);
}
void TearDown() override
{
for(const rclcpp::Node::SharedPtr & node : nodes_)
{
executor_->remove_node(node);
}
nodes_.clear();
executor_->remove_node(helper_);
helper_.reset();
executor_.reset();
}
/**
* @brief Adds a node under test to the shared executor and keeps it alive for the test.
*
* The wait is for tf2_ros, not for anything the test does with the node.
* ~TransformListener cancels its worker's executor and joins it without ordering the
* cancel after the worker reached spin(), so a node dropped microseconds after it was
* built -- which a test that only reads a parameter back does -- hangs the binary for
* good (ros2/geometry2#517). The window is a few instructions wide and nothing here
* can observe that thread, so this buys time instead. Drop it once #752 lands.
*/
template <typename NodeT>
std::shared_ptr<NodeT> addNode(const std::shared_ptr<NodeT> & node)
{
executor_->add_node(node);
nodes_.push_back(node);
spinFor(std::chrono::milliseconds(50));
return node;
}
/// The helper node, used to publish inputs and subscribe to outputs.
rclcpp::Node::SharedPtr helper() { return helper_; }
/**
* @brief Spins until @p done returns true, or the timeout elapses.
* @return true if @p done became true
*/
bool spinUntil(
const std::function<bool()> & done,
std::chrono::milliseconds timeout = std::chrono::milliseconds(5000))
{
const std::chrono::steady_clock::time_point deadline =
std::chrono::steady_clock::now() + timeout;
while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
{
if(done())
{
return true;
}
executor_->spin_once(std::chrono::milliseconds(10));
}
return done();
}
/// Spins for a fixed duration, for the "nothing should happen" assertions.
void spinFor(std::chrono::milliseconds duration)
{
const std::chrono::steady_clock::time_point deadline =
std::chrono::steady_clock::now() + duration;
while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
{
executor_->spin_once(std::chrono::milliseconds(10));
}
}
/**
* @brief Waits until @p publisher has at least @p count matched subscriptions.
*
* Publishing before the node under test has discovered the topic silently drops the
* message, which is the most common cause of a flaky in-process node test.
*/
template <typename PublisherT>
bool waitForSubscriber(const PublisherT & publisher, size_t count = 1)
{
return spinUntil([&]() { return publisher->get_subscription_count() >= count; });
}
/**
* @brief Waits until @p subscription sees at least one publisher.
*
* Every node here publishes only when it has subscribers, so the test's subscription
* has to be discovered before the input is sent.
*/
template <typename SubscriptionT>
bool waitForPublisher(const SubscriptionT & subscription, size_t count = 1)
{
return spinUntil([&]() { return subscription->get_publisher_count() >= count; });
}
/// Collects every message received on @p topic, for later assertions.
template <typename MsgT>
struct Collector
{
typename rclcpp::Subscription<MsgT>::SharedPtr subscription;
std::vector<typename MsgT::ConstSharedPtr> messages;
size_t size() const { return messages.size(); }
bool empty() const { return messages.empty(); }
/**
* @brief The last (first) message received.
*
* A test that reads these without having waited for the topic it is reading --
* having waited for a different one, say -- gets a legible failure rather than a
* segmentation fault: std::vector::back() on an empty vector dereferences
* nullptr-1, which crashes the whole binary and takes the rest of its tests with
* it. gtest turns the exception into a failure of the test that threw it.
*/
const MsgT & back() const { return *checked(messages.empty()?0:&messages.back()); }
const MsgT & front() const { return *checked(messages.empty()?0:&messages.front()); }
private:
const typename MsgT::ConstSharedPtr & checked(
const typename MsgT::ConstSharedPtr * msg) const
{
if(msg == 0)
{
throw std::out_of_range(
std::string("nothing was received on \"") +
(subscription?subscription->get_topic_name():"?") +
"\", so there is no message to read: wait for it to arrive first");
}
return *msg;
}
};
/**
* @brief Subscribes the helper node to @p topic and records everything it receives.
*
* The callback holds the collector weakly. Capturing it by shared_ptr would close a
* cycle -- collector owns the subscription, the subscription owns the callback, the
* callback owns the collector -- and neither would ever be freed. A subscription that
* outlives its test keeps the helper node's rcl handle alive with it, which leaves the
* node's rosout publisher registered and greets the next test with "Publisher already
* registered for node name: 'rtabmap_odom_test_helper'".
*/
template <typename MsgT>
std::shared_ptr<Collector<MsgT>> collect(
const std::string & topic, const rclcpp::QoS & qos = rclcpp::QoS(10))
{
std::shared_ptr<Collector<MsgT>> collector = std::make_shared<Collector<MsgT>>();
std::weak_ptr<Collector<MsgT>> weak = collector;
collector->subscription = helper_->create_subscription<MsgT>(
topic, qos,
[weak](const typename MsgT::ConstSharedPtr msg) {
if(std::shared_ptr<Collector<MsgT>> collector = weak.lock())
{
collector->messages.push_back(msg);
}
});
return collector;
}
private:
rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
rclcpp::Node::SharedPtr helper_;
std::vector<rclcpp::Node::SharedPtr> nodes_;
};
} // namespace rtabmap_odom_test
#endif /* RTABMAP_ODOM_NODE_TEST_UTILS_HPP_ */
rtabmap_odom/test/scan_scenes.hpp
+115
View File
@@ -0,0 +1,115 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#ifndef RTABMAP_ODOM_SCAN_SCENES_HPP_
#define RTABMAP_ODOM_SCAN_SCENES_HPP_
#include <opencv2/core/core.hpp>
#include <cmath>
#include <rclcpp/rclcpp.hpp>
#include <vector>
namespace rtabmap_odom_test {
/**
* A 3D corner -- floor plus two walls -- so all six degrees of freedom are constrained.
*
* Ported from makeCorner3D() in RTAB-Map's corelib/test/test_odometry.cpp. The jitter is
* not decoration: a perfectly flat lattice gives degenerate per-point normals and ICP
* finds no correspondences at all.
*/
inline std::vector<cv::Point3f> corner3D(
const cv::Point3f & offset = cv::Point3f(0,0,0),
float length = 4.0f, int pointsPerSurface = 400, uint64_t seed = 0xC0FFEE)
{
cv::RNG rng(seed);
const float half = 0.5f * length;
std::vector<cv::Point3f> points;
points.reserve(3 * pointsPerSurface);
for(int i=0; i<pointsPerSurface; ++i) // floor z=-half
{
points.push_back(cv::Point3f(rng.uniform(-half, half), rng.uniform(-half, half),
-half + float(rng.gaussian(0.005))) - offset);
}
for(int i=0; i<pointsPerSurface; ++i) // wall x=-half
{
points.push_back(cv::Point3f(-half + float(rng.gaussian(0.005)),
rng.uniform(-half, half), rng.uniform(-half, half)) - offset);
}
for(int i=0; i<pointsPerSurface; ++i) // wall y=-half
{
points.push_back(cv::Point3f(rng.uniform(-half, half),
-half + float(rng.gaussian(0.005)), rng.uniform(-half, half)) - offset);
}
return points;
}
/**
* @brief The same corner seen after the sensor has turned @p yaw about z.
*
* The corner does not move; the sensor does, so in the sensor's own frame every point
* turns the other way. This is what a scan taken after a rotation looks like.
*/
inline std::vector<cv::Point3f> corner3DTurned(double yaw)
{
const double c = std::cos(-yaw);
const double s = std::sin(-yaw);
std::vector<cv::Point3f> points = corner3D();
for(cv::Point3f & p : points)
{
const float x = p.x;
p.x = float(c * x - s * p.y);
p.y = float(s * x + c * p.y);
}
return points;
}
/**
* @brief Range to a 2D corner from a sensor at (@p sensorX, @p sensorY) looking along +x.
*
* Two perpendicular walls, one ahead and one to the left. A single wall would leave the
* motion along it unobservable and ICP would settle wherever it started; the corner pins
* both axes and the heading.
*
* @return the nearer wall along the ray, or 0 if the ray reaches neither
*/
inline float corner2DRange(
double sensorX, double sensorY, double angle,
float frontWall = 5.0f, float leftWall = 3.0f)
{
const double dx = std::cos(angle);
const double dy = std::sin(angle);
double best = 0.0;
if(dx > 1e-6)
{
best = (frontWall - sensorX) / dx;
}
if(dy > 1e-6)
{
const double toLeft = (leftWall - sensorY) / dy;
best = (best <= 0.0 || toLeft < best) ? toLeft : best;
}
return float(best);
}
/**
* The ICP settings RTAB-Map's own odometry tests use for this scene: point-to-point, no
* voxelization, and a correspondence ratio low enough for a synthetic scan.
*/
inline std::vector<rclcpp::Parameter> icpTestParameters()
{
return {
rclcpp::Parameter("Icp/PointToPlane", "false"),
rclcpp::Parameter("scan_voxel_size", 0.0),
rclcpp::Parameter("Icp/CorrespondenceRatio", "0.1"),
};
}
} // namespace rtabmap_odom_test
#endif /* RTABMAP_ODOM_SCAN_SCENES_HPP_ */
rtabmap_odom/test/test_data.hpp
+268
View File
@@ -0,0 +1,268 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#ifndef RTABMAP_ODOM_TEST_DATA_HPP_
#define RTABMAP_ODOM_TEST_DATA_HPP_
#include <geometry_msgs/msg/transform.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <tf2/LinearMath/Matrix3x3.hpp>
#include <tf2/LinearMath/Quaternion.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include "msg_builders.hpp"
/**
* @file
* @brief Real frames for the odometry node tests, from test/data.
*
* Visual odometry needs imagery it can actually track: synthetic noise gives a detector
* corners that match nothing between frames, so a generated "motion" tells us only that
* the node did not crash. These loaders hand the tests the same stereo pairs and RGB-D
* frames that RTAB-Map registers in corelib/test/test_odometry.cpp, which lets a ROS test
* assert that a plausible transform came out the other end.
*
* See test/data/README.md for where the files come from and why they are vendored.
*/
namespace rtabmap_odom_test {
/// Root of the vendored fixtures, set by CMake to test/data in the source tree.
inline std::string testDataRoot()
{
return std::string(RTABMAP_ODOM_TEST_DATA_ROOT);
}
/**
* @brief Opens a ROS camera calibration file with OpenCV's parser.
*
* The files are the ROS flavour: the format line is commented out (`#%YAML:1.0`) so that
* a plain YAML parser -- camera_info_manager, rosparam, PyYAML -- accepts them, since
* `%YAML:1.0` is not a valid YAML directive and makes those parsers fail. cv::FileStorage
* wants the directive, so it is put back here, in memory, and the file on disk stays
* loadable by both.
*
* @return a closed FileStorage if the file is missing
*/
inline cv::FileStorage openCalibration(const std::string & path)
{
std::ifstream file(path.c_str());
if(!file.is_open())
{
return cv::FileStorage();
}
std::ostringstream buffer;
buffer << file.rdbuf();
std::string text = buffer.str();
const std::string rosHeader = "#%YAML:1.0";
if(text.compare(0, rosHeader.size(), rosHeader) == 0)
{
text = "%YAML:1.0\n---" + text.substr(rosHeader.size());
}
return cv::FileStorage(text, cv::FileStorage::READ | cv::FileStorage::MEMORY);
}
/**
* @brief Reads a rows/cols/data matrix from a calibration file node.
*
* ROS calibration files have no OpenCV `dt` field, so `>> cv::Mat` cannot read them;
* the elements are taken one by one instead, as RTAB-Map's CameraModel::load does.
*
* @return an empty vector if the node is missing or malformed
*/
inline std::vector<double> readCalibrationMatrix(
cv::FileStorage & fs, const std::string & name, int rows, int cols)
{
const cv::FileNode node = fs[name];
if(node.empty())
{
return std::vector<double>();
}
std::vector<double> data;
node["data"] >> data;
if((int)node["rows"] != rows || (int)node["cols"] != cols ||
data.size() != size_t(rows * cols))
{
return std::vector<double>();
}
return data;
}
/**
* @brief A CameraInfo from a ROS calibration file.
*
* The RGB-D files carry only camera_matrix, so R falls back to identity and P to [K|0] --
* which is what a driver publishes for an already-rectified monocular camera anyway.
*/
inline sensor_msgs::msg::CameraInfo cameraInfoFromCalibration(
const std::string & path, const std::string & frameId, double stamp)
{
sensor_msgs::msg::CameraInfo info;
info.header.frame_id = frameId;
info.header.stamp = stampOf(stamp);
cv::FileStorage fs = openCalibration(path);
if(!fs.isOpened())
{
return info;
}
info.width = uint32_t((int)fs["image_width"]);
info.height = uint32_t((int)fs["image_height"]);
info.distortion_model = fs["distortion_model"].isString() ?
(std::string)fs["distortion_model"] : std::string("plumb_bob");
const std::vector<double> k = readCalibrationMatrix(fs, "camera_matrix", 3, 3);
const std::vector<double> r = readCalibrationMatrix(fs, "rectification_matrix", 3, 3);
const std::vector<double> p = readCalibrationMatrix(fs, "projection_matrix", 3, 4);
const std::vector<double> d = readCalibrationMatrix(fs, "distortion_coefficients", 1, 5);
info.d.assign(5, 0.0);
for(size_t i=0; i<d.size() && i<info.d.size(); ++i)
{
info.d[i] = d[i];
}
for(size_t i=0; i<9; ++i)
{
info.k[i] = k.empty() ? 0.0 : k[i];
info.r[i] = r.empty() ? (i%4 == 0 ? 1.0 : 0.0) : r[i];
}
for(size_t i=0; i<12; ++i)
{
info.p[i] = p.empty() ? (i%4 == 3 ? 0.0 : info.k[(i/4)*3 + i%4]) : p[i];
}
return info;
}
// ---------------------------------------------------------------------------
// Stereo pairs: data/stereo, from one 640x480 rig at 20 Hz.
//
// "rect" holds rectified pairs (frames 50 and 60), what the nodes expect on
// left/image_rect by default. "raw" holds pairs straight off the camera (frames 420 and
// 425), for the Rtabmap/ImagesAlreadyRectified path.
//
// Each set carries the calibration that describes it, and the two are not
// interchangeable: the rectified one has no distortion and an identity rectification
// matrix, because that is what rectification leaves behind, while the raw one carries the
// lens's real plumb_bob coefficients and the rotation into the rectified frame. Handing
// the rectified calibration to a raw pair claims a distortion-free lens the images do not
// have.
// ---------------------------------------------------------------------------
/// "rect" or "raw"; a rectified pair unless a test says otherwise.
enum StereoSet { kRectified, kRaw };
inline std::string stereoSetDir(StereoSet set)
{
return testDataRoot() + (set == kRaw ? "/stereo/raw" : "/stereo/rect");
}
/// The left image of a pair, in color as a stereo driver publishes it (bgr8).
inline cv::Mat stereoLeftImage(const std::string & name, StereoSet set = kRectified)
{
return cv::imread(stereoSetDir(set) + "/left/" + name + ".jpg", cv::IMREAD_COLOR);
}
/// The right image of a pair, grayscale (mono8), which is all the matcher uses.
inline cv::Mat stereoRightImage(const std::string & name, StereoSet set = kRectified)
{
return cv::imread(stereoSetDir(set) + "/right/" + name + ".jpg", cv::IMREAD_GRAYSCALE);
}
inline sensor_msgs::msg::CameraInfo stereoLeftInfo(
const std::string & frameId, double stamp, StereoSet set = kRectified)
{
return cameraInfoFromCalibration(stereoSetDir(set) + "/stereo_left.yaml", frameId, stamp);
}
/// The right camera's P(0,3) is -fx * baseline, which is where the stereo scale comes from.
inline sensor_msgs::msg::CameraInfo stereoRightInfo(
const std::string & frameId, double stamp, StereoSet set = kRectified)
{
return cameraInfoFromCalibration(stereoSetDir(set) + "/stereo_right.yaml", frameId, stamp);
}
/**
* @brief The right camera's pose in the left camera's frame, from the rig's stereo
* calibration (data/stereo/raw/stereo_pose.yaml).
*
* Stereo calibration stores the transform the other way round -- a point in left
* coordinates maps to the right camera as X_right = R * X_left + T -- so this inverts it
* into what TF wants to publish, camera_left -> camera_right. Feeding it to TF is what
* lets Rtabmap/ImagesAlreadyRectified:=false rectify the pair against the rig's real
* extrinsics, small inter-camera rotation included, rather than an assumed ideal baseline.
*
* @return an identity transform if the file is missing
*/
inline geometry_msgs::msg::Transform stereoRightInLeftFrame()
{
geometry_msgs::msg::Transform transform;
transform.rotation.w = 1.0;
cv::FileStorage fs = openCalibration(stereoSetDir(kRaw) + "/stereo_pose.yaml");
if(!fs.isOpened())
{
return transform;
}
const std::vector<double> r = readCalibrationMatrix(fs, "rotation_matrix", 3, 3);
const std::vector<double> t = readCalibrationMatrix(fs, "translation_matrix", 3, 1);
if(r.empty() || t.empty())
{
return transform;
}
// (R, T) -> (R', -R'T)
const tf2::Matrix3x3 rotation(
r[0], r[1], r[2],
r[3], r[4], r[5],
r[6], r[7], r[8]);
const tf2::Matrix3x3 inverse = rotation.transpose();
const tf2::Vector3 translation = inverse * -tf2::Vector3(t[0], t[1], t[2]);
tf2::Quaternion q;
inverse.getRotation(q);
transform.rotation.x = q.x();
transform.rotation.y = q.y();
transform.rotation.z = q.z();
transform.rotation.w = q.w();
transform.translation.x = translation.x();
transform.translation.y = translation.y();
transform.translation.z = translation.z();
return transform;
}
// ---------------------------------------------------------------------------
// RGB-D frames: data/rgbd, frames "17" and "154".
// ---------------------------------------------------------------------------
inline cv::Mat rgbdColorImage(const std::string & name)
{
return cv::imread(testDataRoot() + "/rgbd/rgb/" + name + ".jpg", cv::IMREAD_COLOR);
}
/// 16-bit millimetres, the encoding the RGB-D drivers publish (16UC1).
inline cv::Mat rgbdDepthImage(const std::string & name)
{
return cv::imread(testDataRoot() + "/rgbd/depth/" + name + ".png", cv::IMREAD_UNCHANGED);
}
inline sensor_msgs::msg::CameraInfo rgbdInfo(
const std::string & name, const std::string & frameId, double stamp)
{
return cameraInfoFromCalibration(
testDataRoot() + "/rgbd/calib/" + name + ".yaml", frameId, stamp);
}
} // namespace rtabmap_odom_test
#endif /* RTABMAP_ODOM_TEST_DATA_HPP_ */
rtabmap_odom/test/test_icp_odometry.cpp
+2103
View File
File diff suppressed because it is too large Load Diff
rtabmap_odom/test/test_odometry_ros.cpp
+1350
View File
File diff suppressed because it is too large Load Diff
rtabmap_odom/test/test_rgbd_odometry.cpp
+800
View File
@@ -0,0 +1,800 @@
/*
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#include <gtest/gtest.h>
#include <tf2_ros/static_transform_broadcaster.hpp>
#include <std_srvs/srv/empty.hpp>
#include <rtabmap_msgs/msg/odom_info.hpp>
#include <rtabmap_msgs/msg/rgbd_images.hpp>
#include <rtabmap_odom/rgbd_odometry.hpp>
#include <cmath>
#include <rtabmap/core/Version.h>
#include "camera_rig.hpp"
#include "msg_builders.hpp"
#include "node_test_utils.hpp"
#include "test_data.hpp"
namespace rtabmap_odom_test {
namespace {
::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
/**
* The frames that carry a scene come from test/data/rgbd -- the same two RGB-D frames
* RTAB-Map registers in corelib/test/test_odometry.cpp. These tests assert the ROS-level
* contract (which topics are subscribed, what is published, how the parameters wire up)
* on real input rather than the accuracy of the registration, which is that test's
* business.
*/
const char * const kFrame = "17";
const char * const kLaterFrame = "154"; // much further along the sequence
/// The blank scene the "lost tracking" tests need is synthetic: there is nothing to see in it.
const int kBlankWidth = 160;
const int kBlankHeight = 120;
double translationNorm(const nav_msgs::msg::Odometry & odom)
{
const geometry_msgs::msg::Point & p = odom.pose.pose.position;
return std::sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
}
/// The angle of the pose's rotation, in radians.
double rotationAngle(const nav_msgs::msg::Odometry & odom)
{
const geometry_msgs::msg::Quaternion & q = odom.pose.pose.orientation;
return 2.0 * std::acos(std::min(1.0, std::fabs(q.w)));
}
/// rtabmap_odom marks a pose it does not trust with a 9999 covariance rather than staying silent.
bool isLost(const nav_msgs::msg::Odometry & odom)
{
return odom.pose.covariance[0] >= 9999.0;
}
class RgbdOdometryTest : public NodeTest
{
protected:
void publishSensorTf()
{
staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
geometry_msgs::msg::TransformStamped tf;
tf.header.stamp = helper()->now();
tf.header.frame_id = "base_link";
tf.child_frame_id = "camera";
tf.transform.rotation.w = 1.0;
staticTf_->sendTransform(tf);
}
/**
* @brief Waits until the node under test has subscribed to /tf_static.
*
* The fixture sends the sensor transform before the node exists, so the node's TF
* listener only sees it as the retained transient-local message it gets on discovery.
* Publishing an image before that arrives makes the node drop the frame after its
* 100 ms wait_for_transform, for no reason the test can see.
*/
bool waitForTfListener()
{
if(!staticTf_)
{
return true;
}
return spinUntil([&]() { return helper()->count_subscribers("/tf_static") >= 1; });
}
std::shared_ptr<rtabmap_odom::RGBDOdometry> makeNode(
std::vector<rclcpp::Parameter> params = {})
{
// Defaults first, so a test that passes the same parameter overrides them.
//
// always_process_most_recent_frame:=false is what the node itself recommends for
// data that arrives faster than its stamps: these tests publish a whole sequence
// back to back with stamps a tenth of a second apart, and when the executor is
// slow enough that two of them land in the same spin -- a loaded CI runner, a
// single core -- the node drops the second as a replay glitch and the test waits
// for a message that will never come. It also keeps processing on the calling
// thread instead of the node's worker, which is what makes these tests observable
// at all: the odometry is finished by the time the publish returns.
std::vector<rclcpp::Parameter> all = {
rclcpp::Parameter("frame_id", "base_link"),
rclcpp::Parameter("publish_tf", false),
rclcpp::Parameter("always_process_most_recent_frame", false),
};
all.insert(all.end(), params.begin(), params.end());
rclcpp::NodeOptions options;
options.parameter_overrides(all);
std::shared_ptr<rtabmap_odom::RGBDOdometry> node =
addNode(std::make_shared<rtabmap_odom::RGBDOdometry>(options));
waitForTfListener();
return node;
}
/// Calls one of the node's std_srvs/Empty services and waits for the answer.
bool callEmptyService(const std::string & name)
{
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client =
helper()->create_client<std_srvs::srv::Empty>("/rgbd_odometry/" + name);
if(!spinUntil([&]() { return client->service_is_ready(); }))
{
return false;
}
std::shared_future<std_srvs::srv::Empty::Response::SharedPtr> future =
client->async_send_request(
std::make_shared<std_srvs::srv::Empty::Request>()).future.share();
return spinUntil([&]() {
return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; });
}
/// Where each camera of a rig is mounted, as its driver would publish it once.
void publishRigTf(const CameraRig & rig)
{
staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
staticTf_->sendTransform(cameraRigTransforms(rig, helper()->now()));
}
/// One frame from test/data/rgbd, as rgbd_sync would deliver it: bgr8 plus 16UC1 millimetres.
rtabmap_msgs::msg::RGBDImage makeFrame(const std::string & name, double stamp)
{
const cv::Mat rgb = rgbdColorImage(name);
const cv::Mat depth = rgbdDepthImage(name);
EXPECT_FALSE(rgb.empty()) << "test/data/rgbd/rgb/" << name << ".jpg missing";
EXPECT_FALSE(depth.empty()) << "test/data/rgbd/depth/" << name << ".png missing";
rtabmap_msgs::msg::RGBDImage msg;
msg.header.frame_id = "camera";
msg.header.stamp = stampOf(stamp);
msg.rgb = makeImage("camera", stamp, rgb, "bgr8");
msg.depth = makeImage("camera", stamp, depth, "16UC1");
msg.rgb_camera_info = rgbdInfo(name, "camera", stamp);
msg.depth_camera_info = msg.rgb_camera_info;
return msg;
}
/// A scene with nothing in it: no features to detect, no motion to recover.
rtabmap_msgs::msg::RGBDImage makeBlankFrame(double stamp)
{
rtabmap_msgs::msg::RGBDImage msg;
msg.header.frame_id = "camera";
msg.header.stamp = stampOf(stamp);
msg.rgb = makeImage("camera", stamp,
cv::Mat::zeros(kBlankHeight, kBlankWidth, CV_8UC1), "mono8");
msg.depth = makeImage("camera", stamp,
cv::Mat(kBlankHeight, kBlankWidth, CV_32FC1, cv::Scalar(2.0f)), "32FC1");
msg.rgb_camera_info = makeCameraInfo("camera", stamp, kBlankWidth, kBlankHeight);
msg.depth_camera_info = msg.rgb_camera_info;
return msg;
}
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> staticTf_;
};
/// The vendored calibration has to survive the trip through CameraInfo, or nothing below means anything.
TEST_F(RgbdOdometryTest, the_test_calibration_describes_the_camera)
{
const sensor_msgs::msg::CameraInfo info = rgbdInfo(kFrame, "camera", 1.0);
ASSERT_EQ(640u, info.width);
ASSERT_EQ(480u, info.height);
EXPECT_DOUBLE_EQ(525.0, info.k[0]);
EXPECT_DOUBLE_EQ(525.0, info.k[4]);
// No projection_matrix in the file: P falls back to [K|0], an already-rectified camera.
EXPECT_DOUBLE_EQ(info.k[0], info.p[0]);
EXPECT_DOUBLE_EQ(0.0, info.p[3]);
}
/// By default the node takes the three raw camera topics.
TEST_F(RgbdOdometryTest, subscribes_to_the_raw_camera_topics_by_default)
{
publishSensorTf();
makeNode();
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 10);
EXPECT_TRUE(waitForSubscriber(rgb));
EXPECT_TRUE(waitForSubscriber(depth));
EXPECT_TRUE(waitForSubscriber(info));
}
/// A synchronized set of the three raw topics produces one odometry message, at the origin.
TEST_F(RgbdOdometryTest, publishes_odom_for_a_synchronized_raw_frame)
{
publishSensorTf();
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
collect<nav_msgs::msg::Odometry>("odom");
makeNode();
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 10);
ASSERT_TRUE(waitForSubscriber(rgb));
ASSERT_TRUE(waitForSubscriber(depth));
ASSERT_TRUE(waitForSubscriber(info));
// Identical stamps, so this works under either synchronization policy.
const rtabmap_msgs::msg::RGBDImage frame = makeFrame(kFrame, 1.0);
rgb->publish(frame.rgb);
depth->publish(frame.depth);
info->publish(frame.rgb_camera_info);
ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
EXPECT_EQ("odom", odom->back().header.frame_id);
EXPECT_EQ("base_link", odom->back().child_frame_id);
// The first frame has nothing to register against: it defines the origin.
EXPECT_NEAR(0.0, translationNorm(odom->back()), 1e-9);
}
/// subscribe_rgbd swaps the three topics for one pre-synchronized RGBDImage.
TEST_F(RgbdOdometryTest, subscribe_rgbd_takes_a_single_rgbd_image_topic)
{
publishSensorTf();
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
collect<nav_msgs::msg::Odometry>("odom");
makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
pub->publish(makeFrame(kFrame, 1.0));
EXPECT_TRUE(spinUntil([&]() { return !odom->empty(); }));
}
/**
* The same frame twice: the registration runs on real features and depth, and the only
* answer consistent with the input is "I have not moved". A node that mangles the depth
* units or the calibration on the way into RTAB-Map fails here, where the textureless
* scenes below cannot tell the difference.
*/
TEST_F(RgbdOdometryTest, registers_a_repeated_frame_as_no_motion)
{
publishSensorTf();
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
collect<nav_msgs::msg::Odometry>("odom");
std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(info->subscription));
pub->publish(makeFrame(kFrame, 1.0));
ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
pub->publish(makeFrame(kFrame, 1.1));
// Both collectors: odom and odom_info are published separately, and the assertions
// below compare the second of each.
ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; }));
const nav_msgs::msg::Odometry & second = odom->back();
ASSERT_FALSE(isLost(second)) << "lost tracking on a frame identical to the previous one";
EXPECT_GT(info->back().features, 20) << "no features found in a real scene";
EXPECT_GT(info->back().inliers, 20)
<< "too few inliers (matches=" << info->back().matches << ")";
// Exactly zero on this build, in both translation and rotation; a millimetre and a
// milliradian leave room for a backend that answers with rounding noise instead.
EXPECT_LT(translationNorm(second), 0.001) << "motion reported between identical frames";
EXPECT_LT(rotationAngle(second), 0.001) << "rotation reported between identical frames";
}
/**
* Frames 17 and 154 are far apart in the sequence, so losing tracking is a legitimate
* outcome; what must hold is that the node's answer agrees with itself -- either a pose
* it stands behind, of a plausible size, or one flagged as unusable.
*/
TEST_F(RgbdOdometryTest, stays_consistent_between_two_distant_frames)
{
publishSensorTf();
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
collect<nav_msgs::msg::Odometry>("odom");
makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
pub->publish(makeFrame(kFrame, 1.0));
ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
pub->publish(makeFrame(kLaterFrame, 1.1));
ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; }));
const nav_msgs::msg::Odometry & second = odom->back();
if(!isLost(second))
{
// 0.41 to 0.46 m over ten runs here. The bound stays a plausibility check rather
// than a fit: how far apart these two frames land is the registration's business,
// and this test's claim is only that the answer is not nonsense.
EXPECT_LT(translationNorm(second), 2.0)
<< "implausible jump of " << translationNorm(second) << " m";
}
}
/**
* @brief Two to six cameras, each on its own numbered topic.
*
* Above six the node has no synchronizer for it and says to use rgbd_cameras:=0 with the
* rgbd_images topic instead, so six is where this stops. Only the subscriptions are
* checked: one numbered topic per camera, none left behind.
*/
class RgbdOdometryCamerasTest :
public RgbdOdometryTest,
public ::testing::WithParamInterface<int>
{
};
TEST_P(RgbdOdometryCamerasTest, subscribes_to_one_numbered_topic_per_camera)
{
const int cameras = GetParam();
publishSensorTf();
makeNode({rclcpp::Parameter("subscribe_rgbd", true),
rclcpp::Parameter("rgbd_cameras", cameras)});
// All of them first, so they are discovered together rather than one wait after another.
std::vector<rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr> publishers;
for(int i=0; i<cameras; ++i)
{
publishers.push_back(helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
"rgbd_image" + std::to_string(i), 10));
}
for(int i=0; i<cameras; ++i)
{
EXPECT_TRUE(waitForSubscriber(publishers[i]))
<< "rgbd_cameras:=" << cameras << " left rgbd_image" << i << " unsubscribed";
}
}
INSTANTIATE_TEST_SUITE_P(
RgbdCameras,
RgbdOdometryCamerasTest,
::testing::Range(2, 7),
[](const ::testing::TestParamInfo<int> & info) {
return std::to_string(info.param) + "_cameras";
});
/// rgbd_cameras:=0 takes any number of cameras in one RGBDImages message.
TEST_F(RgbdOdometryTest, rgbd_cameras_zero_takes_an_rgbd_images_topic)
{
publishSensorTf();
makeNode({rclcpp::Parameter("subscribe_rgbd", true),
rclcpp::Parameter("rgbd_cameras", 0)});
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
EXPECT_TRUE(waitForSubscriber(pub));
}
/// This node matches by nearest stamp unless told otherwise; stereo_odometry does not.
TEST_F(RgbdOdometryTest, approx_sync_is_on_by_default)
{
publishSensorTf();
std::shared_ptr<rtabmap_odom::RGBDOdometry> node = makeNode();
EXPECT_TRUE(node->get_parameter("approx_sync").as_bool());
}
/**
* A textureless scene is the documented failure: there is nothing to match, so the frame
* is lost and the node says so with a null pose rather than publishing nothing.
* See "When it loses track" in doc/rgbd_odometry.md.
*/
TEST_F(RgbdOdometryTest, reports_lost_with_a_null_pose_on_a_textureless_scene)
{
publishSensorTf();
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
collect<nav_msgs::msg::Odometry>("odom");
std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(info->subscription));
pub->publish(makeBlankFrame(1.0));
ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
pub->publish(makeBlankFrame(1.1));
ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; }));
// Nothing to register against: no features, and the pose carries the "do not use me"
// covariance rather than the node going silent.
EXPECT_EQ(0, info->back().features);
EXPECT_TRUE(isLost(odom->back()));
}
/// publish_null_when_lost:=false makes the node go silent instead.
TEST_F(RgbdOdometryTest, publishes_nothing_when_lost_if_null_publishing_is_off)
{
publishSensorTf();
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
collect<nav_msgs::msg::Odometry>("odom");
makeNode({rclcpp::Parameter("subscribe_rgbd", true),
rclcpp::Parameter("publish_null_when_lost", false)});
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
pub->publish(makeBlankFrame(1.0));
pub->publish(makeBlankFrame(1.1));
spinFor(std::chrono::milliseconds(1500));
EXPECT_TRUE(odom->empty());
}
/**
* keep_color decides what reaches RTAB-Map from a color image, and therefore what the
* node republishes: the matcher works in grayscale, so the color is dropped unless asked
* for. Same contract as stereo_odometry, checked here because the doc states it of this
* node too.
*/
TEST_F(RgbdOdometryTest, keeps_the_image_in_color_when_asked)
{
publishSensorTf();
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> frames =
collect<rtabmap_msgs::msg::RGBDImage>("odom_rgbd_image");
makeNode({rclcpp::Parameter("subscribe_rgbd", true),
rclcpp::Parameter("keep_color", true)});
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(frames->subscription));
pub->publish(makeFrame(kFrame, 1.0));
ASSERT_TRUE(spinUntil([&]() { return !frames->empty(); }));
EXPECT_EQ("bgr8", frames->back().rgb.encoding);
}
/// Off by default: what reaches RTAB-Map, and comes back out, is grayscale.
TEST_F(RgbdOdometryTest, converts_the_image_to_grayscale_by_default)
{
publishSensorTf();
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> frames =
collect<rtabmap_msgs::msg::RGBDImage>("odom_rgbd_image");
std::shared_ptr<rtabmap_odom::RGBDOdometry> node =
makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
EXPECT_FALSE(node->get_parameter("keep_color").as_bool());
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(frames->subscription));
pub->publish(makeFrame(kFrame, 1.0));
ASSERT_TRUE(spinUntil([&]() { return !frames->empty(); }));
EXPECT_EQ("mono8", frames->back().rgb.encoding);
}
/**
* The two feature topics the lidar node cannot fill: both are built from the frame's
* visual words, so they carry content only on the visual paths. `odom_local_map` is the
* map the frame was registered against, `odom_last_frame` the frame's own features, both
* in the odom frame.
*/
TEST_F(RgbdOdometryTest, publishes_the_feature_map_and_the_frame_that_registered_against_it)
{
publishSensorTf();
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
collect<nav_msgs::msg::Odometry>("odom");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> localMap =
collect<sensor_msgs::msg::PointCloud2>("odom_local_map");
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> lastFrame =
collect<sensor_msgs::msg::PointCloud2>("odom_last_frame");
makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(localMap->subscription));
ASSERT_TRUE(waitForPublisher(lastFrame->subscription));
pub->publish(makeFrame(kFrame, 1.0));
ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
pub->publish(makeFrame(kFrame, 1.1));
// All three collectors: the clouds are published after the odometry of the same frame,
// so waiting for odom alone leaves them one spin behind.
ASSERT_TRUE(spinUntil([&]() {
return odom->size() >= 2 && !localMap->empty() && !lastFrame->empty(); }));
// 534 features on this frame, in both, expressed in the odometry frame.
ASSERT_FALSE(localMap->empty()) << "no feature map was published";
ASSERT_FALSE(lastFrame->empty()) << "no frame features were published";
EXPECT_GT(localMap->back().width, 0u);
EXPECT_GT(lastFrame->back().width, 0u);
EXPECT_EQ("odom", lastFrame->back().header.frame_id)
<< "these are published in the odometry frame, not the sensor's";
}
/**
* @brief A four-camera rig, driven a metre through a world of points.
*
* The frames carry their features -- keypoints, 3D points, descriptors -- and no image at
* all, as a driver that does its own extraction publishes them. So the trajectory below
* can only come from the features: the control test that follows runs the same frames
* with them stripped off, and it finds nothing.
*
* Both estimation types the multi-camera case supports are run. Vis/EstimationType=0
* aligns the two sets of 3D points, which needs nothing extra; =1 solves a PnP across
* all four cameras at once, which RTAB-Map hands to OpenGV and cannot do without it.
*/
class RgbdOdometryRigTest :
public RgbdOdometryTest,
public ::testing::WithParamInterface<int>
{
};
TEST_P(RgbdOdometryRigTest, recovers_the_trajectory_of_a_rig_from_the_features_it_is_given)
{
const int estimationType = GetParam();
#ifndef RTABMAP_OPENGV
if(estimationType == 1)
{
GTEST_SKIP() << "a multi-camera PnP is solved by OpenGV, which RTAB-Map was built without";
}
#endif
const CameraRig rig = makeCameraRig();
publishRigTf(rig);
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
collect<nav_msgs::msg::Odometry>("odom");
std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
makeNode({rclcpp::Parameter("subscribe_rgbd", true),
rclcpp::Parameter("rgbd_cameras", 0),
rclcpp::Parameter("Vis/EstimationType", std::to_string(estimationType))});
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(info->subscription));
// A metre forward, ten centimetres at a time.
const int frames = 11;
rtabmap_msgs::msg::RGBDImages lastFrame;
for(int i=0; i<frames; ++i)
{
lastFrame = cameraRigFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i);
ASSERT_EQ(rig.cameras(), lastFrame.rgbd_images.size());
pub->publish(lastFrame);
// Both collectors: odom and odom_info are published separately, and the feature
// count asserted below is read from the odom_info of this same frame.
ASSERT_TRUE(spinUntil([&]() {
return odom->size() >= size_t(i+1) && info->size() >= size_t(i+1); }))
<< "nothing came back for frame " << i;
}
const nav_msgs::msg::Odometry & last = odom->back();
ASSERT_FALSE(isLost(last)) << "lost tracking on a rig that sees the whole scene";
EXPECT_NEAR(1.0, last.pose.pose.position.x, 0.05)
<< "the rig travelled a metre along x";
EXPECT_NEAR(0.0, last.pose.pose.position.y, 0.05);
EXPECT_NEAR(0.0, last.pose.pose.position.z, 0.05);
EXPECT_NEAR(0.0, rotationAngle(last), 0.05) << "the rig never turned";
// The frame's own features, reassembled from the four cameras and used as they are.
// A couple can go missing on the way: RTAB-Map drops a feature whose descriptor lands
// on the same visual word as another one of the same frame, both being ambiguous then
// (the `count(*iter) == 1` guards in RegistrationVis). What matters here is that the
// number is the frame's own and not zero, which is all a blank image could give.
const int sent = int(cameraRigFeatureCount(lastFrame));
EXPECT_LE(info->back().features, sent);
EXPECT_GT(info->back().features, sent - 10)
<< "the node did not use the features the frame came with";
}
INSTANTIATE_TEST_SUITE_P(
EstimationTypes,
RgbdOdometryRigTest,
::testing::Values(0, 1),
[](const ::testing::TestParamInfo<int> & info) {
return info.param == 0 ? std::string("3d_to_3d") : std::string("pnp_across_cameras");
});
/**
* The control for the test above: the same frames with the features stripped off. What is
* left is four calibrations and nothing to see, which the node is right to process -- an
* empty scene is still a frame -- and right to report lost.
*/
TEST_F(RgbdOdometryRigTest, the_same_frames_without_their_features_have_nothing_to_track)
{
const CameraRig rig = makeCameraRig();
publishRigTf(rig);
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
collect<nav_msgs::msg::Odometry>("odom");
std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
makeNode({rclcpp::Parameter("subscribe_rgbd", true),
rclcpp::Parameter("rgbd_cameras", 0)});
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
ASSERT_TRUE(waitForSubscriber(pub));
ASSERT_TRUE(waitForPublisher(info->subscription));
for(int i=0; i<2; ++i)
{
rtabmap_msgs::msg::RGBDImages frame =
cameraRigFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i);
for(size_t c=0; c<frame.rgbd_images.size(); ++c)
{
frame.rgbd_images[c].key_points.clear();
frame.rgbd_images[c].points.clear();
frame.rgbd_images[c].descriptors.clear();
}
pub->publish(frame);
ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }));
}
ASSERT_TRUE(spinUntil([&]() { return info->size() >= 2; }));
EXPECT_EQ(0, info->back().features) << "features appeared from a frame that has none";
EXPECT_TRUE(isLost(odom->back()));
}
/**
* Odom/ImageDecimation shrinks the image before registering it, and scales the
* calibration to match. Features that arrived with the frame are placed in the full size
* image, so they have to be brought down with it: read against a calibration half their
* scale, a rig's keypoints land in the wrong camera altogether.
*
* The frames here carry a blank image for the decimation to have something to work on,
* and the trajectory has to come out the same as without it.
*/
TEST_F(RgbdOdometryRigTest, decimation_brings_the_given_features_down_with_the_image)
{
#ifndef RTABMAP_OPENGV
// Only a PnP reads the keypoints this test is about; a 3D-to-3D registration would
// come out right even with every one of them misplaced.
GTEST_SKIP() << "a multi-camera PnP is solved by OpenGV, which RTAB-Map was built without";
#endif
const CameraRig rig = makeCameraRig();
publishRigTf(rig);
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
collect<nav_msgs::msg::Odometry>("odom");
makeNode({rclcpp::Parameter("subscribe_rgbd", true),
rclcpp::Parameter("rgbd_cameras", 0),
rclcpp::Parameter("Odom/ImageDecimation", "2")});
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
ASSERT_TRUE(waitForSubscriber(pub));
const int frames = 11;
for(int i=0; i<frames; ++i)
{
pub->publish(cameraRigFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0),
1.0 + 0.1*i, /*withImages=*/true));
ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }))
<< "nothing came back for frame " << i;
}
const nav_msgs::msg::Odometry & last = odom->back();
ASSERT_FALSE(isLost(last)) << "the features were lost on the way into the decimated frame";
EXPECT_NEAR(1.0, last.pose.pose.position.x, 0.05);
EXPECT_NEAR(0.0, last.pose.pose.position.y, 0.05);
EXPECT_NEAR(0.0, last.pose.pose.position.z, 0.05);
}
/**
* @brief The same rig on the numbered topics, one to six cameras.
*
* `rgbd_cameras:=N` subscribes to N topics and synchronizes them with a callback of its
* own per N, six of them in all. The test above drives the `rgbd_cameras:=0` one; these
* drive the rest, by publishing each camera of the rig on its own topic and asking for
* the same metre back.
*
* Each of them runs both estimation types, except that a multi-camera PnP needs OpenGV
* and is skipped when RTAB-Map was built without it. A single camera does not, so that
* one is run either way.
*/
class RgbdOdometryRigCamerasTest :
public RgbdOdometryTest,
public ::testing::WithParamInterface<std::tuple<int, bool, int>>
{
};
TEST_P(RgbdOdometryRigCamerasTest, recovers_the_trajectory_from_the_numbered_topics)
{
const int cameras = std::get<0>(GetParam());
const bool approxSync = std::get<1>(GetParam());
const int estimationType = std::get<2>(GetParam());
#ifndef RTABMAP_OPENGV
if(estimationType == 1 && cameras > 1)
{
GTEST_SKIP() << "a multi-camera PnP is solved by OpenGV, which RTAB-Map was built without";
}
#endif
const CameraRig rig = makeCameraRig(cameras);
publishRigTf(rig);
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
collect<nav_msgs::msg::Odometry>("odom");
makeNode({rclcpp::Parameter("subscribe_rgbd", true),
rclcpp::Parameter("rgbd_cameras", cameras),
rclcpp::Parameter("approx_sync", approxSync),
rclcpp::Parameter("Vis/EstimationType", std::to_string(estimationType))});
// One camera listens on rgbd_image, more than one on rgbd_image0..N-1.
std::vector<rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr> publishers;
for(int i=0; i<cameras; ++i)
{
publishers.push_back(helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
cameras == 1 ? "rgbd_image" : "rgbd_image" + std::to_string(i), 10));
}
for(int i=0; i<cameras; ++i)
{
ASSERT_TRUE(waitForSubscriber(publishers[i])) << "camera " << i << " has no subscriber";
}
const int frames = 11;
for(int i=0; i<frames; ++i)
{
const rtabmap_msgs::msg::RGBDImages frame =
cameraRigFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i);
ASSERT_EQ(size_t(cameras), frame.rgbd_images.size());
for(int c=0; c<cameras; ++c)
{
publishers[c]->publish(frame.rgbd_images[c]);
}
ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }))
<< "nothing came back for frame " << i;
}
const nav_msgs::msg::Odometry & last = odom->back();
ASSERT_FALSE(isLost(last)) << "lost tracking with " << cameras << " camera(s)";
EXPECT_NEAR(1.0, last.pose.pose.position.x, 0.05) << "the rig travelled a metre along x";
EXPECT_NEAR(0.0, last.pose.pose.position.y, 0.05);
EXPECT_NEAR(0.0, last.pose.pose.position.z, 0.05);
// A reset tears the synchronizer down and builds it again -- one per camera count,
// and a different one for each of the two sync policies. Frames have to keep arriving
// through the new one.
ASSERT_TRUE(callEmptyService("reset_odom"));
const size_t beforeReset = odom->size();
const rtabmap_msgs::msg::RGBDImages frame =
cameraRigFrame(rig, rtabmap::Transform(1.1f, 0, 0, 0, 0, 0), 2.1);
for(int c=0; c<cameras; ++c)
{
publishers[c]->publish(frame.rgbd_images[c]);
}
EXPECT_TRUE(spinUntil([&]() { return odom->size() > beforeReset; }))
<< "nothing came back after the reset rebuilt the synchronizer";
}
INSTANTIATE_TEST_SUITE_P(
RgbdCameras,
RgbdOdometryRigCamerasTest,
::testing::Combine(::testing::Range(1, 7), ::testing::Bool(), ::testing::Values(0, 1)),
[](const ::testing::TestParamInfo<std::tuple<int, bool, int>> & info) {
const int cameras = std::get<0>(info.param);
return std::to_string(cameras) + (cameras == 1 ? "_camera_" : "_cameras_") +
(std::get<1>(info.param) ? "approx_sync_" : "exact_sync_") +
(std::get<2>(info.param) == 0 ? "3d_to_3d" : "pnp");
});
} // namespace
} // namespace rtabmap_odom_test
rtabmap_odom/test/test_stereo_odometry.cpp
+1272
View File
File diff suppressed because it is too large Load Diff
rtabmap_python/README.md
+6 -10
View File
@@ -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).
rtabmap_rviz_plugins/src/MapCloudDisplay.cpp
+3 -2
View File
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap_conversions/PointCloudConversion.h>
#include "rtabmap_rviz_plugins/MapCloudDisplay.h"
#include <QApplication>
@@ -325,7 +326,7 @@ void MapCloudDisplay::processMapData(const rtabmap_msgs::msg::MapData& map)
if(!cloud->empty())
{
pcl::toROSMsg(*cloud, *cloudMsg);
rtabmap_conversions::toPointCloud2Msg(*cloud, *cloudMsg);
}
}
}
@@ -352,7 +353,7 @@ void MapCloudDisplay::processMapData(const rtabmap_msgs::msg::MapData& map)
if(!cloud->empty())
{
pcl::toROSMsg(*cloud, *cloudMsg);
rtabmap_conversions::toPointCloud2Msg(*cloud, *cloudMsg);
}
}
rtabmap_rviz_plugins/src/MapGraphDisplay.cpp
+12
View File
@@ -26,6 +26,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap_rviz_plugins/MapGraphDisplay.h"
// rviz's headers forward declare these, so the ones actually used here are included
// directly rather than counting on what rviz happens to pull in.
#include <OgreColourValue.h>
#include <OgreManualObject.h>
#include <OgreMatrix4.h>
#include <OgreQuaternion.h>
#include <OgreRenderOperation.h>
#include <OgreSceneManager.h>
#include <OgreSceneNode.h>
#include <OgreVector.h>
#include <rviz_common/display_context.hpp>
#include "rviz_common/properties/color_property.hpp"
#include "rviz_common/properties/float_property.hpp"
rtabmap_sync/README.md
+8 -10
View File
@@ -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).
rtabmap_sync/doc/rgb_sync.md
+10
View File
@@ -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
rtabmap_sync/doc/rgbd_sync.md
+12
View File
@@ -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
rtabmap_sync/doc/rgbdx_sync.md
+11
View File
@@ -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
rtabmap_sync/doc/stereo_sync.md
+11
View File
@@ -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
rtabmap_util/README.md
+6
View File
@@ -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.
rtabmap_util/doc/db_player.md
+11 -1
View File
@@ -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.
rtabmap_util/doc/disparity_to_depth.md
+8
View File
@@ -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
rtabmap_util/doc/imu_to_tf.md
+15 -1
View File
@@ -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:
rtabmap_util/doc/lidar_deskewing.md
+11
View File
@@ -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
rtabmap_util/doc/map_assembler.md
+11
View File
@@ -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
rtabmap_util/doc/obstacles_detection.md
+11
View File
@@ -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
rtabmap_util/doc/point_cloud_aggregator.md
+11
View File
@@ -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
rtabmap_util/doc/point_cloud_assembler.md
+14
View File
@@ -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:
rtabmap_util/doc/point_cloud_xyz.md
+9
View File
@@ -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
rtabmap_util/doc/point_cloud_xyzrgb.md
+9
View File
@@ -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
rtabmap_util/doc/pointcloud_to_depthimage.md
+11
View File
@@ -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
rtabmap_util/doc/rgbd_relay.md
+9
View File
@@ -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:
rtabmap_util/doc/rgbd_split.md
+9
View File
@@ -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
rtabmap_util/src/MapsManager.cpp
+9 -8
View File
@@ -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 <rtabmap_conversions/PointCloudConversion.h>
#include "rtabmap_util/MapsManager.h"
#include <rtabmap/utilite/ULogger.h>
@@ -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<pcl::PointXYZRGB> 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<pcl::PointXYZRGB> 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<pcl::PointXYZRGB> 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<pcl::PointXYZRGB> 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<pcl::PointXYZRGB> 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<pcl::PointXYZRGB> 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);
rtabmap_util/src/nodelets/obstacles_detection.cpp
+5 -4
View File
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap_conversions/PointCloudConversion.h>
#include <rtabmap_util/obstacles_detection.hpp>
#include <pcl/point_cloud.h>
@@ -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<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloudMsg, *inputCloud);
rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *inputCloud);
if(inputCloud->isOrganized())
{
std::vector<int> 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_;
rtabmap_util/src/nodelets/point_cloud_xyz.cpp
+3 -2
View File
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap_conversions/PointCloudConversion.h>
#include <rtabmap_util/point_cloud_xyz.hpp>
#include <rtabmap_conversions/MsgConversion.h>
@@ -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;
rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp
+3 -2
View File
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap_conversions/PointCloudConversion.h>
#include <rtabmap_util/point_cloud_xyzrgb.hpp>
#include <pcl_conversions/pcl_conversions.h>
@@ -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;