ORB-SLAM系统优化框架分析概述

邱笑晨，赵晨旭，张 海，许 辉

(1.北京航空航天大学自动化学院，北京 100083；2.北京航空航天大学中法工程师学院，北京 100083；3.北京公共交通控股(集团)有限公司，北京 100161)

0 引言

同步定位与地图构建[1](Simultaneous Localization and Mapping, SLAM)是指搭载特定传感器的运动主体，在没有先验环境信息的条件下，在运动过程中建立环境的模型，并估计自己的实时运动状态的方法[2]。若这里的传感器为相机，则称之为视觉SLAM。

目前，主流的视觉SLAM方法按照估计框架可以分为基于滤波的方法和基于关键帧光束平差法(Bundle Adjustment, BA)[3]。按照图像信息的利用程度可以分为基于特征点法和基于直接法(直接利用像素信息)。基于滤波的视觉SLAM的主要思想是将每一时刻t的系统状态用一个高斯概率模型表示，且系统状态由一个滤波器不断更新，从而得到不同的SLAM系统，其中最具代表性的是MonoSLAM[2]和MSCKF[4]。并行追踪与制图(Parallel Tracking and Mapping, PTAM)由Klein等[5]于2007年提出，是最早期基于关键帧光束平差法的单目视觉SLAM系统，利用传统的特征点法实现。ORB-SLAM是Mur-Artal等[6]于2015年提出的基于PTAM算法框架改进的视觉SLAM系统，它利用了ORB(Oriented FAST and Rotated, BRIEF)特征，是目前性能最好的单目视觉SLAM系统之一，其二代版本加入了双目和RGB-D模式，进一步提高了其对不同传感器配置的适应能力。DTAM[7]和LSD-SLAM[8-9]是两种基于直接法的具有代表性的单目视觉SLAM算法，其主要特点是不依赖特征点的提取与匹配，通常在图像模糊、特征缺失等场景下鲁棒性更好。

从性能上，ORB-SLAM具有良好的泛用性，支持单目、双目、RGB-D等多种模式[10]；其回环检测算法保证了ORB-SLAM能够有效地抑制累计误差，并且利用相同手段实现的重定位功能使得系统在失效后能够迅速重定位；ORB-SLAM使用3个线程完成系统，实现了快速跟踪和建图，能够保证轨迹和建图的一致性[11]。

从功能上，可以将ORB-SLAM分为视觉里程计(Visual Odometry,VO)前端部分和回环(Loop Detection)后端部分。本文主要针对单目ORB-SLAM的系统架构进行简要介绍，以视觉导航中关键视觉里程计为对象，对影响系统精度的不同环节进行分析，结论中提出可能的改进措施。

1 ORB-SLAM系统概述

ORB-SLAM系统的算法部分主要由3个并行的线程组成，分别是：跟踪(Tracking)、局部建图(Local Mapping)和回环检测(Loop Closing)。此外，在初始化构建地图时会临时发起2个线程分别求解单应矩阵和本质矩阵，执行完毕后自行停止；在每次执行完回环位姿图优化后，会在回环检测线程中临时发起1个线程，执行全局BA优化，该线程在执行完毕后自行停止，或者被另一次全局BA优化打断。本节主要介绍单目ORB-SLAM的具体架构及优化框架。

1.1 系统架构分析

ORB-SLAM系统架构如图 1[6]所示，结合图示分析3个主要线程的基本功能如下所示。

图1 ORB-SLAM系统架构Fig.1 Overview of ORB-SLAM system structure

1)跟踪：线程输入为每一帧图像，在未初始化时，尝试利用两帧图像进行初始化。初始化完成后，对获得的每一帧图像，通过特征描述子实现图像特征点与局部地图点的有效匹配，运用光束平差法来最小化重投影误差，从而优化当前帧相机位姿，实现每帧图像采集时刻相机的跟踪和定位。满足特定条件时，将当前帧确定为关键帧。

2)局部建图：线程的输入为跟踪线程插入的关键帧。基于新添加的关键帧，维护和拓展新的局部地图点，并运用光束平差法优化局部地图中所有关键帧的位姿及局部地图中的所有地图点。同时对关键帧进行筛选，剔除冗余关键帧。

3)回环检测：线程的输入为经过局部建图筛选过的关键帧。将当前关键帧的词袋(Bag of Words，BoW)向量存入全局词袋数据库中，从而加速后续帧的匹配。同时检测是否存在回环，若存在则通过执行位姿图(Pose Graph)优化来优化全体关键帧位姿，抑制累计漂移误差。在位姿图优化完成之后，会临时发起一个独立线程执行全局光束平差法，来得到整个系统最优结构(地图点)和运动(关键帧位姿)的结果[12]。

1.2 单目ORB-SLAM中的光束平差法

ORB-SLAM系统在若干个部分采用光束平差法[13]进行优化，包括在跟踪过程中优化当前帧的位姿(纯运动BA)，在局部建图线程中优化局部地图中的关键帧位姿和地图点位置(局部BA)，以及在回环检测后优化所有的关键帧位姿和地图点位置(全局BA)。这三部分的主要内容分别如下：

(1)

式中，ρ是Huber代价函数；Σ是协方差矩阵，与特征点的尺度相关；πm是针孔相机投影函数，定义如下

(2)

其中，(fx,fy)是焦距，(cx,cy)是主点坐标。

由于上述过程不对地图点位置进行优化，相当于进行PnP，但ORB-SLAM作者称之为纯运动BA。

2)局部BA：同样通过最小化重投影误差，同时优化当前关键帧位姿、与其存在共视关系的局部关键帧的位姿TL以及局部关键帧所能观测到的所有地图点的三维坐标PL，并将与局部关键帧形成共视但不与当前关键帧构成共视的关键帧的位姿TF作为约束项参与优化。优化的过程中不改变TF，从而使得优化结果关于全局地图存在一致性。上述过程由公式描述如下

(3)

其中，χj为第j个局部关键帧匹配到地图点的特征点集合。

3)全局BA：具体过程与局部BA类似，其优化对象包括除了第一帧的其余所有关键帧位姿及全局地图中所有的地图点。初始帧位置被固定是为了保持全局一致性，消除随机化[12]。

2 系统精度分析

ORB-SLAM作为基于关键帧BA的视觉SLAM系统，在其运行过程中光束平差法的效果对于系统精度的影响至关重要。在单目ORB-SLAM视觉里程计部分(不包括回环和全局BA后端)，针对关键帧位姿的优化共有三种情况：第一种是当前帧成功匹配足够多的已有地图点后进行的第一次纯运动BA，优化得到当前帧的初始位姿；第二种是成功更新当前帧共视的局部关键帧和局部地图点后进行的第二次纯运动BA，进一步优化当前帧的位姿；第三种是当前帧被确定为关键帧后，针对当前关键帧及其所对应的局部关键帧集合和局部地图点集合的局部BA优化，实现局部地图的整体优化，三种BA的优化作用分析如下。

2.1 首次纯运动BA优化

系统在成功初始化后，会对新来的图像帧进行ORB特征点的提取和对应描述子的计算，然后通过基于速度的模型或基于参考关键帧的模型进行图像特征点与已有地图点的匹配，并获得当前帧的初始位姿。其中基于速度的模型主要适用于载体匀速运动的情况，而基于参考关键帧的模型则适用于其他情况。在成功获得足够多的匹配点对后，通过纯运动BA优化(相当于进行PnP)当前帧位姿。此次纯运动BA的输入量是当前帧已得到的特征点与地图点的匹配关系，输出量为优化后的当前帧位姿。

图 2展示了2组来自KITTI数据集[14]在不同场景下的数据，在首次纯运动BA优化前后的关键帧位姿与真值进行尺度与坐标系对齐[15]后，得到的运动轨迹示意图。

通过对图 2(b)中的轨迹曲线分析可知：若SLAM系统在场景中的运动较为平滑，则在纯运动BA优化前基于速度模型或者参考关键帧模型得到的位姿初值相比于真值的误差已经不是很大，此时此次BA优化的效果并不显著。通过轨迹曲线图也可以看出，优化前后的关键帧位置几乎重合。

(a)基于KITTI数据集004序列得到的轨迹曲线图

(b)基于010序列得到的轨迹曲线图图2 首次纯运动BA优化后的KITTI数据集轨迹曲线Fig.2 Trajectory curves of sequences of KITTIdataset after first motion-only BA optimization

而从图 2(a)中的曲线图可以看出，由于实际场景中存在载体或传感器抖动，即便是在直路行驶的车辆，其位置在微观上也会存在抖动(注意横轴坐标间距相对纵轴而言很小)；又由于传感器测量误差和算法等因素影响，其估计结果存在更加明显的抖动。但从图中可以看出，相比较而言纯运动BA优化前的抖动更加剧烈。纯运动BA能够有效地平滑估计结果，利用已有匹配地图点的位置约束修正当前帧的位姿，从而在一定程度上抑制积累误差，避免将过大的误差带入到后续的优化过程中。

2.2 第二次纯运动BA优化

若当前帧的初始位姿被纯运动BA修正并剔除错误的匹配点对后，仍有足够多的正确匹配，系统则判定跟踪成功。此时系统会基于修正后的当前帧位姿尝试匹配更多的地图点，并更新当前帧的共视局部关键帧集合和局部地图点集合，然后基于这些局部信息进行第二次纯运动BA优化，进一步修正当前位姿。此次纯运动BA的输入量为更新后的当前帧所有特征点与地图点的匹配关系，输出量为进一步修正的当前帧位姿。

图 3所示为基于与2.1节相同的数据集得到的第二次纯运动BA优化前后当前帧位姿与真值的比较结果。

由图 3(b)中的轨迹曲线可知，从宏观角度看，若当前帧的位姿误差已经小于系统修正误差的最小范围，即当前帧的位姿已经足够准确，则此次纯运动BA不再起到很明显的作用。

(a)基于KITTI数据集004序列得到的轨迹曲线图

(b)基于010序列得到的轨迹曲线图图3 第二次纯运动BA优化后的KITTI数据集轨迹曲线Fig.3 Trajectory curves of sequences of KITTIdataset after second motion-only BA optimization

而由图 3(a)的比较结果可以看出，每次优化前的当前帧位姿相比于真值已经没有特别大的偏差，但是相对优化后的结果而言，部分关键帧位置仍然存在着一些不合理的跳跃现象，主要是因为首次纯运动BA优化当前帧位姿时并未充分地利用所有潜在的匹配地图点的约束信息。因此，第二次纯运动BA优化当前帧位姿后，不仅进一步修正了误差较大的帧位姿，同时在有限的地图点位置约束下使不同时刻的当前帧的位姿的变化尽可能的光滑。此时较大的位姿误差已经得到尽可能的修正，接着会通过局部BA的优化调整所有局部关键帧的位姿及局部地图点的世界坐标系坐标，从而使得位姿变化更加光滑。

此处要求位姿变化是光滑的，是因为每次的当前关键帧位姿都是基于已被修正过的局部关键帧位姿得到的，在当前关键帧引入到系统的位姿误差被约束到合理范围的前提下，不同时刻的当前关键帧的位姿变化应当是平滑的。

2.3 局部BA优化

若第二次纯运动BA修正当前帧位姿后仍能得到足够多的有效匹配地图点对，且当前帧满足被设置成关键帧的条件，即当前帧与其局部关键帧的相同的地图匹配点数量不超过90%，且满足局部建图线程空闲或距离上一次插入关键帧的时间超过阈值，当前帧便被设置成关键帧。接着基于当前关键帧三角化新的局部地图点并剔除不合理的局部地图点，然后针对局部地图中所有的关键帧及地图点进行一次局部BA优化。此次局部BA优化的输入量是当前关键帧所在的局部地图中所有的局部关键帧位姿和所有局部关键帧中特征点与地图点的匹配关系，同时利用所有与局部关键帧形成共视关系，但不与当前关键帧形成共视关系的关键帧位姿来约束优化结果，这些位姿在优化过程中保持不变，输出量是所有局部关键帧优化后的位姿和局部地图点优化后的三维坐标。

图 4的三幅曲线图是基于同一数据集局部BA优化得到的轨迹曲线图的不同表现形式。其中(a)和(b)图的含义是：在一次共有83个关键帧生成的系统运行过程中，第74个和第75个关键帧分别插入时，当前所有的局部关键帧在局部BA优化前后的轨迹曲线图以及全部轨迹真值，而(c)图表示所有不同时刻的当前关键帧在局部BA优化前后的轨迹曲线以及真值。第一种方式侧重于横向展示多次局部BA对局部关键帧位姿的连续性影响，而第二种方式则侧重于纵向展示每次BA对当前新插入关键帧的直接性影响。

从图 4(c)的比较结果可以看出，当前关键帧的位姿在局部BA中得到了进一步的修正，因此轨迹曲线变得更加平滑。考虑到局部BA优化是将所有局部关键帧中局部地图点的重投影误差作为代价函数，即在有效修正位姿误差较大的关键帧的同时，不断将系统模型带来的误差平均到所有局部关键帧位姿上。而从图 4(a)和(b)的轨迹曲线可以看出，当前关键帧会在之后有限个关键帧插入时再次参与其局部BA的优化，从而不断平均系统位姿解算误差，使得最终系统输出的关键帧位姿曲线变得十分平滑，且每个关键帧位姿的误差均在一定的范围内，如图 5所示。

(a)

(b)

(c)图4 (a)和(b)表示基于KITTI数据集004序列得到的，第74帧关键帧和第75帧关键帧插入时所有局部关键帧优化前后的轨迹曲线示意图;(c)表示不同时刻的当前关键帧在局部BA优化前后的轨迹曲线图Fig.4 (a) and (b) respectively show the trajectory curves of alllocal keyframes before and after optimization when the 74thkeyfra-me and the 75thkeyframe insert based on the sequence 004 ofKITTI dataset; (c) indicates the trajectory curve of the currentkeyframe before and after local BA optimization at different times

图5 基于KITTI数据集004序列，Visual Odometry部分得到的最终优化的全局关键帧轨迹示意图Fig.5 Final optimized keyframe trajectory graph obtained by thevisual odometry part based on the 004 sequence of KITTI dataset

3 结论

作为基于关键帧光束平差法的代表方案，ORB-SLAM系统在有效修正误差较大的全局关键帧位姿的同时，通过不断调整局部关键帧位姿和局部地图点在世界坐标系中的三维坐标，在系统误差范围内保证位姿变化的连贯性和平滑性。此外，ORB-SLAM还通过各种约束条件和加速手段保证了系统的实时性和鲁棒性，并通过回环检测的方法进一步精细化修正全局地图和关键帧位姿，有效地提高了系统的性能和精度。

然而，由于ORB-SLAM严重依赖特征点的提取与匹配效果，因此在如纹理缺失的环境下运行会无法得到足够多的稳定匹配点对，此时光束平差法缺乏足够的输入信息，无法有效地修正位姿偏差。这种情况下，可以引入外界的观测如惯性测量单元(Inertial Measurement Unit, IMU)等，来约束和修正特征缺失带来的偏差，针对单目情况，还能得到绝对尺度。近年来也有越来越多的研究偏向于视觉/惯性视觉里程计(Visual/Inertial Odometry)方向[16-18]，以期望解决纯视觉SLAM存在的问题。