PCL点云库调库学习系列——点云滤波(附完整代码）

1 点云滤波

PCL中总结了集中需要进行点云滤波处理的情况，分别如下：

1. 点云数据密度不规则需要平滑。
2. 因为遮挡等问题造成离群点需要去除。
3. 大量数据需要进行下采样。
4. 噪声数据需要去除。

对应的方法如下：

1. 按具体给定的规则限制过滤去除点
2. 通过常用滤波算法修改点的部分属性
3. 对数据进行下采样

PCL点云格式分为有序点云和无序点云，针对有序点云提供了双边滤波、高斯滤波、中值滤波等，针对无序点云提供了体素栅格、随机采样等。

1.1 直通滤波

直通滤波主要功能共是指定一个维度，去掉在用户指定范围内或范围外的点

关键代码

//创建一个直通滤波器的对象，并设置相关参数
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 
#include 
#include 	//直通滤波头文件

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
    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, 1.0);			//设置过滤范围，这里选择过滤掉0~1范围内的点云
    //pass.setFilterLimitsNegative (true);	//设置选择保留范围内的还是过滤掉范围内的点云
    pass.filter(*cloud_filtered);			//执行滤波，结果保存在cloud_filter中

    //查看过滤后的结果
    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);
}

1.2 统计滤波StatisticalOutlierRemoval

稀疏离群点移除方法， 通过对查询点与邻域点集之间的距离统计判断来过滤离群点 ：对每个点，我们计算它到它的所有临近点的平均距离。假设得到的结果是一个高斯分布，其形状由均值和标准差决定，平均距离在标准范围（由全局距离平均值和方差定义〉之外的点，可被定义为离群点并可从数据集中去除掉。

关键代码说明

//创建滤波器对象，及相关参数设置
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中

完整代码

#include 
#include 
#include 
#include 

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);										//设置统计时考虑查询点邻近点数
	/*
	* 设置判断是否为离群点的阈值
	* 更具体为设置标准差倍数阈值 std_mul ，点云中所有点与其邻域的距离大于 μ ±σ• std_mul 
	* 则被认为是离群点，其中 μ 代表估计的平均距离， σ 代表标准差 。
	*/
	sor.setStddevMulThresh(1.0);	//设置为1代表：如果一个点的距离超过平均距离一个标准差以上，则会被当做离群点去除
	sor.filter(*cloud_filtered);	//执行滤波，并将结果保存在cloud_filter中

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

1.3 条件滤波ConditionaRemoval

用于删除点云中不符合用户指定的一个或多个条件

关键代码

//创建条件
pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond(new
                                                 pcl::ConditionAnd<pcl::PointXYZ>());    //创建条件定义对象
//添加在z字段上大于0的比较算子   GT: Greater Than
range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new
                          pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GT, 0.0)));
//添加在z字段上小于0.8的比较算子   LT: Less Than
range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new
                         pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LT, 0.8)));

// build the filter
//创建滤波器
pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
condrem.setCondition(range_cond);           //设置先前创建的条件对象
condrem.setInputCloud(cloud);               //设置输入点云
condrem.setKeepOrganized(true);             //设置保持点云的结构

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

完整代码

#include 
#include 
#include 

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
    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;
    // build the condition
    //创建条件
    pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond(new
        pcl::ConditionAnd<pcl::PointXYZ>());    //创建条件定义对象
    //添加在z字段上大于0的比较算子   GT: Greater Than
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new
        pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GT, 0.0)));
    //添加在z字段上小于0.8的比较算子   LT: Less Than
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new
        pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LT, 0.8)));

    // build the filter
    //创建滤波器
    pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
    condrem.setCondition(range_cond);           //设置先前创建的条件对象
    condrem.setInputCloud(cloud);               //设置输入点云
    condrem.setKeepOrganized(true);             //设置保持点云的结构

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

    // 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);
}

1.4 半径滤波RadiusOutlierRemoval

用于用户指定每个点的一定范围内周围至少有足够多的近邻，如果指定至少需要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

完整代码

#include 
#include 
#include 

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

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

    // 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);
}

1.5 voxelGrid栅格滤波（下采样）

通常我们使用体素化网格方法实现下采样，即减少点的数量，减少点云数据，并同时保持点云的形状特征，在提高配准、曲面重建、形状识别等算法速度中非常实用。

PCL实现的voxleGrid类通过输入的点云数据创建一个三维体素栅格（可把体素栅格想象为微小的空间三维立方体的集合），然后在每个体素（即三维立方体）内，用体素中所有的重心来近似显示体素中其他点，这样该体素内所有点就用一个重心点最终表示，对于所有体素处理后得到过滤后的点云。这种方法比用体素中心来逼近更慢，但它对于采样点对应曲面的表示更为准确。

完整代码

#include 
#include 
#include 
#include 			//头文件

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

1.6 参数化模型投影滤波

关键代码说明

/*
    * 通过参数来创建投影平面   平面模型为: ax+by+cz+d=0，其中a,b,d=0,c=1，最后模型为z=0
    * 即X-Y平面
*/ 
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
coefficients->values.resize(4);
coefficients->values[0] = coefficients->values[1] = 0;  //a=0, b=0
coefficients->values[2] = 1.0;                          //c=1
coefficients->values[3] = 0;                            //d=0

//创建对象，及相关参数的设置
pcl::ProjectInliers<pcl::PointXYZ> proj;        //创建滤波对象
proj.setModelType(pcl::SACMODEL_PLANE);         //设置对象的投影模型，这里为随机采样的平面模型
proj.setInputCloud(cloud);                      //设置输入点云
proj.setModelCoefficients(coefficients);        //设置模型的对应参数
proj.filter(*cloud_projected);                  //执行滤波，结果保存在cloud_projected

完整代码

#include 
#include 
#include 
#include           //模型系数头文件
#include     //投影滤波头文件

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 (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 projection: " << 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 a set of planar coefficients with X=Y=0,Z=1
    /*
    * 通过参数来创建投影平面   平面模型为: ax+by+cz+d=0，其中a,b,d=0,c=1，最后模型为z=0
    * 即X-Y平面
    */ 
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
    coefficients->values.resize(4);
    coefficients->values[0] = coefficients->values[1] = 0;  //a=0, b=0
    coefficients->values[2] = 1.0;                          //c=1
    coefficients->values[3] = 0;                            //d=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);                  //执行滤波，结果保存在cloud_projected

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

    return (0);
}

1.7 从一个点集中提取一个子集extraction

程序流程

1. 下采样（非必须），但点云数据量大的时候，有利于后续分割的处理速度
2. 分割SACSegmentation
3. 提取ExtractIndices

关键代码

//分割
pcl::SACSegmentation<pcl::PointXYZ> seg;        //创建分割对象
// Optional
//优化
seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_PLANE);      //分割模型，随机采样的平面模型
seg.setMethodType(pcl::SAC_RANSAC);         //分割方法，随机采样一致性
seg.setMaxIterations(1000);                 //迭代次数
seg.setDistanceThreshold(0.01);             //设置判断是否为模型内点的距离阈值

seg.setInputCloud(cloud_filtered);      //设置输入点云
//inliers:输出参数：找到模型的结果的索引
//coefficients:输出参数：模型参数的结果
seg.segment(*inliers, *coefficients);   

/******************************************/

//提取
extract.setInputCloud(cloud_filtered);  //设置输入点云
extract.setIndices(inliers);            //设置分割后的内点为需要提取的点击
extract.setNegative(false);             //设置提取内点，而不是外点
extract.filter(*cloud_p);               //执行提取，结果保存在cloud_p中

完整代码

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

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(".//pcd//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.01f, 0.01f, 0.01f);
    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());    //创建内点
    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZ> seg;        //创建分割对象
    // Optional
    //优化
    seg.setOptimizeCoefficients(true);
    // Mandatory
    seg.setModelType(pcl::SACMODEL_PLANE);      //分割模型，随机采样的平面模型
    seg.setMethodType(pcl::SAC_RANSAC);         //分割方法，随机采样一致性
    seg.setMaxIterations(1000);                 //迭代次数
    seg.setDistanceThreshold(0.01);             //设置判断是否为模型内点的距离阈值

    // Create the filtering object
    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);      //设置输入点云
        //inliers:输出参数：找到模型的结果的索引
        //coefficients:输出参数：模型参数的结果
        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);               //执行提取，结果保存在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_f保存的是没被过滤掉的点
        cloud_filtered.swap(cloud_f);       //将没被过滤掉的点用cloud_filter保存，并接着进行下一次的过滤操作
        i++;
    }

    return (0);
}