平面拟合算法

2.平面分割算法

1.前言

        输入的点云集中往往包含大量地面点云，这会降低计算速度。此外，地面点云会改变不同实体点云间的连通性，从而严重影响点云分割质量。因此需要在点云预处理过程中去除地面点云。常用的地面点云提取方法有基于网格的方法、平面拟合方法及cloth simulation filter。

2. 基于RANSAC的平面分割算法

2.1 算法介绍

        RANSAC（RANdom SAmple Consensus，随机采样一致性）是一种从包含离群值数据中估计数学模型参数的迭代方法。RANSAC算法假设数据中包含局内点和局外点，局内点可由一组特定参数的数学模型描述，而局外点无法满足该模型。 

        RANSAC通过迭代选择原始数据中的一个随机子集来进行模型拟合与迭代优化，RANSAC理论上可以剔除outliers的影响，并得到全局最优的参数估计。但是RANSAC 有两个问题，首先在每次迭代中都要区分 inliers 和 outlieres，因此需要事先设定阈值，当模型具有明显的物理意义时，这个阈值还比较容易设定，但是若模型比较抽象时，阈值就不那么容易设定了。而且固定阈值不适用于样本动态变化的应用；第二个问题是，RANSAC的迭代次数是运行时决定的，不能预知迭代的确切次数（当然迭代次数的范围是可以预测的）。除此之外， RANSAC 只能从一个特定数据集中估计一个模型。

        平面分割是RANSAC分割的一个应用实例，即模型为三维平面的RANSAC，返回的模型参数a,b,c,d中(a,b,c)为平面的法向量，d为平面的偏置。由n个点可以拟合得到一个平面(最少为3个点)，求得平面的模型参数，计算所有点到平面的距离，若点的距离在阈值范围内，则将该点加入局内点集，其余步骤与RANSAC流程相同。

python代码1如下所示(PCL)：

def plane_segment(self, distance_threshold=0.08):

    cloud = points_to_cloud(self.points[:, :3])

    seg = cloud.make_segmenter()

    seg.set_model_type(pcl.SACMODEL_PLANE)

    seg.set_method_type(pcl.SAC_RANSAC)

    seg.set_distance_threshold(distance_threshold)

    inds, coefs = seg.segment()

    return inds, coefs

  python代码2(PCL): 代码1为直接平面拟合，不考虑法向量的影响，代码2考虑了法向量的影响

def plane_segment_normals(self, kd_ksearch=80, distance_threshold=0.08, normal_weight=0.6, max_iterations=10000000):

    cloud = points_to_cloud(self.points[:, :3])

    seg = cloud.make_segmenter_normals(kd_ksearch)

    seg.set_optimize_coefficients(True)

    seg.set_model_type(pcl.SACMODEL_NORMAL_PLANE)

    seg.set_method_type(pcl.SAC_RANSAC)

    seg.set_distance_threshold(distance_threshold)

    seg.set_normal_distance_weight(normal_weight)

    seg.set_max_iterations(max_iterations)

    inds, coefs = seg.segment()

    return inds, coefs

其中主要的参数包括 kd_ksearch 法线估计邻域点数、distance_threshold 模型阈值、normal_distance_weight 法向量权重、 max_iterations 迭代次数。

open3d代码：

plane_model, inliers = pcd.segment_plane(distance_threshold=0.08, ransac_n=3, num_iterations=1000)

PCL与open3D所使用的RANSAC算法并不完全相同，如PCL中额外增加了normal_distance_weight项(open3D的RANSAC可以认为是PCL法向量权重为0的情况，因此该方法并不能把地面与近地面点区分，但可以增加代码进行过滤)，且PCL可以在达到模型精度阈值后自动终止，而open3d则需要达到最大迭代次数后才会终止。

2.2 模型参数说明

2.2.1 distance_threshold距离阈值

distance_threshold 距离阈值决定了数据点距离模型多远可以视为局内点，该值的确定取决于实际应用场景以及数据集，也可根据实验结果获得。例如对于路面的场景，路边沿与路中心存在一定的高度差，distance_threshold理论上应该选择路面高度差的一半，即可包围路面上的所有点。但由于路面本身平整度并不一定均匀，distance_threshold可以适当放大，distance_threshold过大也会导致非地面点被错误分割。

2.2.2 max_iterations 迭代次数

max_iterations 迭代次数可以由拟合成功率p的函数决定。p为RANSAC算法运行后至少提供一个有用结果的期望概率。RANSAC在选择用于模型参数估计的n个点时，若在某次迭代中只从输入数据集中的一致性点集中选取，则表示返回了成功的结果。设w为每次选择一个点时属于一致性点集的概率，则w^n是选择的n个点都属于一致性点集的概率，1-w^n是至少有一个点属于局外点的概率。

表示算法从未选择一个n个点都属于一致性点集的概率。取对数即可得到k的取值：

该值可以作为理论迭代次数的上限，为了获得更高置信度，在确定k时会额外考虑标准差。但PCL实际分割过程中，算法会根据当前模型误差来判断模型好坏，当误差小于期望值时便跳出循环，因此在不考虑时效的前提下，迭代次数可以选的足够大以达到期望精度。但在open3d中则需要根据情况具体设置迭代次数。

2.2.3 kd_ksearch 法线估计时的邻域点数

kd_ksearch 法线估计时的邻域点数，该值的选取决定了路面物对地面点的影响程度，过小易放大局部起伏，不利于将该点加入地面点集，过大易受地面物的影响，不利于将汽车(行人等)附近的地面点加入地面点集。

kd_ksearch 的下限为拟合平面的最小点数3。kd_ksearch 取值的上限理论为数据集中的总点数，但需要指出的是算法计算耗时与ksearch为对勾函数的关系(测试得到的数据，与kdtree理论相符)，即 t=ak+b/k，其中a，b均为正值。kd_ksearch 是影响计算时间主要参数(对同一点云文件)，kd_ksearch 过大时计算耗时会大幅增加，但当k=ceil(sqrt(b/a)))时计算耗时最小，且大于该点后模型计算耗时与kd_ksearch 线性相关，计算耗时主要受kd_ksearch 影响而与精度无关(因为模型内的地面点基本不变，但局外点仍需法线估计而不会被加入地面点集)。

a和b的选择主要与数据集的大小有关(在模型阈值确定的前提下)，在bag包所取帧数确定后，a，b同样可以大致确定，可以参考k=8*(frame_max - frame_min 1) +45设置ksearch初始值（图达通一帧点云）。或者先取较大kd_ksearch ，逐渐降低(若第一次降低时时间增大，说明初始kd_ksearch 设置过小)，多次降低出现连续的时间增大后，取第一个值与最后一个值求 t=ak+b/k的参数即可。

配置：12th Gen Intel(R) Core(TM) i5-1235U   1.30 GHz，12 cores，16.0 GB
点云	最佳kd_ksearch	平面分割耗时
经直通滤波（高度-1.2~2.5），点云数：67962	15~20	0.19s

2.2.4 normal_distance_weight 法向量权重

normal_distance_weight 法向量权重决定了法向量对算法判断局内点时的影响程度。0表示不考虑法向量权重的影响，此时该算法可以估计数据集中垂直墙面，或垂直板。若法向量权重非0，则算法会部分过滤垂直面的影响，当为1时则完全忽略垂直面。过小会导致大量路沿垂直面、墙面、树等垂直面被划入局内点。过大会导致非地面的水平面点被划入局内点。可以先将法向量权重设为0，调整距离阈值，在距离阈值确定后(能够包括地面绝大多数点)，再确定法向量权重。

3. 基于漫水填充算法的平面分割

3.1 算法介绍

       漫水填充算法，顾名思义就像洪水漫过一样，把一块连通的区域填满，当然水要能漫过需要满足一定的条件，可以理解为满足条件的地方就是低洼的地方，水才能流过去。在图像处理中就是给定一个种子点作为起始点，向附近相邻的像素点扩散，把颜色相同或者相近的所有点都找出来，并填充上新的颜色，这些点形成一个连通的区域。 像素点为有序排布，其可以以种子为中心向四个方向递归实现，在未达到边界时种子邻域中一定存在未填充的点。

       但在激光雷达点云处理中点云为无序排列，对邻域的定义主要有knn和radius两种方法。假使knn中邻域点数为5(包含自己的1)，便可以模拟图像处理时漫水填充的一般场景，但是点云的无序性(点云密度不均匀)会使产生的邻域点可能处于种子点的同一侧，从而导致水流终止。解决办法是增大knn或radius，但这会导致非地面点被误认为地面点(可以从相似点判断部分进行改进)以及计算时间的大幅增加。

       对相似点的判断，图像处理中主要是根据像素颜色值的接近程度，但点云地面分割中颜色并不能作为判断依据，常用的判断依据是点的法向量与点的坐标。基于法向量差作为判据的分割方法的明显缺点在于受地面物影响较大，因为点云法线估计时地面物会严重影响物体附近地面点法线，需要通过法线估计时的邻域半径与点数进行调整(这也是RANSAC考虑法向量权重时车辆下方路面点无法被识别为地面的原因)。

       基于以上分析，实现地面分割有两种比较好的途径：

1. 采用RANSAC，考虑法向量权重，但在法线估计时调低邻域点数(默认是30，用于地面分割时过大)。
2. 采用漫水填充，在法线估计时调低邻域点数，同时考虑点的绝对法线。

但以上方法在多帧叠加时效率很低，相对来说先根据一帧基于RANSAC计算地面法线(不考虑法向量)，然后直接找多帧叠加时符合模型的点效率更高(考虑法向量，是否考虑法向量取决于对地面物精度的要求，在考虑法向量时必须找到合适的kd_ksearch，否则精度会大幅降低)。

3.2 参数说明

3.2.1 threshold_angle角度阈值 与 threshold_curv 曲率阈值

         角度阈值threshold_angle是判断是否属于地面点的关键参数，在本算法中将频率滤波与区域生长的角度阈值进行了统一，取值范围为0~1，近似代表两向量夹角的弧度。

         曲率阈值threshold_curv源自区域生长算法，该值用于判断是否加入种子点集。

3.2.2 kd_ksearch 法线估计点数 和 ksearch 邻域点数

kd_ksearch 法线估计点数是进行法线估计时的邻域点数，该值的选取决定了路面物对地面点的影响程度，过小易放大局部起伏，不利于将该点加入地面点集，过大易受地面物的影响，不利于将汽车(行人等)附近的地面点加入地面点集，在PCL和open3d中不做声明会取默认值(30)，但在实际使用中，地面分割时kd_ksearch可以小一些，去除地面点后进行聚类分割时kd_ksearch可以适当增大。

ksearch 邻域点数是区域增长时的邻域点数，过小会导致邻域点全位于靠近雷达一侧，导致漫水中断。过大会把非地面的平面划入地面点集，解决办法是把点的绝对法线考虑进去。