1 点云滤波
PCL中总结了集中需要进行点云滤波处理的情况，分别如下：
- 点云数据密度不规则需要平滑。
- 因为遮挡等问题造成离群点需要去除。
- 大量数据需要进行下采样。
- 噪声数据需要去除。
对应的方法如下：
- 按具体给定的规则限制过滤去除点
- 通过常用滤波算法修改点的部分属性
- 对数据进行下采样
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 <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>);
// 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 <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); //设置统计时考虑查询点邻近点数
/*
* 设置判断是否为离群点的阈值
* 更具体为设置标准差倍数阈值 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 <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/conditional_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
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 <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/radius_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
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 <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);
}
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 <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.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 (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
程序流程
- 下采样（非必须），但点云数据量大的时候，有利于后续分割的处理速度
- 分割SACSegmentation
- 提取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 <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(".//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);
}