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

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

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

RANSAC算法原理

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

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, 可以根据注释自己调整输出。

你可能感兴趣的:(传感器融合)