Tutorial - Using Camera Tracking

This tutorials shows how to get the position and orientation of the camera in real time. The program will loop until 1000 positions are grabbed. We assume that you have followed the previous tutorials.

Getting Started

First, download the latest version of the ZED SDK.
Download the Positional Tracking sample code in C++, Python or C#.

Code Overview

Open the camera

As in previous tutorials, we create, configure and open the ZED.

1 // Create a ZED camera object
2 Camera zed;
3 
4 // Set configuration parameters
5 InitParameters init_params;
6 init_params.camera_resolution = RESOLUTION::HD720; // Use HD720 video mode (default fps: 60)
7 init_params.coordinate_system = COORDINATE_SYSTEM::RIGHT_HANDED_Y_UP; // Use a right-handed Y-up coordinate system
8 init_params.coordinate_units = UNIT::METER; // Set units in meters
9 
10 // Open the camera
11 ERROR_CODE err = zed.open(init_params);
12 if (err != ERROR_CODE::SUCCESS)
13     exit(-1);

Enable positional tracking

Once the camera is opened, we must enable the positional tracking module with enablePositionalTracking() in order to get the position and orientation of the ZED.

1 // Enable positional tracking with default parameters
2 PositionalTrackingParameters tracking_parameters;
3 err = zed.enablePositionalTracking(tracking_parameters);
4 if (err != ERROR_CODE::SUCCESS)
5     exit(-1);

In the above example, we use the default tracking parameters set in the ZED SDK. For the list of available parameters, check the Tracking API docs.

Capture pose data

Now that motion tracking is enabled, we create a loop to grab and retrieve the camera position. The camera position is given by the class Pose. This class contains the translation and orientation of the camera, as well as image timestamp and tracking confidence.

A pose is always linked to a reference frame. The SDK provides two reference frames: REFERENCE_FRAME::WORLD and REFERENCE_FRAME::CAMERA. For more information, see the Coordinate Frames section.

In this tutorial, we retrieve the camera position in the World Frame.

1 // Track the camera position during 1000 frames
2 int i = 0;
3 sl::Pose zed_pose;
4 while (i < 1000) {
5     if (zed.grab() == ERROR_CODE::SUCCESS) {
6 
7         // Get the pose of the left eye of the camera with reference to the world frame
8         zed.getPosition(zed_pose, REFERENCE_FRAME::WORLD);
9 
10         // Display the translation and timestamp
11         printf("Translation: Tx: %.3f, Ty: %.3f, Tz: %.3f, Timestamp: %llu\n", zed_pose.getTranslation().tx, zed_pose.getTranslation().ty, zed_pose.getTranslation().tz, zed_pose.timestamp);
12 
13         // Display the orientation quaternion
14         printf("Orientation: Ox: %.3f, Oy: %.3f, Oz: %.3f, Ow: %.3f\n\n", zed_pose.getOrientation().ox, zed_pose.getOrientation().oy, zed_pose.getOrientation().oz, zed_pose.getOrientation().ow);
15         i++;
16     }
17 }

Inertial Data

If an IMU is available (ex: ZED 2, ZED Mini), the Positional Tracking module will fuse internal visual and inertial data to provide improved position tracking.

You can also access IMU data using the code below:

1 if (zed.getSensorsData(sensor_data, TIME_REFERENCE::IMAGE) == ERROR_CODE::SUCCESS) {
2      // Get IMU orientation
3      auto imu_orientation = sensor_data.imu.pose.getOrientation();
4      // Get IMU acceleration
5      auto acceleration = sensor_data.imu.linear_acceleration;
6      cout << "IMU Orientation: {" << imu_orientation << "}, Acceleration: {" << acceleration << "}\n";
7 }

For more information on Camera-IMU and other onboard sensors, check the Sensors section.

Close the Camera

After tracking the ZED camera position for 1000 frames, we disable the tracking module and close the camera.

1 // Disable positional tracking and close the camera
2 zed.disablePositionalTracking();
3 zed.close();

Advanced Example

To learn how to retrieve and display the live position and orientation of the camera in a 3D window, transform pose data and change coordinate systems and units, check the advanced Motion Tracking sample code.