雨雪天气下的点云滤波-几种常见的滤波算法实现及效果对比

一、背景

针对常见的雨雪天气，分别采用ApproximateVoxelGrid、StatisticalOutlierRemoval、RadiusOutlierRemoval方法对原始lidar点云数据进行滤波，并做效果对比。

二、代码实现

代码如下：

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/median_filter.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/radius_outlier_removal.h>

ros::Publisher median_pub;
ros::Publisher gaussian_pub;
ros::Publisher statistical_pub;
ros::Publisher ror_pub;

void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& input_cloud)
{
    // Convert PointCloud2 to PCL point cloud
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*input_cloud, *pcl_cloud);

    pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
    voxel_filter.setLeafSize(0.1, 0.1, 0.1);
    voxel_filter.setInputCloud(pcl_cloud);
    voxel_filter.filter(*pcl_cloud);

    // Apply median filtering
    // pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud_median(new pcl::PointCloud<pcl::PointXYZ>);
    // pcl::MedianFilter<pcl::PointXYZ> median_filter;
    // median_filter.setInputCloud(pcl_cloud);
    // median_filter.filter(*filtered_cloud_median);
    // // Convert back to PointCloud2
    // sensor_msgs::PointCloud2 output_median;
    // pcl::toROSMsg(*filtered_cloud_median, output_median);
    // output_median.header = input_cloud->header;
    // median_pub.publish(output_median);


    // Apply Gaussian filtering
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud_gaussian(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ApproximateVoxelGrid<pcl::PointXYZ> voxel_grid;
    voxel_grid.setInputCloud(pcl_cloud);
    voxel_grid.filter(*filtered_cloud_gaussian);
    // Convert back to PointCloud2
    sensor_msgs::PointCloud2 output_gaussian;
    pcl::toROSMsg(*filtered_cloud_gaussian, output_gaussian);
    output_gaussian.header = input_cloud->header;
    gaussian_pub.publish(output_gaussian);


    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud(pcl_cloud);
    // 设置邻域点的数量
    sor.setMeanK(100); 
    // 设置标准差倍数阈值
    sor.setStddevMulThresh(0.01); 
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud_statistical(new pcl::PointCloud<pcl::PointXYZ>);
    sor.filter(*filtered_cloud_statistical);
    sensor_msgs::PointCloud2 output_statistical;
    pcl::toROSMsg(*filtered_cloud_statistical, output_statistical);
    output_statistical.header = input_cloud->header;
    statistical_pub.publish(output_statistical);


    pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror;
    ror.setRadiusSearch(0.2); 
    ror.setMinNeighborsInRadius(4); 
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud_ror(new pcl::PointCloud<pcl::PointXYZ>);
    ror.setInputCloud(pcl_cloud);
    ror.filter(*filtered_cloud_ror);
    sensor_msgs::PointCloud2 output_ror;
    pcl::toROSMsg(*filtered_cloud_ror, output_ror);
    output_ror.header = input_cloud->header;
    ror_pub.publish(output_ror);

}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "point_cloud_filter");
    ros::NodeHandle nh;

    ros::Subscriber cloud_sub = nh.subscribe<sensor_msgs::PointCloud2>("/hesai/pandar", 1, cloudCallback);

    median_pub = nh.advertise<sensor_msgs::PointCloud2>("/median", 1);
    gaussian_pub = nh.advertise<sensor_msgs::PointCloud2>("/gaussian", 1);
    statistical_pub = nh.advertise<sensor_msgs::PointCloud2>("/statistical", 1);
    ror_pub = nh.advertise<sensor_msgs::PointCloud2>("/ror", 1);

    ros::spin();

    return 0;
}

CMakeLists.txt比较简单，大家自行编写，有需要留言。

三、效果对比

可以看到针对雨雪天气点云噪点情况，ror的滤波效果最好。