A-LOAM源码阅读

LOAM 论文地址：https://www.ri.cmu.edu/pub_files/2014/7/Ji_LidarMapping_RSS2014_v8.pdf

A-LOAM地址：https://github.com/HKUST-Aerial-Robotics/A-LOAM

本人注释的A-LOAM：https://github.com/linzs-online/A-LOAM_notes.git

在本文中不再贴出代码进行句句阐述，详细的代码注释可以产看仓库中，这里仅对每个模块的核心内容进行总结，一方面可以帮助刚刚接触这个领域的同学对整个工程有一个初步的认识；另一方面对于比较熟悉的同学快速回顾LOAM中的核心思路。希望能帮到大家，如有错误欢迎指出。

一、跑通A-LOAM

源码地址：https://github.com/HKUST-Aerial-Robotics/A-LOAM

依赖库：Ceres Solver、PCL

sudo apt install libpcl-dev

Ubuntu 20.04 + noetic 编译中遇到的问题：

1、PCL库报错

最新的PCL库需要C++14支持，所以在CMakeLists.txt中：

• set(CMAKE_CXX_FLAGS "-std=c++11") 改为 set(CMAKE_CXX_FLAGS "-std=c++14")

2、OpenCV 4中的一些宏定义冲突

• 将scanRegistration.cpp中的 #include <opencv/cv.h> 修改为#include <opencv2/imgproc.hpp>

• 修改kittiHelper.cpp中 CV_LOAD_IMAGE_GRAYSCALE为 cv::IMREAD_GRAYSCALE

二、代码框架

LOAM作为经典的3D激光SLAM的框架，它的结构比较清晰和简洁，首先是Lidar的输入，然后对点云进行整理；之后是激光里程计部分，这部分主要是scan-to-scan，匹配相邻两帧的特征得到相邻两帧的变换；之后地图构建，这部分主要是scan-to-map，通过当前帧去匹配一个局部地图，进一步得到更准确的位姿。

下面是A-LOAM运行时的节点流图，整个结构还是非常清晰的，从订阅原始的点云数据到最后的位姿输出，基本上和论文里面的结构是一致的。

三、代码总览

• kittiHelper.cpp

这个文件主要是把KITTI数据集转换为rosbag格式的bag文件。因为KITTI数据集本身并不是rosbag格式的，里面的很多个文件夹，分别存放在image、lidar、IMU等数据，ROS没办法之间使用这种数据集格式，所以这个节点主要是用来做数据格式转换的转换成xxx.bag文件，这样ros就可以用rosbag把消息播放出来

• scanRegistration.cpp

这个文件主要是把点云数据进行整理，把特征进行提取、分类。

1. 激光雷达点云原始数据的无序的，首先通过计算垂直角度，把点云按雷达scan line来进行存储
2. 接着给每个点云分配所在scan中的时间，计算point.intensity，这是为后面里程计匹配的时候做准备的
3. 计算每个点的曲率，提取出四种特征 cornerPointsSharp、cornerPointsLessSharp、surfPointsFlat、surfPointsLessFlat

• laserOdometry.cpp

这个文件主要是实现激光里程计的功能，通过ICP匹配相邻两帧的特征，求得相邻两帧的位姿变换。在A-LOAM中使用了Ceres优化库，代码可以写得简洁

1. 遍历当前帧边线特征点，对于当前特征点i，从上一帧的kdtree中找一个最近邻点j 和 次近邻点 l，最小化点i到 jl 的距离
2. 遍历当前帧平面特征点，对于当前特征点i，从上一帧的kdtree中找一个三个近邻点，最小化点i到近邻点构造的平面的法向量

• laserMapping

这个文件主要是实现地图的构建，因为里程计得到的位姿是有累积误差的，在此处通过Scan-to-map的方式进一步修正位姿。下图就很生动的表达了scan-to-map这个过程

主要方法是通过Submap和localmap的ICP把里程计的位姿进一步优化，相当于融合了近期的好几帧的信息，而不是像里程计一样只利用相邻两帧的信息。最后得到的是地图坐标系下的位姿变换，把点云转换到地图坐标系下，发布点云地图。

1、选取一个局部地图，把局部地图划分为21*21*11的栅格地图

2、构造一个Submap，以当前帧位置为中心，向前后拓展两个栅格、向上下拓展一个栅格，并把点云拷贝到栅格中构成Submap

3、对上一帧的点云进行下采样，用Submap中的点云构建KDtree，遍历上一帧中的边线点，在KDtree中取5个近邻点，求这5个近邻点的主方向构造一条直线，然后构造点到该直线的约束加入到优化问题中

4、遍历上一帧中的平面点，选取Submap中的5个近邻点，以最小二乘的方式拟合一个平面，构造点到面的约束加入到优化问题中

5、增量QR分解，用ceres进行位姿优化，并根据优化后的结果得到当前帧到地图坐标系原点的位姿变换，把点云变换到地图坐标系下

在阅读源码的过程中，本人对此工程做了一个思维导图：