引言

在实际实现3D目标检测时，在不依靠深度学习的训练模型时，仅采用传统方法实现目标检测。一般在实施检测之前，均需要删除地面点云才能确保后续其他障碍物点云数据的提取精度，防止因为地面点云产生干扰。
本博客的地面分割参考论文《Fast Segmentation of 3D Point Clouds: A Paradigm on LiDAR Data for Autonomous Vehicle Applications》中的地面提取算法。

地面分割原理

在论文中将地面拟合算法定义为(GPF)，其中整个地面点云提取的方式采用平面拟合提取的方式。我们重点关注一下该算法的基本流程：

GPF算法流程表
该算法表很清楚的表达出:

输入的为待处理的点云数据 $\ P$ ，其结果为地面点云 $P_{g}$ 以及非地面点云 $P_{ng}$ 。
需要人为定义4个参数：迭代次数 $N_{iter}$ 、初始随机采样的点云数量 $N_{LPR}$ 、高度阈值 $Th_{seeds}$ 、距离阈值 $Th_{dist}$ 。

值得注意的是 $Th_{seeds}$ 代表的是激光雷达的坐标系原点到实际地面的距离值。

整个算法思路较为清晰：

首先将点云按照高度排序。 $\ SortOnHeight(P)$ 代表的是将输入的点云按高度排序，简单来说按照 $\ Z$ 轴排序即可。
从完成排序的点云中选取 $N_{LPR}$ 个点视作是初始的地面点云 $P_{g}^0$ 。
对初始的地面点云 $P_{g}^0$ 做平面拟合得到初始的平面模型参数 $\ model$ 。
剩下的则就是计算整个输入的点云 $\ P$ 中的每个点与平面模型参数 $\ model$ 的距离值 $Dis(p_{k},model)$ 。进一步做距离值判定 $model(p_{k})< Th_{dist}$ 。得到新一轮的地面点云 $P_{g}^1$ 以及非地面点云 $P_{ng}^1$
最后则是在针对新一轮的地面点云 $P_{g}^1$ 求解平面模型，重复3、4步迭代计算。得到最终的地面点云 $P_{g}$ 以及非地面点云 $P_{ng}$ 。

相信流程讲解之后，大家会理解地面拟合的原理。然后只需要一步步实现其代码即可。

GPF地面分割代码

首先是对点云排序以及选择出初始点云数据。

void extract_initial_seeds(const pcl::PointCloud<VPoint> &input_point_cloud){
    double height_sum = 0;
    int cnt = 0;
    for (int i = 0; i < input_point_cloud.points.size() && cnt < num_lpr_; i++){
        height_sum += input_point_cloud.points[i].z;
        cnt++;
    }
    double lpr_height = cnt != 0 ? height_sum / cnt : 0;	// 求解其高度均值
    groud_plane_cloud->clear();								// 需要提取的地面点云    
    for (int i = 0; i < input_point_cloud.points.size(); i++){
        if (input_point_cloud.points[i].z < lpr_height + th_seeds_){
            groud_plane_cloud->points.push_back(input_point_cloud.points[i]);
        }
    }
}

点云平面估计可以采用SVD平面拟合方式，也同样可以用pcl中的RANSAC平面估计的方式实现平面模型的提取。

void estimate_plane_parameter(void){
    Eigen::Matrix3f cov_matrix;			
    Eigen::Vector4f pc_mean;		//pc = palne cloud	计算其均值
    pcl::computeMeanAndCovarianceMatrix(*groud_plane_cloud, cov_matrix, pc_mean);
    JacobiSVD<MatrixXf> svd(cov_matrix, Eigen::DecompositionOptions::ComputeFullU);
    plane_normal_ = (svd.matrixU().col(2));	//plane_normal_表示的就是平面估计后的法向量
    Eigen::Vector3f seeds_mean = pc_mean.head<3>();
    dis_ = -(plane_normal_.transpose() * seeds_mean)(0, 0);	// normal.T*[x,y,z] = -d
    th_dist_d_ = th_dist_ - dis_;		// 设定距离偏差值 th_dist_d_
    return;
}

然后便是仅仅做距离判定即可分离出地面点云与非地面点云了。

void extract_plane_cloud(const pcl::PointCloud<VPoint> input_point_cloud,
                        pcl::PointCloud<VPoint>::Ptr & ground_point_cloud,
                        pcl::PointCloud<VPoint>::Ptr & noground_point_cloud){
    //point cloud to matrix
    Eigen::MatrixXf points_matrix(input_point_cloud.points.size(), 3);
    int j = 0;
    for (auto p : input_point_cloud.points){
        points_matrix.row(j++) << p.x, p.y, p.z;
    }
    Eigen::VectorXf result_dis = points_matrix * plane_normal_;
    for (int r = 0; r < result_dis.rows(); r++){
        if (result_dis[r] < th_dist_d_){
            ground_point_cloud->points.push_back(input_point_cloud[r]);
        }
        else{
            noground_point_cloud->points.push_back(input_point_cloud[r]);
        }
    }
    return;
}

后续就较为简单的采用for循环迭代即可完成提取。

GPF地面分割的优缺点

优点：

具有很强的鲁棒性，可以适应绝大多数地面分割场景。
相比布料地面提取、形态学地面分割等方法，其计算速度较快，效率较高。

缺点：

求解的精度取决于迭代的次数，一般常见需要迭代4次以上，才能够有较高的估计精度。
无法适应于一些带有坡度的地面，尤其是在具有凹凸坡度的复杂环境下。
平面估计的精度容易受定义的距离阈值 $Th_{dist}$ 参数的影响，若实际应用需要提取较小的障碍物的话，很容易将障碍物的点云也包含到地面点云里面。

改进思路

在设定迭代次数后，可以将首次提取完成的地面点云 $P_{g}^1$ 当做第二次迭代的输入点云。提高计算效率，因为很多点在第一次计算距离时，就远远大于设定的距离阈值，我们只需要关注离距离阈值附近的点。
距离阈值 $Th_{dist}$ 参数可以随着迭代次数的增加而减少，用最后一两次做优化即可，如第一次估计平面时设置为50cm，第二次可设置为40cm，第三、四次可设置为30cm。这样可以提高地面提取的精度。