Lego-LOAM文章详细解读

文章/代码下载地址

有空了继续更新方法的细节……

打个小广告：我做了一个纯LiDAR SLAM方法，已经开源。开源地址：
链接: Optimized-SC-F-LOAM

再再再打个小广告：我做了一个回环检测并与LIO-SAM结合起来，效果优于SC-LIO-SAM，已经开源。开源地址：
链接: NDTMC-LIO-SAM
链接: NDTMC

链接: Lego-LOAM
如果网络延迟过高，可以采用以下方式加速下载（选择一个即可）：

git clone https://hub.fastgit.org/RobustFieldAutonomyLab/LeGO-LOAM.git
git clone https://github.91chi.fun//https://github.com/RobustFieldAutonomyLab/LeGO-LOAM.git
git clone https://github.com.cnpmjs.org/RobustFieldAutonomyLab/LeGO-LOAM.git

一、框架概述（System Overview）

整个lego-loam系统被分为四个部分：
1.Segmentation：输入一帧点云，将点云投影到（projects onto）范围图（range image）用于分割。
2.Feature Extraction：输入分割后的点云，对点云进行特征提取。
3.LiDAR Odometry：输入提取了特征后的点云，计算小车在两帧之间的位姿变换。
4.LiDAR Mapping：输入激光里程计信息于点云信息，优化里程计并将点云信息转化到全局坐标系（global coordinate）。

二、Segmentation

1.range image

如图所示，假设该表格就是一个range image，机械式激光雷达一帧点云数据每一个点都可以在range image中找到与之对应的小方格，且这个小方格所代表的值为该点距离激光雷达的距离。

2.地面点云分割

由于由于激光雷达水平分辨率为16即16线激光雷达，所以，通过判断其竖直维度的特性，可以很好的标记出地面点和非地面点。通过VLP-16的激光扫描束的范围为[-15,15]度 ，地面点必然出现在 [-15,1]度扫描线上。

这个是一个常识，因为激光束如果相对水平面向上了，永远也不可能照射到水平地面；但是这里并不假设地面都为水平的，所以选择[-15,1]度之间的激光束作为地面点。

3.不同类的分割

将剩余点云采用 基于图像的分割方法 分割为不同的类。
基于图像的分割方法：这里可以理解为广度优先搜索方法bfs。

/** 伪代码 **/
queue Queue_of_a_Segment;
Queue_of_a_Segment.push(cur_point_idx);//用一个队列保存当前点的索引，其中X和Y分别代表该点在垂直和水平分辨率上的位置
int start = 0;
int size = Queue_of_a_Segment.size();
while(size > 0){
	int ind_X =  Queue_of_a_Segment[start].X;
	int ind_Y =  Queue_of_a_Segment[start].Y;
	size--;
	start++;
	labelMat.at<int>(ind_X, ind_Y) = labelCount;//当前点的标签
	for(){
		从该点的上下左右四个点进行搜索，如果满足两点之间的角度阈值则将该点添加到队列Queue_of_a_Segment中，size++,并且将属于一个segment的标签设置为相同的数（该数与你具体有多少个类有关）
	}
}

4.保存的信息

储存每个被保留下的点的三个信息：1.地面点和分割点的标签；2.它在深度图中的行列索引；3.它的深度值

三、特征提取

lego-loam采用的方法与loam中几乎相同，但是具体的形式不同。lego-loam是选取 t时刻的点云 Pt 中的一个点 pi ，在点 pi同一行（即激光雷达某一线）方向上，左右各找5个点，利用这些点的深度值（在二、4中所保存的），计算粗糙度。

将一帧点云数据按照每一线来选去edge和surf，并且每一线氛围6段来选择。根据粗糙度选取edge和surf：
edge点具体选择方式为：1.如果当前点未被选择；2.当前点的粗糙度c大于阈值；3.当前点不属于地面类。每一线的每一段中选择2个最大c值的edge点。
surf点具体选择方式为：1.如果当前点未被选择；2.当前点的粗糙度c小于阈值；3.当前点属于地面类。每一线的每一段中选择4个具有最小c值的surf点。
所有满足edge条件的点，在每一线每一段中最多保留40个；所有满足surf条件的点，在每一线每一段中最多保留80个；

四、雷达里程计

采用与LOAM相同的点到线、点到面的帧间匹配方法。
与LOAM不同的是：
1）label matching：在查找当前帧点云对应特征点时需要首先考虑它们是否属于同一类。当前帧平面特征点所对应的特征点都被标记为地面点（ground points）。对于当前帧边界点，则在与其具有相同标签的簇中进行查找。这样可以增加匹配的效率。
2）Two-step L-M Optimization:在LOAM中计算两帧之间的位姿变换采用LM方法最小化所有点到线和点到面距离所构成的非线性方程。但是在LeGO-LOAM中，采用一个两步优化法对位姿$T$进行求解。(1)首先利用当前帧的平面点到上一帧对应平面的距离，来计算6自由度中的$[t_z,{\theta }_{roll},{\theta }_{pitch}]$，(2)采用当前帧特征点到上一帧中对应点构成的特征线的距离计算剩下的位姿$[t_x,t_y,{\theta }_{yaw}]$。

五、建图

如图所示，假设红色圆圈处为当前帧在上一节中估计得到的位姿，那么红色方框内则为这一节所需要的子地图范围

与LOAM采用的方法相同：
1）首先利用 四 中得到的位姿将当前帧点云投影到世界坐标系下
2）将世界坐标系下的当前帧点云与一个子地图进行匹配（计算位姿变换所采用的方法与四相同）。子地图的选择：以四中得到的当前帧在世界坐标系下的位姿为中心画出一个边长为100m的正方形，将x，y值在其中的世界坐标系下的点云选出，作为子地图。

全文结束。