视觉惯性初始化的惯性优化方法

摘要：我们首次将视觉惯性初始化表述为最优估计问题，即最大后验（MAP）估计的意义上。这使我们能够正确考虑IMU测量不确定性，而以前的方法忽略了这一点，这些方法要么解决代数方程组，要么使用最小二乘法最小化特定的代价函数。我们在EuRoC数据集上进行了详尽的初始化测试，结果显示我们的方法在很大程度上优于文献中最佳的方法，能够在几乎任何轨迹点上在不到4秒的时间内完成初始化，并且平均尺度误差为5.3%。这种初始化已经被集成到ORB-SLAM视觉惯性增强中，提高了其鲁棒性和效率，同时保持了其出色的准确性。

 1、介绍

同时定位与地图构建（SLAM）技术允许机器人和增强现实/虚拟现实系统意识到它们所处的环境，并在重建的场景中定位自己。使用单个单目相机和低成本惯性测量单元（IMU）传感器的视觉惯性SLAM，为大多数应用提供了小型、紧凑和低功耗的解决方案。IMU传感器测量加速度和角速度，能够在快速运动或复杂环境下提供稳健性，并允许获取环境的真实尺度，这在纯单目系统中是未知的。

然而，要开始使用它们，需要在初始化过程中估计一些参数。这些是指比例尺、重力方向、初始速度以及加速度计和陀螺仪的偏差。错误的初始化会导致收敛速度变慢，同时也会导致其他变量的估计不准确。此外，快速初始化与准确初始化同样重要，因为只要惯性测量单元（IMU）未初始化，就无法执行视觉惯性SLAM。

以视觉惯性初始化为主题的先前研究可以分为联合估计和分离（或松耦合）估计方法。联合视觉惯性初始化最初是由Martinelli [1]提出的，他提出了一个闭式解来联合恢复尺度、重力、加速度计偏差和初始速度，以及视觉特征深度。这种方法建立在这样一个假设之上，即可以从IMU读数中粗略地估计相机的姿态。该方法在所有图像中跟踪多个点，并建立一组方程，说明从任何相机对所看到的三维点坐标应该相同，这些方程通过线性最小二乘法求解。这项工作由Kaiser等人[2]扩展，构建了一个类似的线性代数系统，使用非线性最小二乘法进行求解，以找到陀螺仪偏差并考虑重力大小。在模拟中展示了在2秒内找到准确的初始解的能力。

关键是，原始方法和修改后的方法忽略了IMU噪声特性，并最小化了空间中点的3D误差，而不是它们的重投影误差，而重投影误差是基于特征的计算机视觉中的黄金标准。我们之前的工作[3]表明，这会导致大量不可预测的误差，可以通过添加两轮视觉惯性捆绑调整（VI-BA）以及两个测试来检测和丢弃不良初始化来进行纠正。这使得该方法可用，在公共EuRoC数据集[4]上获得联合视觉惯性初始化，仅需2秒钟，尺度误差约为5%。然而，这种方法只在轨迹点的20%中有效。这种低的初始化召回率可能会对AR/VR或无人机应用造成问题，因为系统需要立即启动。

 不相交的视觉惯性初始化是基于一个坚实的假设，即可以从纯单目视觉非常精确地估计出尺度不变的相机轨迹，然后利用这个轨迹来估计惯性参数。

随着现代视觉里程计和视觉SLAM系统执行局部束调整并提供比IMU积分精度更高的轨迹，可以安全地忽略轨迹不确定性，同时估计惯性参数。这个想法最初是由Mur-Artal和Tardós在ORBSLAM-VI [5]中开创的，后来被Qin等人在VINS-Mono[6,7]中采用。在这两种情况下，惯性参数是通过使用最小二乘法解决一组线性方程来找到的。在[5]中，通过消除每个帧的速度来建立线性系统。然而，在这些代数操作之后，要最小化的误差是毫无意义且与传感器噪声特性无关的。为了获得准确的估计值，包括加速度计偏差，该方法需要15秒进行初始化。在[7]中，假设加速度计偏差为零，只需要1-2秒钟进行初始化，具体时间取决于运动情况。在这两种方法中，IMU测量值被操纵并混合在同一个线性系统中，其中所有方程的残差被认为具有相同的权重，忽略了传感器的不确定性。此外，不同的惯性参数是分别在不同的步骤中单独解决的，而不是一次性解决，忽略它们之间的相关性。所有这些都导致了一个估计值，这个估计值在最大后验概率（MAP）估计的意义上并不是最优的。

我们提出了一种新颖的不相交视觉惯性初始化方法，通过将其表述为最大后验估计问题来实现优化估计。在这里，我们基于Forster等人的优秀工作[8]，该工作允许对IMU读数预积分，并考虑传感器噪声的概率特性，正确计算预积分项的协方差。假设单目SLAM轨迹的误差与IMU误差相比可以忽略不计，我们推到出一种非常高效的惯性参数MAP估计器，并将其用于初始化视觉惯性SLAM系统。本文的主要贡献如下：

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

2、我们一次性解决所有惯性参数，避免了由于解耦估计而产生的不一致性。这使得所有估计都是联合一致的。

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

4、我们不假设IMU偏差为零，相反，我们将已知的关于他们的信息编码为概率先验，并通过我们的MAP估计来利用他们。

在下一部分中，我们将介绍我们提案的理论和深入细节。随后，我们将对其进行评估和比较，与最佳的联合和分离初始化方法进行比较，证明其表现更优秀。

2、最大后验估计初始化

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

我们的主要目标是在初始化中使用MAP估计，并充分利用传感器噪声模型。我们的新型初始化方法基于以下思想：

1、尽管BA具有非线性特性，但单目SLAM（或视觉里程计）已经成熟且稳健，足以获得非常精确的结构和运动初始解，唯一的限制是它们的估计是按比例缩放的。

2、在获得IMU变量的第一个解时，视觉SLAM轨迹的不确定性要比IMU的不确定性要小得多，因此可以忽略。因此我们执行仅惯性的MAP估计，将视觉SLAM的轨迹视为恒定的（即不进行优化）。

3、受[9]的工作启发，我们采用一种参数化方法，明确地表示并优化单目SLAM解决方案的尺度因子。

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

初始化方法分为三步：

1）只有视觉的MAP估计：初始化并运行单目ORB-SLAM[10]，使用BA算法在短时间内（通常为2秒）获得仅视觉的尺度一致的MAP估计。同时，计算关键帧之间的IMU预积分及其协方差[8]。

2）只有惯性的MAP估计：惯性优化仅用于将IMU轨迹和ORB-SLAM轨迹对齐，找到比例尺、关键帧速度、重力方向和IMU偏差。

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

在初始化之后，我们启动ORB-SLAM的视觉惯导[5]，执行局部的VI-BA。我们观察到，在进行完整的VI-BA约5-10秒后，尺度估计精度可以进一步提高，或者通过重复仅惯性优化来实现，其计算成本要低很多。接下来将详细介绍三个初始化步骤。

A、只有视觉的MAP估计

我们使用与ORB-SLAM相同的过程来初始化纯单目SLAM，以找到初始运动。使用ORB描述符，对两个初始帧之间的FAST点进行匹配。找到并评分基础矩阵和单应矩阵模型。使用得分较高的模型来找到初始运动并三角化特征点。一旦结构和运动被初始化，我们会进行1到2秒的纯单目SLAM。与ORB-SLAM唯一的区别在于，我们强制更高频率（4 Hz到10 Hz）插入关键帧。在这种情况下，IMU预积分在关键帧之间具有较低的不确定性，因为积分时间非常短。在此期间，我们获得了一个由十个关键帧和数百个点组成的尺度一致的地图，该地图已经通过ORB-SLAM建图线程使用BA进行了优化。

通过视觉惯性校准，将按比例缩放的关键帧姿态转换为本体（或IMU）参考系。这些本体姿势被表示为 T̄0:k = [R, p̄]0:k，其中 Ri ∈ SO(3) 是从第 i 个本体到世界参考系的旋转矩阵，p̄i ∈ R3 是第 i 个本体的位置（经过缩放）。

B、只有惯性的MAP估计

这一步的目标是使用通过视觉获得的尺度不确定的轨迹，通过MAP估计的方式获得惯性参数的最优估计。由于我们目前对惯性参数没有很好的猜测，因此在这个阶段使用完整的VI-BA会过于昂贵并容易陷入局部最小值，正如实验部分所示。一个中间的解决方案是边缘化掉点，以获得轨迹及其（完全密集的）协方差矩阵的先验，并在优化IMU参数时使用它。我们选择一种更高效的解决方案，将轨迹视为固定的，并进行惯性优化。要找到的惯性参数是：

s属于正数，是只有视觉的尺度因子，Rwg ∈ SO(3) 是重力方向，由两个角度参数化，使得世界参考系中的重力表示为 g = Rwg gI，其中 gI = (0, 0, G) T 是重力的大小，b = (ba , bg ) ∈ R 6 是加速度计和陀螺仪的偏置，v̄ 0:k ∈ R 3 是从第一个到最后一个关键帧的未经缩放的本体速度。

我们更倾向于使用按比例缩小的速度v̄i，而不是真实的速度vi=sv̄i，因为这样可以简化初始化过程。偏差被假定为所有涉及的关键帧都是恒定的，因为初始化期仅为1-2秒，而随机游走几乎没有影响。值得注意的是，这个公式从一开始就考虑了重力的大小，而不像[7]和[5]需要一个单独的步骤来确定它的值。

在我们的情况下，仅使用来自IMU的测量，并且这些测量被总结在[8]中定义的IMU预积分术语中。我们用I i,j表示第i个和第j个关键帧之间惯性测量的预积分，用I 0:k表示初始化窗口中相邻关键帧之间的IMU预积分集合。

有了状态和测量的定义，我们可以制定一个最大后验（MAP）估计问题，其中后验分布为：

其中，p(I 0:k |X k )是给定IMU状态时IMU测量值的似然分布。并且p(Xk)是IMU状态的先验概率。

考虑到测量的独立性，似然函数可以分解为：

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

假设IMU预积分和先验分布的误差服从高斯分布，那么最大后验概率（MAP）问题等价于：

其中，r_p和r_Ii-1,i分别是相邻关键帧之间的先验和IMU测量残差，而Σ_p和Σ_Ii-1,i则是它们的协方差。

在这个优化中，视觉重投影误差没有出现，只有惯性残差。

由于IMU测量不会受到数据关联错误的影响，因此使用鲁棒代价函数（如Huber范数）是没有意义的，因为它会减慢优化的速度。

根据文献[11]和[8]，我们定义惯性残差为：

是从第i关键帧到第j关键帧的IMU测量预积分，其只依赖于偏置。这些项可以按照[8]中所述进行线性更新，避免在每次迭代时重新集成。∆t ij 表示两个关键帧之间的时间差。"Log"代表从李群SO（3）到其代数so（3）的对数映射，与R3同构。由于我们假设偏差在初始化窗口期间可以被视为恒定的，因此IMU残差不包括偏差的随机游走。我们假设残差服从高斯分布，并且它们的协方差可以按照[8]中提出的方法计算。

由于我们正在流形上进行优化，因此需要定义一个投影函数[8]来更新优化过程中的重力方向估计：

Exp(.)是从so(3)到SO(3)的指数映射。

为了确保在优化过程中比例因子保持为正，我们定义其更新方式为：

偏差和速度是通过加法更新的。如果我们定义δgdir = (δαg , δβg)，则在优化过程中使用的惯性参数更新为(δs, δgdir , δbg , δba , {δv̄i})。IMU残差相对于这些参数的导数可以在附录中找到。

图1所表示的最终优化问题已经使用g2o C++库[12]实现并求解，使用解析导数和Levenberg-Marquardt算法。

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

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

我们采用一种明智而实用的方法：将ba作为一个需要优化的参数进行考虑，但同时为其添加一个先验残差。。如果所执行的运动不包含足够的信息来估计偏差，先验概率将保持其估计接近零。如果运动使ba成为可观测量，那么对它的估计将会收敛于其真实值。对于bg的先验分布不是必需的，因为它总是可以从关键帧的方向和陀螺仪读数中得到良好的观测。

由于我们需要解决一个非线性优化问题，因此我们需要一个惯性参数的初始猜测。因此，我们将偏差初始化为零，而重力方向则初始化为加速度计测量的平均值，因为加速度通常比重力小得多。

为了保证收敛，比例因子需要被初始化得足够接近其真实值，但我们没有任何初始猜测。利用我们非常高效的惯性优化（5毫秒），我们使用三个初始尺度值启动优化，这些值对应于中位场景深度为1、4和16米，保留提供最低残差的解，如公式5所定义的那样。我们的结果表明，在使用这个尺度值范围的情况下，我们的方法能够在各种场景中收敛。

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

C、视觉-惯性MAP估计

仅惯性优化提供了足够准确的估计，可用作第一次联合视觉惯性束调整的种子，确保其收敛。在这种优化中，如图1所示，纯惯性参数如g_dir和s并未出现，但它们隐含地包含在关键帧位姿中。与[3]相比，这一步骤替换了BA1＆2步骤。实际上，优化过程完全相同，只是初始种子不同。以前的初始种子是通过解线性系统计算得出的，现在则是通过最大后验估计器计算得出的。在启动VI里程计之前，VINS-Mono初始化也进行了类似的优化。

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

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

结论及将来工作

提出的初始化方法已经证明比文献中表现最好的方法更准确，且计算时间非常短。这证实了最优估计理论能够正确利用传感器噪声的概率模型，获得比解线性方程组或使用非加权最小二乘法更准确的结果。

全视觉惯性BA是一个非常非线性的问题，受到局部最小值的困扰，这阻碍了收敛。我们将其分解为一个完全可观测的视觉问题，直到尺度，然后是一个仅惯性的优化阶段，可以非常高效地解决，为VI-BA产生一个初始解决方案，缓解了局部最小值问题。

作为未来的工作，我们强调惯性优化不仅可以用于初始化，还可以用于在SLAM初始化和运行后优化尺度和其他惯性参数。这将比执行完整的视觉惯性捆绑调整（其中涉及所有视觉和惯性参数）具有更低的计算成本。此外，这种新的初始化方法可以轻松地适应立体惯性情况。只需从惯性优化中删除尺度即可。

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

R EFERENCES
[1] A. Martinelli, “Closed-form solution of visual-inertial structure from
motion,” International Journal of Computer Vision, vol. 106, no. 2,
pp. 138–152, 2014.
[2] J. Kaiser, A. Martinelli, F. Fontana, and D. Scaramuzza, “Simultaneous
state initialization and gyroscope bias calibration in visual inertial
aided navigation,” IEEE Robotics and Automation Letters, vol. 2, no. 1,
pp. 18–25, 2017.
[3] C. Campos, J. M. M. Montiel, and J. D. Tardós, “Fast and robust ini-
tialization for visual-inertial SLAM,” in 2019 International Conference
on Robotics and Automation (ICRA). IEEE, 2019, pp. 1288–1294.
[4] M. Burri, J. Nikolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. W.
Achtelik, and R. Siegwart, “The EuRoC micro aerial vehicle datasets,”
The International Journal of Robotics Research, vol. 35, no. 10, pp.
1157–1163, 2016.
[5] R. Mur-Artal and J. D. Tardós, “Visual-inertial monocular SLAM with
map reuse,” IEEE Robotics and Automation Letters, vol. 2, no. 2, pp.
796–803, 2017.
[6] T. Qin and S. Shen, “Robust initialization of monocular visual-inertial
estimation on aerial robots,” in IEEE/RSJ International Conference on
Intelligent Robots and Systems (IROS), 2017, pp. 4225–4232.
[7] T. Qin, P. Li, and S. Shen, “VINS-Mono: A robust and versa-
tile monocular visual-inertial state estimator,” IEEE Transactions on
Robotics, vol. 34, no. 4, pp. 1004–1020, 2018.
[8] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “IMU preinte-
gration on manifold for efficient visual-inertial maximum-a-posteriori
estimation,” in Robotics: Science and Systems, 2015.
[9] H. Strasdat, J. M. M. Montiel, and A. J. Davison, “Scale drift-aware
large scale monocular SLAM,” Robotics: Science and Systems VI,
vol. 2, 2010.
[10] R. Mur-Artal, J. M. M. Montiel, and J. D. Tardós, “ORB-SLAM: a
versatile and accurate monocular SLAM system,” IEEE Transactions
on Robotics, vol. 31, no. 5, pp. 1147–1163, 2015.
[11] T. Lupton and S. Sukkarieh, “Visual-inertial-aided navigation for high-
dynamic motion in built environments without initial conditions,”
IEEE Transactions on Robotics, vol. 28, no. 1, pp. 61–76, 2012.
[12] R. Kümmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard,
“g2o: A general framework for graph optimization,” in IEEE Inter-
national Conference on Robotics and Automation (ICRA), 2011, pp.
3607–3613.
[13] T. Qin, J. Pan, S. Cao, and S. Shen, “A general optimization-based
framework for local odometry estimation with multiple sensors,” arXiv
preprint arXiv:1901.03638, 2019.