ROS::点云PCL(3)直通滤波

理解

在获取点云数据时，由于设备精度、操作者经验、环境因素等带来的影响，以及电磁波衍射特性、被测物体表面性质变化和数据拼接配准操作过程的影响，点云数据中将不可避免地出现一些噪声点。实际应用中除了这些测量随机误差产生的噪声点之外，由于受到外界干扰如视线遮挡、障碍物等因素的影响，点云数据中往往存在着一些离主体点云较远的离散点，即离群点。不同的获取设备点云噪声结构也有不同。

通过滤波完成的功能还包括孔洞修复、最小信息损失的海量点云数据压缩处理等 。在点云处理流程中滤波处理作为预处理的第一步，往往对后续处理流程影响很大，只有在滤波预处理中将噪声点、离群点、孔洞、数据压缩等 按照后续需求处理，才能够更好地进行配准、特征提取、曲面重建、可视化等后续流程。

PCL 中点云滤波模块提供了很多灵活实用的滤波处理算法，例如双边滤波、高斯滤波、条件滤波、直通滤波、基于随机采样一致性滤波RANSAC等。滤波模块是作为 PCL的一个重要处理模块，其在应用中可以非常方便与其他点云处理流程协同使用。

应用场景
1、点云数据密度不规则需要平滑处理
2、去除因为遮挡等问题造成离群点
3、数据量较大，需要进行下采样( Downsample)
4、去除噪声数据。

下图显示了一个噪声消除的示例。 由于测量误差，某些数据集会出现大量阴影点。 这使局部点云3D特征的估算变得复杂。我们通过对每个点的邻域进行统计分析，并修剪掉不符合特定条件的那些异常值，进而可以过滤掉某些异常值。

PCL中的实现这些稀疏离群值的消除，需要计算数据集中的点与邻居距离的分布。 即对于每个点，都会计算从它到所有相邻点的平均距离。 通过假设结果分布是具有均值和标准差的高斯分布，可以将那些平均距离在【由全局距离均值和标准差定义的区间】之外的所有点视为离群值，并将之从数据集中进行修剪。

直通滤波PassThrough

  #include  
#include  
#include  
#include  

#include 
#include 

#include 
#include 
#include 

#include 

main (int argc, char **argv) 
{ 

    ros::init (argc, argv, "pcl_create"); 

    ros::NodeHandle nh; 
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);  
    
    // 用系统时间初始化随机种子
    srand(time(NULL));
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);

    sensor_msgs::PointCloud2 output; 

    int point_num;
    float table[8]={-2,-1.5,-1,-0.5,0.5,1,1.5,2};

    cloud->width = 512;
    cloud->height = 1; 
    cloud->points.resize(cloud->width * cloud->height); 

    for(int a=0;a<8;++a){
        float width = table[a];
        for(int i=0;i<8;++i){
			float length = table[i];
	  	    for(int c=0;c<8;++c){
				point_num = a*64+i*8+c;
				cloud->points[point_num].x = width; 
				cloud->points[point_num].y = length; 
				cloud->points[point_num].z = table[c];
				//std::cout <<"x="<< cloud->points[point_num].x <<"   y="<< cloud->points[point_num].y <<"   z="<< cloud->points[point_num].z  << std::endl;
				if(width==-0.5 && length==-0.5 && table[c]==-0.5){
					cloud->points[point_num].r=0;
					cloud->points[point_num].g=255;
					cloud->points[point_num].b=255;
				}else{
				   cloud->points[point_num].r=0;
				   cloud->points[point_num].g=0;
				   cloud->points[point_num].b=255;
		   		}

			}
		}
	}

    pcl::PassThrough<pcl::PointXYZRGBA> pass;
    pass.setInputCloud(cloud);      // 1. 设置输入源
    pass.setFilterFieldName("x");       // 2. 设置过滤域名
    pass.setFilterLimits(-2, 0);     // 3. 设置过滤范围
	//pass.setFilterLimitsNegative(true); // 设置获取Limits之外的内容
    pass.filter(*cloud);       // 4. 执行过滤，并将结果输出到cloud_filtered

    std::cerr << "Cloud after 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;
	}

    pcl::toROSMsg(*cloud, output); 
    output.header.frame_id = "odom"; 
    ros::Rate loop_rate(1); 

    while (ros::ok()) { 
	pcl_pub.publish(output);
	ros::spinOnce(); 
	loop_rate.sleep(); 
    } 
    return 0;
}

输出过滤后的结果

Cloud after filtering: 
    -2 -2 -2
    -2 -2 -1.5
    -2 -2 -1
    -2 -2 -0.5
    -2 -2 0.5
    -2 -2 1
    -2 -2 1.5
    -2 -2 2
    -2 -1.5 -2
    -2 -1.5 -1.5
    -2 -1.5 -1
    -2 -1.5 -0.5
    -2 -1.5 0.5
    -2 -1.5 1
    -2 -1.5 1.5
    -2 -1.5 2
    -2 -1 -2
    -2 -1 -1.5
    -2 -1 -1
    -2 -1 -0.5
    -2 -1 0.5
    -2 -1 1
    -2 -1 1.5
    -2 -1 2
    -2 -0.5 -2
    -2 -0.5 -1.5
    -2 -0.5 -1
    -2 -0.5 -0.5
    -2 -0.5 0.5
    -2 -0.5 1
    -2 -0.5 1.5
    -2 -0.5 2
    -2 0.5 -2
    -2 0.5 -1.5
    -2 0.5 -1
    -2 0.5 -0.5
    -2 0.5 0.5
    -2 0.5 1
    -2 0.5 1.5
    -2 0.5 2
    -2 1 -2
    -2 1 -1.5
    -2 1 -1
    -2 1 -0.5
    -2 1 0.5
    -2 1 1
    -2 1 1.5
    -2 1 2
    -2 1.5 -2
    -2 1.5 -1.5
    -2 1.5 -1
    -2 1.5 -0.5
    -2 1.5 0.5
    -2 1.5 1
    -2 1.5 1.5
    -2 1.5 2
    -2 2 -2
    -2 2 -1.5
    -2 2 -1
    -2 2 -0.5
    -2 2 0.5
    -2 2 1
    -2 2 1.5
    -2 2 2
    -1.5 -2 -2
    -1.5 -2 -1.5
    -1.5 -2 -1
    -1.5 -2 -0.5
    -1.5 -2 0.5
    -1.5 -2 1
    -1.5 -2 1.5
    -1.5 -2 2
    -1.5 -1.5 -2
    -1.5 -1.5 -1.5
    -1.5 -1.5 -1
    -1.5 -1.5 -0.5
    -1.5 -1.5 0.5
    -1.5 -1.5 1
    -1.5 -1.5 1.5
    -1.5 -1.5 2
    -1.5 -1 -2
    -1.5 -1 -1.5
    -1.5 -1 -1
    -1.5 -1 -0.5
    -1.5 -1 0.5
    -1.5 -1 1
    -1.5 -1 1.5
    -1.5 -1 2
    -1.5 -0.5 -2
    -1.5 -0.5 -1.5
    -1.5 -0.5 -1
    -1.5 -0.5 -0.5
    -1.5 -0.5 0.5
    -1.5 -0.5 1
    -1.5 -0.5 1.5
    -1.5 -0.5 2
    -1.5 0.5 -2
    -1.5 0.5 -1.5
    -1.5 0.5 -1
    -1.5 0.5 -0.5
    -1.5 0.5 0.5
    -1.5 0.5 1
    -1.5 0.5 1.5
    -1.5 0.5 2
    -1.5 1 -2
    -1.5 1 -1.5
    -1.5 1 -1
    -1.5 1 -0.5
    -1.5 1 0.5
    -1.5 1 1
    -1.5 1 1.5
    -1.5 1 2
    -1.5 1.5 -2
    -1.5 1.5 -1.5
    -1.5 1.5 -1
    -1.5 1.5 -0.5
    -1.5 1.5 0.5
    -1.5 1.5 1
    -1.5 1.5 1.5
    -1.5 1.5 2
    -1.5 2 -2
    -1.5 2 -1.5
    -1.5 2 -1
    -1.5 2 -0.5
    -1.5 2 0.5
    -1.5 2 1
    -1.5 2 1.5
    -1.5 2 2
    -1 -2 -2
    -1 -2 -1.5
    -1 -2 -1
    -1 -2 -0.5
    -1 -2 0.5
    -1 -2 1
    -1 -2 1.5
    -1 -2 2
    -1 -1.5 -2
    -1 -1.5 -1.5
    -1 -1.5 -1
    -1 -1.5 -0.5
    -1 -1.5 0.5
    -1 -1.5 1
    -1 -1.5 1.5
    -1 -1.5 2
    -1 -1 -2
    -1 -1 -1.5
    -1 -1 -1
    -1 -1 -0.5
    -1 -1 0.5
    -1 -1 1
    -1 -1 1.5
    -1 -1 2
    -1 -0.5 -2
    -1 -0.5 -1.5
    -1 -0.5 -1
    -1 -0.5 -0.5
    -1 -0.5 0.5
    -1 -0.5 1
    -1 -0.5 1.5
    -1 -0.5 2
    -1 0.5 -2
    -1 0.5 -1.5
    -1 0.5 -1
    -1 0.5 -0.5
    -1 0.5 0.5
    -1 0.5 1
    -1 0.5 1.5
    -1 0.5 2
    -1 1 -2
    -1 1 -1.5
    -1 1 -1
    -1 1 -0.5
    -1 1 0.5
    -1 1 1
    -1 1 1.5
    -1 1 2
    -1 1.5 -2
    -1 1.5 -1.5
    -1 1.5 -1
    -1 1.5 -0.5
    -1 1.5 0.5
    -1 1.5 1
    -1 1.5 1.5
    -1 1.5 2
    -1 2 -2
    -1 2 -1.5
    -1 2 -1
    -1 2 -0.5
    -1 2 0.5
    -1 2 1
    -1 2 1.5
    -1 2 2
    -0.5 -2 -2
    -0.5 -2 -1.5
    -0.5 -2 -1
    -0.5 -2 -0.5
    -0.5 -2 0.5
    -0.5 -2 1
    -0.5 -2 1.5
    -0.5 -2 2
    -0.5 -1.5 -2
    -0.5 -1.5 -1.5
    -0.5 -1.5 -1
    -0.5 -1.5 -0.5
    -0.5 -1.5 0.5
    -0.5 -1.5 1
    -0.5 -1.5 1.5
    -0.5 -1.5 2
    -0.5 -1 -2
    -0.5 -1 -1.5
    -0.5 -1 -1
    -0.5 -1 -0.5
    -0.5 -1 0.5
    -0.5 -1 1
    -0.5 -1 1.5
    -0.5 -1 2
    -0.5 -0.5 -2
    -0.5 -0.5 -1.5
    -0.5 -0.5 -1
    -0.5 -0.5 -0.5
    -0.5 -0.5 0.5
    -0.5 -0.5 1
    -0.5 -0.5 1.5
    -0.5 -0.5 2
    -0.5 0.5 -2
    -0.5 0.5 -1.5
    -0.5 0.5 -1
    -0.5 0.5 -0.5
    -0.5 0.5 0.5
    -0.5 0.5 1
    -0.5 0.5 1.5
    -0.5 0.5 2
    -0.5 1 -2
    -0.5 1 -1.5
    -0.5 1 -1
    -0.5 1 -0.5
    -0.5 1 0.5
    -0.5 1 1
    -0.5 1 1.5
    -0.5 1 2
    -0.5 1.5 -2
    -0.5 1.5 -1.5
    -0.5 1.5 -1
    -0.5 1.5 -0.5
    -0.5 1.5 0.5
    -0.5 1.5 1
    -0.5 1.5 1.5
    -0.5 1.5 2
    -0.5 2 -2
    -0.5 2 -1.5
    -0.5 2 -1
    -0.5 2 -0.5
    -0.5 2 0.5
    -0.5 2 1
    -0.5 2 1.5
    -0.5 2 2

滤波前

滤波后

可见x方向上，-2～0 该区间内的点都被过滤掉了

• 如果使用了pass.setFilterLimitsNegative (true);，则以上结果取反。