Tutorial - Global Localization

This tutorial shows how to use the Global Localization (GNSS Fusion) API to fuse GNSS coordinates with the camera’s visual-inertial odometry, in order to compute the camera’s global position (latitude, longitude, altitude) using the Fusion API. To keep this tutorial self-contained and easy to run without any external hardware, the GNSS data is randomly generated rather than read from a real GNSS receiver. We assume that you have followed the Positional Tracking tutorial before.

This module requires a stereo camera equipped with an inertial sensor (IMU), such as the ZED 2/2i, ZED Mini, ZED X, ZED X Mini, ZED X Nano. The original ZED (no built-in IMU) is not supported.

Getting Started

First, download the latest version of the ZED SDK.
Download the Global Localization sample code in C++ or Python.

Code Overview

Open the camera and enable positional tracking

As in the previous tutorials, we create, configure and open the ZED. Two positional tracking parameters are mandatory here: enable_imu_fusion (to get the gravity direction from the IMU) and set_gravity_as_origin (to align the camera and GNSS reference frames).

1 // Open camera
2 sl::InitParameters init_params;
3 init_params.depth_mode = sl::DEPTH_MODE::NEURAL;
4 init_params.coordinate_system = sl::COORDINATE_SYSTEM::IMAGE;
5 init_params.coordinate_units = sl::UNIT::METER;
6 init_params.camera_resolution = sl::RESOLUTION::AUTO;
7 init_params.camera_fps = 60;
8 sl::Camera zed;
9 sl::ERROR_CODE camera_open_error = zed.open(init_params);
10 if (camera_open_error > sl::ERROR_CODE::SUCCESS) {
11     std::cerr << "[ZED][ERROR] Can't open ZED camera" << std::endl;
12     return EXIT_FAILURE;
13 }
14 
15 // Enable positional tracking
16 sl::PositionalTrackingParameters ptp;
17 ptp.initial_world_transform = sl::Transform::identity();
18 ptp.enable_imu_fusion = true;     // Enable IMU (for having the gravity direction)
19 ptp.set_gravity_as_origin = true; // Set gravity as origin for allowing GNSS to Camera initialization
20 auto positional_init = zed.enablePositionalTracking(ptp);
21 if (positional_init > sl::ERROR_CODE::SUCCESS) {
22     std::cerr << "[ZED][ERROR] Can't start tracking of camera" << std::endl;
23     return EXIT_FAILURE;
24 }

Publish the camera for Fusion

The Fusion module works on a Publish/Subscribe pattern: the camera publishes its data, and a Fusion object subscribes to it. Here, camera and Fusion run in the same process, communicating over shared memory.

1 // Enable camera publishing for fusion
2 sl::CommunicationParameters communication_parameters;
3 communication_parameters.setForSharedMemory();
4 zed.startPublishing(communication_parameters);
5 
6 // Run a first grab to start sending data
7 while (zed.grab() > sl::ERROR_CODE::SUCCESS)
8     ;

We create the Fusion object, initialize it, enable its own positional tracking (with GNSS fusion), and subscribe our camera to it using its serial number as an identifier.

1 // Create fusion object
2 sl::InitFusionParameters init_multi_cam_parameters;
3 init_multi_cam_parameters.coordinate_units = sl::UNIT::METER;
4 init_multi_cam_parameters.coordinate_system = sl::COORDINATE_SYSTEM::IMAGE;
5 init_multi_cam_parameters.output_performance_metrics = true;
6 init_multi_cam_parameters.verbose = true;
7 sl::Fusion fusion;
8 sl::FUSION_ERROR_CODE fusion_init_code = fusion.init(init_multi_cam_parameters);
9 if (fusion_init_code != sl::FUSION_ERROR_CODE::SUCCESS) {
10     std::cerr << "[Fusion][ERROR] Failed to initialize fusion, error: " << fusion_init_code << std::endl;
11     return EXIT_FAILURE;
12 }
13 
14 // Subscribe to camera
15 sl::CameraIdentifier uuid(zed.getCameraInformation().serial_number);
16 fusion.subscribe(uuid, communication_parameters, sl::Transform::identity());
17 
18 // Enable positional tracking, with GNSS fusion
19 sl::PositionalTrackingFusionParameters ptfp;
20 ptfp.enable_GNSS_fusion = true;
21 fusion.enablePositionalTracking(ptfp);

The python snippet above calls enable_positionnal_tracking (with the extra “n”) — this is the actual name exposed by the Fusion Python bindings.

Ingest GNSS data and retrieve the fused position

At each grab(), we get the camera-only pose, generate a fake GNSS position and ingest it into fusion, then call fusion.process() to retrieve both the fused camera pose and the geographic position (latitude, longitude, altitude) once the GNSS-to-camera alignment has converged.

1 unsigned number_detection = 0;
2 while (number_detection < 200) {
3     // Grab camera
4     if (zed.grab() <= sl::ERROR_CODE::SUCCESS) {
5         sl::Pose zed_pose;
6         zed.getPosition(zed_pose, sl::REFERENCE_FRAME::WORLD);
7     }
8 
9     // Ingest (fake) GNSS data, timestamped with the current camera timestamp
10     sl::GNSSData input_gnss = getGNSSData();
11     input_gnss.ts = zed.getTimestamp(sl::TIME_REFERENCE::IMAGE);
12     if (input_gnss.ts != sl::Timestamp(0))
13         fusion.ingestGNSSData(input_gnss);
14 
15     // Process fusion
16     if (fusion.process() == sl::FUSION_ERROR_CODE::SUCCESS) {
17         // Fused camera pose
18         sl::Pose fused_position;
19         sl::POSITIONAL_TRACKING_STATE current_state = fusion.getPosition(fused_position);
20 
21         // Global position (GNSS coordinate system)
22         sl::GeoPose current_geopose;
23         sl::GNSS_FUSION_STATUS current_geopose_status = fusion.getGeoPose(current_geopose);
24         if (current_geopose_status == sl::GNSS_FUSION_STATUS::OK) {
25             number_detection++;
26             double latitude, longitude, altitude;
27             current_geopose.latlng_coordinates.getCoordinates(latitude, longitude, altitude, false);
28             std::cout << "latitude = " << latitude << ", longitude = " << longitude << ", altitude = " << altitude << std::endl;
29         }
30         // Otherwise, the GNSS-to-ZED coordinate system alignment hasn't converged yet:
31         // it is an optimization problem that fits the ZED computed path to the GNSS computed path,
32         // so keep moving the camera until it converges.
33     }
34 }

Because this tutorial uses synthetic GNSS data, only the fusion mechanics are demonstrated. With a real GNSS receiver, you must provide the full GNSSData (coordinates, timestamp, position covariance, latitude/longitude/altitude standard deviation) for accurate results.

Close Fusion and the camera

1 fusion.close();
2 zed.close();

For more information on the Fusion workflow, coordinate frames and calibration, read the Global Localization module documentation.