PointCloud2 message explained

This message structure is been widely used for storing point clouds in the ROS framework. It seems to be the case that a detailed documentation of this message structure (what each field meant and its usage thereof) was found to be lacking. This blog aims to fill this gap and offer rospy and roscpp implementations for the same. In particular, how 3D information along with RGB and intensity is stored in a 1D array in this message structure is detailed here. But first,

The number at the end of the name has to mean something.

PointCloud2 is the successor of PointCloud. The reason for version 2 for this message type is speculated here. According to that blog,

The key takeaway in short for PointCloud2 is efficiency.

Now that, that is out of the way, let’s dive into the how the message is packaged.

Message Structure

https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/PointCloud2.html

Going through what each of these fields one by one, Header stores information such as time stamp and frame_id.

The height of the message denotes the number of vertical channels the device has. For example, Velodyne offers LiDARs ranging from 8–256 channels. If one is using a depth camera, the height of the image corresponds to this value.

The width contains the number of points per vertical channel.

The fields parameter is of sensor_msgs/PointField type. I interpret this as similar to a C++ struct. Each field, corresponds to a particular attribute of the point cloud you have. It takes a name (attribute), offset, datatype and count as its argument.

If one notices how the actual point data is arranged in the PointCloud2 message, it is in the fashion of an 1D array. This is highly non-intuitive from a user perspective yet it is implemented so in the name of efficiency and scalability. The offset parameter helps in differentiating the information of one point data from another in this 1D array.

PointField tells how the data is arranged and what each element means in the 1D array.

Each PointField structure takes up an attribute (x, y, z, intensity, rgb), the start index (i.e offset), dtype of the message and count. Depending on the dtype of the message, the number of bytes allotted for each attribute changes.

Offset tells the start index of each attribute of a point data in the 1D array of bytes.

is_bigendian (is big endian) conveys the direction by which the bytes are read. More on big endian and small endian, here.

point_step, as the documentation suggests is the length of a point in bytes.
Depending on the attributes (fields) the point comprise of and the dtype of the attribute,

point_step = len(fields) * dtype

row_step, determines the length of a row in bytes as suggested by the documentation. This is calculated by the following equations,

total_number_of_points = height * width

row_step = total_number_of_points * point_step

is_dense, if is set to True, assumes that all points are valid.

data, as mentioned before is a 1D array. But depending on whether you are using rospy or roscpp, this field is created and populated in different ways.

Python Boilerplate Code

from sensor_msgs.msg import PointCloud2
from std_msgs.msg import Header

pointcloud_msg = PointCloud2()
pointcloud_msg.header = Header()
pointcloud_msg.header.frame_id = "frame"

# Define the point fields (attributes)        
fields =[PointField('x', 0, PointField.FLOAT32, 1),
        PointField('y', 4, PointField.FLOAT32, 1),
        PointField('z', 8, PointField.FLOAT32, 1),
        PointField('intensity', 12, PointField.FLOAT32,1),
        ]

pointcloud_msg.fields = fields

pointcloud_msg.height = height
pointcloud_msg.width = width

# Float occupies 4 bytes. Each point then carries 16 bytes.
pointcloud_msg.point_step = len(fields) * 4 

total_num_of_points = pointcloud_msg.height * pointcloud_msg.width
pointcloud_msg.row_step = pointcloud_msg.point_step * total_num_of_points
pointcloud_msg.is_dense = True

One can create a numpy array with dimensions (height, width, len(fields)) and populate each point while looping through it.

C++ Boilerplate Code

We use pointcloud iterators to populate the message.

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointField.h>
#include <sensor_msgs/point_cloud2_iterator.h>


sensor_msgs::PointCloud2 pcl_msg;
    
//Modifier to describe what the fields are.
sensor_msgs::PointCloud2Modifier modifier(pcl_msg);

modifier.setPointCloud2Fields(4,
    "x", 1, sensor_msgs::PointField::FLOAT32,
    "y", 1, sensor_msgs::PointField::FLOAT32,
    "z", 1, sensor_msgs::PointField::FLOAT32,
    "intensity", 1, sensor_msgs::PointField::FLOAT32);

//Msg header
pcl_msg.header = std_msgs::Header();
pcl_msg.header.stamp = ros::Time::now();
pcl_msg.header.frame_id = "frame";

pcl_msg.height = height;
pcl_msg.width = width;
pcl_msg.is_dense = true;

//Total number of bytes per point
pcl_msg.point_step = 16;
pcl_msg.row_step = pcl_msg.point_step * pcl_msg.width;
pcl_msg.data.resize(pcl_msg.row_step);

//Iterators for PointCloud msg
sensor_msgs::PointCloud2Iterator<float> iterX(pcl_msg, "x");
sensor_msgs::PointCloud2Iterator<float> iterY(pcl_msg, "y");
sensor_msgs::PointCloud2Iterator<float> iterZ(pcl_msg, "z");
sensor_msgs::PointCloud2Iterator<float> iterIntensity(pcl_msg, "intensity");

//iterate over the message and populate the fields.      
{
  *iterX = //Your x data
  *iterY = //Your y data
  *iterZ = //Your z data
  *iterIntensity = //Your intensity data

  // Increment the iterators
  ++iterX;
  ++iterY;
  ++iterZ;
  ++iterIntensity;
}

To wrap up,

The purpose of this write up was to describe how the PointCloud2 message is structured and how to use it in both in roscpp and rospy. These inferences were derived from personal experiments and implementations, hence decided to pen it down. If these insights aren’t accurate, feel free to criticise in the comments. A detailed documentation on this topic is lacking, hence any good write up is valuable.

Cheers.