LOAM 论文总结

**

LOAM 论文总结

参考以下两篇文章：

1. LOAM 论文及原理分析
2. LOAM 论文 LIDAR MAPPING 部分翻译

LOAM全称是LOAM : Lidar Odometry and Mapping in real-time 在2014年发表在RSS上。
本文使用的激光是二维激光，加一个电机。从而实现40线，线之间的分辨率是0.25度。
本文主要分成两个部分：激光里程计 和 建图。

激光里程计

1. 特征点提取：在激光点云数据中提取特征点（包括：边缘点和平面点）。为了使提取的特征点均匀分布，所以将以此扫描分成四等分，对每一份提取特征点。对每一个点通过其周围最近的5个点，计算曲率大小。通过对每一次扫描中的点的曲率进行排序。最大的前几个点就是边缘点，最小的几个点就是平面点。在提取过程中要避免提取到 与激光线平行的平面和遮挡的边缘点。因为通常认为这样的点是不可靠的，因为它们不能被任何时刻都被提取到。
2. 特征点匹配关联：将当前帧的激光点云数据投影到上一帧上。通过 边缘点到边缘线的对应。首先找到边缘点i对应的上一帧数据中最近的两个点，然后判断这两个点是否是边缘点，j和l必须是不同的线上中的点，因为一次一个线在某一段中最多有一个边缘点。如下图中的图a所示，蓝色线是P¯k线扫探测到的边缘点，橙色线是相邻的线扫。平面点对应上一时刻中的平面。依然首先找到上一帧数据最近点，并在该线扫上找到另一点，再在相邻扫描线上找一点，这样就可以保证三个点不在一条线上组成一个平面。

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

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

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

3. 运动估计
假设激光是在匀速运动的，所以如果知道了一帧数据终止点相对于起始点的转换矩阵，就可以对这一帧数据中的任意点按照其获得时相对于起始点的时间进行插值。获得每一个点的位姿。插值公式如下图所示：

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

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

这样就很容易对旋转矩阵进行求导。
到此我们有了点到线和面的距离，并获得用于优化的误差函数：

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

优化的过程中是对每一次迭代都寻找一次最近邻的点。
此时如果当前帧计算完成，则将当前的$P_{k+1}$点云则在$t_{k+2}$时刻 转换到激光世界坐标系下。
**

Lidar Maping

建图算法的频率比里程计的频率要低。并且每次扫描只调用一次。在k+1帧（sweep暂且翻译成帧）的最后，雷达里程计产生一个不失真的点云，同时产生一个转换 $T_{k+1}^L$，它包含了在tk+1和tk+2帧之间的雷达的运动。建图算法在世界坐标系{W}下匹配和配准（注册）$P_{k+1}$ 如图8所示。为了解释这个过程，我们定义$Q_k$作为地图上到k 帧累积的点云，定义$T_k^w$是雷达在地图上k帧最后时刻$t_{k+1}$的位姿。随着雷达里程计的输出，建图算法将从$t_{k+1}$扩展到$t_{k+2}$进行一次扫描从而得到$T_{k+1}^w$，并且把投影到世界坐标系{W}中，记为${\vec{Q}}_{k+1}$。然后通过优化雷达位姿 使用算法匹配${\vec{Q}}_{k+1}$ 和$Q_k$ 。

图8 展示了建图算法的过程。蓝色曲线表示雷达在地图上的位姿$T_k^w$，它是由建图算法在第k帧产生的。橙色曲线表示在第k+1帧间的雷达运动 $T_{k+1}^L$，它是由里程计算法计算所得。经过$T_k^w$和$T_{k+1}^L$ ，里程计算法发布的失真点云就能被映射到地图上，被定义为${\vec{Q}}_{k+1}$（图中绿线段），并且匹配到已有的地图Qk （黑线段）上。

使用在V-A部分中的方法提取特征点，但是使用10倍的特征点。为找到特征点的对应点，我们把地图Qk上的点云储存在10m方的立方体中。提取与相交的立方体中的点，并将其存储在3D KD-tree中。我们在特征点周围的特定区域内找到Qk中的点，设S´是一组周围的点。对于一个边界点，我们只在S´中的边界线上保留点，对于一个平面点，我们只在平面片上保留点。然后计算S´的协方差矩阵，记为M, M的特征值和特征向量分别记为V和E。如果S´分布在一条边线上，V包含一个显著大于另外两个的特征值，E中与最大特征值相关的特征向量表示边线的方向。另一方面，如果S´分布在一个平面片上，则V包含两个较大的特征值，第三个特征值显著较小，E中与最小特征值相关的特征向量表示平面片的方向。边缘线或平面片的位置通过S´的几何中心来确定。

为了计算特征点到对应点的距离，我们在一条边线上选取了两点，在一个平面上选取了三点。这考虑使用与(2)和(3)相同的公式计算距离。然后，对每个特征点推导出式(9)或(10)，但不同的是中的所有点都有相同的时间戳使用在V-A部分中的方法提取特征点，但是10倍的特征点已被使用。为找到特征点的对应点，我们把地图Qk上的点云储存在10m方的立方体中。提取与相交的立方体中的点，并将其存储在3D KD-tree中。我们在特征点周围的特定区域内找到Qk中的点，设S´是一组周围的点。对于一个边界点，我们只在S´中的边界线上保留点，对于一个平面点，我们只在平面片上保留点。然后计算S´的协方差矩阵，记为M, M的特征值和特征向量分别记为V和E。如果S´分布在一条边线上，V包含一个显著大于另外两个的特征值，E中与最大特征值相关的特征向量表示边线的方向。另一方面，如果S´分布在一个平面片上，则V包含两个较大的特征值，第三个特征值显著较小，E中与最小特征值相关的特征向量表示平面片的方向。边缘线或平面片的位置通过S´的几何中心来确定。

图9 姿态变换的Integration。蓝色区域显示了来自建图算法的激光雷达姿态，每次扫描生成一次。橙色区域是激光雷达在当前扫描范围内的运动，，由里程算法计算。激光雷达的运动估计是这两种变换的结合，频率与相同。

后记：LIDAR MAPPING算法的配准跟 LIDAR ODOMETRY算法的配准过程略有不同（主要体现在对应点确定方式的不同），在ODOMETRY算法中，对应点的确定是为了最快的计算速度（边角点与上一时刻的边角线匹配，平面点与上一时刻的平面匹配），而MAPPING算法是通过对特征点周围的点云簇进行PCA主成分分析（求点云的协方差矩阵的特征值和特征向量），来找到对应边和对应面。对应点确定后的过程就跟之前一样了，实现将​${\vec{Q}}_{k+1}$注册到整个地图中去。