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

pcl::SACSegmentationFromNormals< PointT, PointNT >

类 SACSegmentationFromNormals< PointT, PointNT ＞是利用采样一致性方法进行点云分割的类，与其父类 SACSegmentation 不同之处在于其在算法实现时采用了法线信息，即该类在进行运算输出之前需要设定法线信息 ，其继承关系如图所示。

 关键成员函数：

	SACSegmentationFromNormals (bool random=false)
	构造函数
void	setInputNormals (const PointCloudNConstPtr &normals)
	设置输入点云的法钱， normals 为指向法线的指针。
void	setNormalDistanceWeight (double distance_weight)
	设置相对权重系数 distance_weight ，该权重与距离成正比，与角度成反比。
void	setMinMaxOpeningAngle (const double &min_angle, const double &max_angle)
	该函数配合，当用户指定模型为圆锥模型时，设置圆锥模型锥角的最小值与最大值，作为估计时的取值范围。
void	setDistanceFromOrigin (const double d)
	该函数配合，当用户指定模型为平面模型时，设定原点到平面模型的臣离为 d 。

 测试示例：

程序处理流程：

1、直通滤波器，过滤掉远于1.5米的数据点；

2、估计每个点的表面法线；     

3、分割出平面模型（演示数据集中表示桌面）并保存到磁盘中；

4、分割圆出柱体模型（演示数据集中表示圆杯）并保存到磁盘中；

5、可视化分割结果。

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

typedef pcl::PointXYZ PointT;

int
main(int argc, char** argv)
{
	// All the objects needed
	pcl::PCDReader reader;                    //PCD文件读取对象
	pcl::PassThrough pass;             //直通滤波对象
	pcl::NormalEstimation ne;  //法线估计对象
	pcl::SACSegmentationFromNormals seg;    //分割对象
	pcl::PCDWriter writer;            //PCD文件读取对象
	pcl::ExtractIndices extract;      //点提取对象
	pcl::ExtractIndices extract_normals;    ///点提取对象
	pcl::search::KdTree::Ptr tree(new pcl::search::KdTree());

	// Datasets
	pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
	pcl::PointCloud::Ptr cloud_filtered(new pcl::PointCloud);
	pcl::PointCloud::Ptr cloud_normals(new pcl::PointCloud);
	pcl::PointCloud::Ptr cloud_filtered2(new pcl::PointCloud);
	pcl::PointCloud::Ptr cloud_normals2(new pcl::PointCloud);
	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("..\\..\\source\\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;

	// 过滤后的点云进行法线估计，为后续进行基于法线的分割准备数据
	ne.setSearchMethod(tree);
	ne.setInputCloud(cloud_filtered);
	ne.setKSearch(50);
	ne.compute(*cloud_normals);

	// 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::Ptr cloud_plane(new pcl::PointCloud());
	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::Ptr cloud_cylinder(new pcl::PointCloud());
	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 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 cloud_tr_color_h(cloud_plane, 0, 0, 255);
	viewer.addPointCloud(cloud_plane, cloud_tr_color_h, "cloud_plane");

	//  设置cloud_cylinder点云为绿色
	pcl::visualization::PointCloudColorHandlerCustom 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);
}

cmd命令：.\cylinder_segmentation.exe 

PointCloud has: 307200 data points.
PointCloud after filtering has: 139897 data points.
Plane coefficients: header:
seq: 0 stamp: 0 frame_id:
values[]
  values[0]:   0.0161902
  values[1]:   -0.837667
  values[2]:   -0.545941
  values[3]:   0.528862

PointCloud representing the planar component: 116300 data points.
Cylinder coefficients: header:
seq: 0 stamp: 0 frame_id:
values[]
  values[0]:   0.0543319
  values[1]:   0.100139
  values[2]:   0.787577
  values[3]:   -0.0135876
  values[4]:   0.834831
  values[5]:   0.550338
  values[6]:   0.0387446

PointCloud representing the cylindrical component: 11462 data points.

可视化结果：

                                                图1 原始点云可视化后的结果（三维场景中有平面、杯子以及其他物体）

                                 图2 分割结果：分割得到的圆柱（杯子，绿色点）和分割得到的平面（桌面，蓝色点）