mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-08 18:57:46 +08:00
rtabmap_sync tests and doc (#1454)
* rtabmap_sync tests and doc * Added some diagrams * cleanup some diagrams * fixing running tests in parallels
This commit is contained in:
matlabbe
@@ -0,0 +1,90 @@
|
||||
# rgb_sync
|
||||
|
||||
Groups a camera's color image and calibration into a single [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html), with no depth.
|
||||
|
||||
The monocular counterpart of [rgbd_sync](rgbd_sync.md). It exists for pipelines that have no depth to offer: a single camera doing appearance-based loop closure detection and relocalization against a map built earlier, where images are used to recognize places rather than to reconstruct them.
|
||||
|
||||
Without depth, RTAB-Map cannot build a metric map from these frames alone. It can still detect that a place has been seen before, which is enough for relocalization in an existing map and for adding loop closure constraints to a graph whose geometry comes from odometry or a lidar.
|
||||
|
||||
The camera cannot supply a pose here — visual odometry needs depth or a stereo baseline — so the pose has to come from somewhere else:
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
CAM["camera driver"]
|
||||
SYNC["rgb_sync"]
|
||||
ODOM["odometry source<br>wheel, lidar or external"]
|
||||
MAP["rtabmap"]
|
||||
CAM -->|rgb/image| SYNC
|
||||
CAM -->|rgb/camera_info| SYNC
|
||||
SYNC -->|rgbd_image| MAP
|
||||
ODOM -->|odometry| MAP
|
||||
```
|
||||
|
||||
## Usage
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_sync rgb_sync --ros-args \
|
||||
-r rgb/image:=/camera/image_raw \
|
||||
-r rgb/camera_info:=/camera/camera_info
|
||||
```
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_sync',
|
||||
plugin='rtabmap_sync::RGBSync',
|
||||
name='rgb_sync',
|
||||
remappings=[('rgb/image', '/camera/image_raw'),
|
||||
('rgb/camera_info', '/camera/camera_info')])
|
||||
```
|
||||
|
||||
## Subscribed Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `rgb/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Color image. Goes through `image_transport`. |
|
||||
| `rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Calibration of the camera. |
|
||||
|
||||
## Published Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The image and its calibration. The depth slot is left empty unless `fill_empty_depth`. Published only when someone is subscribed. |
|
||||
| `rgbd_image/compressed` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The same frame with the image as JPEG. Published only when someone is subscribed. |
|
||||
|
||||
The output's `header.frame_id` comes from the camera_info; its `header.stamp` is the image's.
|
||||
|
||||
## Parameters
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `approx_sync` | `bool` | `false` | Match the image and its calibration by nearest stamp. Defaults to **exact**; see [Synchronization](#synchronization). |
|
||||
| `approx_sync_max_interval` | `double` | `0.0` | Reject pairs spanning more than this many seconds. `0` disables. |
|
||||
| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. |
|
||||
| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. |
|
||||
| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. |
|
||||
| `qos` | `int` | `0` | Reliability of the subscription and the publishers: `0` system default, `1` reliable, `2` best effort. |
|
||||
| `qos_camera_info` | `int` | value of `qos` | Reliability of the `rgb/camera_info` subscription alone. |
|
||||
| `fill_empty_depth` | `bool` | `false` | Add an all-zero depth image the size of the color one. See below. |
|
||||
| `compressed_rate` | `double` | `0.0` | Maximum rate, in Hz, of `rgbd_image/compressed`. `0` means every frame. |
|
||||
| `image_transport` | `string` | `"raw"` | Transport for `rgb/image`, e.g. `compressed`. |
|
||||
|
||||
## fill_empty_depth
|
||||
|
||||
By default the output carries no depth image and no depth calibration, which is how a consumer tells "this camera has no depth" from "this frame's depth happens to be all zeros".
|
||||
|
||||
Some consumers refuse a message without one. `fill_empty_depth` gives them a depth image of the right size and encoding (`16UC1`) filled with zeros, registered to the color camera and sharing its calibration. Zero in a depth image means *no reading*, so the frame still carries no geometry — the flag changes the shape of the message, not its content. Leave it off unless something downstream requires it.
|
||||
|
||||
## Synchronization
|
||||
|
||||
`approx_sync` defaults to **false** here. A driver built on `image_transport`'s camera publisher sends the image and its `camera_info` as a pair carrying the same stamp, so there is nothing to approximate: the exact policy is cheaper and cannot mismatch.
|
||||
|
||||
Set `approx_sync:=true` when the two do not share a stamp — a `camera_info` republished on its own timer, or read from a YAML file and stamped with the current time. That is the case to watch for if the node is silent: the calibration values are constant and look fine, but their stamps never match an image.
|
||||
|
||||
```bash
|
||||
ros2 topic echo --once /camera/image_raw --field header.stamp
|
||||
ros2 topic echo --once /camera/camera_info --field header.stamp
|
||||
```
|
||||
|
||||
## Diagnostics
|
||||
|
||||
The node publishes to `/diagnostics` — input rate, output rate, and a warning in the log every 5 seconds while nothing is arriving.
|
||||
@@ -0,0 +1,164 @@
|
||||
# rgbd_sync
|
||||
|
||||
Groups an RGB-D camera's color image, depth image and calibration into a single [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html).
|
||||
|
||||
A camera driver publishes three topics that only mean anything together. Keeping them together as one message is worth doing for its own sake — one topic to remap, one topic to record, and no chance of a bag holding a depth frame whose color frame was dropped — but the reason this node exists is that the synchronization has to happen *somewhere*, and doing it once here is cheaper than doing it again in every consumer.
|
||||
|
||||
Doing it once also keeps the consumers *consistent*. A pipeline usually runs [`rgbd_odometry`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_odom) and `rtabmap` — often `rtabmap_viz` too — over the same camera. Given the three raw topics, each of those nodes synchronizes them independently, and with approximate matching they can settle on different pairings. `rtabmap` then maps a color/depth pair that odometry never saw, at a pose computed from a different one.
|
||||
|
||||
**Without `rgbd_sync`** — each consumer matches the three topics for itself, with its own synchronizer:
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
CAM["camera driver"]
|
||||
ODOM["rgbd_odometry<br>sync A"]
|
||||
MAP["rtabmap<br>sync B"]
|
||||
CAM -->|rgb/image| ODOM & MAP
|
||||
CAM -->|depth/image| ODOM & MAP
|
||||
CAM -->|rgb/camera_info| ODOM & MAP
|
||||
ODOM -->|odometry| MAP
|
||||
```
|
||||
|
||||
**With `rgbd_sync`** — matched once, then fanned out:
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
CAM["camera driver"]
|
||||
SYNC["rgbd_sync"]
|
||||
ODOM["rgbd_odometry"]
|
||||
ODOMT(["odometry"])
|
||||
MAP["rtabmap"]
|
||||
VIZ["rtabmap_viz"]
|
||||
CAM -->|rgb/image| SYNC
|
||||
CAM -->|depth/image| SYNC
|
||||
CAM -->|rgb/camera_info| SYNC
|
||||
SYNC -->|rgbd_image| ODOM & MAP & VIZ
|
||||
ODOM --> ODOMT
|
||||
ODOMT --> MAP & VIZ
|
||||
```
|
||||
|
||||
The same holds when the pose comes from elsewhere — a wheel encoder, a lidar, or an external VIO. The camera then feeds only the mapping side, but every node on it still sees the identical frame:
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
CAM["camera driver"]
|
||||
SYNC["rgbd_sync"]
|
||||
ODOM["odometry source<br>wheel, lidar or external"]
|
||||
ODOMT(["odometry"])
|
||||
MAP["rtabmap"]
|
||||
VIZ["rtabmap_viz"]
|
||||
CAM -->|rgb/image| SYNC
|
||||
CAM -->|depth/image| SYNC
|
||||
CAM -->|rgb/camera_info| SYNC
|
||||
SYNC -->|rgbd_image| MAP & VIZ
|
||||
ODOM --> ODOMT
|
||||
ODOMT --> MAP & VIZ
|
||||
```
|
||||
|
||||
Subscribe them all to one `RGBDImage` and the question does not arise: every node processes the identical message.
|
||||
|
||||
It also gives a pipeline one place to synchronize. A consumer that has to match a camera against something on a different rate — a lidar, an IMU, odometry — matches one `RGBDImage` against them rather than three topics plus the others all at once. Synchronizing a large set in one go is the harder problem: the policy has to find a window that satisfies every input, and the more inputs with different rates and delays, the more often it settles for a poor match or none at all. Resolving the camera first, where the three topics are tightly correlated, leaves the downstream synchronizer a much easier job.
|
||||
|
||||
It can also decimate the images, rescale depth into the unit RTAB-Map expects, and publish a compressed copy for a slow link. See [Compressing for a slow link](#compressing-for-a-slow-link).
|
||||
|
||||
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).
|
||||
|
||||
## Usage
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_sync rgbd_sync --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 approx_sync:=true
|
||||
```
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_sync',
|
||||
plugin='rtabmap_sync::RGBDSync',
|
||||
name='rgbd_sync',
|
||||
parameters=[{'approx_sync': True}],
|
||||
remappings=[('rgb/image', '/camera/color/image_raw'),
|
||||
('depth/image', '/camera/depth/image_rect_raw'),
|
||||
('rgb/camera_info', '/camera/color/camera_info')])
|
||||
```
|
||||
|
||||
**Compose it into the driver's process.** This node copies every pixel of every frame; across a process boundary that copy is a serialization and a memcpy per image, which on a 720p RGB-D stream is real CPU. In the same process with an intra-process-capable driver it is a pointer.
|
||||
|
||||
## Subscribed Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `rgb/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Color image. Goes through `image_transport`, so `rgb/image/compressed` is used instead when `image_transport` is set. |
|
||||
| `depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Depth image, registered to the color camera. `16UC1` in millimeters or `32FC1` in meters. Goes through `image_transport` under `depth_transport`. |
|
||||
| `rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Calibration of the color camera. Copied into both calibration slots of the output — depth is assumed registered. |
|
||||
|
||||
## Published Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The three inputs, raw. Published only when someone is subscribed. |
|
||||
| `rgbd_image/compressed` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The same frame with JPEG color and PNG depth instead of raw images. Published only when someone is subscribed. |
|
||||
|
||||
The output's `header.frame_id` is taken from the **camera_info**, which is the frame the calibration is expressed in — so make sure the images and the `camera_info` carry the same `frame_id`. If they disagree, the output is labelled with the calibration's frame while the pixels were measured in another, and every point projected out of them lands somewhere else.
|
||||
|
||||
The output's `header.stamp` is the **later** of the color and depth stamps, so the message is never stamped before data it contains.
|
||||
|
||||
## Parameters
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `approx_sync` | `bool` | `true` | Match the inputs by nearest stamp. **Set `false` if your camera allows it** — see [Synchronization](#synchronization). |
|
||||
| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. Worth setting; see [Synchronization](#synchronization). |
|
||||
| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. |
|
||||
| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. |
|
||||
| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. Still copied to it, with a warning. |
|
||||
| `qos` | `int` | `0` | Reliability of the subscriptions and the publishers: `0` system default, `1` reliable, `2` best effort. |
|
||||
| `qos_camera_info` | `int` | value of `qos` | Reliability of the `rgb/camera_info` subscription alone. Drivers often publish images best effort and `camera_info` reliable. |
|
||||
| `depth_scale` | `double` | `1.0` | Multiplies every depth pixel. See [Depth units](#depth-units). |
|
||||
| `decimation` | `int` | `1` | Downsample both images by this factor, scaling the calibration to match. Must divide the depth image size exactly, or it is ignored. |
|
||||
| `compressed_rate` | `double` | `0.0` | Maximum rate, in Hz, of `rgbd_image/compressed`. `0` means every frame. Does not affect `rgbd_image`. |
|
||||
| `image_transport` | `string` | `"raw"` | Transport for `rgb/image`, e.g. `compressed`. |
|
||||
| `depth_transport` | `string` | `"raw"` | Transport for `depth/image`, e.g. `compressedDepth`. |
|
||||
| `rgb_image_transport` | `string` | — | **Deprecated**, renamed to `image_transport`. |
|
||||
| `depth_image_transport` | `string` | — | **Deprecated**, renamed to `depth_transport`. |
|
||||
|
||||
## Synchronization
|
||||
|
||||
**Use `approx_sync:=false` when your camera allows it.** The exact policy is cheaper and cannot mismatch a color frame with the wrong depth frame. The catch is that it is all-or-nothing: if the stamps differ by even a nanosecond, **nothing is ever published**, with no error. That is the single most common reason a pipeline built on this node is silent, so check before switching:
|
||||
|
||||
```bash
|
||||
ros2 topic echo --once /camera/color/image_raw --field header.stamp
|
||||
ros2 topic echo --once /camera/depth/image_rect_raw --field header.stamp
|
||||
```
|
||||
|
||||
The default is nevertheless approximate, for backward compatibility: many RGB-D cameras do not stamp color and depth identically, the two sensors being read at slightly different instants. A stereo pair is normally hardware-triggered instead, which is why [stereo_sync](stereo_sync.md) defaults the other way.
|
||||
|
||||
When you do stay on approximate matching, note that it pairs *whatever it has* if that is the best available. A camera that stalls for a second and resumes produces one pairing of a fresh frame with a second-old one, and nothing says so. `approx_sync_max_interval` is the guard: a set spanning more than that many seconds is dropped instead. **Set it.** A tenth of the frame period is a reasonable starting point — `0.003` for a 30 Hz camera.
|
||||
|
||||
Leaving it at `0` is also what enables the warning about a large stamp difference in the log; setting it suppresses that warning.
|
||||
|
||||
## Depth units
|
||||
|
||||
RTAB-Map reads `16UC1` depth as millimeters and `32FC1` as meters. A driver that publishes `16UC1` in some other unit — centimeters, or a raw disparity count — produces a map at the wrong scale, and nothing about it looks broken until you measure something.
|
||||
|
||||
`depth_scale` multiplies every depth pixel on the way through, so a camera publishing centimeters is fixed with `depth_scale:=10.0`. It is applied after decimation and before compression, so both outputs carry the corrected values.
|
||||
|
||||
## Compressing for a slow link
|
||||
|
||||
`rgbd_image/compressed` carries the same frame with the color image as **JPEG** and the depth image as **PNG**. Depth stays lossless deliberately: JPEG artifacts in a depth image are not blur, they are invented geometry.
|
||||
|
||||
Neither output is produced unless it has a subscriber, so the compression costs nothing until something subscribes.
|
||||
|
||||
`compressed_rate` caps the compressed topic's rate without touching the raw one — for a robot that maps locally at full rate while sending a few frames a second to an operator.
|
||||
|
||||
## Decimation
|
||||
|
||||
`decimation` halves (or thirds, …) both images and scales the calibration with them, which is the part that is easy to get wrong by hand: an image downsampled without its focal length being scaled produces a point cloud with the wrong field of view.
|
||||
|
||||
The factor must divide the **depth** image size exactly. If it does not, the node logs a warning and stops decimating rather than resampling depth in a way that would misalign it against color. A value below 1 is treated as 1.
|
||||
|
||||
## Diagnostics
|
||||
|
||||
The node publishes to `/diagnostics`: the rate of the incoming color frames, the rate of the published messages, and a warning in the log every 5 seconds while nothing is arriving at all. If the input rate is healthy and the output rate is not, the inputs are arriving but not pairing — look at `approx_sync` and the stamps first.
|
||||
@@ -0,0 +1,145 @@
|
||||
# rgbdx_sync
|
||||
|
||||
Groups the [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) topics of 2 to 8 cameras into a single [`rtabmap_msgs/msg/RGBDImages`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImages.html).
|
||||
|
||||
For a robot carrying several RGB-D cameras. Each camera gets its own [rgbd_sync](rgbd_sync.md) (or [stereo_sync](stereo_sync.md)), and this node synchronizes those outputs into one message so that the SLAM node sees all of them as one measurement.
|
||||
|
||||
**Consider the alternative first.** Rebuilt with `RTABMAP_SYNC_MULTI_RGBD=ON`, the SLAM node subscribes to each camera's `RGBDImage` and synchronizes them itself, with no node in between — one process and one full-frame copy per camera per frame less than passing through here. This node exists for the cases that rule that out: running against binary packages, or more than the 6 cameras the build option supports. See [Feeding it to rtabmap](#feeding-it-to-rtabmap).
|
||||
|
||||
The reason it is a build option at all is that each supported camera count is a separate synchronizer template, and instantiating them all costs build time and binary size.
|
||||
|
||||
This node only groups: it never touches the images, the calibrations or the individual stamps.
|
||||
|
||||
Each camera is packed by its own [rgbd_sync](rgbd_sync.md) first, and this node groups those into the one message the consumers subscribe to:
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
CAM0["camera 0 driver"]
|
||||
CAM1["camera 1 driver"]
|
||||
SYNC0["rgbd_sync"]
|
||||
SYNC1["rgbd_sync"]
|
||||
XSYNC["rgbdx_sync"]
|
||||
ODOM["rgbd_odometry"]
|
||||
ODOMT(["odometry"])
|
||||
MAP["rtabmap"]
|
||||
VIZ["rtabmap_viz"]
|
||||
CAM0 -->|"rgb, depth,<br>camera_info"| SYNC0
|
||||
CAM1 -->|"rgb, depth,<br>camera_info"| SYNC1
|
||||
SYNC0 -->|rgbd_image0| XSYNC
|
||||
SYNC1 -->|rgbd_image1| XSYNC
|
||||
XSYNC -->|rgbd_images| ODOM & MAP & VIZ
|
||||
ODOM --> ODOMT
|
||||
ODOMT --> MAP & VIZ
|
||||
```
|
||||
|
||||
**With odometry from elsewhere** — a wheel encoder, a lidar, or an external VIO — the cameras feed only the mapping side:
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
CAM0["camera 0 driver"]
|
||||
CAM1["camera 1 driver"]
|
||||
SYNC0["rgbd_sync"]
|
||||
SYNC1["rgbd_sync"]
|
||||
XSYNC["rgbdx_sync"]
|
||||
ODOM["odometry source<br>wheel, lidar or external"]
|
||||
ODOMT(["odometry"])
|
||||
MAP["rtabmap"]
|
||||
VIZ["rtabmap_viz"]
|
||||
CAM0 -->|"rgb, depth,<br>camera_info"| SYNC0
|
||||
CAM1 -->|"rgb, depth,<br>camera_info"| SYNC1
|
||||
SYNC0 -->|rgbd_image0| XSYNC
|
||||
SYNC1 -->|rgbd_image1| XSYNC
|
||||
XSYNC -->|rgbd_images| MAP & VIZ
|
||||
ODOM --> ODOMT
|
||||
ODOMT --> MAP & VIZ
|
||||
```
|
||||
|
||||
## Usage
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_sync rgbdx_sync --ros-args \
|
||||
-p rgbd_cameras:=3 \
|
||||
-r rgbd_image0:=/camera_front/rgbd_image \
|
||||
-r rgbd_image1:=/camera_left/rgbd_image \
|
||||
-r rgbd_image2:=/camera_right/rgbd_image
|
||||
```
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_sync',
|
||||
plugin='rtabmap_sync::RGBDXSync',
|
||||
name='rgbdx_sync',
|
||||
parameters=[{'rgbd_cameras': 3}],
|
||||
remappings=[('rgbd_image0', '/camera_front/rgbd_image'),
|
||||
('rgbd_image1', '/camera_left/rgbd_image'),
|
||||
('rgbd_image2', '/camera_right/rgbd_image')])
|
||||
```
|
||||
|
||||
The topics are numbered from **0**, and only the first `rgbd_cameras` of them are subscribed.
|
||||
|
||||
## Subscribed Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `rgbd_image0` … `rgbd_image7` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | One per camera. Only the first `rgbd_cameras` are subscribed. |
|
||||
|
||||
## Published Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `rgbd_images` | [`rtabmap_msgs/msg/RGBDImages`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImages.html) | The set, in topic order. Stamped and framed with `rgbd_image0`'s header; each camera keeps its own header inside the array. |
|
||||
|
||||
Unlike the other nodes in this package, this one publishes whether or not anyone is subscribed — it does no per-frame work worth skipping.
|
||||
|
||||
## Parameters
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `rgbd_cameras` | `int` | `2` | How many cameras to group, 2 to 8. Anything outside that range aborts at start-up. |
|
||||
| `approx_sync` | `bool` | `true` | Match the cameras by nearest stamp. Set `false` only for hardware-triggered cameras. |
|
||||
| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. Worth setting; see below. |
|
||||
| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. |
|
||||
| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. |
|
||||
| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. |
|
||||
| `qos` | `int` | `0` | Reliability of the subscriptions and the publisher: `0` system default, `1` reliable, `2` best effort. |
|
||||
|
||||
`rgbd_cameras` outside 2–8 is a hard error rather than a clamp: one camera needs no grouping at all, and nine cannot be synchronized by any of the templates the node holds. For one camera, subscribe to its `RGBDImage` topic directly.
|
||||
|
||||
## Order matters
|
||||
|
||||
The array is published in topic order — `rgbd_image0` first — and consumers index into it. RTAB-Map matches each image against the calibration and the TF frame it saw at that index, so swapping two remappings places a camera's images at another camera's extrinsics, and the map comes out with the world duplicated at an angle.
|
||||
|
||||
The order is a naming convention, not something the node can check. Keep the numbering consistent with whatever else refers to those cameras.
|
||||
|
||||
## Synchronization
|
||||
|
||||
Separate cameras are rarely triggered together, so `approx_sync` defaults to true. Nothing is published until **every** camera has contributed: a partial set would silently drop one camera's field of view from the map, which is worse than a dropped frame.
|
||||
|
||||
That also makes one silent camera stop the whole node. If `rgbd_images` goes quiet, check each input in turn:
|
||||
|
||||
```bash
|
||||
ros2 topic hz /camera_front/rgbd_image
|
||||
```
|
||||
|
||||
Set `approx_sync_max_interval` here as well. With several free-running cameras the synchronizer has more opportunities to pair a fresh frame with a stale one, and each camera's images are placed in the map using the robot's pose at the *set's* stamp — so a camera whose frame is 200 ms old is placed wherever the robot was not.
|
||||
|
||||
## Feeding it to rtabmap
|
||||
|
||||
Set `rgbd_cameras` to **0** on the consumer, and remap its `rgbd_images` input to this node's output:
|
||||
|
||||
```python
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap',
|
||||
parameters=[{'subscribe_rgbd': True, 'rgbd_cameras': 0}],
|
||||
remappings=[('rgbd_images', '/rgbd_images')])
|
||||
```
|
||||
|
||||
`rgbd_cameras:=0` is what selects the `RGBDImages` interface: the count then comes from each message rather than from a parameter, so the same consumer handles any number of cameras without a rebuild.
|
||||
|
||||
With `RTABMAP_SYNC_MULTI_RGBD=ON` instead, drop this node and point the consumer straight at the cameras — `rgbd_cameras:=3` and one remapping per `rgbd_image0`…`rgbd_image2`. Same topics, same order, one hop fewer.
|
||||
|
||||
Either way, every camera needs its extrinsics in TF — a transform from the robot's base frame to each camera's frame, at each frame's stamp.
|
||||
|
||||
## Diagnostics
|
||||
|
||||
The node publishes to `/diagnostics`: the rate of `rgbd_image0`, the rate of published sets, and a warning in the log every 5 seconds while nothing is arriving. Since a set needs every camera, a healthy input rate with no output points at one of the *other* cameras.
|
||||
@@ -0,0 +1,135 @@
|
||||
# stereo_sync
|
||||
|
||||
Groups a stereo pair's four topics — left image, right image and their two calibrations — into a single [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html).
|
||||
|
||||
The same idea as [rgbd_sync](rgbd_sync.md), for a stereo camera: four topics that only mean anything together become one message, synchronized once instead of in every consumer.
|
||||
|
||||
The default topic names say `image_rect` because rectified images are what a stereo pipeline normally carries, and what RTAB-Map assumes by default — but this node does not require it and does not rectify anything itself. Feeding it unrectified images is fine as long as you tell the consumer: set `Rtabmap/ImagesAlreadyRectified` to `false` on the `stereo_odometry` and `rtabmap` nodes, and they rectify from the calibration themselves. Otherwise run [`stereo_image_proc`](https://docs.ros.org/en/jazzy/p/stereo_image_proc/) upstream.
|
||||
|
||||
In a pipeline, the one `RGBDImage` feeds everything downstream — odometry included, so every node works from the same pair:
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
CAM["stereo driver"]
|
||||
SYNC["stereo_sync"]
|
||||
ODOM["stereo_odometry"]
|
||||
ODOMT(["odometry"])
|
||||
MAP["rtabmap"]
|
||||
VIZ["rtabmap_viz"]
|
||||
CAM -->|left/image_rect| SYNC
|
||||
CAM -->|right/image_rect| SYNC
|
||||
CAM -->|left/camera_info| SYNC
|
||||
CAM -->|right/camera_info| SYNC
|
||||
SYNC -->|rgbd_image| ODOM & MAP & VIZ
|
||||
ODOM --> ODOMT
|
||||
ODOMT --> MAP & VIZ
|
||||
```
|
||||
|
||||
**With odometry from elsewhere** — a wheel encoder, a lidar, or an external VIO — the stereo pair feeds only the mapping side:
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
CAM["stereo driver"]
|
||||
SYNC["stereo_sync"]
|
||||
ODOM["odometry source<br>wheel, lidar or external"]
|
||||
ODOMT(["odometry"])
|
||||
MAP["rtabmap"]
|
||||
VIZ["rtabmap_viz"]
|
||||
CAM -->|left/image_rect| SYNC
|
||||
CAM -->|right/image_rect| SYNC
|
||||
CAM -->|left/camera_info| SYNC
|
||||
CAM -->|right/camera_info| SYNC
|
||||
SYNC -->|rgbd_image| MAP & VIZ
|
||||
ODOM --> ODOMT
|
||||
ODOMT --> MAP & VIZ
|
||||
```
|
||||
|
||||
## Usage
|
||||
|
||||
```bash
|
||||
ros2 run rtabmap_sync stereo_sync --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
|
||||
```
|
||||
|
||||
```python
|
||||
ComposableNode(
|
||||
package='rtabmap_sync',
|
||||
plugin='rtabmap_sync::StereoSync',
|
||||
name='stereo_sync',
|
||||
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')])
|
||||
```
|
||||
|
||||
As with `rgbd_sync`, compose it into the driver's process where you can: this node copies both images of every pair.
|
||||
|
||||
## Subscribed Topics
|
||||
|
||||
| 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, mono or color. Goes through `image_transport`. |
|
||||
| `right/image_rect` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified right image, same size and encoding. |
|
||||
| `left/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Calibration of the left camera. |
|
||||
| `right/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Calibration of the right camera. **Its `P[3]` must carry the baseline**; see below. |
|
||||
|
||||
## Published Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---|---|---|
|
||||
| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | Left image in the color slot, right image in the depth slot. Published only when someone is subscribed. |
|
||||
| `rgbd_image/compressed` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The same pair, both images JPEG. Published only when someone is subscribed. |
|
||||
|
||||
The output's `header.frame_id` comes from the **left** camera_info, and its `header.stamp` is the later of the two image stamps.
|
||||
|
||||
## Parameters
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|---|---|---|---|
|
||||
| `approx_sync` | `bool` | `false` | Match the inputs by nearest stamp. Defaults to **exact** here; see [Synchronization](#synchronization). |
|
||||
| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. Only meaningful with `approx_sync`. |
|
||||
| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. |
|
||||
| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. |
|
||||
| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. |
|
||||
| `qos` | `int` | `0` | Reliability of the subscriptions and the publishers: `0` system default, `1` reliable, `2` best effort. |
|
||||
| `qos_camera_info` | `int` | value of `qos` | Reliability of the two `camera_info` subscriptions alone. |
|
||||
| `compressed_rate` | `double` | `0.0` | Maximum rate, in Hz, of `rgbd_image/compressed`. `0` means every frame. |
|
||||
| `image_transport` | `string` | `"raw"` | Transport for both images, e.g. `compressed`. |
|
||||
|
||||
## How a stereo pair travels in an RGBDImage
|
||||
|
||||
There is no separate stereo message: the left image goes where color goes and the right image goes where depth goes. What tells a consumer to read it as a stereo pair rather than as color plus depth is the **baseline** in the second calibration — `P[3]` of the right `camera_info`, which by the ROS convention is `-fx * baseline`.
|
||||
|
||||
So a right `camera_info` with `P[3] == 0` describes a camera sitting exactly on top of the left one. Nothing downstream can triangulate from that, and the failure is silent: the pair is forwarded, RTAB-Map reads a zero baseline and produces no depth. If a stereo pipeline comes out with no 3D points at all, check `P[3]` of the right camera first:
|
||||
|
||||
```bash
|
||||
ros2 topic echo --once /stereo/right/camera_info --field p
|
||||
```
|
||||
|
||||
## Synchronization
|
||||
|
||||
`approx_sync` defaults to **false** here, unlike the other nodes in this package. A stereo pair is normally hardware-triggered, so the two frames carry the same stamp, and the exact policy is both cheaper and impossible to mismatch. Mismatching a stereo pair is worse than mismatching color and depth: the disparity between two frames taken at different instants is a measurement of the camera's own motion, read as scene geometry.
|
||||
|
||||
Set `approx_sync:=true` only for two free-running cameras that are not triggered together — and then set `approx_sync_max_interval` alongside it. The node warns whenever a pair's stamps differ by more than 10 ms regardless of the setting, because at that point the pair is unlikely to be worth anything.
|
||||
|
||||
If the pipeline is silent with the default, the stamps are not identical. Check with:
|
||||
|
||||
```bash
|
||||
ros2 topic echo --once /stereo/left/image_rect --field header.stamp
|
||||
ros2 topic echo --once /stereo/right/image_rect --field header.stamp
|
||||
```
|
||||
|
||||
## Compressing for a slow link
|
||||
|
||||
`rgbd_image/compressed` carries both images as **JPEG**. Unlike [rgbd_sync](rgbd_sync.md), there is no lossless path: both halves of a stereo pair are ordinary camera images, and neither is depth.
|
||||
|
||||
JPEG artifacts do affect stereo matching, so a pipeline that computes odometry from the compressed stream will match slightly fewer features than one on the raw images. For sending frames to an operator, that does not matter; for running odometry at the far end of a link, prefer a higher JPEG quality over a lower frame rate.
|
||||
|
||||
`compressed_rate` caps the compressed topic without touching `rgbd_image`.
|
||||
|
||||
## Diagnostics
|
||||
|
||||
The node publishes to `/diagnostics`: the rate of incoming left frames, the rate of published pairs, and a warning in the log every 5 seconds while nothing is arriving. A healthy input rate with no output means the pairs are not matching — the stamps and `approx_sync` are what to look at.
|
||||
Reference in New Issue
Block a user