Fusing ZED X VIO and GPS with robot_localization on Jetson (ROS 2 Humble)

Aadesh_Varude · September 4, 2025, 6:02pm

Hello,
I’m using a Jetson device running ROS 2 Humble along with the ZED X camera. I’m currently trying to fuse the ZED’s visual-inertial odometry (VIO) with GPS data using the robot_localization package.

I’ve noticed that the ZED only publishes visual odometry in the odom frame, and not the fused odometry. The topics that include both visual odometry and IMU data are published in the map frame.

Is there a topic that provides VIO data specifically in the odom frame, or a recommended way to obtain it?

Additionally, I’d like to know if there’s a best practice for fusing visual odometry, IMU, and GPS using the robot_localization package in this setup.

I have tried using zed ros2 wrapper to fuse GPS and VIO but I stuck with this issue.

Thanks in advance!

Myzhar · September 5, 2025, 1:14pm

Hi @Aadesh_Varude

This is not correct. The odometry published with the /odom topic is the result of a Visual Inertial Fusion algorithm.
The /pose message (map frame) is the result of our full localization with loop closure.

I recommend you follow the robot_localization package documentation to perform this fusion.

You should follow all the instructions to be able to perform a good calibration.

Aadesh_Varude · September 8, 2025, 12:38pm

Hello Myzhar,

I have already followed this instructions.

Could you please clarify which instructions you are referring to? Is there any additional documentation that I might have missed?

I tried following and implementing the example provided in the tutorial. My SDK GNSS fusion works properly, but in the ROS2 wrapper the fusion does not complete — it keeps showing “Calibration in Progress” and remains stuck there. I also tried driving it around, but that did not resolve the issue.

Any guidance on what might be missing or required steps to resolve this would be greatly appreciated.

Myzhar · September 9, 2025, 9:32am

Have you tried to change the parameters to reflect the same configuration of the SDK application that you tested?

github.com/stereolabs/zed-ros2-wrapper

zed_wrapper/config/common_stereo.yaml

master


      
          gnss_fusion:
              gnss_fusion_enabled: false # fuse 'sensor_msg/NavSatFix' message information into pose data
              gnss_fix_topic: '/fix' # Name of the GNSS topic of type NavSatFix to subscribe [Default: '/gps/fix']
              gnss_zero_altitude: false # Set to `true` to ignore GNSS altitude information
              h_covariance_mul: 1.0 # Multiplier factor to be applied to horizontal covariance of the received fix (plane X/Y)
              v_covariance_mul: 1.0 # Multiplier factor to be applied to vertical covariance of the received fix (Z axis)
              publish_utm_tf: true # Publish `utm` -> `map` TF
              broadcast_utm_transform_as_parent_frame: false # if 'true' publish `utm` -> `map` TF, otherwise `map` -> `utm`
              enable_reinitialization: false # determines whether reinitialization should be performed between GNSS and VIO fusion when a significant disparity is detected between GNSS data and the current fusion data. It becomes particularly crucial during prolonged GNSS signal loss scenarios.
              enable_rolling_calibration: true # If this parameter is set to true, the fusion algorithm will used a rough VIO / GNSS calibration at first and then refine it. This allow you to quickly get a fused position.
              enable_translation_uncertainty_target: false # When this parameter is enabled (set to true), the calibration process between GNSS and VIO accounts for the uncertainty in the determined translation, thereby facilitating the calibration termination. The maximum allowable uncertainty is controlled by the 'target_translation_uncertainty' parameter.
              gnss_vio_reinit_threshold: 5.0 # determines the threshold for GNSS/VIO reinitialization. If the fused position deviates beyond out of the region defined by the product of the GNSS covariance and the gnss_vio_reinit_threshold, a reinitialization will be triggered.
              target_translation_uncertainty: 0.1 # defines the target translation uncertainty at which the calibration process between GNSS and VIO concludes. By default, the threshold is set at 10 centimeters.
              target_yaw_uncertainty: 0.1 # defines the target yaw uncertainty at which the calibration process between GNSS and VIO concludes. The unit of this parameter is in radian. By default, the threshold is set at 0.1 radians.

Are you moving your robot, or are you waiting for the calibration to complete before moving it?
If the robot remains static, the calibration cannot complete.

Aadesh_Varude · September 9, 2025, 12:57pm

Yes, I have tried to replicated the parameters similar to my SDK settings.
The only difference is when I use SDK my GPS is directly connected to Jetson and I use the tutorial code to generate the GPS message with suitable type.

When using the ROS2 wrapper, I read messages from the Pixhawk and convert them into NavSat messages, which are compatible with the ROS wrapper fusion code. All other parameters are the same in both the SDK and ROS2 code.

I am using ZED SDK 4.2 and the ROS2 wrapper humble release 4.2.5 for these experiments. In the ROS2 wrapper, I noticed that when I set the gnss parameter to true, the zed_camera_launch files overwrite it and reset it to false. To start the fusion, I need to manually change enable_gnss in the zed_camera_launch file to true.

Yes I am moving robot for the calibration to complete but that does not help and I get stuck on the error “Calibration in Progress” .

Myzhar · September 9, 2025, 1:05pm

Verify that the covariance information is correctly propagated.

I recommend you use the latest ZED SDK v5 and the latest ZED ROS2 Wrapper.
We are not upgrading/fixing older versions of the ZED ROS2 Wrapper that have been frozen.

Aadesh_Varude · September 9, 2025, 1:54pm

I have verified the covariance update and it looks correct.

I will try this.

Thank you for your response.

Aadesh_Varude · September 11, 2025, 3:30pm

Hello @Myzhar,
I’m testing the ZED SDK 5.0.6 together with the latest ROS2 wrapper and have a few questions about the wrapper’s behavior.

It’s my understanding that the ~/geopose topic provides the filtered latitude/longitude poses after GNSS + VIO fusion. I am running tests outdoors.

If that understanding is correct, then using GNSS and VIO fusion with the ROS2 wrapper works properly in areas with a strong GPS signal. However, when I enter locations with a weak GPS signal and large covariance, the fused pose appears to drift along with the GPS drift.

Is this behavior expected?
Additionally, can I use the ROS2 wrapper to transition smoothly from an environment with good GPS reception to a GPS-denied environment?

Myzhar · September 12, 2025, 10:32am

Normally, this should not happen. If the covariance of the GPS reflects the weak quality of the signal, then the fusion algorithm will trust the VIO information and provide correct tracking.

Yes, as long as the covariance information is good.