基于PCA主成分分析的GPF算法，利用求解平面法线和高度距离阈值，反复迭代拟合法线。

1、初始化

首先对z值进行深度排序，对z值进行从小到大排序。然后，把z值小于1.5 * sensor_height_（安装高度）的点删除掉，剩余的点作为初始化点laserCloudIn。

2、平面拟合

2.1单独面的平面拟合

首先，提取初始化地面种子点（用于第一次的平面拟合。）

在初始化后的点中，满足小于平均高+高度阈值（一般为1.2）的前20个点，作为选取的地面点种子g_seeds_pc。

将种子点g_seeds_pc存入地面点容器g_ground_pc，进入迭代次数的循环进行地面检测地面点容器g_ground_pc的点云，并且在地面估计函数后清空地面点和非地面点容器。

地面估计函数：

用地面点容器进行估计平面点，首先用地面点容器g_ground_pc计算协防差矩阵cov和均值pc_mean，SVD分解求cov的U，其中协防差矩阵最小的特征向量（法线）normal 指向了数据变化最小方向，即z轴。然后利用法线求均值到平面的负距离-d，再求中心点距离平面的高度+0.1的值，也即是高度阈值（0.1+d）th_dist_d_。

再得到拟合中心点到平面高度阈值和法线后，清空地面点和非地面点。

接下来为第二次的平面拟合准备初始数据。

如果迭代次数不是最后一次，我们拿初始化点laserCloudIn作为初始点云，然后利用法线normal求解每个点距离平面的距离，并存入result中，接下来迭代每个点到拟合的平面距离result值，如果小于上述高度距离阈值th_dist_d_，则将点云存入地面点容器，然后利用地面点容器的点云再次进入迭代的for循环来重启拟合平面法相量normal和高度阈值th_dist_d_。只有当最后一次迭代的时候，将所有点云带入前面拟合的法相量和高度距离阈值，求出对应的地面点云和非地面点云。

2.2单面分割的平面拟合

这里主要把点云传入分割好的三个平面中，以前中后三个平面分别迭代对应平面和非平面点云（和2.1一致），以20，-20的x轴分别为界限，划分三个平面。

#define X_FRONT 20.0          //宏定义X轴第0-1区域分界线（单位：m）
#define X_REAR -20.0          //宏定义X轴第1-2区域分界线