引言
在实际实现3D目标检测时,在不依靠深度学习的训练模型时,仅采用传统方法实现目标检测。一般在实施检测之前,均需要删除地面点云才能确保后续其他障碍物点云数据的提取精度,防止因为地面点云产生干扰。
本博客的地面分割参考论文《Fast Segmentation of 3D Point Clouds: A Paradigm on LiDAR Data for Autonomous Vehicle Applications》中的地面提取算法。
地面分割原理
在论文中将地面拟合算法定义为(GPF),其中整个地面点云提取的方式采用平面拟合提取的方式。我们重点关注一下该算法的基本流程:
该算法表很清楚的表达出:
- 输入的为待处理的点云数据 P \ P P,其结果为地面点云 P g \ P_{g} Pg以及非地面点云 P n g \ P_{ng} Png。
- 需要人为定义4个参数:迭代次数 N i t e r \ N_{iter} Niter、初始随机采样的点云数量 N L P R \ N_{LPR} NLPR、高度阈值 T h s e e d s \ Th_{seeds} Thseeds、距离阈值 T h d i s t \ Th_{dist} Thdist。
值得注意的是 T h s e e d s \ Th_{seeds} Thseeds代表的是激光雷达的坐标系原点到实际地面的距离值。
整个算法思路较为清晰:
- 首先将点云按照高度排序。 S o r t O n H e i g h t ( P ) \ SortOnHeight(P) SortOnHeight(P) 代表的是将输入的点云按高度排序,简单来说按照 Z \ Z Z 轴排序即可。
- 从完成排序的点云中选取 N L P R \ N_{LPR} NLPR个点视作是初始的地面点云 P g 0 \ P_{g}^0 Pg0。
- 对初始的地面点云 P g 0 \ P_{g}^0 Pg0做平面拟合得到初始的平面模型参数 m o d e l \ model model。
- 剩下的则就是计算整个输入的点云 P \ P P中的每个点与平面模型参数 m o d e l \ model model的距离值 D i s ( p k , m o d e l ) \ Dis(p_{k},model) Dis(pk,model)。进一步做距离值判定 m o d e l ( p k ) < T h d i s t \ model(p_{k})< Th_{dist} model(pk)<Thdist。得到新一轮的地面点云 P g 1 \ P_{g}^1 Pg1以及非地面点云 P n g 1 \ P_{ng}^1 Png1
- 最后则是在针对新一轮的地面点云 P g 1 \ P_{g}^1 Pg1求解平面模型,重复3、4步迭代计算。得到最终的地面点云 P g \ P_{g} Pg以及非地面点云 P n g \ P_{ng} Png。
相信流程讲解之后,大家会理解地面拟合的原理。然后只需要一步步实现其代码即可。
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地面分割的优缺点
优点:
- 具有很强的鲁棒性,可以适应绝大多数地面分割场景。
- 相比布料地面提取、形态学地面分割等方法,其计算速度较快,效率较高。
缺点:
- 求解的精度取决于迭代的次数,一般常见需要迭代4次以上,才能够有较高的估计精度。
- 无法适应于一些带有坡度的地面,尤其是在具有凹凸坡度的复杂环境下。
- 平面估计的精度容易受定义的距离阈值 T h d i s t \ Th_{dist} Thdist参数的影响,若实际应用需要提取较小的障碍物的话,很容易将障碍物的点云也包含到地面点云里面。
改进思路
- 在设定迭代次数后,可以将首次提取完成的地面点云 P g 1 \ P_{g}^1 Pg1当做第二次迭代的输入点云。提高计算效率,因为很多点在第一次计算距离时,就远远大于设定的距离阈值,我们只需要关注离距离阈值附近的点。
- 距离阈值 T h d i s t \ Th_{dist} Thdist参数可以随着迭代次数的增加而减少,用最后一两次做优化即可,如第一次估计平面时设置为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
…