Working with 3D point clouds in ROS

A point cloud is a set of data points in 3D space. Such data is usually derived from time-of-flight, structured light or stereo reconstruction. Laser scanners such as the Hukuyo or Velodyne provide a planar scan or 3D coloured point cloud respectively.

The perception_pcl package is the PCL ROS interface stack. A previous post covered how to integrate the Point Cloud Library (PCL) with ROS.  This library offers a convenient set of classes for working with point clouds taking computational time into account. A concise overview of using PCL in ROS is available here.

The RealSense D435 camera uses active IR stereo to construct a depth stream at a resolution of up to 1280×720 at 90 FPS. The point cloud is simply a projection of the stereo reconstruction in 3D space. A simple visualisation of the depth stream published on the /camera/aligned_depth_to_infra1/image_raw ROS topic is shown in the Rqt figure below. Each pixel in this grayscale image represents the distance from the camera to the surface of the object.

Untitled6

Depth stream image from the RealSense D435 displayed in Rqt

Data type conversion

Using PCL within ROS requires working with different data types. The PCL library defines a more convenient generic data type pcl::Pointcloud which allows one to work with the individual points as objects rather than the entire raw data. ROS directly supports this data type and such messages can be passed within the ROS system and visualized in Rviz as sensor_msgs/PointCloud2.

In ROS, the Intel realsense2 nodelet outputs an RGBD point cloud on the ROS topic /camera/depth_registered/points. This is a ROS sensor_msgs/PointCloud2 message type which contains a header, the 2D structure of the point cloud, the actual point cloud data (x,y,z,r,g,b,depth) and other characteristics.

PCL does not directly support ROS message types. ROS however provides the pcl::fromROSMsg and pcl::toROSMsg functions for converting between these two point cloud datatypes.  The snippet of code below shows a ROS callback function that converts a ROS point cloud message into a PCL datatype and then publishes the output on a previously defined ROS topic:

 

void cameraCallback(const sensor_msgs::PointCloud2ConstPtr& cloud){
pcl::fromROSMsg(*cloud, *pcl_cloud);
pub.publish(pcl_cloud);
}

Visualisation in RViz

A point cloud that is represented using either the sensor_msgs/PointCloud2 or pcl::Pointcloud data type can be visualised directly within Rviz. Using RViz to visualize point clouds requires publishing to a topic that uses either of these message types. Simply open a terminal and type rviz , then select the appropriate topic containing PointCloud2 messages to visualise the point cloud. An example of this is shown in Integrating the RealSense D435 with ROS.

Visualisation in PCL

PCLVisualizer is the native visualiser provided by PCL. It can display point cloud data, normals, drawn shapes and multiple viewpoints, and is very useful for debugging purposes. A C++ class interface is provided for this purpose.

The snippet of code below shows how to create a visualization object and then use it to display a point cloud.

To initialise and open a 3D viewer use:

boost::shared_ptr createViewer(){
boost::shared_ptr viewer (new pcl::visualization::PCLVisualizer ("3D    Viewer"));
viewer->setBackgroundColor (0, 0, 0);
viewer->addCoordinateSystem (1.0);
viewer->initCameraParameters ();
return (viewer);
}

To update the viewer with a point cloud use:

void updateViewer (boost::shared_ptr viewer, pcl::PointCloud::ConstPtr cloud)
{
pcl::visualization::PointCloudColorHandlerRGBField rgb(cloud);
viewer->removePointCloud();
viewer->addPointCloud (cloud, rgb);
viewer->spinOnce();
}

Using the methods defined above, a point cloud can be viewed using PCLVisualizer in just two lines of code:

vis = createViewer();
vis.updateViewer(*pcl_cloud);

An example of a point cloud visualised using PCLVisualizer is shown below. The coordinate system is also shown where the forward, right and down directions are represented with blue, red and green arrows respectively.

Untitled4

A point cloud displayed in PCLVisualizer

A zoomed out version of the same pointcloud is shown below.

Untitled5

A zoomed out version of the same point cloud displayed in PCLVisualizer

The coordinate frame in PCLVisualizer is the same as the one shown in Intel’s realsense-viewer tool which is part of the camera’s SDK.

Untitled3

A point cloud displayed using Intel’s realsense-viewer tool

Selecting a visualisation method

The choice of visualiser for point clouds depends on the application and whether or not ROS is running.

Visualising without ROS

Realsense-viewer is a plug-and-play visualiser which is useful for seeing the effect of adjusting camera parameters, checking for firmware updates or quickly testing whether the camera works.

PCLVisualizer is ideal for debugging purposes, while developing point cloud based algorithms, in a fairly light-weight fashion.

Visualising with ROS

Visualising point clouds in ROS requires publishing sensor_msgs/PointCloud or sensor_msgs/PointCloud2 messages.

Rqt does not support 3D point cloud visualization, but it great for quickly visualising an RGBD image.

Rviz  is a heavy-weight visualiser that also supports other message types (point clouds can be visualized alongside other sensor data published within ROS).

Assembling point clouds

A common robotics application is to build a map representation from point clouds. Assuming you have a way of estimating the camera pose (transforms between consecutive point clouds as the robot moves along), you will need to assemble multiple point clouds to build a map (and display in RViz).

The laser_assembler package provides nodes that accumulate point clouds (using the provided tf data) which can later be fetched via the laser_assembler/AssembleScans2 ROS service.

Point clouds are a rich representation of our 3D world. ROS and PCL provide a nice set of tools for getting started with building applications that use this data.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s