lego loam 代码阅读

前言

LEGO LOAM这篇文章发在18年的IROS上，因为其代码开源以及对设备性能要求低等优点，到现在依然用的比较多。论文里面主要是和LOAM对比，其相比LOAM具有以下三个特点，

• 轻量级，能在嵌入式设备上实时运行。
• 地面优化，在点云处理部分加入了分割模块，这样做能够去除地面点的干扰，只在聚类的目标中提取特征。
• 两步L-M优化法估计6个维度的里程计位姿，匹配方式还是Scan2Scan。

在UGV运动的场景中，由于地形的变化采集到的数据经常是畸变比较大的，并且UGV自身性能有限，难以负担过大的计算，在一些树木草丛较多的地方，基于几何特征的提取方法噪声太大。LEGO-LOAM有以下特点：

• 点云分割，有效的去除噪声点，保证提取特征的质量。
• 两步L-M优化，提取相邻帧的平面特征进行匹配用来计算$t_z、roll、pitch$，通过线特征匹配获取$t_x、t_y$和$yaw$。
• 回环检测和后端优化来修正里程计的偏移。
  

系统框架如下图所示:

本文主要分为四个章节去解析，主要对应着lego loam四个cpp文件。分别是

imageProjection
FeatureAssociation
mapOptmization
transformFusion

首先是对输入的原始点云进行点云分割（Segmentation），找到地面并且进行点云分割，接着对分割好的点云进行特征提取（Feature Extraction），找到面特征和边特征，提取出特征之后接下来进行特征匹配，并且输出位姿，最后对点云进行注册，生成全局地图，并且进行回环检测，对生成的地图进行优化。

我们首先要讲的就是imageProjection这个文件。

分割模块

分割模块，这里的主要思路是将点云投影成二维深度图，然后在图像上进行地面提取，对非地面点云进行聚类，然后在聚类簇中提取曲率较大的特征点，作者认为这样能够显著提升点云匹配的效率和精度。

在点云投影部分，这里参考的文献是以下两篇。采用的是VLP16的激光雷达，因为激光的线束为16，横向解析度为0.2，那么一圈360度就可以分成1800个角度的线束。因此投影的深度图尺寸为1800*16，其像素值存储的是当前点的深度。

地面分割部分，文章并没有假设地面是平坦的，根据深度图中相邻的列的高度变化，设定阈值，来判断每个像素点是否为地面点，这一步的分割比较粗糙。

聚类部分，这里只对非地面点进行BFS聚类，并在聚类簇上提取线特征，进一步选择曲率较大的30个点

imageProjection

imageProjecion.cpp为独立的线程，接收3D 点云数据，主要针对3D激光器采集的激光点云数据，因为代码中间所采用策略原理基本依赖于3D激光雷达扫描测距方式而来。
本线程主要工作是对3D点云进行预处理，主要将其地面分割、聚类、非聚类、无序点云变有效点云等功能；

点云预处理基本流程：

• 接收新的一帧cloud数据；
• 初始化临时变量数据；
• 将ros cloud 转换成pcl point格式；
• 采用pcl库剔除无效值；
• 统计雷达数据扫描起始角度和终止角度，包括角度差范围；
• 将无序的点云数据，根据3d雷达（如16线 velodyne）的基本扫描原理，转换为有序点云，包括水平index和垂直index。可- - 认为将3D点云转换为2维平面图像存储。
• 地面和非地面分割；
• 点云聚类
• 发布点云

参考文章链接:

1. lego-loam代码分析（1）-地面提取和点云类聚
2. 【论文阅读5】LEGO LOAM：可变地形下，轻量级和地面优化的激光里程计和建图