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

函数 void labelComponents(int row, int col) 的形参是一个点在 range map 上的横纵坐标，该函数就是以点[row, col] 为中心，进行BFS(广度优先搜索)，在BFS的基础之上进行角度对比以进一步判断两点是否为同类点。最终的结果是，获得点[row, col] 所在区域的同类点，结果更新在 labelMat 上。

在详细分析代码之前，为了缕清结构，我们先看一下大致的代码流程：

图6 函数void labelComponents()流程

• Step 1： 初始化队列信息

// 1. 使用C++ 容器会导致计算速度变慢, 作者使用数组模拟了一个队列
float d1, d2, alpha, angle;
int fromIndX, fromIndY, thisIndX, thisIndY; 
bool lineCountFlag[N_SCAN] = {false};

// 使用数组模拟队列
queueIndX[0] = row;
queueIndY[0] = col;
int queueSize = 1;
int queueStartInd = 0; // pop 数据的哨兵
int queueEndInd = 1; // push 数据的哨兵

// 存储分割的点云
allPushedIndX[0] = row;
allPushedIndY[0] = col;
int allPushedIndSize = 1; // 记录分割点云的点数量

作者在此处使用了两个数组来模拟队列的功能，不使用C++容器的原因是，当容器容量不够是，C++会进行两倍扩容；同时存储效率也没有数组高。这部分代码把形参当作第一个点传入队列中，设置好队列的 pop 哨兵和 push 哨兵，并初始化数组allPushedIndX 和 allPushedIndY。

• Step 2 使用BFS进行点云分割 这段代码最好对照图3**的伪代码流程进行查看。

// 2. 进行BFS点云分割
while(queueSize > 0) {
    // Pop point 提取一个数据
    fromIndX = queueIndX[queueStartInd];
    fromIndY = queueIndY[queueStartInd];
    --queueSize;
    ++queueStartInd;
    // Mark popped point
    labelMat.at(fromIndX, fromIndY) = labelCount;
    // Loop through all the neighboring grids of popped grid
    for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){
        // 2.1. 提供有效的邻居点(正确的坐标和未被标记)
        // new index
        thisIndX = fromIndX + (*iter).first; // 纵轴坐标
        thisIndY = fromIndY + (*iter).second; // 横轴坐标
        // index should be within the boundary
        // 避免纵轴坐标正确
        if (thisIndX < 0 || thisIndX >= N_SCAN)
            continue;
        // at range image margin (left or right side)
        // 调整横轴边界数据是相连的
        if (thisIndY < 0)
            thisIndY = Horizon_SCAN - 1;
        if (thisIndY >= Horizon_SCAN)
            thisIndY = 0;
        // prevent infinite loop (caused by put already examined point back)
        // 保证所取点的有效性:必须是未被标记的点
        if (labelMat.at(thisIndX, thisIndY) != 0)
            continue;

        // 2.2. 计算d1(长边)和d2(短边)
        d1 = std::max(rangeMat.at(fromIndX, fromIndY), 
                      rangeMat.at(thisIndX, thisIndY));
        d2 = std::min(rangeMat.at(fromIndX, fromIndY), 
                      rangeMat.at(thisIndX, thisIndY));

        // 2.3. 根据所取点的位置设置alpha值
        if ((*iter).first == 0) // 邻居点与中心点同一条扫描线上
            alpha = segmentAlphaX;
        else // 邻居点与中心点在相连的扫描线上
            alpha = segmentAlphaY;
        
        // 2.4. 计算角度, 并根据阈值分类
        angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
        if (angle > segmentTheta){ // 此处segmentTheta = 60.0
            // 把此有效点push入队列中
            queueIndX[queueEndInd] = thisIndX;
            queueIndY[queueEndInd] = thisIndY;
            ++queueSize;
            ++queueEndInd;

            // 标记此点与中心点同样的label
            labelMat.at(thisIndX, thisIndY) = labelCount;
            lineCountFlag[thisIndX] = true;

            //追踪分割的点云
            allPushedIndX[allPushedIndSize] = thisIndX;
            allPushedIndY[allPushedIndSize] = thisIndY;
            ++allPushedIndSize;
        }
    }
}

代码：

// Pop point 提取一个数据
fromIndX = queueIndX[queueStartInd];
fromIndY = queueIndY[queueStartInd];
--queueSize;
++queueStartInd;
// Mark popped point
labelMat.at(fromIndX, fromIndY) = labelCount;

从队列中 pop 出一个点，并给该点标记 labelCount。

代码 for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter) 是以给定的形参点 [row, col] 为中心，进行上下左右四邻居遍历。

2.1 提供有效的邻居点

// 2.1. 提供有效的邻居点(正确的坐标和未被标记)
// new index
thisIndX = fromIndX + (*iter).first; // 纵轴坐标
thisIndY = fromIndY + (*iter).second; // 横轴坐标
// index should be within the boundary
// 避免纵轴坐标正确
if (thisIndX < 0 || thisIndX >= N_SCAN)
    continue;
// at range image margin (left or right side)
// 调整横轴边界数据是相连的
if (thisIndY < 0)
    thisIndY = Horizon_SCAN - 1;
if (thisIndY >= Horizon_SCAN)
    thisIndY = 0;
// prevent infinite loop (caused by put already examined point back)
// 保证所取点的有效性:必须是未被标记的点
if (labelMat.at(thisIndX, thisIndY) != 0)
    continue;

遍历上下左右的邻居点，有可能出现以下情况：1. 纵轴坐标越界，则跳过该点；2. 横轴坐标越界，则调整横轴坐标，因为range map 的左右边界是相连的；3. 所取得点如果已经被标记，则跳过该点。

2.2 计算d1和d2的边长

图7 计算的d1和d2

计算 d1和d2 代码实现如下：

// 2.2. 计算d1(长边)和d2(短边)
d1 = std::max(rangeMat.at(fromIndX, fromIndY), 
              rangeMat.at(thisIndX, thisIndY));
d2 = std::min(rangeMat.at(fromIndX, fromIndY), 
              rangeMat.at(thisIndX, thisIndY));

2.3 根据所取点的位置设置alpha值

// 2.3. 根据所取点的位置设置alpha值
if ((*iter).first == 0) // 邻居点与中心点同一条扫描线上
    alpha = segmentAlphaX;
else // 邻居点与中心点在相连的扫描线上
    alpha = segmentAlphaY;

2.4. 计算角度, 并根据阈值分类

// 2.4. 计算角度, 并根据阈值分类
angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
if (angle > segmentTheta){ // 此处segmentTheta = 60.0
    // 把此有效点push入队列中
    queueIndX[queueEndInd] = thisIndX;
    queueIndY[queueEndInd] = thisIndY;
    ++queueSize;
    ++queueEndInd;

    // 标记此点与中心点同样的label
    labelMat.at(thisIndX, thisIndY) = labelCount;
    lineCountFlag[thisIndX] = true;

    //追踪分割的点云
    allPushedIndX[allPushedIndSize] = thisIndX;
    allPushedIndY[allPushedIndSize] = thisIndY;
    ++allPushedIndSize;
}

最后，计算角度值，并与角度阈值作比较。如果大于角度阈值，则表示所遍历的邻居点与所选取的点是同类点云，标记此邻居点为 labelCount，并填充到 allPushedIndX/Y 中。

直到所构建的队列为空为止。

• Step 3： 检测分类结果是否正确

// 3. 判断分隔是否有效
// check if this segment is valid 
bool feasibleSegment = false;
// 如果分割出的点云个数大于30个则认为是此分割是有效的
if (allPushedIndSize >= 30) 
    feasibleSegment = true;
else if (allPushedIndSize >= segmentValidPointNum){ // 如果分割的点云数量不满足30且大于最小点数要求，则进一步分析
    int lineCount = 0;
    // 统计行数
    for (size_t i = 0; i < N_SCAN; ++i)
        if (lineCountFlag[i] == true)
            ++lineCount;
    // 如果行数大于3，则认为也是有效的点云分割
    if (lineCount >= segmentValidLineNum)
        feasibleSegment = true;            
}

获取分割结果之后，我们还需要作进一步的验证，如果所分割的点大于30个，则认为此次分割结果是有效的；如果分割的点数小于30 ，大于5，则要作进一步的分析；如果，分割的点所占据的行数大于3 ，也认为此次分割是有效的。因为像树干这类目标，水平点的分布很少，但是竖直点的分布较多，激光雷达在垂直方向的分辨率又比较低，所以，这个地方的线束阈值设为3 。这样可以有效排除掉树叶，树枝等特征不稳定的点云。

• Step 4： 后续处理

// 4. 如果分隔有效, 则更新labelCount; 如果无效, 则标记为噪声
// segment is valid, mark these points
if (feasibleSegment == true){ // 如果是有效分割, 则更新labelCount
    ++labelCount;
}else{ // segment is invalid, mark these points, 如果分割是无效的, 则标记为无效标签值(很大的标签值, 即位噪点)
    for (size_t i = 0; i < allPushedIndSize; ++i){
        labelMat.at(allPushedIndX[i], allPushedIndY[i]) = 999999; // 
    }
}

如果分割结果是有效的，则累加 labelCount；如果分割结果是无效的，则把此处分割的点标记为噪点。

最终呈现出来的效果图如下：

我们只勾选左侧的 Segmentation Pure，右侧就呈现出来了分割点云（不包括地面点）不同的聚类效果，不同的颜色代表不同的类别。

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

2. 《Fast Range Image-based Segmentation of Sparse 3D Laser Scans for Online Operation》

3. 基于LiDAR的多传感器融合SLAM：LOAM、LeGO-LOAM、LIO-SAM

独家重磅课程！

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

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

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

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

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

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

扫描关注视频号，看最新技术落地及开源方案视频秀 ↓
链接：代码实战 | 用LeGO-LOAM实现BFS点云聚类和噪点剔除

— 版权声明 —

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