Global Localization Overview

The ZED SDK’s Global Localization module enables real-time camera tracking and localization in a global coordinate system. It leverages the power of the ZED stereo camera and GNSS data to provide accurate and robust localization capabilities for your applications. By combining visual odometry with GNSS positioning, the ZED SDK can determine the camera’s precise position and orientation relative to a global reference frame.

The ZED SDK’s Global Localization feature not only provides real-time camera tracking and localization in a global coordinate system but also operates in GNSS-denied environments. This means that even when satellite-based GNSS data is unavailable or unreliable, the ZED SDK can still determine the camera’s global position with centimeter precision.

How It Works #

Global localization in the ZED SDK is achieved by fusing data from three key sources: stereo vision, IMU, and GNSS. By combining these three data sources, the ZED SDK can deliver robust and accurate localization. The fusion process involves advanced algorithms that merge the data from these sensors to create a cohesive and accurate estimate of the camera’s position and orientation. These algorithms dynamically adjust the weight assigned to each sensor’s input based on its real-time reliability and accuracy.

The different sensors have complementary strengths: each data source compensates for the weaknesses of the others. For example, when GNSS data is unreliable or unavailable, stereo vision and IMU continue to provide accurate positioning. Similarly, GNSS helps correct any drift that might accumulate in the visual odometry and IMU data over time. Through this integrated approach, the ZED SDK ensures continuous and precise global localization, even in challenging environments where one or more data sources may be compromised.

The next figure gives a high-level overview of the sensor fusion algorithm. More details and explanations are provided in dedicated subsections.

Data synchronization #

ZED cameras, IMU, and GNSS are distinct sensors operating at different acquisition frequencies and timestamps. Fusing them requires data captured at a simultaneous time. Unfortunately, most of the time this is not the case. ZED cameras provide data at a rate of 15 to 120 frames per second, GNSS typically returns data at a frequency of 1Hz, while IMU can stream data at 400 Hz.

This is why we need data synchronization between camera and GNSS. This block aims to provide well-synchronized ZED camera and GNSS data to the fusion algorithm. Allowing fusion between these sensors. More documentation is available here.

VIO / GNSS calibration #

Initially, VIO and GNSS trajectories are not aligned when the application starts. Aligning these two is crucial for sensor fusion, so we need to calibrate the system during an initialization period after the GNSS fix has been obtained. To speed up the process, we output an initial calibration as soon as limited data is available, and refine this calibration online as the system moves over time. More information is available here.

Sensor Fusion #

When VIO and GNSS are aligned, we start fusing visual-inertial data with GNSS. If GNSS drops, VIO will take over during GNSS outage.

Global Localization Output #

While sensor fusion is running, it is possible to retrieve the device’s global position and orientation. You can convert it into different coordinate system formats using the SDK’s conversion functions. A variety of coordinate formats supported by the SDK, along with detailed explanations on conversion, can be found here.

Getting Started #

This section provides a brief explanation of how to use the GeoTracking API alongside the ZED SDK API.

Setting Up Visual-Inertial Tracking #

To enable global localization tracking, we need to run visual-inertial odometry first on the camera. Start by opening the ZED camera to access both image and IMU data:

sl::Camera zed;
sl::InitParameters init_params;
init_params.depth_mode = sl::DEPTH_MODE::ULTRA;
init_params.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Y_UP;
init_params.coordinate_units = sl::UNIT::METER;
sl::ERROR_CODE camera_open_error = zed.open(init_params);
if (camera_open_error != sl::ERROR_CODE::SUCCESS) {
    std::cerr << "Can't open ZED camera" << std::endl;
    // Handle the error in your application.
}

init_params = sl.InitParameters(depth_mode=sl.DEPTH_MODE.ULTRA,
                                coordinate_units=sl.UNIT.METER,
                                coordinate_system=sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP)

# create the camera that will input the position from its odometry
zed = sl.Camera()
status = zed.open(init_params)
if status != sl.ERROR_CODE.SUCCESS:
    print("[ZED][ERROR] Camera Open : " + repr(status) + ". Exit program.")
    exit()

Next, enable the positional tracking module:

sl::PositionalTrackingParameters pose_tracking_params;
pose_tracking_params.mode = sl::POSITIONAL_TRACKING_MODE::GEN_2;
pose_tracking_params.enable_area_memory = false;
auto positional_init = zed.enablePositionalTracking(pose_tracking_params);
if (positional_init != sl::ERROR_CODE::SUCCESS) {
    std::cerr << "[ZED][ERROR] Can't start tracking of camera" << std::endl;
    // Handle the error in your application.
}

pose_tracking_params = sl.PositionalTrackingParameters()
pose_tracking_params.mode = sl.POSITIONAL_TRACKING_MODE.GEN_2()
pose_tracking_params.enable_area_memory = False
positional_init = zed.enable_positional_tracking(pose_tracking_params)
if positional_init != sl.ERROR_CODE.SUCCESS:
    print("[ZED][ERROR] Can't start tracking of camera : " + repr(status) + ". Exit program.")
    exit()

For precise tracking, set a Region of Interest (ROI) if certain areas of your ZED camera remain static during tracking (e.g. part of a vehicle is present in the field of view of the camera):

std::string roi_file_path = ""; // Set the path of your ROI file
sl::Mat mask_roi;
auto err = mask_roi.read(roi_file_path.c_str());
if (err == sl::ERROR_CODE::SUCCESS)
    zed.setRegionOfInterest(mask_roi, {sl::MODULE::ALL});
else
    std::cout << "Error loading Region of Interest file: " << err << std::endl;

mask_roi = sl.Mat()
err = mask_roi.read(opt.roi_mask_file)
if err == sl.ERROR_CODE.SUCCESS:
    zed.set_region_of_interest(mask_roi, [sl.MODULE.ALL])
else:
    print(f"Error loading Region of Interest file {opt.roi_mask_file}. Please check the path.")

Now that positional tracking is running, send the VIO data to the Sensor Fusion module with the startPublishing function:

zed.startPublishing();

configuration = sl.CommunicationParameters()
zed.start_publishing(configuration)

Setting Up Global Positional Tracking #

Once the ZED SDK starts publishing data, set up the Fusion object:

sl::Fusion fusion;
sl::InitFusionParameters init_fusion_param;
init_fusion_param.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP ;
init_fusion_param.coordinate_units = sl::UNIT::METER;
init_fusion_param.verbose = true;
sl::FUSION_ERROR_CODE fusion_init_code = fusion.init(init_fusion_param);
if (fusion_init_code != sl::FUSION_ERROR_CODE::SUCCESS) {
    std::cerr << "[Fusion][ERROR] Failed to initialize fusion, error: " << fusion_init_code << std::endl;
    // Handle the error in your application.
}

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

Subscribe to the data published by the ZED camera:

sl::CameraIdentifier uuid(zed.getCameraInformation().serial_number);
fusion.subscribe(uuid);

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

Set the calibration parameters for GNSS/VIO:

sl::GNSSCalibrationParameters gnss_calibration_parameter;
gnss_calibration_parameter.enable_reinitialization = false;
gnss_calibration_parameter.enable_translation_uncertainty_target = false;
gnss_calibration_parameter.gnss_vio_reinit_threshold = 5;
gnss_calibration_parameter.target_yaw_uncertainty = 1e-2;
gnss_calibration_parameter.gnss_antenna_position = sl::float3(0,0,0); // Set your antenna position

gnss_calibration_parameters = sl.GNSSCalibrationParameters()
gnss_calibration_parameters.target_yaw_uncertainty = 7e-3
gnss_calibration_parameters.enable_translation_uncertainty_target = False
gnss_calibration_parameters.target_translation_uncertainty = 15e-2
gnss_calibration_parameters.enable_reinitialization = False
gnss_calibration_parameters.gnss_vio_reinit_threshold = 5

Enable Fusion positional tracking:

sl::PositionalTrackingFusionParameters positional_tracking_fusion_parameters;
positional_tracking_fusion_parameters.enable_GNSS_fusion = true;
positional_tracking_fusion_parameters.gnss_calibration_parameters = gnss_calibration_parameter;
sl::FUSION_ERROR_CODE tracking_error_code = fusion.enablePositionalTracking(positional_tracking_fusion_parameters);
if (tracking_error_code != sl::FUSION_ERROR_CODE::SUCCESS) {
    std::cout << "[Fusion][ERROR] Could not start tracking, error: " << tracking_error_code << std::endl;
    // Handle the error in your application
}

positional_tracking_fusion_parameters = sl.PositionalTrackingFusionParameters()
positional_tracking_fusion_parameters.enable_GNSS_fusion = True
positional_tracking_fusion_parameters.gnss_calibration_parameters = gnss_calibration_parameters

tracking_error_code fusion.enable_positionnal_tracking(positional_tracking_fusion_parameters)
if(tracking_error_code != sl.FUSION_ERROR_CODE.SUCCESS):
    print("[Fusion][ERROR] Could not start tracking, error: ", tracking_error_code)

Retrieving Fused Data #

Start a loop to continuously retrieve and process fused data:

while (true) {
    // Call the `.grab()` function of the ZED camera to send data to Fusion:
    sl::ERROR_CODE zed_status = zed.grab();

    // Ingest your GNSS data using the ingestGNSSData method:
    sl::GNSSData input_gnss; // Set the input_gnss data with your data
    sl::FUSION_ERROR_CODE ingest_error = fusion.ingestGNSSData(input_gnss);
    if (ingest_error != sl::FUSION_ERROR_CODE::SUCCESS) {
        std::cout << "Ingest error occurred when ingesting GNSSData: " << ingest_error << std::endl;
    }

    // After grabbing the camera data, process it to extract position and geo-position information:
    if (fusion.process() == sl::FUSION_ERROR_CODE::SUCCESS) {
        // Retrieve the current position from the fusion object:
        sl::Pose fused_position;
        sl::POSITIONAL_TRACKING_STATE current_state = fusion.getPosition(fused_position);

        // Monitor the VIO/GNSS calibration progress using getCurrentGNSSCalibrationSTD method:
        float yaw_std;
        sl::float3 position_std;
        fusion.getCurrentGNSSCalibrationSTD(yaw_std, position_std);

        if (yaw_std != -1.f)
            std::cout << "GNSS State: " << current_geopose_status << ": calibration uncertainty yaw_std " << yaw_std << " rad position_std " << position_std[0] << " m, " << position_std[1] << " m, " << position_std[2] << " m\t\t\t\r";

        // Once calibration is complete, retrieve the GeoPose of your system:
        sl::GeoPose current_geopose;
        fusion.getGeoPose(current_geopose);
    }
}

while(True):
    # Call the `.grab()` function of the ZED camera to send data to Fusion:
    status = zed.grab()

    # Ingest your GNSS data using the ingestGNSSData method:
    input_gnss = sl.GNSSData() # Set the input_gnss data with your data
    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)

    # After grabbing the camera data, process it to extract position and geo-position information:
    if fusion.process() == sl.FUSION_ERROR_CODE.SUCCESS:
        # Retrieve the current position from the fusion object:
        fused_position = sl.Pose()
        current_state = fusion.get_position(fused_position)

        # Monitor the VIO/GNSS calibration progress using get_current_gnss_calibration_std
        print(calibration_std)

        # Once calibration is complete, retrieve the GeoPose of your system:
        current_geopose = sl.GeoPose()
        current_geopose_satus = fusion.get_geo_pose(current_geopose)

Complete samples #

To help you get started, we provide Geotracking samples on our GitHub repository. These samples allow you to view live fused GNSS global data, record GNSS sequences, and even replay them for testing purposes.

Troubleshooting #

Common troubleshot issue help is available here.