Adding Positional Tracking in ROS 2

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

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