PCL点云库学习笔记(滤波)

点云滤波

一、使用直通滤波器

#include <iostream>
#include <pcl/point_types.h>
#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 (std::size_t i = 0; i < cloud->points.size(); ++i)
	{
		cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);//为点云填充数据
		cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
		cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
	}

	std::cerr << "Cloud before filtering: " << std::endl;//打印所有点
	for (std::size_t i = 0; i < cloud->points.size(); ++i)
		std::cerr << "    " << cloud->points[i].x << " "
		<< cloud->points[i].y << " "
		<< cloud->points[i].z << std::endl;

	// Create the filtering object
	pcl::PassThrough<pcl::PointXYZ> pass;//设置滤波器对象
	pass.setInputCloud(cloud);//输入点云
	pass.setFilterFieldName("z");//设置过滤式所需要的点云类型的z字段
	pass.setFilterLimits(0.0, 500.0);//设置过滤字段的范围
	//pass.setFilterLimitsNegative (true);
	pass.filter(*cloud_filtered);//执行滤波

	//输出滤波点云
	std::cerr << "Cloud after filtering: " << std::endl;
	for (std::size_t i = 0; i < cloud_filtered->points.size(); ++i)
		std::cerr << "    " << cloud_filtered->points[i].x << " "
		<< cloud_filtered->points[i].y << " "
		<< cloud_filtered->points[i].z << std::endl;

	return (0);
}

二、使用VoxelGrid滤波器进行下采样

对点云数据集进行降采样，即减少点数。VoxelGrid类在输入点云数据上创建一个三维体素网格（将体素网格视为空间中的微小的三维立方体集合）。在每个体素（立方体）中，用体素中所有点的重心来表示体素内的所有点（即下采样）。 这种方法比用体素的中心逼近它们要慢一些，但是它可以更准确地表示曲面。

#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.01f, 0.01f, 0.01f);//设置体素大小为1cm
	sor.filter(*cloud_filtered);//执行滤波

	std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height
		<< " data points (" << pcl::getFieldsList(*cloud_filtered) << ")." << std::endl;
 
   //执行完会生成一个PCD文件
	pcl::PCDWriter writer;
	writer.write("table_scene_lms400_downsampled.pcd", *cloud_filtered,
		Eigen::Vector4f::Zero(), Eigen::Quaternionf::Identity(), false);

	return (0);
}

运行

三、概述

由于测量误差，某些数据集会出现大量阴影点。 通过对每个点的邻域进行统计分析，并修剪不符合特定条件的那些异常值，可以过滤掉某些异常值。 PCL中的稀疏离群值的消除，是基于输入数据集中点到邻域点的距离分布。 对于每个点，将计算从该点到其所有邻居的平均距离。 通过假定结果分布是具有均值和标准差的高斯分布，可以将其平均距离在由全局距离均值和标准差定义的区间之外的所有点视为离群值。

#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);//临近点的个数为50
	sor.setStddevMulThresh(1.0);//标准差偏离阈值为1，一个点的距离超过平均距离1个标准差以上，就是离群点
	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);
}

运行：直接用vs调试，运行过程很慢，用的笔记本，花了两三分钟

四、从一个点云中提取子集

使用ExtractIndiceslvboqi ，根据分割算法输出的索引从点云中提取一个点子集。

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

	//读取点云数据 
	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
	//利用voxelgrid滤波器进行下采样，这是为了加速处理的过程
	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;//体素栅格下采样对象
	sor.setInputCloud(cloud_blob);//设置原始的点云数据
	sor.setLeafSize(0.01f, 0.01f, 0.01f);//设置体素的代销为0.01
	sor.filter(*cloud_filtered_blob);//将采样后的结果保存到点云中

	// Convert to the templated PointCloud
	//将二进制数据blob转化为pointcloud<T>
	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->points.size();
	// While 30% of the original cloud is still there
	while (cloud_filtered->points.size() > 0.3 * nr_points)
	{
		// Segment the largest planar component from the remaining cloud
		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 the inliers
		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);
}

五、使用ConditionalRemoval或RadiuOutlierRemoval移除离群点

1.RadiuOutlierRemoval
指定每个点的一定范围内周围至少有足够多的近邻。 例如，如果指定了必须要有1个近邻点，则只会删除黄点。 如果指定了2个邻居，则黄点和绿点都将从PointCloud中删除。

2.ConditionalRemoval
从PointCloud中删除所有不满足用户指定的一个或多个条件的点

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

int main(int argc, char** argv)
{
	//保证用户输入了正确的命令行参数
	if (argc != 2)
	{
		std::cerr << "please specify command line arg '-r' or '-c'" << std::endl;
		exit(0);
	}


	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 (std::size_t i = 0; i < cloud->points.size(); ++i)
	{
		cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
		cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
		cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
	}

	//RadiusOutlierRemoval移除离群点
	if (strcmp(argv[1], "-r") == 0) {
		pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;//创建滤波器
		// build the filter
		outrem.setInputCloud(cloud);//设置输入点云
		outrem.setRadiusSearch(0.8);//设置在0.8半径的范围内查找邻近点
		outrem.setMinNeighborsInRadius(2);//邻近点小于2的点集删除
		// apply filter
		outrem.filter(*cloud_filtered);//移除离群点后储存
	}

	//条件移除离群点
	else if (strcmp(argv[1], "-c") == 0) {
		
		// 创建条件限定下的滤波器
		pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZ>());
		//为条件定义对象添加比较算子，分别添加z字段上大于0和小于0.8的比较算子
		range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GT, 0.0)));
		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);
		// apply filter
		condrem.filter(*cloud_filtered);
	}
	else {
		std::cerr << "please specify command line arg '-r' or '-c'" << std::endl;
		exit(0);
	}

	std::cerr << "Cloud before filtering: " << std::endl;
	for (std::size_t i = 0; i < cloud->points.size(); ++i)
		std::cerr << "    " << cloud->points[i].x << " "<< cloud->points[i].y << " "<< cloud->points[i].z << std::endl;

	// display pointcloud after filtering
	std::cerr << "Cloud after filtering: " << std::endl;
	for (std::size_t i = 0; i < cloud_filtered->points.size(); ++i)
		std::cerr << "    " << cloud_filtered->points[i].x << " "<< cloud_filtered->points[i].y << " "<< cloud_filtered->points[i].z << std::endl;

	
	return (0);
}