代码实战 | 用LeGO-LOAM实现BFS点云聚类和噪点剔除

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

作者丨Zach

来源丨计算机视觉life

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

LeGO-LOAM的软件框架分为五个部分：

1. 分割聚类：这部分主要操作是分离出地面点云；同时，对剩下的点云进行聚类，剔除噪声(数量较少的点云簇，被标记为噪声)；

2. 特征提取：对分割后的点云(排除地面点云部分)进行边缘点和面点特征提取；

3. Lidar里程计：在连续帧之间(边缘点和面点)进行特征匹配找到连续帧之间的位姿变换矩阵；

4. Lidar Mapping：对特征进一步处理，然后在全局的 Point Cloud Map 中进行配准；

5. Transform Integration：Transform Integration 融合了来自 Lidar Odometry 和 Lidar Mapping 的 pose estimation 进行输出最终的 pose estimate。

我们将详细介绍LeGO-LOAM是如何对地表上点云进行聚类分割，以便剔除噪声点。

LeGO-LOAM论文中对地表上点云进行聚类分割参考了论文《Fast Range Image-based Segmentation of Sparse 3D Laser Scans for Online Operation》，这篇文章的作者号称分割速度极快，到底有多快!大家可以看看原论文。

我们先学习一下这篇论文实现地表点云分割原理，再详细分析一下LeGO-LOAM框架是如何实现的。

地表点云聚类分割的原理

作者先把激光雷达扫面的数据投影到一个2D的深度图上(与LeGO-LOAM的 Range Map 一样)，深度图的长/横向是激光雷达单条 scan 扫描的点云，深度图的高/纵向是激光雷达的线束。需要注意的是，作者对横向数据进行了压缩。例如， 表示横向个点，条scan。

图1 Rang Map. 栅格里的值表示聚类值, -1表示无返回值.

有一点需要注意的是，2D的Range Map 左边界和右边界是相连的，因为激光雷达的扫描线是环形的。

图2 点云快速分割原理示意图

具体的分割方法如上图左下所示：， 为任意两点， 为激光雷达的位置， 为连续的两道光束，以较长的光束为 轴，连接 作直线， 为 轴与 的夹角，设置一个阈值 ，如果 ，我们认为两点 属于同一目标。 角的计算公示如下：

其中 是相连线束的夹角。

上图中右下图中绿色线段表示连接的是同一目标，红色线段连接的是不同目标。在实践过程中， 值为两点是否属于同一目标提供了有价值的判断。

上述方法也有一些缺陷，当激光雷达距离墙面很近时，墙上离激光雷达较远的点容易判定为另一个目标。作者认为这并不影响上述算法的实际有效性。从直观上来看，该方法确实能区分深度差异较大的点。

作者在整个分割聚类的流程中使用了 邻居的 BFS 搜索，极大的加快了聚类分割的速度，伪代码如下：

图3 Range Image Label

• 遍历Rang Map 上所有点(Line 1--8)

  • 遍历方式是逐行遍历，先行后列(Line 4--9);

  • 如果遇到未标记的像素，则执行BFS操作，并打标签(Line 6--8);

• 对每个点进行四领域BFS搜索(Line 9--19)

  • 建立一个队列，push种子点 (Line 10)

  • 从队列中取出一个点和该点标签，判断该点与其四领域是否是同类点(Line 12--17)；如果是同类点，则填入队列并打上标签(Line 18)

  • 从队列中删除刚刚取出的点(Line 19)

如果我们深层次的去思考的话，会发现BFS起到了在Rang Map 上的聚类，这样聚类出来的点云，要么同类簇点云，要么只是深度距离值存在明显差异的点云；然后再进一步使用角度阈值分离出在深度距离上存在明显差异的不同类点云；最后，对点云起到了一个很好的聚类分割效果。

LeGO-LOAM源码实现地表点云聚类分割

点云分割的主要流程是先进行地面提取(在上一篇文章中已进行说明)，然后对剩下的点云进行分割聚类，最后拿分割好的点云进一步进行特征提取。在这个过程之后，只保留大物体的点云，例如地面和树干(剔除了树叶和树枝等点云)，以供进一步处理。如下图所是：

图4 点云聚类分割效果图

上图(a) 是原始点云，图(b)是经过聚类分割后的点云，红色的点表示地面点，剩下的点是分割后的点云。

下面对照官方代码详细说明这个过程是如何实现的。

函数void cloudSegmentation()实现

点云分割的代码主要由:LEGO-LOAM/src/imageProjection.cpp文件中的函数void cloudSegmentation()实现。该函数的核心流程和作用如下图所示：

图5 函数void cloudSegmentation()流程

我们先从整理上来分析这个函数的作用，后细致分析里面的具体细节。

• Step 1： 按行遍历 RangMap 对所有的点进行聚类分割，分割的结果存储在labelMat中，这部分基本上实现了图3中的算法流程，稍后会对这部分进行详细的分析说明。

// 1. 按行遍历所有点进行分割聚类，更新labelMat
for (size_t i = 0; i < N_SCAN; ++i) {
    for (size_t j = 0; j < Horizon_SCAN; ++j) {
        // labelMat 存储了每个点的聚类标记
        if (labelMat.at(i,j) == 0) {
            // 对未被标记的点执行BFS, 同时进行聚类
            labelComponents(i, j); 
        }
    }
}

• Step 2： 经过 Step 1，就获得了点云分割的结果，其结果存储在labelMat中，其中同类点云具有相同的标号，噪声的标号为999999。代码如下：

// 2. 对噪声/地面点/分隔点进一步处理
int sizeOfSegCloud = 0;
// extract segmented cloud for lidar odometry
for (size_t i = 0; i < N_SCAN; ++i) {
    // 记录每一条scan中分割出的有效点起始index和终止index
    segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5;  

    for (size_t j = 0; j < Horizon_SCAN; ++j) {
        // 只处理有效的聚类点或者地面点
        if (labelMat.at(i, j) > 0 || groundMat.at(i, j) == 1) {
            // outliers that will not be used for optimization (always continue) 不用于优化的异常值
            // 2.1. 过滤掉所有的噪声点, 另作存储;
            if (labelMat.at(i, j) == 999999) { // 异常点
                // 是噪点，噪点横轴坐标是5的倍数就进行存储
                if (i > groundScanInd && j % 5 == 0) {
                    outlierCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                    continue;
                } else {
                    continue;
                }
            }

            // majority of ground points are skipped
            // 2.2. 压缩地面点, 只取五分之一的地面点进行存储;
            if (groundMat.at(i, j) == 1) {
                if (j%5 !=0 && j>5 && j