论文阅读：Stereo Visual-Inertial Odometry With Online Initialization and Extrinsic Self-Calibration

前言

Stereo Visual-Inertial Odometry With Online Initialization and Extrinsic Self-Calibration这篇论文是2023年TIM上的一篇文章，主要是针对双目视觉惯性里程计的初始化问题，实现了一个除了估计IMU偏置，速度，重力，IMU -相机外参和平移比例因子的初始值等参数，同时还可以估计外参的初始化系统。

一、问题背景

视觉和IMU互补。不精确的外参标定和长时间运动外参的微小变化会影响双目VIO的准确性。可靠的离线外参标定方法需要固定的视觉标记和理想的仪器套件运动。此外，VIO的性能高度依赖于精确的初始化，这个过程估计加速度计和陀螺仪的偏差，速度和重力方向。

目前大多数的研究都集中在单目VIO，只适用于估计一个IMU -相机对的变换，很少有关于双目VIO的研究，它可以估计两个IMU -相机对的变换。

自校准的主要困难是处理仪器套件中相机的未知变换。此外，现有的研究将测量系统划分为若干个相机- IMU对，间接地解决了相机之间的转换，这可能会带来潜在的误差。相机-相机变换是立体视觉视觉里程计的一个重要参数，因为地标点的位置由立体视觉计算得到。

二、主要假设

双目视觉和IMU的融合定位。

三、理论流程

本文提出的立体视觉视觉里程计的流程，主要包含3个模块：1 )测量预处理模块；2 )初始化和外参标定模块；3 )后端求解器模块。

1、单目惯性初始化

（1）IMU-相机方位标定和陀螺仪零偏校正

图2给出了从世界坐标系到两个连续坐标系i和j的变换。在左相机(图2中的Cam1)的情况下，从bi到clj的旋转可以用两种方式(红色箭头和蓝色箭头见图2)来计算，并且它们在理论上是相等的。等价关系表示为

上述式子可以变换为

在实际中，存在N个框架和相应的几何约束，根据( 9 )式，可以用来构造超定线性方程组

利用下式去除异常值：

根据旋转矩阵与轴线夹角的关系，残差ri j可由下式计算：

理论上，残差ri j趋于零。当残差变大时，( 10 )中的权重变小。求解( 10 )后，陀螺仪的零偏可以通过下面的方程进行估计：

在这一步中，我们只考虑从IMU到左相机的变换。相机-相机外参数的估计将在后面的步骤中进行。

（2）偏差，重力，比例因子和Imu -相机平移估计

在这一步中，我们首先执行单目SFM [ 32 ]来计算相机的位姿，然后我们通过将视觉测量与IMU预积分对齐来估计必要的参数。它们被定义为

令i和j为连续的相机帧，第一个相机c0帧为世界坐标，相机帧c0与第i个IMU本体帧之间的转换为

根据( 6 )，( 7 )和( 15 )，我们可以定义如下的IMU测量残差[ 6 ]，[ 11 ]：

其中1tk是帧与帧之间的时间间隔。给定N个相机和它们之间的IMU测量残差，通过最小化残差之和可以得到最优估计

2、单目VI-BA

在单目惯性初始化后，我们继续使用单目VI - BA进行高精度的状态估计。全状态变量定义为

我们最小化残差之和以获得最大的后验估计。成本函数写为

其中视觉重投影误差为

3、右相机位姿估计

单目VI - BA程序已经估计了一组路标的逆深度和几个身体帧的姿态，因此，我们可以计算每个左相机帧中的三维路标位置。图3显示了一个路标的位置和它在连续三个帧中的投影。这样的地标有数百处。此外，测量预处理模块实现了对右摄像头( Cam2中的xr ,见图3)中路标的观测。在这里，在每个摄像机坐标系中，我们有一组三维地标在左摄像机坐标系中的位置和相应的在右摄像机中的观测值。因此，可以通过最小化3 - D - 2 - D重投影误差来估计相机-相机变换，以地标投影到第k帧为例

4、立体VI - BA和边缘化

在线初始化和外部校准模块在初始化阶段提供必要参数的初始猜测。但其中一些还需要在后续的后端求解器模块中进一步优化。定义整个状态变量为

与单目VI - BA不同，我们的立体VI - BA方法考虑了外部参数包括左IMU -相机和相机-相机变换。视觉测量的残差包含了左右相机的重投影误差。地标点到右相机的投影与相机-相机变换plr和qlr有关。考虑在第i帧首先观测到一个路标，则第j帧的视觉测量残差定义为

其中，Pl cj可通过第i帧到第j帧的位姿变换计算得到

式中：Pl ci可由第j帧下的归一化图像坐标和逆深度计算得到：Pci = xi / ρ i。Pr c j通过相机-相机变换计算得到

整个立体VI - BA被定义为一个代价函数

四、实验方案

我们在广泛使用的基准数据集EuRoC MAV [ 37 ]上评估了我们的立体VIO系统。它由机载微型飞行器采集，包含20 Hz的同步立体图像和200 Hz的IMU。通过Leica MS50激光跟踪仪获得了精确的地面真实位姿。同时给出了外参数和IMU零偏的真实值。该数据集是从没有动态物体的工业机器大厅和Vicon房间收集的。EuRoC MAV数据集已经成为VIO性能评估的标准。所有实验均在Ubuntu 16.04、Intel i5 - 4200四核2.5 GHz CPU、8 GB RAM的低成本笔记本电脑上进行。对于EuRoC数据集中的每条序列，第IV节中每一步的平均处理时间如图4所示。实现外参数的初始猜测用时不到300毫秒。

1、外参标定结果

本节使用EuRoC数据集中的V1 - 02序列展示自校准的性能。我们使用旋转误差和平移误差来评估外参数的自校准结果。评价参数定义为如下公式：

实验表明，初始化和外参校准模块可以提供可靠的外参初始猜测，后端求解器可以有效地进一步优化外参。

2、偏差估计结果

我们使用V1 - 02序列来评估和分析偏差估计结果的表现。在这些实验中，我们将所提出的在线偏差相关性方法与VINS - Fusion [ 24 ]进行了比较，VINS - Fusion是目前最先进的具有在线偏差相关性的立体VIO算法。

3、轨迹准确度比较

使用绝对轨迹误差( ATE )度量的均方根误差( RMSE )来评估所提出的立体VIO的轨迹精度。在本实验中，我们将我们的VIO方法与最先进的使用立体相机的VIO方法进行比较，包括VINS - Fusion [ 24 ]和S - MSCKF [ 8 ]。为了保证公平性，我们关闭了VINS - Fusion的闭环模式。值得注意的是，所提出的方法使用完全未知的外部参数，具体来说，旋转设置为3 × 3的恒等矩阵，平移向量设置为零。

五、创新总结

针对双目相机之间存在共视点的情况，我们提出了一种基于优化的立体VIO方法，该方法能够处理完全未知的外部参数。该方法可以完成初始化过程，并在初始化阶段使用帧数估计所有必要的外参数对。左、右两幅图像中的特征位置均为Kanade - Lucas - Tomasi ( KLT )光流算法跟踪的右图像。我们首先使用左IMU -相机对进行单目视觉-惯性初始化和外参标定[ 5 ]，[ 6 ]，[ 11 ]，提供IMU偏置，速度，重力，IMU -相机外参和平移比例因子的初始值。并将其作为单目视觉-惯性捆绑调整( VI-BA )的初始值来估计关键帧的位姿和三维地标的位置。由于右图像中3 - D特征点的观测值是已知的，因此可以通过最小化3 - D - 2 - D重投影误差来估计右相机的位姿。根据上述步骤，我们获得了立体VIO的所有初始值，包括IMU -相机和相机-相机外参。然而，外生参数的初始猜测并不准确足以支持立体声VIO的运行。它们在立体VI - BA期间被进一步细化。

本文的主要贡献在于：

1 )提出了一种立体视觉视觉里程计，采用由粗到精的策略来处理未知的外部参数。

2 )在初始阶段估计了IMU -相机和相机-相机变换的初始猜测，充分利用了立体相机之间的共同观测特征。

3 )结合位姿、速度和IMU零偏进一步优化外参数。

4 )综合评估表明，即使在没有外部校准的情况下，所提出的立体VIO也能很好地工作。

在本文中，我们提出了一种具有在线初始化和外部自校准的立体VIO。该系统包含3个主要模块：测量预处理模块、初始化和外参标定模块、后端解算模块。初始化和外参标定模块不仅计算IMU偏置、速度和重力方向，而且在没有先验知识的情况下创造性地估计IMU -相机和相机-相机变换。此外，所提出的后端求解器能够进一步细化IMU -相机和相机-相机的外参。实验结果表明，该方法能够在秒级时间内准确估计出无初值的外参数。同时，IMU偏差也被成功估计。在外部参数精确的情况下，轨迹精度与VINS - Fusion和S - MSCKF具有相似的性能。此外，在外参数不准确的情况下，轨迹精度略高于VINS - Fusion。所提出的立体VIO允许设备实现"接通电源-and-go "，而无需繁琐的离线外部校准。