PCL：点云分割-圆柱体模型分割

#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>
#include <pcl/visualization/pcl_visualizer.h>
#pragma comment(lib,"User32.lib")
#pragma comment(lib, "gdi32.lib")
typedef pcl::PointXYZ PointT;

int
main(int argc, char** argv)
{
	// All the objects needed
	pcl::PCDReader reader;                    //PCD文件读取对象
	pcl::PassThrough<PointT> pass;             //直通滤波对象
	pcl::NormalEstimation<PointT, pcl::Normal> ne;  //法线估计对象
	pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;    //分割对象
	pcl::PCDWriter writer;            //PCD文件读取对象
	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->points.size() << " data points." << std::endl;

	// 直通滤波，将Z轴不在（0，1.5）范围的点过滤掉，将剩余的点存储到cloud_filtered对象中
	pass.setInputCloud(cloud);
	pass.setFilterFieldName("z");
	pass.setFilterLimits(0, 1.5);
	pass.filter(*cloud_filtered);
	std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl;
	writer.write("直通滤波删除1.5以外的点.pcd", *cloud_filtered, false);
	
	// 过滤后的点云进行法线估计，为后续进行基于法线的分割准备数据
	ne.setSearchMethod(tree);
	ne.setInputCloud(cloud_filtered);
	ne.setKSearch(50);
	ne.compute(*cloud_normals);
	writer.write("点云法线估计.pcd", *cloud_normals, false);

	// Create the segmentation object for the planar model and set all the parameters
	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->points.size() << " data points." << std::endl;
	writer.write("table_scene_mug_stereo_textured_plane.pcd", *cloud_plane, false);

	// Remove the planar inliers, extract the rest
	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);

	// 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);               //设置迭代的最大次数10000
	seg.setDistanceThreshold(0.05);            //设置内点到模型的距离允许最大值
	seg.setRadiusLimits(0, 0.1);               //设置估计出的圆柱模型的半径的范围
	seg.setInputCloud(cloud_filtered2);
	seg.setInputNormals(cloud_normals2);

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

	// Write the cylinder inliers to disk
	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->points.size() << " data points." << std::endl;
		writer.write("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false);
	}



	// 可视化部分
	pcl::visualization::PCLVisualizer v0("segmention");
	// 我们将要使用的颜色
	float bckgr_gray_level = 0.0;  // 黑色
	float txt_gray_lvl = 1.0 - bckgr_gray_level;

	// 设置初始点云为白色
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h(cloud, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl,
		(int)255 * txt_gray_lvl);//赋予显示点云的颜色
	v0.addPointCloud(cloud, cloud_in_color_h, "cloud");


	// 可视化部分
	pcl::visualization::PCLVisualizer viewer("segmention");

	// 设置cloud_plane点云为红色
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h(cloud_plane, 0, 0, 255);
	viewer.addPointCloud(cloud_plane, cloud_tr_color_h, "cloud_plane");

	//  设置cloud_cylinder点云为绿色
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h(cloud_cylinder, 0, 255, 0);
	viewer.addPointCloud(cloud_cylinder, cloud_icp_color_h, "cloud_cylinder");

	 启动可视化
	//v0.addCoordinateSystem(0.0);
	//v0.initCameraParameters();
	//viewer.addCoordinateSystem(0.0);
	//viewer.initCameraParameters();

	//等待直到可视化窗口关闭。
	while (!viewer.wasStopped())
	{
		v0.spinOnce(100);
		viewer.spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	return (0);
}