LeGo-LOAM 跑通与源码学习

论文链接：https://www.researchgate.net/LeGO-LOAM

源码仓库：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM

本人注释：https://github.com/linzs-online/lego-LOAM_note.git

1、因为LeGo-LAOM的优化是基于gtsam库实习的，所以首先要安装该库

• Ubuntu 20.04 安装gtsam

git clone git@bitbucket.org:gtborg/gtsam.git
cd gtsam
mkdir build
cd build
cmake ..
make -j
make install

• 编译LeGo-LOAM

cd xx_ws/src
git clone https://github.com/RobustFieldAutonomyLab/LeGO-LOAM.git
cd ../..
catkin_make

OpenCV 版本问题报错

#include<opencv/cv.h>，修改为#include <opencv2/imgproc.hpp>

• 运行LeGo-LOAM

1、报错找不到libmetis-gtsam.so

error while loading shared libraries: libmetis-gtsam.so: cannot open shared object file: No such file or directory

解决办法：安装libparmetis

sudo apt install libparmetis-dev

2、tf出warning，找不到变换，rviz黑屏

解决办法：tf2默认不需要在frame前面已经加了/，所以这里将代码中所有的/camera和/camera_init的/去掉

LeGO与LOAM的不同点

LeGO-LOAM是LOAM的升级版，LeGO-LOAM相比LOAM的改进有如下几点：

1、对点云进行了两大类划分：地面点云、非地面聚类点云

2、激光里程计部分采用两步优化的方式得到6ToF的位姿估计，相比LOAM减少了冗余计算，提升了速度

3、采用关键帧进行局部地图和全局地图管理，后端采用因子图iSAM优化的方式进行了位姿优化

LeGO-LOAM整体框架

• imageProjection节点负责将原始点云投影为深度图，并利用深度图把点云分割为地面点和目标点聚类。

1、计算点云的水平角度、垂直角度，把点云投影到深度图rangeMat中，深度图为16 $\times$ 1800。每个scan有1800个点，对于16线的雷达有16条scan line。

2、遍历深度图中的点，对于地面上的点云（scan line 为 0-7）分割为groundMat，groundMat中 -1 代表无效点、0代表虽然是在scan line 为 0-7但是并不是地面点，1代表地面点

3、对非地面的点进行聚类，通过判断邻域与雷达原点构成的∠OAB的大小判断是否是属于同一个类别。填充labelMat，labelMat为-1则代表是地面的点，1-n代表着聚类之后的不同的类别。把能聚成类的点统一存放到segmentedCloudPure中，后面用来提取边线点特征

原始点云图

原始的点云地图中的激光点的数量太多，下面进行点云分割，分离地面点和非地面点

分离了地面的点云，然后进行了一次聚类之后的点云 分离出来的地面点云

• featureAssociation节点负责从分割点中提取线面特征，并进行scan-to-scan的特征匹配

1、对每个点云进行插值，用点云前后两个时刻的IMU信息，根据该点云离这两个时刻的时间段在这两个时刻的时间段的占比进行插值

2、计算每个点的曲率，并记遮挡点（比如：一颗树后面还有一辆车，如果不剔除这些点，因为深度的跳变那么会使得在树干的边缘的点也会被检测为边线特征，这显然是不合理的）

3、提取四种特征，和LOAM一样，这里也把每个scan分成6个区域进行特征提取，每个区域提取2个边线点（在非地面点中提取）、4个平面点（在地面点中提取）

可以看到，对应平面特征点只在地面点云中进行提取，边线特征点只在非地面点云中提取。

4、根据特征点构建KDtree，这里构建了两棵KDtree，一个是边线点的，一个是平面点的

5、两步LM优化。这里平面点就去平面点对应的KDtree中查找，边线点就去边线点对应的KDtree中查找，（这样显然比LOAM中的要更快找到，LOAM中是把所有的特征点用来构造KDtree），先用平面点对$\left[t_z,t_{\text{roll }},t_{\text{pitch }}\right]$ 进行优化，然后在第一步优化的结果上再用边线点对 $\left[t_x,t_y,t_{yaw}\right]$ 进行优化，这样很快就可以优化出6ToF的位姿，这相对于LOAM是一个比较大的进步。另外，这里并没有使用Ceres优化库，这里是手动构造非线性最小二乘求解的方程 J^(T) * J * f(x)= -J^(T) * f(x)，调用的是OpenCV里面的矩阵方程求解API（cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR)）

6、之后就是积分姿态结果，发布里程计的结果以及点云

• mapOptmization节点负责回环检测、因子图优化、可视化

1、首先根据里程计输出的上一帧到当前帧的变换给当前位姿优化赋一个初始值

2、提取关键帧，构建子地图。如果开启了回环检测，那么就选取过去的50个关键帧作为子地图，如果没有开启回环检测，那么就选取当前位置50m以内的帧构建子地图

3、对于关键帧的选取标准是：机器人移动相较上一帧位置大于0.3m才会把当前帧作为关键帧标志。之后就是构建因子图进行优化。

4、根据因子图优化的结果修正所有关键帧的位姿，并发布出去

• transformFusion节点主要负责整合前面优化的结果发布TF变换

下图是本人根据源码梳理出来的一个思维导图，里面基本包含了一些技术细节。