rtabmap_ros/rtabmap_odom/doc/stereo_odometry.md

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; 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. This page covers what is specific to this node.

Contents

• Pipeline arrangements
• Usage
• Subscribed Topics
• Published Topics
• Parameters
• Synchronization
• Getting the scale right
• Features computed elsewhere
• Repetitive patterns
• When it loses track

Pipeline arrangements

A typical stereo pipeline, rectification included:

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:

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.

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:

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

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

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 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	Rectified left image, color or mono.
right/image_rect	sensor_msgs/msg/Image	Rectified right image, color or mono.
left/camera_info	sensor_msgs/msg/CameraInfo	Left calibration.
right/camera_info	sensor_msgs/msg/CameraInfo	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:

rgbd_cameras	Topic	Type
1 (default)	rgbd_image	rtabmap_msgs/msg/RGBDImage
2–6	rgbd_image0 … rgbd_image5	rtabmap_msgs/msg/RGBDImage
0	rgbd_images	rtabmap_msgs/msg/RGBDImages

Topic	Type	Description
imu	sensor_msgs/msg/Imu	Optional. See the README.

Published Topics

Common to all three nodes; see the README.

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.
approx_sync	bool	false	Match the raw topics by nearest stamp. Defaults to exact, unlike rgbd_odometry — see 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:

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: 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 — 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.