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

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

干货第一时间送达

作者丨Zach

来源丨计算机视觉life

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

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

分割聚类：这部分主要操作是分离出地面点云；同时，对剩下的点云进行聚类，剔除噪声(数量较少的点云簇，被标记为噪声)；
特征提取：对分割后的点云(排除地面点云部分)进行边缘点和面点特征提取；
Lidar里程计：在连续帧之间(边缘点和面点)进行特征匹配找到连续帧之间的位姿变换矩阵；
Lidar Mapping：对特征进一步处理，然后在全局的 Point Cloud Map 中进行配准；
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<int>(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和终止indexsegMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5;  for (size_t j = 0; j < Horizon_SCAN; ++j) {// 只处理有效的聚类点或者地面点if (labelMat.at<int>(i, j) > 0 || groundMat.at<int8_t>(i, j) == 1) {// outliers that will not be used for optimization (always continue) 不用于优化的异常值// 2.1. 过滤掉所有的噪声点, 另作存储;if (labelMat.at<int>(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<int8_t>(i, j) == 1) {if (j%5 !=0 && j>5 && j<Horizon_SCAN-5)continue;}// 2.3. 保留所有的有效分割点;// mark ground points so they will not be considered as edge features later// 标记地面点，以便以后不会将其视为边缘特征segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1);// mark the points' column index for marking occlusion later// 标记点的列索引, 稍后标记遮挡segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;// save range info 保存范围信息segMsg.segmentedCloudRange[sizeOfSegCloud]  = rangeMat.at<float>(i,j);// save seg cloud 保存分割点云segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]); // 既有分割的地表点云，又有地面点云// size of seg cloud 更新分割点云数量++sizeOfSegCloud;}}segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;
}

代码采取按行遍历，通过条件语句 if (labelMat.at<int>(i, j) > 0 || groundMat.at<int8_t>(i, j) == 1) 限制了处理对象只包括地面点云 和 分割点云。

2.1 过滤掉所有的噪声点

if (labelMat.at<int>(i, j) == 999999) { // 异常点// 是噪点，噪点横轴坐标是5的倍数就进行存储if (i > groundScanInd && j % 5 == 0) {outlierCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);continue;} else {continue;}
}

上述代码表面，如果是噪声点，则不执行双层for循环的余下代码，相当于过滤掉这些噪声点。把噪声点另外存储在 outlierCloud 中，存储的时候时压缩存储的，只有遇到点的横轴坐标遇整除才进行存储。

2.2 压缩地面点

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

如果检测到的是地面点，会过滤掉绝大部分的地面点，只保留横轴遇整除的点作进一步处理。

2.3 保留所有有效分割点和压缩后的地面点

// 2.3. 保留所有的有效分割点;
// mark ground points so they will not be considered as edge features later
// 标记地面点，以便以后不会将其视为边缘特征
segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1);
// mark the points' column index for marking occlusion later
// 标记点的列索引, 稍后标记遮挡
segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
// save range info 保存范围信息
segMsg.segmentedCloudRange[sizeOfSegCloud]  = rangeMat.at<float>(i,j);
// save seg cloud 保存分割点云
segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]); // 既有分割的地表点云，又有地面点云
// size of seg cloud 更新分割点云数量
++sizeOfSegCloud;

凡是没有过滤掉的点，都执行上述的数据保存工作。最后的 segmentedCloud 保存的既有稳定的分割点(剔除噪声点)，又有压缩的地面点。

Step 3： 给分割的点进行着色，同类点云的强度值相同。

// 3. 可视化分割后的点云(同类点云分配相同的强度值)，不包括地面点云
// extract segmented cloud for visualization
if (pubSegmentedCloudPure.getNumSubscribers() != 0) {// 遍历range map 中的点云                                                               for (size_t i = 0; i < N_SCAN; ++i){for (size_t j = 0; j < Horizon_SCAN; ++j) {if (labelMat.at<int>(i,j) > 0 && labelMat.at<int>(i,j) != 999999){// 给点云分配强度值: 同类点云, 强度值相同segmentedCloudPure->push_back(fullCloud->points[j + i*Horizon_SCAN]);segmentedCloudPure->points.back().intensity = labelMat.at<int>(i,j);}}}
}

通过限制条件可知，上述代码只给分割点进行着色，不包括地面点云。

在函数 void cloudSegmentation()中，除了 Step 1 部分外，其他的都很好理解。Step 1 里面的核心部分是函数void labelComponents(int row, int col)，其包含了使用BFS进行聚类，使用角度判断是否同属一类，还涉及到一些经验参数的设置。但是，大体上该函数的实现是依据论文《Fast Range Image-based Segmentation of Sparse 3D Laser Scans for Online Operation》而来。如果文章的前半部分看懂了，理解这部分代码应该没有什么问题。

函数 `void labelComponents(int row, int col)` 实现

函数 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 pointlabelMat.at<int>(fromIndX, fromIndY) = labelCount;// Loop through all the neighboring grids of popped gridfor (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){// 2.1. 提供有效的邻居点(正确的坐标和未被标记)// new indexthisIndX = 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<int>(thisIndX, thisIndY) != 0)continue;// 2.2. 计算d1(长边)和d2(短边)d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY), rangeMat.at<float>(thisIndX, thisIndY));d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY), rangeMat.at<float>(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;// 标记此点与中心点同样的labellabelMat.at<int>(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<int>(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<int>(thisIndX, thisIndY) != 0)continue;

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

2.2 计算d1和d2的边长

图7 计算的d1和d2

计算和代码实现如下：

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

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

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

如果所选的点是左、右点，则为 alpha = segmentAlphaX ，是；如果选择是上，下点，则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;// 标记此点与中心点同样的labellabelMat.at<int>(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;
}

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

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<int>(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》

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

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天内无条件退款

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

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