描述

这一篇文章介绍2017年的一篇基于点云数据的地面分割算法——GPF(Ground Plane Fitting)
论文全称:Fast Segmentation of 3D Point Clouds: A Paradigm on LiDAR Data for Autonomous Vehicle Applications

这篇文章实际上有两个贡献:其中一个是GPF。而另外一个是SLR(Scan Line Run ),用于对非地面的数据点进行聚类。由于我最近关注的是地面点划分,所以这篇文章只介绍GPF,以后有机会再介绍SLR

由于仅仅介绍GPF,因此这篇文章与以往稍有不同,仅仅介绍原文中的GPF,而不是全篇性质的翻译分析。文章的最后还会给出我的部分实现代码

Ground Plane Fitting

分析

点云数据的大部分实际上都属于地面,去除地面对于减少计算量来说很有效。地面点的识别和提取,适合用于去除地面,原因有两个:

1.地面点容易被识别,因为它们属于平面,而平面是具有简单数学模型的几何对象

2.最低的点最有可能属于地面。用这一先验,我们可以不再像RANSAC(随机一致性采样算法)那样随机选择初始点,而是以最低点为初始,这样收敛更快。

通常,一个平面模型难以还原真实的地面。因为地面并不是一个完美平面,并且激光雷达在远距离时有显著噪声。我们观察到,大多数情况下,地面的表示需要检测出坡度变化。本文所提出的地面拟合技术,通过将点云沿x轴(车辆行驶方向)均匀划分为N_{segs}段,每一个分段中都做地面拟合。这种方法可以解决上述问题。

算法步骤

算法流程如下。

步骤:

  1. 对于每个点云数据段,会先提取一组具有低高度值的种子点,然后用这些种子点来估计出一个地面的初始模型。

  2. 根据估计出的平面模型,点云段P中的每个点都要参与计算,计算从该点到其在候选平面上的正交投影的距离(简单理解成垂线段的长度吧)。

  3. 将该距离与用户定义的阈值Th_{dist}进行比较,该阈值决定该点是否属于地面。

  4. 属于地面的点被用作新模型的种子,该过程重复N_{iter}次。

  5. 最后,由该算法产生的每个点云段的地面点连接起来,就是整个地面。

我们选择初始种子点的方法叫做最低点代表(LPR),该点定义为点云的N_{LPR}个最低高度值点的平均值。LPR保证了噪声不会影响平面估计结果。一旦计算出了LPR,则将其视为点云P的最低高度值点,并且高度阈值Th_seeds内的点会成为初始种子点,用作后续的平面模型估计。

对于每个平面,我们使用一个简单的线性模型

我们利用由种子点集S∈ R^3计算出的协方差矩阵C∈R^{3x3} ,求解出法向量n

式子中的,ŝ ∈ R^3 代表所有种子点s_i ∈ S的平均值

协方差矩阵C可以捕获到种子点的分散度,其三个特征向量可通过其奇异值分解(SVD)计算,描述了分散度的三个主要方向。由于平面是一个平滑的面,垂直于平面的法线n表示出了方差最小的方向。因此法向量实际上就是特征值最小的那个特征向量。

在获取到法向量n之后,用ŝ代替x(平均值ŝ可以代表地面点)带入到等式1进行计算,就可以得到d了。

算法分析

通过仔细阅读论文,我们很容易理解GPF的算法是如何实现的。

显然这种算法与之前介绍的Linefit有着相同的算法思路,先将点云进行了分区。不同的是,GPF利用了点云数据的x方向值,进行了分割。这种分割方法显然比LineFit算法,更加粗放。在分区之后,对每个区的点云数据利用z值寻找了种子点,利用种子点拟合了一个平面。最后利用这个平面,求每个点到这个平面的高度,以此来划分地面与非地面。

在算法实现的过程中,我发现值得注意的有两点:

  1. 算法如文中所说,效果是依赖于参数设定的,算法中的几个阈值和迭代次数是很关键的。

  2. 注意,算法首先要对点云进行分区。网上有很多文章,对全部数据去做算法,大错特错, 误人子弟

GPF算法的C++实现

只贴出一段核心代码吧

void gpf_processor::segment_piece(pcl::PointCloud<pcl::PointXYZ>& cloud, std::vector<int>* segmentation) {
    segmentation->clear();
    segmentation->resize(cloud.size(), -1); // all points set as -1 initially

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_input(new pcl::PointCloud<pcl::PointXYZ>);
    cloud_input = cloud.makeShared();

    extract_initial_seeds(*cloud_input);

    std::vector<int> id_judge;
    id_judge.clear();
    id_judge.resize(cloud_input->points.size(), 0);
    for (int i = 0; i < m_N_iter; i++) {
        estimatePlane();
        m_cloud_ground.clear();
        m_cloud_noground.clear();
        extract_plane_cloud(*cloud_input, id_judge);
    }

    for (int i = 0; i < id_judge.size(); i++) {
        segmentation->at(i) = id_judge[i];
    }
}

算法的效果示意

总结

这篇文章介绍了GPF的核心算法步骤,给出了C++实现的部分代码及实验效果

抱歉GPF的代码不打算完全开源,想要的朋友可以私聊我有偿提供,谢谢。

写的不容易,欢迎各位朋友点赞并加关注,谢谢!