这篇论文“Robust High Accuracy Visual-Inertial-Laser SLAM System”发表于2019年的 IROS 会议上。它提出了一个融合了相机、惯性元件、激光这三种传感器的系统。通过不同传感器之间的互补作用，该系统，较之于视觉惯性系统和激光系统，具有更好的鲁棒性。
纯视觉SLAM在光照变化明显、纹理较少的环境中容易跟踪丢失，因此研究者将惯性测量元件(IMU)加入到视觉SLAM中，组成 VI-SLAM (视觉惯性SLAM)。但作者发现， IMU 只能在短期内解决纯视觉SLAM 所面对的问题，如果机器长期工作在光照变化明显、纹理缺失的环境中，系统仍会跟踪丢失(因为IMU的偏差是随机变化的，长时间不修正会直接影响位姿估计)。此外，作者还指出了激光SLAM的不足：在结构性特征缺失的环境中（(比如在走廊)）会跟踪丢失。所以作者将视觉惯性、激光两类SLAM相结合，搭建了基于三种传感器工作的更加鲁棒的系统。该系统可大致分为视觉惯性和激光两大位姿估计模块。它先通过视觉惯性模块估计出位姿的初值，再根据激光扫描的结果完成位姿的优化。两个模块可以来联合工作，也可以各自独立工作。这也是该系统鲁棒的原因：当一个模块跟丢了，系统也可以只通过剩余模块完成定位和建图。
1、假设相机内参 K K K已知，同时三个传感器(相机、IMU、激光)之间的外参矩阵（相对位姿变换关系）也已知； 2、默认这三个传感器在时间上是同步的！！(这点很重要)； 3、三个传感器都有各自的坐标系，依次记为相机： C C C，IMU： I I I，激光： L L L。此外将世界坐标系记为 W W W 。在初始化后，将 L L L 坐标系作为主要观测坐标系（系统主要记录3D点在 L L L 上的坐标）； 4、使用 S i S_{i} Si 表示 i i i 时刻的某个坐标系。并用 T b a T^{a}_{b} Tba 表示从 b 到 a 的位姿变换矩阵； 5、用 X j S X^{S}_{j} XjS 表示地图点 j j j 在 S S S 坐标系上的三维齐次坐标； 6、为了方便，下文用 VI 代表 “视觉惯性” 。
系统先通过一个紧耦合的 VI 方法估计位姿，再通过激光扫描的结果来优化该估计值，最后再完成建图。整个过程的框图如下所示： 下面介绍每个模块。
该系统的 VI 前端是基于 VINS-Mono 系统实现的，主要细节可查看 VINS-Mono 的论文。
（这部分主要涉及激光定位，主要是 LOAM系统中的知识）扫描匹配（scan matching）模块的工作流程框图如下所示： 激光雷达会连续检测到地图点，并获得其在对应时刻 L t i L_{t_{i}} Lti 坐标系下的坐标。以第 k k k 次激光扫描为例， 记 t k t_{k} tk 为这次扫描的开始时间， t k + 1 t_{k+1} tk+1 为结束时间。令 P k ~ \widetilde{P_{k}} Pk 为第 k k k 次扫描过程中检测到的所有 3D 点的集合。因为所有传感器是时间同步的，同时 t k t_{k} tk、 t k + 1 t_{k+1} tk+1 时刻机器的位姿已由 VI 里程计估计出，所以可通过 IMU 与激光传感器间的外参矩阵估计出 L k L_{k} Lk、 L k + 1 L_{k+1} Lk+1 间的位姿关系，如下式： 式中 T I L T^{L}_{I} TIL 是 IMU 与激光传感器间的外参矩阵， T I k + 1 I k ~ \widetilde{T^{I_{k}}_{I_{k+1}}} TIk+1Ik 是 VI 里程计估计的结果。
此时要引入一个假设：在 t k t_{k} tk、 t k + 1 t_{k+1} tk+1间，机器以恒定的速度运动。此假设使得我们能通过线性插值的方法获得 t i ∈ [ t k , t k + 1 ] t_{i}\in [t_{k},t_{k+1}] ti∈[tk,tk+1] 时刻， L i L_{i} Li 与 L k L_{k} Lk间的位姿关系，如下所示：
通过上式计算的位姿，可以将任意 t i t_{i} ti 时刻检测到的点都转换到 L k + 1 L_{k+1} Lk+1 坐标系上表示：
此时，将转换后所有在 L k + 1 L_{k+1} Lk+1 坐标系中的点的集合记为 P k {P_{k}} Pk。
根据 P k P_{k} Pk 中各点局部表面的平滑程度，决定某点属于边缘特征点或平面特征点。将提取出来的特征点的集合记为 F k F_{k} Fk 。此时，再根据已经通过优化得出的 L k L_{k} Lk 与世界坐标系的相对位姿结果 T L k W T^{W}_{L_{k}} TLkW 和 公式 (1) ，推出 L k + 1 L_{k+1} Lk+1 与世界坐标系之间的相对位姿估计值 T L k + 1 W ~ \widetilde{T^{W}_{L_{k+1}}} TLk+1W ： 通过 公式(4) 求得的 T L k + 1 W ~ \widetilde{T^{W}_{L_{k+1}}} TLk+1W 将 F k F_{k} Fk 包含的所有特征点都投影到世界坐标系中，并为它们在已构建好的地图中寻找相匹配的线边缘、平面块等结构特征（由此就建立了 L k + 1 L_{k+1} Lk+1 与地图的联系。根据这个联系就能构建出图优化的边）。然后计算 F k F_{k} Fk 中特征点到相匹配的线边缘或平面块的距离 d d d 。这个过程可以用下面的函数 f f f 表示： （PS:特征点寻找匹配的边缘线和平面块的方法，以及距离的计算方法可在 “LOAM: Lidar odometry and mapping in real time” 论文中查看） 将所有 d i d_{i} di 相加，即为需要优化的变量：
由此，激光的扫描匹配（scan matching）的结果建模出了一个非线性优化问题。通过牛顿梯度下降的方法使 d d d 的值趋近于 0 （因为理论上特征点到与其匹配的线或面的距离应该为 0）。如果结果能够收敛，则能获得一个关于激光传感器的优化后位姿： T L k + 1 W T^{W}_{L_{k+1}} TLk+1W。最后通过 T L k + 1 W T^{W}_{L_{k+1}} TLk+1W 将第 k k k 次激光扫描获得的点云图转换到世界坐标系中，更新地图。 （个人想法：从系统的流程可以看出，系统对于 VI 模块和激光扫描匹配模块采用的是松耦合的方式（先 VI 估计再扫描匹配优化）。因此激光模块优化的效果比较依赖于 VI 的估计初值。这或许能通过紧耦合的方式来改善，但会提高系统的复杂度且降低各模块的灵活性）
如最初在摘要中所说的，视觉惯性SLAM 和激光SLAM 在某些环境下都会存在跟踪丢失的风险。所以作者构建的这个系统主要是由两个独立性较强的模块（VI 位姿估计模块和激光扫描匹配模块）组成的。在某个模块失效后，另一个能够独立工作，以保证系统的正常运行。这就是该系统具有较高鲁棒性的原因。 根据系统工作时所使用的模块的不同，可分成以下三种工作模式： （1）正常工作模式： 此时 VI 和激光 模块都正常工作，系统按照前几节描述的流程完成定位和建图； （2）scan to scan matching 工作模式： 此时 VI 模块跟踪丢失，启动 “两次激光扫描间的匹配（scan to scan matching）” 定位模块（自己取的名字，见笑见笑）。这个模块是基于 LOAM 系统构造的，它的大致工作过程和上一小节中的 “扫描匹配（scan matching）优化” 相似，只是这里估计的是 L k L_{k} Lk 和 L k − 1 L_{k-1} Lk−1 之间的位姿 T L k − 1 L k T^{L_{k}}_{L_{k-1}} TLk−1Lk ，而不是 T L k W T^{W}_{L_{k}} TLkW。此模式下系统的工作流程图如下所示：
（PS：当发生以下几种情况时，系统会认定 VI 模块失效：当前跟踪到的特征点很少、IMU 偏差变化较大、VI 滑动窗口中的位姿估计值与先前估计的结果偏差较大） （3）仅使用 VI 工作模式： 此时系统仅使用 VI 模块完成定位和建图，流程图如下所示： （PS：此模式主要是在结构信息缺失的环境中使用） 这三种工作模式使得系统能在多种不同的、难度较高的环境下继续工作。同时，当环境条件变好后，系统仍会重新启用之前失效的模块，变回正常的工作模式。 （疑惑：以第三个模式为例，当激光模块重新工作时，由 VI 模块构建的地图作为激光模块的初始地图，还是激光模块会重新构建新的地图？即 VI 和激光模块两者所构建的地图是否能够互相使用。）
老规矩，为了消除累积误差，系统需要有闭环检测的能力。因为当前系统搭载了视觉、激光传感器，所以它实现闭环检测的方式有两种： 1、基于视觉和词袋向量的闭环检测，也就是最经典的视觉闭环检测方法； 2、基于激光雷达的临近检测。 作者指出，第一种方法存在一定缺点：必须观测到相同物体，才能检测到闭环(对机器的观测视角有要求)。而第二种方法则没有这个限制，因为激光雷达是 360° 扫描的。为了避免过多的约束和计算，某个关键帧( KF )在实现视觉闭环检测后，将不再进行临近检测。 闭环检测想必大家都较为了解，所以这里主要介绍一下临近检测（这部分内容也是第一次看，理解不对之处望谅解和指出），它的过程图如下： 首先在 KF 数据库中计算各 KF 与当前关键帧( K F c KF_{c} KFc)在世界坐标系下的相对距离（即二者三维坐标的距离）。记所有与 K F c KF_{c} KFc 距离小于 R 1 R_{1} R1 关键帧的集合为 K F s l o o p KFs_{loop} KFsloop 。以 K F s l o o p KFs_{loop} KFsloop 中的一个关键帧（ K F a KF_{a} KFa）为例： 在 K F c KF_{c} KFc 和 K F a KF_{a} KFa 已有的估计位姿基础上，求出两者间的相对位姿 T K F c K F c T^{KF_{c}}_{KF_{c}} TKFcKFc ，再根据先前提到的 “两次激光扫描间的匹配方法” ，优化 T K F c K F c T^{KF_{c}}_{KF_{c}} TKFcKFc 。此时如果 K F c KF_{c} KFc 和 K F a KF_{a} KFa 之间的相对距离小于 R 2 R_{2} R2，则两帧满足临近检测要求。 （PS：每个关键帧包含机器的位姿、对应图片的特征点和从激光扫描中获得的几何特征点信息） 对 K F c KF_{c} KFc 和 K F s l o o p KFs_{loop} KFsloop 中每个 KFs 都进行如上操作。如果有多个 KFs 满足临近检测要求，则只选择产生时间最早的那个 KF 与 K F c KF_{c} KFc 构成闭环（因为这样可以获得较大的闭环，有利于消除更多的累积误差）。此外，假设 K F c KF_{c} KFc 的编号为 k k k ，那么编号为 k − e , . . . , k − 1 k-e,...,k-1 k−e,...,k−1的 KF 不会参与临近检测的判定（ e 的值根据经验设定）。这样可以降低计算量。 （个人理解：从论文的实验结果来看，同时使用视觉闭环和临近检测与只使用其中之一相比，最终精度的差距不是很大。但是为了保证系统的两个模块的独立性，这两个检测方法需要要同时存在。）
当完成闭环产生后，系统将执行全局位姿图优化。图中 KF 作为顶点，它包含三种边： 1、相邻 KF 之间的连续边，这个边计算的是相邻 KF 间的相对平移和旋转变换： 2、闭环检测边。这个边计算的是闭环 KF 之间的相对平移和旋转变换，使用的是 PnP方法。 3、临近检测边。和闭环边类似，只不过采用的是 “两次激光扫描间的匹配方法”。 所以，图中顶点 i i i 和 j j j 之间的误差可表示为： （个人理解：式中减号左边的项可能由 IMU 预积分获得，因为 IMU 测量值的预积分结果可能更接近于真值） 整个优化图中的总误差为： 式中 A 、 B 、 C A、B、C A、B、C 分别对应上述 1、2、3 类边。这个非线性优化问题通过 Ceres Solver解决。为了降低计算量，系统会限制 KF 数据库中关键帧的数量。构成闭环的 KFs 会被保留，而那些与自己邻近 KFs 距离较近的关键帧将被剔除。
这个系统应该是多传感器融合 SLAM 中较简单的一个实现方法。它将视觉惯性和激光匹配两种位姿估计方法进行松耦合（先 VI 估计，再激光匹配优化），让两个不同、相对独立的模块共同完成机器的位姿估计。这使得系统的工作流程更为清晰，降低了复杂度，同时也保证系统能够灵活地对切换工作模式，以应对不同的环境条件。此外，系统在闭环检测时采用的两种不同方法，也能提高整体精度。
