《PCL》PCL点云分割

#include <pcl/io/pcd_io.h>    ///pcl文件的IO
#include <pcl/point_types.h>  ///pcl文件中PointT
#include <pcl/sample_consensus/method_types.h>  ///随机采样一致性 随机参数方法头文件
#include <pcl/sample_consensus/model_types.h>   ///随机采样一致性 模型定义头文件
#include <pcl/segmentation/sac_segmentation.h>  ///基于随机采样一致性分割的类的头文件
#include <pcl/features/normal_3d.h>   ///法线计算头文件
#include <pcl/filters/passthrough.h>       ///执行统计滤波
#include <pcl/filters/extract_indices.h>   ///执行滤波提取的头文件  pass.filter() 
#include <pcl/visualization/cloud_viewer.h>  ///点云可视化

typedef pcl::PointXYZRGB PointT;
typedef pcl::Normal Normal;


int main(int argc, char** argv)
{
	///read data.
	pcl::PCDReader reader;
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
	reader.read(argv[1], *cloud);

	///passthrough filter.
	pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
	pcl::PassThrough<PointT> pass;
	pass.setInputCloud(cloud);
	pass.setFilterFieldName("z");
	pass.setFilterLimits(0, 1.5);
	pass.filter(*cloud_filtered);
	
	std::cerr << "PointCloud has:  " << cloud_filtered->size() << "  data points." << std::endl;
	pcl::PCDWriter writer;
	writer.write("cloud_filtered.pcd", *cloud_filtered, false);

	///Normal estimate.
	/*
	存放法线的变量;
	创建法线估计变量;
	创建估计法线时需要的近邻搜索方式变量;
	开始估计法线
	    输入点云
		近邻数量
		执行
	*/
	pcl::PointCloud<Normal>::Ptr cloud_normals(new pcl::PointCloud<Normal>);
	pcl::NormalEstimation<PointT, Normal> normalEstimate;
	pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
	normalEstimate.setInputCloud(cloud_filtered);
	normalEstimate.setSearchMethod(tree);
	normalEstimate.setKSearch(30);
	normalEstimate.compute(*cloud_normals);

	std::cerr << "normal estimate has:  " << cloud_normals->size() << "  data points." << std::endl;
	writer.write("cloud_normals.pcd", *cloud_normals, false);

	///concatenateFileld.  连接字段。横向
	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_XYZRGBNormal(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
	pcl::concatenateFields(*cloud_filtered, *cloud_normals, *cloud_XYZRGBNormal);

	std::cerr << "cloud_XYZRGBNormal has:  " << cloud_XYZRGBNormal->size() << "  data points." << std::endl;
	writer.write("cloud_XYZRGBNormal.pcd", *cloud_XYZRGBNormal, false);

	/*///visualization  可视化
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Normals"));
	viewer->addPointCloud< PointT>(cloud_filtered, "cloud");

	viewer->addPointCloudNormals< PointT, Normal>(cloud_filtered, cloud_normals, 1, 0.01, "normals");
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}*/

	/// Create the segmentation object for the planar model and set all the parameters
	/*
	存放内点的变量;
	创建模型系变量;
	创建模型分割变量;
	开始估计法线
	   输入点云
	   输入参考向量
	   输入模型
	   输入方法
	   设置优化
	   设置迭代
	   设置阈值
	   执行
	*/
	pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices);
	pcl::ModelCoefficients::Ptr coefficient_plane(new pcl::ModelCoefficients);
	pcl::SACSegmentationFromNormals<PointT, Normal> sacSeg;
	sacSeg.setInputCloud(cloud_filtered);
	sacSeg.setInputNormals(cloud_normals);
	sacSeg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
	sacSeg.setMethodType(pcl::SAC_RANSAC);
	sacSeg.setOptimizeCoefficients(true);
	sacSeg.setMaxIterations(100);
	sacSeg.setDistanceThreshold(0.03);
	sacSeg.segment(*inliers_plane, *coefficient_plane);
	std::cerr << "Plane coefficients: " << *coefficient_plane << std::endl;

	/// Extract the planar inliers from the input cloud
	pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>);
	pcl::ExtractIndices<PointT> extract;
	extract.setNegative(false);
	extract.filter(*cloud_plane);

	std::cerr << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;
	writer.write("cloud_plane.pcd", *cloud_plane, false);

	/// Remove the planar inliers, extract the rest  点云与向量
	pcl::PointCloud<PointT>::Ptr cloud_filtered2(new pcl::PointCloud<PointT>);
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2(new pcl::PointCloud<pcl::Normal>);
	extract.setNegative(true);
	extract.filter(*cloud_filtered2);
	
	pcl::ExtractIndices<pcl::Normal> extract_normals;
	pcl::ExtractIndices<Normal> extract_Normal;
	extract_Normal.setNegative(true);
	extract_Normal.setInputCloud(cloud_normals);
	extract_Normal.setIndices(inliers_plane);
	extract_Normal.filter(*cloud_normals2);

	/// Create the segmentation object for cylinder segmentation and set all the parameters
	pcl::ModelCoefficients::Ptr coefficients_cylinder(new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices);
	sacSeg.setInputCloud(cloud_filtered2);
	sacSeg.setInputNormals(cloud_normals2);
	sacSeg.setModelType(pcl::SACMODEL_CYLINDER);
	sacSeg.setMethodType(pcl::SAC_RANSAC);
	sacSeg.setOptimizeCoefficients(true);
	sacSeg.setMaxIterations(10000);
	sacSeg.setDistanceThreshold(0.05);
	sacSeg.setNormalDistanceWeight(0.1);
	sacSeg.setRadiusLimits(0, 0.1);
	sacSeg.segment(*inliers_cylinder, *coefficients_cylinder);
	std::cerr << "Cylinder cofficients: " << *coefficients_cylinder << std::endl;
	
	/// Extract the cylinder inliers from the input cloud
	pcl::PointCloud<PointT>::Ptr cloud_cylinder(new pcl::PointCloud<PointT>);
	extract.setInputCloud(cloud_filtered2);
	extract.setIndices(inliers_cylinder);
	extract.setNegative(false);
	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("cloud_cylinder.pcd", *cloud_cylinder, false);
	}

	return (0);
}