无人驾驶传感器融合系列(一)——激光雷达点云的分割原理及实现
本章摘要:激光雷达扫描得到的点云含有大部分地面点,这对后续障碍物点云的分类、识别和跟踪带来麻烦,所以需要将其分割掉。本章主要讲解点云的基础分割算法—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<int> 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, 可以根据注释自己调整输出。