【SchurVINS论文】: 基于舒尔补的轻量级视觉惯性导航系统

搬了一篇开源的VINS框架的论文翻译，供自己和爱好者们学习和探讨，原文如下。本文仅做学术分享，如有侵权，请联系删文。

标题：SchurVINS: Schur Complement-Based Lightweight Visual Inertial Navigation System
作者：Yunfei Fan, Tianyu Zhao, Guidong Wang
机构：字节跳动
论文：https://arxiv.org/abs/2312.01616
代码：https://github.com/bytedance/SchurVINS

摘要

准确性和计算效率是视觉惯性导航系统（VINS）中最重要的指标。现有的VINS算法要么具有很高的准确性，要么具有较低的计算复杂度，很难在资源受限的设备上提供高精度的定位。为此，我们提出了一种新颖的基于滤波器的VINS框架，名为SchurVINS（SV），它通过构建完整的残差模型来保证高准确性，并利用舒尔补来降低计算复杂度。技术上，我们首先制定了完整的残差模型，其中明确建模了梯度、 Hessian矩阵和观测协方差。然后，利用舒尔补将完整模型分解为自我运动残差模型和地标残差模型。最后，在这两个模型中实现了高效的扩展卡尔曼滤波器（EKF）更新。在EuRoC和TUM-VI数据集上的实验表明，我们的方法在准确性和计算复杂度方面明显优于最先进的方法。SchurVINS的实验代码可在https://github.com/bytedance/SchurVINS 上获得。

1 引言

高精度定位技术已成为各种工业领域的基石，在机器人、增强现实（AR）和虚拟现实（VR）等领域发挥着不可或缺的作用。近几十年来，视觉惯性导航系统（VINS）因其低成本和普及性而备受关注。VINS模块仅由相机和惯性测量单元（IMU）组成，可以提供与昂贵的传感器（如激光雷达）一样准确的六自由度（6-DOF）定位，并且更适用于安装在便携设备，如智能手机和微型无人机（MAV）等。

据报道，各种优秀的开源VINS算法能够实现高精度的姿态估计，主要包括两种方法论：基于优化和基于滤波。典型的基于优化的方法[4, 17, 21, 24, 32, 33]将姿态和对应的观测地标联合建模。借助舒尔补技术[1]，这种具有特殊稀疏性的高维模型可以通过捆绑调整（BA [31]）高效求解。理论上[11]，尽管基于优化的方法在定位的高精度方面非常显著，但可能会受到高计算复杂性的影响。相比之下，主流的基于滤波的方法[2, 7, 10, 29]源自于MSCKF [22]，利用左零空间方法简化残差模型。然后，EKF [28] 更新被执行在简化的残差模型上，以估计相应的姿态。最后，它们实现了高效率，但牺牲了准确性，因为地标没有与相机姿态联合优化，并且所有观测只被利用一次。总之，基于优化的方法在准确性方面具有优势，而基于滤波的方法则更加高效。

因此，迫切需要开发一个将它们的高精度和高效性结合起来的框架。正如上文所讨论的，没有简化的传统残差模型可以实现高精度。尽管如此，当地标和姿态都被并入状态向量进行联合估计时，EKF-SLAM的效率显著降低[22]。受基于优化的方法中的舒尔补的启发，我们充分利用了由姿态和地标构建的高维残差模型中固有的稀疏结构，以在EKF中实现高效率。因此，提出了一种同时实现高效率和准确性的基于EKF的VINS框架。在该框架中，基于传统残差模型导出了等效残差模型，包括梯度、黑塞矩阵和相应的观测协方差。考虑到黑塞矩阵的特殊稀疏结构，进行舒尔补操作将等效残差方程分解为两个较小的方程：等效姿态残差模型和等效地标残差模型。由于其自身的稀疏结构，等效地标残差模型能够进一步分解为一系列小的等效残差模型。最后，利用导出的等效残差模型实现了EKF更新，以联合估计姿态和相应的地标。如图1所示，所得到的框架在延迟、计算复杂度和准确性方面优于SOTA方法。我们的主要贡献总结如下：

• 提出了一个等效残差模型来处理超高维观测，其中包括梯度、黑塞矩阵和相应的观测协方差。这种方法在EKF系统中具有很大的通用性。
• 提出了一种轻量级的基于EKF的地标求解器，以高效地估计地标的位置。
• 开发了一种新颖的基于EKF的VINS框架，实现了自我运动和地标的同时估计，具有高精度和高效性。实验代码已发布，以造福社区。

2 相关工作

提高VINS算法的效率和准确性是一个持续不断的工作。迄今为止，已经进行了大量研究，旨在降低计算复杂度并提高精度。许多VINS算法专注于提高效率。一些研究利用先前优化的中间结果来减少重复计算的量 [14–16, 21]。虽然这些方法可能会导致轻微的精度损失，但计算过程可以显著加速。一些其他研究尝试通过工程技术实现高效率。在[23, 35]中，采用了高效的黑塞矩阵构建和舒尔补计算来提高缓存效率并避免冗余矩阵表示。在[6, 34]中，变量声明为单精度而不是传统的双精度，以加速算法。除了效率之外，一些研究集中于提高准确性。在[12, 13, 20]中，通过改进基于EKF的VINS中的一致性来保证高准确性。一些改进的MSCKF，即混合MSCKF [10, 18]（结合了MSCKF和EKF-SLAM），近期提出以平衡效率和准确性，通过将信息丰富的地标选择性地作为其状态变量的一部分来进行联合估计 [19]。一些研究人员构建了运行在其他线程上的局部捆绑调整（LBA）来减少漂移 [4, 9]。然而，LBA需要大量的计算资源，这在小型设备上实施可能不太实际。

3 SchurVINS框架

本文提出的SchurVINS是基于开源的SVO2.0 [8, 9]（采用立体配置）开发的，其中采用了基于滑动窗口的EKF后端来替换SVO2.0中的原始后端，并且采用了基于EKF的地标求解器来替换原始的地标优化器。SchurVINS算法的框架和SVO之间的关系如图2所示。

Figure 2. SchurVINS框架，展示了SVO和SchurVINS之间的关系。P1到Pm表示周围环境的有效地标，这些地标被用于构建残差模型

3.1

通常情况下，对于传统的基于EKF的VINS系统[7, 10, 20]，基本的IMU状态定义如下：
$${\mathbf{x}}_I={\left[\begin{array}{lllll}{}_I^G{\mathbf{q}}^{\top }& G_{{\mathbf{p}}_I}{}^{\top }& G_{{\mathbf{v}}_I}{}^{\top }& {\mathbf{b}}_a^{\top }& {\mathbf{b}}_g^{\top }\end{array}\right]}^{\top }\text{\hspace{1em}(1)}$$
其中， { G } , { I } \{\mathrm{G}\},\{\mathrm{I}\} {G},{I}和 { C } \{\mathrm{C}\} {C}分别表示全局坐标系、本地坐标系和相机坐标系。${}^G{\mathbf{p}}_I$和${}^G{\mathbf{v}}_I$分别表示IMU在全局坐标系 { G } \{\mathrm{G}\} {G}中的位置和速度。${}_I^G\mathbf{q}$表示从 { I } \{\mathrm{I}\} {I} 到 { G } \{\mathrm{G}\} {G} 的旋转四元数（在本文中，四元数遵循哈密顿规则[28]）。向量${\mathbf{b}}_a$和${\mathbf{b}}_g$ 分别表示来自IMU的角速度和线性加速度的偏置。因此，对应的EKF误差状态${\mathbf{x}}_I$定义如下：
$${\tilde{\mathbf{x}}}_I={\left[\begin{array}{lllll}{}_I^G{\tilde{\mathbf{\theta }}}^{\top }& {}^G{\tilde{\mathbf{p}}}_I^{\top }& {}^G{\tilde{\mathbf{v}}}_I^{\top }& {\tilde{\mathbf{b}}}_a^{\top }& {\tilde{\mathbf{b}}}_g^{\top }\end{array}\right]}^{\top }\text{\hspace{1em}(2)}$$
其中，${}_I^G\tilde{\mathbf{\theta }}$表示 ${}_I^G\mathbf{q}$的误差状态。除了四元数外，其他状态可以使用标准的加法误差（例如，$\mathbf{x}=\hat{\mathbf{x}}+\tilde{\mathbf{x}}$）。与[28]类似，四元数的扩展加法误差定义如下（在本文中，四元数误差定义在 { G } \{G\} {G}坐标系中）：
$${\mathbf{q}}_I^G={\delta }_I^G\mathbf{q}\otimes {}_I^G\hat{\mathbf{q}},{\delta }_I^G\mathbf{q}={\left[\begin{array}{cc}1& \frac{1}{2}{\delta }_I^G\tilde{\mathbf{\theta }}\end{array}\right]}^{\top }\text{\hspace{1em}(3)}$$
同样，旋转矩阵的扩展加法误差定义如下：
$$\begin{array}{c}\mathbf{R}\left({}_I^G\mathbf{q}\right)={}_I^G\mathbf{R},{}_I^G\mathbf{R}={\left(\mathbf{I}+{\left[{}_I^G\tilde{\mathbf{\theta }}\right]}_{\times }\right)}_I^G\hat{\mathbf{R}}\end{array}\text{\hspace{1em}(4)}$$

3.2. 传播和扩展

SchurVINS遵循了[28]中介绍的状态传播策略。IMU状态的时间演化描述如下
$$\begin{array}{c}{}_I^G\dot{\hat{\mathbf{q}}}=\frac{1}{2}{}_I^G\hat{\mathbf{q}}\otimes \Omega (\hat{\mathbf{\omega }})\\ {\dot{\hat{\mathbf{b}}}}_g=0_{3\times 1},{\dot{\hat{\mathbf{b}}}}_a=0_{3\times 1}\\ {}^G{\dot{\hat{\mathbf{p}}}}_I={}^G{\mathbf{v}}_I,{}^G{\dot{\hat{\mathbf{v}}}}_I={}_I^G\hat{\mathbf{R}}\hat{\mathbf{a}}+{}^G\mathbf{g}\end{array}\text{\hspace{1em}(5)(6)(7)}$$
其中，$\hat{\mathbf{\omega }}={\mathbf{\omega }}_m-{\hat{\mathbf{b}}}_g$和 $\hat{\mathbf{a}}={\mathbf{a}}_m-{\hat{\mathbf{b}}}_a$ 是去除偏置后的IMU测量值，并有
$$\Omega (\hat{\mathbf{\omega }})=\left(\begin{array}{cc}0& -{\hat{\mathbf{\omega }}}^{\top }\\ \hat{\mathbf{\omega }}& -[\hat{\mathbf{\omega }}{]}_{\times }\end{array}\right)$$
其中，$[\hat{\mathbf{\omega }}{]}_{\times }$ 是$\hat{\mathbf{\omega }}$的反对称矩阵。
基于公式（5）到（7），误差IMU状态的线性化连续动力学定义为：$$\begin{array}{c}{\dot{\tilde{\mathbf{x}}}}_I=\mathbf{F}{\tilde{\mathbf{x}}}_I+\mathbf{G}{\mathbf{n}}_I\end{array}\text{\hspace{1em}(8)}$$

其中，${\mathbf{n}}_I={\left[\begin{array}{llll}{\mathbf{n}}_a{}^{\top }& {\mathbf{n}}_{a\omega }{}^{\top }& {\mathbf{n}}_g{}^{\top }& {\mathbf{n}}_{g\omega }{}^{\top }\end{array}\right]}^{\top }$。向量${\mathbf{n}}_a$和${\mathbf{n}}_g$表示加速度计和陀螺仪测量的高斯噪声，而${\mathbf{n}}_{a\omega }$和${\mathbf{n}}_{g\omega }$ 则表示加速度计和陀螺仪测量偏置的随机游走率。$\mathbf{F}$和$\mathbf{G}$的定义如下：
$$\begin{array}{rl}& \mathbf{F}=\left[\begin{array}{ccccc}{0}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& -{}_I^G\mathbf{R}\\ 0_{3\times 3}& 0_{3\times 3}& {\mathbf{I}}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ -{\left[{}_I^G\mathbf{R}\hat{\mathbf{a}}\right]}_{\times }& 0_{3\times 3}& 0_{3\times 3}& -{}_I^G\mathbf{R}& 0_{3\times 3}\\ 0_{6\times 3}& 0_{6\times 3}& 0_{6\times 3}& 0_{6\times 3}& 0_{6\times 3}\end{array}\right]\\ & \mathbf{G}=\left[\begin{array}{cccc}{0}_{3\times 3}& 0_{3\times 3}& -{}_I^G{\mathbf{R}}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ {}_I^G{\mathbf{R}}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& {\mathbf{I}}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& {\mathbf{I}}_{3\times 3}\end{array}\right]\end{array}\text{\hspace{1em}(9)(10)}$$

在公式（3）到（7）中采用了$4^{\text{th }}$ 龙格-库塔数值积分方法来传播估计的IMU状态。基于公式（8），离散时间状态转移矩阵$\mathbf{\Phi }$和离散时间噪声协方差$\mathbf{Q}$被定义如下：
$$\begin{array}{rl}& \mathbf{\Phi }={\mathbf{I}}_{15\times 15}+\mathbf{F}dt+{\mathbf{F}}^{2}d{t}^2+{\mathbf{F}}^{3}d{t}^3\\ & \mathbf{Q}=\mathbf{\Phi G}{\mathbf{Q}}_I{\mathbf{G}}^{\top }{\mathbf{\Phi }}^{\top }dt\end{array}\text{\hspace{1em}(11)}$$
其中，${\mathbf{Q}}_I=E\left[{\mathbf{n}}_I{\mathbf{n}}_I{}^{\top }\right]$ 是系统的连续时间噪声协方差矩阵。因此，协方差传播的公式为
$$\begin{array}{c}{\mathbf{P}}_{II}\leftarrow \mathbf{\Phi }{\mathbf{P}}_{II}{\mathbf{\Phi }}^{\top }+\mathbf{Q},{\mathbf{P}}_{IA}\leftarrow \mathbf{\Phi }{\mathbf{P}}_{IA}\end{array}\text{\hspace{1em}(12)}$$
协方差矩阵$\mathbf{P}$被划分如公式（13）所示。${\mathbf{P}}_{II}$是基本状态的协方差。${\mathbf{P}}_{IA}$和${\mathbf{P}}_{AI}$是基本状态和扩展状态之间的协方差。${\mathbf{P}}_{AA}$是扩展状态的协方差。
$$\mathbf{P}=\left[\begin{array}{ll}{\mathbf{P}}_{II}& {\mathbf{P}}_{IA}\\ {\mathbf{P}}_{IA}^{\top }& {\mathbf{P}}_{AA}\end{array}\right]\text{\hspace{1em}(13)}$$

图3. 我们系统的示意图，展示了十个地标和尺寸为三的滑动窗口，如图(a)所示，以及不同方法的海森矩阵或协方差，如图(b)-(d)所示。图(b)展示了我们的算法，其中每个单独地标的协方差与滑动窗口中姿态的整体协方差是独立的。图©展示了滑动窗口中地标和姿态的海森矩阵。图(d)展示了传统的混合MSCKF，其中选择的地标和姿态在滑动窗口中的协方差。

当新的图像到达时，当前的IMU姿态${\mathbf{x}}_{Ai}=$ ${\left[{}_I^G{\mathbf{q}}^{\top }{}^G{\mathbf{p}}_I^{\top }\right]}^{\top }$也被扩展，并更新其协方差。扩展的公式为：
$$\begin{array}{rl}& \mathbf{X}={\left[\begin{array}{lllll}{\mathbf{x}}_I{}^{\top }& {\mathbf{x}}_{A0}{}^{\top }& {\mathbf{x}}_{A1}{}^{\top }& \dots & {\mathbf{x}}_{Ai}{}^{\top }\end{array}\right]}^{\top }\\ & \mathbf{P}\leftarrow \left[\begin{array}{cc}\mathbf{P}& {\mathbf{P}}_{21}{}^{\top }\\ {\mathbf{P}}_{21}& {\mathbf{P}}_{22}\end{array}\right]\end{array}\text{\hspace{1em}(14)}$$其中， ${\mathbf{P}}_{21}={\mathbf{J}}_a\mathbf{P},{\mathbf{P}}_{22}={\mathbf{J}}_a\mathbf{P}{\mathbf{J}}_a^{\top }$。而${\mathbf{J}}_a$是关于误差状态${\tilde{\mathbf{x}}}_{Ai}$的雅可比矩阵，定义如下：$$\mathbf{J}=\left[\begin{array}{lll}{\mathbf{I}}_{3\times 3}& 0_{3\times 3}& 0_{3\times (9+6N)}\\ 0_{3\times 3}& {\mathbf{I}}_{3\times 3}& 0_{3\times (9+6N)}\end{array}\right]\text{\hspace{1em}(15)}$$

3.3 基于舒尔补的状态更新

在SchurVINS方案中，与MSCKF方法不同，EKF更新是基于滑动窗口中所有成功三角化的地标及其观测进行的，这可以尽可能地消除由于每个图像时间戳间隔中状态传播而引起的漂移。对于单个观测，相机测量的重投影误差${\mathbf{r}}_{i,j}$被定义为：$$\begin{array}{rl}{\mathbf{r}}_{i,j}& ={\mathbf{z}}_{i,j}-{\hat{\mathbf{z}}}_{i,j}\\ {\mathbf{r}}_{i,j}& ={\mathbf{J}}_{x,i,j}\tilde{\mathbf{X}}+{\mathbf{J}}_{f_j}{}^G{\tilde{\mathbf{p}}}_{f_j}+{\mathbf{n}}_{i,j}\\ {\hat{\mathbf{z}}}_{i,j}& =\frac{1}{C_i{\hat{Z}}_j}\left[\begin{array}{c}{}^{C_i}{\hat{X}}_j\\ {}^{C_i}{\hat{Y}}_j\end{array}\right]\end{array}\text{\hspace{1em}(16)}$$
其中，${\mathbf{r}}_{i,j}$和${\mathbf{z}}_{i,j}$分别是滑动窗口中第$i$个姿态处的第$j$个地标的重投影误差和相机测量值，${\hat{\mathbf{z}}}_{i,j}$是由估计状态计算得到的对应的理论测量值。${}^{C_i}{\mathbf{p}}_j=$ $\left[\begin{array}{lll}{}^{C_i}{\hat{X}}_j& {}^{C_i}{\hat{Y}}_j& {}^{C_i}{\hat{Z}}_j\end{array}\right]$是第$i$个滑动窗口中的相机姿态中的地标坐标。${\mathbf{n}}_{i,j}$表示相应的测量标准差（或测量噪声）。$\tilde{\mathbf{X}}$和${}^G{\tilde{\mathbf{p}}}_{fj}$分别是状态扰动和地标位置扰动。${\mathbf{J}}_{x,i,j}$和${\mathbf{J}}_{f_i}$分别是相对于系统状态和地标位置的残差的雅可比矩阵。这些雅可比矩阵定义如下：$$\begin{array}{rl}& {\mathbf{J}}_{x,i,j}=\left[\begin{array}{lll}{0}_{2\times (15+6i)}& {\mathbf{J}}_A& 0_{2\times 6(N-i-1)}\end{array}\right]\\ & {\mathbf{J}}_A={\mathbf{J}}_{i,j}\left[{}_C^I{\hat{\mathbf{R}}}^{\top }\left[{}^{I_i}{\hat{\mathbf{p}}}_{f_j}\right]\times {}_{I_i}^G{\hat{\mathbf{R}}}^{\top }-{}_C^G{\hat{\mathbf{R}}}^{\top }\right]\\ & {\mathbf{J}}_{f_j}=\left[{\mathbf{J}}_{i,j}{}_{C_i}^G{\hat{\mathbf{R}}}^{\top }\right]\end{array}$$为了方便起见，我们使用针孔模型定义相机模型。因此，${\mathbf{J}}_{i,j}$定义为：$${\mathbf{J}}_{i,j}=\frac{1}{C_i{\hat{Z}}_j^2}\left[\begin{array}{ccc}{}^{C_i}{\hat{Z}}_j& 0& -{}^{C_i}{\hat{X}}_j\\ 0& {}^{C_i}{\hat{Z}}_j& -{}^{C_i}{\hat{Y}}_j\end{array}\right]\text{\hspace{1em}(18)}$$
针对滑动窗口中所有地标的所有观测，我们可以通过堆叠所有残差方程来获取完整的残差模型：$$\mathbf{r}=\left[\begin{array}{ll}{\mathbf{J}}_x& {\mathbf{J}}_f\end{array}\right]\left[\begin{array}{c}\tilde{\mathbf{X}}\\ {}^{{\tilde{\mathbf{p}}}_f}\end{array}\right]+\mathbf{n},\mathbf{n}=[u,u,u,\cdots ,u{]}^{\top }\text{\hspace{1em}(19)}$$其中，$\mathbf{r}$和$\left[\begin{array}{ll}{\mathbf{J}}_x& {\mathbf{J}}_f\end{array}\right]$分别是堆叠后的残差和堆叠后的雅可比矩阵。${\mathbf{J}}_x$和${\mathbf{J}}_f$分别是相对于状态和地标位置的雅可比矩阵。$\mathbf{n}$是堆叠后的测量噪声，$\mathbf{n}$的测量协方差是$\mathbf{R}=\mathrm{diag}\left(u^2,u^2,\cdots ,u^2\right)$，其中$u$是$\mathbf{n}$的标准差之一。

与[7, 10, 29]不同，本文中，残差模型方程（19）被投影到雅可比空间 ${\left[\begin{array}{ll}{\mathbf{J}}_x& {\mathbf{J}}_f\end{array}\right]}^{\top }$中，用于制定等效残差方程，其包括了梯度、海森矩阵和观测协方差，如下面的方程（20）和（21）所示。值得强调的是，这种策略是任何具有超高维测量的EKF系统加速的一种替代策略，而不是QR分解策略。$$\begin{array}{rl}\left[\begin{array}{l}{\mathbf{J}}_x^{\top }\\ {\mathbf{J}}_f^{\top }\end{array}\right]\mathbf{r}& =\left[\begin{array}{l}{\mathbf{J}}_x^{\top }\\ {\mathbf{J}}_f^{\top }\end{array}\right]\left[\begin{array}{ll}{\mathbf{J}}_x& {\mathbf{J}}_f\end{array}\right]\left[\begin{array}{c}\tilde{\mathbf{X}}\\ {}^{\ast }\\ {\tilde{\mathbf{P}}}_f\end{array}\right]+{\mathbf{n}}^{\prime }\\ {\mathbf{R}}^{\prime }& =\left[\begin{array}{l}{\mathbf{J}}_x^{\top }\\ {\mathbf{J}}_f^{\top }\end{array}\right]\mathbf{R}\left[\begin{array}{ll}{\mathbf{J}}_x& {\mathbf{J}}_f\end{array}\right]\end{array}\text{\hspace{1em}(20)(21)}$$其中，${\mathbf{n}}^{\prime }$和${\mathbf{R}}^{\prime }$分别是等效观测噪声和协方差。显然，方程（20）和（21）可以简化为：$$\begin{array}{rl}\underset{\left[\begin{array}{l}{\mathbf{b}}_1\\ {\mathbf{b}}_2\end{array}\right]}{\underbrace{\left[\begin{array}{l}{\mathbf{J}}_x^{\top }\mathbf{r}\\ {\mathbf{J}}_f^{\top }\mathbf{r}\end{array}\right]}}& =\underset{\left[\begin{array}{cc}{\mathbf{C}}_1& {\mathbf{C}}_2\\ {\mathbf{C}}_2^{\top }& {\mathbf{C}}_3\end{array}\right]}{\underbrace{\left[\begin{array}{ll}{\mathbf{J}}_x^{\top }{\mathbf{J}}_x& {\mathbf{J}}_x^{\top }{\mathbf{J}}_f\\ {\mathbf{J}}^{\top }{\mathbf{J}}_x& {\mathbf{J}}_f^{\top }{\mathbf{J}}_f\end{array}\right]}}\left[\begin{array}{c}\tilde{\mathbf{X}}\\ W{\tilde{\mathbf{P}}}_f\end{array}\right]+\underset{\left[\begin{array}{l}{\mathbf{n}}_1^{\prime }\\ {\mathbf{n}}_2^{\prime }\end{array}\right]}{\underbrace{{\mathbf{n}}^{\prime }}}\\ {\mathbf{R}}^{\prime }& =\left[\begin{array}{ll}{\mathbf{J}}_x^{\top }{\mathbf{J}}_x& {\mathbf{J}}_x^{\top }{\mathbf{J}}_f\\ {\mathbf{J}}_f^{\top }{\mathbf{J}}_x& {\mathbf{J}}_f^{\top }{\mathbf{J}}_f\end{array}\right]u^2\end{array}\text{\hspace{1em}(22)(23)}$$
图4. SchurVINS在TUM-VI和EuRoC数据集上的实验轨迹和点云。
由于${}^G{\tilde{\mathbf{P}}}_f$没有包含在方程（14）中的状态中，因此需要在方程（20）和（21）上应用舒尔补[27]来边缘化隐含状态。简单来说，方程（22）和（23）应该投影到$\mathbf{L}$空间，如方程（24）和（25）所示。$$\begin{array}{rl}\mathbf{L}\left[\begin{array}{l}{\mathbf{b}}_1\\ {\mathbf{b}}_2\end{array}\right]& =\mathbf{L}\left[\begin{array}{cc}{\mathbf{C}}_1& {\mathbf{C}}_2\\ {\mathbf{C}}_2^{\top }& {\mathbf{C}}_3\end{array}\right]\left[\begin{array}{c}\tilde{\mathbf{X}}\\ W{\tilde{\mathbf{P}}}_f\end{array}\right]+\left[\begin{array}{l}{\mathbf{n}}_1^{\prime \prime }\\ {\mathbf{n}}_2^{\prime \prime }\end{array}\right]\\ {\mathbf{R}}^{\prime \prime }& =\mathbf{L}\left[\begin{array}{cc}{\mathbf{C}}_1& {\mathbf{C}}_2\\ {\mathbf{C}}_2^{\top }& {\mathbf{C}}_3\end{array}\right]{\mathbf{L}}^{\top }{u}^2=\left[\begin{array}{cc}{\mathbf{R}}_1^{\prime \prime }& 0\\ 0& {\mathbf{R}}_2^{\prime \prime }\end{array}\right]\end{array}\text{\hspace{1em}(24)(25)}$$

其中，${\left[\begin{array}{ll}{\mathbf{n}}_1^{\prime \prime \top }& {\mathbf{n}}_2^{\prime \prime \top }\end{array}\right]}^{\top }$和${\mathbf{R}}^{\prime \prime }$是导出的观测噪声和协方差。$\mathbf{L}$定义如下：$$\mathbf{L}=\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{C}}_2{\mathbf{C}}_3^{-1}\\ 0& \mathbf{I}\end{array}\right]\text{\hspace{1em}(26)}$$
将方程（26）代入方程（24）和（25）得到简化的公式：$$\begin{array}{c}\left[\begin{array}{c}{\mathbf{b}}_1-{\mathbf{C}}_2{\mathbf{C}}_3^{-1}{\mathbf{b}}_2\\ {\mathbf{b}}_2\end{array}\right]=\mathbf{C}\left[\begin{array}{c}\tilde{\mathbf{X}}\\ {}^W{\tilde{\mathbf{P}}}_f\end{array}\right]+\left[\begin{array}{l}{\mathbf{n}}_1^{\prime \prime }\\ {\mathbf{n}}_2^{\prime \prime }\end{array}\right]\\ {\mathbf{R}}^{\prime \prime }=\left[\begin{array}{cc}\left({\mathbf{C}}_1-{\mathbf{C}}_2{\mathbf{C}}_3^{-1}{\mathbf{C}}_2^{\top }\right)& 0\\ 0& {\mathbf{C}}_3\end{array}\right]u^2\end{array}\text{\hspace{1em}(27)(28)}$$其中$$\mathbf{C}=\left[\begin{array}{cc}\left({\mathbf{C}}_1-{\mathbf{C}}_2{\mathbf{C}}_3^{-1}{\mathbf{C}}_2^{\top }\right)& 0\\ {\mathbf{C}}_2^{\top }& {\mathbf{C}}_3\end{array}\right]\text{\hspace{1em}(29)}$$
将方程（27）和（28）分解为如下的方程（30）到（31）和方程（32）到（33）：$$\begin{array}{rl}\left[{\mathbf{b}}_1-{\mathbf{C}}_2{\mathbf{C}}_3^{-1}{\mathbf{b}}_2\right]& =\left[{\mathbf{C}}_1-{\mathbf{C}}_2{\mathbf{C}}_3^{-1}{\mathbf{C}}_2^{\top }\right]\tilde{\mathbf{X}}+{\mathbf{n}}_1^{\prime \prime }\\ {\mathbf{R}}_1^{\prime \prime }& =\left[{\mathbf{C}}_1-{\mathbf{C}}_2{\mathbf{C}}_3^{-1}{\mathbf{C}}_2^{\top }\right]u^2\\ \left[{\mathbf{b}}_2-{\mathbf{C}}_2^{\top }\tilde{\mathbf{X}}\right]& =\left[{\mathbf{C}}_3\right]{}^W{\tilde{\mathbf{P}}}_f+{\mathbf{n}}_2^{\prime \prime }\\ {\mathbf{R}}_2^{\prime \prime }& =\left[{\mathbf{C}}_3\right]u^2\end{array}\text{\hspace{1em}(30)(31)(32)(33)}$$显然，方程（30）和（31）是等效的残差方程和观测噪声协方差。它们可以代入标准的EKF模型方程（34）和（37）直接进行状态更新。
$$\begin{array}{rl}\mathbf{K}& =\mathbf{P}{\mathbf{J}}^{\top }{\left({\mathbf{JPJ}}^{\top }+\mathbf{R}\right)}^{-1}\\ \Delta \mathbf{x}& =\mathbf{Kr}\\ \mathbf{P}& =(\mathbf{I}-\mathbf{KJ})\mathbf{P}(\mathbf{I}-\mathbf{KJ}{)}^{\top }+\mathbf{KR}{\mathbf{K}}^{\top }\\ \mathbf{x}& =\mathbf{x}\oplus \Delta \mathbf{x}\end{array}\text{\hspace{1em}(34)(35)(36)(37)}$$

3.4 EKF-based Landmark Solver

通过将方程（30）和（31）代入方程（34）到（37）中，可以得到$\tilde{\mathbf{X}}$。然后，将得到的$\tilde{\mathbf{X}}$代入方程（32）中，建立地标等效残差方程。$$\left[\begin{array}{c}{\mathbf{r}}_1\\ {\mathbf{r}}_2\\ ⋮\\ {\mathbf{r}}_m\end{array}\right]=\left[\begin{array}{llll}{\mathbf{C}}_{3_1}& & & \\ & {\mathbf{C}}_{3_2}& & \\ & & \ddots & \\ & & & {\mathbf{C}}_{3_m}\end{array}\right]\left[\begin{array}{c}W{\tilde{\mathbf{P}}}_{f_1}\\ {}^W{\tilde{\mathbf{P}}}_{f_2}\\ ⋮\\ {}^W{\tilde{\mathbf{P}}}_{f_m}\end{array}\right]+{\mathbf{n}}_2^{\prime \prime }\text{\hspace{1em}(38)}$$其中，$\mathbf{C}31,\cdots ,\mathbf{C}{3}_m$是方程（22）中$\mathbf{C}3$的对角元素。相应的协方差$\mathbf{R}{2}^{\prime \prime }$是：$${\mathbf{R}}_2^{\prime \prime }=\left[\begin{array}{llll}{\mathbf{C}}_{3_1}{u}^2& & & \\ & {\mathbf{C}}_{3_2}{u}^2& & \\ & & \ddots & \\ & & & {\mathbf{C}}_{3_m}{u}^2\end{array}\right]\text{\hspace{1em}(39)}$$由于得到的地标等效残差方程的稀疏性，方程（38）和（39）被拆分为一组小型独立的残差模型，如方程（40）所示，这使得每个地标的EKF更新可以逐个进行。这显著降低了计算复杂度。$$\begin{array}{rl}\left[{\mathbf{r}}_i\right]& =\left[{\mathbf{C}}_{3_i}\right]\left[{}^W{\tilde{\mathbf{P}}}_{f_i}\right]+{\mathbf{n}}_{2_i}^{\prime \prime },i=1,\cdots ,m\\ \mathbf{R}& =\left[{\mathbf{C}}_{3_i}{u}^2\right]\end{array}\text{\hspace{1em}(40)}$$
此外，还有一种替代方法，即可以将得到的$\tilde{\mathbf{X}}$反向代入一般残差模型方程（16），以建立地标的替代等效残差方程。

${}^1\mathrm{S}$和$\mathrm{M}$分别表示立体和单眼方法。
${}^2\mathrm{F}$和$\mathrm{O}$分别表示基于滤波器和基于优化的方法。
${}^3$ OpenVINS-4表示在OpenVINS中，滑动窗口的最大大小被配置为4。
结果取自[5]。
${}^5$由作者手动评估。
所有其他结果均取自相应的论文。

表1. 在EuRoC数据集上对各种单目和立体VINS算法进行的准确性评估。在上部，我们总结了运行滑动窗口优化以估计姿态的基于优化的方法的结果。在下部，我们评估了基于滤波器的方法的结果。最佳结果用粗体标出，下划线表示在基于滤波器的方法中的最佳结果。SchurVINS在基于滤波器的方法中达到了最低的平均APE RMSE，并超过了大多数基于优化的方法。
${}^1$ 结果取自[33]。

${}^2$ 由作者手动评估。

表2. 在TUM-VI数据集上的准确性评估。$\mathrm{c}1$至$\mathrm{c}5$表示TUM-VI数据集中的走廊1至走廊5。$\mathrm{r}1$至r6表示TUM-VI数据集中的房间1至房间6。最佳结果用粗体标出，下划线表示在基于滤波器的方法中的最佳结果。

3.5 Frontend

我们的代码实现充分利用了SVO2.0作为SchurVINS的前端。SchurVINS的集成组件包括来自原始SVO2.0的特征对准和深度滤波模块。同时，稀疏图像对准模块被提出的EKF传播方案替换，以确保向特征对准模块提供准确的姿态。与基于帧间特征跟踪[10, 24, 29]相比，通过将局部地图中的共视地标投影并匹配到帧中来实现的特征对准策略，由于在短时间内丢失的地标可以再次被跟踪，因此实现了出色的长期地标跟踪性能。深度滤波用于执行地标位置初始化。一旦地标被充分初始化，它将被转移到提出的基于EKF的地标求解器中，以便与滑动窗口一起进行估计。

基于先进先出（FIFO）策略，局部地图仅保留最近的十个关键帧以支持地标跟踪。由于已经实现了高精度，因此传统的LBA不再必要，被放弃在提出的SchurVINS中。

${}^1$ DM-VIO和BASALT的$1\times$评估结果是作者手动转换的结果。

${}^2$ SVO2.0-wo表示禁用了LBA的SVO2.0。

表3. 不同知名VINS算法的CPU开销评估。在所有提到的算法中，都禁用了GBA、PGO和LC，除了SVO2.0，它启用了LBA模块。与SOTA VINS算法相比，我们的方法在效率方面有明显改善。

3.6 Keyframe Selection

在VINS系统中，关键帧选择策略至关重要。在SchurVINS中有三种选择关键帧的策略。如果候选帧与前一关键帧之间的平均视差达到阈值，或者跟踪到的地标数量低于某个特定阈值，则将相应帧定义为关键帧。一旦选择了关键帧，就会提取FAST角点[30]，通过深度滤波模块生成新的地标。此外，当候选帧与局部地图中保持的共视关键帧之间的方向和位置差距超出一定范围时，关键帧将被确定，通过这种方式，跟踪模块可以克服候选帧与局部地图之间的发散。

4 Experiments

SchurVINS算法的准确性和效率通过两个实验进行评估。并进行了额外的消融实验，以展示所提出的基于EKF的地标求解器的有效性。

系统配置：我们基于SVO2.0的开源代码库开发了SchurVINS，具体来说，是svo_pro_open。大部分系统参数无需修改。为了提高效率，丢弃或停用了边缘特征，循环闭合（LC），姿态图优化（PGO），局部束调整（LBA）和全局束调整（GBA）。对于下面的实验，我们将局部地图中关键帧的数量阈值配置为最多十个。这个局部地图主要维护共视的关键帧和地标以实现特征对齐。在SchurVINS的后端，有一个滑动窗口，由2个旧关键帧和2个最新的时间帧组成。关键帧策略与原始的SVO2.0。
${}^1$ 表示SchurVINS的高斯牛顿优化地标估计（GN-based），与SVO2.0中最初使用的相同。

${}^2$ MSCKF更新和SLAM更新的运行时间。

${}^3$ 它包含一些SVO2.0 LBA在异步线程中的运行时间。

${}^4$ 总时间还包括其他模块。

表4. SchurVINS主要部分的运行时间评估与SVO2.0和OpenVINS在EuRoC MH01上的比较（平均时间为ms）。请注意，SVO-NonBA和SchurVINS-GN之间optimizeStructure的不同开销主要归因于特征匹配数量的变化，这是定位精度的结果

4.1 精度

这些算法的整体准确性是使用两个知名数据集EuRoC和TUM-VI上的均方根误差（RMSE）进行评估的。SchurVINS在TUM-VI和EuRoC数据集上的相应实验轨迹和点云如图4所示。为了防止算法的波动导致不合理的评估结果，我们自己的评估方法是运行算法7轮，去掉最大和最小值，然后计算剩余结果的平均值作为评估结果。

在表1中，我们的方法在迄今为止在数据集上报告的基于滤波器的方法中获得了最低的平均RMSE，并且优于大多数基于优化的方法。此外，我们的方法在准确性方面与知名的基于优化的方法BASALT相当，并且比最近的竞争对手DMVIO的准确性稍低。表2中的重新评估实验完全符合预期。值得强调的是，尽管与两个基于优化的竞争对手相比准确性略有下降，但我们的方法在计算复杂度方面显然比它们都要低，具体细节在下一小节中。

4.2 效率

效率评估是在Intel i7-9700 (3.00GHZ) 台式机平台上进行的。所有以下算法都禁用了全局BA (GBA)、位姿图优化和闭环检测。此外，仅原始SVO2.0启用了LBA。效率实验分为两部分：分析处理器使用情况和额外开销，分别报告在表3和表4中。

如表3所示，与所有其他提到的VINS算法相比，SchurVINS的处理器使用情况几乎是最低的。特别是，SVO2.0-wo的CPU使用情况与SchurVINS相似，但它在准确性上明显不足，因为它几乎是纯视觉里程计（VO）。为了彻底调查导致SchurVINS效率优势的潜在原因，进行了详细分析SchurVINS的额外开销，包括与SVO2.0和广泛认可的基于滤波器的OpenVINS的比较，如表4所示。

在表4中，SchurVINS中的optimizeStructure模块几乎比SchurVINS-GN快3倍。因为我们的方法通过利用Schur补偿的中间结果获得了显著的计算节约。相反，SchurVINS-GN重建了用于估计地标的问题。与SVO2.0-wo相比，SchurVINS更快，原因是它从高计算量的SparseImageAlign转换为传播模块。相反，SVO2.0-wo的optimizeStructure明显比SchurVINS-GN快。导致算法运行时间显著增加的根本原因是LBA的高计算复杂性。考虑到OpenVINS，值得注意的是，默认配置和滑动窗口最大尺寸为4的配置都无法实现OpenVINS在效率上优于SchurVINS。从这个分析中突出的是，与SchurVINS中呈现的基于EKF的地标估计相比，OpenVINS中的SLAM点更新需要显着更多的计算资源。如图3所示，SchurVINS充分利用了问题的稀疏性，比混合MSCKF和基于优化的方法都要好。

${}^1$ SV-OFF表示禁用了EKF-based地标解算器的SchurVINS，仅使用深度滤波器初始化地标。

表5. 在EuRoC上的消融评估。

4.3 消融研究

上面的实验强力支持了SchurVINS。因此，有必要研究我们算法的不同组件的影响。基于SchurVINS，我们替换或丢弃了基于EKF的地标解算器来分析其有效性。

如表5所示，如果没有GN-based或EKF-based的地标解算器，SchurVINS无法充分限制全局漂移。此外，在一些挑战性场景中，在SchurVINS中同时缺少地标估计可能导致系统分歧。表5中SchurVINS和SchurVINS-GN的比较表明，提出的基于EKF的地标解算器和原始SVO2.0中的GN-based地标解算器都是有效和可靠的，以保证高精度。此外，在表4和表5中对它们的比较说明，虽然提出的基于EKF的地标解算器导致了轻微的精度下降，但它可以实现明显的低计算复杂度。对精度降低的直观解释是，我们的方法仅使用滑动窗口中的所有观测来进行地标估计。

5 结论与未来工作

本文开发了一种基于EKF的VINS算法，包括新颖的基于EKF的地标解算器，以实现高效和准确的6自由度估计。具体来说，利用由Hessian、Gradient和相应观测协方差组成的等效残差模型共同估计姿态和地标，以保证高精度的定位。为了实现高效率，等效残差模型通过Schur补分解为姿态残差模型和地标残差模型，分别进行EKF更新。受到周围环境元素的概率独立性的益处，产生的地标残差模型被分割为一组小的独立残差模型，用于每个地标的EKF更新，从而显著降低了计算复杂性。据我们所知，我们是首先利用Schur补分解残差模型加速EKF-based VINS算法。基于EuRoC和TUM-VI数据集的实验表明，我们的方法在精度和效率方面明显优于整体EKF-based方法[10,29]和大多数基于优化的方法。此外，我们的方法所需的计算资源几乎比SOTA优化方法[32,33]少了50%以上，并且精度可与之媲美。与此同时，消融研究清楚地表明，我们提出的基于EKF的地标解算器不仅效率显著，而且能够确保高精度。

在未来的工作中，我们将专注于SchurVINS中的局部地图细化，以探索更高的精度。