[自动驾驶-目标检测] C++ PCL 点云噪声滤波

文章目录

1、引言
2、点云噪声过滤原理
3、总结
4、参考文献

1、引言

3D点云的噪声滤波在激光雷达领域往往是最常见也同样是最容易忽略的地方，在实际应用场景下往往很容易产生噪声点，比如点云灰尘、雨水、雪雾等等。而常见的3D点云去噪方法往往无法根除，因此需要结合场景及激光雷达的特点来设计不同的噪声滤波器，从而满足实际需求，废话不多说，本文主要采用PCL来实现噪声过滤。

2、点云噪声过滤原理

首先向各位引荐一片关于点云去噪的论文:《Design of Dust-Filtering Algorithms for LiDAR Sensors Using Intensity and Range Information in Off-Road Vehicles》，该论文简单分析了各个噪声过滤方式的特点。
注意：点云PCL内已经封装好的滤波器就不再赘述了。

2.1、 $\ DROR$ 滤波器

首先是 $\ DROR$ 滤波器，该滤波器本质上依然是由半径滤波构成，所以原理就不赘述了，其中的特点就是将半径滤波器中的半径参数动态化，通过计算点到原点的距离来约束搜索半径的参数，而其 $\ α、Ф$ 系数可以通过激光雷达的分辨率以及手动设计来确定。
在这里插入图片描述
基本流程就不多介绍了，相信很容易看懂，直接上代码了、

/**
 * @brief   DROR_filter
 * @param input_point_cloud    Enter the point cloud to filter
 * @param output_point_cloud    Output filtered point cloud
 * @param raise_dust_point_cloud Output noise point and dust point cloud
 * @param min_search_radius Minimum search radius
 * @param point_num_threshold   Minimum number of adjacent points
 * @param search_factor   Coefficient factor of search radius
 */
void NoiseFilter::DROR_filter(const PointCloudPtr& input_point_cloud,
                                                            PointCloudPtr& output_point_cloud,
                                                            PointCloudPtr& raise_dust_point_cloud,
                                                            const double & min_search_radius,
                                                            const int & point_num_threshold,
                                                            const double & search_factor){
    if (input_point_cloud->empty()) return;
    /*Create Kdtree Searcher*/
    pcl::KdTreeFLANN<Point>::Ptr kd_tree(new pcl::KdTreeFLANN<Point>());
    kd_tree->setInputCloud(input_point_cloud);
    std::vector<int> noise_cloud_indices(input_point_cloud->size());
    std::vector<int> filter_cloud_indices(input_point_cloud->size());
    
#pragma omp parallel for num_threads(4)
    for (size_t i = 0; i <  input_point_cloud->points.size(); ++i){
        Point point = input_point_cloud->points[i];
        double search_radius_dynamic = std::sqrt(std::pow(point.x, 2) + std::pow(point.y, 2));
        if (search_radius_dynamic < min_search_radius) search_radius_dynamic = min_search_radius;
        else  search_radius_dynamic *= search_factor ;
        /*Radius Search Around Points*/
        std::vector<int> point_idx_radius_search;
        std::vector<float> point_radius_squared_distance;
        int point_neighbors = kd_tree->radiusSearch(point, 
                                                                                                      search_radius_dynamic, 
                                                                                                      point_idx_radius_search, 
                                                                                                      point_radius_squared_distance);

        if (point_neighbors < point_num_threshold) 
            noise_cloud_indices[i] = i;
        else filter_cloud_indices[i] = i;
    }
    noise_cloud_indices.erase(remove(noise_cloud_indices.begin(),
                                                                              noise_cloud_indices.end(),0),
                                                                              noise_cloud_indices.end());
    filter_cloud_indices.erase(remove(filter_cloud_indices.begin(),
                                                                            filter_cloud_indices.end(),0),
                                                                            filter_cloud_indices.end());
    pcl::copyPointCloud(*input_point_cloud, noise_cloud_indices,*raise_dust_point_cloud);
    pcl::copyPointCloud(*input_point_cloud, filter_cloud_indices,*output_point_cloud);
    return;
}

2.2、 $\ LIOR$ 滤波器

上述的 $\ DROR$ 滤波器是通过利用点云的几何信息来进行滤波处理的，在实际状态下，灰尘、雨水、雪雾等点云的反光强度往往都会低于其他场景的反光强度，因此有根据反光强度来设计滤波器的，也就是Low Intensity Outlier Removal。

在这里插入图片描述
这个滤波器的设计流程也同样容易理解：

首先是遍历整个点云数据，首先通过判定该点的强度值来进行分类。
然后将不满足条件的点进行半径搜索来完成过滤，最后实现二分类。

该滤波器适用于带有轻度灰尘、雨水等场景。

/**
 * @brief   LIOR_filter
 * @param input_point_cloud    Enter the point cloud to filter
 * @param output_point_cloud    Output filtered point cloud
 * @param raise_dust_point_cloud Output noise point and dust point cloud
 * @param search_radius  search radius
 * @param point_num_threshold   Minimum number of adjacent points
 * @param threshold_intensity Intensity threshold of point cloud
 */
void NoiseFilter::LIOR_filter(const PointCloudPtr& input_point_cloud,
                                                           PointCloudPtr& output_point_cloud,
                                                           PointCloudPtr& raise_dust_point_cloud,
                                                           const double & search_radius,
                                                           const int & point_num_threshold,
                                                           const int & threshold_intensity){
    if (input_point_cloud->empty()) return;
    /*Create Kdtree Searcher*/
    pcl::KdTreeFLANN<Point>::Ptr kd_tree(new pcl::KdTreeFLANN<Point>());
    kd_tree->setInputCloud(input_point_cloud);
    std::vector<int> noise_cloud_indices(input_point_cloud->size());
    std::vector<int> filter_cloud_indices(input_point_cloud->size());

#pragma omp parallel for num_threads(4)
    for (size_t i = 0; i <  input_point_cloud->points.size(); ++i){
        Point point = input_point_cloud->points[i];
        if (point.intensity >threshold_intensity){
            filter_cloud_indices[i] = i;
            continue;
        }
        /*Radius Search Around Points*/
        std::vector<int> point_idx_radius_search;
        std::vector<float> point_radius_squared_distance;
        int point_neighbors = kd_tree->radiusSearch(point, search_radius, 
                                                                                                      point_idx_radius_search, 
                                                                                                      point_radius_squared_distance);
        if (point_neighbors < point_num_threshold) noise_cloud_indices[i] = i;
        else filter_cloud_indices[i] = i;
    }
    noise_cloud_indices.erase(remove(noise_cloud_indices.begin(),
                                                                              noise_cloud_indices.end(),0),
                                                                              noise_cloud_indices.end());
    filter_cloud_indices.erase(remove(filter_cloud_indices.begin(),
                                                                            filter_cloud_indices.end(),0),
                                                                            filter_cloud_indices.end());
    pcl::copyPointCloud(*input_point_cloud, noise_cloud_indices,*raise_dust_point_cloud);
    pcl::copyPointCloud(*input_point_cloud, filter_cloud_indices,*output_point_cloud);
    return;
}

2.3、 $\ LIDROR$ 滤波器

这个滤波器是 $\ LIOR$ 滤波器的升级版，也就是加入动态半径参数的设定，从而提高搜索效率。该滤波器同样仅适用于轻度灰尘等场景。

在这里插入图片描述


/**
 * @brief   LIOR_filter Low-Intensity Dynamic Radius Outlier Removal
 * @param input_point_cloud    Enter the point cloud to filter
 * @param output_point_cloud    Output filtered point cloud
 * @param raise_dust_point_cloud Output noise point and dust point cloud
 * @param min_search_radius Minimum search radius
 * @param search_factor   Coefficient factor of search radius
 * @param point_num_threshold   Minimum number of adjacent points
 * @param threshold_intensity Intensity threshold of point cloud
 */
void NoiseFilter::LIDROR_filter(const PointCloudPtr& input_point_cloud,
                                                                PointCloudPtr& output_point_cloud,
                                                                PointCloudPtr& raise_dust_point_cloud,
                                                                const double & min_search_radius,
                                                                const double & search_factor,
                                                                const int & point_num_threshold,
                                                                const int & threshold_intensity){
    
    if (input_point_cloud->empty()) return;
    /*Create Kdtree Searcher*/
    pcl::KdTreeFLANN<Point>::Ptr kd_tree(new pcl::KdTreeFLANN<Point>());
    kd_tree->setInputCloud(input_point_cloud);
    std::vector<int> noise_cloud_indices(input_point_cloud->size());
    std::vector<int> filter_cloud_indices(input_point_cloud->size());
    
#pragma omp parallel for num_threads(4)
    for (size_t i = 0; i <  input_point_cloud->points.size(); ++i){
        Point point = input_point_cloud->points[i];
        if (point.intensity > threshold_intensity){
            filter_cloud_indices[i] = i;
            continue;
        }
        /*Set Dynamic Search Radius*/
        double search_radius_dynamic = std::sqrt(std::pow(point.x, 2) + std::pow(point.y, 2));
        if (search_radius_dynamic < min_search_radius) search_radius_dynamic = min_search_radius;
        else  search_radius_dynamic *= search_factor ;
        /*Radius Search Around Points*/
        std::vector<int> point_idx_radius_search;
        std::vector<float> point_radius_squared_distance;
        int point_neighbors = kd_tree->radiusSearch(point, search_radius_dynamic, 
                                                                                                      point_idx_radius_search, 
                                                                                                      point_radius_squared_distance);
        if (point_neighbors < point_num_threshold) noise_cloud_indices[i] = i;
        else filter_cloud_indices[i] = i;
    }
    noise_cloud_indices.erase(remove(noise_cloud_indices.begin(),
                                                                              noise_cloud_indices.end(),0),
                                                                              noise_cloud_indices.end());
    filter_cloud_indices.erase(remove(filter_cloud_indices.begin(),
                                                                            filter_cloud_indices.end(),0),
                                                                            filter_cloud_indices.end());
    pcl::copyPointCloud(*input_point_cloud, noise_cloud_indices,*raise_dust_point_cloud);
    pcl::copyPointCloud(*input_point_cloud, filter_cloud_indices,*output_point_cloud);
    return;
}

2.4、 $\ LIOLS$ 滤波器

上述滤波方法都是通过遍历整个点云并通过半径搜索来进行过滤，这种方式在处理大型数据时，计算量非常大，因此我们可以结合局部分块的方法来设计新的滤波器，也就是借助PCL内的八叉树Octree的数据结构来提高过滤的效率。设计出Low Intensity Filtering Based on Octree Local Statistics过滤方式。

整个设计思路如下：

首先建立八叉树结构的数据结构。
通过遍历八叉树的叶结构来统计八叉树内部满足条件的点云。
将满足条件的八叉树点云进行分类提取。

判定的条件可以参考 $\ LIOR$ 滤波器。其中统计的方式可以简单设定为： $scale_{low Intensity} = num_{low Intensity} / num_{octree}$

那么可以很容易设计出该算法代码：

/**
 - @brief  LIOLS_filter Low Intensity Filtering Based on Octree Local Statistics
 - @param input_point_cloud    Enter the point cloud to filter
 - @param output_point_cloud    Output filtered point cloud
 - @param raise_dust_point_cloud Output noise point and dust point cloud
 - @param octree_size Octree size
 - @param min_low_Intensity_scale   Statistical coefficient of low intensity information
 - @param point_num_threshold   Minimum number of adjacent points
 - @param threshold_intensity Intensity threshold of point cloud
 */
void NoiseFilter::LIOLS_filter(const PointCloudPtr& input_point_cloud,
                                                           PointCloudPtr& output_point_cloud,
                                                           PointCloudPtr& raise_dust_point_cloud,
                                                           const double & octree_size,
                                                           const double & min_low_Intensity_scale,
                                                           const int & point_num_threshold,
                                                           const int & threshold_intensity){
    if (input_point_cloud->empty()) return;
    /*Grid blocking*/
    pcl::octree::OctreePointCloud<Point> octree(octree_size);
    octree.setInputCloud(input_point_cloud);
    octree.addPointsFromInputCloud(); // Constructing octree

    pcl::PointIndices::Ptr outliners(new pcl::PointIndices());
	for (auto iter = octree.leaf_begin(); iter != octree.leaf_end(); ++iter){
        std::vector<int> vec_point_index = iter.getLeafContainer().getPointIndicesVector();
        /*Noise point*/
        if(vec_point_index.size() < point_num_threshold){
            outliners->indices.insert( outliners->indices.end(), vec_point_index.begin(), vec_point_index.end());
            continue;
        }
        /*Statistical octree internal strength*/
        int low_intensity_point_num = 0;
        for (size_t i = 0; i < vec_point_index.size(); ++i){
            if (input_point_cloud->points[vec_point_index[i]].intensity <= threshold_intensity){
                low_intensity_point_num++;
            }
        }
        double low_Intensity_scale = low_intensity_point_num / vec_point_index.size();
        if (low_Intensity_scale > min_low_Intensity_scale){
            outliners->indices.insert( outliners->indices.end(), vec_point_index.begin(), vec_point_index.end());
            continue;
        }
    }
	pcl::ExtractIndices<Point> extract;
	extract.setInputCloud(input_point_cloud);
	extract.setIndices(outliners);
	extract.setNegative(false);
	extract.filter(*raise_dust_point_cloud);
  	extract.setNegative(true);  
 	extract.filter(*output_point_cloud);   
    return;
}

2.5、 $\ OLIDROR$ 滤波器

可以参考 $\ LIDROR$ 滤波器的设计思路，那么可以不可以将 $\ LIOLS$ 滤波器与 $\ DROR$ 滤波器结合起来呢，答案当然显而易见。那么怎么设计出合理的基于八叉树下的动态滤波器，其中重点需要考虑判定条件。由于是自己的一些设计想法，大家可以根据自己的场景来参考一下。本文的设计思路如下:

首先建立八叉树结构的点云数据，并通过八叉树中心下采样的方式提取整个八叉树点云，并统计八叉树内部点云的判定条件一进行分类。其中判定条件参考 $\ LIOR$ 滤波器，设定： $points.intensity <= Threshold_{intensity}$
统计方式则还是 $\ LIOLS$ 滤波器的数量比例来统计。
然后将八叉树的中心点云建立搜索器，通过半径搜索方式来实现二分类，剔除最终的噪声点云。二分类的判定条件则是根据邻近点的统计分类特征来实现。其中判定条件为：如果邻近八叉树的噪声比例值大于设定值则将该八叉树区域视作是噪声区域。
最后筛选出噪声点云以及八叉树内部的区域点云，实现整个输入点云的二分类。

与 $\ LIOLS$ 滤波器不相同的是 $Threshold_{intensity}$ 并不是人为设定的固定值，而是通过计算设定的动态阈值。其中计算的方式为：

$\ Th_{intensity} = max_{i} -\tfrac{(max_{i}-min_{i})}{range_{max}} \times\sqrt{Pt_{x}^2 + Pt_{y}^2}$

其中 $max_{i} 、min_{i}$ 分别标定人为设定的噪声点云最大反光强度与最小反光强度。 $distance_{max}$ 则表示为设定的反光强度的最大过滤范围。

 struct OLIDRORParam
 {
     int min_point_cloud_num = 5;
     double octree_depth = 0.5;
     double min_low_Intensity_scale = 0.3;
     double min_search_radius = 0.1;
     double search_factor = 0.014;
 }olidror_param_; 

 enum OctreeState { free_space, hight_intensity, low_intensity };  
 
/**
 * @brief  OLIDROR_filter Low-Intensity Dynamic Radius Outlier Removal base on Octree
 * @param input_point_cloud    Enter the point cloud to filter
 * @param output_point_cloud    Output filtered point cloud
 * @param raise_dust_point_cloud Output noise point and dust point cloud
 * @param filter_param Corresponding filtering OLIDROR parameters
 */
float NoiseFilter::GetIntensityThreshold(const Point& point,
                                                                                    const int & max_intensity_threshold,
                                                                                    const int & min_intensity_threshold,
                                                                                    const int & high_intensity_filter_range) {
    double distance = std::sqrt(std::pow(point.x, 2) + std::pow(point.y, 2));
    int high_intensity_filter_threshold = min_intensity_threshold;
    if (distance <= high_intensity_filter_range) {
        high_intensity_filter_threshold = (max_intensity_threshold - min_intensity_threshold) 
                * - 1.0  / high_intensity_filter_range * distance + max_intensity_threshold;
    } 
    return high_intensity_filter_threshold;
}

void NoiseFilter::OLIDROR_filter(const PointCloudPtr& input_point_cloud,
                                                                    PointCloudPtr& output_point_cloud,
                                                                    PointCloudPtr& raise_dust_point_cloud,
                                                                    const OLIDRORParam & filter_param){
    if (input_point_cloud->empty()) return;
    /*Octree blocking processing*/
    AlignedPointTVector octree_center_list_arg;
    pcl::octree::OctreePointCloudSearch<Point> octree(filter_param.octree_depth);
    octree.setInputCloud(input_point_cloud);
    octree.addPointsFromInputCloud();
	octree.getOccupiedVoxelCenters(octree_center_list_arg); 
	//std::cerr << "the number of voxel are : " << voxel_center_list_arg.size() << std::endl;
    PointCloudPtr octree_center_point_cloud(new PointCloud);
    octree_center_point_cloud->resize(octree_center_list_arg.size());
    std::vector<std::vector<int>> octree_center_idx_vec;
    octree_center_idx_vec.resize(octree_center_list_arg.size());

    #pragma omp parallel for num_threads(4)
    for (size_t i = 0; i < octree_center_list_arg.size(); ++i){
        Point octree_center_point;
        octree_center_point.x = octree_center_list_arg[i].x;
        octree_center_point.y = octree_center_list_arg[i].y;
        octree_center_point.z = octree_center_list_arg[i].z;
        octree_center_point.intensity = OctreeState::hight_intensity;
        /*Extraction of point cloud features inside octree*/
        std::vector<int> voxel_point_idx_vec;
        if (octree.voxelSearch(octree_center_point, voxel_point_idx_vec)){
            if (voxel_point_idx_vec.size() <= 5){
                octree_center_point.intensity = OctreeState::free_space;
                continue;
            }else{
                /*Statistical intensity information*/
                int low_intensity_num = 0;
                double voxel_point_intensity_threshold = GetIntensityThreshold(octree_center_point);
                if (octree_center_list_arg[i].intensity <= voxel_point_intensity_threshold)
                     low_intensity_num++;
                for (size_t j = 0; j < voxel_point_idx_vec.size(); ++j){
                    Point octree_voxel_point = input_point_cloud->points[voxel_point_idx_vec[j]];
                    if (octree_voxel_point.intensity <= voxel_point_intensity_threshold){
                        low_intensity_num++;
                    }
                }
                double low_Intensity_scale = (low_intensity_num *1.0) / (voxel_point_idx_vec.size() + 1);
                if ( low_Intensity_scale > filter_param.min_low_Intensity_scale) {
                    octree_center_point.intensity = OctreeState::low_intensity;
                }
            }
        }else{
            octree_center_point.intensity = OctreeState::free_space;
        }
        octree_center_idx_vec[i] = voxel_point_idx_vec;
        octree_center_point_cloud->points[i] = octree_center_point;
    }

    /*Create Kdtree Searcher*/
    pcl::KdTreeFLANN<Point>::Ptr kd_tree(new pcl::KdTreeFLANN<Point>());
    kd_tree->setInputCloud(octree_center_point_cloud);

    std::vector<int> noise_cloud_indices(octree_center_point_cloud->size(), -1);
#pragma omp parallel for num_threads(4)
    for (size_t i = 0; i <  octree_center_point_cloud->points.size(); ++i){
        Point point = octree_center_point_cloud->points[i];
        if (point.intensity == OctreeState::free_space){
            noise_cloud_indices[i] = i;
            continue;
        }
        /*Set Dynamic Search Radius*/
        double search_radius_dynamic = std::sqrt(std::pow(point.x, 2) + std::pow(point.y, 2)) *
                                                                              filter_param.search_factor;
        double min_search_radius = filter_param.min_search_radius;
        if (min_search_radius < filter_param.octree_depth)
            min_search_radius *=  2* std::sqrt(std::pow(filter_param.octree_depth, 2) * 2)  + 0.1;
        if (search_radius_dynamic < min_search_radius) 
            search_radius_dynamic = min_search_radius;
        /*Radius Search Around Points*/
        std::vector<int> point_idx_radius_search;
        std::vector<float> point_radius_squared_distance;
        int point_neighbors = kd_tree->radiusSearch(point, search_radius_dynamic, 
                                                                                                      point_idx_radius_search, 
                                                                                                      point_radius_squared_distance);
                                                                                                      
        if (point_neighbors < 2 && point.intensity != OctreeState::hight_intensity){
             noise_cloud_indices[i] = i;
             continue;
        }
        int low_intensity_region = 0;
        if (point.intensity == OctreeState::low_intensity) low_intensity_region++;
        for (size_t j = 0; j < point_neighbors; ++j){
            if (octree_center_point_cloud->points[point_idx_radius_search[j]].intensity != 
                 OctreeState::hight_intensity){
                low_intensity_region++;
            }
        }
        double low_intensity_region_scale =  (low_intensity_region * 1.0)   / (point_neighbors + 1);
        if((low_intensity_region_scale > ( filter_param.min_low_Intensity_scale - 0.3) && 
              point.intensity == OctreeState::low_intensity) || 
             (low_intensity_region_scale > (filter_param.min_low_Intensity_scale) &&
             point.intensity == OctreeState::hight_intensity)){
                noise_cloud_indices[i] = i;
        }
    }
	noise_cloud_indices.erase(remove(noise_cloud_indices.begin(),
                                                                              noise_cloud_indices.end(),-1),
                                                                              noise_cloud_indices.end());
    /*classification*/
    pcl::PointIndices::Ptr noise_outliners(new pcl::PointIndices());
    for (size_t i = 0; i < noise_cloud_indices.size(); ++i){
        noise_outliners->indices.insert(noise_outliners->indices.end(), 
                                                                    octree_center_idx_vec[noise_cloud_indices[i]].begin(), 
                                                                    octree_center_idx_vec[noise_cloud_indices[i]].end());
    }
    // std::cout<<noise_outliners->indices.size()<<std::endl;
    pcl::ExtractIndices<Point> extract;
	extract.setInputCloud(input_point_cloud);
	extract.setIndices(noise_outliners);
	extract.setNegative(false);
	extract.filter(*raise_dust_point_cloud);
  	extract.setNegative(true);  
 	extract.filter(*output_point_cloud);  
    return;
}

3、总结

点云的滤波器设计并不是固定不变的，一定要结合实际滤波需求来设计不同的滤波方式以及滤波条件。
每个滤波器适用的场景也不一样，需要深度理解滤波的特点来搭配使用。
一般点云滤波器不支持直接对原始点云数据适用，容易造成计算量较大影响点云处理效率。正常都是放在特征提取之后，防止因为滤波器的原因导致特征提取不明显或者特征点较少。

4、参考文献

…