圆柱模型分割

本教程举例说明了如何为圆柱模型运行样本共识分割。为了使示例更实用，对输入数据集进行以下操作（按顺序）：

• 超过 1.5 米的数据点被过滤

• 估计每个点的表面法线

• 平面模型（描述我们演示数据集中的表）被分割并保存到磁盘

• 一个圆柱形模型（描述我们演示数据集中的杯子）被分割并保存到磁盘

下载数据集 https://raw.github.com/PointCloudLibrary/data/master/tutorials/table_scene_mug_stereo_textured.pcd

创建cylinder_segmentation.cpp文件

源码：

  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}

 说明：
1、相关的头文件

  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>

 2、创建所需要的对象

 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> ());//kd搜索对象

 3、设置对应的存储容器

 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);

4、 读取数据到cloud

 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;

5、创建一个直通滤波器过滤cloud中不需要的点，存入cloud_filtered

 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;

6、估计cloud_filtered中点法线，存入cloud_normals

 46  // Estimate point normals
 47  ne.setSearchMethod (tree);
 48  ne.setInputCloud (cloud_filtered);
 49  ne.setKSearch (50);
 50  ne.compute (*cloud_normals);

7、为平面模型创建分割对象并设置所有参数

 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);//设置采用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);//分割结果索引点云存入inliers_plane，模型系数存入coefficients_plane
 63  std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;

8、从输入点云中提取内点

 65  // Extract the planar inliers from the input cloud
 66  extract.setInputCloud (cloud_filtered);
 67  extract.setIndices (inliers_plane);
 68  extract.setNegative (false);

9、将分割后的点云通过索引找到该块点云 存入

 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);

10、删除内切线，提取其他点

 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);

11、为圆柱体分割创建分割对象并设置所有参数

 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);//输入分割圆柱的法线向量

12、分割结果索引保存 系数保存

 95  // Obtain the cylinder inliers and coefficients
 96  seg.segment (*inliers_cylinder, *coefficients_cylinder);
 97  std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;

13、提取圆柱并输出圆柱点云

 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}

编译和运行程序

1、将以下行添加到 CMakeLists.txt 文件中：

 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})

2、运行

$ ./cylinder_segmentation

3、输出

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.