核心思想

将建图与定位两个部分分开，通过分别实现高频(10hz)低精的激光里程计和低频(1hz)高精的建图，达到低漂移，低复杂度的效果

相关的定义

sweep： 相当于现在激光雷达的一帧

软件系统框图

在每个 sweep 中会有多次里程计计算，当激光点云累积到达到 1 sweep 时候就会做一次建图操作

激光里程计

提取线特征(edge)和面特征(planar)

根据激光扫描的特性和点云分布的属性提取特征：

以下为具体步骤：

（1）对每一个点，利用同一线束上的其他点(S)计算局部 smoothness

（2）对同一线束上的点按照 smoothness 进行排序，并根据阈值分成 edge point 和 planar point

（3）为了保证分布均匀，划分四个区域，每个阈值只保留2个 edge point 和4个 planar point

作者为了进一步保证特征分布均匀做出规定：周围点没有被选取为特征，以及为了保证特征的质量，对 S 做出了限制：

（1）S 中的所有点不能组成平行于激光束的 local planar，如图(a)示

（2）S 中的任一点与当前点在激光束方向上不能有太大的 gap，且相比于当前点，更靠近激光雷达(可能存在遮挡)，如图(b)所示

特征匹配

以下为具体步骤：

（1）将上一个 sweep 的所有点云按照估计出来的位姿投影到当前 sweep 的开始处，并构建 3D KD-tree

（2）对当前 sweep 中已有的点云提取线特征和面特征，然后将其投影当前 sweep 的开始处，并在 KD-tree 中寻找最近的点

（3）如图(a)所示，对于当前 sweep 线特征 i，在最近点 l 的上一条或者下一条线束中寻找另一个最近点 j(由特征提取性质可知，同一线束不可能再找到另一个距离近的线特征)，同时对这两个最近点验证是不是线特征。如图(b)所示，对于面特征 i ，在最近点 j 的同一线束上寻找第二个最近点 l ，在最近点 j 的上一条或者下一条线束中寻找第三个最近点 m ，同时对这三个最近点验证是不是面特征

注：在将当前 sweep 中的点云投影到开始处时，利用时间比例和当前的位姿变换对点进行投影(雷达的运动在短时间内是匀速的(旋转角度不变，平移速度不变))

运动估计

利用 L-M 算法最小化如图所示的两种距离(点到线，点到面)

算法流程

建图

每次 sweep 都会执行一次。主要是与局部地图匹配来 refine 当前的位姿(局部地图是边长为 10 m 的立方体，并以 5 cm 大小的体素网格进行滤波)

以下为具体步骤：

（1）利用局部地图中的点云构建 3D KD-tree

（2）对当前 sweep 中的每一个特征，在 KD-tree 中寻找一定范围的点来构建集合 S’

（3）对于线特征，只保留 S’ 中的线特征，并利用 PCA 求解 S’ 的直线方向；对于面特征，只保留 S’ 中的面特征，并利用 PCA 求解 S’ 的法线方向。同时通过 S’ 的几何中心可以确定直线或者面的位置

（3）对于线特征，在直线上选取两个点构造相应的点到线距离；对于面特征，在面上选取三个点构造相应的点到面距离

（4）利用 L-M 算法来求解位姿

注：建图中的特征提取与里程计中的一样，只不过会使用更多的特征(10倍)

ALOAM代码

Advanced implementation of LOAM，秦通实现，是原版LOAM的改进(原版已经闭源)，主要是使用Eigen和Ceres对代码进行重构，原版LOAM有大量的欧拉角计算，很难懂