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:

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

Next, enable the positional tracking module:

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

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

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

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

1
zed.startPublishing();

Setting Up Global Positional Tracking

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

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

Subscribe to the data published by the ZED camera:

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

Set the calibration parameters for GNSS/VIO:

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

Enable Fusion positional tracking:

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

Retrieving Fused Data

Start a loop to continuously retrieve and process fused data:

1
while (true) {
2
    // Call the `.grab()` function of the ZED camera to send data to Fusion:
3
    sl::ERROR_CODE zed_status = zed.grab();
4
5
    // Ingest your GNSS data using the ingestGNSSData method:
6
    sl::GNSSData input_gnss; // Set the input_gnss data with your data
7
    sl::FUSION_ERROR_CODE ingest_error = fusion.ingestGNSSData(input_gnss);
8
    if (ingest_error != sl::FUSION_ERROR_CODE::SUCCESS) {
9
        std::cout << "Ingest error occurred when ingesting GNSSData: " << ingest_error << std::endl;
10
    }
11
12
    // After grabbing the camera data, process it to extract position and geo-position information:
13
    if (fusion.process() == sl::FUSION_ERROR_CODE::SUCCESS) {
14
        // Retrieve the current position from the fusion object:
15
        sl::Pose fused_position;
16
        sl::POSITIONAL_TRACKING_STATE current_state = fusion.getPosition(fused_position);
17
18
        // Monitor the VIO/GNSS calibration progress using getCurrentGNSSCalibrationSTD method:
19
        float yaw_std;
20
        sl::float3 position_std;
21
        fusion.getCurrentGNSSCalibrationSTD(yaw_std, position_std);
22
23
        if (yaw_std != -1.f)
24
            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";
25
26
        // Once calibration is complete, retrieve the GeoPose of your system:
27
        sl::GeoPose current_geopose;
28
        fusion.getGeoPose(current_geopose);
29
    }
30
}

Complete samples

To help you get started, we provide Global localization 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.