【点云处理】基于模型的点云分割

最近一直在忙着毕业论文的事情，随着自己做实验的过程中发现之前的点云知识忘得差不多了~~现在重新拾起来~~

希望大论文可以顺利完成~~

直接上代码：

#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 <time.h>
typedef pcl::PointXYZ PointT;
using namespace std;

int main()
{
	clock_t t1 = clock();
	pcl::PCDReader reader; //文件读取对象
	pcl::PassThrough<PointT> pass;//直通滤波对象
	
	pcl::PCDWriter writer;
	pcl::ExtractIndices<PointT> extract; //点提取对象 索引滤波对象
	pcl::ExtractIndices<pcl::Normal> extract_normals; //点提取对象
	pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>()); //kd搜索对象

	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);
	pcl::ModelCoefficients::Ptr	coefficients_cylinder(new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices);
	pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices);

	reader.read("table_scene_mug_stereo_textured.pcd", *cloud);
	cerr << "初始点云 ：" << cloud->points.size() << endl;

	//直通滤波
	pass.setInputCloud(cloud);
	pass.setFilterFieldName("z");
	pass.setFilterLimits(0, 1.5); //只保留 0~1.5 米之间的距离
	pass.filter(*cloud_filtered);
	cerr << "直通滤波后的点云 ：" << cloud_filtered->points.size() << endl;

	//法线估计
	//法线估计需要查询邻近点，使用kd作为搜索树对象，邻近点的阈值设置为50
	pcl::NormalEstimation<PointT, pcl::Normal> ne; //法线估计对象
	ne.setSearchMethod(tree);
	ne.setInputCloud(cloud_filtered);
	ne.setKSearch(50);
	ne.compute(*cloud_normals); //计算法线估计

	//基于法线的分割
	//SACMODEL_NORMAL_PLANE 定义为有限制条件的平面模型 ：在限制内，每一个局内点的法线必须与估计的平面模型的法线平行
	//1、为平面建立分割对象
	pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; //分割对象
	seg.setOptimizeCoefficients(true); //选择 模型参数是否需要优化
	seg.setModelType(pcl::SACMODEL_NORMAL_PLANE); //设置分割的模型类别：具有一定限制的平面
	seg.setNormalDistanceWeight(0.1); //设置表面法线权重系数
	seg.setMethodType(pcl::SAC_RANSAC);  //设置采用RANSAC作为算法的参数估计方法
	seg.setMaxIterations(100); //设置最大迭代次数
	seg.setDistanceThreshold(0.03); //设置内点到模型的距离允许最大值
	seg.setInputCloud(cloud_filtered);
	seg.setInputNormals (cloud_normals);
	seg.segment(*inliers_plane, *coefficients_plane); //分割结果索引点云存入inliers_plane，模型系数存入coefficients_plane
	cout << "平面模型系数： " << *coefficients_plane << endl;

	extract.setInputCloud(cloud_filtered);
	extract.setIndices(inliers_plane);
	extract.setNegative(false); //如果设为true,可以提取指定index之外的点云，即除了平面之外的点云 
	//将分割后的点云通过索引找到该块点云 存入
	pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>());
	extract.filter(*cloud_plane);
	cout << "平面分割出来的点云数量为 : " << cloud_plane->points.size()  << endl;
	writer.write("平面点云.pcd", *cloud_plane, false);

	extract.setNegative(true);
	extract.filter(*cloud_filtered2);  //cloud_filtered2中存储的是包括杯子把的点云数据
	extract_normals.setNegative(true); 
	extract_normals.setInputCloud(cloud_normals);  //输入初始数据为 所有点的法线向量
	extract_normals.setIndices(inliers_plane);  //inliers_plane 中里面是去除了平面的点云索引
	extract_normals.filter(*cloud_normals2); //根据圆柱的索引 找到了属于圆柱的法线向量


	//2、为圆柱建立分割对象
	seg.setOptimizeCoefficients(true); //模型参数需要优化
	seg.setModelType(pcl::SACMODEL_CYLINDER); //设置分割模型为圆柱
	seg.setMethodType(pcl::SAC_RANSAC); //设置分割方法
	seg.setNormalDistanceWeight(0.1);  //设置表面法线权重系数
	seg.setMaxIterations(10000); //设置最大迭代次数
	seg.setDistanceThreshold(0.05);//设置内点到模型之间的最大距离
	seg.setRadiusLimits(0, 0.1); //设置圆柱模型的半径范围
	seg.setInputCloud(cloud_filtered2);  //输入分割圆柱的点云数据
	seg.setInputNormals(cloud_normals2); //输入分割圆柱的法线向量
	seg.segment(*inliers_cylinder, *coefficients_cylinder); //分割结果索引保存 系数保存
	cout << "圆柱系数为 ： " << *coefficients_cylinder << std::endl;

	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())
		cout << "Can't find the cylindrical component." << endl;
	else
	{
		cout << "圆柱点云数量: " << cloud_cylinder->points.size() << endl;
		writer.write("圆柱点云.pcd", *cloud_cylinder, false);
	}

	clock_t t2 = clock();
	std::cout << "time: " << t2 - t1 << std::endl;
	system("pause");
	return 0;
}

这是原始点云：

分割后的点云：