同时定位与地图构建(SLAM)是指移动机器人即使在未知环境中也能通过传感器建立环境模型并确定自身位置。SLAM技术可应用于许多领域，如自动驾驶、增强和虚拟现实、移动机器人^[1]和无人机导航。然而现实生活中的运动往往比较复杂，例如相机运动过快时产生的图像运动模糊、以及在高亮或低亮、环境纹理特征较弱等情况下都会导致视觉SLAM跟踪失败，因此仅仅通过视觉传感器已经无法满足实际应用需求。而惯性传感器因其能在短时间内的快速运动中提供较好的状态估计，与相机具有明显的互补性，加之在移动设备中基本都同时具有相机与惯性传感器，视觉惯性融合的SLAM算法已成为当今研究热点。现在主流的视觉惯性融合框架是基于非线性或滤波优化的紧耦合(将IMU(inertial measurement unit)状态和相机的状态联合优化)方法。在滤波方面，传统的EKF(extended kalman filter)^[2]和MSCKF(multi state constraint kalman filter)^[3]都取得了一定的进展，但仍然存在一定的缺陷，Bloesch等人^[4]提出的ROVIO(robust visual inertial odometry)算法是基于像素点的EKF滤波优化融合算法，体现了极高的鲁棒性。在非线性优化方面，Leutenegger等人^[5]提出了一种基于关键帧的非线性优化的方法将视觉和IMU测量进行融合。无论是滤波的方案还是优化的方案，尽管都提高了系统的鲁棒性，但是在实际应用中由于环境和运动的复杂性，仍然可能会跟踪失败。

目前采用最多的解决跟踪失败的方案是重定位。在基于滤波的SLAM系统框架中，Williams^[6]提出了一种利用3点位姿算法估计当前帧的位姿的重定位技术。

而基于关键帧的重定位技术主要分为两种^[7]：

第1种是图像到图像的方法，即通过寻找当前帧与地图中的帧之间的对应关系，确定当前帧的位姿。目前使用最多的为Bag of Words^[8]模型，通过将词袋与特征点结合对图像进行描述^[9]，从而检测图像之间的相似性，再依据相似的场景往往具有相似的位姿这一假设，实现重定位。例如在PTAM(parallel tracking and mapping)^[10]，ORB_SLAM(oriented brief simultaneous localization and mapping)^[11]等基于关键帧的单目SLAM算法均采用这种策略进行重定位。上述方法只有当相机采集到的图像与已建好的地图中的某帧关键帧非常相似时，才能恢复出当前相机的位姿，从而重定位成功。在实际应用中的表现形式为，当系统丢失后设备必须回到之前能跟踪上的场景中系统才能继续跟踪建图。但是在许多应用场景中，如无人驾驶汽车和智能无人机需要一直前进时，这种重定位方式并不适用。

第2种则是图像到地图的方法，即从当前帧图像的特征点中寻找与地图中的地图点的对应关系。Straub等人^[12]提出将当前帧的描述符与存储在地图中与地图点相关联的描述符进行匹配，从而估计当前帧的位姿。Moteki等人^[13]提出了一种将这两种方法结合的重定位方法，根据当前帧和目标帧之间几何模型采用不同的方法。这些重定位的方式要求当前场景与地图具有较高的相似性。若系统丢失后，一直未检测到相似的场景，则无法继续建图定位，直到重定位成功，那么系统跟踪失败之后到重定位成功之前的地图信息均无法恢复。

针对此问题，提出了一种基于IMU的地图恢复融合算法解决跟踪失败问题。算法基于单目ORB_SLAM的框架，在系统跟踪失败后，即使无法满足重定位要求，也能继续进行跟踪建图。该方法对场景的依赖较弱，只要初始化成功，就能保存重定位之前的地图信息。实验结果表明, 对同一数据集, 融合后的相机轨迹与系统未丢失情况下的相机轨迹相比误差为厘米级别，并在某些运动场景境下，优于ORB_SLAM的重定位方法。

在SLAM算法中，当系统跟踪失败后，一般是通过重定位算法恢复相机位置，再重新进行跟踪。但是如果无法满足重定位的条件，系统将一直处于丢失状态而无法继续进行建图。本文算法在视觉跟踪失败后立刻进行重新初始化，并继续跟踪建图，从而解决上述SLAM系统跟踪失败后存在的问题，同时保存丢失前的地图。然后在适当的时候融合前后两个地图。因为新地图的世界坐标系和旧地图的世界坐标系的坐标原点、坐标方向以及尺度均不相同，要实现地图融合，就必须统一两个坐标系。因此本文通过IMU提供的信息，计算两个地图坐标系之间的转换关系，从而实现新旧地图融合。但是IMU数据存在一定的误差，特别是手持智能终端, 往往采用廉价的IMU设备，从而导致误差更大，因此仅仅依靠IMU数据进行融合得到的结果并不够精确。所以本文使用IMU数据计算粗略转换关系，然后在此基础上进一步开发了一种快速匹配策略得到前后两个地图的地图点匹配关系，通过非线性优化算法计算匹配点组间的运动估计，补偿IMU误差，并对匹配上的地图点进行融合。这么做的优点在于不仅仅是将前后两个地图进行拼接，而且建立了前后两个地图关键帧与地图点之间的联系，达到真正意义上的融合，使得在后续的跟踪建图中，能够同时使用两个地图的信息进行优化。

由于ORB_SLAM算法框架比较完善，具有极高的精度和鲁棒性，因此将ORB_SLAM框架作为地图融合算法的基本框架并在其基础上进行修改，本文算法框架如图 1所示。

ORB_SLAM的的算法框架可分为3个线程，分别为跟踪、局部建图和回环检测，本文算法将跟踪线程改为丢失后不进行重定位，而是重新进行初始化，新建第2个地图，同时保存丢失前的已建好的地图，在局部建图线程中加入尺度估计算法。此外，还增加了一个新的线程进行地图融的过程。地图融合算法的伪代码如下:

1) if scale_compute_done then

2) lastmap_to_currentmap(imu_rt); /*将前一个地图坐标系通过IMU计算得到的转换关系转换到当前地图的坐标系中*/

3) if first_fusion then

4) matched_points⇐find_matched_mappoints(lastmap, currentmap); /*寻找两个地图之间的匹配点*/

5) else

6) kf⇐wait_for_next_keyframe();

7) matched_points⇐find_matched_mappoints(lastmap, kf); /*寻找新的关键帧与丢失前的地图的匹配点*/

8) end if

9) if mathed_points not null then

10)error_rt⇐estimate_imu_error(matched_points);

/*计算匹配点之间的运动估计*/

11)lastmap_to_currentmap(error_rt);

12)end if

13)end if

单目SLAM系统中由于深度信息未知，无法得到运动轨迹以及地图的真实大小，生成的地图的尺度是任意的。因此，每次初始化后的地图之间存在一个尺度差异，进行地图融合之前首先必须统一尺度。采用Mur-Artal^[14]中提出的真实尺度估计方法，利用纯视觉SLAM的地图和IMU测量数据结合，通过预积分^[15]减少重复积分，并避免初始值的影响。算法首先将视觉估计得到的旋转变量和预积分IMU数据得到的旋转变量之差作为误差项，通过优化估计陀螺仪偏差${\boldsymbol{b}_g}$，优化公式为

$ \mathop {\min }\limits_{{\mathit{\boldsymbol{b}}_g}} \sum\limits_i^{N - 1} {{{\left\| {\log \left( {{{\left( {\Delta {\mathit{\boldsymbol{R}}_{i,i + 1}}{\rm{Exp}}\left( {\frac{{\partial \Delta {\mathit{\boldsymbol{R}}_{i,i + 1}}}}{{\partial {\mathit{\boldsymbol{b}}_g}}}\delta {\mathit{\boldsymbol{b}}_g}} \right)} \right)}^{\rm{T}}}\mathit{\boldsymbol{R}}_i^{\rm{T}}{\mathit{\boldsymbol{R}}_{i + 1}}} \right)} \right\|}^2}} $

(1)

式中，$\Delta {\boldsymbol{R}_{i, i + 1}}$表示第$i$帧到第$i$+1帧之间的旋转关系，通过预积分计算得到。${\boldsymbol{R}_i}$表示第$i$帧在IMU坐标系下姿态，由视觉计算得到的姿态通过标定转换得到。

然后利用帧间相机坐标系和IMU坐标系之间的转换关系估计SLAM的尺度$S$；最后引入重力量级，将重力加速度分解为重力方向向量和重力量级，用惯性传感器中的重力加速度表示世界坐标系下的重力加速度, 从而分离出加速度中重力加速度的部分, 得到加速度偏差${\boldsymbol{b}_a}$, 获得更加准确的尺度$S$。尺度计算完成后, 将当前的地图尺度转换到真实的尺度下。当视觉跟踪失败后，第二次初始化之后仍然需要计算新地图的尺度并将新地图转换到真实尺度下，从而使两个地图的尺度统一。

单目视觉SLAM系统创建的地图往往是以第1帧关键帧的相机坐标系作为世界坐标系, 因此，当SLAM系统重新初始化后, 系统的世界坐标系也会改变。IMU坐标系、世界坐标系和相机坐标系相互之间都存在一个转换关系，各坐标系之间的关系如图 2所示。

当视觉跟踪失败，系统重新初始化创建的地图与丢失前创建的地图的坐标系不同。但在世界坐标系下，两个地图之间是有关联的。系统跟踪失败前创建的地图坐标系到重新初始化之后创建的地图坐标系之间的旋转和平移关系分别用${\boldsymbol{R}_{sf}}$和${T_{sf}}$表示。实际上，就是在同一个世界坐标系下跟踪失败前创建的地图第1帧关键帧到重新初始化后创建地图第1帧关键帧之间的转换。它们主要由两部分组成，如图 3所示，第1部分是丢失前地图的第1帧关键帧到最后1帧的转换关系，由视觉计算得到。第2部分是跟踪失败部分由IMU估计的转换关系, 计算公式为

$ \left\{ \begin{array}{l} {\mathit{\boldsymbol{R}}_{sf}} = {\mathit{\boldsymbol{R}}_{be}}{\mathit{\boldsymbol{R}}_{cw}}\\ {\mathit{\boldsymbol{T}}_{sf}} = S{\mathit{\boldsymbol{R}}_{be}}{\mathit{\boldsymbol{T}}_{cw}} + {\mathit{\boldsymbol{T}}_{be}} \end{array} \right. $

(2)

式中，$S$为估计得到的尺度因子，${\boldsymbol{R}_{be}}$、${\boldsymbol{T}_{be}}$表示在第1个地图中的世界坐标系下, 最后1帧到第2个地图第1帧关键帧的转换关系。${\boldsymbol{R}_{cw}}$、${\boldsymbol{T}_{cw}}$表示第1个地图中, 第1帧关键帧到最后1帧之间的转换关系。${\boldsymbol{R}_{be}}$、${\boldsymbol{T}_{be}}$与IMU估计的姿态${\boldsymbol{R}_{wi}}$、${\boldsymbol{T}_{wi}}$可表示为

$ \left\{ \begin{array}{l} {\mathit{\boldsymbol{R}}_{be}} = {\left( {{\mathit{\boldsymbol{R}}_{wi}}{\mathit{\boldsymbol{R}}_{ic}}} \right)^{ - 1}}\\ {\mathit{\boldsymbol{T}}_{be}} = - {\left( {{\mathit{\boldsymbol{R}}_{wi}}{\mathit{\boldsymbol{R}}_{ic}}} \right)^{ - 1}}\left( {{\mathit{\boldsymbol{R}}_{wi}}{\mathit{\boldsymbol{T}}_{ic}} + {\mathit{\boldsymbol{T}}_{wi}}} \right) \end{array} \right. $

(3)

式中，${\boldsymbol{R}_{ic}}$、${\boldsymbol{T}_{ic}}$表示VI(Visual-Inertial)设备中相机系统坐标系到IMU坐标系的转换关系，通过视觉惯性标定获得。${\boldsymbol{R}_{wi}}$、${\boldsymbol{T}_{wi}}$和速度变量${\boldsymbol{V}_{wi}}$计算公式为

$ \left\{ \begin{array}{l} {\mathit{\boldsymbol{R}}_{wi}}\left( {t + \Delta t} \right) = {\mathit{\boldsymbol{R}}_{wi}}\left( t \right)\exp \left( {\left( {{\mathit{\boldsymbol{\omega }}_i}\left( t \right) - {\mathit{\boldsymbol{b}}_g}} \right)\Delta t} \right)\\ {\mathit{\boldsymbol{v}}_{wi}}\left( {t + \Delta t} \right) = {\mathit{\boldsymbol{v}}_{wi}}\left( t \right) + {\mathit{\boldsymbol{R}}_{wi}}\left( t \right)\left( {{\mathit{\boldsymbol{a}}_i} - {\mathit{\boldsymbol{b}}_a}} \right)\Delta t\\ {\mathit{\boldsymbol{T}}_{wi}}\left( {t + \Delta t} \right) = {\mathit{\boldsymbol{T}}_{wi}}\left( t \right) + {\mathit{\boldsymbol{v}}_{wi}}\left( t \right)\Delta t + \\ \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\frac{1}{2}{\mathit{\boldsymbol{R}}_{wi}}\left( t \right)\left( {{\mathit{\boldsymbol{a}}_i} - {\mathit{\boldsymbol{b}}_a}} \right)\Delta {t^2} \end{array} \right. $

(4)

式中，${\boldsymbol{\omega} _i}(t)$表示IMU在$t$时刻测量得到瞬时角速度，${\boldsymbol{a}_{i}}$表示IMU在$t$时刻测量得到瞬时加速度。

通过IMU估计视觉跟踪失败的时间段内的相机位姿，可以将两个坐标系不同的地图转换到同一坐标系下。

虽然通过IMU和视觉结合可以估计出陀螺仪和加速度的偏差，但是IMU的测量数据仍然存在噪声，特别是求解位移需要对加速度进行两次积分，进一步累计了误差，导致地图融合产生较大的偏差。为解决这个问题，提出一种误差修正的方法，通过非线性优化求解两个地图点之间的运动估计。

求解两个地图点的运动估计，首先需要得到两个地图中一组匹配的3维点。为了找到这样的一组匹配点，传统的做法是遍历两个地图所有的关键帧，直接对两帧关键帧的关键点利用DBow2^[16]进行匹配，然后找到匹配的2维点对应的3维点。但是当关键帧数量增加时，这种算法会比较耗时，因此本文基于关键帧之间的共视关系，提出了一种改进的匹配策略，共分3种情况分别考虑，如图 4所示。需要注意的是，在地图融合的过程中，地图中的关键帧不增加。由于融合时间很短，并不会影响建图。

情况1，将丢失前的地图中的最后1帧关键帧${X_n}$和新地图的第1帧关键帧${Y_1}$进行地图点匹配, 因为对两个地图来说，这两帧在时间上是最接近的，那么空间上相对也比较接近，能够匹配的可能性比较大。若匹配成功的地图点数量小于5，那么${X_n}$直接跳过与${Y_1}$共视点最多的前10帧，而与接下来的关键帧进行匹配, 若遍历完新地图中所有的关键帧后，若匹配成功的地图点数都小于5，那么接下来就对${X_{n -10}}$进行匹配。直接跳至${X_{n -10}}$是因为，与${X_n}$相邻的几帧都与其具有极高的相似性，若与${X_n}$匹配上的概率很低，那么与其相邻几帧的匹配概率也相对较低。

情况2，若${X_n}$和${Y_1}$匹配上的地图点数量大于20, 那么对${X_n}$和${Y_1}$共视点最多${Y_1}$的前10帧进行匹配，为减少重复匹配，选取其中匹配成功的地图点数量最多的那一帧，完成后重新从${X_{n -10}}$开始对新地图中的关键帧进行匹配。

情况3，若${X_n}$与${Y_1}$配上的地图点数在5和20之间，那么继续与${Y_2}$行匹配，直到符合情况2或者对新地图中的关键帧都遍历结束，对${X_{n -1}}$和新地图中的关键帧进行匹配。

为了获得尽可能正确的匹配，当丢失前的地图中的每一帧关键帧遍历完所有的当前地图中的所有帧之后，保存下匹配成功最多的那组地图点。

若当前新建的地图与丢失前的的地图没有匹配上的地图点。那么每当新地图中新建一帧关键帧时，将当前关键帧与丢失前的地图按照上述策略进行搜索匹配，直到得到一组匹配的地图点。

通过这种跳跃式的遍历方式能有效减少匹配的时间。搜索匹配不同大小的两个地图的时间如表 1所示，基本稳定在0.08 s左右，满足系统实时的需要，而传统的方法搜索同样数量的关键帧则需要0.2 s左右，时间缩短了2.5倍。

得到一组匹配的地图点后，利用非线性优化求解两组3维点之间的运动估计。将都包含$n$个3维点的两组点集分别记为$\boldsymbol{P}$和$\boldsymbol{P}′$，这两组3维点之间的转换关系为$\boldsymbol{R} \in \boldsymbol{SO}$(3)，$\boldsymbol{t} \in {\mathbb{R}^3}, \boldsymbol{SO}$(3)为3维旋转群，那么构造优化函数为

$ \mathop {\min }\limits_{\mathit{\boldsymbol{R}},\mathit{\boldsymbol{t}}} \frac{1}{2}\sum\limits_{j = 1}^n {{{\left\| {{\mathit{\boldsymbol{P}}_j} - \left( {\mathit{\boldsymbol{R}}{{\mathit{\boldsymbol{P'}}}_j} + \mathit{\boldsymbol{t}}} \right)} \right\|}^2}} $

(5)

通过迭代优化求解得到两组3维点之间的运动估计结果，接着对旧地图进行相应的变换, 从而弥补IMU测量的误差。

通过对丢失前的地图按照式(5)优化得到的变换参数进行变换后，丢失前后两个地图即统一到同一个坐标系下。接下来描述两个地图进行融合的过程。在同一个地图中，关键帧和地图点之间是存在联系的，即某一关键帧上可以观察到哪些地图点，以及某个地图点出现在哪些关键帧中。仅将两个地图坐标系统一并不能真正将两个地图融合在一起，两个地图的关键帧和地图点并没有相互联系。因此，地图融合的关键就是将新(旧)地图中的关键帧和旧(新)地图中的地图点关联。在2.3.1节中通过搜索匹配已经得到两个地图之间的匹配点，用丢失前地图的地图点替代新建地图的地图点，相应的，原本新地图中与这些匹配点中的新地图点关联的关键帧将转换成与匹配点中旧地图点关联，从而达到了地图融合的目的。

地图融合结束后，后续的跟踪建图中将会同时使用两个地图的关键帧和地图点信息进行优化。

假设丢失前的地图点的3维坐标为${\boldsymbol{X}_l} = {[{x_l}, {y_l}, {z_l}]^{\rm{T}}}$，新建地图点3维坐标为${\boldsymbol{X}_c} = {[{x_c}, {y_c}, {z_c}]^{\rm{T}}}$，设重投影函数为$\pi $，其计算公式为

$ \pi \left( \mathit{\boldsymbol{X}} \right) = \left[ \begin{array}{l} {f_u}\frac{x}{z} + {c_u}\\ {f_v}\frac{y}{z} + {c_v} \end{array} \right] $

(6)

式中，$({f_u}, {f_v})$是焦距，$({c_u}, {c_v})$是光学中心的坐标。那么这两种地图点重投影到第$n$帧的重投影误差分别为

$ \left\{ \begin{array}{l} \mathit{\boldsymbol{e}}_n^c = \mathit{\boldsymbol{x}}_n^c - \pi \left( {{\mathit{\boldsymbol{R}}_{nw}}{\mathit{\boldsymbol{X}}_c} + {\mathit{\boldsymbol{t}}_{nw}}} \right)\\ \mathit{\boldsymbol{e}}_n^l = \mathit{\boldsymbol{x}}_n^l - \pi \left( {{\mathit{\boldsymbol{R}}_{nw}}{\mathit{\boldsymbol{X}}_l} + {\mathit{\boldsymbol{t}}_{nw}}} \right) \end{array} \right. $

(7)

式中，${\boldsymbol{R}_{nw}} \in \boldsymbol{SO}$(3), ${t_{nw}} \in {\mathbb{R}^3}$为地图点转换到第$n$帧的坐标系中的旋转平移关系，$\boldsymbol{x}_n^l$表示第$n$帧关键帧中能与丢失前地图匹配的关键点，$\boldsymbol{x}_n^c$表示第$n$帧关键帧中能与新建地图匹配的关键点。优化函数为

$ \min \frac{1}{2}\sum\limits_n {{{\left\| {\mathit{\boldsymbol{e}}_n^c + \mathit{\boldsymbol{e}}_n^l} \right\|}^2}} $

(8)

地图融合后，每当新的关键帧生成时，都会对当前关键帧附近的地图点进行优化，此时附近的关键帧的位姿是固定的。而在位姿优化中，地图点是固定的，只有相机姿态被优化。

地图融合后，当检测到回环时需要通过优化位姿图关闭回环修正地图。由于本文算法已经通过惯性视觉融合计算得到绝对尺度，因此不需要求解两帧之间的尺度差。优化位姿图只需要用到两帧位姿之间的变换，在优化求解两帧之间的变换时则同样需要进行联合优化。

假设关键帧1和关键帧2有一组匹配关系为$m \Rightarrow n$的关键点${\boldsymbol{x}_1}$和${\boldsymbol{x}_2}$，与关键点相关联的地图点${\boldsymbol{X}_1}$和${\boldsymbol{X}_2}$，其中包含了丢失前的地图点$\boldsymbol{X}_1^l$和$\boldsymbol{X}_2^l$，当前地图点$\boldsymbol{X}_1^c$，$\boldsymbol{X}_2^c$，那么关键点相应地分为$\boldsymbol{x}_1^l$, $\boldsymbol{x}_1^c$和$\boldsymbol{x}_2^l$, $\boldsymbol{x}_2^c$，重投影误差项为

$ \left\{ \begin{array}{l} \mathit{\boldsymbol{e}}_1^c = \mathit{\boldsymbol{x}}_{1,m}^c - \pi \left( {{\mathit{\boldsymbol{T}}_{12}}\mathit{\boldsymbol{\tilde X}}_{2,n}^c} \right)\\ \mathit{\boldsymbol{e}}_1^l = \mathit{\boldsymbol{x}}_{1,m}^l - \pi \left( {{\mathit{\boldsymbol{T}}_{12}}\mathit{\boldsymbol{\tilde X}}_{2,n}^l} \right)\\ \mathit{\boldsymbol{e}}_2^c = \mathit{\boldsymbol{x}}_{2,n}^c - \pi \left( {\mathit{\boldsymbol{T}}_{12}^{ - 1}\mathit{\boldsymbol{\tilde X}}_{1,m}^c} \right)\\ \mathit{\boldsymbol{e}}_2^c = \mathit{\boldsymbol{x}}_{2,n}^c - \pi \left( {\mathit{\boldsymbol{T}}_{12}^{ - 1}\mathit{\boldsymbol{\tilde X}}_{1,m}^c} \right) \end{array} \right. $

(9)

式中，${\boldsymbol{T}_{12}} \in \boldsymbol{SE}$(3)表示两帧之间的变换矩阵，其中包含3个自由度的旋转变换和3个自由度的平移变换。$\boldsymbol{\tilde X}$表示$\boldsymbol{X}$的其次坐标。通过最小化重投影误差得到的优化函数为

$ \mathop {\min }\limits_{{\mathit{\boldsymbol{T}}_{12}}} \frac{1}{2}\sum {{{\left\| {\mathit{\boldsymbol{e}}_1^c + \mathit{\boldsymbol{e}}_1^l + \mathit{\boldsymbol{e}}_2^c + \mathit{\boldsymbol{e}}_2^l} \right\|}^2}} $

(10)

在优化时，地图点是固定的，只优化变换矩阵

实验条件：一台4 GB内存的Intel Core i5-540M的笔记本电脑。实验数据: EuRoC数据集^[17], 其中包含由微型飞行器采集的不同的室内环境图像序列和IMU数据, 数据集的场景如图 5(a)-(c)所示。根据数据集的光照、纹理和运动模糊情况将数据分成3个等级，分别为easy、medium和difficult。图 5(d)场景数据由IMU设备3DM-GX3-25和摄像头UI-1221-LE组合而成的VI设备采集，频率分别为200 HZ和20 HZ，VI标定由Kalibr^[18]完成。

本实验分别从融合地图的精确性和完整性两方面进行比较。在精确性方面，由于ORB_SLAM算法在easy数据中和序列V2_02_meduim上并不会产生跟踪失败的情况，为测试算法，将对图像序列进行加噪处理，并将本文算法结果分别与ground truth和未处理数据的建图结果进行比较。在地图完整性方面，由于在光照变化剧烈或者运动模糊较多的数据中，ORB_SLAM算法会出现跟踪失败的情况，将本文算法结果和ORB_SLAM算法运行结果进行比较。

首先，在未进行处理的原始数据集上，用ORB_SLAM算法在序列V1_01_easy上的运行结果如图 6(b)所示。然后对数据进行处理，对数据图像序列中的连续10帧图像中进行加噪处理，这样原始ORB_SLAM算法会出现视觉跟踪失败的情况，通过运行本文算法进行恢复融合，可以得到地图融合后的结果。如图 6(a)所示，其中黄色部分为视觉跟踪失败前创建的地图点，红色和黑色部分是系统在跟踪失败后重新初始化后创建的地图点。紫色为系统丢失前创建的地图中的关键帧，蓝色为系统重新初始化后创建的新地图中的关键帧。通过结果图观察, 融合后的地图与原始未丢失的结果对比差异不大。为了得到更精确的对比结果，算法运行结束后, 保存融合后的地图的轨迹信息，分别与原始地图的关键帧位姿信息和数据集的ground truth进行对比。

图 7(a)-(c)为3个数据集融合后的相机轨迹和原始的相机轨迹的对比图，其中蓝色虚线部分为原始的未丢失的相机轨迹，红色实现为融合后的相机轨迹，而黑色打叉的位置是视觉跟踪失败的地方。为保证估计尺度的准确性, 需要有足够的时间进行尺度估计, 由于本文算法对尺度估计的时间要求并不高，但是尺度估计的准确性会对算法会产生影响，因此将计算尺度时间设置为15 s, 同时提供了足够时间让系统重新建图，使得新建地图有足够的数据与丢失前的地图进行匹配。实验证明，尺度在15 s的时间足以收敛，并只有1%的误差。当尺度估计结束后，地图融合时间如表 1所示，仅需0.18 s左右。

图 8(a)-(c)为3个数据集融合后的运动轨迹与其ground truth的对比结果，其中黑色部分为数据集运动轨迹的ground truth，蓝色为融合后的运动轨迹, 红色表示两者之间的差异。根据表可以得出, 在数据集easy上和序列V2_02_medium上，算法结果与ground truth对比，在30 m²的室内环境中，仅有9 cm的误差, 而在300 m²工厂环境中误差仅有7 cm。而与系统未出现丢失情况下的结果相比，30 m²室内环境下误差仅有1 cm左右，300 m²的工厂环境中误差仅4 cm。

最后采用Sturm提出的一种SLAM系统评估方法^[19]对融合后的地图的全局姿态误差进行评估，地图融合后的轨迹与数据集的ground truth对比, 其全局位姿误差如表 2所示。地图融合后的轨迹与系统未丢失情况下的轨迹结果进行对比，其全局位姿误差如表 3所示。

在数据V1_02_meduim上，不需要经过数据处理也会出现跟踪失败的情况。将本文算法与ORB_SLAM中重定位算法进行比较，在不同数据集上，本文算法得到的融合地图的关键帧数量和ORB_SLAM系统通过重定位后得到的地图关键帧数量如表 4所示，由本文算法得到的融合地图所包含的关键帧数量比ORB_SLAM创建的地图多30%，而多出的部分均为ORB_SLAM丢失部分的地图信息。本文算法与ORB_SLAM中重定位算法的轨迹结果如图 9所示，图 9(a)为ORB_SLAM在数据V1_02_meduim上的运行结果与ground truth的轨迹对比结果，图 9(b)为本文算法在数据V1_02_meduim上运行结果与ground truth的轨迹对比结果，其中黄色方框圈出的部分为ORB_SLAM算法得到的轨迹所缺失的部分。绿色方框所圈出的部分为本文算法轨迹所缺失的部分。通过两者对比可以很清楚地看出，虽然两者得到的轨迹均有缺失，但是本文算法缺失部分更少，保存的地图更为完整。图 9(c)表示本文算法和ORB_SLAM的关键帧分布对比图，图中红色表示本文算法在数据V1_02_medium上得到的地图关键帧，蓝色表示ORB_SLAM得到的地图关键帧。

两个地图与ground truth相比，本文算法全局位姿误差的均方根误差为0.086 m，ORB_SLAM的全局位姿误差的均方根误差为0.062 m。误差仅差0.02 m。

在运动较为剧烈的情况下，例如在图 5(d)的实验数据中，由于相机运动较为剧烈，在系统初始化不久后就产生丢失的情况，ORB_SLAM算法运行结果如图 10(a)所示，系统跟踪失败之后，由于场景变化，无法实现重定位，等相机运动回到之前建图的场景附近后，重定位成功，但是系统跟踪失败之后到重定位成功之前的数据丢失，导致所建的地图只停留在运动开始的场景中。由于地图信息较少，无法产生回环，对后续的建图和优化也产生了影响。本文算法运行结果如图 10(b)所示，在系统跟踪失败后，立刻进行初始化，在重定位成功之前保存下地图信息，当两个地图融合成功后，地图信息相对完整，在后续建图中保证了地图的鲁棒性。如图 10所示，图 10(a)中用矩形框框出的重建的场景为图 10(b)中的椭圆圈出的场景，本文算法在系统跟踪失败后实现继续跟踪的同时也保证了地图的完整性。

基于视觉的SLAM系统在实际应用中往往会出现跟踪失败导致的定位丢失的问题。本文在结合IMU数据的基础上，提出了一种有效的策略解决这一问题。当系统出现跟踪失败的情况时，立刻重新初始化，并创建一个新的地图，通过IMU测量跟踪失败时间段内的相机姿态，将丢失前的地图融合到新创建的地图中。实验结果表明，在地图精确性方面，算法能有效解决跟踪失败后无法继续进行定位和建图的问题，并且和系统未丢失前相比，建图和跟踪的精度仅有1~5 cm的误差，与ground truth相比，误差在10 cm以内。同时，本文算法在实际应用当中也能提高用户体验，在运动过程中，即使跟踪失败，也能保持当前的运动状态，来达到重定位的效果。此外，与原始ORB_SLAM算法相比，本文提出的地图恢复融合方法能获得更加完整的地图。由于ORB_SLAM的重定位方法很大程度上依赖于丢失前的地图，而本文提出的方法更依赖于初始化，因此在当地图初始化不久后就丢失的情况，本文算法建图效果更好。

但是通过本文算法恢复的地图其精度略差于ORB_SLAM的重定位算法，因此提高融合地图的精度是下一步研究的方向。