Address SDK camera_to_geo issue

I am attempting to find a way to project from world coordinates to geo coordinates with a calibrated fusion instance. To do this I am using a custom function to project from local coordinates to the geo coordinates with fusion.camera_to_geo.
This current implementation is inspired by a suggestion from @TanguyHardelin following the discovery of an issue with SDK V 4.1.1.



import pandas as pd
import as sl
import json
import cv2
from datetime import datetime
import numpy as np
import matplotlib.pyplot as plt
import numpy as np

def gnss_str_to_pyzed(data:str) -> sl.GNSSData:
    Converts a GNSS string to a PyZED GNSSData object.

        data (str): The GNSS string to be converted.

        sl.GNSSData: The converted GNSSData object.

    data = json.loads(data)
    gps_data = sl.GNSSData()

    sl_fix_mode = sl.GNSS_MODE.FIX_2D
    sl_status = sl.GNSS_STATUS.SINGLE
    gps_data.gnss_mode = sl_fix_mode.value
    gps_data.gnss_status = sl_status.value

    date_time = datetime.fromisoformat(data["timestamp"])
    zed_ts = sl.Timestamp()
    gps_data.ts = zed_ts
    covariances = json.loads(data["covariance"])
    gps_data.position_covariances = [float(x) for x in covariances]
    return gps_data

def xyz_to_georectified(xyz, fusion) -> np.ndarray:
    Converts XYZ coordinates to georectified coordinates using the given fusion object.

        xyz (np.ndarray): Array of XYZ coordinates.
        fusion: The fusion object used for conversion.

        np.ndarray: Array of georectified coordinates in the format [longitude, latitude, elevation].
    georectified_coords = []
    pose = sl.Pose()
    geopose = sl.GeoPose()

    for point in xyz.tolist():
        translation = sl.Translation()
        transform = sl.Transform()
        translation.init_vector(point[0], point[1], point[2])
        status = fusion.camera_to_geo(pose, geopose)
        lat, lon, elevation = geopose.latlng_coordinates.get_coordinates(False)
        georectified_coords.append([lon, lat, elevation])

    return georectified_coords

def plot_computed_vs_true_positions(positions_df:pd.DataFrame):
    fig, ax = plt.subplots()

def main(input_svo_file):
    zed_pose = sl.Pose()
    py_translation = sl.Translation()
    text_translation = ""
    text_rotation = ""

    init_params = sl.InitParameters(depth_mode=sl.DEPTH_MODE.ULTRA,


    # create the camera that will input the position from its odometry
    zed = sl.Camera()
    status =
    if status != sl.ERROR_CODE.SUCCESS:
        print("[ZED][ERROR] Camera Open : " + repr(status) + ". Exit program.")
    # 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.")

    # Display
    display_resolution = sl.Resolution(1280, 720)
    left_img = sl.Mat()

    # Create Fusion object:

    fusion = sl.Fusion()
    init_fusion_param = sl.InitFusionParameters()
    init_fusion_param.coordinate_units = sl.UNIT.METER
    init_fusion_param.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP
    init_fusion_param.verbose = True

    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()

    uuid = sl.CameraIdentifier(zed.get_camera_information().serial_number)
    fusion.subscribe(uuid, configuration, sl.Transform(0, 0, 0))

    # Enable positional tracking for Fusion object
    positional_tracking_fusion_parameters = sl.PositionalTrackingFusionParameters()
    positional_tracking_fusion_parameters.enable_GNSS_fusion = True
    gnss_calibration_parameters = sl.GNSSCalibrationParameters()
    gnss_calibration_parameters.target_yaw_uncertainty = 7e-3
    gnss_calibration_parameters.enable_translation_uncertainty_target = True
    gnss_calibration_parameters.target_translation_uncertainty = 15e-2
    gnss_calibration_parameters.enable_reinitialization = False
    gnss_calibration_parameters.gnss_vio_reinit_threshold = 5
    gnss_calibration_parameters.gnss_antenna_position = np.asarray([0.01,.01,0.01])
    positional_tracking_fusion_parameters.gnss_calibration_parameters = gnss_calibration_parameters


    last_timestamp = sl.Timestamp()

    positions = []
    is_running = True
    while is_running:
        if zed.grab() == sl.ERROR_CODE.SUCCESS:
            zed.get_position(zed_pose, sl.REFERENCE_FRAME.WORLD)
            zed.retrieve_image(left_img, sl.VIEW.LEFT, sl.MEM.CPU, display_resolution)
            current_ts = zed.get_timestamp(sl.TIME_REFERENCE.IMAGE)
            current_ts.set_nanoseconds(int(current_ts.get_nanoseconds() + 5e6))
            cv2.imshow("left", left_img.numpy())

        elif zed.grab() == sl.ERROR_CODE.END_OF_SVOFILE_REACHED:
            print("End of SVO file.")
            is_running = False

        data_map = {}
        ing = zed.retrieve_svo_data("GNSS",data_map,last_timestamp,current_ts)
        data_map = [(k,v) for k,v in data_map.items()]
        for data in data_map:
            data_string = data[1].get_content_as_string()
            input_gnss = gnss_str_to_pyzed(data_string)

            ingest_error = fusion.ingest_gnss_data(input_gnss)

            if ingest_error != sl.FUSION_ERROR_CODE.SUCCESS and ingest_error != sl.FUSION_ERROR_CODE.NO_NEW_DATA_AVAILABLE:
                print("Ingest error occurred when ingesting GNSSData: ", ingest_error)

        last_timestamp = current_ts

        # get the fused position
        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)
            if current_state == sl.POSITIONAL_TRACKING_STATE.OK:                    
                image_geopose = sl.GeoPose()
                current_geopose_status = fusion.get_geo_pose(image_geopose)

                if current_geopose_status == sl.GNSS_FUSION_STATUS.OK:

                    lat,lon,elev= image_geopose.latlng_coordinates.get_coordinates(False)
                    x,y,z = fused_position.get_translation().get().tolist()

                    _, yaw_std, position_std = fusion.get_current_gnss_calibration_std()
                    if yaw_std != -1 and yaw_std<1:
                        gnss_vio_matrix = fusion.get_geo_tracking_calibration().m

                        xyz_np = np.array([x,y,z,1]).T
                        xyz_np =,xyz_np)
                        x,y,z = xyz_np[:3]
                        position_data = {

    positions_df = pd.DataFrame(positions)
    positions_df_xyz = positions_df[["x","y","z"]].to_numpy()
    positions_df_georectified = xyz_to_georectified(positions_df_xyz,fusion)
    # add computed georectified coordinates to the dataframe
    positions_df["lon_computed"] = [x[0] for x in positions_df_georectified]
    positions_df["lat_computed"] = [x[1] for x in positions_df_georectified]
    positions_df["elev_computed"] = [x[2] for x in positions_df_georectified]


    return positions_df

if __name__ == "__main__":

    input_svo_file = "camera_recording_0.svo2"
    positions_df: pd.DataFrame = main(input_svo_file)

From the image below, you can see that the current approach does not work.


Hello @fdundar,
I reviewed your code and I noted few things:

  • The position you want to transform into geographic coordinates must be in the “IMAGE” coordinate system. If it is not, you will need to either change the transformation formula or project your position into the “IMAGE” coordinate system before applying the transformation.
  • Once you use the VIO / GNSS calibration matrix the resulted position is not straight away in lat/long coordinate format. It is in EDN (East - Down - North) actually. You must convert it in ENU and then you could use library like pymap3d for convert it back to lat / long coordinate format.
  • In your example, you have omitted the antenna transformation between the GNSS and camera. This transformation is important as it is part of the GNSS-VIO model. Make sure to include this transformation in your code.

I have made the necessary modifications to your code based on the insights provided. After applying these changes, I used the generated CSV file and observed the following results.

I attached the modified code at the end of this message. Do not hesitate if you have questions.

Tanguy (10.0 KB)

1 Like

Much thanks @TanguyHardelin!!!