【论文分享】LOAM-SLAM：激光雷达里程计和实时建图
重磅干货，第一时间送达
小白学视觉团队举行了每周精读并分享一篇论文的活动，欢迎各位小伙伴来报名参加。以后，我们会每天选择一篇优秀的论文总结与各位小伙伴们分享。 本总结的论文题目为：LOAM_Lidar Odometry and Mapping in Real-time
LOAM SLAM的三维激光使用velodyne-16。代码中主要由四个.cpp文件ScanRegistration, LaserOdometry, LaserMapping, TransformMaintenanc 构成。
1.特征点提取
这一部分主要包括对激光点云进行预处理，移除空点，对运动畸变进行补偿，计算每个点的水平和垂直角度分为不同的scan，计算曲率值，划分特征点，发布特征点信息。
算出两帧之间机器人的相对位移的传统方法是直接在原始的点云上操作（比如ICP算法），但LOAM采用了在点云的基础上提取出相对较少的特征点，，然后再用特征点进行匹配。采用曲率c的概念来选取特征点，对点云中的任意一个点X，分别选取X点前后五个点进行如下公式计算。
公式（1）
根据曲率值大小，可以将点进行分类。论文中分为两类：曲率大的为角点，曲率小得为平面点。在程序中，对特征点分的更细，把曲率c不太大的点称为less sharp point，把曲率c不太小的点称为less flat point。
由于velodyne16线程激光雷达返回的点云数据是按照先上下，后左右的顺序进行的，所以首先将线号重新排列，才能计算曲率。Velodyne-16雷达每次返回的数据称为一帧（sweep），一帧由16条线组成（每条线称为一个scan）。在程序中将返回的点存储到一个数组中。
2.LaserOdometry（运动估计）
这一部分主要包括通过求解最小二乘法进行点云匹配，从而得到状态估计，运动畸变补偿，查找相邻对应的特征点，计算点到直线的距离和点到平面的距离，计算雅可比矩阵，迭代解最小二乘问题，发布估计的状态。
在LOAM中点云匹配的方法是分别计算与特征点相接近的线或面（corner对应线，surface对应面），然后再计算点与线/面的距离，这个距离就是loss，然后loss对位姿T求雅可比，使用高斯牛顿的方法，迭代优化T减少loss直到收敛。
计算角点到折线的距离要用到点到直线的距离公式，如下式所示。为角点，直线用两点表示。
公式(2)
计算平面点到平面的距离要用到点到面的距离公式，如下式所示。其中是平面点，平面点用不共线的表示。
公式（3）
运动畸变产生的原因就是激光雷达在采集数据的过程中是出于运动状态的。但如果激光雷达的扫描频率很高，比自身的运动快得多，那么就可以假设运动畸变很小从而忽略。但是大多数类得得频率都不是很高，velodyne-16线雷达常用频率为10Hz，这样的话运动畸变就不能忽略了。LOAM解决运动畸变的方法比较简单，就是根据每个点的相对时间进行补偿。雷达扫描一帧的时间是固定的，可以得到每个点的采集时刻，将所有点都统一到同一时刻，作者选择的是每完成一帧扫描的末尾时刻，如下图所示。就是一帧扫描开始的时刻，就是完成一次扫描的时刻，水平的箭头表示将所有点都投影到时刻。就是这一帧扫描生成的点云。
将时刻相对于时刻的雷达相对位姿记为，对每个特征点i计算它的补偿变换矩阵。如下式所示。在该LOAM实验中，是假设匀速运动的。需要注意的是，是对点云中所有的点都进行补偿，不单单是对特征点进行补偿。用变换矩阵对特征点i进行变换即可完成补偿。
公式（4）
3.LaserMapping（建图）
这一部分主要包括不断拼接每帧点云建立地图，包括对点云坐标变换，求雅可比矩阵和解最小二乘问题，最后发布环境消息。
通常求解非线性最小二乘问题的常用方法有LM（levenberg-Marquardt）方法和高斯牛顿方法。作者在论文中写的是LM方法，但在程序中用的是高斯牛顿法。最繁琐的一步是计算目标函数的雅可比矩阵，在改进版本ALOAM中，就图省事直接用了Ceres库，省去了计算雅可比矩阵的工作。ceres库也被用于Cartographer中。建图的过程就是不断地把匹配好的点云堆积在一起的过程，其中的思路与状态估计有些类似，但是有很多地方不一样。特征点的定义和使用与前面状态估计的一样，但是数量更多了，多了10倍。在寻找对应特征点时，将地图中已有的点云（）按照10立方米的格子存储。使用已经粗略估计出来的单帧点云（），它是相对于世界全局坐标系的。凡是与有交集的方格，从中取出位于这些方格中的点，再存入一个KDtree中。然后，再针对不同的特征点找它同类的那些点，中的角点找中对应的角点，平面点同理。
在寻找时限制搜索范围，只找一定半径范围内的。然后在直线上取两个点利用前面的公式计算角点到直线的距离，在平面上取三个点计算平面点到平面的距离。后面的操作跟前面的一样，也是利用LM方法得到最优的变换，利用新得到的最优变换，将最近的一帧单帧点云叠加到地图点云之上，就完成了建图。新得到的最优变换与之前粗略估计出来的变换一般是不同的，新的变换由于使用了更多点参与计算，应该更接近真实，所以用新的变换作为以后变换的基础。这样LOAM就完成了整个状态估计和建图。
4.TransformMaintenanc（整合过程）
这一部分功能就是用第三步得到的结果矫正第2步的状态估计，得到跟好的位姿，都是一些坐标变换和相乘，然后把矫正后的位姿发布出来就行了。
实验如下图所示: