激光SLAM平面点的提取、使用学习

1.20240625 学习了PaGO-LOAM

论文地址：

PaGO-LOAM: Robust Ground-Optimized LiDAR Odometry

github地址：

GitHub - url-kaist/AlterGround-LeGO-LOAM: The page for PaGO-LOAM: Robust Ground-Optimized LiDAR Odometry

其提取地面点方法采用了PatchWork方法，在提取到地面点坐标之后，在特征提取featureAssociation.cpp文件中：

在非地面点上提取角点

复制代码

                int largestPickedNum = 0;
                for (int k = ep; k >= sp; k--) {
                    int ind = cloudSmoothness[k].ind;
                    if (cloudNeighborPicked[ind] == 0 &&
                        cloudCurvature[ind] > edgeThreshold &&
                        segInfo.segmentedCloudGroundFlag[ind] == false) {  // 在非地面点上提取边点

在地面点上提取面点：

复制代码

                int smallestPickedNum = 0;
                for (int k = sp; k <= ep; k++) {
                    int ind = cloudSmoothness[k].ind;
                    if (cloudNeighborPicked[ind] == 0 &&
                        cloudCurvature[ind] < surfThreshold &&
                        segInfo.segmentedCloudGroundFlag[ind] == true) { // 在地面点上提取面点

总结，地面/平面并没有直接参与平面约束的优化，而是只是为特征提取提供了一个参照，让角点从非地面点上提取，而面点从地面点上提取。最后的优化方式和LOAM算法是一致的，就是使用点到线和面的距离来做两步优化。

2.lego-loam

提取面的方式，使用了1）就是点投影到了range图上，对比该点和上一个点的垂直角，如果小于一定角度，则认为是地面点。2）在后续的连通域提取中，去掉了连通域无效的地面点。这种平面提取的方法，当遇到崎岖不平的地形或灌木丛区域时，该模块可能很敏感，这是基于线的地面分割的潜在限制。

在优化的时候，也非直接使用平面-平面的约束，还是从点线距和点面距的角度来计算位姿变换。

3.patchwork++

在代码中，首先看estimateGround这个函数，包含了：

1）反射噪声点移除函数 reflected_noise_removal

分别从垂直角、点的z值、点的反射强度来进行约束去掉反射噪声点。

2）函数 flush_patches

清空扇区内bin的存储数据

3）pc2czm 函数

把点投射到4个扇区内的Bin中

4）sort 和 extract_piecewiseground 函数

sort是对一个bin中的点按照z值进行排序。

extract_piecewiseground函数中包含了：

extract_initial_seeds ：提取出来小于z阈值的该bin中的点，并求得这些点的均值，然后求取小于该均值的bin点。

estimate_plane 就是一个简单的平面拟合函数。

5）Region-wise Ground Plane Fitting (R-GPF)

同样用的是 extract_initial_seeds 和 estimate_plane 这两个函数，然后计算点到平面之间距离，使用满足距离约束的点来迭代拟合平面。

6）temporal_ground_revert

根据均值和方差来矫正一些误判的bin

参考：基于规则的地面检测算法（二） - 知乎

4.平面参与优化

这一块先写到这里，后续补充上，先找个简单的问题来发篇文章才是最重要的。要不然时间越来越紧了。

参考：

PaGO-LOAM:基于地面优化的激光雷达里程计-腾讯云开发者社区-腾讯云