LOAM个人总结

1. 将点云分成两类：角点和平面点，分别对应点线、点面约束。

2. 位姿变化估计采用高频粗估计和低频优化两套算法实现定位。

Laser odometry雷达里程计--高频粗估计：在雷达坐标系下，通过点到线、点到面距离，利用L-M法得到雷达位姿估计

Mapping odometry 低频位姿精估计，在世界坐标系下，通过点到线、点到面距离，得到世界坐标系下的雷达位姿估计。

一、点云提取及处理

根据点的曲率c将点分为角点、面点，其实就是计算当前点与前后点的平均距离差。

选取当前点在同一个scan中相邻的前后5个点，计算这5个点与当前点的平均距离差。且为了防止因雷达的距离而带来的曲率过大或者过小，要进行归一化。

曲率小的为面点、曲率大的为角点。

均匀化：为了防止特征点聚集，一条scan上分成4份，从每份中选取两个曲率最大的点作为边缘点，四个曲率最小的点作为平面点。

去掉不可靠的点：遮挡点：两相邻点的深度大于一定阈值被认为是遮挡点，其附近点也被认定为不稳定的点。

平行光束点：平行光束当前点与前后两点的距离是很大的，超过一定阈值被认为是平行点。

二、雷达里程计--高频位姿粗估计 scan-to-scan的帧间运动

采用匀速运动模型，在一次扫描过程中雷达的线速度和角速度都是不变的。

去畸变：运动畸变产生的原因就是激光雷达在采集数据的过程中是处于运动状态的，去畸变的方法是根据每个点的相对时间进行线性插值补偿。

提取出特征点，借助特征点计算两帧点云之间的相对位移，即配准。

配准时，将第k帧点云映射到本帧末尾时刻也即k+1起始时刻，将k+1帧点云映射到本帧起始时刻，对两帧点云的特征点建立点、面关系。

点线约束：1、当前点在上一帧找距离最近点j，再找与j相邻scan的最近点l构成直线

                  2、计算当前角点到直线jl的距离（向量叉乘除以底边模长）

点面约束：1、当前点再上一帧点云找到距离最近的点j，找与j同scan的最近点l和相邻scan的最近                          点m构成平面

                  2、计算当前点到平面的距离

构建好点线、点面残差后进行位姿优化，构建一个最小二乘问题，目标函数由许多个误差的二次型组成，求使距离最小的位姿变换，考虑到离群点，为了降低离群点的影响，利用鲁棒核函数，构建最小一乘问题，降低残差过大时的影响。

构建好最小二乘问题后，计算目标函数的雅可比矩阵，使用L-M法进行求解。

根据优化后的位姿估计及得到的点云进行建图，创建点云地图。

三、雷达建图--低频位姿精优化

使用点云数量时雷达odom的10倍，使用分块存储点云，同时处理频率时odom的1/10

Scan-To-SubMap 匹配

不同于前端的scan-to-scan的过程，LOAM的后端是scan-to-map的算法,具体来说就是把当前帧和地图进行匹配，得到更准的位姿同时也可以构建更好的地图。由于是scan-to-map的算法，因此计算量会明显高于scan-to-scan的前端，所以,后端通常处一个低频的运行频率,但是由scan-to-map的精度往往优于scan-to-scan，因此后端也有着比起前端来说更高的精度。

首先需要了解的一件事情就是地图的构成,地图通常是当前帧通过匹配得到在地图坐标系下的准确位姿之后拼接而成，如果我们保留所有的拼接的点云，此时随着时间的运行，内存很容易就吃不消了，因此考虑存储离当前帧比较近的部分地图，同时，为了便于地图更新和调整，在原始LOAM中，使用的是基于珊格的地图存储方式。具体来说,将整个地图分成21×21×11个珊格，每个珊格是一个边长50m的正方体，当地图逐渐累加时，珊格之外的部

分就被舍弃,这样可以保证内存空间不会随着程序的运行而爆炸。

如果使用全部的地图数据，在计算效率上会大打折扣，所以，建图环节将整个空间三维区域划分为多个子cube(边长为10m的立方体)，并接着计算当前帧位姿所在的cube 索引,然后在当前帧位姿附近5*5*5个cube中寻找最有效的 cube作为局部地图,用以代替全局地图，但是，我们注意到，如果当前位姿远离的珊格覆盖范围,则地图也就没有意义了,因此，局部珊格地图也需要随着当前位姿动态调整，当当前位姿即将到达地图边界的时候,当前珊格地图就必须动态调整，保证当前帧处于一个安全的位置。从而保证我们可以从珊格地图中取出离当前位姿比较近的点云来进行scan-to-map算法，优化得到最终的姿态变换矩阵。

后端约束构建:

在前端里程记部分,我们通过当前帧的线特征和面特征分别和上一帧的线特征和面特征进行匹配，构建约束,然后进行优化求解。由于机械式激光雷达的性质,我们在寻找匹配的过程中需要注意线束相关的约束,以免构建的约束不符合实际。在后端的当前帧和地图匹配的时候,我们就需要从地图里寻找线特征和面特征的约束对,此时由于没有了线束信息,我们就需要采取额外的操作来判断其是否符合线特征和面特征的给定约束。

点线约束

提取所有边缘点，这个边缘点是当前帧的边缘点，投影到地图坐标系下面,并对每个边缘点在已构建地图中找出5个与其最近的点,通过kdtree在地图中找到5个最近的线特征,为了判断他们是否符合线特征的特性,我们需要对其进行特征值分解，通常来说,当上述5个点都在一条直线上时,他们只有一个主方向,也就是特征值是一个大特征值,以及两个小特征值，最大特征值对应的特征向量就对应着直线的方向向量。

1.计算5个点的均值

2.构建协方差矩阵，(原点-均值）=3×1矩阵，乘以它自己的转置构成协方差3×3

3.对协方差进行特征值分解

4.判断最大特征值大于次大特征值的3倍，符合的话则认为是线特征，特征向量就是线特征

的方向,也是直线方向向量

5.符合的话则用这个向量构建两个虚拟的点

6.构建点线约束

点面约束

在地图中提取离当前帧的每个面点最近的5个点,构建一个超定方程来求解这个平面方程，然后用 eigen中的QR分解求解,求得法向量，将这最近5个点带入平面方程，检验平面，大于一定阈值认为平面无效。

当前点带入平面方程得到点与平面的距离。

构建好残差，计算残差关于优化变量的雅可比矩阵，构建最小二乘问题，使用L-M法求解非线性最小二乘。