作者开篇点名：我要做“视觉惯性初始化公式化为最佳估计问题”

在最大后验（MAP）估计的意义上，我们首次将视觉惯性初始化公式化为最佳估计问题。

作者提出，以前的研究不行：

我们能够正确考虑 IMU 测量不确定性，这在以前的方法中被忽略

以前，1、要么求解代数方程组；2、要么使用最小二乘法最小化损失函数。

4秒初始化，误差5.3%

文献中的方法，几乎可以在轨迹的任何点上在不到 4 秒的时间内初始化，平均尺度误差为 5.3%。

此初始化已集成到 ORB-SLAM Visual-Inertial 中，以提高其鲁棒性和效率，同时保持其出色的准确性

系统介绍

单个单目相机+低成本惯性测量单元 (IMU) 传感器

IMU 传感器测量加速度和角速度，可适应快速运动的情况，可以测量环境的真实尺度。

IMU 准确初始化，初始参数影响算法收敛性

要开始使用它们，需要在初始化过程中估计一些参数。

scale,比例
gravity direction, 重力方向
initial velocity, 初始速度
accelerometer and gyroscope biases.加速度计和陀螺仪偏差

错误的初始化会导致收敛性差，以及对所有其他变量的估计不准确。

IMU 快速初始化

此外，快速初始化与同样重要，因为只要 IMU 未初始化，视觉惯性 SLAM 就无法执行

联合和不相交（或松散耦合）估计方法

以前关于视觉惯性初始化的工作可以分为联合和不相交（或松散耦合）估计方法。

联合视觉惯性初始化

联合视觉惯性初始化由 Martinelli [1] 开创，他提出了一种封闭形式的解决方案来联合检索尺度、重力、加速度计偏差和初始速度，以及视觉特征深度。

该方法建立在可以从 IMU 读数粗略估计相机姿势的假设上。该方法跟踪所有图像中的几个点，并建立一个方程系统，说明从任何相机对看到的 3D 点坐标应该是相同的，这是通过线性最小二乘法求解的。

Kaiser 等人 [2] 扩展了这项工作，构建了一个类似的线性代数系统，该系统使用非线性最小二乘法求解，还可以找到陀螺仪偏差并考虑重力大小。模拟显示了在 2 秒内找到准确初始解的能力

至关重要的是，原始和修改后的方法忽略了 IMU 噪声属性，并最小化空间中点的 3D 误差，而不是它们的重投影误差，这是基于特征的计算机视觉的黄金标准。

我们之前的工作 [3] 表明，这会导致大量不可预测的错误，可以通过添加两轮视觉惯性束调整 (VI-BA) 以及用于检测和丢弃不良初始化的两个测试来纠正。

这使得该方法可用，在 2 秒内获得公共 EuRoC 数据集 [4] 联合视觉惯性初始化，比例误差约为 5%。

但是，该方法仅适用于 20% 的轨迹点。对于需要立即启动系统的 AR/VR 或无人机应用程序来说，如此低的初始化召回率可能是一个问题。

不相交（或松散耦合）估计视觉惯性初始化

不相交的视觉惯性初始化是基于这样一个坚实的假设，即可以从纯单目视觉非常准确地估计出按比例缩放的相机轨迹，然后使用该轨迹来估计惯性参数。

由于现代视觉里程计和视觉 SLAM 系统执行局部BA调整并提供比 IMU 集成精度高得多的轨迹，因此在估计惯性参数时可以安全地忽略这种轨迹不确定性。

这个想法是由 Mur-Artal 在 ORBSLAM-VI[5] 中率先提出的，后来被 Qin 等人在 VINS-Mono[6][7]中采用。
在这两种情况下，通过使用最小二乘法求解一组线性方程，可以在不同的步骤中找到惯性参数。

在 [5] 中，通过消除每个帧的速度来构建线性系统。然而，在这些代数运算之后，要最小化的误差是没有意义的，并且与传感器噪声特性无关。为了获得准确的估计，包括加速度计偏差，该方法需要 15 秒的初始化时间。

在 [7] 中，加速度计偏置假设为零，根据运动情况，只需要 1-2 秒即可初始化。

在这两种方法中，IMU 测量值在同一个线性系统中进行处理和混合，其中所有方程的残差都被视为具有相同的权重，忽略传感器的不确定性。

此外，不同的惯性参数在不同的步骤中单独求解，而不是一次全部求解，忽略了它们之间的相关性。所有这些都会导致估计在最大后验 (MAP) 估计的意义上不是最佳的

我们提出了一种新的不相交视觉惯性初始化方法

我们提出了一种新的不相交视觉惯性初始化方法，在 MAP 估计的意义上将其表述为最优估计问题。为此，我们以 Forster 等人 [8] 的出色工作为基础，该工作允许对 IMU 读数进行预积分，并考虑到传感器噪声的概率特征，正确计算预积分项的协方差。

假设单目 SLAM 轨迹的误差与 IMU 误差相比可以忽略不计，我们推导出一个非常有效的惯性参数的 MAP 估计器，并用它来初始化视觉惯性 SLAM 系统。

我们工作的主要贡献

1、对于MAP 估计，把视觉惯性初始化公式，作为仅惯性最优估计问题，适当考虑了 IMU 噪声的概率模型

2、我们一次性解决所有惯性参数，避免因解耦估计而导致的不一致。这使得所有估计联合一致。

3、我们不对初始速度或姿态做任何假设，这使得我们的方法适用于任何初始化情况。

4、我们不假设 IMU 偏差为零，而是将关于它们的已知信息编码为我们的 MAP 估计所利用的概率先验。

原理讲解：MAXIMUM-A-POSTERIORI INITIALIZATION

基于特征的视觉惯性 SLAM 的黄金标准方法是视觉惯性束调整 (VI-BA)，它适当地考虑了所有传感器中的噪声特性，并获得所有传感器的最大后验联合估计变量（参见 [5]，了解使用 [8] 中流形上的 IMU 预积分的现代公式）。

VI-BA 的主要限制是它需要一个好的种子来快速收敛并避免陷入局部最小值，因为它具有很强的非线性特性。基于最小二乘估计的联合 [3] 和不相交 [5] 初始化方法表明 VI-BA 极大地改进了它们的初始解。

我们的主要目标是更进一步，并在初始化中使用 MAP 估计，正确使用传感器噪声模型。我们新颖的初始化方法基于以下思想：

• 尽管 BA 具有非线性特性，但单目 SLAM（或视觉里程计）已经足够成熟和稳健，可以为结构和运动获得非常准确的初始解决方案，唯一需要注意的是，它们的估计是按比例进行的。

• 视觉SLAM 轨迹的不确定性远小于IMU 的不确定性，在获得IMU 变量的第一个解时可以忽略。因此，我们执行仅惯性的 MAP 估计，将按比例放大的视觉 SLAM 轨迹作为常数。

• 受[9] 工作的启发，我们采用了一种参数化，明确表示并优化了单目SLAM 解决方案的比例因子。与 [5] [7] 不同的是，我们一步联合优化所有 IMU 变量，同时考虑到位置的预积分项与线速度和角速度 [8] 之间的互协方差。

我们的初始化方法可以分为三个步骤：

仅视觉 MAP 估计：使用 BA 在短时间内（通常为 2 秒）初始化并运行单目 ORB-SLAM [10]，以获得按比例缩放的仅视觉 MAP 估计。同时，计算关键帧及其协方差之间的 IMU 预积分 [8]。
仅惯性 MAP 估计：仅惯性优化以对齐 IMU 轨迹和 ORB-SLAM 轨迹，找到比例、关键帧的速度、重力方向和 IMU 偏差。

3）视觉惯性MAP估计：使用上一步的解作为完整VI-BA的种子，以获得联合最优解。

初始化后，我们启动 ORB-SLAM Visual-Inertial [5]，执行本地 VI-BA。我们观察到，在执行完整的 VI-BA 5-10 秒后，或者以低得多的计算成本重复仅惯性优化后，可以进一步提高尺度估计精度。下面进一步详细介绍这三个初始化步骤。

A. Vision-only MAP Estimation

我们初始化纯单目 SLAM，使用与 ORB-SLAM 相同的程序来找到初始运动。在两个初始帧之间执行使用 ORB 描述符的 FAST 点匹配。找到并评分基本矩阵和单应性模型。得分较高的那个用于找到初始运动并对特征进行三角测量。

一旦结构和运动被初始化，我们就进行 1 或 2 秒的纯单目 SLAM。与 ORB-SLAM 的唯一区别是我们以更高的频率（4 Hz 到 10 Hz）强制插入关键帧。这样，关键帧之间的 IMU 预积分具有较低的不确定性，因为积分时间非常短。在此之后，我们有一个由十个关键帧和数百个点组成的按比例放大的地图，该地图已通过 ORB-SLAM 映射线程使用 BA 进行了优化。

关键帧姿势转换为body（或 IMU），

body 表示为T0:k=[R,p]0:kT_{0:k} = [R, p]_{0:k}T0:k=[R,p]0:k，其中 Ri∈SO(3)Ri ∈ SO(3)Ri∈SO(3)是从第 i 个body到世界参考的旋转矩阵，pi∈R3pi ∈ R^3pi∈R3 是第 i 个body的位置（尺度缩放）。

B. Inertial-only MAP Estimation

仅惯性 MAP 估计

这一步的目标是使用视觉获得的按比例缩放的轨迹，在 MAP 估计的意义上获得惯性参数的最佳估计。由于我们对惯性参数没有很好的猜测，此时使用完整的 VI-BA 会太昂贵并且容易陷入局部最小值，如实验部分所示。

一个中间解决方案是边缘化这些点以获得轨迹及其（完全密集的）协方差矩阵的先验，并在优化 IMU 参数时使用它。

我们选择更有效的解决方案，将轨迹视为固定，并执行仅惯性优化。

惯性参数是：

Xk=s,Rwg,b,v^0:kX_k = {s,R_{wg},b, \hat {v}_{0:k}}Xk=s,Rwg,b,v^0:k

其中sss 是尺度缩放因子，来源于仅视觉MAP估计。
Rwg∈SO(3)R_{wg}∈ SO(3)Rwg∈SO(3) 是重力的方向矩阵，由两个角度参数化，使得世界参考系中的重力表示为 g=RwggIg = R_{wg}g_Ig=RwggI，其中 gI=(0,0,G)Tg_I = (0,0,G)^TgI=(0,0,G)T 为 G 重力大小。（停留5秒思考下，这就是一个方向矩阵，依靠重力加速度这个恒定的量，左乘方向矩阵，得到实际的body的加速度）

b=(ba,bg)∈R6b = (ba,bg) ∈ R^6b=(ba,bg)∈R6 是加速度计和陀螺仪偏差
v^0:k∈R3\hat{v}_{0:k}∈ R3v^0:k∈R3 从第一个关键帧到最后一个关键帧的按比例缩放的body速度。

我们更喜欢使用按比例缩放的速度 ̄vi，而不是真正的速度 vi = s ̄vi，因为它简化了初始化过程。由于初始化周期仅为 1-2 秒，因此所有涉及的关键帧的偏差被假定为常数，并且随机游走几乎没有影响。值得注意的是，该公式从一开始就考虑了重力大小，而不是 [7] 和 [5] 需要单独的步骤来确定其值

在我们的例子中，使用的唯一测量来自 IMU，并在 [8] 中定义的 IMU 预积分术语中进行了总结。我们用 Ii,j 表示第 i 个和第 j 个关键帧之间惯性测量的预积分，用 I0:k 表示初始化窗口中连续关键帧之间的 IMU 预积分集。

定义状态和测量后，我们可以制定一个 MAP 估计问题，其中后验分布是 :

p(Xk∣I0:k)∝p(I0:k∣Xk)p(Xk)p(X_k|I_{0:k}) ∝p(I_{0:k}|X_k)p(X_k)p(Xk∣I0:k)∝p(I0:k∣Xk)p(Xk)

其中 p(I0:k|Xk) 是给定 IMU 状态的 IMU 测量值的似然分布，而 p(Xk) 是 IMU 状态的先验。考虑到测量的独立性，似然可以分解为：

p(I0:k∣Xk)=∏i=1kp(Ii−1,i∣s,gdir,b,vi−1,vi)p(I_{0:k}|X_k) =∏^k_{i=1}p(I_{i−1,i}|s,g_{dir},b,v_{i−1},v_i)p(I0:k∣Xk)=∏i=1kp(Ii−1,i∣s,gdir,b,vi−1,vi)

为了获得 MAP 估计量，我们需要找到使后验分布最大化的参数，即等效于最小化其负对数，因此：

Xk∗=argmaxXkp(Xk∣I0:k)=argminXk(−log(p(Xk))−∑i=1klog(p(Ii−1,i∣s,gdir,b,vi−1,vi)))X^∗_k = arg max_{Xk} p(X_k|I_{0:k}) = arg min_{Xk}(−log(p(X_k))−∑^k_{i=1}log(p(I_{i−1,i}|s,g_{dir},b,v_{i−1},v_i)))Xk∗=argmaxXkp(Xk∣I0:k)=argminXk(−log(p(Xk))−∑i=1klog(p(Ii−1,i∣s,gdir,b,vi−1,vi)))

假设 IMU 预积分和先验分布的高斯误差，MAP 问题等价于

Xk∗=argminXk(‖rp‖Σp2+∑i=1k‖rIi−1,i‖ΣIi−1,i2)X^∗_k = arg min_{Xk}(‖r_p‖^2_{Σp} +∑^k_{i=1}‖r_{I_{i−1,i}} ‖^2_{ΣIi−1,i})Xk∗=argminXk(‖rp‖Σp2+∑i=1k‖rIi−1,i‖ΣIi−1,i2)

其中 rpr_prp 和 rIi−1,ir_{Ii−1,i}rIi−1,i 是连续关键帧之间的先验和 IMU 测量的残差，而 Σp 和 ΣIi−1,i 是它们的协方差。

在此优化中，不会出现视觉重投影误差，只会出现惯性残差。由于 IMU 测量不受数据关联错误的影响，因此使用鲁棒成本函数（如 Huber 范数）没有意义，因为它会减慢优化速度

根据[11]和[8]，我们将惯性残差定义为 :

rIi,j=[r∆Rij,r∆vij,r∆pij]r_{Ii,j} = [r_{∆Rij} ,r_{∆vij} ,r_{∆pij} ]rIi,j=[r∆Rij,r∆vij,r∆pij]
∆Rij=Log(∆Rij(bg)TRiTRj)∆R_{ij} = Log (∆R_{ij} (b^g)^TR^T_i R_j)∆Rij=Log(∆Rij(bg)TRiTRj)
r∆vij=RiT(svj−svi−RwggI∆tij)−∆vij(bg,ba)r_{∆vij}= R^T_i(sv_j −sv_i - R_{wg} g_I{∆t_{ij}} ) −∆v_{ij} (b^g,b^a)r∆vij=RiT(svj−svi−RwggI∆tij)−∆vij(bg,ba)
r∆pij=RiT(spj−spi−svi∆tij−1/2∗RwggI∆tij2)−∆pij(bg,ba)r_{∆pij} = R^T_i(sp_j −sp_i −sv_i∆t_{ij} − 1/2*R_{wg}g_I∆t^2_{ij})−∆p_{ij} (b^g,b^a)r∆pij=RiT(spj−spi−svi∆tij−1/2∗RwggI∆tij2)−∆pij(bg,ba)

其中 ∆Rij (bg)、∆vij (bg,ba) 和 ∆pij (bg,ba) 是从第 i 到第 j 个关键帧的预积分的 IMU 测量值，仅取决于偏差。这些项可以按照 [8] 中的解释线性更新，避免在每次迭代时重新整合。

∆tij 是两个关键帧之间的时间。
Log 代表从李群 SO(3) 到其代数 so(3) 的对数映射，同构于 R3R^3R3。
由于我们假设在初始化窗口期间偏差可以被认为是恒定的，因此 IMU 残差不包括偏差的随机游走。我们假设残差遵循高斯分布，它们的协方差可以按照 [8] 中的建议进行计算。

当我们在流形中进行优化时，我们需要定义一个 retraction [8] 以在优化过程中更新重力方向估计：

Rwgnew=RwgoldExp(δαg,δβg,0)R^{new}_{wg} = R^{old}_{wg} Exp(δα_g,δβ_g,0)Rwgnew=RwgoldExp(δαg,δβg,0)

是 Exp(.) 从 so(3) 到 SO(3) 的指数映射。为了保证在优化过程中比例因子保持为正，我们将其更新定义为：

snew=soldexp(δs)s^{new} = s^{old} exp (δs)snew=soldexp(δs)

偏差和速度是相加更新的。

如果我们定义 δgdir=(δαg,δβg)δg_{dir} = (δα_g,δβ_g)δgdir=(δαg,δβg)，则优化过程中使用的惯性参数更新为 (δs,δgdir,δbg,δba,δˉvi)(δs,δg_{dir},δb^g,δb^a,{δ̄vi})(δs,δgdir,δbg,δba,δˉvi)。

IMU 残差的导数 w.r.t. 这些参数可以在附录中找到。

图 1 中表示的最终优化问题是使用 g2o C++ 库 [12]、解析导数和 Levenberg-Marquardt 算法实现和解决的。

图 1：仅惯性优化（左）和第一个视觉惯性束调整（右）的底层图形表示。黄色框代表 IMU 残差，红色框代表重投影误差，而紫色框代表加速度计偏差的先验信息。虚线指出固定变量（仅惯性优化的关键帧姿势）

正如文献中众所周知的那样，重力和加速度计偏差往往是耦合的，在大多数情况下难以区分。为了避免这个问题，一些技术在初始化过程中忽略了加速度计偏差，假设为零 [7]，而其他技术则等待很长时间以保证它是可观察的 [5]。

在这里，我们采用了一种合理且务实的方法：我们将 ba 作为要优化的参数，但为其添加了先验残差：rp = ‖ba‖2Σp 。如果执行的运动不包含足够的信息来估计偏差，则先验将保持其估计接近于零。如果运动使 ba 可观察，则其估计将向其真实值收敛。不需要 bg 的先验，因为它总是可以从关键帧方向和陀螺仪读数中很好地观察到。

由于我们必须解决非线性优化问题，因此我们需要对惯性参数进行初始猜测。因此，我们将偏差初始化为零，而重力方向沿加速度计测量值的平均值进行初始化，因为加速度通常远小于重力。

比例因子需要初始化得足够接近其真实值以保证收敛，但我们没有任何初始猜测。利用我们非常有效的仅惯性优化 (5ms)，我们使用三个初始比例值启动优化，分别对应于 1、4 和 16 米的中位场景深度，保持提供最低残差的解决方案，如公式中所定义5.

我们的结果表明，使用这个范围的尺度值，我们的方法能够在各种场景中收敛。在优化结束时，帧位姿和速度以及 3D 地图点根据找到的比例值进行缩放，并旋转以将 z 轴与估计的重力方向对齐。使用新的偏置估计重复 IMU 预积分，旨在减少未来的线性化误差

C. Visual-Inertial MAP Estimation

Inertial-only optimization 仅惯性优化提供了足够准确的估计，可用作第一个联合视觉惯性束调整的种子，确保其收敛。

在这个优化中，也如图 1 所示，像 gdirg_{dir}gdir 和 sss 这样的纯惯性参数没有出现，但它们被隐含地包含在关键帧姿势中。

与[3]相比，该步骤取代了BA1&2步骤。事实上，优化是完全一样的，只是初始种子不同，以前是通过求解线性系统计算的，现在是通过 MAP 估计器计算的。在启动 VI 里程计之前，在 VINS-Mono 初始化中也进行了类似的优化。

在文献中，有几个提议的测试来确定初始化是否成功。在 [3] 中，检查了优化问题的可观察性和不同测量集之间的一致性。相比之下，VINS-Mono 检查估计重力大小的误差低于 10%，并且 IMU 读数有足够的方差。在这里，我们建议丢弃平均加速度低于某个阈值（重力的 0.5%）的初始化。

这只会丢弃最糟糕的尝试->速度几乎恒定->这是不可观察的 [1]。我们注意到所有初始化步骤都是在一个并行线程中执行的，对实时跟踪线程没有任何影响。优化完成后，系统已经初始化，我们从视觉SLAM切换到视觉惯性SLAM。

参考

https://arxiv.org/pdf/2003.05766.pdf

（ORBSLAM3关联文章）论文翻译Inertial-Only Optimization for Visual-Inertial Initialization视觉惯性初始化的仅惯性优化相关推荐

论文翻译解读：Distributed Representations of Words and Phrases and their Compositionality【Word2Vec优化】
文章目录 Distributed Representations of Words and Phrases and their Compositionality 简要信息重点内容概括摘要 1 介绍 ...
【论文阅读】PL-VIO: Tightly-Coupled Monocular Visual–Inertial Odometry Using Point and Line Features
这篇文章是中科大的一篇关于点线SLAM的文章,主要是做了一个点线结合的SLAM框架,结合视觉和VIO,但是貌似不包含回环检测的部分. 一.公式约定论文用右下角标i表示时间,用c表示相机坐标系,用b表 ...
论文阅读《Pedestrian Dead Reckoning-Assisted Visual Inertial Odometry Integrity Monitoring》
目录摘要 1 介绍 2 背景 3 视觉误差分析和自主完好性监测 3.1 视觉误差分析 3.1.1 不足的特征 3.1.2 光照导致特征跟踪失败 3.1.3 特征分布不均匀 3.1.4 运动特征 3. ...
视觉惯性里程计综述 VIO Visual Inertial Odometry msckf ROVIO ssf msf okvis ORB-VINS VINS-Mono gtsam
视觉惯性里程计 VIO - Visual Inertial Odometry 视觉−惯性导航融合SLAM方案博文末尾支持二维码赞赏哦 _ 视觉惯性SLAM专栏 VINS技术路线与代码详解 VINS理 ...
【ORB-SLAM3论文翻译】ORB-SLAM3：一个精确的视觉、视觉惯性和多地图SLAM的开源库（注：带着原文看，很多公式和变量不好输入）
文章目录前言(非论文部分) 摘要 1. 简介 2. 相关工作 3. 系统概述 4. 相机模型 5. 视觉惯性SLAM(重点) 6. 地图合并与闭环 7. 实验结果 8. 结论 REFERENCES( ...
ECCV2020 AABO: Adaptive Anchor Box Optimization for Object Detection via Bayesian Sub-sampling论文翻译
ECCV2020 AABO论文翻译摘要 1.介绍 2.相关工作 3.提出的方法 3.1 初步分析 3.2 anchors的搜索空间优化 3.3 通过子抽样的贝叶斯锚优化 4.实验 4.1数据集,指标 ...
ORBSLAM3论文翻译
ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM 摘要本文介绍了OR ...
ORB-SLAM3 双目 Stereo + 论文
文章目录 Stereo 双目流程总结 System TrackStereo Tracking ParseCamParamFile GrabImageStereo StereoInitializati ...
DM-VIO论文翻译
简介 DM-VIO: Delayed Marginalization Visual-Inertial Odometry DM-VIO: 延迟边缘化惯性视觉里程计花了两天时间捏着鼻子把这篇论文翻译完了 ...
【转】分布式一致性算法：Raft 算法（Raft 论文翻译）
编者按:这篇文章来自简书的一个位博主Jeffbond,读了好几遍,翻译的质量比较高,原文链接:分布式一致性算法:Raft 算法(Raft 论文翻译),版权一切归原译者. 同时,第6部分的集群成员变更读 ...

（ORBSLAM3关联文章）论文翻译Inertial-Only Optimization for Visual-Inertial Initialization视觉惯性初始化的仅惯性优化