GNSS Fusion for Jetpack 4.6.x (ros1)

Dear community,

I want to use the zed sdk ( to fused the gnss data since it have not been supported for Zed_wrapper yet. So far, I want to subscribe to topic /ublox/fix and fuse with my gnss data and publish to /ublox/fix_fused. I attached my code below since I cannot attached the file (new user policy). The problem is after the robot moving, the status turns out to be “NOT_CALIBRATED”. Is that my parameters are wrong or any other problems. How can I fix it? Thank youu.

GPS reader from ros

import threading
import time
import rospy
from sensor_msgs.msg import NavSatFix
import as sl

class ROSGPSReader:
    def __init__(self):
        self.continue_to_grab = True
        self.new_data = False
        self.current_gnss_data = None
        self.subscriber = None
        self.gnss_data_lock = threading.Lock()

    def initialize(self):
    #     rospy.init_node('gnss_reader', anonymous=True)
        self.subscriber = rospy.Subscriber("/ublox/fix", NavSatFix, self.gnss_callback)
        print("Waiting for GNSS fix...")

    def gnss_callback(self, data):
        with self.gnss_data_lock:
            self.current_gnss_data = sl.GNSSData()
            self.current_gnss_data.set_coordinates(data.latitude, data.longitude, data.altitude, False)
            self.current_gnss_data.longitude_std = data.position_covariance[0]
            self.current_gnss_data.latitude_std = data.position_covariance[4]
            self.current_gnss_data.altitude_std = data.position_covariance[8]
            position_covariance = data.position_covariance
            ts = sl.Timestamp()
            ts.set_microseconds( // 1000)
            self.current_gnss_data.ts = ts
            self.new_data = True

    def grab(self):
        with self.gnss_data_lock:
            if self.new_data:
                self.new_data = False
                return sl.ERROR_CODE.SUCCESS, self.current_gnss_data
        return sl.ERROR_CODE.FAILURE, None

    def stop(self):
        self.continue_to_grab = False

if __name__ == "__main__":
    reader = ROSGPSReader()
        while not rospy.is_shutdown():
            time.sleep(1)  # Main thread continues to run, allowing Ctrl+C to stop ROS
FUSE with zed VIO
import as sl
import rospy
from sensor_msgs.msg import Image, CompressedImage, NavSatFix
from cv_bridge import CvBridge
import cv2
import numpy as np
from gnss_reader.gpsd_reader_ros import ROSGPSReader

def publish_gnss_data(gnss_data, publisher):
    Publish GNSS data to ROS topic
    fix = NavSatFix()
    fix.header.stamp =
    fix.latitude = gnss_data["latitude"]
    fix.longitude = gnss_data["longitude"]
    fix.altitude = gnss_data["altitude"]

def main():
    # Initialize ROS node
    rospy.init_node('zed_gps_camera_publisher', anonymous=True)
    pub_raw = rospy.Publisher('/zed2i/zed_node/left/image_rect_color', Image, queue_size=10)
    pub_compressed = rospy.Publisher('/zed2i/zed_node/rgb/image_rect_color/compressed', CompressedImage, queue_size=10)
    pub = rospy.Publisher('/ublox/fix_fused', NavSatFix, queue_size=10)

    # Open the camera
    zed = sl.Camera() 
    init_params = sl.InitParameters()
    init_params.camera_resolution = sl.RESOLUTION.HD720  # Choose camera resolution
    init_params.camera_fps = 30 #Choose camera FPS

    init_params.sdk_verbose = 1 
    status =
    if status != sl.ERROR_CODE.SUCCESS:
        print("[ZED][ERROR] Camera Open : "+repr(status)+". Exit program.")

    # Create a CV bridge
    bridge = CvBridge()

    # Enable positional tracking:
    positional_init = zed.enable_positional_tracking()
    if positional_init != sl.ERROR_CODE.SUCCESS:
        print("[ZED][ERROR] Can't start tracking of camera : "+repr(status)+". Exit program.")

    # Create Fusion object:
    fusion = sl.Fusion()
    # fusion.enable_positionnal_tracking({"gnss_calibration_parameters" : gnss_calibration_parameters , "enable_GNSS_fusion" : True})
    init_fusion_param = sl.InitFusionParameters()
    init_fusion_param.coordinate_units = sl.UNIT.METER
    fusion_init_code = fusion.init(init_fusion_param)
    if fusion_init_code != sl.FUSION_ERROR_CODE.SUCCESS:
        print("[ZED][ERROR] Failed to initialize fusion :"+repr(fusion_init_code)+". Exit program")

    # Enable odometry publishing:
    configuration = sl.CommunicationParameters()

    # Enable GNSS data producing:
    gnss_reader = ROSGPSReader()  # Modified line to use ROS GNSS reader
    gnss_reader.initialize()  # Initialize ROS GNSS reader

    # Subscribe to Odometry
    uuid = sl.CameraIdentifier(zed.get_camera_information().serial_number)

    # Enable positional tracking for Fusion object
    positional_tracking_fusion_parameters = sl.PositionalTrackingFusionParameters()
    positional_tracking_fusion_parameters.enable_GNSS_fusion = True 
    positional_tracking_fusion_parameters.gnss_calibration_parameters.target_yaw_uncertainty = 0.1
    positional_tracking_fusion_parameters.gnss_calibration_parameters.enable_translation_uncertainty_target = False
    positional_tracking_fusion_parameters.gnss_calibration_parameters.target_translation_uncertainty = 0.1
    positional_tracking_fusion_parameters.gnss_calibration_parameters.enable_reinitialization = True
    positional_tracking_fusion_parameters.gnss_calibration_parameters.gnss_vio_reinit_threshold = 10
    positional_tracking_fusion_parameters.gnss_calibration_parameters.enable_rolling_calibration = True
    # {
    #     "target_yaw_uncertainty" : 0.1,
    #     "enable_translation_uncertainty_target" : False,
    #     "target_translation_uncertainty" : 10e-1,
    #     "enable_reinitialization" : True,
    #     "gnss_vio_reinit_threshold" : 5,
    #     "enable_rolling_calibration" : True
    # }
    # fusion.enable_positionnal_tracking({"gnss_calibration_parameters" : gnss_calibration_parameters , "enable_GNSS_fusion" : True})

    py_translation = sl.Translation()
    # Setup viewer:
    # viewer = GenericDisplay()
    # viewer.init(zed.get_camera_information().camera_model)

    # Capture images
    runtime_parameters = sl.RuntimeParameters()
    print("Start grabbing data ...")
    # while viewer.isAvailable():
    while not rospy.is_shutdown():
            # Grab camera:
            if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
                zed_pose = sl.Pose()
                # You can still use the classical getPosition for your application, just not that the position returned by this method
                # is the position without any GNSS/cameras fusion
                zed.get_position(zed_pose, sl.REFERENCE_FRAME.CAMERA)

                # Retrieve left image
                left_image = sl.Mat()
                zed.retrieve_image(left_image, sl.VIEW.LEFT)

                # Convert image to numpy array
                left_image_array = left_image.get_data()

                # Convert RGBA to BGR
                left_image_array_bgr = cv2.cvtColor(left_image_array, cv2.COLOR_RGBA2BGR)

                # Convert the image to ROS message
                ros_image = bridge.cv2_to_imgmsg(left_image_array_bgr, encoding="rgb8")

                # Publish raw image

                # Convert image to compressed ROS message
                ros_compressed_image = bridge.cv2_to_compressed_imgmsg(left_image_array_bgr)

                # Publish compressed image

                # Release the image

            # Get GNSS data:
            status, input_gnss = gnss_reader.grab()
            if status == sl.ERROR_CODE.SUCCESS:
                # Publish GNSS data to Fusion
                ingest_error = fusion.ingest_gnss_data(input_gnss)
                if ingest_error != sl.FUSION_ERROR_CODE.SUCCESS:
                    print("Ingest error occurred when ingesting GNSSData: ", ingest_error)
            # Process data and compute positions:
            if fusion.process() == sl.FUSION_ERROR_CODE.SUCCESS:
                fused_position = sl.Pose()
                # Get position into the ZED CAMERA coordinate system:
                current_state = fusion.get_position(fused_position)
                # Display it on OpenGL:
                rotation = fused_position.get_rotation_vector()
                translation = fused_position.get_translation(py_translation)
                text_rotation = str((round(rotation[0], 2), round(rotation[1], 2), round(rotation[2], 2)))
                text_translation = str((round(translation.get()[0], 2), round(translation.get()[1], 2), round(translation.get()[2], 2)))
                # viewer.updatePoseData(fused_position.pose_data(), text_translation, text_rotation, current_state) 
                # Get position into the GNSS coordinate system - this needs a initialization between CAMERA 
                # and GNSS. When the initialization is finish the getGeoPose will return sl.POSITIONAL_TRACKING_STATE.OK
                current_geopose = sl.GeoPose()
                current_geopose_satus = fusion.get_geo_pose(current_geopose)
                print(rotation, translation.get())
                print(text_rotation, text_translation)
                if current_geopose_satus == sl.GNSS_CALIBRATION_STATE.CALIBRATED:
                    gnss_input= {
                    "latitude": current_geopose.latlng_coordinates.get_latitude(False),
                    "longitude": current_geopose.latlng_coordinates.get_longitude(False),
                    "altitude": current_geopose.latlng_coordinates.get_altitude()}
                    publish_gnss_data(gnss_input, pub)

                    # viewer.updateGeoPoseData(current_geopose, zed.get_timestamp(sl.TIME_REFERENCE.CURRENT).data_ns/1000)
                    GNSS coordinate system to ZED coordinate system is not initialize yet
                    The initialisation between the coordinates system is basicaly an optimization problem that
                    Try to fit the ZED computed path with the GNSS computed path. In order to do it just move
                    your system by the distance you specified in positional_tracking_fusion_parameters.gnss_initialisation_distance
        except rospy.ROSException as e:
            print("ROS Exception:", e)
if __name__ == '__main__' : 

Hi @LNBC1411,

First of all, welcome to the Stereolabs community! :wave:

The Global Localization Fusion module performs a calibration between the GNSS and camera at startup. You can monitor the state of the calibration with the following method: Fusion.get_current_gnss_calibration_std

Can you display the values returned from this method, and validate that the error reduces over time?
The system should return CALIBRATED once this procedure is finished and requires the system to move a few meters to do so.

Thank @mattrouss for your prompt response. Yes, it return NOT_CALIBRATED at first but then it turns to be CALIBRATED (Even I did not move anything!). The weird thing is that If I move the robot then it turns out to be like this:
(CALIBRATED, nan, array([ nan, 0.00342098, nan])) Test!!!
current_geopose_satus = fusion.get_geo_pose(current_geopose)

Hi @LNBC1411

It looks like it could be a covariance issue.
Are you able to use the recording and playback samples correctly?

No, Since I always stuck as GPSD initialize, I write my own GPSD to read msg from ROS TOPIC (connection refuse). Is that any other way we can set it up right?

When I try to use the live geotracking, the viewer pop up a totally black screen. Is that any problem that I have to solve? If the problem is covariance issue, how can I fix it

Hello @LNBC1411
It seems that you may have problems with your ingested GNSS data. Could you save your ingested GNSS data in the same format of the recording sample with corresponding SVO ?
It will helps us to understand what’s wrong with your data.

Can I record it as rosbag file? and How can I attach it here

Hi @LNBC1411,

It is currently not possible for us to replay ZED data using a rosbag, which is why we linked to the recording sample, which would allow us to test your data easily.

Have you tried setting up GPSD using our guide here: Setting up GNSS / RTK on Linux - Stereolabs ?

Dear @mattrouss , I tried to but cannot. Therefore, I publish the gps fix on the rostopic and get data from there. Does that works?

Hi @LNBC1411,

Yes that’s fine, you can share the data with a google drive link or equivalent.

1 Like