PCL点云初步

一、PCL滤波

直通滤波

设置一个x，y，z的要保存的点的范围，滤出超过范围的点

//创建一个直通滤波器的对象，并设置相关参数
pcl::PassThrough<pcl::PointXYZ> pass;	//创建对象
pass.setInputCloud(cloud);				//设置输入点云
pass.setFilterFieldName("z");			//设置过滤字段，这里对z轴上的点云进行过滤
pass.setFilterLimits(0.0, 1.0);			//设置过滤范围，这里选择过滤掉0~1范围内的点云
//pass.setFilterLimitsNegative (true);	//设置选择保留范围内的还是过滤掉范围内的点云
pass.filter(*cloud_filtered);			//执行滤波，结果保存在cloud_filter中

体素滤波

将空间划分为一定体积的网格，用网格内所有点的质心来代替所有点（适合于一个比较稠密的场景）特征后期需要根据应用场景进行调参，去寻找一个合适的网格大小

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>			//头文件

int
main(int argc, char** argv)
{
    pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2());
    pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2());

    // Fill in the cloud data
    pcl::PCDReader reader;	//读文件对象
    // Replace the path below with the path where you saved your file
    //可以换成你自己电脑上的对应文件路径
    reader.read("./pcd/table_scene_lms400.pcd", *cloud); //确保你有这个pcd的文件

    std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height
        << " data points (" << pcl::getFieldsList(*cloud) << ").";

	//创建voxelGrid对象，及相关参数的设置
    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;	//创建voxelGrid对象
    sor.setInputCloud(cloud);					//设置输入点云
    sor.setLeafSize(0.01f, 0.01f, 0.01f);		//设置体素大小，单位是m，这里设置成了1cm的立方体
    sor.filter(*cloud_filtered);				//执行滤波，结果保存在cloud_filter中

    std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height
        << " data points (" << pcl::getFieldsList(*cloud_filtered) << ").";

    //写入文件中
    pcl::PCDWriter writer;
    writer.write("table_scene_lms400_downsampled.pcd", *cloud_filtered,
        Eigen::Vector4f::Zero(), Eigen::Quaternionf::Identity(), false);

    return (0);
}

基于统计学的离群点滤波

统计每个点与周围最邻近的若干个点的平均距离，滤除大于阈值的离群点

//创建滤波器对象，及相关参数设置
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;		//创建对象
sor.setInputCloud(cloud);								//设置输入点云
sor.setMeanK(50);										//设置统计时考虑查询点邻近点数
/*
	* 设置判断是否为离群点的阈值
	* 更具体为设置标准差倍数阈值 std_mul ，点云中所有点与其邻域的距离大于 μ ±σ• std_mul 
	* 则被认为是离群点，其中 μ 代表估计的平均距离， σ 代表标准差 。
*/
sor.setStddevMulThresh(1.0);	//设置为1代表：如果一个点的距离超过平均距离一个标准差以上，则会被当做离群点去除
sor.filter(*cloud_filtered);	//执行滤波，并将结果保存在cloud_filter中

半径滤波

用于用户指定每个点的一定范围内周围至少有足够多的近邻，如果指定至少需要2个近邻，那么以某个点为圆心，在一定半径大小的圆内，除了它自己还需要存在两个点，否则这个圆内的所有点都将被删除。

//创建过滤器
pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
outrem.setInputCloud(cloud);        //设置输入点云
outrem.setRadiusSearch(0.8);        //设置在0.8的半径范围内找近邻点
outrem.setMinNeighborsInRadius(2);  //设置查询查询点的近邻点集数小于2的删除

outrem.filter(*cloud_filtered);     //执行滤波，结果保存在cloud_filter

可以用于处理空气中的噪点和边缘的噪点

二、PCL聚类与分割

RANSAC随机抽样一致性算法（主要是平面、聚类、法线、圆柱分割、索引提取）

1.平面分割

pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setModelType (pcl::SACMODEL_PLANE);//待拟合的类型
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);//设置阈值

seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);//前者存放内点的索引和模型参数

2.法线估计

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>//法线

int main()
{
	//加载点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile("E:/pcl_project/normal/normal/table_scene_lms400.pcd", *cloud);
	//估计法线
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
	ne.setInputCloud(cloud);
	//创建一个空的kdtree对象，并把它传递给法线估计对象
	//基于给出的输入数据集，kdtree将被建立
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	ne.setSearchMethod(tree);
	//输出数据集
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
	//使用半径在查询点周围3厘米范围内的所有邻元素，这里参数的单位为米
	ne.setRadiusSearch(0.03);
	//计算特征值
	ne.compute(*cloud_normals);
	// cloud_normals->points.size ()应该与input cloud_downsampled->points.size ()有相同尺寸
	//法线可视化
	pcl::visualization::PCLVisualizer viewer("PCL Viewer");
	viewer.setBackgroundColor(0.0, 0.0, 0.0);
	viewer.addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud, cloud_normals);

	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}

	return 0;
}

3.圆柱拟合

#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>				// 法向量估计
#include <pcl/segmentation/sac_segmentation.h>	// 模型分割
#include <pcl/filters/extract_indices.h>		// 索引提取
#include <pcl/visualization/cloud_viewer.h>		// 可视化

using namespace std;

typedef pcl::PointXYZ PointT;

int main()
{	
	//-----------------------------加载点云--------------------------------
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
	if (pcl::io::loadPCDFile("test.pcd", *cloud) < 0)
	{
		PCL_ERROR("\a点云文件不存在！\n");
		system("pause");
		return -1;
	}
	cout << "->加载数据点的个数：" << cloud->points.size() << endl;
	//=====================================================================

	//-----------------------------法线估计--------------------------------
	cout << "->正在计算法线..." << endl;
	pcl::NormalEstimation<PointT, pcl::Normal> ne;	// 创建法向量估计对象
	pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
	ne.setSearchMethod(tree);						// 设置搜索方式
	ne.setInputCloud(cloud);						// 设置输入点云
	ne.setKSearch(50);								// 设置K近邻搜索点的个数
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
	ne.compute(*cloud_normals);						// 计算法向量，并将结果保存到cloud_normals中
	//=====================================================================

	//----------------------------圆柱体分割--------------------------------
	cout << "->正在圆柱体分割..." << endl;
	pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;		// 创建圆柱体分割对象
	seg.setInputCloud(cloud);										// 设置输入点云：待分割点云
	seg.setOptimizeCoefficients(true);								// 设置对估计的模型系数需要进行优化
	seg.setModelType(pcl::SACMODEL_CYLINDER);						// 设置分割模型为圆柱体模型
	seg.setMethodType(pcl::SAC_RANSAC);								// 设置采用RANSAC算法进行参数估计
	seg.setNormalDistanceWeight(0.1);								// 设置表面法线权重系数
	seg.setMaxIterations(10000);									// 设置迭代的最大次数
	seg.setDistanceThreshold(0.05);									// 设置内点到模型距离的最大值
	seg.setRadiusLimits(3.0, 4.0);									// 设置圆柱模型半径的范围
	seg.setInputNormals(cloud_normals);								// 设置输入法向量
	pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices);	// 保存分割结果
	pcl::ModelCoefficients::Ptr coefficients_cylinder(new pcl::ModelCoefficients);	// 保存圆柱体模型系数
	seg.segment(*inliers_cylinder, *coefficients_cylinder);			// 执行分割，将分割结果的索引保存到inliers_cylinder中，同时存储模型系数coefficients_cylinder
	cout << "\n\t\t-----圆柱体系数-----" << endl;
	cout << "轴线一点坐标：(" << coefficients_cylinder->values[0] << ", "
		<< coefficients_cylinder->values[1] << ", "
		<< coefficients_cylinder->values[2] << ")"
		<< endl;
	cout << "轴线方向向量：(" << coefficients_cylinder->values[3] << ", "
		<< coefficients_cylinder->values[4] << ", "
		<< coefficients_cylinder->values[5] << ")"
		<< endl;
	cout << "圆柱体半径：" << coefficients_cylinder->values[6] << endl;
	//=====================================================================

	//------------------------------提取分割结果----------------------------
	cout << "->正在提取分割结果..." << endl;
	pcl::ExtractIndices<PointT> extract;	// 创建索引提取点对象
	extract.setInputCloud(cloud);			// 设置输入点云：待分割点云
	extract.setIndices(inliers_cylinder);	// 设置内点索引
	extract.setNegative(false);				// 默认false，提取圆柱体内点；true，提取圆柱体外点
	pcl::PointCloud<PointT>::Ptr cloud_cylinder(new pcl::PointCloud<PointT>());
	extract.filter(*cloud_cylinder);		// 执行滤波，并将结果点云保存到cloud_cylinder中
	//=====================================================================

	//----------------------------------保存分割结果------------------------
	if (!cloud_cylinder->points.empty())
	{
		pcl::PCDWriter writer;
		writer.write("cylinder.pcd", *cloud_cylinder, true);
		cout << "->圆柱体模型点云个数：" << cloud_cylinder->size() << endl;
	}
	else
	{
		PCL_ERROR("未提取出圆柱体模型点！\a\n");
	}
	//=====================================================================

	//-------------------------可视化分割结果（可选操作）--------------------
	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("可视化分割结果"));

	///视口1：原始点云
	int v1;
	viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1); //设置第一个视口在X轴、Y轴的最小值、最大值，取值在0-1之间
	viewer->setBackgroundColor(0, 0, 0, v1);
	viewer->addText("original_point_cloud", 10, 10, "v1 text", v1);
	viewer->addPointCloud<PointT>(cloud, "original_point_cloud", v1);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "original_point_cloud", v1);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "original_point_cloud", v1);

	///视口2：分割后点云
	int v2;
	viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
	viewer->setBackgroundColor(0.3, 0.3, 0.3, v2);
	viewer->addText("segment_point_cloud", 10, 10, "v2 text", v2);
	pcl::visualization::PointCloudColorHandlerCustom<PointT> set_color(cloud_cylinder, 0, 255, 0);
	viewer->addPointCloud<PointT>(cloud_cylinder, set_color, "segment_point_cloud", v2);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "segment_point_cloud", v2);

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
	//=====================================================================

	return 0;
}

可视化

1.展示一个窗口

(采用while循环)

2.直接在终端查看

pcl_viewer xxx.pcd