loam学习笔记

LOAM：

参考：
LOAM 论文及原理分析
LOAM细节分析
LOAM, ALOAM, LegoLOAM, hdl graph slam比较
LOAM: LiDAR Odometry and Mapping In Real Time源码解析
１．简介：
　　loam核心思想是将定位和建图的分割，通过两个算法：一个是执行高频率的里程计但是低精度的运动估计（定位），另一个算法在比定位低一个数量级的频率执行匹配和注册点云信息（建图和校正里程计）。将这两个算法结合就获得了高精度、实时性的激光里程计。
　　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存在计算量大的问题，因此 我们可以让其执行的频率降低。这样的高低频率结合就保证了计算量的同时又兼具了精度。
　　loam算法由四个节点构成。分别完成特征提取，高频低精度odom，低频高精度odom，双频odom融合的功能，每个节点以rosnode形式存在，也就是独立的进程。

以下为各节点简述：
ScanRegistration：
　　根据VLP16的激光扫描模型, 对单帧点云（paper中称为一个Sweep）进行分线束（分为16束）, 每束称为一个Scan， 并记录每个点所属线束和每个点在此帧电云内的相对扫描时间（相对于本帧第一个点）。
　　针对单个Scan提取特征点,因为角分辨率越大, 代表越远的物体, 反射的两点距离越大, 中间丢失的信息越多。因此, LOAM没有针对Scan和Scan之间的点的关联性提取和描述特征, 而是直接针对单个Scan提取特征. 而相对时间会在laserOdometry中用于运动补偿.所有Scan的特征点,拼到两个点云中(因为是corner和surface两种特征点,所以是两个点云).至此,每帧点云,输出两帧对应的特征点云, 给下一个节点laserOdometry。

laserOdometry
　　实现运动补偿和帧间配准.每帧激光都会参与（所以帧率同VLP16的扫描帧率，10hz）.通过对每一帧激光的配准,可以得到一个精度较差的ODOM,帧与帧配准的初始POSE可以由IMU得到，或者在没有IMU的时候由匀速运动模型(简单的说就是假设这一帧运动量与上一帧一致)得到。
本节点输出的ODOM频率为10hz.此ODOM的作用:

（a） 在laserMapping中用于位姿的预测。
（b） 在transformMaintenance中为laserMapping输出的低频ODOM提供插值,以获得高频(10HZ)的ODOM.

在实际应用中,帧间匹配实现的ODOM,可以由IMU, 视觉里程计,底盘编码器等替代.而运动补偿在高速场景是不可或缺的。
　　
laserMapping
本节点使用实现一个较为完整的SLAM过程，主要工作：

（a）通过多帧的激光特征点云基于POSE拼接，形成特征点云地图。由于包含conrner和surface两种，所以要建立两种地图。
（b）将新入的帧与地图做配准，得到更精确的POSE，然后按照ａ进行建图。
实际过程中由于单帧计算量大，所以采取每隔几帧配准一次（比如没５帧配一次，也就是２Hz）。如果一帧配一次，transformMaintenance也就没有存在的意义。
这个地图可以作为离线地图。用作定位使用。
当然这个地图功能还不完善，比如没有回环检测，这时候可以参考LeGO-LOAM，此文运用iSAM和ICP实现回环检测和全局优化建图。

２．配准过程
laserOdometry和laserMapping中都涉及到点云配准问题。
在laserOdometry中配准的source是新一帧的　点云,target是前一帧的点云。
在laserMapping中配准的source是新一帧的点云,target是前面的点云拼接成的地图。
当然,我们这里说的点云指的是特征点云, 分为corner和surface两种,corner对应corner map, surface对应surface map，分别对应场景的平面区域和曲折区域.LOAM没有使用特征描述子(连曲率都没有参与后续的匹配).从代码中的corner与surface的曲率判断阈值可以看出,LOAM提取的corner和surface特征点的曲率, 并没有特别大的差别,这使得LOAM有较强的场景适应性,在场景中比较曲折的区域,corner点会占据主导,而在较为平缓的区域,surface点占据主导. 在激光扫描到的一块区域，总会提取出几个特征点。

优化流程:
通过ODOM预测得到初始位姿,source中的corner在target的corner中寻找对应直线, source中的surface在target的surface中寻找对应平面。
通过点-线和点面关联共同构建约束, 以点线距离和点面距离作为loss, 基于非线性最小二乘的方法进行优化.求解得出最优pose。
本处只为简述laserOdometry和laserMapping中的共性,配准优化的具体细节后面会详述。

2.1　特征提取：
　　特征点选取边缘点和平面点，方法是得到我们想要求曲率点i周围连续几个点集合S用于求曲率。S中的点一半在i的一侧从而避免雷达顺时针和逆时针的影响。
　　以VLP16为例,每一次激光扫描, 包含16条线, 每条线由1800个点组成,我们对每条线, 按照曲率提取特征点, 曲率的计算方式为:
a.每条SCAN的边缘5个点不参与特征点选取,因为周边不满足左右各五个点计算曲率的条件.
b.对任意点A, 选取左边五个点, 右边五个点, 共十个点.
c.每个点的x坐标与点A的x坐标求差, 将所有的差求和,得到sx
d.每个点y坐标与点A的y坐标求差, 将所有的差求和,得到sy
e.每个点的z坐标与点A的z坐标求差, 将所有的差求和,得到sz
f.曲率

　　计算完曲率, 就可以根据曲率挑选特征点, 为了使得在一周360度上有均匀的约束, 我们将一条激光线平均分为6块, 将块内的点按曲率大小排列,设置一个曲率阈值t, 比如t=0.1,来区分边缘点和平面点.我们设定一个每块的最大点数N。
　　在选择点时希望避免选取已经选过点周围的点或者是在于激光线接近平行的平面上的点，如下图中的a中的点，这些点通常认为是不可靠的，因为它们不能被任何时刻都能看到。同时我们也希望避免可能遮挡点，如下图中b中的点。

边缘点选择条件:
　　从曲率最大的点开始,最多选择N个, 只有曲率大于t的点才能被选取
　　若一个点周围五个点中已有点被选为边缘点,跳过这个点, 从曲率更小的点中选取

平面点选择条件:
　　从曲率最小的点开始,最多选择N个, 只有曲率小于t的点才能被选取
　　若一个点周围五个点中已有点被选为平面点,跳过这个点, 从曲率更大的点中选取

2.2 寻找特征点的对应：
　　里程计算法估计获取一帧点云时间内的运动，使用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线扫探测到的边缘点，橙色线是相邻的线扫。平面点对应上一时刻中的平面。依然首先找到上一帧数据最近点，并在该线扫上找到另一点，再在相邻扫描线上找一点，这样就可以保证三个点不在一条线上组成一个平面。