【点云PCL入门】点云滤波

1.直通滤波器-PassThrough

对指定的某一维度实现滤波，去掉在用户指定范围内部（或外部）的点。

#include<pcl/point_types.h>
#include<iostream>
#include<pcl/filters/passthrough.h>
int main(int argc, char* argv[])
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

	//填充点云
	cloud->width = 5;
	cloud->height = 1;
	cloud->points.resize(cloud->width*cloud->height);
	for(auto& point:*cloud)
	{
		point.x = 1024 * rand() / (RAND_MAX + 1.0f);
		point.y = 1024 * rand() / (RAND_MAX + 1.0f);
		point.z = 1024 * rand() / (RAND_MAX + 1.0f);
	}
	std::cerr << "Cloud before filtering: " << std::endl;
	for (const auto& point : *cloud)
		std::cerr  << " " 
	    << point.x << " "
		<< point.y << " "
		<< point.z << std::endl;
	// Create the filtering object
	pcl::PassThrough<pcl::PointXYZ> pass;
	pass.setInputCloud(cloud);
	pass.setFilterFieldName("z");//设置滤波字段z轴
	pass.setFilterLimits(0.0, 1.0);//过滤字段上可接受范围
	pass.filter(*cloud_filtered);
	std::cerr << "Cloud after filtering:" << std::endl;
	for (const auto& point : *cloud_filtered)
		std::cerr  << " " << point.x << " "<< point.y << " "<< point.z << std::endl;
	return 0;
}

2.点云下采样-VoxelGrid

使用体素化网格方法实现下采样，减少点的数量，减少点云数据，并同时保证点云的形状特征。在输入点云数据上创建一个3D体素网格（将体素网格视为一组空间三维立方体）。然后，在每个体素（即3D立方体）中，所有存在的点都将以其质心进行近似（即降采样）。这种方法比以体素的中心逼近它们要慢一些，但是它可以更准确地表示下面的表面。

#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);

	pcl::PCDReader reader;
	reader.read("table_scene_lms400.pcd", *cloud);

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

	// Create the filtering object
	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
	sor.setInputCloud(cloud);
	sor.setLeafSize(0.01, 0.01, 0.01);//设置滤波时创建的体素大小为1cm立方体
	sor.filter(*cloud_filtered);

	std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height
		<< " data points (" << pcl::getFieldsList(*cloud_filtered) << ")." << std::endl;
	pcl::PCDWriter writer;
	writer.write("table_scene_lms400_downsampled.pcd", *cloud_filtered,
		Eigen::Vector4f::Zero(), Eigen::Quaternionf::Identity(), false);
	return (0);
}

3.删除离群值-StatisticsOutlierRemoval

对每个点的邻域进行一个统计分析，并修剪掉那些不符合一定标准的点。我们的稀疏离群点移除方法基于在输入数据中对点到临近点的距离分布的计算。对每个点，我们计算它到它的所有临近点的平均距离。假设得到的结果是一个高斯分布，其形状由均值和标准差决定，平均距离在标准范围（由全局距离平均值和方差定义）之外的点，可被定义为离群点并可从数据集中去除掉。

#include <iostream>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include<pcl/filters/statistical_outlier_removal.h>

int main(int argc, char* argv[])
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

	// Fill in the cloud data
	pcl::PCDReader reader;
	// Replace the path below with the path where you saved your file
	reader.read<pcl::PointXYZ>("table_scene_lms400.pcd", *cloud);

	std::cerr << "Cloud before filtering: " << std::endl;
	std::cerr << *cloud << std::endl;

	// Create the filtering object
	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
	sor.setInputCloud(cloud);
	sor.setMeanK(50);//分析的临近点个数
	sor.setStddevMulThresh(1.0);//标准差倍数，判断是否为离群点的阈值
	sor.filter(*cloud_filtered);

	std::cerr << "Cloud after filtering: " << std::endl;
	std::cerr << *cloud_filtered << std::endl;

	pcl::PCDWriter writer;
	writer.write<pcl::PointXYZ>("table_scene_lms400_inliers.pcd", *cloud_filtered, false);
	
	//再次滤波，获取离群点数据
	sor.setNegative(true);
	sor.filter(*cloud_filtered);
	writer.write<pcl::PointXYZ>("table_scene_lms400_outliers.pcd", *cloud_filtered, false);

	return (0);

}

4.使用参数模型投影点云-ProjectInliers

将点投影到一个参数化模型上（例如平面或球等）。参数化模型通过一组参数来设定，对于平面来说，使用其等式形式ax+by+cz+d=0，在PCL中有特意存储常见模型系数的数据结构。

#include<iostream>
#include<pcl/io/pcd_io.h>
#include<pcl/ModelCoefficients.h>
#include<pcl/filters/project_inliers.h>

int main(int argc, char* argv[])
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);

	// Fill in the cloud data
	cloud->width = 5;
	cloud->height = 1;
	cloud->points.resize(cloud->width * cloud->height);

	for (auto& point : *cloud)
	{
		point.x = 1024 * rand() / (RAND_MAX + 1.0f);
		point.y = 1024 * rand() / (RAND_MAX + 1.0f);
		point.z = 1024 * rand() / (RAND_MAX + 1.0f);
	}

	std::cerr << "Cloud before projection: " << std::endl;
	for (const auto& point : *cloud)
		std::cerr << "    " << point.x << " "
		<< point.y << " "
		<< point.z << std::endl;

	// Create a set of planar coefficients with X=Y=0,Z=1
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	coefficients->values.resize(4);
	coefficients->values[0] = coefficients->values[1] = 0;
	coefficients->values[2] = 1.0;
	coefficients->values[3] = 0;

	// Create the filtering object
	pcl::ProjectInliers<pcl::PointXYZ>proj;
	proj.setModelType(pcl::SACMODEL_PLANE);//平面模型
	proj.setInputCloud(cloud);
	proj.setModelCoefficients(coefficients);
	proj.filter(*cloud_projected);

	std::cerr << "Cloud after projection: " << std::endl;
	for (const auto& point : *cloud_projected)
		std::cerr << "    " << point.x << " "
		<< point.y << " "
		<< point.z << std::endl;
	return 0;
}

5.基于分段算法输出的索引提取点云中的子集-ExclIndices

#include<iostream>
#include<pcl/ModelCoefficients.h>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>   //随机参数估计方法头文件
#include <pcl/sample_consensus/model_types.h>   //模型定义头文件
#include <pcl/segmentation/sac_segmentation.h>   //基于采样一致性分割的类的头文件
#include<pcl/filters/voxel_grid.h>
#include<pcl/filters/extract_indices.h>

int main(int argc, char* argv[])
{
	pcl::PCLPointCloud2::Ptr cloud_blob(new pcl::PCLPointCloud2), cloud_filtered_blob(new pcl::PCLPointCloud2);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>), cloud_p(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>);

	//fill in the cloud data
	pcl::PCDReader reader;
	reader.read("table_scene_lms400.pcd", *cloud_blob);
	std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl;

	// Create the filtering object: downsample the dataset using a leaf size of 1cm
	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
	sor.setInputCloud(cloud_blob);
	sor.setLeafSize(0.01, 0.01, 0.01);
	sor.filter(*cloud_filtered_blob);

	// Convert to the templated PointCloud
	pcl::fromPCLPointCloud2(*cloud_filtered_blob, *cloud_filtered);
	std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;

	// Write the downsampled version to disk
	pcl::PCDWriter writer;
	writer.write<pcl::PointXYZ>("table_scene_lms400_downsampled.pcd", *cloud_filtered, false);

	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr  inliers(new pcl::PointIndices);

	pcl::SACSegmentation<pcl::PointXYZ> seg;// 采样一致性分割算法
	seg.setOptimizeCoefficients(true);//设置对估计的模型参数进行优化处理
	seg.setModelType(pcl::SACMODEL_PLANE);//设置分割模型类别
	seg.setMethodType(pcl::SAC_RANSAC);//设置用哪个随机参数估计方法
	seg.setMaxIterations(1000);
	seg.setDistanceThreshold(0.01);//设置判断是否为模型内点的距离阈值

	pcl::ExtractIndices<pcl::PointXYZ> extract;
	int i = 0, nr_points = (int)cloud_filtered->size();
	// While 30% of the original cloud is still there
	//循环运行该过程，并在提取每个模型之后，返回以获取剩余点并进行迭代
	while (cloud_filtered->size() > 0.3 * nr_points)
	{
		seg.setInputCloud(cloud_filtered);
		seg.segment(*inliers, *coefficients);
		if(inliers->indices.size()==0)
		{
			std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
			break;
		}
		extract.setInputCloud(cloud_filtered);
		extract.setIndices(inliers);
		extract.setNegative(false);
		extract.filter(*cloud_p);
		std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;

		std::stringstream ss;
		ss << "table_scene_lms400_plane_" << i << ".pcd";
		writer.write<pcl::PointXYZ>(ss.str(), *cloud_p, false);

		// Create the filtering object
		extract.setNegative(true);
		extract.filter(*cloud_f);
		cloud_filtered.swap(cloud_f);
		i++;

	}
	return 0;
}

6.ConditionalRemoval和RadiusOutlierRemoval移除离群点

RadiusOutlierRemoval滤波器类非常适合去除单个的离群点，ConditionalRemoval比较灵活，可以根据用户设置的条件灵活过滤。

#include<iostream>
#include<pcl/point_types.h>
#include<pcl/filters/radius_outlier_removal.h>
#include<pcl/filters/conditional_removal.h>

int main()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

	// Fill in the cloud data
	cloud->width = 25;
	cloud->height = 1;
	cloud->resize(cloud->width * cloud->height);

	for (auto& point : *cloud)
	{
		point.x = 1024 * rand() / (RAND_MAX + 1.0f);
		point.y = 1024 * rand() / (RAND_MAX + 1.0f);
		point.z = 1024 * rand() / (RAND_MAX + 1.0f);
	}

	int args = 1;
	if(args==0)
	{
		pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
		outrem.setInputCloud(cloud);
		outrem.setRadiusSearch(0.8);//设置在0.8半径的范围内找邻近点
		outrem.setMinNeighborsInRadius(2); //设置查询点的邻近点集数小于2的删除
		outrem.setKeepOrganized(true);//设置保持点云的结构
		outrem.filter(*cloud_filtered);
	}
	if(args==1)
	{
		pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZ>);
		//为条件定义对象添加比较算子
		/*EQ 就是 EQUAL等于
		GT 就是 GREATER THAN大于
		LT 就是 LESS THAN小于
		GE 就是 GREATER THAN OR EQUAL 大于等于
		LE 就是 LESS THAN OR EQUAL 小于等于*/
		//添加在z字段上大于0的比较算子
		range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GT, 0.0)));
		//添加在z字段上小于0.8的比较算子
		range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LT, 0.8)));
		pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
		condrem.setCondition(range_cond);
		condrem.setInputCloud(cloud);
		condrem.setKeepOrganized(true);
		condrem.filter(*cloud_filtered);
	}
	std::cerr << "Cloud before filtering: " << std::endl;
	for (const auto& point : *cloud)
		std::cerr << "    " << point.x << " "
		<< point.y << " "
		<< point.z << std::endl;
	// display pointcloud after filtering
	std::cerr << "Cloud after filtering: " << std::endl;
	for (const auto& point : *cloud_filtered)
		std::cerr << "    " << point.x << " "
		<< point.y << " "
		<< point.z << std::endl;

	std::cerr << cloud_filtered->size();
	return (0);
}

7.CropHull任意多边形内部点云提取

通过输入空间点以构成一个平面多边形，对其中的点云进行提取（沿着该平面的法线方向，且第三个维度没有限制）

#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <vector>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/crop_hull.h>
#include <pcl/surface/concave_hull.h>

int main(int argc, char* argv[])
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCDReader reader;
	reader.read("./pig.pcd", *cloud);

	pcl::PointCloud<pcl::PointXYZ>::Ptr boundingbox_ptr(new pcl::PointCloud<pcl::PointXYZ>);//用来存放边界
	boundingbox_ptr->push_back(pcl::PointXYZ(0.1, 0.1, 0));
	boundingbox_ptr->push_back(pcl::PointXYZ(0.1, -0.1, 0));
	boundingbox_ptr->push_back(pcl::PointXYZ(-0.1, 0.1, 0));
	boundingbox_ptr->push_back(pcl::PointXYZ(-0.1, -0.1, 0));
	boundingbox_ptr->push_back(pcl::PointXYZ(0.15, 0.1, 0));

	//构造2D凸包形状
	pcl::ConvexHull<pcl::PointXYZ> hull;
	hull.setInputCloud(boundingbox_ptr);
	hull.setDimension(2);
	std::vector<pcl::Vertices> polygons;
	pcl::PointCloud<pcl::PointXYZ>::Ptr surface_hull(new pcl::PointCloud<pcl::PointXYZ>);
	hull.reconstruct(*surface_hull, polygons);

	//滤波
	pcl::PointCloud<pcl::PointXYZ>::Ptr objects(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::CropHull<pcl::PointXYZ> bb_filter;
	bb_filter.setDim(2);
	bb_filter.setInputCloud(cloud);
	bb_filter.setHullIndices(polygons);
	bb_filter.setHullCloud(surface_hull);
	bb_filter.filter(*objects);
	std::cout << objects->size() << std::endl;

	//visualize
	boost::shared_ptr<pcl::visualization::PCLVisualizer> for_visualizer_v(new pcl::visualization::PCLVisualizer("crophull display"));
	for_visualizer_v->setBackgroundColor(255, 255, 255);

	int v1(0);
	for_visualizer_v->createViewPort(0.0, 0.0, 0.33, 1, v1);
	for_visualizer_v->setBackgroundColor(255, 255, 255, v1);
	for_visualizer_v->addPointCloud(cloud, "cloud", v1);
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 255, 0, 0, "cloud");
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");
	for_visualizer_v->addPolygon<pcl::PointXYZ>(surface_hull, 0, .069 * 255, 0.2 * 255, "backview_hull_polyline1", v1);

	int v2(0);
	for_visualizer_v->createViewPort(0.33, 0.0, 0.66, 1, v2);
	for_visualizer_v->setBackgroundColor(255, 255, 255, v2);
	for_visualizer_v->addPointCloud(surface_hull, "surface_hull", v2);
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 255, 0, 0, "surface_hull");
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 8, "surface_hull");
	for_visualizer_v->addPolygon<pcl::PointXYZ>(surface_hull, 0, .069 * 255, 0.2 * 255, "backview_hull_polyline", v2);
	int v3(0);
	for_visualizer_v->createViewPort(0.66, 0.0, 1, 1, v3);
	for_visualizer_v->setBackgroundColor(255, 255, 255, v3);
	for_visualizer_v->addPointCloud(objects, "objects", v3);
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 255, 0, 0, "objects");
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "objects");
	while (!for_visualizer_v->wasStopped())
	{
		for_visualizer_v->spinOnce(1000);
	}
	system("pause");
	return 0;
}

参考

1.https://pcl.readthedocs.io/projects/tutorials/en/latest/#filtering
2.