大家好，又见面了，我是你们的朋友全栈君。

前言：

       由于对三维激光SLAM比较感兴趣，并且最近也在找无人驾驶激光SLAM算法的岗位，所以花了一个多月把LOAM的论文和源码好好看了一遍。发现论文还是比较容易明白，但一看代码全是坑。看论文懂了，看代码似懂非懂。为了尽快把这坑填上，所以诚邀读者一起探讨。作者始终认为填坑最好的方法是拉别人和你一起填坑。由于三千多行的源码不是一篇博客能够讲明白的，所以这篇博客主要讲一下我对LOAM论文的理解，后续会有代码的介绍，希望对大家能有帮助。

摘要：

       论文作者的目标是使用一个三维空间中运动的两轴单线激光雷达来构建实时激光里程计并建图。文章提出了构建实时里程计的难点在于点云不是相同时间获得的，为什么呢，因为点云中的点随着激光雷达运动会产生运动畸变，也就是点云中的点会相对实际环境中的物品表面上的点存在位置上的误差。这种运动畸变会造成点云在匹配时发生错误，从而不能正确获得两帧点云的相对位置关系也就无法获得正确的里程计信息。论文提出了一种方法可以同时获得低漂移和低复杂度，并且不需要高精度的测距和惯性测量。核心思想是将定位和建图的分割，通过两个算法：一个是执行高频率的里程计但是低精度的运动估计（定位），另一个算法在比定位低一个数量级的频率执行匹配和注册点云信息（建图和校正里程计）。将这两个算法结合就获得了高精度、实时性的激光里程计。

第一章 简介：

      激光雷达的优势在于测距频率高、准确，并测量产生相对固定的误差且与距离无关。如果激光传感器本体静止，激光只是绕固定轴做旋转运动，那么点云融合是很简单的。但我们使用的环境中激光通常是运动的，这就造成建图需要运动过程中激光传感器的位姿。通常使用的方法是使用独立的位姿估计如：GPS/INS来提供位姿信息来融合点云。另一种方法是使用车轮编码器或是视觉里程计提供的里程计信息来注册点云信息。但里程计是不断对运动积分，积分的过程中会不断累积误差，这就造成需要花费精力去抑制里程计的漂移。

       论文作者提出了使用低漂移的两轴激光雷达来建图，激光的关键优势是对光线和物体纹理不敏感。激光的发展使激光雷达传感器的体积和成本不断下降，因此激光可以手持遍历环境或是固定在小型移动设备上。由于论文的目的是最小化里程计漂移所以不涉及回环检测的问题。

       LOAM是怎么解决低漂移和低计算量这两个难题呢？我感觉是作者很好的利用了点云匹配和特征点提取。为什么这么说呢，我们知道匹配的问题分为scan-to-scan、scan-to-map、map-to-map。而三者各有特点，首先说scan-to-scan匹配：优点是计算量小速度快，缺点是误差累计大，长距离误差累计后地图就无法看了。map-to-map的匹配：优点是精度高，误差累计小；缺点就是计算量大，实时性压力大。scan-to-map的匹配居中。LOAM的优点就是很好地利用力匹配的优缺点，首先，虽然scan-to-scan匹配精度差，但是我们可以只是使用它做一个获取粗的里程计，用获取的结果用于去除匀速运动造成的运动畸变，由于scan-to-scan的计算量较小因此我们可以高频执行。其次，有了里程计校正后的点云数据，接下来我们就可以做一个map-to-map的匹配了。但是map-to-map存在计算量大的问题，因此 我们可以让其执行的频率降低。这样的高低频率结合就保证了计算量的同时又兼具了精度。不得不说论文作者思考很深啊！

       再有减少计算量的方法就是特征点的选取，像以前的特征点提取方法：特征向量、直方图、旋转图片等，方法好不好，好但是计算量大。 作者提出了新颖的方法就是提取边缘点和平面点作为特征点来使用，这种提取的方法就很简单只需要计算一个点前后五个点就可以得到该点的曲率，计算量就减少了很多。

       回到论文中，针对计算量问题，作者提出了高低搭配的方案，即执行高频的里程计但低精度的运动估计和执行低频的地图匹配来修正里程计。这两个方法结合就保证了实时性和精度两方面的要求。

第二章 相关工作

       激光雷达在导航中是非常有用的测距传感器，激光的优势在于其可以直接获得物品表面到点的准确距离信息，因此单线激光雷达可以用来同时定位和建图。如果激光的转速相对比较激光雷达本体运动的速度高很多的话就可以忽略由于运动造成的运动畸变。如果没有运动畸变的话直接使用标准的ICP就可以完成两帧点云匹配。但实际情况下如果要用ICP做匹配的话需要有速度估计来去除运动造成的运动畸变。在三维激光也用相似的方法，但是如果激光雷达的旋转速度相对较慢，则会产生非常严重的运动畸变。特别是对两轴激光雷达，其中一个轴的速度相对较慢。通常使用其他的传感器获得运动速度用于去除运动畸变。例如可以使用视觉里程计和IMU进行插值去除运动畸变，当使用多传感器时可以使用卡尔曼进行滤波。这样就可以获得实时的地图用于机器人导航时的路径规划和避障。

   如果两轴激光雷达没有其它传感器辅助，那么运动估计和畸变校正就是一个难题。一种方法是使用激光点云的强度构建图像，根据两帧数据间图像的位移估计地面车辆的速度，车辆是基于匀速运动模型进行计算。论文作者也是使用匀速模型但特征的提取方法不同。论文提取特征的方法是在笛卡尔坐标下提取和匹配几何特征，并且对点云密度要求低。

       这里简单介绍一下什么是运动畸变，这里以单线激光雷达为例。我们知道激光雷达没旋转一周返回一帧数据，.如上图左边所示我们使用单线激光对环境进行观测，如果激光雷达静止的话激光返回的点云应该如上图中右边图中的红色点云，但是激光在获取一帧点云的过程中时不断运动的，这就造成一帧点云数据中越早获得的点相对其对应的真实位置偏移量越大，形成上图右边的蓝色线。上图中激光雷达朝向障碍物运动，旋向为逆时针，这样蓝色点云右边点在激光雷达运动方向上的偏移量（畸变）就要大于左边点的偏移量。

 第三章 表示和任务描述

      论文的任务是使用激光雷达获得的点云数据进行运动估计并构建遍历过环境的地图。假设激光雷达时预先校准好的并机器人雷达两轴的运动是光滑并连续的。第二个假设是靠IMU实现的。 

      接下来是坐标系，在局部坐标系中，x轴指向左边，y轴指向上，z轴指向前（这里需要注意和通常传感器安装的方向不一样，可能是两轴的关系）。

      全局坐标系在初始时刻和局部坐标系相同。

     主要解决问题：给定点云计算每次扫描的运动并使用点云构建地图。

第四章 系统概括

       硬件上使用一个单线激光雷达加上两个机械轴实现三维环境的探测。激光的分辨率为0.25度，频率为40HZ。固定激光雷达的轴旋转角度为180度，即从-90度到90之间往复摆动。

      软件系统概括：

        和源码中代码构架一样，激光里程计主要分四部分完成。首先是获得激光雷达坐标系下的点云数据P^，然后把第k次扫描获得的点云组成一帧数据Pk。然后将Pk在两个算法中进行处理，也就是上面Liar Odometry节点和Lidar Mapping节点。Liar Odometry节点的作用是获取两帧连续点云数据间的运动，估计出来的运动用于去除Pk中的运动畸变。这个节点执行的频率为10Hz，作用相当于scan-to-scan匹配获得粗糙的运动估计用于去除匀速运动造成的运动畸变，并将处理后的结果给了Lidar Mapping节点做进一步处理。Lidar Mapping节点使用地图去匹配和注册没有畸变的点云数据以1Hz的频率。最后由Transform integration节点接收前面两个节点输出的Transform信息并将其进行融合处理以活动频率为10Hz的Transform信息即里程计。

第五章 激光里程计

a 特征点提取：

使用上图中的两轴激光雷达获得的点云数据提取特征点信息，由于两个雷达的构造会产生非均匀分布的点云数据。在一次扫描scan数据中点的分辨率为0.25度，并且这些点分布在一个扫描平面上。然而固定激光雷达的轴的转速为180度每秒。而激光的频率为40Hz，因此这个轴上点的分辨率为4.5度。考虑到点云数据的非均匀性，采取从一次csan中获取特征点。

特征点选取边缘点和平面点，方法是得到我们想要求曲率点i周围连续几个点集合S用于求曲率。S中的点一半在i的一侧从而避免雷达顺时针和逆时针的影响。

论文中按上面公式求点的曲率，因为在实际运用中我们只是比较点曲率的大小，因此在代码中只是求一个点周围五个点差的平方坐标该点的曲率。这样我们就可以求出每个点的曲率c。通过比较曲率我们就可以选出曲率大的边缘点和曲率小的平面点。为了防止特征点聚集，将每一次扫描的点云分成四份，从每份中选取两个曲率最大的点作为边缘点和四个曲率最小的点作为平面点。

在选择点时希望避免选取已经选过点周围的点或者是在于激光线接近平行的平面上的点，如下图中的a中的点，这些点通常认为是不可靠的，因为它们不能被任何时刻都能看到。同时我们也希望避免可能遮挡点，如下图中b中的点。

                                                                              点云数据中的边角点和平面点

b寻找特征点的对应

       里程计算法估计获取一帧点云时间内的运动，使用tk表示第K次扫描开始的时间。在一次扫描结束时，这一次扫描获得点云Pk会有一个获取时间tk+1。将投影后的点云记为P¯k，在下一次扫描时会使用P¯k和Pk+1做匹配求激光雷达的运动。接下来就要开始寻找对应关系了，也就是匹配两帧点云的特征点，Pk+1的边角点与P¯k的边角线匹配，Pk+1的平面点与P¯k的平面匹配。

       在第k+1扫描起始，Pk+1为空，随着扫描的进行Pk+1中的点云不断增加。激光里程计也不断估计激光在三维空间六自由度的运行。每一次迭代Pk+1中的边缘点和平面点都会被投影到扫描起始点的坐标系下（用于与上一帧数据进行匹配）表示为。P¯k中曲率大的点和曲率小的点保存在K_D树中用于寻找对应点。

       边缘点到边缘线的对应。首先找到边缘点i对应的上一帧数据中最近的两个点，然后判断这两个点是否是边缘点，j和l必须是不同的线上中的点，因为一次一个线在某一段中最多有一个边缘点。如下图中的图a所示，蓝色线是P¯k线扫探测到的边缘点，橙色线是相邻的线扫。平面点对应上一时刻中的平面。依然首先找到上一帧数据最近点，并在该线扫上找到另一点，再在相邻扫描线上找一点，这样就可以保证三个点不在一条线上组成一个平面。

      有了点到线和点到面的对应关系，接下来就要求点到线和点到面的 距离。首先求点到线段的距离，公式如下图所示：

       公式的分子是两个向量的叉乘，而叉乘后求模就变成了求两个向量构成的三角形的面积。公式的分母是向量的模相当于三角形底边的长。三角形面积除以一条边就可以求出该边到对应顶点的距离，也就是边角点到边角线的距离。直观上的理解如下图所示：

接下来求平面点到对应平面的距离，距离公式如下图所示：

公式的分子包括两部分，上边是获得一个向量，下边也是获得一个向量，但两个向量叉乘再取模就表示的求下边得到三角形面积上面表示立方体的高，两者相乘就表示立方体体积。而分母表示立方体底面三角形的面条，得到的高就是平面点到平面的距离。直观上的理解如下图所示:

C运动估计

这里就到了里程计算法的关键部分，首先，论文假设激光雷达的运动是匀速的。因此如果知道了一帧数据终止点相对于起始点的转换矩阵就可以对这一帧数据中的任意点按照其获得时相对于起始点的时间进行插值。获得每一个点的位姿。插值公式如下图所示：

为了获得这一帧数据中的点和上一帧数据中点的对应关系，我们使用一个旋转矩阵R和一个平移量T表示。

由于旋转矩阵求导非常复杂，所以用罗德里杰斯公式将旋转矩阵R展开为:

这样就很容易对旋转矩阵进行求导。

到此我们有了点到线和面的距离，并获得用于优化的误差函数：

f中的每一行代表一个特征点，接下就要求解雅克比矩阵，最后使用LM的方法进行优化。

里程计算法流程：

第六章 激光建图

建图算的执行频率小于里程计算法，每秒执行一次。