简介

　　在上一篇文章pcl学习系列之GroundSegmentation滤波(一)通过使用pcl库来回归拟合平面进行地面点提取，本篇博文介绍一篇2010年的快速地面点云分割算法，该论文链接：Fast segmentation of 3d point clouds for ground vehicles，当然，作者还是很赞的开源了代码，传送门Github。当然，我也调试作者的代码，同时进行一定的理解。通过简单的cmake编译即可运行，代码已经放在Github/FloorSegmentation上面，见末尾链接。说起该篇地面点提取的策略，主要还是通过对点云进行降维，同时采取不同的划分扇区点云策略，通过局部拟合直线来判断是否为地面点。具体细节步骤会在如下进行详细解释。

算法浅析

点云无序转有序策略

　　这篇论文地面点提取的思路首先将点云从无序变为有序的策略，将点云划分成为扇形区域(segment)和每个扇形区域继续划分的子区域(bins)。通过这两步划分，将所有的点云能够有续的存储与访问。下面简单说一下，是如何划分为segment以及每个segment进行bins的划分：

　　上图代表对点云进行segment的划分，代码中是设置一共划分多少个segments，然后通过360度除以就可以获取每个segment的扇形大小，这样就将$S_0$与$S_1$等划分出来。每一个扇形区域代表一个segment。

　　划分完segment后，我们得到每个扇形区域都有一个编号$S_i$，这样我们在对每个扇形区域$S_i$进行划分Bins，以此进一步缩小点云的区域。划分扇形区域的Bins是依据点云的X-Y平面到中心点距离来进行划分的。即根据 $$r_j^{min}<\sqrt{(x_i^2+y_i^2)}<r_j^{(max)}$$
　　这样划分完Bins之后，我们就可以通过将这个区域$b_j^s$来代表第s个segment与j个bin来找到这个子区域。这样一来我们就可以将所有的点都通过这种映射方式把点云变为有序存储。

点云降维存储

　　点云按照上述进行有序存储时候，不可能存储所有的三维点云信息，该文章通过降维操作来进行存储点云压缩后的信息。那么它是如何进行压缩点云信息的呢？该文章将每个点云信息$(x,y,z)$变成$(d,z)$的方式，将三维信息减少至二维存储。其中该点云的segment计算是通过$segment(p_i)=\frac{atan2(y_i,x_i)}{△\alpha }$这里的$△\alpha$代表划分segment的角度。$d=\sqrt{(x^2+y^2)}$通过计算出d来确定该点的Bins位置。这样每个点云都能够有效的进行降维索引。

现在，我们简单通过下面一个图来看下每个点云序列化后的索引关系：

主要就是两个步骤：

1. 根据点云的坐标$p_i$计算出位于哪个Segments，$S_i=\frac{atan2(y_i,x_i)}{△\alpha }$
2. 然后，确定$S_i$之后，根据映射的点$p_i^{{}^{\prime }}(d,z)$来计算出位于哪个Bin，$d=\sqrt{(x^2+y^2)}$。

　　附：这里的点云映射并不是一对一的关系，同时映射过后无法完全恢复原始点云的坐标，只能获取大概的区域范围。当然，作者也并不想通过映射后的点云来恢复原始点云，因为原始点云一直存在。代码里面是通过存储映射坐标时候顺序存储来进行与原始点云建立对应关系。这样只需要获取映射后的点云是否为地面点就能够知道原始点云是否为地面点。文章中描述如下：

直线拟合 -----------------------------------------------------------------------

　　我们通过上面的点云映射步骤，获取到所有点云的映射数据存储，主要为$(d,z)$的结构，这样关于d-z平面的所有点。每个区域里面都会存在很多映射点，由于我们的点云有序存储，我们采用增量算法【论文A comparison of line extraction algorithms using 2d range data for indoor mobile robotics 提出6种离散点拟合直线的方法，其中就包括增量算法】来进行直线的拟合既有效又快捷。具体拟合直线算法细节这里就不在赘述了，感兴趣的可以去阅读该论文。

判断直线拟合的条件

直线的斜率：该直线的斜率(slope)必须不能够超过一个阈值$T_m$。$(d,z)$该直线斜率代表Z轴到X-Y平面的倾斜程度，不能够出现垂直结构现象。

直线的截距：$(d,z)$平面上的直线，当斜率$m<T_m^{small}$的时候，$y-b$不能超过一个设定的阈值$T_b$。这样的措施是不会将高出地面的点云误认作地面点。例如：激光雷达扫描的室内屋顶，几乎也是一个平面，但是它的点云高程信息较大。

拟合误差：进行直线拟合时候均方根误差不该超过设定的阈值$T_{RMSE}$。

直线对链接条件：为了确保直线对之间的平滑过渡连接起来，直线的第一个点到之前拟合的直线的距离不能超过阈值$T_{d_{prev}}$。不同直线对连接，通过距离阈值来判断是否两个地面点云的距离是否比较接近。远距离的话没有必要把两个点连接在一起。毕竟直线的拟合在远距离会导致误差加大。

分割非地面点

　　获取所有标记后的扫描点之后，最后一个步骤是将接近非地面点分为若干个对象。当然，可以通过实验三维聚类的方式来实现这一策略，但是并不能够实时性完成很好。所以，该篇论文采取网格技术，应用二进制的网格结构来完善地面的分割。正如论文Real-Time Object
Classification in 3D Point Clouds Using Point Feature Histograms所叙述那样，所有非地面点都映射到网格单元C，使得网格中的某些单元被占用，而其他单元保持不变。然后，通过查找存储在已占用网格单元的连接组件上的三维点，可以找到单个对象的点云表示。虽然这依赖于维度的中间降低，但是在我们的方法中并不存在占用网格常见的分割不足问题。因为在上一步中，非接地点已经与接地点分开，并且可以仅根据非接地点来确定小区占用。此外，由于单个非地面点已经渲染了一个单元被占用，因此可以在更大范围内检测到对象。

3D Voxel Grid Segmentation

　　如果一个非地面对象被放置在另一个非地面对象之下，例如树下的汽车，那么目前所述的方法将导致分割不足，将汽车和树都分配到同一个段。所以，论文提到需要检测这种罕见的情况，并通过在全三维中（例如在体素网格中）执行最终分割来细化分割结果。然而，简单地将最终的3D分割应用于所有的片段是不合适的，由于违反实时性要求。因此，我们在检测需要特殊3D处理的片段，而不是在最后一步时候处理所有片段。幸运在于，我们知道线段的哪些点映射到单个网格单元。为了确定哪些分割段segments需要在3D维度进行处理，该论文提出通过遇到Z轴高程坐标差来分割对象$Oi$的每一个单元 $c\in C_i$。设置$P_c^{O_i}$表示映射到$c\in C_i$的$O_i$段的所有非接地点。随后，该论文提出对所有的点$P_{c\in C_i}^{O_i}$进行三维体素网格分割，如果任意一对点之间的距离$|p_{i_z}-p_{j_z}|>T_{3D}$。如果在被分割对象的几个单元c上，任何两点z坐标之间有很大的间隙，没有第三个点落入其中，我们就对分段的点进行三维分割。三维体素网格分割直接进行，并进行类似于上文所述的2D占用网格分割。在$N_{3D}=2$和$T_{3D}=0.4$的情况下执行最终的三维体素网格分割的结果如下图所示。

　　上面说了这么多，主要是通过栅格地图来判断是否需要进行三维聚类，以此来划分出高于地面的点树与车的分离，特别是在于车停在树下面。当然判断条件就是栅格地图中所对应的点云对距离是否超出一个设定的阈值$T_{3D}$。同时要时刻保持实时性，只能怪进行局部的聚类算法，当判断条件满足时候，就划分Voxel Grid网格进行聚类。

代码分析

　　这里主要是针对我已经调试过的代码进行分析一下，代码的函数可能略有改动，大体上逻辑与原论文作者的代码没有什么太大变化。我的GitHub上传的代码FloorSegmentation主要有如下几个文件：

主要调用逻辑

　　从上面的文件图片显示可以明显看出这几个文件的功能：CloudBin就是将点云分割成为Segment之后，进一步划分该点云属于哪一个Bins里面。CloudSegment就是划分点云属于哪个Segment范围里面。FloorSegment是点云提取地面点的主要文件。test_floorseg是测试该算法文件，里面有关于该算法的入口函数。下面就简单说下整个模块函数的调用顺序，方便代码阅读以及理解。

主要是函数调用流程：(关于pcl可视化的相关函数并未列举出来)

阈值变量解释

下面就是该算法涉及到的参数：

struct floorSegmentParams
{
    floorSegmentParams()
      : visualize(false)
      , r_min_square(0.3 * 0.3)
      , r_max_square(20 * 20)
      , n_bins(30)
      , n_segments(180)
      , max_dist_to_line(0.15)
      , max_slope(1)
      , max_error_square(0.01)
      , long_threshold(2.0)
      , max_long_height(0.1)
      , max_start_height(0.2)
      , sensor_height(0.2)
      , line_search_angle(0.2)
      , n_threads(4)
    {
    }

    bool   visualize; // 是否可视化
    double r_min_square; // 最小分割范围阈值
    double r_max_square; // 最大分割范围阈值
    int    n_bins;  // 每个segment划分多少个bins
    int    n_segments; // 划分多少个segments
    double max_dist_to_line; // 点到线的最大垂直距离阈值，视为地面
    double max_slope; // 拟合直线的斜率阈值
    double max_error_square; // 拟合直线的rmse
    double long_threshold; // 彼此点之间很远距离阈值
    double max_long_height; // 线点之间的距离大于long_threshold时，它们之间的最大高度差阈值
    double max_start_height; // 起点线最大高度阈值，标记为地面
    double sensor_height;  // 激光雷达安装的高度
    double line_search_angle; // 在角方向搜索直线的最大角阈值
    int    n_threads; // 多少个线程进行工作
};

　　附：如果点云数量过大，可能会导致点云运行分割程序导致崩溃的情况，有过这个测试。

实验结果

通过我的代码进行CMake编译后，获取的结果如下：

原始点云 提取地面点云 非地面点云

　　通过上图可以看到，还是存在一些地面点没有能够完全的分割提取出来，如果想要获取更好的效果，还是需要调节一下参数。同时，还要能够进一步的进行改进，例如划分策略的改变等等。

小结

　　点云地面分割一直以来都是在无人驾驶车辆感知、定位模块比较重要的预处理环节。难度最主要在于需要实时的进行点云分割处理，要精度同时也要速度，这个似乎是一个相互矛盾的现象。目前随着算力的提升，以深度学习为首的点云分割逐步被使用起来。但是，似乎结合深度学习的同时，使用经典的算法也可以覆盖一些绝大部分道路的场景。例如，本篇论文提及的算法基本可以使用在一般的道路上，针对固定的线路做出适当的参数调整即可。后续点云地面点提取或许有几个先验信息可以当做已知条件：1、地面点是扫描出来点云数量最多的点；2、地面点是扫描出来点云高程信息最低的点；3、针对Velodyne这种雷达扫描出来的地面点几乎几何形状为环视的线；4、扫描出来的地面点每个环点之间 间隔随着距离雷达中心位置越来越大，等等；由这些已知条件来设定算法，例如：针对本文的论文方法在划分segment以及bins就可以采取近细远粗的策略，这样能够更加有效的处理。以上都是个人见解，如有错误，还请批评指正。

参考

https://ieeexplore.ieee.org/document/5548059/
https://zhuanlan.zhihu.com/p/123220950
https://github.com/SmallMunich/FloorSegmentation
https://blog.csdn.net/u013630299/article/details/107175498
https://blog.csdn.net/Super_Mage413/article/details/98171655
https://www.sohu.com/a/334672903_715754