i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 16:57: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
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.