Segmentation is the process of partitioning a dataset into different blocks of data satisfying certain criteria. The segmentation process can be done in many different ways and with varied criteria; sometimes, it may involve extracting structured information from a point cloud based on a statistical property, and in other cases, it can simply require extracting points in a specific color range.
In many cases, our data might fit a specific mathematical model, such as a plane, line, or sphere, among others. When this is the case, it is possible to use a model estimation algorithm to calculate the parameters for the model that fits our data. With those parameters, it is then possible to extract the points belonging to that model and evaluate how well they fit it.
In this example, we are going to show how to perform model-based segmentation of a point cloud. We are going to constrain ourselves to a planar model, which is one of the most common mathematical models you can usually fit to a point cloud. For this example, we will also perform the model estimation using a widespread algorithm named Random Sample Consensus (RANSAC), which is an iterative algorithm capable of performing accurate estimations even in the presence of outliers.
The example code can be found in the chapter10_tutorials package, and its named pcl_planar_segmentation.cpp:
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <sensor_msgs/PointCloud2.h>
class cloudHandler
{
public:
cloudHandler()
{
pcl_sub = nh.subscribe("pcl_downsampled", 10,
&cloudHandler::cloudCB, this); pcl_pub =
nh.advertise<sensor_msgs::PointCloud2>("pcl_segmented",
1); ind_pub =
nh.advertise<pcl_msgs::PointIndices>("point_indices", 1); coef_pub =
nh.advertise<pcl_msgs::ModelCoefficients>("planar_coef",
1);
}
void cloudCB(const sensor_msgs::PointCloud2 &input)
{
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::PointCloud<pcl::PointXYZ> cloud_segmented;
pcl::fromROSMsg(input, cloud);
pcl::ModelCoefficients coefficients;
pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
pcl::SACSegmentation<pcl::PointXYZ> segmentation;
segmentation.setModelType(pcl::SACMODEL_PLANE);
segmentation.setMethodType(pcl::SAC_RANSAC);
segmentation.setMaxIterations(1000);
segmentation.setDistanceThreshold(0.01);
segmentation.setInputCloud(cloud.makeShared());
segmentation.segment(*inliers, coefficients);
pcl_msgs::ModelCoefficients ros_coefficients;
pcl_conversions::fromPCL(coefficients, ros_coefficients);
ros_coefficients.header.stamp = input.header.stamp;
coef_pub.publish(ros_coefficients);
pcl_msgs::PointIndices ros_inliers;
pcl_conversions::fromPCL(*inliers, ros_inliers);
ros_inliers.header.stamp = input.header.stamp;
ind_pub.publish(ros_inliers);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud.makeShared());
extract.setIndices(inliers);
extract.setNegative(false);
extract.filter(cloud_segmented);
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(cloud_segmented, output);
pcl_pub.publish(output);
}
protected:
ros::NodeHandle nh;
ros::Subscriber pcl_sub;
ros::Publisher pcl_pub, ind_pub, coef_pub;
};
main(int argc, char **argv)
{
ros::init(argc, argv, "pcl_planar_segmentation");
cloudHandler handler;
ros::spin();
return 0;
}
As the reader might have noticed, two new message types are being used in the advertised topics. As their names suggest, the ModelCoefficients messages store the coefficients of a mathematical model, and PointIndices stores the indices of the points of a point cloud. We will publish these as an alternative way of representing the extracted information, which could then be used in combination with the original point cloud (pcl_downsampled) to extract the correct point. As a hint, this can be done by setting the timestamp of the published objects to the same timestamp of the original point cloud message and using ROS message filters:
pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_segmented",
1);ind_pub = nh.advertise<pcl_msgs::PointIndices>("point_indices",
1);coef_pub =
nh.advertise<pcl_msgs::ModelCoefficients>("planar_coef", 1);
As always, in the callback function, we perform the conversion from the PointCloud2 message to the point cloud type. In this case, we also define two new objects that correspond to the native ModelCoefficients and PointIndices types, which will be used by the segmentation algorithm:
pcl::PointCloud<pcl::PointXYZ> cloud; pcl::PointCloud<pcl::PointXYZ> cloud_segmented; pcl::fromROSMsg(input, cloud); pcl::ModelCoefficients coefficients; pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
The segmentation algorithm lets us define ModelType and MethodType, with the former being the mathematical model we are looking to fit and the latter being the algorithm to use. As we explained before, we are using RANSAC due to its robustness against outliers. The algorithm also lets us define the two stopping criteria: the maximum number of iterations (setMaxIterations) and the maximum distance to the model (setDistanceThreshold). With those parameters set, plus the input point cloud, the algorithm can then be performed, returning the inliers (points which fall in the model) and the coefficients of the model:
pcl::SACSegmentation<pcl::PointXYZ> segmentation; segmentation.setModelType(pcl::SACMODEL_PLANE); segmentation.setMethodType(pcl::SAC_RANSAC); segmentation.setMaxIterations(1000);
segmentation.setDistanceThreshold(0.01); segmentation.setInputCloud(cloud.makeShared()); segmentation.segment(*inliers, coefficients);
Our next step is to convert and publish the inliers and the model coefficients. As usual, conversions are performed with the standard functions, but you might notice that the namespace and signature of the conversion function is different from the one being used for point cloud conversions. To further improve this example, these messages also include the timestamp of the original point cloud in order to link them together. This also allows the use of the ROS message filters on other nodes to create callbacks containing objects that are linked together:
pcl_msgs::ModelCoefficients ros_coefficients; pcl_conversions::fromPCL(coefficients, ros_coefficients); ros_coefficients.header.stamp = input.header.stamp; coef_pub.publish(ros_coefficients); pcl_msgs::PointIndices ros_inliers; pcl_conversions::fromPCL(*inliers, ros_inliers); ros_inliers.header.stamp = input.header.stamp; ind_pub.publish(ros_inliers);
In order to create the segmented point cloud, we extract the inliers from the point cloud. The easiest way to do this is with the ExtractIndices object, but it could be easily done by simply looping through the indices and pushing the corresponding points into a new point cloud:
pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud(cloud.makeShared()); extract.setIndices(inliers); extract.setNegative(false); extract.filter(cloud_segmented);
Finally, we convert the segmented point cloud into a PointCloud2 message type and we publish it:
sensor_msgs::PointCloud2 output; pcl::toROSMsg (cloud_segmented, output); pcl_pub.publish(output)
The result can be seen in the following image; the original point cloud is represented in white and the segmented inliers are represented in bluish. In this particular case, the floor was extracted as it's the biggest flat surface. This is quite convenient as it is probably one of the main elements we will usually want to extract from our point clouds:
