无人驾驶传感器融合系列（一）——激光雷达点云的分割原理及实现

无人驾驶传感器融合系列（一）——激光雷达点云的分割原理及实现

本章摘要：激光雷达扫描得到的点云含有大部分地面点，这对后续障碍物点云的分类、识别和跟踪带来麻烦，所以需要将其分割掉。本章主要讲解点云的基础分割算法—RANSAC算法，通过例子分析其基本原理，然后讲解如何运用PCL实现RANSAC算法。

RANSAC算法原理

RANSAC (Random Sample Consensus) 随机采样一致性算法，其实就是想办法找出代表地面的平面。如下图所示，绿色的点为打在地面上的点，红色的点为打在障碍物上的点。打在地面上的点基本上是处在一个平面上的，所以我们的目标就是找到这个平面，然后将距离此平面一定距离内的点分割成地面。
由于算法逻辑本质上是一致的，为了简便起见，这里将三维空间寻找平面问题，转化为二维空间寻找直线的问题。算法逻辑如下：
（1）要得到一个直线模型，需要两个点唯一确定一个直线方程。所以第一步随机选择两个点。
（2）通过这两个点，可以计算出这两个点所表示的方程y=ax+b。
（3）计算所有的数据点到这个方程的距离。
（4）找到所有满足距离阈值的点。
（5）然后再重复（1）~（4）这个过程，直到达到一定迭代次数后，选出那个包含最多点的直线，作为要求的直线，其所包含的点作为要分割的点。过程如下图所示：

PCL实现RANSAC算法

PCL是一个开源的点云处理库，是在吸收了前人点云相关研究基础上建立起来的大型跨平台开源C++编程库，它实现了大量点云相关的通用算法和高效数据结构，包含点云获取、滤波、分割、配准、检索、特征提取、识别、追踪、曲面重建、可视化等大量开源代码。下面代码介绍如何用PCL实现RANSAC算法：

template<typename PointT>
std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> ProcessPointClouds<PointT>::SegmentPlane(typename pcl::PointCloud<PointT>::Ptr cloud, int maxIterations, float distanceThreshold)
{
    auto startTime = std::chrono::steady_clock::now();  //记录起始时间
	
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices());   //std::vector indices，包含在平面一定范围内所有点云id的vector.
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); //定义模型的系数
    pcl::SACSegmentation<PointT> seg;        // Create the segmentation object
    seg.setOptimizeCoefficients (true);      // Optional
    seg.setModelType (pcl::SACMODEL_PLANE);  //设置采用的模型
    seg.setMethodType (pcl::SAC_RANSAC);     //设置方法为ransac分割
    seg.setMaxIterations (maxIterations);    //设置最大迭代次数
    seg.setDistanceThreshold (distanceThreshold);   //设置距离阈值
    seg.setInputCloud (cloud);               //输入点云
    seg.segment (*inliers, *coefficients);   //得到分割结果
    
    if (inliers->indices.size () == 0)
    {
      std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
    }

    auto endTime = std::chrono::steady_clock::now();  //记录结束时间
    auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
    std::cout << "plane segmentation took " << elapsedTime.count() << " milliseconds" << std::endl;

    std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult = SeparateClouds(inliers,cloud);
    return segResult;
}

template<typename PointT>
std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> ProcessPointClouds<PointT>::SeparateClouds(pcl::PointIndices::Ptr inliers, typename pcl::PointCloud<PointT>::Ptr cloud) 
{
  // TODO: Create two new point clouds, one cloud with obstacles and other with segmented plane
    typename pcl::PointCloud<PointT>::Ptr plancloud (new pcl::PointCloud<PointT>());
    typename pcl::PointCloud<PointT>::Ptr obstcloud (new pcl::PointCloud<PointT>());

    for(int index : inliers->indices)
        plancloud->points.push_back(cloud->points[index]);
    
    // Create the filtering object
    pcl::ExtractIndices<PointT> extract;
    // Extract the inliers
    extract.setInputCloud (cloud);
    extract.setIndices (inliers);
    extract.setNegative (true);
    extract.filter (*obstcloud);

    std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult(plancloud, obstcloud);
    return segResult;
}

完整代码见：github

显示不同的输出结果见 sensors/ environment.cpp/ simpleHighway, 可以根据注释自己调整输出。