[论文笔记|VIO]:Fast and Robust Initialization for Visual-Inertial SLAM

摘要——VI-SLAM要求对初始速度、重力的朝向、陀螺仪和加速度计的偏置进行良好的初始化估计。本文以Martinelli（closed-form solution of visual-inertial structure from motion）提出的、Kaiser等人在(1)基础上扩展的初始化方法(Simultaneous State Initialization and Gyroscope Bias Calibration in Visual Inertial Aided Navigation)为基础，对其进行了改进，使其更通用、更高效。我们通过几轮视觉-惯性BA来提高精度，并通过新的可观测性和一致性检验来验证该方法，从而抛弃掉错误的解。实验在EuRoC数据集上的结果表明，虽然原始方法产生的刻度误差高达156%，但本篇论文的方法能够在不到2秒的时间内一致地初始化，刻度误差在5%左右，10秒后执行视觉-惯性BA可以进一步降低到不到1%。

【补充】闭式求解的方法的三篇论文，一脉相承：

2011 年 ICRA ，Closed-Form Solution for Absolute Scale Velocity Determination Combining Inertial Measurements and a Single Feature Correspondence. 闭式求解相机尺度，只需要三帧和一个特征点。
2014 年 IJCV，closed-form solution of visual-inertial structure from motion，也是闭式求解，但是求解过程中忽略了 gyro bias 和 acc bias 的影响，因此，该方法不太实用，在实际系统中也没被采用过（17年该作者作为二作的一篇RAL说的）。
2017 年 RAL，Simultaneous State Initialization and Gyroscope Bias Calibration in Visual Inertial Aided Navigation，作者对上篇论文所述方法受 bias 的影响进行了分析，发现 acc bias 对系统影响不那么大，但是 gyro bias 影响较大，所以在 14 年论文的基础上提出了加入了标定 gyro bias 的方法。

I. INTRODUCTION

视觉-惯性SLAM使用一个摄像头和一个惯性测量单元(IMU)作为输入。两个传感器在使用前都需要初始化或校准。虽然相机校准一次就可以了，因为它不随时间演变，但IMU必须在每次使用前初始化。 惯性初始化的目的是计算初始速度、重力方向、陀螺仪和加速度计的偏差，作为视觉-惯性SLAM估计过程的初始seed，保证其收敛性。一旦这些值被确定，惯性测量可以用来改进跟踪，使其更健壮以及可以帮助找到地图的真实比例，这是不能用纯单目系统得到的。

一些技术首先初始化单目SLAM部分，建立一个符合比例尺的地图，然后，足够长的时间后才能对所有变量具有良好的可观测性，计算比例尺因子、重力方向和IMU偏差。这项技术最初是由Mur Artal和Tardos提出的，后来也进行了修改，给出了非常精确的结果。特别是，环境的真实尺度可以以大约1%的误差来估计。然而，这种方法有两个主要缺点：

依赖于视觉初始化。它需要有一个有足够的地图点的初始地图。如果视觉部分无法初始化，或者在短时间内丢失，惯性系统将无法初始化
初始化花费的时间太长。该方法大约需要运行单目SLAM15秒，以为视觉-惯性BA找到正确的惯导参数。

Martinelli提出了一种非常有趣的闭式求解方法，它只需要跟踪几个点，并联合求解了摄像机运动、环境结构和惯性参数。文中对问题可解的条件进行了理论分析，并给出了仿真结果。Kaiser等人最近的工作将该方法扩展到估计陀螺仪的偏差，并给出了来自无人机的真实数据的结果，表明初始化在3秒内是可能的，速度和重力的相对误差在15%左右。这种初始化方法有几个限制：

它假设在所有帧中跟踪所有特性，并且提供的所有跟踪都是正确的。在出现伪轨迹的情况下，它就会提供比较糟糕的结果。
与[5]相比，初始化精度较低。为了改进它，需要大量的跟踪和帧，这增加了它的计算成本，在实时情况下并不可行。
对于有噪声的传感器，接近于[1]中分析的不可解情况的轨迹使得一些变量的可观测性很弱。该方法缺乏确定初始化何时足够准确的健壮标准。

这篇论文在martineli - kaiser方案(上文已经提及)(或简单的MK-solution)的基础上，对其进行了改进，使其更加通用、高效、健壮和精确。我们的初始化算法的主要新颖之处在于：

1)通用性:我们推广了使用部分轨迹的方法，并考虑了摄像机与imu的相对姿态
2)效率:我们使用固定数量的m个特征和精心挑选的n个关键帧，采用预积分方法，减少了运行时间。
3)可观测性检验:经过MK-solution后，对m个点和n个关键帧进行视惯性BA，并采用一种新的可观测性检验方法来检验解的精度。如果测试失败，则丢弃初始化。
4)一致性检验:我们尝试通过对3D点进行三角化并检查重投影误差来初始化额外被跟踪的特征。如果找到足够的满足一致性，我们对所有点和关键帧执行第二个视觉-惯性BA，并使用它们初始化SLAM地图。否则，初始化将被丢弃。

我们在EuRoC数据集上提供了详尽的初始化测试，表明该方法能够在小于2s的时间内进行一致性初始化，且尺度误差为5%。该误差在10秒后收敛到1%，此时轨迹足够长，所有变量都具有良好的可观测性。

II. INITIAL SOLUTION

A. Feature extraction and tracking

我们使用multiscale ORB extractor来寻找M个均匀分布的点进行跟踪。由于高分辨率的点更可取，我们只提取三个最优图像尺度上的ORB点，使用的尺度因子为1.2。我们发现，FAST特征的提取与ORB描述子匹配的方法不够稳定，所以大部分特征在一些帧中丢失了，因此无法获得长的轨迹。为了解决这个问题，我们使用OpenCV中可用的L-K光流算法的金字塔来进行特征跟踪。

然而，简单的跟踪是不够的，因为可能有一些轨迹特别是在重复或弱纹理的领域,是不正确的。为了解决这个问题，通过使用RANSAC找到一个基础矩阵(
fundamental matrix)来检查这些轨迹的几何一致性。ORB关键点、L-K光流跟踪(KLT)和基础矩阵检查的组合，可以产生大量特征的长跟踪。当某些轨迹丢失或被RANSAC算法拒绝时，提取新的ORB点并开始跟踪，以保持M个轨迹的数量不变。每次检测到至少有m个跟踪点且跟踪长度至少为 l l l个像素时，我们就启动初始化方法。决定是否进行初始化的第一个测试称为轨迹长度测试。

B. Modifified Martinelli-Kaiser solution

在这一部分中，我们扩展了Martinelli-Kaiser solution方法，使其能够处理所有相机所没有看到的特征，并使用惯性预积分有效地计算。

设
m：用于初始化的被跟踪特征数，放置在 ( x 1 … x m ) (x_1…x_m) (x1…xm)[世界坐标系下]；

C = { C 1 … C n } C = \{C_1…C_n\} C={C1…Cn}：用于初始化的n个相机索引集合，也称为关键帧[沿时间均匀分布]；

C i = { C 1 i . . . C n i } C^i = \{C_{1_i}. . . C_{n_i} \} Ci={C1i...Cni} ：能够看到第 i i i个特征点的 n i n_i ni个相机索引的集合

【注意】与[1]不同的是，在这篇论文的公式中，并不是所有的相机都必须观察所有的特征，并且通过校准得到的相机到IMU的转换也被考虑在内。在不失一般性的情况下，我们使用第一个用于初始化的imu pose作为全局引用。

图1:观察同一特征的两个body(IMU)与相机之间的关系。
从图1中，我们可以写出等式:

p j ∈ R 3 p_j∈R^3 pj∈R3：第 j j j个IMU在世界坐标系中的位置。
R j ∈ S O ( 3 ) R_j∈SO(3) Rj∈SO(3)：第 j j j个IMU到世界参考系的旋转矩阵。
λ j i λ^i_j λji：第 i i i个特征点到第 j j j个相机的距离。
u j i u^i_j uji：世界坐标系中第j个相机到第i个特征点的单位向量。
[ R B C ∣ t B C ] [R_{BC} |t_{BC}] [RBC∣tBC]:从相机到IMU的变换

公式含义是观测到同一点的的IMU位置（相对世界系）+相机到IMU距离（转换到世界系，乘以外参）+相机到目标点距离（乘以u，转换到世界系）=目标点到世界系的距离(矢量的加法运算)，利用的是同一个路标点在世界系上位置一样

R j 和 p j R_j和p_j Rj和pj可以通过对惯性测量值进行积分得到。考虑初始化时的恒定偏差:

然而，上面的表达式有一个重要的缺点:每次 b g 或 b a b^g或b^a bg或ba被改变时，所有的IMU积分都需要重新计算，这非常耗时。为了解决这个问题，我们使用线性预积分，将表达式分为与偏置相关的和非相关的项，使用增量项 ∆ R 1 , j , ∆ v 1 , j , ∆ p 1 , j ∆R_{1,j} , ∆v_{1,j} , ∆p_{1,j} ∆R1,j,∆v1,j,∆p1,j，它们直接从IMU测量中计算出来，定义如下：
如果在中间步骤 b g b^g bg(用于预积分的值)的变化超过0.2 rad/sec，则使用新的偏差重新计算预积分，否则，delta项将使用其相对于bias的雅可比直接更新：

这样，每次离线性化点太远时，我们就会重新线性化。我们使用表达式(5)、(6)和(7)重写eq. (1)：

现在，我们可以加上重力大小信息。不是把它作为重力大小的约束，我们宁愿模型的重力旋转矩阵参数化的意思是只有两个角度(α,β)(旋转围绕z轴没有影响)和矢量 g I = ( 0 , 0 , − g ) g_I = (0, 0,-g) gI=(0,0,−g)，因此我们仍然是一个无约束问题:

方程(8)就变成:

其中，

现在，每次偏差更新时，我们不需要再次预积分所有的测量值，只需要用雅可比矩阵更新它们。忽略加速度计的bias,唯一的未知数是 λ j i ( ∀ i = 1... m , j ∈ C i 1 i ) , v 1 , α , β 和 b g λ^i_j(∀i= 1...m, j∈C^i \ 1_i),v_1,α,β和b^g λji(∀i=1...m,j∈Ci 1i),v1,α,β和bg。所有可能的 i 和 j i和j i和j值的叠加方程我们建立了一个超定稀疏线性系统，每行只有三个非零元素，例如：

其中 x = ( v 1 , { λ j i } ) x=(v_1,\{λ^i_j\}) x=(v1,{λji})是具有线性依赖性的未知向量。为了联合寻找线性和非线性相关参数，我们求解了下一个无约束极小化问题：

损失函数：

对于相关的变量用以下方案求解：

更新 ∆ R 1 , j , ∆ p 1 , j ∆R_{1,j},∆p_{1,j} ∆R1,j,∆p1,j:不需要在 b g b^g bg每次变化时重新集成所有IMU测量。我们只是用相对于bias的雅可比矩阵更新项。这意味着一个重要的计算节省。
计算 A ( b g ) 和 s ( b g , α , β ) A(b^g)和s(b^g,α,β) A(bg)和s(bg,α,β),建立线性系统。
解出 x = A ( b g ) s ( b g , α , β ) x = \frac{A(b^g)}{s(b^g,α,β)} x=s(bg,α,β)A(bg)，使用共轭梯度,它适用于稀疏的系统。

为了优化 c ( b g , α , β ) c(b^g,α,β) c(bg,α,β)并找到正确的陀螺偏差和重力方向，我们使用Levenberg-Marquardt算法，其中代价函数的雅可比矩阵是数值计算的。

结果不仅得到了IMU的初始化参数 ( g , b g , v 1 ) (g, b^g , v_1) (g,bg,v1)，而且得到了被跟踪点的位置 ( λ j i u j i ） (λ^i_ju^i_j） (λjiuji）。我们强调，在初始化过程中并没有使用所有M个跟踪特征，而只使用了一小部分m个特征，目的是降低计算复杂度。但是，在这一步骤之后找到的解决方案不够精确，无法启动系统，需要进一步的中间阶段。

III. IMPROVED SOLUTION

A. First BA and observability test

由初始值开始，做BA，对H矩阵SVD分解，分析最小值是否小于阈值。observability test。

在确定初始参数 ( g , b g , v 1 , { λ j i } ） (g,b^g,v_1,\{λ^i_j\}） (g,bg,v1,{λji}）后，我们建立了一个V-I BA问题，该问题的n个imu pose和m个点与前一步相同（见图2）

图2:第一个V-I BA的图形。优化中包含的body poses和points与初始化解决方案中使用的相同.

我们在估计的重力方向上设置z轴。所有的imu poses都有六个可优化的变量 ( φ , t ) ∈ s e ( 3 ) (φ, t) ∈ se(3) (φ,t)∈se(3)，除了第一个，它只有两个(俯仰和滚转)，因为平移和偏航已经固定，以消除V-I问题固有的四个规范自由度(初始位置和偏航)。body速度也包括在优化任务中，它们根据惯性测量进行演化。使用来自MK-solution的结果添加每个顶点的初始估计。

加速度偏差 b a b^a ba也被包括在这个优化中，但与 b g b^g bg类似，它被假定为对所有帧都是常数。根据MK-solution的步骤，通过先验的方法包括先前的 b g b^g bg估计，并且 b a b^a ba被强制接近于零。我们将这种优化称为first BA或BA1.

通常，这种优化提供了更好的初始化解决方案。然而，如果执行的运动使得IMU变量有一个比较低的可观察性，那么优化就会收敛到任意坏的结果(例如，发生在纯旋转运动或非加速运动的情况下)。为了检测这些失败的情况，我们提出了一个可观察性测试，从而分析与估计变量相关的不确定性。这可以通过分析估计变量的协方差矩阵并检查其奇异值是否足够小来实现。然而，这将需要对信息矩阵求逆，即从第一个BA开始的Hessian矩阵，它的维数很高(3m + 6 + 9n - 4)，在计算上过于昂贵。相反，我们执行可观察性测试，对与first BA关联的H矩阵的所有奇异值设置最小阈值。Hessian矩阵可以从与图中每个边相关联的Jacobian矩阵构建，如下所述：

{ x 1 . . . x p } \{x_1 . . . x_p\} {x1...xp} ：p个states的集合
{ e 1 . . . e q } \{e_1 . . . e_q\} {e1...eq}：出现在first BA中的q个观测值的集合
ε i \varepsilon_i εi：涉及状态 i i i的测量集合

状态i和状态j的Hessian块矩阵，采用一阶近似，可以如下建立：

其中， Ω e Ω_e Ωe表示测量值 e e e的信息矩阵， J i , e J_{i,e} Ji,e表示e相对于第 i i i个状态的雅可比矩阵。为了得到一个非零 ( i , j ) (i, j) (i,j)块矩阵，如图3所示，图中i和j节点之间必须有一条边(测量值取决于两个变量)

图3.一个具有5个关键帧(KF)和20个映射点(MP)的原始地图的海森矩阵的例子。用虚线区分不同的块。在左上部分中，有表示关键帧之间关系的对角线块(红色)，IMU度量与连续关键帧相关的块(蓝色)，关键帧和IMU偏差相关的块(粉红色)。在右下角，表示路标点之间关系的对角线块(橙色)。非对角块将路标点与观察它们的关键帧联系起来(棕色)。在这个例子中，所有的相机都能观察到所有的特征。

为帮助理解，现给出信息矩阵中变量的大体分布：

对H矩阵进行SVD分解，并通过查看最小奇异值，可以确定执行的运动是否保证了所有IMU变量的可观测性。因此，当最小的Hessian奇异值低于阈值 t o b s t_{obs} tobs时，我们将丢弃所有的初始值。如果此可观测性测试未通过，则放弃初始化尝试。成功案例和被拒绝案例的示例如图4所示。

B. Consensus test and second BA

使用M个点中其他未使用的点，评估重投影误差。consensus test。然后把内点加入，再做BA。

正如我们之前所提到的，并不是所有被检测到的M个特征点都在MK-solution和first BA中使用，只用了m个特征点。为了利用这些额外未使用的跟踪点，我们建议执行一致性测试，以便检测是否使用了虚假数据(例如错误的跟踪特征)执行了初始化。

首先，利用SVD分解进行最小二乘三角化——在能看到该点的最远的两个帧之间对每个未进行追踪的3D点位置进行三角化。只使用视差大于0.01弧度的轨迹。

然后，将每个3D点重新投影到能观察到它的所有帧中，计算重投影误差，并利用服从自由度为 2 n i − 3 2n_i -3 2ni−3的 χ 2 ( 95 % ) χ^2(95\%) χ2(95%)分布进行检验，确定内点。其中 n i n_i ni是观察该点的帧数。

执行一致性测试，计算内点的百分比：如果值大于阈值 t c o n s t_{cons} tcons，我们认为提议的方案是准确的;如果不正确，我们放弃初始化尝试。

【补充】卡方检验就是统计样本的实际观测值与理论推断值之间的偏离程度，实际观测值与理论推断值之间的偏离程度就决定卡方值的大小，卡方值越大，越不符合；卡方值越小，偏差越小，越趋于符合

如果一致测试成功，我们执行second BA(或BA2)，BA2包括初始解决方案中使用的m个点，加上所有经过三角化并检测为inliers的点，总共有 M ′ M' M′个点。这种优化的图与BA1的情况类似，但是有更多的点。

C. Map initialization

在second BA之后，关键帧的姿态足够精确，但是我们只有几个点来初始化映射。在整个ORB-SLAM视觉-惯性系统开始之前，我们对新的点进行三角化测量，目的是为了使点云更密集，并简化后验跟踪操作。因为我们已经有了关键帧姿态，所以我们在每个关键帧中提取ORB特征点，并使用ORB描述子在彼此之间执行epipolar search。所有这些新点，加上BA2中的 M ′ M' M′个点，都被提升为映射点，初始化使用的n个帧被提升为映射关键帧。考虑到对点的观察，还创建了这个新地图的covisiblity graph。

IV. EXPERIMENTS

我们的方法最重要的参数如表I所示。

我们的实现使用带有跟踪、建图和闭环三个线程的VIORB。初始化是在一个并行的线程中执行的，因此它对实时跟踪线程没有影响。对于MK-solution，使用Eigen c++ library，对于BA1和BA2的图优化，使用g2o c++ library。在EuRoC的V1数据集上进行实验。

A. Results

EuRoC dataset为三种具有不同的复杂性的室内环境提供双目图像和同步的IMU测量。我们已经在三个难度级别上测试了EuRoC V1环境的方法。我们做了两个不同的实验。

在第一个实验中，我们试图尽可能实时地进行初始化。沿着整个轨迹，每当跟踪线程有m个长度为l的轨迹，如果初始化线程空闲，则启动新的初始化操作。图5显示了经过观察性和一致性检验后对轨迹V101的初始化。红色轨迹其RMSE ATE(绝对轨迹均方根)误差大于初始轨迹长度的5%。从图中可以看出，我们的初始化算法几乎在所有的轨迹上都是成功的。没有初始化的部分是由于可观测性或一致性测试失败。

图5:经过观察性和一致性检验后，EuRoC V101轨迹的初始化。在蓝色，ground truth trajectory;绿色为初始化估计轨迹，其RMSE ATE误差小于5%;红色的是误差较大的轨迹。我们的方法能够沿着整个轨迹找到511个正确的初始化，并实时运行。

在表2中，我们展示了三个V1序列的实验的主要数值结果。RMSE ATE用初始化轨迹长度的百分比表示。在每个序列名下面，我们显示了成功的初始化总数。首先要注意的是大量的初始化尝试。例如，在持续130秒的sequence V101中，一共计算了728个初始化，其中511个已经通过了可观测性和一致性检验。由表可知，原
Martinelli-Kaiser solution对这些序列的平均尺度误差在32.9% ~ 156.7%之间。应用本文提出的两轮V-I BA，误差可减小至8.8% ~ 24.5%。更有趣的是，应用新的可观察性和一致性测试，不准确的初始化始终被拒绝，所有序列的平均尺度误差降低到5%左右，比原来的方法有了很大的改进。在两次测试之后，ATE的误差也大大降低了。

考虑到初始化时间，我们可以看到V101和V102之间有明显的差异，V101平均需要2.2秒的初始化轨迹，而V102和V103只要1秒就足够了。与第一个序列相比，后两个序列的运动速度更快，满足航迹长度测试所需的时间更短，其中四翼飞行器以低速飞行。在计算成本方面，V102和V103序列初始化所需的平均CPU时间小于85ms, V101序列初始化所需的CPU时间约为121ms，这是由于预积分周期较长造成的。在所有情况下，M-Ksolution步骤占用总初始化CPU时间的75%左右。

表二:对三个V1 EuRoC序列的初始化测试结果。

在表III中，我们展示了使用带有一阶偏差校正的预积分的方法的计算时间。与使用Martinelli和Kaiser的原始公式相比，计算时间减少了60%。

表III每次迭代中MK-Solu+BA1+BA2重复IMU预积分运行时间与使用一阶偏差校正的预积分的比较

在第二个实验中，我们运行VIORB，并在初始化后获取RMSE ATE和尺度误差，并在距第一个关键帧时间戳5秒和10秒时执行完整的VI BA。从表4可以看出，10秒后，三个序列收敛到小于1%的尺度误差，验证了我们的初始化方法足够精确，可以很好的实现VI-SLAM。与VIORB中提出的初始化方法相比，我们的方法需要1或2秒的轨迹，而不是15秒，使用更少的CPU时间，并且能够成功初始化序列V103，这是之前方法失败的地方。

结论

基于Martinelli和Kaiser等人的工作，我们提出了一种快速联合单目惯性初始化方法。我们使用IMU预积分方法使其更通用，允许不完整的特征轨迹，并提高了计算效率。我们的结果表明，在大多数实际情况下，原有的martini - kaiser技术并不能提供足够好的初始化，因此我们提出了两个V-I BA步骤来改进解决方案，并提出了两个新的测试来检测糟糕的初始化。这些技术已经证明是有效的，可以将尺度误差降低到5%，并排除了糟糕的初始化。这些步骤之后找到的解决方案足以运行VIORB，并映射到非常精确的地图上。

综上所述，我们开发了一种单目VI SLAM联合初始化的快速方法，使用1 ~ 2秒的轨迹，比原始技术更精确、更健壮，最大计算量为215ms。

在以后的工作中，考虑到从图像中可以直接观察到尺度，我们希望研究初始化方法对双目情况的适应性。我们也有兴趣利用陀螺仪读数甚至在初始化之前进行跟踪。最后，我们想用我们自己获得的序列在更困难的场景中测试初始化性能。