论文翻译-Accurate Monocular Visual-inertial SLAM using a Map-assisted EKF Approach

摘要

本文提出了一个新的紧耦合单目视觉惯性SLAM方法，能够在标准的CPU上进行精确、鲁棒的实时定位。首先，本文使用视觉惯性EKF产生一个高频的运动估计，但是由于线性化问题，其结果会变得不一致。因此，本文使用基于关键帧的视觉惯性BA以提高系统的一致性和精度。另外，一个闭环检测与校正的模块被加入到系统中以消除累积漂移误差。最终，优化后的运动估计和地图反馈到基于EKF的视觉惯性里程计模块中，进一步降低了EKF估计器的不一致性和估计误差。通过这种方式，该系统能够持续地提供可靠的运动估计。本文算法的性能在公开数据集和实验场景下进行了验证，证明了本文方法的优越性。

引言

本文的出发点在于，基于EKF的状态估计过程由于线性化会引入误差，因此，本文引入BA降低估计器的不一致性。在[1]中证明，BA技术由于迭代的进行线性化能够很好的处理非线性，因此能够获得较高的精度。但是，优化方法会产生较高的计算代价。另外，提高特征匹配和关键帧的数量能够显著提升精度，然而特征的提取与匹配，优化过程都具有较高的计算量。因此，如果我们提高特征数量，在EKF之后执行BA优化，系统将不能实时运行。为了保证系统的实时运行以及高精度的建图能力，我们只对关键帧提取较大数量的特征，使用这些特征进行全局地图的构建和BA并行优化。
本文所提出的系统分为三个线程进行：1. 执行EKF VIO; 2. BA; 3.闭环。

相关工作

最初，单目SLAM问题被基于滤波的方法解决，一般都是在KF的框架下对均值和方差进行操作。基于滤波的方法需要较少的计算资源，由于不断地将历时状态边缘化，但是由于线性化误差，系统的精度收到影响。根据对观测信息的处理方式的不同，滤波方法可以分为两种：扩展卡尔曼滤波的方法和滑窗方法。基于EKF的SLAM方法中，状态向量包含了位姿以及特征位置，因此只要这些特征被持续的观测到并被包含到当前的状态向量中，所估计的位姿相对于这些特征就不会漂移。但是具有较高的计算复杂度（特征数量的二次方）；因此，只有当前被观测到的特征被跟踪以保证实时运行。相反，滑窗滤波方法维持一个滑窗包含历史的相机位姿，使用特征观测在这些位姿上增加概率约束。因此，通过将特征排除在状态向量之外，使得计算复杂度是特征数量的线性关系。一般，VIO问题具有四个不可观测的方向，但是，由于具有线性化误差，系统只具有三个不可观测的方向（不懂？）。
OKVIS提出了一种将惯性观测紧耦合到基于关键帧的视觉SLAM中的方法，使得非线性代价函数包含IMU误差项和特征的重投影误差。另外的，将旧的状态边缘化被用来保持固定大小的优化窗口。因此，OKVIS能够实现较精确、鲁棒的实时定位。但是，系统需要不断地计算IMU积分当线性化点改变。为了消除重复的计算，[15]提出预积分理论。然后，预积分IMU模型和structureless视觉模型被无缝地以一种全概率的方法集成到一起，以建立一个计算更加高效的优化方法。因此，系统的精度比Project Tango要高。
基于滤波的方法和基于优化的方法都有其优点，我们紧密的将两种方法融合在一起以实现最高的精度、鲁棒性和效率。

Notations

VIO Description

在本节中，我们介绍EKF VIO系统，其基于[24]。惯性观测用于预测运动，然后基于匹配的视觉特征更新状态。通过这种方式，我们可以高效地估计body坐标系的状态。
A. Full State Vector
需要被估计的向量包括IMU状态和一系列特征点参数：
$${\mathbf{X}}_{\mathbf{k}}=[{\mathbf{X}}_{B_k}^T,{\mathbf{X}}_{L_k}^T{]}^T$$
其中，IMU状态可表示为
$${\mathbf{X}}_{B_k}=[{{\xi }_{B_k}^W}^T,{{\mathbf{p}}_{B_k}^W}^T,{{\mathbf{v}}_{B_k}^W}^T,{\mathbf{b}}_{a_k}^T,{\mathbf{b}}_{g_k}^T{]}^T$$
其中，${\xi }_{B_k}^W\in {\mathbb{R}}^3$是从body坐标系到世界坐标系的旋转矩阵的李代数。IMU的误差向量为
$$\delta {\mathbf{X}}_{B_k}=[\delta {\xi }_{B_k}^T,\delta {{\mathbf{p}}_{B_k}^W}^T,\delta {{\mathbf{v}}_{B_k}^W}^T,\delta {\mathbf{b}}_{a_k}^T,\delta {\mathbf{b}}_{g_k}^T{]}^T$$
假设在$k$时刻，$m$个特征被包含在地图中，然后特征的坐标为
$${\mathbf{X}}_{L_k}=[{{\mathbf{f}}_1^W}^T,\dots ,{{\mathbf{f}}_m^W}^T{]}^T$$
其中第$l$个特征的参数表达为
$${\mathbf{f}}_l^W=[x_l,y_l,z_l,{\theta }_l,{\varphi }_l,{\rho }_l{]}^T$$
其中，$x_l,y_l,z_l$是第一次观测到特征的相机位置，${\theta }_l,{\varphi }_l$是从光心到特征点的单位射线的azimuth和elevation角度，${\rho }_l$是逆深度。
因此，EKF的误差向量为
$$\delta {\mathbf{X}}_k=[\delta {\mathbf{X}}_{B_k}^T,\delta {{\mathbf{f}}_1^W}^T,\dots ,\delta {{\mathbf{f}}_m^W}^T{]}^T$$
B. IMU Propagation Model
与其他视觉惯性方法不同，我们直接定义离散时间的状态迭代模型。使用所测量的加速度和角速度，离散时间迭代模型${\mathbf{X}}_{B_{k|k-1}}={\mathbf{f}}_k({\mathbf{X}}_{B_{k-1}})$为

本文忽略随机游走模型的惯性观测偏置，线性化离散时间IMU误差状态迭代模型为

C. Measurement Model
特征点的全局三维坐标可以表示为：

因此，将第$l$特征点投影到第$k$个图片的观测模型表示为

D. Filter Update

E. State Augmentation

Note: 感觉这里在计算${\sum }_{h\rho }$的时候有点问题，${\sigma }_{\rho }$是一维的，而${\sum }_{h\rho }$是$6\times 6$维度的。

视觉惯性BA

一旦一个图像帧被选择为关键帧，我们应用非线性优化方法提高所估计状态的精度。本节，我们将视觉和惯性观测融合进一个统一的框架中。
A. BA Representation

BA优化中状态包括了一系列IMU的状态和全局特征点的状态。代价函数包含了先验偏差，IMU偏差和重投影偏差。使用的是Hube核函数。注意是对关键帧对应的IMU状态进行优化。

B. Inertial Measurement Model
IMU测量获取的频率远高于相机观测，为了避免当线性化点改变的时候频繁的积分，本文采用IMU预积分，其与初始状态相独立，能够将IMU的偏置的变化融合进来。

给定一个偏置的更新$\delta \mathbf{b}$,使用一阶展开，IMU预积分可以表示为

根据几何约束，我们可以得到IMU的观测为

则，IMU预积分的残差为
C. Bias Model
IMU偏置是随着时间缓慢变化的，

D. Viusal Measurement Model

E. Error Term Representation

紧耦合的单目VISLAM

本节，我们介绍一种紧耦合的单目VISLAM系统，其将基于EKF更新与BA优化的VIO前端与闭环后端结合在一起，产生高精度、鲁棒的状态估计。
A. Map Initialization
地图初始化用于构建一个初始地图点用于后续的非线性优化和闭环。初始地图的构建是根据EKF VIO所估计的状态。由于初始的地图精度对整个系统的精度有较大的影响，我们在EKF系统收敛之后构建初始地图。
首先提取ORB特征，然后搜索得到特征匹配。然后进行三角化。如果得到足够多的map point，一个包含重投影误差和IMU偏差的full BA用于优化初始地图。最终，使用优化后的状态，优化后的状态和地图对EKF状态进行校正。

B. Front-End
1.State Estimation:我们首先执行VIO对${\mathbf{X}}_{B_k}$进行估计，为了保证实时性能，我们只在当前帧能够观测到的一定数目的特征点。最终，如果地图在后端被优化，当前帧的状态将根据反馈机制进行校正。在本文中，前端能够给出较为可靠的状态估计。
2.Keyframe Selection：选择标准：(1)与上一帧的时间间隔；（2）后端的非线性优化完成。(3) 相比于上一帧的旋转角度超过一个阈值。
C. Back-end
一旦一个关键帧被插入，非线性优化就会执行。

D. Feedback Mechnism
基于EKF的VIO能够对相机状态进行高效估计，但是由于累积误差，and the absence of the loop closure, 状态估计的误差会随着时间积累。如果前段提供的状态估计漂移的太厉害，local BA很难去最终得到较好的优化结果。因此，为了对状态估计的误差进行约束，本文提出了反馈机制。包括以下两个步骤：
(1) 状态校正。在后端执行完BA和闭环之后，最后一个关键帧的状态估计已经足够精确。利用优化后的状态对当前帧的状态估计进行校正。因此，
(2) EKF地图校正

实验