lego loam 代码阅读

短命女 2022-09-09 11:46 124阅读 0赞

### 前言 ###

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

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

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

*  **点云分割**，有效的去除噪声点，保证提取特征的质量。
 *  **两步L-M优化**，提取相邻帧的平面特征进行匹配用来计算 t z 、 r o l l 、 p i t c h t\_z、roll、pitch tz、roll、pitch，通过线特征匹配获取 t x 、 t y t\_x、t\_y tx、ty和 y a w yaw yaw。
 *  **回环检测**和**后端优化**来修正里程计的偏移。  
    ![在这里插入图片描述][watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_17_color_FFFFFF_t_70_g_se_x_16_pic_center]

**系统框架如下图所示:**  
![在这里插入图片描述][watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_20_color_FFFFFF_t_70_g_se_x_16_pic_center]

本文主要分为四个章节去解析，主要对应着`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）-地面提取和点云类聚][lego-loam_1_-]
2.  [【论文阅读5】LEGO LOAM：可变地形下，轻量级和地面优化的激光里程计和建图][5_LEGO LOAM]

[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_17_color_FFFFFF_t_70_g_se_x_16_pic_center]: /images/20220829/f21de3a30bd14f3380b4f1f416c93be2.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_20_color_FFFFFF_t_70_g_se_x_16_pic_center]: /images/20220829/383ae5fd49754aed8a3193b9481ec61c.png
[lego-loam_1_-]: https://blog.csdn.net/jiajiading/article/details/110948284
[5_LEGO LOAM]: http://xchu.net/2020/09/04/2legoloam/#more