Adding Positional Tracking in ROS

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/zed_node/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 of 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 to is /zed/zed_node/pose`.

Camera path

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

• /zed/zed_node/path_map: The history of the camera pose in Map frame
• /zed/zed_node/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 camera pose is instead continuously fixed using the StereoLabs tracking algorithm that combines visual information, space memory information and if using a “ZED-M,” inertial information.

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:

• ZED:

  $
  roslaunch zed_wrapper zed.launch

• ZED Mini:

  $
  roslaunch zed_wrapper zedm.launch

• ZED 2:

  $
  roslaunch zed_wrapper zed2.launch

• ZED 2i:

  $
  roslaunch zed_wrapper zed2i.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: odometryThe position is calculated as the sum of the movements relative to the previous position. Only 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-M) are fused to get a better 6 DoF pose

The ROS wrapper follows ROS REP105 conventions.

Running the tutorial

If you properly followed the ROS 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-M is connected or you have loaded an SVO file, you will receive the following stream of messages confirming that you are correctly subscribing to the ZED image topics:

$
[ INFO] [1536571277.284918933]: Received odom in 'odom' frame : X: 0.01 Y: 0.11 Z: 0.27 - R: -59.13 P: 29.17 Y: -38.27
$
[ INFO] [1536571277.308147447]: Received pose in 'map' frame : X: -0.02 Y: -0.04 Z: 0.03 - R: -3.36 P: -4.32 Y: -12.08
$
[ INFO] [1536571277.328975670]: Received odom in 'odom' frame : X: 0.03 Y: 0.11 Z: 0.28 - R: -62.26 P: 29.26 Y: -37.74
$
[ INFO] [1536571277.344163358]: Received pose in 'map' frame : X: -0.03 Y: -0.02 Z: 0.03 - R: -1.21 P: -1.00 Y: -13.21
$
[ INFO] [1536571277.372976799]: Received odom in 'odom' frame : X: 0.04 Y: 0.12 Z: 0.28 - R: -65.35 P: 27.69 Y: -36.27
$
[ INFO] [1536571277.375593051]: Received pose in 'map' frame : X: -0.04 Y: -0.01 Z: 0.02 - R: -0.24 P: 1.28 Y: -14.98
$
[ INFO] [1536571277.408942979]: Received pose in 'map' frame : X: -0.06 Y: -0.01 Z: 0.03 - R: -0.85 P: 2.67 Y: -17.63
$
[ INFO] [1536571277.416963850]: Received odom in 'odom' frame : X: 0.06 Y: 0.13 Z: 0.29 - R: -68.11 P: 26.79 Y: -33.93
$
[ INFO] [1536571277.417057266]: Received odom in 'odom' frame : X: 0.07 Y: 0.14 Z: 0.29 - R: -69.27 P: 25.64 Y: -29.91
$
[ INFO] [1536571277.442988345]: Received pose in 'map' frame : X: -0.05 Y: -0.01 Z: 0.03 - R: -0.85 P: 2.67 Y: -17.63
$
[ INFO] [1536571277.465031616]: Received odom in 'odom' frame : X: 0.09 Y: 0.16 Z: 0.30 - R: -72.27 P: 24.17 Y: -28.24
$
[ INFO] [1536571277.476189234]: Received pose in 'map' frame : X: -0.06 Y: -0.00 Z: 0.03 - R: -0.85 P: 2.66 Y: -17.63
$
[ INFO] [1536571277.476251272]: Received odom in 'odom' frame : X: 0.10 Y: 0.18 Z: 0.31 - R: -75.13 P: 23.33 Y: -25.71
$
[ INFO] [1536571277.509004649]: Received pose in 'map' frame : X: -0.07 Y: -0.01 Z: 0.04 - R: -2.80 P: 5.82 Y: -17.74
$
[ INFO] [1536571277.516935704]: Received odom in 'odom' frame : X: 0.11 Y: 0.19 Z: 0.31 - R: -73.16 P: 20.36 Y: -20.36
$
[ INFO] [1536571277.543752689]: Received pose in 'map' frame : X: -0.08 Y: -0.02 Z: 0.06 - R: -4.38 P: 7.86 Y: -17.73
$
[ INFO] [1536571277.561048985]: Received odom in 'odom' frame : X: 0.11 Y: 0.19 Z: 0.31 - R: -70.84 P: 16.76 Y: -14.78
$
[ INFO] [1536571277.575540826]: Received pose in 'map' frame : X: -0.09 Y: -0.02 Z: 0.07 - R: -5.79 P: 9.17 Y: -17.61
$
[ INFO] [1536571277.605050948]: Received odom in 'odom' frame : X: 0.10 Y: 0.20 Z: 0.32 - R: -68.37 P: 12.53 Y: -9.49
$
$
[...]

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 code

1
#include <ros/ros.h>
2
#include <geometry_msgs/PoseStamped.h>
3
#include <nav_msgs/Odometry.h>
4
5
#include "tf2/LinearMath/Quaternion.h"
6
#include "tf2/LinearMath/Matrix3x3.h"
7
8
#include <string>
9
10
#define RAD2DEG 57.295779513
11
12
/**
13
 * This tutorial demonstrates receiving ZED odom and pose messages over the ROS system.
14
 */
15
16
17
/**
18
 * Subscriber callbacks
19
 */
20
21
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
22
23
    // Camera position in map frame
24
    double tx = msg->pose.pose.position.x;
25
    double ty = msg->pose.pose.position.y;
26
    double tz = msg->pose.pose.position.z;
27
28
    // Orientation quaternion
29
    tf2::Quaternion q(
30
        msg->pose.pose.orientation.x,
31
        msg->pose.pose.orientation.y,
32
        msg->pose.pose.orientation.z,
33
        msg->pose.pose.orientation.w);
34
35
    // 3x3 Rotation matrix from quaternion
36
    tf2::Matrix3x3 m(q);
37
38
    // Roll Pitch and Yaw from rotation matrix
39
    double roll, pitch, yaw;
40
    m.getRPY(roll, pitch, yaw);
41
42
    // Output the measure
43
    ROS_INFO("Received odom in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f",
44
             msg->header.frame_id.c_str(),
45
             tx, ty, tz,
46
             roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG);
47
}
48
49
void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
50
51
    // Camera position in map frame
52
    double tx = msg->pose.position.x;
53
    double ty = msg->pose.position.y;
54
    double tz = msg->pose.position.z;
55
56
    // Orientation quaternion
57
    tf2::Quaternion q(
58
        msg->pose.orientation.x,
59
        msg->pose.orientation.y,
60
        msg->pose.orientation.z,
61
        msg->pose.orientation.w);
62
63
    // 3x3 Rotation matrix from quaternion
64
    tf2::Matrix3x3 m(q);
65
66
    // Roll Pitch and Yaw from rotation matrix
67
    double roll, pitch, yaw;
68
    m.getRPY(roll, pitch, yaw);
69
70
    // Output the measure
71
    ROS_INFO("Received pose in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f",
72
             msg->header.frame_id.c_str(),
73
             tx, ty, tz,
74
             roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG);
75
}
76
77
/**
78
 * Node main function
79
 */
80
int main(int argc, char** argv) {
81
    // Node initialization
82
    ros::init(argc, argv, "zed_tracking_subscriber");
83
    ros::NodeHandle n;
84
85
    // Topic subscribers
86
    ros::Subscriber subOdom    = n.subscribe("/zed/zed_node/odom", 10, odomCallback);
87
    ros::Subscriber subPose    = n.subscribe("/zed/zed_node/pose", 10, poseCallback);
88
89
    // Node execution
90
    ros::spin();
91
92
    return 0;
93
}

The code explained

The following is a brief explanation of the source code above.

1
void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
2
3
    // Camera position in map frame
4
    double tx = msg->pose.position.x;
5
    double ty = msg->pose.position.y;
6
    double tz = msg->pose.position.z;
7
8
    // Orientation quaternion
9
    tf2::Quaternion q(
10
        msg->pose.orientation.x,
11
        msg->pose.orientation.y,
12
        msg->pose.orientation.z,
13
        msg->pose.orientation.w);
14
15
    // 3x3 Rotation matrix from quaternion
16
    tf2::Matrix3x3 m(q);
17
18
    // Roll Pitch and Yaw from rotation matrix
19
    double roll, pitch, yaw;
20
    m.getRPY(roll, pitch, yaw);
21
22
    // Output the measure
23
    ROS_INFO("Received pose in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f",
24
             msg->header.frame_id.c_str(),
25
             tx, ty, tz,
26
             roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG);
27
}

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.

1
    double tx = msg->pose.position.x;
2
    double ty = msg->pose.position.y;
3
    double tz = msg->pose.position.z;

Extracting the position is straightforward since the data is stored in a vector of three floating-point elements.

1
    // Orientation quaternion
2
    tf2::Quaternion q(
3
        msg->pose.orientation.x,
4
        msg->pose.orientation.y,
5
        msg->pose.orientation.z,
6
        msg->pose.orientation.w);
7
8
    // 3x3 Rotation matrix from quaternion
9
    tf2::Matrix3x3 m(q);
10
11
    // Roll Pitch and Yaw from rotation matrix
12
    double roll, pitch, yaw;
13
    m.getRPY(roll, pitch, yaw);

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.

1
    ROS_INFO("Received pose in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f",
2
             msg->header.frame_id.c_str(),
3
             tx, ty, tz,
4
             roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG);

Finally, we can print the information received to the screen after converting the radian values to degrees.

The main function is very standard and is explained in detail in the “Talker/Listener” ROS tutorial.

It is important to note how the subscribers are defined:

1
    // Subscribers
2
    ros::Subscriber subOdom    = n.subscribe("/zed/zed_node/odom", 10, odomCallback);
3
    ros::Subscriber subPose    = n.subscribe("/zed/zed_node/pose", 10, poseCallback);

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/zed_node/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/zed_node/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.