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

编辑丨计算机视觉life

作者介绍：Zach，移动机器人从业者，热爱移动机器人行业，立志于科技助力美好生活。

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

地面提取的思路

如上图所示，相连的两条扫描线打在地面同一列的两点A()和B()。计算A，B两点高度差 ，平面距离 ，计算 和 构建的三角角度；如果 ，则认为两点 ，属于地面点。

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

地面提取源码实现

在查看地面提取源码之前，先看一下几个关键性的参数。在文件 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，论文中把线激光雷达的所有扫描点 构成了一个 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;

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

// 如果所取的两点存在无效点, 该点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的高度差与平面距离的夹角 ，如果小于 ，则在 groundMap对应的位置置。

// 获取两点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 显示出来。如下图所示，我们为了明显区分地面点云，把它的颜色调整为了 。

参考资料

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

本文仅做学术分享，如有侵权，请联系删文。

3D视觉精品课程推荐：

1.面向自动驾驶领域的多传感器数据融合技术

2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
4.国内首个面向工业级实战的点云处理课程
5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

9.从零搭建一套结构光3D重建系统[理论+源码+实践]

10.单目深度估计方法：算法梳理与代码实现

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~