点云滤波
刚刚产生的点云往往伴随着将噪声点、离群点、孔洞、数据压缩等 按照后续需求处理,才能够更好地进行配准、特征提取、曲面重建、可视化等后续流程。
- PCL滤波模块提供了很多滤波处理算法
- 双边滤波
- 高斯滤波
- 条件滤波
- 直通滤波
- 基于随即采样一致性滤波RANSAC等。
- 滤波相关文档
- https://pcl-tutorials.readthedocs.io/en/latest/#filtering
- 应用场景
- 点云数据密度不规则需要平滑处理
- 去除因为遮挡等问题造成离群点
- 数据量较大,需要进行下采样( Downsample)
- 去除噪声数据。
- 取出噪声实例: 我们通过对每个点的邻域进行统计分析,并修剪掉不符合特定条件的那些异常值,进而可以过滤掉某些异常值。
PCL中的实现这些稀疏离群值的消除,需要计算数据集中的点与邻居距离的分布。 即对于每个点,都会计算从它到所有相邻点的平均距离。 通过假设结果分布是具有均值和标准差的高斯分布,可以将那些平均距离在【由全局距离均值和标准差定义的区间】之外的所有点视为离群值,并将之从数据集中进行修剪。
直通滤波 PassThrouh.cpp
/*PassThrogh
* 直通滤波
*/
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/visualization/cloud_viewer.h>
typedef pcl::PointXYZ PointT;
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(size_t i=0;i<cloud->points.size();++i)
{
cloud->points[i].x=1024*rand()/(RAND_MAX+1.0f);
cloud->points[i].x=1024*rand()/(RAND_MAX+1.0f);
cloud->points[i].z=1024*rand()/(RAND_MAX+1.0f);
}
std::cerr <<"cloud before filtering:"<< std::endl;
for(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;
}
//Creatte the filter object
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud); //设置输入源
pass.setFilterFieldName("z"); //设置过滤域名
pass.setFilterLimits(0.0,1.0); //设置过滤范围 (保留0-1之间的内容)
pass.setFilterLimitsNegative(true); //保留0-1之外的内容
pass.filter(*cloud_filtered); //设置输入容器
std::cerr <<"Cloud after filtering: "<<std::endl;
for(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;
}
pcl::visualization::CloudViewer viewer("Cloud Viewer");
//这里会一直阻塞知道点云被渲染
viewer.showCloud(cloud);
while(!viewer.wasStopped()){}
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);
//从文件读取点云图
//File in the cloud data
pcl::PCDReader reader;
//Replace the path below with the path where you saved your file
reader.read("./data/table_scene_lms400.pcd",*cloud);
std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height
<< " data points (" << pcl::getFieldsList (*cloud) << ")."<<std::endl;
// 创建一个长宽高分别是1cm的体素过滤器,cloud作为输入数据,cloud_filtered作为输出数据
float leftsize=0.01f; //pcl中标准长度以m作为单位
// Create the filtering object
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloud);
sor.setLeafSize(leftsize,leftsize,leftsize);
sor.filter(*cloud_filtered);
std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height
<< " data points (" << pcl::getFieldsList (*cloud_filtered) << ").";
//将结果输出到文件
pcl::PCDWriter writer;
writer.write("./data/table_scene_lms400_downsampled.pcd", *cloud_filtered);
return (0);
}
离群点移除
如激光扫描器,通常会生成不同点密度的点云数据集。此外,测量误差会导致稀疏的异常值,从而进一步破坏结果。这会使局部点云特征(例如表面或曲率变化)的估计复杂化,从而导致错误的值。
稀疏离群值的消除基于输入数据集中点到邻居距离的分布的计算。对于每个点,我们计算从它到所有相邻点的平均距离。通过假设结果分布是具有均值和标准差的高斯分布,可以将其平均距离在由全局距离均值和标准差定义的区间之外的所有点视为离群值并从数据集中进行修剪。 下图显示了稀疏离群值分析和删除的效果:原始数据集显示在左侧,结果数据集显示在右侧。数据集图显示了滤波前后每个点的邻域中平均K最近邻距离。
接下来,我们使用StatisticalOutlierRemoval移除噪点
实现步骤:
-
查找每一个点的所有邻域点
-
计算每个点到其邻居的距离 d i j d_{ij} dij,其中 i = [ 1 , 2 , 3 , ⋯ , m ] i=[1,2,3,\cdots,m] i=[1,2,3,⋯,m]表示共m个点,j=[1,\cdots,k]每个点有k个邻居。也就是记录了每个点到他们K个邻居的平均距离
-
根据高斯分布 d − − N ( u , σ ) d -- N(u,\sigma) d−−N(u,σ) 模型化距离参数,计算所有点与邻居的 μ \mu μ(距离的均值), σ \sigma σ(距离的标准差),如下:
μ = 1 n k ∑ i = 1 m ∑ j = 1 k d i j , σ = 1 n k ∑ i = 1 m ∑ j = 1 k ( d i j − μ ) 2 \mu=\frac{1}{nk} \sum_{i=1}^m \sum_{j=1}^k d_{ij},\sigma=\sqrt{\frac{1}{nk} \sum_{i=1}^m \sum_{j=1}^k (d_{ij}-\mu)^2} μ=nk1i=1∑mj=1∑kdij,σ=nk1i=1∑mj=1∑k(dij−μ)2 -
为每一个点,计算其与邻居的距离均值 μ i = 1 k ∑ j = 1 k d i j \mu_i=\frac{1}{k} \sum_{j=1}^k d_{ij} μi=k1∑j=1kdij
-
遍历所有点,如果其距离的均值大于高斯分布的指定置信度,或者指定置信区间,则移除,比如 μ i > μ + 3 σ , o r , μ i < μ − 3 σ \mu_i>\mu+3\sigma \ ,\ or\ ,\ \mu_i<\mu-3\sigma μi>μ+3σ , or , μi<μ−3σ
-
相应的代码实现:
#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>("./data/table_scene_lms400.pcd",*cloud);
std::cerr << "Cloud before filtering: "<<std::endl;
std::cerr << *cloud <<std::endl;
// 创建过滤器,每个点分析计算时考虑的最近邻居个数为50个;
// 设置标准差阈值为1,这意味着所有距离查询点的平均距离的标准偏差均大于1个标准偏差的所有点都将被标记为离群值并删除。
// 计算输出并将其存储在cloud_filtered中
// Create the filtering object
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud);
// 设置平均距离估计的最近邻居的数量K
sor.setMeanK(50);
//设置标准差阈值稀疏
sor.setStddevMulThresh(1.0);
sor.filter(*cloud_filtered);
std::cerr<<"cloud after filtering: "<<std::endl;
std::cerr<<*cloud_filtered<<std::endl;
//将留下来的点保存到后缀为_inliers.pcd的文件中
pcl::PCDWriter writer;
writer.write("./data/table_scene_lms400_inliers.pcd", *cloud_filtered, false);
// 使用个相同的过滤器,但是对输出结果取反,则得到那些被过滤掉的点,保存到_outliers.pcd文件
sor.setNegative(true);
sor.filter(*cloud_filtered);
writer.write<pcl::PointXYZ> ("./data/table_scene_lms400_outliers.pcd", *cloud_filtered, false);
return (0);
}
条件滤波与半径离群值滤波
条件滤波:设置不同维度滤波规则进行滤波
半径滤波:用户指定邻居的个数,要每个点必须在指定半径内具有指定个邻居才能保留在PointCloud中。例如,如果指定了1个邻居,则只会从PointCloud中删除黄点。如果指定了2个邻居,则黄色和绿色的点都将从PointCloud中删除。
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/visualization/pcl_visualizer.h>
typedef pcl::PointXYZ PointType;
void showPointClouds(const pcl::PointCloud<PointType>::Ptr &cloud,const pcl::PointCloud<PointType>::Ptr &cloud2){
// 创建PCLVisualizer
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
// 设置背景色为灰色
viewer->setBackgroundColor(0.05, 0.05, 0.05, 0);
// 添加一个普通点云 (可以设置指定颜色,也可以去掉single_color参数不设置)
pcl::visualization::PointCloudColorHandlerCustom<PointType> single_color(cloud, 0, 255, 0);
viewer->addPointCloud<PointType>(cloud, single_color, "sample cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud");
// 添加一个第二个点云 (可以设置指定颜色,也可以去掉single_color2参数不设置)
pcl::visualization::PointCloudColorHandlerCustom<PointType> single_color2(cloud, 255, 0, 0);
viewer->addPointCloud<PointType>(cloud2, single_color2, "sample cloud 2");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "sample cloud 2");
viewer->addCoordinateSystem(1.0);
while (!viewer->wasStopped()) {
viewer->spinOnce();
}
}
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>);
//Fill in the cloud data
cloud->width=100;
cloud->height=1;
cloud->points.resize(cloud->width*cloud->height);
for (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);
}
if(strcmp(argv[1],"-r")==0){
pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
//build the filter
outrem.setInputCloud(cloud);
outrem.setRadiusSearch(0.4);
outrem.setMinNeighborsInRadius(10);
//apply filter
outrem.filter(*cloud_filtered);
}else if(strcmp(argv[1],"-c")==0)
{
//build the condition
pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZ>());
/*
pcl::ComparisonOps::LE //小于等于
pcl::ComparisonOps::GT //大于
pcl::ComparisonOps::LT //小于
pcl::ComparisonOps::EQ //相等
*/
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)));
//build the filter
pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
condrem.setCondition(range_cond);
condrem.setInputCloud(cloud);
condrem.setKeepOrganized(false);
//condrem.setKeepOrganized(true); 是否保留原有的点云组织,保留的话去掉的点会以nan的形式出现
//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 (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 (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;
showPointClouds(cloud,cloud_filtered);
return (0);
}