Inertial-Only Optimization for Visual-Inertial Initialization

摘要-我们首次将视觉惯性初始化表述为最大后验（MAP）估计意义上的最优估计问题。这允许我们适当地考虑IMU测量的不确定性，这在以前的方法中被忽略，这些方法要么解决代数方程组，要么使用最小二乘法最小化ad-hoc成本函数。我们对EuRoC数据集的详尽初始化测试表明，我们的建议大大优于文献中的最佳方法，能够在几乎轨迹的任何点上在不到4秒的时间内进行初始化，平均尺度误差为5.3%。该初始化已集成到ORB-SLAM视觉惯性系统中，提高了其鲁棒性和效率，同时保持了其优异的精度。

1、介绍

、、、

本文的主要贡献：

1)在MAP估计的意义上，将视觉惯性初始化表述为仅惯性的最优估计问题，适当考虑IMU噪声的概率模型。

2)我们一步解决所有惯性参数，避免了解耦估计产生的不一致性。这使得所有的估计都一致。

3)我们没有对初始速度或姿态进行任何假设，这使得我们的方法适用于任何初始化情况。

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

在下一节中，我们将介绍我们建议背后的理论和深入细节。稍后，我们将其与联合和不相交初始化方法的最佳示例进行评估和比较，证明其性能优于它们。

2、最大后验估计初始化

基于特征的视觉惯性SLAM的黄金标准方法是视觉惯性束调整（VI-BA），它适当地考虑了所有传感器中的噪声特性，并获得了所有变量的最大后验联合估计（参见[5]中使用流形上的IMU预积分的现代公式[8]）。VI-BA的主要限制是，由于其强非线性特性，它需要一个好的种子来快速收敛，避免陷入局部极小值。基于最小二乘估计的联合[3]和不相交[5]初始化方法表明，VI-BA大大改进了它们的初始解。我们的主要目标是更进一步，并在初始化中使用MAP估计，正确使用传感器噪声模型。我们新颖的初始化方法基于以下思想：

1）尽管BA具有非线性性质，但单眼SLAM（或视觉里程计）已经足够成熟和稳健，能够获得结构和运动的非常精确的初始解，唯一需要注意的是它们的估计符合规模。

2）视觉SLAM轨迹的不确定性比IMU不确定性小得多，在获得IMU变量的第一解时可以忽略。因此，我们执行仅惯性MAP估计，将最新的视觉SLAM轨迹作为常数。

3）受[9]的工作启发，我们采用了一种参数化，该参数化明确表示并优化了单目SLAM解决方案的比例因子。

4）与[5][7]不同，我们在一个步骤中联合优化了所有IMU变量，同时考虑了位置的预积分项与线速度和角速度之间的交叉协方差[8]。

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

1）只有视觉的最大后验估计：

使用BA初始化并运行单眼ORB-SLAM[10]一段短时间（通常为2秒），以获得符合比例的仅视觉MAP估计。同时，计算关键帧及其协方差之间的IMU预积分[8]。

2）只有惯性的最大后验估计：

仅惯性优化，以对齐IMU轨迹和ORB-SLAM轨迹，找到比例、关键帧速度、重力方向和IMU偏差。

3）视觉-惯性的最大后验估计：

使用上一步的解决方案作为完整VI-BA的种子，以获得联合最优解决方案。

初始化后，我们启动ORB-SLAM视觉惯性[5]，它执行本地VI-BA。我们已经观察到，在执行完整的VI-BA后5-10秒，或者以低得多的计算成本重复仅惯性优化后，可以进一步提高尺度估计精度。

接下来将进一步详细介绍三个初始化步骤。

A、只有视觉的MAP估计

我们初始化纯单眼SLAM，使用与ORB-SLAM相同的过程来找到初始运动。使用ORB描述符在两个初始帧之间执行FAST点的匹配。找到了基本矩阵和单应性模型并进行了评分。得分较高的一个用于找到初始运动并三角化特征。一旦结构和运动被初始化，我们就进行1或2秒的单目SLAM。与ORB-SLAM的唯一区别是，我们以更高的频率（4 Hz到10 Hz）执行关键帧插入。这样，关键帧之间的IMU预积分具有很低的不确定性，因为集成时间非常短。在这段时间之后，我们有一个由十个关键帧和数百个点组成的最新地图，ORB-SLAM建图线程使用BA对其进行了优化。使用视觉惯性校准将缩放关键帧姿势转换为body（或IMU）参考。

B、只有惯性的MAP估计

该步骤的目标是使用视觉获得的最新轨迹，在MAP估计的意义上获得惯性参数的最佳估计。由于我们对惯性参数没有很好的估计，在这一点上使用完整的VI-BA将代价太大，并且容易陷入局部极小值，如实验部分所示。中间解决方案是边缘化这些点以获得轨迹及其（完全密集）协方差矩阵的先验，并在优化IMU参数时使用它。我们选择一个更有效的解决方案，考虑到轨迹是固定的，并执行仅惯性优化。惯性参数如下：

我们更喜欢使用最大比例的速度v̄i，而不是真正的速度v i=sv̄i，因为它简化了初始化过程。假设所有涉及的关键帧的偏移都是恒定的，因为初始化周期仅为1-2秒，随机行走几乎不会产生任何影响。值得注意的是，该公式从一开始就考虑了重力大小，而[7]和[5]则需要单独的步骤来确定其值。

在我们的案例中，使用的唯一测量值来自IMU，并在[8]中定义的IMU预积分项中进行了总结。我们用I I，j表示第I个和第j个关键帧之间惯性测量的预积分，用I 0：k表示初始化窗口中连续关键帧之间的IMU预积分集。

通过定义状态和测量，我们可以制定MAP估计问题，其中后验分布为：

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

由于我们假设在初始化窗口期间偏差可以被认为是恒定的，所以IMU残差不包括偏差的随机游走。我们假设残差遵循高斯分布，其协方差可以按照[8]中的建议计算。当我们在流型中进行优化时，我们需要定义回缩[8]以在优化期间更新重力方向估计：

图1所示的最终优化问题是使用g2o C++库[12]、解析导数和Levenberg-Marquardt算法实现和解决的。正如文献中众所周知的那样，重力和加速度计偏置往往是耦合的，在大多数情况下很难区分。为了避免这个问题，一些技术在初始化过程中忽略了加速度计偏置，假设值为零[7]，而另一些技术则等待很长时间以确保其可观察[5]。在这里，我们采用了一种稳健实用的方法：我们将ba作为一个要优化的参数，但为其添加了一个先验残差：

如果执行的运动不包含足够的信息来估计偏差，则先验将保持其估计接近于零。如果运动使ba成为可观察的，则其估计值将向其真实值收敛。不需要bg的先验，因为从关键帧方向和陀螺仪读数可以很好地观察到。

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

比例因子需要初始化得足够接近其真实值，以保证收敛，但我们没有任何初始猜测。利用我们非常有效的仅惯性优化（5ms），我们使用三个初始比例值启动优化，这三个初始尺度值对应于1、4和16米的中值场景深度，保持方程5中定义的提供最低残差的解。我们的结果表明，使用这个范围的尺度值，我们的方法能够在各种场景中收敛。

在优化结束时，使用找到的比例值缩放帧姿态和速度以及3D地图点，并旋转以使z轴与估计的重力方向对齐。IMU预积分使用新的偏差估计重复，旨在减少未来的线性化误差。

C、视觉惯性MAP估计

仅惯性优化提供了足够精确的估计，以用作第一联合视觉惯性束调整的种子，确保其收敛。在该优化中，如图1所示，不显示像gdir和s这样的纯惯性参数，但它们隐含地包含在关键帧姿势中。与[3]相比，该步骤取代了BA1&2步骤。事实上，优化是完全相同的，它只在初始种子上有所不同，该种子以前是通过求解线性系统来计算的，现在是通过MAP估计器来计算的。在VINS Mono初始化中，在启动VI里程计之前，也进行了类似的优化。

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

我们注意到，所有初始化步骤都在并行线程中执行，对实时跟踪线程没有任何影响。优化完成后，系统已经初始化，我们从视觉SLAM切换到视觉惯性SLAM。

III、实验

、、、

IV、结论和将来工作

所提出的初始化方法已显示出比文献中表现最好的方法更准确，计算时间非常短。这证实了最优估计理论能够正确利用传感器噪声的概率模型，获得比求解线性方程组或使用非加权最小二乘更准确的结果。全视觉惯性BA是一个非常非线性的问题，受到局部极小值的困扰，这阻碍了收敛。我们将其分解为一个完全可观察的大规模视觉问题，然后是一个仅惯性的优化阶段，该阶段可以非常有效地解决，从而产生VI-BA的初始解决方案，缓解了局部最小问题。

作为未来的工作，我们强调，这种仅惯性的优化不仅可以用于初始化，还可以用于在SLAM初始化并运行后细化比例和其他惯性参数。这将具有比执行全视觉惯性束调整（其中涉及所有视觉和惯性参数）低得多的计算成本。此外，这种新的初始化可以很容易地适应立体惯性情况。这将足以从仅惯性优化中去除尺度。