点云滤波的内容主要在apps/prefiltering_nodelet.cpp中,主要包括两步滤波,一是降采样,而是滤除离群点,我们分别介绍。

1. 降采样滤波

降采样滤波的代码如下

// select a downsample method (VOXELGRID, APPROX_VOXELGRID, NONE)
    std::string downsample_method = pnh.param<std::string>("downsample_method", "VOXELGRID");
    double downsample_resolution = pnh.param<double>("downsample_resolution", 0.1);
    if(downsample_method == "VOXELGRID") {
      std::cout << "downsample: VOXELGRID " << downsample_resolution << std::endl;
      boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(new pcl::VoxelGrid<PointT>());
      voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
      downsample_filter = voxelgrid;
    } else if(downsample_method == "APPROX_VOXELGRID") {
      std::cout << "downsample: APPROX_VOXELGRID " << downsample_resolution << std::endl;
      boost::shared_ptr<pcl::ApproximateVoxelGrid<PointT>> approx_voxelgrid(new pcl::ApproximateVoxelGrid<PointT>());
      approx_voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
      downsample_filter = approx_voxelgrid;
    } else {
      if(downsample_method != "NONE") {
        std::cerr << "warning: unknown downsampling type (" << downsample_method << ")" << std::endl;
        std::cerr << "       : use passthrough filter" <<std::endl;
      }
      std::cout << "downsample: NONE" << std::endl;
      boost::shared_ptr<pcl::PassThrough<PointT>> passthrough(new pcl::PassThrough<PointT>());
      downsample_filter = passthrough;
    }

这段代码的内容也比较清晰,它先从配置文件中读取参数downsample_method,根据参数决定选择哪种滤波方法。一共有三个选项:

1)VOXELGRID

对应pcl::VoxelGrid,它的原理是根据输入的点云,首先计算一个能够刚好包裹住该点云的立方体,然后根据设定的分辨率,将该大立方体分割成不同的小立方体。对于每一个小立方体内的点,计算他们的质心,并用该质心的坐标来近似该立方体内的若干点,从而达到对点云下采样的目的。

2)APPROX_VOXELGRID

对应pcl::ApproximateVoxelGrid,从名字也可以看出,该方法与VoxelGrid类似。唯一的不同在于这种方法是利用每一个小立方体的中心来近似该立方体内的若干点。相比于VoxelGrid,计算速度稍快,但也损失了原始点云局部形态的精细度。

3)NONE

就是不进行降采样

最后要注意一点,就是在选择完方法并初始化之后,都赋给了downsample_filter这个变量,它的定义也在这个cpp文件中

pcl::Filter<PointT>::Ptr downsample_filter;

在pcl中,pcl::Filter是pcl::VoxelGrid和pcl::ApproximateVoxelGrid的基类,所以这种赋值是合理的,这涉及到c++中继承和多态的知识,如果不太清楚的可以去网上查详细资料。

降采样滤波的代码在apps/scan_matching_odometry_nodelet.cpp中也有,内容完全一样,是给前端里程计使用的,不做解释了。

2. 滤除离群点

直接看代码吧

std::string outlier_removal_method = private_nh.param<std::string>("outlier_removal_method", "STATISTICAL");
    if(outlier_removal_method == "STATISTICAL") {
      int mean_k = private_nh.param<int>("statistical_mean_k", 20);
      double stddev_mul_thresh = private_nh.param<double>("statistical_stddev", 1.0);
      std::cout << "outlier_removal: STATISTICAL " << mean_k << " - " << stddev_mul_thresh << std::endl;

      pcl::StatisticalOutlierRemoval<PointT>::Ptr sor(new pcl::StatisticalOutlierRemoval<PointT>());
      sor->setMeanK(mean_k);
      sor->setStddevMulThresh(stddev_mul_thresh);
      outlier_removal_filter = sor;
    } else if(outlier_removal_method == "RADIUS") {
      double radius = private_nh.param<double>("radius_radius", 0.8);
      int min_neighbors = private_nh.param<int>("radius_min_neighbors", 2);
      std::cout << "outlier_removal: RADIUS " << radius << " - " << min_neighbors << std::endl;

      pcl::RadiusOutlierRemoval<PointT>::Ptr rad(new pcl::RadiusOutlierRemoval<PointT>());
      rad->setRadiusSearch(radius);
      rad->setMinNeighborsInRadius(min_neighbors);
    } else {
      std::cout << "outlier_removal: NONE" << std::endl;
    }

流程和降采样是一样的,都是根据配置文件选择,我们就直接看它有哪些选项吧

1)StatisticalOutlierRemoval

它的原理是对每个点的邻域进行一个统计分析,并修剪掉那些不符合一定标准的点。我们的稀疏离群点移除方法基于在输入数据中对点到临近点的距离分布的计算。对每个点,我们计算它到它的所有临近点的平均距离。假设得到的结果是一个高斯分布,其形状由均值和标准差决定,平均距离在标准范围(由全局距离平均值和方差定义)之外的点,可被定义为离群点并可从数据集中去除掉。

2)RadiusOutlierRemoval

它的原理是在点云数据中,用户指定每个的点一定范围内周围至少要有足够多的近邻。例如,如果指定至少要有1个邻居,只有黄色的点会被删除,如果指定至少要有2个邻居,黄色和绿色的点都将被删除。