DevelopmentZED SDKModulesPositional Tracking

Using the Positional Tracking API

Open in ClaudeOpen in ChatGPT

Positional Tracking Configuration

To configure positional tracking, use InitParameters at initialization and RuntimeParameters to change specific parameters during use.

1// Set configuration parameters
2InitParameters init_params;
3init_params.camera_resolution = RESOLUTION::HD720; // Use HD720 video mode (default fps: 60)
4init_params.coordinate_system = COORDINATE_SYSTEM::RIGHT_HANDED_Y_UP; // Use a right-handed Y-up coordinate system
5init_params.coordinate_units = UNIT::METER; // Set units in meters

Enabling Positional Tracking

After opening the camera, enable positional tracking using enablePositionalTracking() with default PositionalTrackingParameters.

1// Enable positional tracking with default parameters
2sl::PositionalTrackingParameters tracking_parameters;
3err = zed.enablePositionalTracking(tracking_parameters);

You can disable tracking anytime using disablePositionalTracking().

For more information on positional tracking settings, refer to the Positional Tracking Settings page.

Getting Pose

The camera position gets updated with every new frame.

To retrieve pose data, use getPosition() after grabbing a frame.

In the following example, we extract the pose relative to the World Frame and retrieve the translation and orientation quaternion values using getTranslation() and getOrientation().

1sl::Pose zed_pose;
2if (zed.grab() == ERROR_CODE::SUCCESS) {
3    // Get the pose of the camera relative to the world frame
4    POSITIONAL_TRACKING_STATE state = zed.getPosition(zed_pose, REFERENCE_FRAME::WORLD);
5    // Display translation and timestamp
6    printf("Translation: tx: %.3f, ty:  %.3f, tz:  %.3f, timestamp: %llu\r",
7    zed_pose.getTranslation().tx, zed_pose.getTranslation().ty, zed_pose.getTranslation().tz, zed_pose.timestamp);
8    // Display orientation quaternion
9    printf("Orientation: ox: %.3f, oy:  %.3f, oz:  %.3f, ow: %.3f\r",
10    zed_pose.getOrientation().ox, zed_pose.getOrientation().oy, zed_pose.getOrientation().oz, zed_pose.getOrientation().ow);
11}

The class Pose is used to store camera position and additional information such as timestamp and confidence. Since position is always relative to a reference, it is important to set the coordinate frame that will be used as base. To get camera position in real world space, use REFERENCE_FRAME::WORLD, otherwise use REFERENCE_FRAME::CAMERA to get the change in pose relative to the last position (odometry).

By default, the pose of the left eye of the camera is returned. To get the pose at the center of the camera, see Frame Transforms. You can also retrieve a translation, orientation or rotation matrix using getTranslation(), getOrientation() and getRotation().

Getting Velocity

The velocity gets updated with every new frame. To retrieve velocity data, use getPosition() after grabbing a frame. In the following example, we extract the velocity relative to the Camera Frame and retrieve the linear and angular velocities along the [x,y,z] axes.

1sl::Pose zed_pose;
2if (zed.grab() == ERROR_CODE::SUCCESS) {
3    // Get the pose of the camera relative to the world frame
4    POSITIONAL_TRACKING_STATE state = zed.getPosition(zed_pose, REFERENCE_FRAME::CAMERA);
5    // Display linear velocity
6    printf("Linear Twist: vx: %.3f, vy:  %.3f, vz:  %.3f, timestamp: %llu\r",
7    zed_pose.twist[0], zed_pose.twist[1], zed_pose.twist[2], zed_pose.timestamp);
8    // Display orientation quaternion
9    printf("Angular Twist: x: %.3f, y:  %.3f, z:  %.3f, timestamp: %llu\r",
10    zed_pose.twist[3], zed_pose.twist[4], zed_pose.twist[5], zed_pose.timestamp);
11}

Saving an Area Map

1#include <sl/Camera.hpp>
2#include <iostream>
3#include <thread>
4
5int main(int argc, char **argv) {
6    sl::Camera zed;
7
8    sl::InitParameters init_params;
9    init_params.depth_mode = sl::DEPTH_MODE::NONE;
10    init_params.coordinate_units = sl::UNIT::METER;
11
12    auto status = zed.open(init_params);
13    if (status != sl::ERROR_CODE::SUCCESS) {
14        std::cout << "Failed to open ZED: " << status << std::endl;
15        return -1;
16    }
17
18    // Enable GEN3 Tracking
19    sl::PositionalTrackingParameters track_params;
20    track_params.enable_area_memory = true;
21    track_params.mode = sl::POSITIONAL_TRACKING_MODE::GEN3;
22
23    status = zed.enablePositionalTracking(track_params);
24    if (status != sl::ERROR_CODE::SUCCESS) {
25        std::cout << "Failed to enable tracking: " << status << std::endl;
26        return -1;
27    }
28
29    // Main Capture Loop — record the space
30    std::cout << "Move the camera following the mapping procedure..." << std::endl;
31
32    while (true) {
33        if (zed.grab() == sl::ERROR_CODE::SUCCESS) {
34            sl::Pose pose;
35            zed.getPosition(pose);
36
37            // Add your exit condition here
38            if (/* exit condition */ false)
39                break;
40        }
41    }
42
43    // Save AREA (ASYNC)
44    std::string area_file = "environment.area";
45    status = zed.saveAreaMap(area_file.c_str());
46
47    if (status == sl::ERROR_CODE::SUCCESS) {
48
49        sl::AREA_EXPORTING_STATE export_state = zed.getAreaExportState();
50
51        while (export_state == sl::AREA_EXPORTING_STATE::RUNNING) {
52            std::this_thread::sleep_for(std::chrono::milliseconds(10));
53            export_state = zed.getAreaExportState();
54        }
55
56        if (export_state == sl::AREA_EXPORTING_STATE::SUCCESS) {
57            std::cout << "Successfully saved area map to " << area_file << std::endl;
58        } else {
59            std::cout << "Failed to save area map: " 
60                      << sl::toString(export_state).c_str() << std::endl;
61        }
62
63    } else {
64        std::cout << "Failed to start area save: " << status << std::endl;
65    }
66
67    zed.disablePositionalTracking();
68    zed.close();
69}

Relocalizing within an Area map

1// Enable positional tracking with default parameters
2sl::PositionalTrackingParameters tracking_parameters;
3tracking_parameters.area_file_path = "example.area";
4err = zed.enablePositionalTracking(tracking_parameters);

Code Example

For code examples, check out the Tutorial and Sample on GitHub.