[代码实践] 自动驾驶中常见的激光雷达点云去噪处理

简介

在 [代码实践] 自动驾驶中常见的激光雷达点云滤波处理中整理了自动驾驶常用的几种降低点云规模的预处理方法，那些方法除了能够去除无关区域的点以及降低点云规模以外，或多或少也能起到点云去噪的作用。这篇博客尝试整理一下基于激光雷达常见的点云去噪方法。博客用到的代码在：noise_removal

基于点云分布密度进行去噪

第一个直观的感受是可以根据点云中每个点所在区域判断其是否是噪声，一般来说噪声点所在区域都比较稀疏。

PCL 自带滤波器

PCL 内部包含了一个 RadiusOutlierRemoval 可以进行此种方法滤波，使用时需要设定每个点的搜索半径，已经需要满足的最少邻近点数，使用方法如下：

    pcl::RadiusOutlierRemoval<pcl::PointXYZ>::Ptr radius_outlier_removal(
        new pcl::RadiusOutlierRemoval<pcl::PointXYZ>(true));
    radius_outlier_removal->setInputCloud(cloud_ptr);
    radius_outlier_removal->setRadiusSearch(1.0);
    radius_outlier_removal->setMinNeighborsInRadius(10);
    radius_outlier_removal->filter(*filtered_cloud_ptr);

自己实现版本

函数比较简单，自己实现起来也比较方便，只需要构建一个 KD 树，对每个点进行范围搜索（最近邻搜索），最后判断邻近点数（最大距离）是否满足要求即可，如下所示：

template <typename PointType>
void radius_outlier_removal(boost::shared_ptr<pcl::PointCloud<PointType>>& src_cloud_ptr,
                            boost::shared_ptr<pcl::PointCloud<PointType>>& dst_cloud_ptr,
                            double search_radius,
                            int min_neighbors,
                            boost::shared_ptr<std::vector<int>> removed_indices = nullptr) {
    boost::shared_ptr<pcl::KdTreeFLANN<PointType>> kdtree(new pcl::KdTreeFLANN<PointType>());
    kdtree->setInputCloud(src_cloud_ptr);

    boost::shared_ptr<pcl::PointCloud<PointType>> cloud_ptr(new pcl::PointCloud<PointType>());

    cloud_ptr->points.reserve(src_cloud_ptr->points.size());
    if (removed_indices) removed_indices->reserve(src_cloud_ptr->points.size());

    std::vector<int> neighbors(min_neighbors);
    std::vector<float> distances(min_neighbors);
    for (size_t i = 0; i < src_cloud_ptr->points.size(); ++i) {
        if (kdtree->nearestKSearch(i, min_neighbors, neighbors, distances) > 0 &&
            distances.back() * distances.back() < search_radius) {
            cloud_ptr->points.push_back(src_cloud_ptr->points[i]);
        } else {
            if (removed_indices) removed_indices->push_back(i);
        }
    }
}

这里我用的是 PCL 的 KD 树实现，但是它的范围搜索速度比最近邻搜索速度慢很多，目前没有看实现细节所以不太清楚原因，因此这里使用最近邻搜索来进行搜索。在使用时可以考虑使用其他的 KD 树实现。

效果

两种方法的效果差不多，如下所示。红色的点表示被去除的点，可以看到，除了一些很明显的噪声点被去除以外，由于激光雷达点云的特性，较远处分布比较稀疏的点也被去除了。

基于点云所在区域分布规律进行去噪

除了可以根据点云中每个点所在区域是否足够稠密的情况下，还可以根据该点与其他周围点能否形成一个较好的分布来作为筛选标准。具体来说，针对每个点计算其到周围若干点的平均距离，如果这个平均距离相对于整体点云中所有点的平均距离较近，则认为其不是噪点。

PCL 自带滤波器及源码

同样，PCL 提供了相关滤波器的实现，首先来看一下使用方法：

    // PCL built-in radius removal
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ>::Ptr statistical_outlier_removal(
        new pcl::StatisticalOutlierRemoval<pcl::PointXYZ>(true)); // set to true if we want to extract removed indices
    statistical_outlier_removal->setInputCloud(cloud_ptr);
    statistical_outlier_removal->setMeanK(20);
    statistical_outlier_removal->setStddevMulThresh(2.0);
    statistical_outlier_removal->filter(*filtered_cloud_ptr);

由于实现过程和 RadiusOutlierRemoval 大同小异，都是对每个点搜索 k 个最近点，只是在判断标准有些许不一样，所以这次没有自己实现，而是直接来看一下 PCL 是怎么进行距离判断的，以下为源码的核心部分：

template <typename PointT> void
pcl::StatisticalOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)
{
  // Initialize the search class -- 初始化需要用到的搜索树结构
  if (!searcher_)
  {
    if (input_->isOrganized ())
      searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
    else
      searcher_.reset (new pcl::search::KdTree<PointT> (false));
  }
  searcher_->setInputCloud (input_);

  // The arrays to be used -- 提前分配搜索结果需要的空间
  std::vector<int> nn_indices (mean_k_);
  std::vector<float> nn_dists (mean_k_);
  std::vector<float> distances (indices_->size ());
  indices.resize (indices_->size ());
  removed_indices_->resize (indices_->size ());
  int oii = 0, rii = 0;  // oii = output indices iterator, rii = removed indices iterator

  // First pass: Compute the mean distances for all points with respect to their k nearest neighbors
  // 一次遍历，对每个点搜索到附近若干个点的平均距离
  int valid_distances = 0;
  for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)  // iii = input indices iterator
  {
    if (!pcl_isfinite (input_->points[(*indices_)[iii]].x) ||
        !pcl_isfinite (input_->points[(*indices_)[iii]].y) ||
        !pcl_isfinite (input_->points[(*indices_)[iii]].z))
    {
      distances[iii] = 0.0;
      continue;
    }

    // Perform the nearest k search
    if (searcher_->nearestKSearch ((*indices_)[iii], mean_k_ + 1, nn_indices, nn_dists) == 0)
    {
      distances[iii] = 0.0;
      PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
      continue;
    }

    // Calculate the mean distance to its neighbors
    double dist_sum = 0.0;
    for (int k = 1; k < mean_k_ + 1; ++k)  // k = 0 is the query point
      dist_sum += sqrt (nn_dists[k]);
    distances[iii] = static_cast<float> (dist_sum / mean_k_);
    valid_distances++;
  }

  // Estimate the mean and the standard deviation of the distance vector
  // 计算所有点到其邻居的平均距离的均值、方差和标准差
  double sum = 0, sq_sum = 0;
  for (size_t i = 0; i < distances.size (); ++i)
  {
    sum += distances[i];
    sq_sum += distances[i] * distances[i];
  }
  double mean = sum / static_cast<double>(valid_distances);
  double variance = (sq_sum - sum * sum / static_cast<double>(valid_distances)) / (static_cast<double>(valid_distances) - 1);
  double stddev = sqrt (variance);
  //getMeanStd (distances, mean, stddev);

  // 根据用户指定比例计算距离阈值
  double distance_threshold = mean + std_mul_ * stddev;

  // Second pass: Classify the points on the computed distance threshold
  // 根据距离判断是否要筛除
  for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)  // iii = input indices iterator
  {
    // Points having a too high average distance are outliers and are passed to removed indices
    // Unless negative was set, then it's the opposite condition
    if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold))
    {
      if (extract_removed_indices_)
        (*removed_indices_)[rii++] = (*indices_)[iii];
      continue;
    }

    // Otherwise it was a normal point for output (inlier)
    indices[oii++] = (*indices_)[iii];
  }

  // Resize the output arrays
  indices.resize (oii);
  removed_indices_->resize (rii);
}

效果分析

对激光雷达的点云使用效果如下所示。可以看到除了筛除到一部分噪点以外，对较远的激光线也筛除掉了。我们可以来分析一下背后的原因。首先对激光雷达点云数据中的每个点而言，由于每个激光线束本身扫描频率是一致的，显然越远的扫描线点会越稀疏，因此其平均距离会越远（即便其本身可能不一定是噪点）。因此更合理的方法可能是对每个点周围小范围内（同一条线内）的点进行高斯拟合，然后判断该点是否满足该高斯分布，但是如果需要得到比较准确的高斯分布参数，点数需要达到一定数量，否则和上述基于点云密度进行去噪差别不大。

激光雷达点云特点

机械式的激光雷达产生的一帧点云数据，通常由多个激光束扫描一圈得到，某些激光雷达也可以由一条激光束进行多圈扫描得到一帧点云。为了方便对点云按线进行区分，我们需要先了解得到点云的分布顺序。想要知道点云的排列顺序，最直观的数据来源的手册，例如 KITTI 数据集的就去官网查询他们怎么记录数据的。如果来源不清楚的话，也可以简单地对点云按索引进行着色，来进行简单判断，这里我将点云分成 3 部分，每部分用颜色深浅循环表示先后顺序，如下所示。观察可以发现，这一帧数据中点的排列顺序为从最高的线束到最低的线束进行排列，每条线束之间点按逆时针的顺序排列。

某些激光雷达点云的点云可以自带线束信息（如 Ouster），某些数据则不带这个信息，此时我们需要根据点云中每个点的高度角以及该激光雷达的垂直视域和分辨率来进行分线。KITTI 中使用的激光雷达为 HDL-64E，垂直视域为 +2 ～ -24.9 度，分辨率为 0.4 度。在知道扫描数据是按线的顺序来进行存储时，按扫描线分组有两种思路：

通过高度角结合分辨率进行分线，这样做的好处是简单，但是由于 64 线激光雷达的分辨率较小，在一些扫描线较密的地方时容易计算不准确
按水平角进行分组，大体思路是以水平角经过 180 度作为标志来判断是否完成当前线的扫描，具体可以参考这篇文章：kitti 64 线数据抽取其中 32 线，感觉上用水平角转折点比使用高度角计算扫描线可能会准确一点，毕竟相对于通过分辨率计算扫描线，判断是否转过一圈计算起来数值区分比较明显。然而，这是建立在每条线都很完美的转过一圈的情况下才能成立，事实上，根据传感器本身以及环境的区别，有可能扫描线转不到一圈，或者稍微转多了一圈，此时计算起来就比较复杂，需要判断多种情况。

这里我们只是想进行去噪，本质上希望找到每个点在水平方向上相邻的若干个点即可，因此这里采用高度角来计算扫描线 ID，计算的方法偷个懒直接从 A-LOAM 拿来，如果想知道具体的线分布情况可以查官网手册。注：目前大部分主流激光雷达应该都可以直接在点云中提供每个点对应的扫描线已经时间戳，所以其实可以不用这种粗略的方法来进行计算。

/***
 * @brief get start index for each line from kitti pointcloud data
 */
template <typename PointType>
void sortPointCloudByScanLine(boost::shared_ptr<pcl::PointCloud<PointType>>& src_cloud_ptr,
                              std::vector<boost::shared_ptr<pcl::PointCloud<PointType>>>& dst_cloud_ptr_vec) {
    const int N_SCAN = 64;
    dst_cloud_ptr_vec.resize(N_SCAN);
    dst_cloud_ptr_vec.clear();

    PointType pt;
    int line_id;

    for (int i = 0; i < src_cloud_ptr->points.size(); ++i) {
        pt      = src_cloud_ptr->points[i];
        line_id = 0;

        double elevation_angle = std::atan2(pt.z, std::sqrt(pt.x * pt.x + pt.y * pt.y)) / M_PI * 180.0;

        // adapt from a-loam
        if (elevation_angle >= -8.83)
            line_id = int((2 - elevation_angle) * 3.0 + 0.5);
        else
            line_id = 64 / 2 + int((-8.83 - elevation_angle) * 2.0 + 0.5);

        if (elevation_angle > 2 || elevation_angle < -24.33 || line_id > 63 || line_id < 0) {
            continue;
        }

        if (dst_cloud_ptr_vec[line_id] == nullptr)
            dst_cloud_ptr_vec[line_id] = boost::make_shared<pcl::PointCloud<PointType>>();
        dst_cloud_ptr_vec[line_id]->points.push_back(pt);
    }
}

将点云分线后，给不同线的点云着色查看如下：

可以看到大部分的线应该划分没问题，有一小部分有问题的考虑可能是由于点云畸变的原因，这里暂时不不管。关于点云去畸变可以根据点云中点的水平角和两帧之间的相对运动计算进行，参考：激光雷达测量模型及运动畸变去除

结合扫描线信息对点云进行去噪

在将点云按照扫描线分组后，同样的我们可以将应用在整个点云的去噪算法应用在每条线上。

基于线密度进行去噪

将上述提到的按照点云分布密度进行去噪，函数不变，但作用在每条线上的局部点云上，最后一起显示，效果如下，和之前对整个点云的去噪结果比较，可以看到结果有两个特点：

对小特征进行了筛选，在一些比较细的物体上，同一条线的扫到的点不多，这些点会被去除，而对于整个点云，如果特征比较高，有多条线都扫掉，则不一定会被去除
较远的扫描线由于点之间相隔本身较远，因此去除的更多

这只是同一种参数的比较，事实上，如果我们对每条线大致的范围有了解，可以对每条线调整参数来获得想要的结果

基于扫描线的统计信息进行去噪

同样，可以针对每条线根据点云分布去噪，效果如下所示，通过和对整个点云的去噪效果对比：

对较远的扫描线也保存的比较完整
同样，某些较远的扫描线也扫到近处的墙壁（细杆）时，该处密集的分布反而不符合其他大多数点的分布规律而被筛除

LOAM 系列中对点云的筛选

以上我们分别针对整个点云去噪和按扫描线去噪进行了整理，最后简单提一下 LOAM 中对点云的提取。LOAM 中对点云中的点是否能形成可靠特征的一个判断标准是它能否被稳定观察到。LOAM 中着重提了这两种情况的点是不稳定的：

特征成平面和扫描线近乎平行
特征扫描到的其中一端被另一个平面挡住，这部分的点也不稳定

对于第一个标准，实现方法如下（参考 LIO-SAM），对每一对相邻点，我们进行以下检查：
- 首先这两个点在水平方向应该相隔比较近，防止由于信号丢失等原因导致扫描环在中间断掉了，导致两端距离相差较远
- 检查两个点的距离差，如果发现某一个点在另一个点的后面，则将该点周围一定区域的点都过滤掉

template <typename PointType>
void filter_occuluded_points(boost::shared_ptr<pcl::PointCloud<PointType>>& src_cloud_ptr,
                             boost::shared_ptr<pcl::PointCloud<PointType>>& dst_cloud_ptr,
                             int neighbor_count,
                             float distance_threshold,
                             float horizontal_angle_diff_threshold,
                             boost::shared_ptr<std::vector<int>> removed_indices = nullptr) {
    int cloud_size     = src_cloud_ptr->points.size();
    distance_threshold = std::fabs(distance_threshold);

    boost::shared_ptr<pcl::PointCloud<PointType>> cloud_ptr(new pcl::PointCloud<PointType>());
    cloud_ptr->points.reserve(cloud_size);

    std::vector<int> status(cloud_size, 0);

    for (int i = neighbor_count; i < cloud_size - neighbor_count; ++i) {
        const PointType& pt1 = src_cloud_ptr->points[i];
        const PointType& pt2 = src_cloud_ptr->points[i + 1];

        double horizontal_angle_1 = std::atan2(pt1.y, pt1.x) / M_PI * 180.0;
        double horizontal_angle_2 = std::atan2(pt2.y, pt2.x) / M_PI * 180.0;

        if (std::fabs(horizontal_angle_1 - horizontal_angle_2) > horizontal_angle_diff_threshold) continue;

        float range1 = std::sqrt(pt1.x * pt1.x + pt1.y * pt1.y + pt1.z * pt1.z);
        float range2 = std::sqrt(pt2.x * pt2.x + pt2.y * pt2.y + pt2.z * pt2.z);

        if (range1 - range2 > distance_threshold)  // pt1 is occluded
        {
            for (int j = i; j >= i - neighbor_count; j--) {
                status[j] = 1;
            }
        } else if (range2 - range1 > distance_threshold) {  // pt2 is occluded
            for (int j = i + 1; j <= i + neighbor_count; j++) {
                status[j] = 1;
            }
        }
    }

    for (int i = 0; i < cloud_size; ++i) {
        if (status[i] == 0) {
            cloud_ptr->points.push_back(src_cloud_ptr->points[i]);
        } else if (removed_indices != nullptr) {
            removed_indices->push_back(i);
        }
    }

    dst_cloud_ptr = cloud_ptr;
}

以 1.0 度为相邻点判断标准，1 米为筛选距离，过滤点范围为 5 个，筛选效果如下，可以发现被筛选的点基本都是被其他平面覆盖掉的不稳定的点。其中部分误删除的点可能是由于在计算扫描线时出现的误匹配点。

对于第二个标准，实现方法如下，同样参考自 LIO-SAM 的代码，主要的思路对每一个点 O 计算其相邻两个点 A, B 的距离，有以下几种情况我们需要筛选：
- 如果两个距离都很大，且 OA 和 OB 同号，那么该点要么在两个点前面，要么在两个点的后面，可以考虑为噪声
- 如果两个距离都很大，且 OA 和 OB 异号，那么该三个点形成的线段较长，这种情况在该平面和激光束平行性较好的时候出现，符合这里的第二个标准

template <typename PointType>
void filter_parallel_points(boost::shared_ptr<pcl::PointCloud<PointType>>& src_cloud_ptr,
                            boost::shared_ptr<pcl::PointCloud<PointType>>& dst_cloud_ptr,
                            float distance_ratio,
                            float horizontal_angle_diff_threshold,
                            boost::shared_ptr<std::vector<int>> removed_indices = nullptr) {
    int cloud_size = src_cloud_ptr->points.size();

    boost::shared_ptr<pcl::PointCloud<PointType>> cloud_ptr(new pcl::PointCloud<PointType>());
    cloud_ptr->points.reserve(cloud_size);

    std::vector<int> status(cloud_size, 0);

    for (int i = 1; i < cloud_size - 1; ++i) {
        const PointType& pt0 = src_cloud_ptr->points[i];
        const PointType& pt1 = src_cloud_ptr->points[i - 1];
        const PointType& pt2 = src_cloud_ptr->points[i + 1];

        double horizontal_angle_0 = std::atan2(pt0.y, pt0.x) / M_PI * 180.0;
        double horizontal_angle_1 = std::atan2(pt1.y, pt1.x) / M_PI * 180.0;
        double horizontal_angle_2 = std::atan2(pt2.y, pt2.x) / M_PI * 180.0;

        if (std::fabs(horizontal_angle_0 - horizontal_angle_1) > horizontal_angle_diff_threshold ||
            std::fabs(horizontal_angle_0 - horizontal_angle_2) > horizontal_angle_diff_threshold)
            continue;

        float range0 = std::sqrt(pt0.x * pt0.x + pt0.y * pt0.y + pt0.z * pt0.z);
        float range1 = std::sqrt(pt1.x * pt1.x + pt1.y * pt1.y + pt1.z * pt1.z);
        float range2 = std::sqrt(pt2.x * pt2.x + pt2.y * pt2.y + pt2.z * pt2.z);

        float diff1 = std::fabs(range0 - range1);
        float diff2 = std::fabs(range0 - range2);

        if (diff1 > distance_ratio * range0 && diff2 > distance_ratio * range0) {
            status[i] = 1;
        }
    }

    for (int i = 0; i < cloud_size; ++i) {
        if (status[i] == 0) {
            cloud_ptr->points.push_back(src_cloud_ptr->points[i]);
        } else if (removed_indices != nullptr) {
            removed_indices->push_back(i);
        }
    }

    dst_cloud_ptr = cloud_ptr;
}

筛选效果如下，可以看出来这种方式能够比较好的筛选出与激光束平行的平面。