提供了编程的基础技术教程

zl程序教程

您现在的位置是：首页 >  其他

当前栏目

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

代码
2023-03-15 22:02:41 时间

编辑丨计算机视觉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

相关文章