代码实战 | 用LeGO-LOAM实现地面提取

原文链接：代码实战 | 用LeGO-LOAM实现地面提取[https://mp.weixin.qq.com/s/7G47_qBn7cRv9UtdvzlSSQ]
作者介绍：Zach，移动机器人从业者，热爱移动机器人行业，立志于科技助力美好生活。也是我们课程学员：基于LiDAR的多传感器融合SLAM：LOAM、LeGO-LOAM、LIO-SAM

LeGO-LOAM框架设计思路的第一步就是提取并分离地面。本篇文章就来详细说明LeGO-LOAM是如何来进行地面提取的。

地面提取的思路

上述检测地面的方法比较简单，如果遇到一些平整的大台面，也会误识别为地面。我们可以利用激光雷达的安装位置作为先验信息(例如高度信息)来进一步判断地面点。

地面提取源码实现

在查看地面提取源码之前，先看一下几个关键性的参数。在文件 LEGO-LOAM/LeGO-LOAM/include/utility.h中：

// VLP-16
extern const int N_SCAN = 16; // 激光雷达的线术条数
extern const int Horizon_SCAN = 1800; // VLP-16 一条线束1800个点, 360度. 
extern const float ang_res_x = 0.2; // 单条线束的水平分辨率
extern const float ang_res_y = 2.0; // 线束之间的垂直分辨率
extern const float ang_bottom = 15.0+0.1; // 
extern const int groundScanInd = 7; // 地面线束ID。因为激光雷达是水平放置, 并非所有线束会扫向地面，这里取7条线束扫向地面

上述几个参数定义了Velodyne-16线激光雷达的几个属性值，在后面的代码中会使用到。除了extern const int groundScanInd = 7;，其他的属性值可能比较好理解。

LeGO-LOAM 在检测地面点云时，并不是遍历所有scan(扫描线)的，因为雷达是水平放置，有一部分scan(扫描线)是射向天空的，框架里只取了贴近地面的七条scan(扫描线)

在文件LEGO-LOAM/LeGO-LOAM/src/imageProjection.cpp中，先介绍几个重要的变量：

cv::Mat rangeMat; // range matrix for range image (range Map)
cv::Mat labelMat; // label matrix for segmentaiton marking (用于点云分割的 label 矩阵)
cv::Mat groundMat; // ground matrix for ground cloud marking (用于标记地面点云)

上述变量 cv::Mat rangeMat; 对应论文中的 range Map，论文中把线激光雷达的所有扫描点16*1800 构成了一个 range Map；

对应论文中的 label，label 对非地面点进行了分类聚簇标记；cv::Mat groundMat; 在代码中标记了地面点。

地面提取功能的实现在imageProhection/void groundRemoval()函数中。这个函数中代码分为三部分：

• 第一部分：遍历所有点，检测地面点，在groundMat中进行标记地面点；

// groundMat: 把识别到的地面点, 标记于groundMat中
// -1, no valid info to check if ground of not(无效点)
//  0, initial value, after validation, means not ground (非地面点)
//  1, ground(表示地面点)
for (size_t j = 0; j < Horizon_SCAN; ++j) { // 遍历列, 一列1800个点
    for (size_t i = 0; i < groundScanInd; ++i) { // 遍历行，地面线束行7个
        // 同一列相连两行的点云ID
        lowerInd = j + ( i )*Horizon_SCAN;
        upperInd = j + (i+1)*Horizon_SCAN;
        // 如果所取的两点存在无效点, 该点lowerInd或者(i,j)在点云地图groundMat中也为无效点
        if (fullCloud->points[lowerInd].intensity == -1 ||
            fullCloud->points[upperInd].intensity == -1) {
            // no info to check, invalid points
            groundMat.at<int8_t>(i,j) = -1;
            continue;
        }
                
        // 获取两点lowerInd和upperInd在x/y/z方向的差值
        diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
        diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
        diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;
        // 计算两点lowerInd和upperInd垂直高度diffZ和水平距离的夹角
        angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;
        // 如果上述夹角小于10, 则(i, j) 和 (i+1, j)设置为地面标志1
        if (abs(angle - sensorMountAngle) <= 10) {
            groundMat.at<int8_t>(i,j) = 1;
            groundMat.at<int8_t>(i+1,j) = 1;
        }
    }
}

在上述代码中，首先依次提取同列相连两行的两个点：

// 同一列相连两行的点云ID
lowerInd = j + ( i )*Horizon_SCAN;
upperInd = j + (i+1)*Horizon_SCAN;

然后，根据强度值判断所提取的两个点是否为无效点；如果为无效点，则标记为-1，重新取点；

// 如果所取的两点存在无效点, 该点lowerInd或者(i,j)在点云地图groundMat中也为无效点
if (fullCloud->points[lowerInd].intensity == -1 ||
    fullCloud->points[upperInd].intensity == -1) {
    // no info to check, invalid points
    groundMat.at<int8_t>(i,j) = -1;
    continue;
}

最后，计算两点lowerInd 和 upperInd的高度差与平面距离的夹角

如果小于10 ，则在 groundMap对应的位置置1。

// 获取两点lowerInd和upperInd在x/y/z方向的差值
diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;
// 计算两点lowerInd和upperInd垂直高度diffZ和水平距离的夹角
angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;
// 如果上述夹角小于10, 则(i, j) 和 (i+1, j)设置为地面标志1
if (abs(angle - sensorMountAngle) <= 10) {
    groundMat.at<int8_t>(i,j) = 1;
    groundMat.at<int8_t>(i+1,j) = 1;
}

• 第二部分：在labelMat中移除地面点和无效点；labelMat 对所有的点进行了聚类标记，我们要在其中剔除掉地面点和无效点，这里的无效点 可以理解为未收到返回信号的点。

// 在labelMat中移除地面点和无效点(未收到返回值的点)
for (size_t i = 0; i < N_SCAN; ++i) {
    for (size_t j = 0; j < Horizon_SCAN; ++j) {
        if (groundMat.at<int8_t>(i,j) == 1 || rangeMat.at<float>(i,j) == FLT_MAX) {
            labelMat.at<int>(i,j) = -1;
        }
    }
}

• 第三部分：提取地面点云存储到 groundCloud；

// 提取地面点云存储到 groundCloud
if (pubGroundCloud.getNumSubscribers() != 0) {
    for (size_t i = 0; i <= groundScanInd; ++i) {
        for (size_t j = 0; j < Horizon_SCAN; ++j) {
        if (groundMat.at<int8_t>(i,j) == 1)
            groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
        }
    }
}

提取的点云通过主题发布出去，可以通过rviz 显示出来。如下图所示，我们为了明显区分地面点云，把它的颜色调整为了[255,0,0] 。

参考资料

1. https://github.com/RobustFieldAutonomyLab/LeGO-LOAM

作者来自我们课程学员：基于LiDAR的多传感器融合SLAM：LOAM、LeGO-LOAM、LIO-SAM

独家重磅课程！

1、 VINS Mono+Fusion 手推公式 + 源码解析课程

2、VIO课程：VIO最佳开源算法：ORB-SLAM3超全解析课程重磅升级！

3、图像三维重建课程（第2期）：视觉几何三维重建教程（第2期）：稠密重建，曲面重建，点云融合，纹理贴图

4、重磅来袭！基于LiDAR的多传感器融合SLAM 系列教程：LOAM、LeGO-LOAM、LIO-SAM

5、系统全面的相机标定课程：单目/鱼眼/双目/阵列 相机标定：原理与实战

6、视觉SLAM必备基础（第2期）：视觉SLAM必学基础：ORB-SLAM2源码详解

7、深度学习三维重建课程：基于深度学习的三维重建学习路线

8、激光定位+建图课程：详解最常用的激光SLAM框架Cartographer（Google团队开源）

链接：代码实战 | 用LeGO-LOAM实现地面提取[https://mp.weixin.qq.com/s/7G47_qBn7cRv9UtdvzlSSQ]

全国最棒的SLAM、三维视觉学习社区↓

链接：代码实战 | 用LeGO-LOAM实现地面提取[https://mp.weixin.qq.com/s/7G47_qBn7cRv9UtdvzlSSQ]

技术交流微信群

欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群，请添加微信号 chichui502 或扫描下方加群，备注：”名字/昵称+学校/公司+研究方向“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~

投稿、合作也欢迎联系：simiter@126.com

链接：代码实战 | 用LeGO-LOAM实现地面提取[https://mp.weixin.qq.com/s/7G47_qBn7cRv9UtdvzlSSQ]

扫描关注视频号，看最新技术落地及开源方案视频秀 ↓

链接：代码实战 | 用LeGO-LOAM实现地面提取[https://mp.weixin.qq.com/s/7G47_qBn7cRv9UtdvzlSSQ]

—   版权声明  —

本公众号原创内容版权属计算机视觉life所有；从公开渠道收集、整理及授权转载的非原创文字、图片和音视频资料，版权属原作者。如果侵权，请联系我们，会及时删除。