欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

《PCL》PCL点云分割

程序员文章站 2024-03-25 10:36:58
...
#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);
}