Skip to navigation

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.

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

Enabling Positional Tracking

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

// Enable positional tracking with default parameters
sl::PositionalTrackingParameters tracking_parameters;
err = 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().

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

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.

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

Using a Motion Prior

Since ZED SDK 5.6, GEN_3 positional tracking can fuse the motion measured by your own source, such as wheel odometry, an external visual-inertial system, or the difference between two consecutive GNSS fixes. This motion prior keeps the tracking accurate through fast motion, low texture and occlusion. It constrains how far the camera can have moved between two images; the images still determine the final pose.

Give each measurement to ingestMotionPrior() as a RelativePoseData:

  • start_timestamp and end_timestamp: the time interval the motion was measured over, on the same clock as the images (getTimestamp(TIME_REFERENCE::IMAGE)). Feed the samples back to back: each interval starts where the previous one ended.
  • motion: the motion of your source over that interval, along its own axes, in the coordinate_units of InitParameters.
  • measured_axes: the axes your source actually measures, for example TX, TY and RZ for wheel encoders. Declaring an axis the source does not measure is the most common reason for a motion prior to degrade the tracking.
  • covariance: the uncertainty of the motion, 6x6 row-major, ordered [tx, ty, tz, rx, ry, rz], like Pose::pose_covariance. Only the measured axes are read.
  • sensor_transform: where your source is mounted relative to the camera, in the coordinate_system of InitParameters. Leave it at identity when the source measures along the axes of that coordinate system.
  • is_stationary: set it, with a small but non-zero covariance, when the vehicle is stopped, so the position holds perfectly still.
  • rejection_threshold: how far the motion may disagree with the images, as a multiple of its uncertainty, before it is discarded (default 3).

There is nothing to enable: the motion prior is used from the first sample on. Call ingestMotionPrior() before grab(), from any thread, as many times as your source produces samples. A source faster than the camera is used on every image; a slower one, or a sample that arrives after its image, corrects the trajectory over longer stretches, provided it arrives within 2 seconds. A missing sample is never an error: the tracking then uses the images alone.

// Wheel odometry on a ground robot, with InitParameters::coordinate_system set to
// COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD (ROS): X forward, Y left, Z up. The encoders
// measure along those very axes, so sensor_transform is left at identity.
sl::RelativePoseData prior;
prior.start_timestamp = previous_ts;
prior.end_timestamp = current_ts;
// Sideways stays 0: the wheels cannot move sideways, and sideways_variance says how sure of that they are.
prior.motion.setTranslation(sl::Translation(forward_motion, 0, 0));
prior.motion.setRotationVector(sl::float3(0, 0, heading_change));
prior.measured_axes = sl::POSE_AXIS::TX | sl::POSE_AXIS::TY | sl::POSE_AXIS::RZ;
prior.covariance[0 * 6 + 0] = forward_variance;
prior.covariance[1 * 6 + 1] = sideways_variance;
prior.covariance[5 * 6 + 5] = heading_variance;
zed.ingestMotionPrior(prior);

ingestMotionPrior() returns:

  • ERROR_CODE::INVALID_FUNCTION_CALL when the tracking is not enabled in GEN_3.
  • ERROR_CODE::INVALID_FUNCTION_PARAMETERS for an unusable sample: timestamps left at 0 or not ordered, no measured axis, unusable covariance, or invalid sensor_transform or rejection_threshold.
  • ERROR_CODE::INVALID_TIMESTAMP for a sample ending more than 2 seconds before the last grabbed image.

getPositionalTrackingStatus().relative_pose_fusion_status reports how the motion prior was used for each image, see Positional Tracking Status.

A wrong sensor_transform rotation is the most likely reason for a motion prior to be rejected. Your source must also have the same handedness as the selected coordinate system: convert the motion of a left-handed source before ingesting it.

The motion prior is also available in C# (IngestMotionPrior()) and in the Sensors API (Sensors::ingestMotionPrior(), see Multi-Camera Tracking). To replay a motion prior from an SVO2 file, record it into the file with the -ingest-motion-prior option of ZED SVO Editor.

Saving an Area Map

#include <sl/Camera.hpp>
#include <iostream>
#include <thread>
int main(int argc, char **argv) {
sl::Camera zed;
sl::InitParameters init_params;
init_params.depth_mode = sl::DEPTH_MODE::NONE;
init_params.coordinate_units = sl::UNIT::METER;
auto status = zed.open(init_params);
if (status != sl::ERROR_CODE::SUCCESS) {
std::cout << "Failed to open ZED: " << status << std::endl;
return -1;
}
// Enable GEN_3 Tracking
sl::PositionalTrackingParameters track_params;
track_params.enable_area_memory = true;
track_params.mode = sl::POSITIONAL_TRACKING_MODE::GEN_3;
status = zed.enablePositionalTracking(track_params);
if (status != sl::ERROR_CODE::SUCCESS) {
std::cout << "Failed to enable tracking: " << status << std::endl;
return -1;
}
// Main Capture Loop — record the space
std::cout << "Move the camera following the mapping procedure..." << std::endl;
while (true) {
if (zed.grab() == sl::ERROR_CODE::SUCCESS) {
sl::Pose pose;
zed.getPosition(pose);
// Add your exit condition here
if (/* exit condition */ false)
break;
}
}
// Save AREA (ASYNC)
std::string area_file = "environment.area";
status = zed.saveAreaMap(area_file.c_str());
if (status == sl::ERROR_CODE::SUCCESS) {
sl::AREA_EXPORTING_STATE export_state = zed.getAreaExportState();
while (export_state == sl::AREA_EXPORTING_STATE::RUNNING) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
export_state = zed.getAreaExportState();
}
if (export_state == sl::AREA_EXPORTING_STATE::SUCCESS) {
std::cout << "Successfully saved area map to " << area_file << std::endl;
} else {
std::cout << "Failed to save area map: "
<< sl::toString(export_state).c_str() << std::endl;
}
} else {
std::cout << "Failed to start area save: " << status << std::endl;
}
zed.disablePositionalTracking();
zed.close();
}

Relocalizing within an Area map

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

Code Example

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