经典文献阅读之--Stereo-NEC(全新双目VIO初始化)

0. 简介

双目VI-SLAM初始化方法分为两种类型：联合方法和分离方法。联合方法通过融合视觉观测和IMU积分来同时处理视觉和惯性参数，但有可能在闭合解中忽视陀螺仪偏置，而且计算成本高。分离方法首先独立解决SfM问题，然后根据纯视觉SLAM系统中的相机姿态推导惯性参数，其准确性在很大程度上取决于纯视觉SLAM的性能。即使在ORB-SLAM3这样的最新双目VI-SLAM系统中，也假设在相邻帧之间有足够的基线和轻微的旋转的情况下，进行准确的相机轨迹估计。

为此《Stereo-NEC: Enhancing Stereo Visual-Inertial SLAM Initialization with Normal Epipolar Constraints》一文提出了一种准确而稳健的双目视觉惯性SLAM系统初始化方法。与当前最先进的方法不同，后者严重依赖于纯视觉SLAM系统的准确性来估计惯性变量，而不更新相机姿态，这可能会影响准确性和稳健性，我们的方法提供了一种不同的解决方案。我们意识到精确陀螺仪偏置估计对旋转准确性的关键影响。这反过来又会由于累积的平移误差而影响轨迹准确性。为了解决这个问题，我们首先独立估计陀螺仪偏置，并使用它来制定进一步细化的最大后验问题。在这一细化之后，我们继续通过从陀螺仪测量中去除陀螺仪偏置来执行IMU积分以更新旋转估计。然后，我们利用健壮而准确的旋转估计通过三自由度捆绑调整来增强平移估计。相关方法在Github上已经开源了。

1. 主要贡献

1. 提出一种新方法，利用立体视觉的正常极线约束来估计初始陀螺仪偏差，并使用后者初始化最大后验（MAP）问题以进行进一步精细化。
2. 通过分别估计旋转来提高初始化的准确性和鲁棒性，首先通过IMU旋转积分进行，然后利用精确可靠的旋转估计来通过3自由度（DoF）束调整增强平移估计。
3. 引入一种新颖的方法来评估初始化成功与否，通过评估正常极线约束的残差来实现。

2. 预备知识

2.1 符号表示

在本文中，我们采用以下符号表示：世界坐标系、物体坐标系和相机坐标系分别用$(\cdot {)}^w$、$(\cdot {)}^b$和$(\cdot {)}^c$表示。对于立体相机，$c_L$和$c_R$分别代表左相机和右相机。我们用$R$表示旋转矩阵，用$v$表示速度向量，用$t$表示平移向量，用$g=(0,0,G{)}^T$表示重力向量，其中$G$是重力的大小。$R_i^j$表示从坐标系$i$到坐标系$j$的相对旋转，$t_i^j$表示相对平移。$b_k$表示在拍摄第$k$张图像时的物体坐标系，$c_k$表示在拍摄第$k$张图像时的左相机坐标系。加速度偏差和陀螺仪偏差在本地物体坐标系中分别用$b_a$和$b_g$表示。${\alpha }_{b_{k+1}}^{b_k}$、${\beta }_{b_{k+1}}^{b_k}$、${\gamma }_{b_{k+1}}^{b_k}$表示从$b_k$到$b_{k+1}$的平移、速度和旋转的预积分。$∆t_{k,k+1}$表示从时间$k$到时间$k+1$的间隔。$\lambda$代表矩阵的一个特征值，${\lambda }_{min}$特指最小特征值。$n$指的是一个法向量。$N$代表用于初始化的关键帧数量。

2.2 单目正常极线约束(MNEC可以计算陀螺仪偏差)

单目正常极线约束(MNEC) [23]编码了单个移动相机两个姿态之间的视线向量与相应极平面法线的几何关系。如图1所示，当考虑两个连续时间点的特征对应时，一对单位视线向量($f_i$和$f_i^{\prime }$)分别来源于时间点$k$和$k+1$的光学中心($O_k$和$O_{k+1}$)分别在时间点$k$和$k+1$，指向3D点($p_i$)。每对视线向量定义了一个极平面以及其关联的法线向量$n_i=f_i\times R_{c_{k+1}}^{c_k}{f}_i^{\prime }$。这些极平面的交点形成一条线，即两帧之间的基线$t_{c_{k+1}}^{c_k}$（由虚线标记）。法线向量共同形成相应的极平面法线，该法线垂直于基线$t_{c_{k+1}}^{c_k}$。假设观测到$n$个3D点，我们将$n$个极平面的法线向量堆叠成一个矩阵$N=[n_1...n_n]$。共面性的要求在数学上等同于矩阵$M=N{N}^T$的最小特征值为零。

MNEC的残差由下式给出：

$$e_i=\left|{\mathbf{n}}_i^{\top }{\mathbf{t}}_{c_{k+1}}^{c_k}\right|.$$

MNEC有两个应用：

1. 旋转：Kneip和Lynen [21] 的目标是确定相对旋转 $R_{c_{k+1}}^{c_k}$ ，它使得最小特征值 ${\lambda }_{\text{min}}(M_{k,k+1}$) 最小化：
   

   矩阵 $M_{k,k+1}$ 具有特定属性：它是实数、对称的、正半定的。此外，由于法线向量的共面性约束，其秩为2。使用Levenberg-Marquardt算法可以求解方程(1)。

   
   

2. 陀螺仪偏差：受Kneip和Lynen的工作启发，He等人[22]利用MNEC直接优化陀螺仪偏差，结合图像观测和相机-IMU外部标定矩阵$[R_{cb}|t_{cb}]$。这改变了从方程(1)到方程(2)的目标函数：

   

在考虑了陀螺仪偏差对第$k$帧和第$k+1$帧之间时间段的影响后，使用${\gamma }_{b_{k+1}}^{b_k}$的一阶泰勒近似估算${\hat{\gamma }}_{bk+1}^{b_k}$，并且$J_{b_g}^{\gamma }$代表了由于陀螺仪偏差估计的微小差异导致的预积分变化。

3. 提出的方法

我们的方法是基于这样一个认识：精确的陀螺仪偏差估计显著影响旋转精度，最终由于平移误差的累积，影响轨迹精度。我们在之前的工作[9]基础上进行了改进，该工作采用了一个误差状态卡尔曼滤波器(ESKF)来估计陀螺仪偏差并校正旋转估计，我们减少了对纯视觉SLAM进行旋转估计的依赖。我们方法的基本思想植根于认识到陀螺仪偏差的重要性。我们首先独立估计陀螺仪偏差，然后使用它来构建MAP问题以进行进一步的细化。在这种细化之后，我们继续更新旋转估计。有了稳健和准确的旋转估计，我们利用它来辅助平移估计。我们的方法包括五个步骤，旨在得到关键帧的初始姿态和速度、重力方向和IMU偏差的精确初始值：

• 步骤0. 纯视觉SLAM：从立体视觉仅SLAM获得初始关键帧姿态。
• 步骤1. 基于特征值的陀螺仪偏差估计器：使用视觉和陀螺仪测量数据，通过构建一个最小化特征值问题来得出初始陀螺仪偏差。
• 步骤2. 陀螺仪偏差精细化，加速度偏差，速度和重力估计器：通过解决一个仅惯性的MAP估计问题，细化陀螺仪偏差并估计关键帧的速度、重力方向和加速度偏差。
• 步骤3. 旋转-平移解耦优化：通过整合去除陀螺仪偏差的陀螺仪测量数据来更新相机旋转估计，并使用3-DoF BA优化相机平移。
• 步骤4. 联合视觉-惯性束调整：使用从前面步骤中得出的解作为视觉-惯性MAP的初始估计，以获得关键帧姿态、关键帧速度、3D点、重力方向和IMU偏差的最优估计。
  在以下小节中，我们将提供对我们初始化步骤的更详细解释。

图2：展示了立体正常极线约束的几何结构图。蓝色和红色虚线三角形分别代表左右相机。左侧的$O_k$和左侧的$O_{k+1}$分别代表时间$k$和$k+1$时的两个左光学中心。类似地，右侧的$O_k$和右侧的$O_{k+1}$对应于两个右光学中心。橙色线代表立体相机的基线。左相机的时间极平面以绿色和黄色表示，而右相机的时间极平面以蓝色和紫色表示。每个时间极平面的相应法向量$n_i$以相应颜色表示。只有来自同一相机的法线向量是共面的。

3.1 基于特征值的陀螺仪偏差估计器

如图2所示，立体正常极线约束扩展了单目正常极线约束，从而能够整合额外的视觉观测信息，增强陀螺仪偏差估计的鲁棒性和准确性。为了在利用立体观测信息的同时估计初始陀螺仪偏差，我们通过最小化方程(3)中的最小特征值${\lambda }_{min}$来初始化陀螺仪偏差：

其中$(f_{L_i},f_{L_i}^{\prime })$表示左相机中的一对视线向量，$(f_{R_i},f_{R_i}^{\prime })$表示右相机中的一对视线向量。$n_L$和$n_R$表示左相机和右相机观测到的特征数量。$R_{c_L}^b$和$R_{c_R}^b$分别代表左相机-IMU外部标定的旋转矩阵和右相机-IMU外部标定的旋转矩阵。${}^L{M}_{k,k+1}$和${}^R{M}_{k,k+1}$是利用来自左右相机的法线构建的。$E$表示在连续两个时间点的关键帧对集合。

3.2 陀螺仪偏差精细化，加速度偏差，速度和重力估计器

…详情请参照古月居