Positional Tracking

Position with RVIZ

In this tutorial, you will learn in detail how to configure your own RVIZ session to see only the position data information that you require.

RVIZ provides plugins for visualizing the camera’s pose and its path over time.

Camera odometry

The Odometry plugin provides a clear visualization of the odometry of the camera (nav_msgs/Odometry) in the Map frame.

Key parameters:

Topic: Selects the odometry topic /zed/odom

Unreliable: Check this to reduce the latency of the messages

Position tolerance and Angle tolerance: set both to 0 to be sure to visualize all positional data

Keep: Set to the number of messages to visualize at a time

Shape: You can visualize odometry information as an arrow pointing in the direction of the X axis, or as a three Axes frame. Both shapes can be resized to your preference

As of ZED SDK v2.6, pose covariance is available if the spatial_memory parameter is set to false in the ZED launch file.

You can get a visual estimation of the covariance with the odometry plugin by checking the Covariance option.

The position covariance is visualized as an ellipsoid centered in the camera frame. The three orientation covariances are visualized as three 2D ellipses centered on the relative axis.

You can change the Scale factors to get a better visualization if the ellipsoid and the ellipses are too big (high covariance) or not visible (low covariance).

Camera pose

The Pose plugin provides a visualization of the position and orientation of the camera (geometry_msgs/PoseStamped) in the Map frame similar to the Odometry plugin, but the Keep parameter and the Covariance parameter are not available.

The Topic to be subscribed is /zed/pose.

As of ZED SDK v2.6, the pose covariance is available if the spatial_memory parameter is set to false in the ZED launch file and a new topic of type geometry_msgs/PoseWithCovarianceStamped is published.

Camera path

The ZED wrapper provides two different paths for the camera position and orientation:

/zed/path_map: The history of the camera pose in Map frame

/zed/path_odom: The history of the odometry of the camera in Map frame

Above you can see both the Pose (green) and the Odometry (red) paths.

The odometry pose is calculated with a pure “visual odometry” algorithm as the sum of the movement from one step to the next. It is therefore affected by drift.

The parameters to be configured are analogous to the parameters seen above for the Pose and Odometry plugins.

Position subscribing in C++

In this tutorial, you will learn how to write a simple C++ node that subscribes to messages of type
geometry_msgs/PoseStamped and nav_msgs/Odometry to retrieve the position and the orientation of the ZED camera in the Map and in the Odometry frames.

Introduction

Use the following command to connect the ZED camera to the ROS network:

$ roslaunch zed_wrapper zed.launch

The ZED node starts to publish messages about its position in the network only if there is another node that subscribes to the relative topic.

The ZED wrapper publishes two kinds of positions:

odometry: The position calculated as the sum of the movements relative to the previous position. Only the pure visual odometry is used

pose: The position calculated relative to the world map. Historical information about the environment is used and Inertial data (if using a ZED Mini) are fused to get a better 6 DoF pose

Running the tutorial

If you properly followed the installation guide, the executable of this tutorial has just compiled and you can run the subscriber node using the following command:

$ rosrun zed_tracking_sub_tutorial zed_tracking_sub

If the ZED node is running, and a ZED or ZED Mini is connected or you have loaded and SVO file, you will receive the following stream of messages confirming that your are correctly subscribing to the ZED image topics:

If you move your camera by hand, you will see how the position and orientations are updated in real-time, and how odom and pose will drift one by the other due to the fact that odom pose is pure odometry data and is not fixed.

The two callbacks are very similar; the only difference is that poseCallback receives messages of type geometry_msgs/PoseStampedand odomCallback receives messages of type nav_msgs/Odometry. These are similar but not identical.
However, the information extracted by the two topics is the same: camera position and camera orientation.

Extracting the orientation is less straightforward as it is published as a quaternion vector.
To convert the quaternion to a more readable form, we must first convert it to a 3x3 rotation matrix from which we can finally extract the three values for Roll, Pitch and Yaw in radians.

A ros::Subscriber is a ROS object that listens on the network and waits for its own topic message to be available.
When a message is received, it executes the callback assigned to it.

In this tutorial, we declared two subscribers to the pose data:

The subscriber to the topic /zed/odom calls the odomCallback function when it receives a message of type
nav_msgs/Odometry that matches that topic

The subscriber to the topic /zed/pose calls the poseCallback function when it receives a message of type
geometry_msgs/PoseStamped that matches that topic

Conclusion

The full source code of this tutorial is available on GitHub in the zed_tracking_sub_tutorial sub-package.
Along with the node source code, you can find the package.xml and CMakeLists.txt files that complete the tutorial package.