代码实战 | 用LeGO-LOAM实现地面提取
代码实战 | 用LeGO-LOAM实现地面提取
作者介绍：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线激光雷达的所有扫描点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] 。
参考资料
- https://github.com/RobustFieldAutonomyLab/LeGO-LOAM
作者来自我们课程学员：基于LiDAR的多传感器融合SLAM：LOAM、LeGO-LOAM、LIO-SAM
— 版权声明 —
本公众号原创内容版权属计算机视觉life所有；从公开渠道收集、整理及授权转载的非原创文字、图片和音视频资料，版权属原作者。如果侵权，请联系我们，会及时删除。