开源SLAM系统:LEGO-LOAM源码解析

之前看的SC LEGO LOAM的代码,在本篇记录一下,顺便对LEGO LOAM做一下总结。SC LEGO LOAM是在LEGO LOAM的基础上新增了基于Scan context的回环检测,其他流程完全一致,总体看来,scan context还是有一定的作用,主要是在回环检测的速度上有些许提升。整体感觉提升不大,最终精度还是由ICP来保证。这里很多部分和前面一篇LIO-SAM的代码是一致的,重复部分直接略过。

image-20200820220749138

SC-LEGO LOAM项目地址:https://github.com/irapkaist/SC-LeGO-LOAM

个人注释版地址:https://github.com/JokerJohn/opensource_slam_noted

概述

LEGO LOAM架构图如下:

image-20200820221038549

主要有点云分割、特征提取、激光里程计、地图优化等模块。其相比于LOAM主要有以下改进

  • 点云预处理部分,将点云投影成深度图,特征提取时,用图像的方法进行聚类和分割,只对聚类簇进行边缘点提取。并进一步选取了更突出的边缘点和更扁平的平面点,根据label进行匹配。
  • 在激光里程计部分,采用两步优化的方法估计位姿,第一步是利用平面点的匹配。优化roll、pitch和t_z的结果,并以此为初值,利用边缘点匹配优化tx、ty和yaw偏移,这样能显著减小迭代次数,比直接优化6个自由度的速度更快。
  • 摈弃LOAM的体素网格建图模式,添加基于欧氏距离+ICP的回环检测。

而SC LEGO LOAM则是在LEGO LOAM的基础上添加了基于scan context的回环检测。

其主要代码结构如下:

image_projection.cpp:点云投影成深度图,并聚类分割。

feature_association.cpp:线面特征提取、点云去畸变,估计位姿。

map_optmization.cpp:回环检测、地图优化、可视化。

transform_fusion.cpp:位姿融合,将里程计估计的位姿和图优化之后的位姿进行融合,输出最终位姿估计。

和之前解析的LIO-SAM代码重复比较多,重复部分不一一说明了。

其TF如下:

image-20200824194715813

分割

这里主要是把原始点云投影成二维深度图,并且对点云进行分割和聚类,分割主要是去除地面,聚类则是对非地面点进行聚类,方便后续边缘点的提取。

首先看其发布和订阅。

1
2
3
4
5
6
7
8
9
10
11
12
  // subscriber
subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic.c_str(), 1,
&ImageProjection::cloudHandler, this);

// publisher
pubFullCloud = nh.advertise<sensor_msgs::PointCloud2>("/full_cloud_projected", 1);
pubFullInfoCloud = nh.advertise<sensor_msgs::PointCloud2>("/full_cloud_info", 1);
pubGroundCloud = nh.advertise<sensor_msgs::PointCloud2>("/ground_cloud", 1);
pubSegmentedCloud = nh.advertise<sensor_msgs::PointCloud2>("/segmented_cloud", 1);
pubSegmentedCloudPure = nh.advertise<sensor_msgs::PointCloud2>("/segmented_cloud_pure", 1);
pubSegmentedCloudInfo = nh.advertise<cloud_msgs::cloud_info>("/segmented_cloud_info", 1);
pubOutlierCloud = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud", 1); // 离群点或异常点

最终处理后的点云分为以下六类

  • fullCloud:投影后的点云
  • fullInfoCloud:整体具有深度信息的点云
  • groundCloud:地面点云
  • segmentedCloud:分割后的部分点云
  • segmentedCloudPure:分割后点云的几何信息
  • outlierCloud:分割时的异常点

这里分割后的部分点云用自定义msg的存储,其消息结构如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
Header header 

int32[] startRingIndex # 点的起始的ring,用来计算扫描一帧时间内,点的相对位置,即:时间戳/扫描频率
int32[] endRingIndex # 扫描结束时的ring

float32 startOrientation # 当前帧的起始角度,扫描期间角度的变化量
float32 endOrientation
float32 orientationDiff

# 一些标志位,比如是否是地面点,以及在深度图中的索引
bool[] segmentedCloudGroundFlag # true - ground point, false - other points
uint32[] segmentedCloudColInd # point column index in range image
float32[] segmentedCloudRange # point range 点的深度

在cloudHandler中可以看到此模块的主流程,分别是以下几步:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
 // 1. Convert ros message to pcl point cloud
// 点云转换成pcl,去NAN
copyPointCloud(laserCloudMsg);
// 2. Start and end angle of a scan
// 计算一帧点云水平方向的起始旋转角度, 角度差/360 * 0.1用来得到每个点在当前帧时间内运动的相对时间
findStartEndAngle();
// 3.点云投影成深度图, 计算点的深度, 确定点的索引方式
projectPointCloud();
// 4.粗略标记地面点云
groundRemoval();
// 5. 点云分割,先聚类再分割,方便提取特征
cloudSegmentation();
// 6. Publish all clouds
publishCloud();
// 7. Reset parameters for next iteration
resetParameters();

点云预处理

这里在copyPointCloud函数中会进行去NAN点以及检查点云是否有ring信息。

findStartEndAngle函数中会记录激光雷达启动时在水平方向的起始角度,这样就可以计算出这一帧点云在扫描过程中转过的角度以及耗费时间。在这一帧的扫描时间内。车辆本身是继续向前运动的,所以会产生畸变,这个数据也可以在后续过程中去畸变。这个角度信息存储与一个全局变量,也就是之前自定义的消息结构cloud_msgs::cloud_info segMsg中。

1
2
3
4
5
6
7
8
9
10
11
12
13
void ImageProjection::findStartEndAngle() {
// 这个水平上看激光雷达启动时的初始角度,一圈下来应该和原始角度一致,实际运动中会有畸变
// start and end orientation of this cloud 计算角度时以x轴负轴为基准
segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
segMsg.endOrientation = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,
laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI;
// 最末角度为2π减去计算值
if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI) {
segMsg.endOrientation -= 2 * M_PI;
} else if (segMsg.endOrientation - segMsg.startOrientation < M_PI)
segMsg.endOrientation += 2 * M_PI;
segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation;
}

projectPointCloud函数主要是将点云投影成二维深度图,这里根据激光的横向解析度和线束来投影成1800*16的图像,其像素值存储的是点的深度。并且每一行1800个点并非都有数值,没有深度的单独标记出来,需要记录的还有每个点的像素坐标,即行号和列号。这里的列号即激光雷达的第几线,如果是vlp或者ouster的雷达,数据中会直接给出ring通道的信息,如果是其他雷达,则需要根据点的俯仰角,自行进行计算。此处理之后,一帧点云就变成有序的了

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
void ImageProjection::projectPointCloud() {
// 激光点云投影成二维图像,行表示激光线束数量,列表示每一个线上扫描到的点(0.1s扫描一圈,一个圆圈摊平就是360度)
// 计算点云深度,保存到深度图中
// range image projection
float verticalAngle, horizonAngle, range;
size_t rowIdn, columnIdn, index, cloudSize;
PointType thisPoint;

cloudSize = laserCloudIn->points.size();

for (size_t i = 0; i < cloudSize; ++i) {
thisPoint.x = laserCloudIn->points[i].x;
thisPoint.y = laserCloudIn->points[i].y;
thisPoint.z = laserCloudIn->points[i].z;
// find the row and column index in the iamge for this point
// 计算竖直方向上的点的角度以及在整个雷达点云中的哪一条水平线上
if (useCloudRing == true) {
// 用vlp的时候有ring属性
rowIdn = laserCloudInRing->points[i].ring;
} else {
// 其他lidar需要根据计算垂直方向的俯仰角,根据俯仰角大小判定当前点属于哪个线束
// 从上往下当前点属于第几线,16线一般俯仰角在-15到+15,每个线束俯仰角相隔2度。
verticalAngle =
atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
rowIdn = (verticalAngle + ang_bottom) / ang_res_y;
}
if (rowIdn < 0 || rowIdn >= N_SCAN)
continue;

// 激光雷达的水平解析度为0.2, 水平方向上的角度,一行 360/0.2=1800 个像素点
horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;

// round是四舍五入
columnIdn = -round((horizonAngle - 90.0) / ang_res_x) + Horizon_SCAN / 2;
if (columnIdn >= Horizon_SCAN)
columnIdn -= Horizon_SCAN;

if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
continue;

// 每个点的深度
range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
if (range < sensorMinimumRange)
continue;
// 在rangeMat矩阵中保存该点的深度,保存单通道像素值
rangeMat.at<float>(rowIdn, columnIdn) = range;

// 点的索引
thisPoint.intensity = (float) rowIdn + (float) columnIdn / 10000.0;

// 点云逐一计算深度,将具有深度的点云保存至fullInfoCloud intensity字段中
index = columnIdn + rowIdn * Horizon_SCAN; // index = 列数 + 行数×1800
fullCloud->points[index] = thisPoint;
fullInfoCloud->points[index] = thisPoint;
fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity"
}
}

地面去除

地面去除部分主要在深度图中进行,相邻的线束对应角度的点,如果是平面的话,起伏应该比较小,远处点噪声较大。因此选取前7个线束,计算这个俯仰角,判断每个点是否属于地面点。这里像素值-1 0 1分别代表无效点、非地面点和地面点。

img

如图,底部黑色粗实线代表路面,蓝色粗实线代表障碍物。P1和P2时同一列上相邻的两个激光点。此时∠a>0。当不存在障碍物时,P2落在地面上的P3位置,此时∠a=0\angle a = 0∠a=0。根据这个性质就可以把路面点给标记出来。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
for (size_t j = 0; j < Horizon_SCAN; ++j) {   // 每行
for (size_t i = 0; i < groundScanInd; ++i) { // 每列
// 只用前7个光束检测地面,远处的点不可靠
// lowerInd upperInd分别表示相邻光束扫描的相邻点,在深度图中的索引index
// 在深度中就是列前后两个点,比如(16,6)和(16,7),通过相邻光束对应点的俯仰角可以知道局部位置的高度起伏
// 进而判定其是否是地面点
lowerInd = j + (i) * Horizon_SCAN;
upperInd = j + (i + 1) * Horizon_SCAN;

// intensity在投影的时候已经计算好了
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;
}

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;

// 计算这两个点在z方向的俯仰角
angle = atan2(diffZ, sqrt(diffX * diffX + diffY * diffY)) * 180 / M_PI;

// 相邻圈对应点俯仰角小于10度认为是地面点
if (abs(angle - sensorMountAngle) <= 10) {
groundMat.at<int8_t>(i, j) = 1;
groundMat.at<int8_t>(i + 1, j) = 1;
}
}
}
// extract ground cloud (groundMat == 1)
// mark entry that doesn't need to label (ground and invalid point) for segmentation
// note that ground remove is from 0~N_SCAN-1, need rangeMat for mark label matrix for the 16th scan
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;
// 非地面点或者深度图中空点都设为-1
}
}
}

点云分割与聚类

在提取出地面点云后,对非地面的点云进行聚类,这么做的原因是考虑在一些树木或者草丛过多的地方,点云误差比较大,直接影响到后续特征的提取,这里聚类分割之后,可以减小特征的数量,提取滤出掉一些不可靠的特征点。这里聚类采用的是4邻居的BFS广度优先搜索算法,在labelComponents函数中以当前的(rows, cols)为中心,在深度图中扩散。先看一下伪代码,这里BFS聚类参考博文:https://zhuanlan.zhihu.com/p/72932303

img

这里的分割思路如图,左图中A、B为任意两点,O为雷达所在位置,OA、OB为连续的两道光束。过A、B作一条直线,以较长的光束为y轴,β为这道光束与AB的夹角。因此,可以设置一个阈值θ,当β>θ时,就判定这两个点不属于同一个物体。α 角表示雷达水平方向相邻两道光束之间的夹角,是已知的。d1和d2分别OA和OB的长度,在深度图内也是已知的,对于大多数目标上的点,β值都是很大的,只有在两个点深度值差很多的时候β才会比较小。因此通过设置一个阈值θ来进行点云分割是完全可行的。该方法仅仅只用了一个参数来进行点云分割的处理,非常简单,计算速度也很快。

image-20200907095934146

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
void ImageProjection::labelComponents(int row, int col) {
// use std::queue std::vector std::deque will slow the program down greatly
// 聚类
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;
int queueEndInd = 1;

allPushedIndX[0] = row;
allPushedIndY[0] = col;
int allPushedIndSize = 1;

// queueSize指的是在特征处理时还未处理好的点的数量,因此该while循环是在尝试检测该特定点的周围的点的几何特征
// 以当前点(row,col)为中心,在深度图中向四周扩散,进行聚类
while (queueSize > 0) {
// Pop point
fromIndX = queueIndX[queueStartInd];
fromIndY = queueIndY[queueStartInd];
--queueSize;
++queueStartInd;
// Mark popped point
labelMat.at<int>(fromIndX, fromIndY) = labelCount;

//检查上下左右四个邻点,判定其在xoz平面的俯仰角是否满足要求,否则纳入聚类簇
for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter) {
// 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;
// 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));

if ((*iter).first == 0)
alpha = segmentAlphaX;
else
alpha = segmentAlphaY;

// angle其实是该特点与某邻点的连线与XOZ平面的夹角,这个角代表了局部特征的敏感性
angle = atan2(d2 * sin(alpha), (d1 - d2 * cos(alpha)));

// 如果夹角大于60°,则将这个邻点纳入到局部特征中,该邻点可以用来配准使用
if (angle > segmentTheta) {
queueIndX[queueEndInd] = thisIndX;
queueIndY[queueEndInd] = thisIndY;
++queueSize;
++queueEndInd;

labelMat.at<int>(thisIndX, thisIndY) = labelCount;
lineCountFlag[thisIndX] = true;

allPushedIndX[allPushedIndSize] = thisIndX;
allPushedIndY[allPushedIndSize] = thisIndY;
++allPushedIndSize;
}
}
}

// check if this segment is valid
// 当邻点数目达到30后,则该帧雷达点云的几何特征配置成功
bool feasibleSegment = false;
if (allPushedIndSize >= 30)
feasibleSegment = true;
else if (allPushedIndSize >= segmentValidPointNum) {
// 聚类簇之后5-30个点时,统计垂直方向的点数lineCount,数量满足要求则认为此聚类粗有效
int lineCount = 0;
for (size_t i = 0; i < N_SCAN; ++i)
if (lineCountFlag[i] == true)
++lineCount;
if (lineCount >= segmentValidLineNum)
feasibleSegment = true;
}
// segment is valid, mark these points
if (feasibleSegment == true) {
++labelCount;
} else { // segment is invalid, mark these points
for (size_t i = 0; i < allPushedIndSize; ++i) {
labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
}
}
}

特征提取

特征提取模块主要是接收分割模块发布的segmented_cloud_info自定义的点云,进行特征提取,其中在地面点云中提取平面点,在聚类簇中提取边缘点。其中为了保证各个方向的点云均匀提取特征,将整个深度图在行方向上划分为6个子图,每个子图的分辨率为300*6,在每个子图上提取固定数量的特征,比如40个边缘特征和80个平面特征,其中分别选取粗糙度最大和最小的点标记为更突出的边缘点和更扁平的平面点。最后进行相邻帧的特征匹配,估计位姿。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
 subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/segmented_cloud", 1,  // 分割后的点云&FeatureAssociation::laserCloudHandler, this);
subLaserCloudInfo = nh.subscribe<cloud_msgs::cloud_info>("/segmented_cloud_info", 1, // 分割后带几何信息的点云
&FeatureAssociation::laserCloudInfoHandler, this);
subOutlierCloud = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud", 1, &FeatureAssociation::outlierCloudHandler, this);

subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 50, &FeatureAssociation::imuHandler, this); // imu数据处理

// publisher
pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 1);
pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 1);
pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 1);
pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 1);

pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);
pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);
pubOutlierCloudLast = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2);

pubLaserOdometry = nh.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 5);// odom

runFeatureAssociation函数中可以看到以下流程,

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
/**
1. Feature Extraction
*/
adjustDistortion(); // imu去畸变

calculateSmoothness(); // 计算光滑性

markOccludedPoints(); // 距离较大或者距离变动较大的点标记

extractFeatures(); // 特征提取

publishCloud(); // cloud for visualization

/**
2. Feature Association
*/
if (!systemInitedLM) {
checkSystemInitialization();
return;
}

updateInitialGuess(); // 更新初始位姿

// 一个是找特征平面,通过面之间的对应关系计算出变换矩阵。
// 另一个部分是通过角、边特征的匹配,计算变换矩阵。
updateTransformation();

integrateTransformation(); // 计算旋转角的累积变化量。

publishOdometry();

publishCloudsLast(); // cloud to mapOptimization
-------------    本文结束  感谢您的阅读    -------------
胡想成 wechat
欢迎关注个人公众号!
您的支持,是我装逼的最大动力!

本文标题:开源SLAM系统:LEGO-LOAM源码解析

文章作者:胡想成

发布时间:2020年09月20日 - 21:09

最后更新:2020年10月01日 - 22:10

原始链接:xchu.net/2020/09/20/52lego/

版权声明:本博客所有文章除特别声明外,均采用 CC BY-NC-SA 4.0许可协议,转载请注明出处!