本文是百度刚刚开源的视觉惯性SLAM算法中用到的优化算法，文章发表在CVPR-2018上，作者也包括浙大张国锋老师。后端优化耗时限制了SLAM在移动设备中的应用，百度改进的BA算法，将运算速度提高了一个数量级，工程性很强，在估计性能上略优于state-of-the-art，可融合在各种滑窗SLAM算法中使用。

1 引言

SLAM在很多应用中都是一个经典但需要继续研究的问题，由于大量的图像和惯性传感器的出现，近年来视觉惯性VI-SLAM在增强现实AR、无人机、无人车、机器人等领域广泛应用。类似于纯视觉SLAM，VI-SLAM提取并建立图像帧间的特征匹配，但是VI-SLAM进一步利用惯性测量作为运动估计的约束，惯性测量在快速和不规则运动的估计中尤其有效，而这正是视觉特征匹配面临挑战的地方。假如运算能力足够大，state-of-the-art VI-SLAM算法的6DOF估计精度非常高。

由于大部分的SLAM应用是移动和实时性要求比较高的场景，VI-SLAM的计算复杂度也引起了学者们很大注意，只有很少一部分VI-SLAM系统能够在嵌入式设备上运行，提高计算效率是进一步应用的关键因素。在VI-SLAM中有两个主要运算任务：前端和求解器。前端包括视觉特征提取和匹配，一般并行性很好，可以使用各种现在的计算架构高效完成。求解器目标是优化位姿参数，通过给定一系列视觉特征和惯性测量后最小化目标函数实现，求解器优化常常是VI-SLAM算法速度的瓶颈。

大部分VI-SLAM框架使用传统的数值求解器优化目标函数，如视觉BA。本文将BA推广为联合优化视觉和惯性测量。常规的高斯牛顿法或LM求解器设计目标是提供数值精确结果，并没有考虑实时性。因此，目前基于这些求解器的实时VI-SLAM算法仅能够使用最近很小的滑窗估计位姿。理论上，长时间测量能够提供估计精度，BA的效率是VI-SLAM算法性能的主要因素。

我们改进了VI-SLAM中BA处理部分，显著提高了局部和全局优化效率，并解决了闭环检测的一致性问题。在SLAM中，视觉和惯性测量顺序到来，我们高效的使用以前优化的中间结果避免新的冗余计算。我们的一般算法明显提高了求解速度，能够应用于大部分基于滑窗的VI-SLAM中。

此外，本文算法解决了全局一致性问题，这对AR等应用至关重要。如果可以很好地闭环并且重投影误差足够小，则认为全局地图具有一致性。对于视觉SLAM来讲，全局一致性可以通过全局BA或位姿图近似实现；而VI-SLAM问题更复杂，帧间速度约束和IMU偏差使得优化问题有很多局部最小值。当测量从滑动窗口中去除时，边缘化累积误差会与闭环约束冲突。以前的方法或者跳过边缘化（如VI-ORB-SLAM2），或者不考虑冲突直接边缘化（如VINS）。

本文主要贡献如下：（1）提出了一种新的滑窗优化算法，改变了SLAM测量的增量本性，获得比state-of-the-art算法10倍效率的提升；（2）提出了一种新的相对边缘化算法，解决了滑窗边缘化偏差和全局闭环检测约束直接的冲突；（3）开源了本文算法。

2 相关工作

早期SLAM大部分是基于EKF的，6DOF运动参数和3D路标在一个状态向量中用概率表示，运算复杂度随路标个数二次增长。

视觉SLAM仅使用视觉特征解决SLAM问题，通过仔细提取并匹配大量精致的视觉特征，能够提供较高跟踪精度。

视觉惯性SLAM一般不需要数量特别大的图像特征以便达到合理精度，因为惯性测量提供了额外约束。MSCKF等在EKF SLAM基础上，从状态向量中去除了3D路标，能够在状态中建模多帧；然而与EKF算法类似，它们仅能够维护最新状态，从而对测量噪声敏感，很难从不稳定跟踪状态中恢复。VINS-MONO等算法使用滑动窗口选择最近视觉和惯性测量优化目标函数，很多情况下滑窗VI-SLAM比基于滤波的算法更稳健、更精确。然而目标函数优化运算量非常大，滑窗VI-SLAM高度依赖于运算能力，在移动设备应用中很受限。本文提出的算法可以显著提高优化求解效率。

很多SLAM算法优化求解部分使用通用的实现方式，尽管它们前端和框架直接区别很大。时间SLAM的BA利用了重投影函数的稀疏性结构和Hessian。本文将BA扩展至联合优化视觉和惯性测量。一些学者提高了大尺度配置时的BA，Jeong等人指出分块PCG（preconditioned conjugate gradient）算法能够用于解决Schur补空间以提高效率。ceres和g2o中很好的工程实现了BA，并且应用于state-of-the-art SLAM系统中。然而所有这些方法都有一个缺点：复杂度随图像帧数二次增长，因此基于这些求解器的SLAM系统仅仅能够使用非常有限的测量，以便实现实时位姿估计。

增量求解器试图利用以前的优化结果降低新计算的运算量。iSAM2通过测量矩阵QR分解进行优化。每次迭代仅仅更新很小一部分分解结果而不是分解整个图。SLAM++增量恢复估计和方差，在BA中增量更新Schur补空间。然而上述算法仅适用于解决稀疏相机问题（大部分关键点仅仅在很少帧中可见），这符合大规模SfM问题，但在SLAM问题中，局部滑动窗口内的大部分帧共视很大部分的关键点，从而使得上述增量算法演变为常规BA求解器，因此定位精度并不能比其它的state-of-the-art SLAM算法好。本文提出的新的增量求解器，更好的利用了SLAM中的块矩阵结构，在速度和精度上都有显著提升。此外，本文进一步讨论了局部BA的加速和全局一致性相对边缘化，提供了更多的实验结果。据我们所知，本文是第一次提出了VI-SLAM BA求解器，在达到stahe-of-the-art精度的同时，实现了前所未有的效率提升，同时保障了全局一致性。

6 评估

为了评估本文提出的优化求解器，构建了一个SLAM系统，前端检测Harris特征，使用光流进行帧间特征跟踪，双目帧间直接匹配特征；闭环检测使用关键帧提取的词袋特征，一旦检测到闭环，使用匹配帧间的相对位姿和方差作为全局BA的相对约束。

使用EuRoC数据集进行定量评估，并与Google Tango进行对比。滑窗大小设为50，更大的滑窗增加不了精度还降低了效率。

定位精度对比如下表所示，OKVIS和SVO都是VIO，iSAM2使用与本文相同的特征跟踪，没有闭环。为公平起见，我们给出了本文算法带闭环检测和不带闭环检测的结果。不带闭环检测时，本文算法精度已经比state-of-the-art好，因为滑窗使用了50帧；带闭环检测时，相对约束使得本文算法RMSE明显降低。

在Intel i7 CPU上不同求解器耗时如下表所示，禁用了多线程，并且不包含前端处理。OKVIS滑动窗口使用5个关键帧和3个IMU帧，本文算法使用50帧情况下仍达到10倍加速。SVO的优化时间并不能直接测量，我们将我们系统前端结果输入到iSAM2估计SVO优化耗时。iSAM2是SVO使用的优化求解器，也是state-of-the-art增量求解器。我们也测试了使用g2o的ORB-SLAM优化时间，远远慢于我们的算法，一个原因在于ORB-SLAM需要更多的特征才能文件跟踪，如果将特征数从默认的1200降为490，ORB-SLAM在3个数据集上会跟踪失败。

在oct-core ARM CPU（4个A9和4个A15），单线程运行模式，本文算法在不带闭环和带闭环时，局部BA和全局BA耗时分别为12.18ms、78.14ms和193.72ms，因此具备应用在移动低功耗设备上的潜力。

Tango是稳健性和精度高度优化的运动跟踪商业设备，本文双目SLAM与Tango Phab 2对比结果如下图所示。不带闭环检测时，本文系统轨迹和Tango相比拟，尺度比它更精确；带闭环检测时，一致性比Tango更好。

英文名称：ICE-BA: Incremental, Consistent and Efficient Bundle Adjustment for Visual-Inertial SLAM

源码：https://github.com/baidu/ICE-BA