[自动驾驶-目标检测] C++ PCL 地面点云分割

引言

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

地面分割原理

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

该算法表很清楚的表达出:

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

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

整个算法思路较为清晰：

1. 首先将点云按照高度排序。$SortOnHeight(P)$ 代表的是将输入的点云按高度排序，简单来说按照$Z$ 轴排序即可。
2. 从完成排序的点云中选取$N_{LPR}$个点视作是初始的地面点云$P_g^0$。
3. 对初始的地面点云$P_g^0$做平面拟合得到初始的平面模型参数$model$。
4. 剩下的则就是计算整个输入的点云$P$中的每个点与平面模型参数$model$的距离值$Dis(p_k,model)$。进一步做距离值判定$model(p_k)<T{h}_{dist}$。得到新一轮的地面点云$P_g^1$以及非地面点云$P_{ng}^1$
5. 最后则是在针对新一轮的地面点云$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地面分割测试结果

从测试结果可观察出效果还是可以的，适合特定场景使用。

GPF地面分割的优缺点

优点：

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

缺点：

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

改进思路

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

参考文献

[1] Zermas D, Izzat I, Papanikolopoulos N. Fast segmentation of 3d point clouds: A paradigm on lidar data for autonomous vehicle applications[C]2017 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2017: 5067-5073.
[2] TiRan_Yang——地面分割：Fast Segmentation of 3D Point Clouds for Ground Vehicles
…