简介
传统的激光 SLAM 算法一般是采用 ICP 方法来计算前后两帧点云的位姿变化。ICP 方法是通过寻找点与点之间的匹配关系，然后通过计算一个位姿转换来使得所有点的匹配误差最小化从而得到前后两帧的位姿变化量。但是原始的 ICP 算法通常计算量比较大，不适用于大规模场景，有不同变种的 ICP 被提出来尝试解决计算量大的问题，例如对点到面进行匹配, 面到面匹配，可以参考我之前的这篇博客 激光SLAM 点云配准 - 基于 ICP 及其变种的方法，除了算法层面的改进以外，还可以利用 GPU 的并行计算等方法来降低计算时间，不过会需要额外的硬件资源。除此之外还有一类方法是从点云中提取出特征，通过对特征之间进行匹配来降低及三辆，LOAM 通过判断点周围的平滑性来提取出角点特征（点与点之间曲率变化很大）和面特征（周围曲率很小），并且其将估计问题分解成两个独立的子问题，分别利用高频率相对低精度和低频率高精度两个线程独立工作来达到整体实时性的效果。
这篇论文中主要针对小型嵌入式系统（轮式小车）来优化 LOAM 算法，从而得到高质量的六自由度的位姿。对轮式小车而言，位姿估计的难点主要在于：
- 小型嵌入式系统的算力通常不足，很难达到实时性计算的要求
- 小车在不同平面运动时位姿的变化曲线不会很平滑，因此获取的点云数据会存在一定畸变
- 小车快速运动时，两帧点云之间的覆盖率可能不高，给特征匹配造成一定困难
除此之外，在小车使用 LOAM 算法还会有以下问题：
- 小车的算力不足，在使用 LOAM 时，特征匹配很难达到实时性
- 由于小车（以及配备的雷达）比较靠近地面，因此地面上的一些测量误差会经常出现，例如草地中的草附近由于环境比较复杂会被识别成角点特征，或者叶子被识别成平面特征等等。这些不可靠的特征会很大程度上影响匹配的精度
LeGO-LOAM 主要就是为了解决这些问题，具有轻量级（Light-weighted）以及对地面情况进行优化（Ground-Optimized），相对于原始的 LOAM 改变主要有：
- 对激光点进行分类，从而排除掉可能来自不可靠特征的点
- 对地面进行优化，分两步来对位姿估计进行优化：首先，通过平面特征匹配来的得到：
$[t_z, \theta_{roll}, \theta_{pitch}]$，再通过角点特征匹配来估计$[t_x, t_y, \theta_{yaw}]$ - 加入回环检测来修正偏移
硬件平台
- 激光雷达：在两个雷达上测试过，参数分别是：
- VLP-16：检测距离远达 100m，精度 3cm；垂直视场 30°，分辨率 2°；水平视场 360°，分辨率根据旋转速度从 0.1° 到 0.4°
- HDL-64E 3D：垂直视场 26.9°
- 计算平台：嵌入式计算设备：Nvidia Jetson TX2 以及带 2.5 GHz i7-4710MQ CPU 的笔记本
LeGO-LOAM
系统总览
系统流程如上图所示：系统从激光雷达以 10 Hz 的频率得到激光点云，再以同样频率输出六自由度位姿。系统整体分为 5 个模块：
- Segmentation：接收点云并将其投影至一个距离图像中进行识别
- Feature Extraction：对识别好的点云进行角点和平面点的提取
- Lidar Odometry：利用提取出的特征进行前后两帧点云位姿之间的变化
- Lidar Mapping：同样接受提取出的特征并将其插入到全局点云地图中
- Transform Integration：将里程计和建图模块输出的位姿进行融合来生成最终位姿
点云分割
设 $P_t = {p_1, p_2, ... , p_n}$ 是 $t$ 时刻获得的点云。点云首先被投影到距离图像中，图像分辨率为 1800x16，$P_t$ 中的每个点在图像中占据一个像素点。每个点的像素值（距离）为该点到传感器的欧式距离，然后对图像中的每一列距离评估来识别出属于地面的点（具体的做法是在每一列中选取一系列相邻点，计算其连线的俯仰角，如果小于一定阈值则考虑是地面）；这一步识别出来的地面点被被单独归类不会进行后续的识别。对图像中剩余的部分利用图像识别算法分割成不同组（cluster），每个组被分配一个单独的标签，（地面也是一个标签）。将图像中小于 30 个像素点的点云组忽略，从而过滤掉一些不可靠的特征，例如树叶和草等等。经过过滤之后，剩余的点表示了一些较大的特征，例如树干、墙面等等。对于剩余的这些点，系统会利用他们的三个参数：标签（地面点或者已分类点）、在距离图像中的行和列以及它的距离。
特征提取
特征提取方法参考：Low-drift and real-time lidar odometry and mapping，不过只针对点云中的地面点以及其他被识别出来的点。设 $\mathcal{S}$ 是距离图像跟点 $p_i$ 同处一行中的连续点集。其中 $p_i$ 两侧各有一半点（即 $p_i$ 为这个行点集的中点），论文中每个点集的大小设为 10，然后通过以下公式利用每个点的距离来计算点 $p_i$ 周围的平滑程度（曲率）：
$$
c = \frac{1}{|\mathcal{S}|\cdot||r_i||}||\sum_{j\in\mathcal{S},j\neq i}(r_j - r_i)||
$$
为了从各个方法均匀的提取出特征点，将距离图像水平分成几个子图；然后在每个子图中对每一行的点按照他们的曲率大小排序。跟 LOAM 一样，这里也有了一个曲率阈值 $c_{th}$ 来区分不同类别的特征。将曲率大于阈值的点归类为角点（Edge），否则为平面点 (Surface)。分类后，设 $\mathbb{F}_e, \mathbb{F}_s$ 分别为所有子图中的角点和平面点的点集，在每个子图中的每一行，选取不属于地面点并且曲率最大的数量为 $n_{\mathbb{F}_e}$ 的角点，同理选取不属于地面点并且曲率最小的数量为 $n_{\mathbb{F}_s}$ 的平面点。设 $F_e, F_s$ 是通过上述步骤筛选出来的角点和平面点集。算法中将 360° 对应的距离图像分成六部分，即每个子图的分辨率为 300x16，$n_{F_e}, n_{F_s}, n_{\mathbb{F}_e}, n_{\mathbb{F}_s}$ 分别设为：2，4，40，80。
雷达里程计
雷达里程计需要找到相邻两帧之间的传感器的相对运动，这里需要用到点到角点以及点到平面的匹配，即需要从前一阵的角点以及平面点点云 $\mathbb{F}_e^{t-1}, \mathbb{F}_s^{t-1}$ 中找到当前帧提取出的特征点集 $F^t_e, F^t_s$，具体的算法流程同样参考 LOAM，LeGO-LOAM 中做出了以下改变：
- 根据标签进行匹配：在第一步中我们对点进行分类，除了筛除掉的点以外，剩余点的被归类为地面点以及被识别的一般点，这里对于当前帧的平面点集
$F^t_s$中的点，我们只在上一帧的点集中的平面点进行匹配；对于当前帧的角点集$F^t_e$中的点则从上一帧中被聚类的点组中寻找匹配。 - 分两步进行 LM 优化：和 LOAM 中把所有角点和平面点的匹配约束都放在一个优化问题中求解不同，LeGO-LOAM 中将优化问题分成两布，先离开用平面点之间的约束构建第一个优化问题，对
$[t_z, \theta_{roll}, \theta_{pitch}]$进行优化（这一步也会同时优化其他三个变量，只是我们不会将其用在第二个优化问题中）；将角点的约束构建第二个优化问题，对$[t_y, y_z, \theta_{yaw}]$进行估计，通过这个方法，可以在保持计算精度的同时显著降低计算时间（35%）。
简单的理解一下为什么分两步可以降低计算资源，假设有 m 个角点和 n 个平面点，如果我们放在一个问题进行分析，那么求解的优化问题的规模是 (m+n)x(3+3)，如果上述分成两部分问题求解的话，则这两个问题的规模分别是 mx3, nx3。 在第一步中由于我们进行地面点约束，这个约束对 $x, y, yaw$ 方向的变化不敏感，因此可以只对其他三个自由度进行优化。
雷达建图
雷达建图模块将当前帧的特征点对周围已经建好的局部点集 $\bar{Q}^{t-1}$ 进行匹配，然后使用 LM 优化对位姿进行估计，同样具体过程可以参考 LOAM，这里 LeGO-LOAM 做出的改变主要是地图存储方式不同：LOAM 中将所有点云统一存储，我们不知道哪个点对应哪一时刻的哪一个位姿，LeGO-LOAM 中将每一帧的特征点集单独存储，即将点云和位姿的对应关系储存起来。局部地图 $\bar{Q}^{t-1}$ 的提取有两种方法：
第一种方法是根据传感器的视场选取在传感器视野范围内的点集，论文中的方法是将传感器周围 100m 的点集提取出来转换至同一坐标系然后进行融合，方法和 LOAM 基本一样。
第二种方法是引入图优化 SLAM 的思想，将每个特征点集（包括角点和平面点）对应的位姿作为图的节点（node）,特征点集可以作为这些节点的观测量，因为雷达建图模块的偏移相对较低，因此可以假设在相邻帧之间不存在偏移。通过这种方法，$\bar{Q}^{t-1}$ 可以通过挑选一组时间上较近的特征点集来组成，即 $\bar{Q}^{t-1} = \{\{\mathbb{F}_e^{t-k}, \mathbb{F}_s^{t-k}\}, ..., \{\mathbb{F}_e^{t-1}, \mathbb{F}_s^{t-1}\}\}$，然后将新的节点和 $\bar{Q}^{t-1}$ 中涉及到的节点再用 LM 估计后的位姿关系添加进图中构建约束；接下来可以通过回环检测来消除里程计漂移。具体的方法是如果利用 ICP 方法检测到当前帧和以前帧的特征点集存在匹配关系时，建立约束，对构建的图进行优化来更新传感器的位姿。
实验
TODO
结论
TODO