（二十一）圆柱体模型分割点云

以下代码实现使用平面和圆柱形模型对点云进行采样一致性分割。

cylinder_segmentation.cpp

#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

typedef pcl::PointXYZ PointT;

int main ()
{
      // All the objects needed
      pcl::PCDReader reader;
      pcl::PassThrough<PointT> pass;
      pcl::NormalEstimation<PointT, pcl::Normal> ne;
      pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; 
      pcl::PCDWriter writer;
      pcl::ExtractIndices<PointT> extract;
      pcl::ExtractIndices<pcl::Normal> extract_normals;
      pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());

      // Datasets
      pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
      pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
      pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
      pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<PointT>);
      pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
      pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
      pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);

      // Read in the cloud data
      reader.read ("table_scene_mug_stereo_textured.pcd", *cloud);
      std::cerr << "PointCloud has: " << cloud->size () << " data points." << std::endl;

      // 只保留z轴方向上[0，1.5]内的点
      pass.setInputCloud (cloud);
      pass.setFilterFieldName ("z");
      pass.setFilterLimits (0, 1.5);
      pass.filter (*cloud_filtered);
      std::cerr << "PointCloud after filtering has: " << cloud_filtered->size () << " data points." << std::endl;

      // Estimate point normals
      ne.setSearchMethod (tree);
      ne.setInputCloud (cloud_filtered);
      ne.setKSearch (50);
      ne.compute (*cloud_normals);
      /*========== 平面分割 ==========*/
      // 创建平面分割模型对象并设置参数
      seg.setOptimizeCoefficients (true);
      seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
      seg.setNormalDistanceWeight (0.1);
      seg.setMethodType (pcl::SAC_RANSAC);
      seg.setMaxIterations (100);  
      seg.setDistanceThreshold (0.03);
      seg.setInputCloud (cloud_filtered);
      seg.setInputNormals (cloud_normals);

      // 获得平面模型内部点和模型参数
      seg.segment (*inliers_plane, *coefficients_plane);
      std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;

      // 提取并保存点云中属于平面的内部点
      extract.setInputCloud (cloud_filtered);
      extract.setIndices (inliers_plane);
      extract.setNegative (false);
      pcl::PointCloud<PointT>::Ptr cloud_plane (new pcl::PointCloud<PointT> ());
      extract.filter (*cloud_plane);
      std::cerr << "PointCloud representing the planar component: " << cloud_plane->size () << " data points." << std::endl;
      writer.write ("table_scene_mug_stereo_textured_plane.pcd", *cloud_plane, false);
     
      /*========== 圆柱形分割 ==========*/
      // 去掉属于平面模型的内部点
      extract.setNegative (true);
      extract.filter (*cloud_filtered2);
      extract_normals.setNegative (true);
      extract_normals.setInputCloud (cloud_normals);
      extract_normals.setIndices (inliers_plane);
      extract_normals.filter (*cloud_normals2);

      // 创建圆柱体分割对象并设置参数
      seg.setOptimizeCoefficients (true);
      seg.setModelType (pcl::SACMODEL_CYLINDER);
      seg.setMethodType (pcl::SAC_RANSAC);
      seg.setNormalDistanceWeight (0.1);  // 曲面法线的影响权重设置为0.1的
      seg.setMaxIterations (10000);
      seg.setDistanceThreshold (0.05);      //每个内点到模型的距离阈值:5cm。
      seg.setRadiusLimits (0, 0.1);              // 将圆柱形模型的半径限制为小于10厘米
      seg.setInputCloud (cloud_filtered2);
      seg.setInputNormals (cloud_normals2);

      // 获得圆柱模型内部点和模型参数
      seg.segment (*inliers_cylinder, *coefficients_cylinder);
      std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;

      // 提取并保存cloud_filtered2中属于圆柱模型的内部点
      extract.setInputCloud (cloud_filtered2);
      extract.setIndices (inliers_cylinder);
      extract.setNegative (false);
      pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ());
      extract.filter (*cloud_cylinder);
      
      if (cloud_cylinder->points.empty ()) 
            std::cerr << "Can't find the cylindrical component." << std::endl;
      else
      {
            std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->size () << " data points." << std::endl;
            writer.write ("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false);
      }
      return (0);
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
project(cylinder_segmentation)
find_package(PCL 1.2 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable (cylinder_segmentation cylinder_segmentation.cpp)
target_link_libraries (cylinder_segmentation ${PCL_LIBRARIES})

数据样例

./cylinder_segmentation

红点和蓝点为分割结果