Estimate Motion with Stereo Visual Odometry

Required Version

This feature will not be released until MoveIt Pro version 10.2.

Stereo visual odometry (VO) estimates how a stereo camera moves by tracking visual features from frame to frame. MoveIt Pro runs it as a ROS 2 component that publishes the camera's motion as nav_msgs/msg/Odometry. You start, stop, reset and query it through one control service, or from an Objective with four Behaviors.

VO is a relative motion estimate: it drifts over distance and has no map. Use it as one odometry source in a state estimator, next to wheel odometry and an IMU, rather than as a localization system.

The video below is an offline rendering of the pipeline on a recorded lunar_sim drive: the dots are ALIKED keypoints, the green trails are the LightGlue matches that PnP used, and the top view compares the VO path with ground truth.

How It Works

The Parts

VO is two neural networks and a geometry stage, packaged in one component:

Part	What it does
ALIKED	A neural network that finds distinctive points in an image, such as corners and texture edges, and describes what each one looks like with a short list of numbers (a descriptor).
LightGlue	A neural network that pairs keypoints between two images by comparing their descriptors, and scores each pair. VO runs it twice per stereo pair: left to right, and previous left to current left.
Stereo geometry	Turns each left-to-right match into a 3D point, using how far apart the point lands in the two images.
PnP with RANSAC	Finds the one camera motion that best explains where the previous pair's 3D points appear in the current left image, ignoring matches that disagree with it.
Checks	Rejects a motion that too few matches support, that fits them poorly, or that is faster than the camera can move.

What Happens to Each Stereo Pair

Every step can drop data. A pose is published only when enough survives to every check.

1. Keypoints. ALIKED finds keypoints in both images. Keypoints that score at or below score_threshold are dropped.
2. Depth. LightGlue matches left keypoints to right keypoints. On rectified images a true match lies on the same image row, so a match more than max_epipolar_error_px off its row is dropped. A match with too little disparity, or deeper than max_depth_m, is dropped too.
3. Motion. LightGlue matches the previous left image to the current one. That links the previous pair's 3D points to pixels in the current image, and PnP with RANSAC finds the camera motion that best maps one onto the other.
4. Checks. The solve must keep enough inliers (min_pnp_inlier_ratio), a low mean reprojection error, enough spread across the image, and a speed below max_linear_velocity_mps and max_angular_velocity_rps.

The first pair after a start or a reinitialization has no previous pair to compare with. It only seeds the tracker, publishes no pose, and tracking_state reads initializing for it.

Depth Comes From Disparity

A point seen by both cameras lands at a different horizontal position in each image. That offset is the disparity, and depth is f × B / disparity, where f is the focal length in pixels and B is the baseline between the cameras in meters. Disparity is inversely proportional to depth: a near point has a large disparity, a far point has a small one, and a point at infinity, such as a star, has zero.

Near points have a large disparity, so a small matching error barely changes their depth. Far points have a small disparity, so the same error changes their depth a lot. VO is therefore most accurate when the camera sees textured surfaces within a few meters, such as floors, walls and nearby objects, and degrades when most of the view is distant or featureless, such as an open hangar door or a blank wall. Points with a disparity at or below min_disparity_px are not used, which for the OAK-D Pro-style pair in hangar_sim and lunar_sim means anything beyond about 28.6 m.

What It Publishes

The component publishes on ~/odometry (by default /stereo_visual_odometry/odometry) and ~/diagnostics. It publishes no TF.

Field	Value
header.stamp	The stereo pair's source timestamp.
header.frame_id	odom_frame parameter, default camera_odom.
child_frame_id	camera_frame parameter, default camera_left_optical.
pose	The left camera's pose relative to the first pair after the first START, or after the last RESET with reset_pose.
pose.covariance	pose_variance (default 1e6) on the diagonal. The first pose after a tracking gap carries pose_discontinuity_variance (default 1e9).
twist	Not computed. Published as NaN with covariance[0] = -1.

The pose is in the camera's optical frame

The pose is expressed in the left optical frame of that first pair: x right, y down, z forward. It is not a REP-105 odom frame, and camera_odom is not in the TF tree. A consumer must transform the pose into its own frame, using the camera's mounting on the robot. Using it directly as odom swaps the axes.

Treat a pose with the enlarged covariance as a break in the trajectory, not as a less certain measurement. It means the camera moved while tracking was lost, and nothing measured that motion.

Before You Start

• Rectified images. Subscribe to rectified topics (image_rect), not image_raw. The component applies no distortion model, so raw images produce biased depth and drift without any error.
• Stereo calibration. The right camera's CameraInfo must encode the baseline in P (Tx = -fx × baseline).
• One stamp. Both images and both CameraInfo messages must share one exact timestamp.
• Encoding. rgb8 or mono8, the same for both images.
• Frame IDs. camera_frame must match the frame_id of the left image and left CameraInfo.
• Aspect ratio. model_width and model_height must have the same aspect ratio as the camera, or every pair is rejected. Use 1024×576 for a 16:9 camera and 1024×768 for 4:3.
• Model bundle. VO loads ALIKED and LightGlue from a model bundle. Pass its manifest with bundle_manifest, preferably as an absolute path. The manifest names an aliked and a lightglue model for the moveit_pro.stereo_visual_odometry.v1 graph ABI.
• CPU. VO runs both networks on the CPU, with GPU acceleration planned for a future release.

Try It in lunar_sim

Coming soon

An end-to-end walkthrough that runs VO on the lunar_sim rover's OAK-D Pro stereo pair is coming soon. Until then, use Launch It on Your Robot with any rectified stereo camera.

Launch It on Your Robot

ros2 launch moveit_pro_nav_ros stereo_visual_odometry.launch.py \
  bundle_manifest:=/path/to/manifest.yaml \
  left_image_topic:=/oakd/left/image_rect \
  right_image_topic:=/oakd/right/image_rect \
  left_camera_info_topic:=/oakd/left/camera_info \
  right_camera_info_topic:=/oakd/right/camera_info \
  camera_frame:=oakd_left_optical_frame \
  model_width:=1024 model_height:=576

Set parameters that have no launch argument through a YAML file with vo_parameters_file:=; a launch argument overrides the same name in that file. The file uses the component's node name:

stereo_visual_odometry:
  ros__parameters:
    max_linear_velocity_mps: 1.5
    min_pnp_inlier_ratio: 0.5

The launch file loads the component into its own container, or into an existing one with container_name:=. Loading it into your camera driver's container avoids copying every image between processes, but a crash in either then stops both.

The component waits for START unless you launch it with autostart:=true. If an autostart fails, fix the reported cause and send START.

Parameters

Parameter	Default	What it controls
autostart	false	Start tracking when the component loads.
model_width, model_height	640, 480	Size both images are resized to. Must match the camera's aspect ratio, so the default rejects every frame from a 16:9 camera. 1024 on the long side matches the qualified models.
score_threshold	0.2	ALIKED keypoint score must exceed this.
min_match_score	0.1	LightGlue match confidence must exceed this.
min_disparity_px	2.0	Smallest stereo disparity used for depth.
max_epipolar_error_px	1.0	Largest row difference allowed in a stereo match.
max_depth_m	100.0	Farthest point used.
min_pnp_points	6	Fewest 3D points a solve can use.
min_pnp_inlier_ratio	0.6	Fewest RANSAC inliers, as a fraction of matches.
pnp_reprojection_error_px	2.0	RANSAC inlier threshold.
max_pnp_reprojection_residual_px	1.5	Largest mean residual over the inliers. Must be below pnp_reprojection_error_px.
max_linear_velocity_mps	5.0	Fastest plausible camera speed. Set it to your platform's top speed.
max_angular_velocity_rps	6.28	Fastest plausible camera rotation rate.
reset_after_failures	3	Consecutive rejected pairs before tracking reinitializes.
max_consecutive_reinitializations	10	Consecutive reinitializations before tracking latches (see below).
queue_depth	2	Stereo synchronization queue per topic.
pose_variance	1e6	Pose covariance diagonal on a normal pose.
pose_discontinuity_variance	1e9	Pose covariance diagonal on the first pose after a tracking gap. Must exceed pose_variance.

The table lists the parameters most deployments change; the component declares a few more advanced PnP settings. All parameters are read-only: set them at launch. Invalid values stop the component from loading, with a message that names the parameter.

Control It

The component exposes moveit_pro_nav_msgs/srv/ControlStereoVisualOdometry on ~/control (by default /stereo_visual_odometry/control), with four commands:

Command	Behavior	What it does
START (1)	StartStereoVisualOdometry	Loads the models and starts tracking. After a STOP, the pose continues from where it stopped.
RESET (2)	ResetStereoVisualOdometry	Clears tracking history and keeps the pose. With reset_pose (a port on the Behavior), also moves the pose back to identity. A stopped runtime stays stopped.
STOP (3)	StopStereoVisualOdometry	Stops tracking and disconnects the inputs. Stopping a stopped runtime succeeds.
STATUS (4)	GetStereoVisualOdometryStatus	Reports state without changing it.

From a shell, send a command by its number, for example to start tracking:

ros2 service call /stereo_visual_odometry/control \
  moveit_pro_nav_msgs/srv/ControlStereoVisualOdometry "{command: 1}"

Every response reports runtime_state, tracking_state, synchronized_frames (pairs that produced a pose), rejected_frames, reinitializations and a message. The Behaviors report the same values on their output ports, with the message as status_message. The Behaviors take service_name and response_timeout ports. response_timeout defaults to 60 s for StartStereoVisualOdometry, because starting loads both models, and to 5 s for the others.

Two States to Watch

runtime_state says whether the component is running. tracking_state says whether it is producing poses. Watch both.

Inside RUNNING, tracking_state moves between initializing, tracking, degraded and lost. A single rejected pair is retried. After reset_after_failures rejected pairs in a row, tracking reinitializes. After max_consecutive_reinitializations reinitializations with no good pair between them, tracking latches: it stops consuming pairs until you send RESET.

A latched tracker still reports RUNNING

When tracking latches, runtime_state stays RUNNING while tracking_state reads lost and no poses are published. An Objective that checks only runtime_state sees a healthy component. Check tracking_state too, and treat anything other than tracking as not usable.

Only START recovers from FAULTED. RESET is refused while faulted.

When a Behavior Fails

runtime_state is a number: 0 stopped, 1 running, 2 faulted, or 3 unknown. tracking_state is one of stopped, initializing, tracking, degraded, lost, faulted or unknown. Only tracking means poses are being published; treat any other value, including one you do not recognize, as not usable.

A VO Behavior returns FAILURE in these cases:

Case	Which Behaviors	What the output ports report
The runtime refuses the command: START cannot read the model bundle, cannot load the models, or the models do not run on the CPU; RESET while the runtime is faulted.	StartStereoVisualOdometry, ResetStereoVisualOdometry	The runtime's own values. status_message gives the reason.
The runtime is faulted.	GetStereoVisualOdometryStatus	runtime_state 2 (faulted) and tracking_state faulted.
An internal error faults the runtime while it handles the command.	All four	runtime_state 2 (faulted). status_message gives the error.
The service does not answer: the component is not loaded or the call times out.	All four	runtime_state 3 (unknown), tracking_state unknown, and a status_message that says there was no reading. The frame counters keep their last values.
A port is rejected before the call: service_name is empty, or response_timeout is not a finite positive number. Unlike other service Behaviors, a negative value is rejected rather than meaning "wait forever".	All four	The same as when the service does not answer.

The runtime never refuses STOP: stopping a stopped runtime succeeds.

• GetStereoVisualOdometryStatus returns SUCCESS for a stopped runtime and for lost or degraded tracking, so its SUCCESS means "answered and not faulted", not "running". Check runtime_state for 1 (running) and tracking_state for tracking yourself.
• A timeout, or the tree halting the Behavior, ends the wait, not the work. START can finish after its Behavior stops waiting and leave the runtime running. A halted Behavior's output ports keep the unknown values. Pair a start with StopStereoVisualOdometry on your cleanup path.

Diagnostics

The component publishes diagnostic_msgs/msg/DiagnosticArray on its own ~/diagnostics topic, not the global /diagnostics, as pairs are processed and once per second while running. A running session reports ERROR / lost if it produces no pose for 2 s, whether the cameras went silent or every pair was rejected. Check tracking_state rather than waiting for a sustained ERROR.

Message says	Likely cause
No synchronized stereo packet arrived	Wrong topic names, a camera not publishing, or the four messages do not share one exact stamp.
No stereo packet produced a pose ... the most recent reason was ...	Pairs arrive but are rejected. The reason names the failed check.
aspect ratio ... does not match	model_width and model_height do not match the camera's aspect ratio.
RANSAC PnP inlier ratio ... is below configured minimum	Too few consistent matches, usually a featureless or mostly distant view.
latched unrecoverable after repeated reinitialization	Tracking failed repeatedly. Fix the view or inputs, then send RESET.