Cylinder model segmentation

This tutorial exemplifies how to run a Sample Consensus segmentation for cylindrical models. To make the example a bit more practical, the following operations are applied to the input dataset (in order):

  • data points further away than 1.5 meters are filtered

  • surface normals at each point are estimated

  • a plane model (describing the table in our demo dataset) is segmented and saved to disk

  • a cylindrical model (describing the mug in our demo dataset) is segmented and saved to disk

Note

The cylindrical model is not perfect due to the presence of noise in the data.

The code

First, download the dataset table_scene_mug_stereo_textured.pcd and save it somewhere to disk.

Then, create a file, let’s say, cylinder_segmentation.cpp in your favorite editor, and place the following inside it:

  1#include <pcl/ModelCoefficients.h>
  2#include <pcl/io/pcd_io.h>
  3#include <pcl/point_types.h>
  4#include <pcl/filters/extract_indices.h>
  5#include <pcl/filters/passthrough.h>
  6#include <pcl/features/normal_3d.h>
  7#include <pcl/sample_consensus/method_types.h>
  8#include <pcl/sample_consensus/model_types.h>
  9#include <pcl/segmentation/sac_segmentation.h>
 10
 11typedef pcl::PointXYZ PointT;
 12
 13int
 14main ()
 15{
 16  // All the objects needed
 17  pcl::PCDReader reader;
 18  pcl::PassThrough<PointT> pass;
 19  pcl::NormalEstimation<PointT, pcl::Normal> ne;
 20  pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; 
 21  pcl::PCDWriter writer;
 22  pcl::ExtractIndices<PointT> extract;
 23  pcl::ExtractIndices<pcl::Normal> extract_normals;
 24  pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
 25
 26  // Datasets
 27  pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
 28  pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
 29  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
 30  pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<PointT>);
 31  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
 32  pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
 33  pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);
 34
 35  // Read in the cloud data
 36  reader.read ("table_scene_mug_stereo_textured.pcd", *cloud);
 37  std::cerr << "PointCloud has: " << cloud->size () << " data points." << std::endl;
 38
 39  // Build a passthrough filter to remove spurious NaNs and scene background
 40  pass.setInputCloud (cloud);
 41  pass.setFilterFieldName ("z");
 42  pass.setFilterLimits (0, 1.5);
 43  pass.filter (*cloud_filtered);
 44  std::cerr << "PointCloud after filtering has: " << cloud_filtered->size () << " data points." << std::endl;
 45
 46  // Estimate point normals
 47  ne.setSearchMethod (tree);
 48  ne.setInputCloud (cloud_filtered);
 49  ne.setKSearch (50);
 50  ne.compute (*cloud_normals);
 51
 52  // Create the segmentation object for the planar model and set all the parameters
 53  seg.setOptimizeCoefficients (true);
 54  seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
 55  seg.setNormalDistanceWeight (0.1);
 56  seg.setMethodType (pcl::SAC_RANSAC);
 57  seg.setMaxIterations (100);
 58  seg.setDistanceThreshold (0.03);
 59  seg.setInputCloud (cloud_filtered);
 60  seg.setInputNormals (cloud_normals);
 61  // Obtain the plane inliers and coefficients
 62  seg.segment (*inliers_plane, *coefficients_plane);
 63  std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
 64
 65  // Extract the planar inliers from the input cloud
 66  extract.setInputCloud (cloud_filtered);
 67  extract.setIndices (inliers_plane);
 68  extract.setNegative (false);
 69
 70  // Write the planar inliers to disk
 71  pcl::PointCloud<PointT>::Ptr cloud_plane (new pcl::PointCloud<PointT> ());
 72  extract.filter (*cloud_plane);
 73  std::cerr << "PointCloud representing the planar component: " << cloud_plane->size () << " data points." << std::endl;
 74  writer.write ("table_scene_mug_stereo_textured_plane.pcd", *cloud_plane, false);
 75
 76  // Remove the planar inliers, extract the rest
 77  extract.setNegative (true);
 78  extract.filter (*cloud_filtered2);
 79  extract_normals.setNegative (true);
 80  extract_normals.setInputCloud (cloud_normals);
 81  extract_normals.setIndices (inliers_plane);
 82  extract_normals.filter (*cloud_normals2);
 83
 84  // Create the segmentation object for cylinder segmentation and set all the parameters
 85  seg.setOptimizeCoefficients (true);
 86  seg.setModelType (pcl::SACMODEL_CYLINDER);
 87  seg.setMethodType (pcl::SAC_RANSAC);
 88  seg.setNormalDistanceWeight (0.1);
 89  seg.setMaxIterations (10000);
 90  seg.setDistanceThreshold (0.05);
 91  seg.setRadiusLimits (0, 0.1);
 92  seg.setInputCloud (cloud_filtered2);
 93  seg.setInputNormals (cloud_normals2);
 94
 95  // Obtain the cylinder inliers and coefficients
 96  seg.segment (*inliers_cylinder, *coefficients_cylinder);
 97  std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
 98
 99  // Write the cylinder inliers to disk
100  extract.setInputCloud (cloud_filtered2);
101  extract.setIndices (inliers_cylinder);
102  extract.setNegative (false);
103  pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ());
104  extract.filter (*cloud_cylinder);
105  if (cloud_cylinder->points.empty ()) 
106    std::cerr << "Can't find the cylindrical component." << std::endl;
107  else
108  {
109	  std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->size () << " data points." << std::endl;
110	  writer.write ("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false);
111  }
112  return (0);
113}

The explanation

The only relevant lines are the lines below, as the other operations are already described in the other tutorials.

  // Create the segmentation object for cylinder segmentation and set all the parameters
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_CYLINDER);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setNormalDistanceWeight (0.1);
  seg.setMaxIterations (10000);
  seg.setDistanceThreshold (0.05);
  seg.setRadiusLimits (0, 0.1);

As seen, we’re using a RANSAC robust estimator to obtain the cylinder coefficients, and we’re imposing a distance threshold from each inlier point to the model no greater than 5cm. In addition, we set the surface normals influence to a weight of 0.1, and we limit the radius of the cylindrical model to be smaller than 10cm.

Compiling and running the program

Add the following lines to your CMakeLists.txt file:

 1cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 2
 3project(cylinder_segmentation)
 4
 5find_package(PCL 1.2 REQUIRED)
 6
 7include_directories(${PCL_INCLUDE_DIRS})
 8link_directories(${PCL_LIBRARY_DIRS})
 9add_definitions(${PCL_DEFINITIONS})
10
11add_executable (cylinder_segmentation cylinder_segmentation.cpp)
12target_link_libraries (cylinder_segmentation ${PCL_LIBRARIES})

After you have made the executable, you can run it. Simply do:

$ ./cylinder_segmentation

You will see something similar to:

PointCloud has: 307200 data points.
PointCloud after filtering has: 139897 data points.
[pcl::SACSegmentationFromNormals::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE
[pcl::SACSegmentationFromNormals::initSACModel] Setting normal distance weight to 0.100000
[pcl::SACSegmentationFromNormals::initSAC] Using a method of type: SAC_RANSAC with a model threshold of 0.030000
[pcl::SACSegmentationFromNormals::initSAC] Setting the maximum number of iterations to 100
Plane coefficients: header:
  seq: 0
  stamp: 0.000000000
  frame_id:
values[]
  values[0]: -0.0161854
  values[1]: 0.837724
  values[2]: 0.545855
  values[3]: -0.528787

PointCloud representing the planar component: 117410 data points.
[pcl::SACSegmentationFromNormals::initSACModel] Using a model of type: SACMODEL_CYLINDER
[pcl::SACSegmentationFromNormals::initSACModel] Setting radius limits to 0.000000/0.100000
[pcl::SACSegmentationFromNormals::initSACModel] Setting normal distance weight to 0.100000
[pcl::SACSegmentationFromNormals::initSAC] Using a method of type: SAC_RANSAC with a model threshold of 0.050000
[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] LM solver finished with exit code 2, having a residual norm of 0.322616.
Initial solution: 0.0452105 0.0924601 0.790215 0.20495 -0.721649 -0.661225 0.0422902
Final solution: 0.0452105 0.0924601 0.790215 0.20495 -0.721649 -0.661225 0.0396354
Cylinder coefficients: header:
  seq: 0
  stamp: 0.000000000
  frame_id:
values[]
  values[0]: 0.0452105
  values[1]: 0.0924601
  values[2]: 0.790215
  values[3]: 0.20495
  values[4]: -0.721649
  values[5]: -0.661225
  values[6]: 0.0396354

PointCloud representing the cylindrical component: 8625 data points.