# 一种高效的全视觉-惯性状态初始化闭式解
Samuel Cerezo $^{ID}$、Seong Hun Lee $^{ID}$ 和 Javier Civera $^{ID}$，IEEE会员
摘要—在本文中，我们提出了一种闭式初始化方法，无需非线性优化即可恢复完整的视觉-惯性状态。与以往依赖迭代求解器的方法不同，我们的公式能够给出解析的、易于实现且数值稳定的解决方案，从而实现可靠的启动。该方法基于小旋转和恒速近似，既保持了公式的简洁性，又保留了运动与惯性测量之间的本质关联。我们进一步提出了一种以可观测性为导向的两阶段初始化方案，在精度与初始化延迟之间取得平衡。在EuRoC数据集上的大量实验验证了我们的假设：与基于优化的方法相比，我们的方法将初始化误差降低了10%–20%，同时初始化窗口长度缩短了4倍，计算成本降低了5倍。
关键词—视觉-惯性SLAM、状态初始化、传感器融合。
## I. 引言
视觉-惯性SLAM已成为机器人技术和移动设备的核心技术，通过融合相机与惯性测量数据实现实时定位与建图[^1]、[^2]、[^3]。虽然相机内参只需校准一次且随时间相对稳定，但惯性传感器每次运行时都需要重新初始化。这一初始化步骤需要估计初始速度、重力方向以及陀螺仪和加速度计偏差等关键量，这些量可作为后续状态估计的初始值。因此，准确的初始化至关重要：不恰当的初始估计可能导致优化或滤波算法收敛到局部最小值，进而引发尺度漂移、跟踪性能下降，甚至导致系统完全失效。相反，恰当的初始化能提升系统在复杂环境下的鲁棒性，并恢复单目视觉无法提供的度量尺度。
SLAM文献中的一个核心观点是：姿态起着至关重要的作用——一旦旋转参数能够被可靠估计，其余状态的恢复也会变得更加稳健。Carlone和Censi [^4]指出，精确的姿态估计能够避免位姿图优化过程中出现虚假极小值，而Carlone等人也持相同观点。
TABLEI
COMPARISON OF EXISTING INITIALIZATION METHODS. SUMMARY OF WHETHER EACH METHOD PROVIDES A CLOSED-FORM SOLUTION AND ESTIMATES GYROSCOPE AND ACCELEROMETER BIASES.
| Method | Closed-form | Gyro-bias | Accel-bias |
| Martinelli [^7] | ✓ | ✗ | ✗ |
| Kaiser et al. [^8] | ✗ | ✓ | ✓ |
| Domínguez et al. [^9] | ✗ | ✓ | ✓ |
| Campos et al. [^10], [^11] | ✗ | ✓ | ✓ |
| Concha et al. [^13] | ✗ | ✓ | ✓ |
| Evangelidis and Micušík [^16] | ✓ | ✗ | ✗ |
| Wang et al. [^15] | ✗ | ✓ | ✓ |
| Ours | ✓ | ✓ | ✓ |
Carlone等人[^5]的研究表明，良好的旋转初始化能够显著提升大规模SLAM问题的收敛性与精度。这些研究结果为我们所采用的方法提供了依据：该方法依靠简单却精确的解耦姿态近似来估计其余状态。
现有的视觉-惯性初始化策略主要分为两类。Martinelli[^6]、[^7]提出了闭式解，但这些方法假定对状态已有部分认知，尤其是传感器偏差信息。后续研究通过迭代非线性优化解决了这一局限[^8]、[^9]、[^10]、[^11]，虽能实现精确的全状态初始化，但需要仔细设置初始值且计算成本较高。更近期的研究试图通过解耦旋转与平移[^12]、[^13]、[^14]，或将闭式解与滤波方案结合[^15]来降低复杂度。然而，这些方法均未能提供针对完整视觉-惯性状态的完全解析解。
在这封信中，我们提出了首个针对视觉-惯性系统所有状态的闭式初始化方法，包括陀螺仪和加速度计偏差。我们的公式基于小旋转和恒定速度近似，这一假设在常见的传感器硬件和运动模式下均成立。我们还提出了一种两阶段准则，用于评估视觉-惯性状态的可观测性，从而能够在各种条件下实现可靠的初始化。大量实验表明，与现有基准方法相比，我们的方法不仅精度更高，还能显著减少计算成本和初始化延迟。
### II. 相关工作
Martinelli [^6]、[^7] 最早提出了闭式初始化方法，他们在已知陀螺仪和加速度计偏差的假设下推导出了唯一的最小解。后续的研究转向非线性优化，以联合估计这些偏差以及其余状态。
---
这些状态 [^8]、[^9]，而后来Campos等人 [^10]、[^11] 提出的改进方法虽然取得了稳健的结果，但仍依赖需要仔细初始化的迭代过程。
最近，人们提出了多种旨在提升效率和鲁棒性的替代方案。Evangelidis和Micušík [^16] 重新审视了这一问题，为视觉-惯性同时定位与地图构建问题设计了新的闭式求解器，该方案虽简洁却更适用于离线重建。此外，还有人提出了关于相对 [^17] 和多视图 [^12] 相机运动的解耦方法 [^13]、[^14]，但它们仍需要非线性优化，且在某些情况下无法恢复加速度计偏差。Wang等人 [^15] 提出的混合策略可以通过误差状态卡尔曼滤波器估计两种偏差，但会带来额外的算法复杂度。
与此同时，近似方法在相关问题中也发挥了重要作用：三角测量中的小角度近似 [^18]、[^19]，以及仅依靠视觉的意外运动重建 [^20] 和高效相对位姿求解器 [^21] 中的小旋转近似。但据我们所知，小旋转近似从未被应用于视觉-惯性状态初始化。
总而言之，现有方法要么假设偏差已知，要么依赖迭代优化，要么采用会增加复杂度的混合方案（见表I）。相比之下，我们仅通过假设小旋转这一对典型硬件和大多数应用场景而言合理的条件，就提出了全状态的闭式解决方案。因此，我们的方法结合了基于优化的方法的精度，以及解析求解器的简洁性和效率。
### III. 符号说明与预备知识
向量用粗体小写字母（x）表示，单位向量则用带帽符号表示（$\hat{\mathbf{x}}$）。粗体大写字母（A）代表矩阵，而小写字母（a）则表示标量。旋转矩阵$\mathbf{R}_{ab} \in \mathrm{SO}(3)$表示从坐标系b到坐标系a的变换，当指代世界坐标系w时，会省略第一个下标，即$\mathbf{R}_{a} = \mathbf{R}_{wa}$。在轴角形式下，$\mathbf{r} = \theta \hat{\mathbf{r}} \in \mathbb{R}^{3}$中，单位旋转轴为$\hat{\mathbf{r}} = \frac{\mathbf{r}}{\|\mathbf{r}\|} \in \mathbb{S}^{2}$，旋转角度为$\theta = \|\mathbf{r}\| \in \mathbb{R}$。$\mathbf{r}$的斜对称矩阵记为$\mathbf{r}^\wedge$。在时间区间$t_i$到$t_j$之间的IMU积分遵循标准公式[^22]。
$$ \begin{array}{r l}&{\mathtt{R}_{j}=\mathtt{R}_{i}\prod_{k=i}^{j-1}\mathrm{E x p}\left(\left(\tilde{\pmb{\omega}}_{k}-\mathbf{b}_{k}^{g}-\pmb{\eta}_{k}^{g d}\right)\Delta t\right),}\end{array} $$
$$ \mathbf{v}_{j}=\mathbf{v}_{i}+\mathbf{g}\Delta t_{ij}+\sum_{k=i}^{j-1}\mathrm{R}_{k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{k}^{a}-\boldsymbol{\eta}_{k}^{a d}\right)\Delta t $$
$$ \mathbf{t}_{j}=\mathbf{t}_{i}+\sum_{k=i}^{j-1}\left[\mathbf{v}_{k}\Delta t+\frac{1}{2}\mathbf{g}\Delta t^{2}+\frac{1}{2}\mathbb{R}_{k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{k}^{a}-\boldsymbol{\eta}_{k}^{a d}\right)\Delta t^{2}\right] $$
其中$\mathbf{v}_i$和$\mathbf{v}_j$分别表示时间$t_i$和$t_j$时的线速度，$\mathbf{g}$为重力矢量，$\Delta t$则是连续测量之间的采样间隔。$\tilde{\omega}_k$和$\tilde{a}_k$分别是时刻k的陀螺仪和加速度计读数，对应的偏差分别为$\mathbf{b}_k^g$和$\mathbf{b}_k^a$。我们假设在初始化期间陀螺仪和加速度计的偏差基本保持不变，即$\mathbf{b}_k^g \approx \mathbf{b}^g$和$\mathbf{b}_k^a \approx \mathbf{b}^a$。我们用$L = j - i - 1$表示预积分的IMU测量次数，因此$t_j - t_i = L\Delta t$。
### 四、旋转与陀螺仪偏差初始化
对于较小的$(\theta \to 0)$旋转向量$\mathbf{r}$，罗德里格斯公式将其与旋转矩阵表示联系起来：
$$ \mathbf{R}=\mathbf{I}_{3}+\frac{\sin\theta}{\theta}\mathbf{r}^{\wedge}+\frac{1-\cos\theta}{\theta^{2}}\mathbf{r}^{\wedge2}\underset{\theta\rightarrow0}{\approx}\mathbf{I}_{3}+\mathbf{r}^{\wedge}, $$
其中$\mathbf{I}_3$是$3 \times 3$单位矩阵。值得注意的是，在此近似下，旋转矩阵仅在二阶项上可交换。近似处理$\mathbf{R}_{ab} \approx \mathbf{I} + \mathbf{r}_{ab}^\wedge$和$\mathbf{R}_{cd} \approx \mathbf{I} + \mathbf{r}_{cd}^\wedge$后，乘积$\mathbf{R}_{ab}\mathbf{R}_{cd}$和$\mathbf{R}_{cd}\mathbf{R}_{ab}$可表示为：
$$ \mathbf{R}_{a b}\mathbf{R}_{c d}\approx\mathbf{I}+\mathbf{r}_{a b}{^\wedge}+\mathbf{r}_{c d}{^\wedge}+\underbrace{\mathbf{r}_{a b}{^\wedge}\mathbf{r}_{c d}{^\wedge}} $$
$$ \mathbf{R}_{c d}\mathbf{R}_{a b}\approx\mathbf{I}+\mathbf{r}_{c d}{^\wedge}+\mathbf{r}_{a b}{^\wedge}+\underbrace{\mathbf{r}_{e d}{^\wedge}\mathbf{r}_{a b}}{^\wedge} $$
两者的差异仅在于忽略了斜对称矩阵$\mathbf{r}_{ab}^{\wedge} \mathbf{r}_{cd}^{\wedge}$与$\mathbf{r}_{cd}^{\wedge} \mathbf{r}_{ab}^{\wedge}$的二阶交叉项，而这些项通常不满足交换律。$^{1}$不过在小旋转假设下，这些二阶项相对于一阶项而言可以忽略不计，因此这两个矩阵乘积近似相等。
罗德里格斯公式还给出了指数映射$\exp: \mathrm{so}(3) \to \mathrm{SO}(3)$。对于小旋转向量$\mathbf{r}_{ab}$和$\mathbf{r}_{cd}$，它们之和的指数近似等于指数乘积：
$$ \begin{array}{r l}&{\mathrm{E x p}(\mathbf{r}_{a b}+\mathbf{r}_{c d})\underset{\theta\to0}{\approx}\mathbf{I}_{3}+\left(\mathbf{r}_{a b}+\mathbf{r}_{c d}\right)^{\wedge}=\mathbf{I}_{3}+\mathbf{r}_{a b}{^{\wedge}}+\mathbf{r}_{c d}{^{\wedge}}}\\ &{\quad=\left(\mathbf{I}_{3}+\mathbf{r}_{a b}{^{\wedge}}\right)\left(\mathbf{I}_{3}+\mathbf{r}_{c d}{^{\wedge}}\right)=\mathrm{E x p}(\mathbf{r}_{a b})\mathrm{E x p}(\mathbf{r}_{c d})}\end{array} $$
据此，(1)中的指数映射可分解为如下乘积形式：
$$ \prod_{k=i}^{j-1}\mathrm{Exp}((\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}^{g})\Delta t)\underset{\theta\to0}{\approx}\prod_{k=i}^{j-1}\mathrm{Exp}(\tilde{\boldsymbol{\omega}}_{k}\Delta t)\mathrm{Exp}(-\mathbf{b}^{g}\Delta t) $$
先交换右侧各乘积的顺序，再应用(7)，即可得到下述表达式：
$$ \mathtt{R}_{i j}=\mathtt{R}_{i}^{\top}\mathtt{R}_{j}\underset{\theta\to0}{\approx}\prod_{k=i}^{j-1}\left(\mathrm{E x p}\left(\tilde{\omega}_{k}\Delta t\right)\mathrm{E x p}\left(-\mathbf{b}^{g}\Delta t\right)\right) $$
$$ \overset{\approx}{\underset{\theta\to0}{\sim}}\prod_{k=i}^{j-1}\left(\operatorname{E x p}\left(\tilde{\omega}_{k}\Delta t\right)\right)\prod_{k=i}^{j-1}\left(\operatorname{E x p}\left(-\mathbf{b}^{g}\Delta t\right)\right) $$
$$ =\prod_{k=i}^{j-1}\left(\operatorname{E x p}\left(\tilde{\omega}_{k}\Delta t\right)\right)\operatorname{E x p}\left(-L\mathbf{b}^{g}\Delta t\right) $$
由此可解析求解出$b^{g}$。这就得到了我们的首个闭式解，称为交换近似解。
$$ \begin{aligned}&\frac{\mathbf{b}^{g} using~the~commutative~approximation}{}\\ &\frac{}{\mathbf{b}^{g}\approx-\frac{1}{L\Delta t}\mathrm{Log}\left(\left(\prod_{k=i}^{j-1}\mathrm{Exp}\left(\tilde{\omega}_{k}\Delta t\right)\right)^{\top}\mathrm{R}_{ij}\right)}\\ \end{aligned} $$
通过假设每次IMU积分内的旋转角度保持恒定，我们可以推导出更高效的$b^{g}$闭式解。首先，我们将(9)式的左侧表示为：
---
L次相同旋转的复合：
$$ \mathtt{R}_{i j}=\prod_{k=i}^{j-1}\mathrm{E x p}\bigg(\frac{\mathrm{L o g}(\mathtt{R}_{i j})}{L}\bigg)\overset{\approx}\theta\to0\prod_{k=i}^{j-1}(\mathrm{E x p}(\tilde{\omega}_{k}\Delta t)\mathrm{E x p}(-\mathbf{b}^{g}\Delta t)) $$
现在，假设在两帧图像之间的时间间隔内$\tilde{\omega}_k$基本保持恒定。将(13)式中的$\tilde{\omega}_k$用其平均值$\overline{\omega}_k$近似代替，再将等式两边除以L次相同旋转，即可得到：
$$ \mathrm{E x p}\bigg(\frac{\mathrm{L o g}(\mathtt{R}_{i j})}{L}\bigg)\approx\mathrm{E x p}(\overline{{\omega}}_{k}\Delta t)\mathrm{E x p}(-\mathbf{b}^{g}\Delta t), $$
由此我们也能解析求解出$b^g$。我们可以通过积分所有IMU测量值来计算$\overline{\omega}_k$，或者简单地对它们取算术平均值，这样就能得到两种闭式解，我们分别将其称为平均近似解和算术平均近似解。
$$ \begin{aligned}&\begin{cases}\\ &\mathbf{b}^{g}using~the~average~approximation\\ &\end{cases}\\ &\quad\mathbf{b}^{g}\approx-\frac{1}{\Delta t}\mathrm{Log}\biggl(\mathrm{Exp}(\overline{\omega}_{k}\Delta t)^{\top}\mathrm{Exp}\biggl(\frac{\mathrm{Log}(\mathbb{R}_{ij})}{L}\biggr)\biggr)\\ &\quad with\quad\overline{\omega}_{k}\approx\frac{1}{L\Delta t}\mathrm{Log}\biggl(\prod_{k=i}^{j-1}\mathrm{Exp}(\tilde{\omega}_{k}\Delta t)\biggr)\quad(15)\\ \end{aligned} $$
$$ \begin{aligned}&\text{b}^{g}\text{using the arithmetic average approximation}\\ &\begin{aligned}\\ &\mathbf{b}^{g}\approx-\frac{1}{\Delta t}\mathrm{Log}\left(\mathrm{Exp}(\overline{\omega}_{k}\Delta t)^{\top}\mathrm{Exp}\left(\frac{\mathrm{Log}(\mathrm{R}_{ij})}{L}\right)\right)\\&\text{with}\quad\overline{\omega}_{k}=\frac{1}{L}\sum_{k=i}^{j-1}\tilde{\omega}_{k}\tag{16}\\ &\end{aligned}\\ \end{aligned} $$
$R_{ij}$可以通过任意两帧之间的特征对应关系来估计。不过在实践中，仅利用最初的两帧来初始化$R_{ij}$和$\mathbf{b}^{g}$既可行又方便。首先，估计误差会随时间推移而增大：惯性漂移不断累积，且随着涉及的IMU读数增多，我们的近似结果也会变得不够精确。其次，与平移估计不同，相对旋转估计未必能从较大的帧间运动中获得好处；事实上，大幅度的运动往往会导致特征外观出现透视畸变，从而减少可可靠追踪的点数并增加匹配噪声[^23]。虽然$R_{ij}$是从视觉对应关系中获取的，但我们的方法并不依赖完整的基于SFM的流程。旋转是利用经过严格验证的最优双视图求解器独立于结构进行估计的，因此避免了尺度模糊和与深度相关的不稳定性。这种仅考虑旋转的建模方式，比那些估计完整六自由度位姿和深度的多视图SFM方法，能实现更稳健的陀螺仪偏差估计。
### 五、初始速度、平移、重力和加速度计偏差
### A. 基于线性点约束的闭式初始化
一旦初始化了$\mathbf{R}_{ij}$和$\mathbf{b}^g$，惯性测量值与图像轨迹之间的关系就可以用线性形式表达。设集合$\mathcal{Z}_i = \{\mathbf{z}_i^1, \ldots, \mathbf{z}_i^n, \ldots, \mathbf{z}_i^N\}$和$\mathcal{Z}_j = \{\mathbf{z}_j^1, \ldots, \mathbf{z}_j^n, \ldots, \mathbf{z}_j^N\}$分别表示在时刻$t_i$拍摄的图像中提取的N个视觉特征的坐标，这些特征一直被追踪到后续时刻$t_j$拍摄的图像中，且这两幅图像不一定连续。假设采用针孔相机模型，两幅图像$\mathbf{z}_c^n \in \Omega \subset \mathbb{R}^2$中的每个特征$c \in \{i, j\}$都可以按如下方式反投影到局部相机坐标系中的三维点$\mathbf{p}_c^n \in \mathbb{R}^3$。
$$ \mathbf{p}_{c}^{n}=\lambda_{c}^{n}\mathbf{K}^{-1}\left[\begin{matrix}{\mathbf{z_{c}^{n\top}}}&{1}\\ \end{matrix}\right]^{\top}=\lambda_{c}^{n}\boldsymbol{\mu}_{c}^{n}, $$
其中，$K \in \mathbb{U}\mathbb{T}^+(3)$（其中$K[3,3] = 1$）为相机校准矩阵，$\Omega$为图像域，$\mu_j^n \in \mathcal{S}^2$为归一化的特征方位角，$\lambda_j^n \in \mathbb{R}$则表示沿该方位角的距离。参照[^7]的方法，针对在时刻$\mathbf{z}_c^n$和$t_i$拍摄的每对图像中的每个特征$t_j$，都能推导出一条线性约束。
$$ -\frac{(L\Delta t)^{2}}{2}\mathbf{g}-(L\Delta t)\mathbf{v}+\boldsymbol{\Gamma}_{j}\mathbf{b}_{a}+\lambda_{i}^{n}\boldsymbol{\mu}_{i}^{n}-\lambda_{j}^{n}\boldsymbol{\mu}_{j}^{n}=\mathbf{s}_{j}, $$
其中向量$\mathbf{s}_{j}$和矩阵$\boldsymbol{\Gamma}_{j}$是通过在区间$[t_{i}, t_{j}]$内积分IMU测量值得到的，具体形式如下：
$$ \mathbf{s}_{j}=\int_{t_{i}}^{t_{j}}\left(t_{j}-\tau\right)\mathbb{R}_{1\tau}\tilde{\mathbf{a}}_{\tau}d\tau,\mathbf{\Gamma}\mathbf{f}_{j}=\int_{t_{i}}^{t_{j}}\left(t_{j}-\tau\right)\mathbb{R}_{i\tau}d\tau. $$
旋转量$\mathbf{R}_{i\tau} \in SO(3)$来自从时刻$t_i$到时刻$\tau$的IMU预积分结果，计算时采用公式(1)。注意，$\mathbf{b}^g$已经通过最初的两帧完成了初始化。向量$\tilde{\mathbf{a}}_r$包含了加速度计读数。将第i帧到第j帧中N个被追踪特征的约束条件堆叠起来，就能得到超定线性系统$\boldsymbol{\Xi}\mathbf{x} = \mathbf{s}$，进而形成如下的最小二乘问题。
$$ \begin{array}{l}{\left|\mathbf{g},\mathbf{v},\mathbf{b}^{\mathrm{a}}\mathbf{\Sigma}initialization\right.}\\ {\left.\vphantom{\frac{x}{x}}\right|=\operatorname{a r g}\operatorname*{m i n}_{\mathbf{x}}\left(\mathbf{\Xi x-s}\right)^{2}\rightarrow\mathbf{\hat{x}}=\mathbf{v}_{\mathrm{m i n}}\left(\mathbf{\Xi}^{\top}\mathbf{\Xi}\right)}\\ \end{array} $$
我们通过奇异值分解（SVD）来求解该问题，其最优解为与最小特征值对应的特征向量$\hat{\mathbf{x}}$，即$\mathbf{v}_{\min}(\boldsymbol{\Xi}^{\top}\boldsymbol{\Xi})$。矩阵$\boldsymbol{\Xi}$的每一行对应式(18)的一个实例，由此可得到$3(M-1)N$个方程和$9+NM$个未知数，其中M表示所考虑的图像数量。$\boldsymbol{\Xi}$指的是下一页底部所示的无编号方程中的系数矩阵，其中包含$T_j = -\frac{\Delta t^2}{2}\mathbf{I}_3$。待初始化的状态定义如下：
$$ \mathbf{x}=\left[\mathbf{g}^{\top},\mathbf{v}^{\top},\mathbf{b}^{a\top},\lambda_{1}^{1},\ldots,\lambda_{1}^{N},\ldots\lambda_{j}^{1},\ldots\lambda_{j}^{N}\right]^{\top}, $$
s中包含了由式(19)得到的测量结果。
$$ \mathbf{s}=[\mathbf{s}_{2}^{\top},\ldots,\mathbf{s}_{2}^{\top},\mathbf{s}_{3}^{\top},\ldots,\mathbf{s}_{3}^{\top},\mathbf{s}_{j}^{\top}\ldots,\mathbf{s}_{j}^{\top}]^{\top}. $$
由此便得到了一种初始化方法：仅利用最初的两帧图像就能初始化$R_{ij}$和$b^g$，而当其余状态变得可观测时再对其进行初始化，伪代码中的算法1对此进行了说明。下一小节将对可观测性准则展开详细分析。尽管特征深度会出现在线性公式中，但它们被视作干扰变量，不会进行递归估计。与OpenVINS等基于滤波的方法不同[^24]，深度会被隐式消除，且在初始化之后不再被使用。我们的可观测性测试旨在评估简化系统的数值条件，而非强制要求深度收敛。
---
```pseudo
// Algorithm 1: Visual-Inertial State Initialization.
Require: Visual-inertial data; observability thresholds
```
$\Delta z_{\mathrm{th}}$, $\Delta \rho_{\mathrm{th}}$
确保：视觉-惯性状态$\mathbf{R}_{ij}$、$\mathbf{b}^{g}$、$\mathbf{t}_{j}$、$\mathbf{v}_{j}$、$\mathbf{b}_{a}$、$\mathbf{g}$。
1: $\mathcal{Z}_{i}$ ← 获取第一帧图像并提取特征()
2: $\mathcal{Z}_{j}$ ← 获取下一帧图像并跟踪特征($\mathcal{Z}_{i}$)
3: $\{\tilde{\omega}_{k}\}$、$\{\tilde{a}_{k}\}$ ← 获取IMU读数()
4: $\mathbf{R}_{ij}$ ← 根据($\mathcal{Z}_{i}$、$\mathcal{Z}_{j}$)计算旋转量()
5: $\mathbf{b}^{g}$ ← 根据($\mathbf{R}_{ij}$)计算偏差()
6: full_obs ← false
7: while full_obs = false do
8: $t_{obs}$ ← translation_obs_test($\mathcal{Z}_{i}$、$\mathcal{Z}_{j}$、$\mathbf{R}_{ij}$、$\Delta z_{th}$)
9: if $t_{obs}$ = true then
10: $s_{j}$ ← integrate_IMU($\{\tilde{\omega}_{k}\}$、$\{\tilde{a}_{k}\}$、$\mathbf{b}^{g}$)
11: full_obs ← full_obs_test($\mathcal{Z}_{i}$、$\mathcal{Z}_{j}$、$\mathbf{s}_{j}$、$\Delta \rho_{th}$)
12: if $full_{obs}$ = true then
13: $t_{j}$、$v_{j}$、$b_{a}$、g ← get_rest_of_vi_states($\mathbf{s}_{j}$)
14: full_obs ← true
15: end if
16: end if
17: $\mathcal{Z}_{j}$ ← 获取下一帧图像并跟踪特征($\mathcal{Z}_{i}$)
18: $\{\tilde{\omega}_{k}\}$、$\{\tilde{a}_{k}\}$ ← 获取IMU读数()
19: $\mathbf{R}_{ij}$ ← 根据($\mathcal{Z}_{i}$、$\mathcal{Z}_{j}$)计算旋转量()
20: end while
### B. 两阶段可观测性测试
虽然从任意两帧图像中都能观测到相机旋转和陀螺仪偏差，实际中我们也会利用序列中的前两帧来初始化它们，但其余的视觉-惯性状态却无法被观测到。具体而言，利用式(20)定义的线性系统，只有当$\Xi$的秩为满秩时，完整状态才可被观测，进而我们才能对其初始化。不过，在每一帧处都评估这一准则可能成本较高，因为随着帧数增加，矩阵$\Xi$会迅速增大。基于这一原因，我们将分两个阶段评估可观测性，具体描述如下。
翻译可观测性测试（第一阶段）：该阶段用于评估图像平面中在 $t_i$ 处的第一帧与随后的某一帧 $t_j$ 之间的平均视差，并沿着图像序列逐步推进，直至满足判定条件。具体而言，我们首先对在 $\mathbf{z}_i^n$ 处提取的每个特征点 $t_i$ 进行反向投影，并在假设两帧之间仅存在旋转运动的前提下，将其投影到 $t_j$ 处。
$$ \begin{array}{r}{\hat{\mathbf{z}}_{j}^{n}\;=\;h\left(\mathtt{K R}_{i j}^{\top}\mathtt{K}^{-1}\left[\begin{matrix}{\mathbf{z_{i}^{n}}^{\top}}&{1}\\ \end{matrix}\right]^{\top}\right),}\end{array} $$
其中 $h(\cdot)$ 负责将图像点从齐次坐标转换为欧几里得坐标。$t_i$ 与 $t_j$ 之间的视差会体现在仅考虑旋转运动所预测的轨迹 $\hat{z}_j^n$ 与实际轨迹 $\mathbf{z}_j^n$ 之间的差异上。我们通过求平均值来汇总所有特征的视差：
$$ \bar{\Delta z}_{j}\;=\;\frac{1}{N}\sum_{n=1}^{N}\big\|\mathbf{z}_{j}^{n}-\hat{\mathbf{z}}_{j}^{n}\big\|_{2}, $$
需满足的条件表述如下：
$$ \bar{\Delta}z_{j}>\bar{\Delta}z_{\mathrm{t h}},\quad(\mathrm{T r a n s l a t i o n~O b s e r v a b i l i t y~T e s t}) $$
若该条件未得到满足，我们就切换到下一帧并重复测试，直到满足阈值要求为止。
完全可观测性测试（第二阶段）：一旦运动提供了足够的平移信息，即满足公式(23)中的条件时，便会触发该阶段。依据 [^25] 的描述，我们通过海森矩阵 H 来评估数值上的可观测性，具体过程如下：
$$ \mathbf{H}=\boldsymbol{\Xi}^{\top}\boldsymbol{\Sigma}^{-1}\boldsymbol{\Xi}, $$
此处 $\Sigma$ 表示根据 [^26] 得到的残差权重。直接使用完整的 $\mathbf{H}$ 会产生误导，因为在视差较小的情况下，干扰深度 $\lambda_j^n$ 往往条件不佳，还容易压低某些奇异值。因此，我们通过舒尔补来边缘化深度，从而得到关于待估计的九个状态（重力、速度、加速度计偏差）的 $\mathbf{H}^*$，并基于这个简化后的矩阵来评估可观测性。
接着我们计算 $\mathbf{H}^* \in \mathbb{S}_+$ 的奇异值分解，并定义奇异值比 $\rho_j = \sigma_{\max} / \sigma_{\min}$，其中 $\sigma_{\min}$ 和 $\sigma_{\max}$ 分别表示最小的非零奇异值和最大的奇异值。为了监测随着数据窗口扩大而出现的稳定性变化，我们会对相对变化量进行监控：
$$ \Delta\rho_{j}\;=\;|\big(\rho_{j}-\rho_{j-1}\big)/\rho_{j-1}|\;, $$
较大的数值表明可观测性仍在发生变化，而持续较小的值则意味着海森矩阵已经稳定，系统状态具备可观测性。一旦
$$ \Delta\rho_{j}<\Delta\rho_{\mathrm{t h}},\quad(\mathrm{F u l l O b s e r v a b i l i t y T e s t}) $$
其中 $\Delta\rho_{th}$ 为固定阈值，此时我们便认为系统具备可观测性，进而求解公式(20)。图1展示了完全可观测性测试相对于翻译可观测性测试的相对额外开销。随着窗口尺寸的增大，完全可观测性测试的开销稳步上升：对于较小的窗口而言，翻译可观测性测试的开销约为 $\times$3–4，而当窗口达到约十帧时，其开销便会超过 $\times$15。这些结果证明了将视差门限作为主要触发机制、把海森矩阵测试留作最终验证手段的合理性。
$$ \Xi=\left[\begin{matrix}{T_{2}}&{-t_{2}\mathbf{I}_{3}}&{\Gamma_{2}}&{\mu_{1}^{1}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{-\mu_{2}^{1}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}\\ {T_{2}}&{-t_{2}\mathbf{I}_{3}}&{\Gamma_{2}}&{\mathbf{0}_{3}}&{\mu_{1}^{2}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{-\mu_{2}^{2}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}\\ {\vdots}&{\vdots}&{\vdots}&{\vdots}&{\vdots}&{\vdots}&{\vdots}&{\vdots}&{\vdots}&{\vdots}&{\vdots}&{\vdots}\\ {T_{2}}&{-t_{2}\mathbf{I}_{3}}&{\Gamma_{2}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{\mu_{1}^{N}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{-\mu_{2}^{N}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}\\ {\vdots}&{\vdots}&{\vdots}&{\vdots}&{\vdots}&{\vdots}&{\vdots}&{\vdots}&{\vdots}&{\vdots}&{\vdots}&{\vdots}\\ {T_{j}}&{-t_{j}\mathbf{I}_{3}}&{\Gamma_{j}}&{\mu_{1}^{1}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{-\mu_{j}^{1}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}\\ {T_{j}}&{-t_{j}\mathbf{I}_{3}}&{\Gamma_{j}}&{\mathbf{0}_{3}}&{\mu_{1}^{2}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{-\mu_{j}^{2}}&{\mathbf{0}_{3}}\\ {T_{j}}&{-t_{j}\mathbf{I}_{3}}&{\Gamma_{j}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{\mu_{1}^{N}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{\mathbf{0}_{3}}&{-\mu_{j}^{N}}\\ \end{matrix}\right], $$
---
Fig. 1. Relative computational costs of our observability tests. Note the significantly higher cost of the full observability test, which motivates our proposal of a first stage assessing translational observability.
### 六、实验部分
### A. 实验设置
All experiments were conducted on a desktop PC equipped with an Intel Core i7-11700 K (3.6 GHz, 64 GB RAM). Our method is implemented in C++, and nonlinear least-squares optimization is performed using Ceres [^27]. Unless otherwise specified, all methods are executed on a single thread to ensure fair timing comparisons, and we report wall-clock times. Performance is evaluated in terms of the root mean squared error (RMSE), expressed in the corresponding physical units. In line with previous research, we use the publicly available EuRoC MAV benchmark [^28], which provides a diverse set of motion profiles commonly used to benchmark visual–inertial initialization methods, and is therefore adopted as our primary real-world evaluation dataset. EuRoC provides synchronized $752 \times 480$ stereo at 20 Hz, inertial data at 200 Hz and accurate ground truth. The dataset spans a range of motion profiles (easy/medium/difficult) and exhibits variations in texture, illumination, and dynamics, making it a suitable testbed for evaluating initialization accuracy, robustness, and runtime under realistic conditions. Following previous works [^11], [^14], we perform exhaustive initialization every 0.5 s (i.e., every 10 frames) across all sequences, yielding a total of 2248 independent initialization tests.
### B. 闭式陀螺仪偏差估计
Synthetic data: We conduct synthetic experiments in which the ground-truth relative rotations $R_{ij}$ from EuRoC are progressively corrupted with zero-mean Gaussian noise, allowing us to isolate the effect of rotational uncertainty on the estimation accuracy of $b^g$. We report de-aggregated per-sequence metrics to assess generalization over different motion patterns. We compare the three closed-form approximation methods introduced in Section IV with an iterative nonlinear least-squares optimization of $b^g$ based on (1). All methods use the same measurements and relative rotations (estimated by C2P [^29]) within identical time windows selected by our observability-driven triggering strategy. As shown in Table II, in an ideal noise-free setup, nonlinear optimization yields the lowest errors, while our closed-form solutions perform only slightly worse. However, as the noise level increases, the errors of our closed-form methods and the Gyroscope BIAS ERRORS [rad/s] in EuRoC, for DIFFERENT NOISE SETUPS FOR THE RELATIVE ROTATION $R_{ij}$. (A) NO NOISE, (B) STANDARD DEVIATION OF 0.006 deg, (C) STANDARD DEVIATION OF 0.06 deg, AND (D) NOISE FROM IMAGE MATCHES. WE COMPARE FOUR METHODS: NONLINEAR OPTIMIZATION (NONLIN. OPT.). COMMUTATIVE APPROXIMATION (COMUTATIVE.), AVERAGE APPROXIMATION (AVG.) AND ARITHMETIC AVERAGE APPROXIMATION (ARITH. AVG.). BEST PER ROW IN BOLD. EXCEPT FOR THE UNREALISTIC NOISELESS CASE, ALL METHODS BRING SIMILAR PERFORMANCE.
| Seq. | Nonlin. Opt. | Closed-form solutions (ours) |
| Commutative | Avg. | Arithm. Avg. |
| (a) Noise-free relative rotation |
| MH01 | 0.000002 | 0.000299 | 0.000042 | 0.000315 |
| MH02 | 0.000003 | 0.000303 | 0.000043 | 0.000329 |
| MH03 | 0.000004 | 0.000444 | 0.000055 | 0.000486 |
| MH04 | 0.000003 | 0.000327 | 0.000045 | 0.000338 |
| MH05 | 0.000003 | 0.000275 | 0.000042 | 0.000283 |
| V101 | 0.000003 | 0.000446 | 0.000052 | 0.000453 |
| V102 | 0.000004 | 0.000947 | 0.000104 | 0.000965 |
| V103 | 0.000007 | 0.001099 | 0.000123 | 0.001157 |
| V201 | 0.000004 | 0.000439 | 0.000059 | 0.000463 |
| V202 | 0.000004 | 0.000985 | 0.000109 | 0.001049 |
| V203 | 0.000008 | 0.001384 | 0.000166 | 0.001552 |
| Mean | 0.000004 | 0.000631 | 0.000076 | 0.000672 |
(b) Relative rotation noise of 0.006 deg standard deviation
| MH01 | 0.004148 | 0.004166 | 0.004149 | 0.004182 |
| MH02 | 0.004144 | 0.004144 | 0.004144 | 0.004185 |
| MH03 | 0.004148 | 0.004169 | 0.004148 | 0.004189 |
| MH04 | 0.004148 | 0.004147 | 0.004145 | 0.004145 |
| MH05 | 0.004148 | 0.004161 | 0.004147 | 0.004170 |
| V101 | 0.004148 | 0.004181 | 0.004147 | 0.004183 |
| V102 | 0.004147 | 0.004350 | 0.004158 | 0.004353 |
| V103 | 0.004147 | 0.004422 | 0.004163 | 0.004430 |
| V201 | 0.004148 | 0.004181 | 0.004148 | 0.004180 |
| V202 | 0.004149 | 0.004233 | 0.004144 | 0.004208 |
| V203 | 0.003711 | 0.004116 | 0.003729 | 0.004132 |
| Mean | 0.004108 | 0.004206 | 0.004111 | 0.004214 |
| (c) Relative rotation noise of 0.06 deg standard deviation |
| MH01 | 0.041481 | 0.041484 | 0.041482 | 0.041498 |
| MH02 | 0.041481 | 0.041462 | 0.041476 | 0.041502 |
| MH03 | 0.041481 | 0.041470 | 0.041481 | 0.041471 |
| MH04 | 0.041481 | 0.041462 | 0.041478 | 0.041458 |
| MH05 | 0.041481 | 0.041480 | 0.041479 | 0.041486 |
| V101 | 0.041481 | 0.041484 | 0.041480 | 0.041484 |
| V102 | 0.041481 | 0.041571 | 0.041490 | 0.041561 |
| V103 | 0.041481 | 0.041605 | 0.041495 | 0.041581 |
| V201 | 0.041481 | 0.041484 | 0.041481 | 0.041478 |
| V202 | 0.041483 | 0.041432 | 0.041477 | 0.041377 |
| V203 | 0.037116 | 0.037212 | 0.037129 | 0.037109 |
| Mean | 0.041084 | 0.041104 | 0.041086 | 0.041091 |
(d) Relative rotation noise from image matches
| MH01 | 0.030048 | 0.030020 | 0.030046 | 0.030028 |
| MH02 | 0.033075 | 0.033052 | 0.033072 | 0.033064 |
| MH03 | 0.029123 | 0.029152 | 0.029126 | 0.029161 |
| MH04 | 0.045381 | 0.045410 | 0.045384 | 0.045392 |
| MH05 | 0.038635 | 0.038620 | 0.038633 | 0.038622 |
| V101 | 0.044986 | 0.044941 | 0.044980 | 0.044946 |
| V102 | 0.090967 | 0.090799 | 0.090948 | 0.090825 |
| V103 | 0.351947 | 0.351923 | 0.351939 | 0.351934 |
| V201 | 0.048773 | 0.048756 | 0.048769 | 0.048765 |
| V202 | 0.057977 | 0.058014 | 0.057978 | 0.058041 |
| V203 | 0.122305 | 0.122289 | 0.122303 | 0.122330 |
| Mean | 0.081202 | 0.081180 | 0.081198 | 0.081192 |
---
Fig. 2. Initialization accuracy vs. activation policy. We trigger closed-form initialization (i) after a fixed 25-frame window ( $\approx$ 1.25 s); (ii) when Translation Observability Test is satisfied; and (iii) when Full Observability Test is satisfied. Panels report RMSE distributions in (a) gravity direction, (b) velocity, and (c) accelerometer bias. The Translation Observability Test already improves initialization accuracy, while the Full Observability Test yields the best overall performance.
TABLE III
GYROSCOPE BIAS INITIALIZATION RUNTIME. OUR APPROXIMATIONS ACHIEVE UP TO TWO ORDERS OF MAGNITUDE SPEED-UP COMPARED TO THE NONLINEAR OPTIMIZATION BASELINE.
| Method | Mean [ $\mu s$] | Std dev. [ $\mu s$] |
| Nonlinear optimization | 3687 | 275 |
| Commutative approximation (ours) | 94 | 6 |
| Avg. approximation (ours) | 25 | 1 |
| Arithm. Avg approximation (ours) | 19 | 1 |
非线性基线方法的结果迅速趋同。即便在相对旋转误差的标准差仅为0.006度的极端情况下，$b^g$误差的差异也已低于3%；对于更大的扰动，这种差异更是微乎其微。这些结果表明，在存在噪声的相对旋转估计的实际场景中，我们的闭式解所能达到的$b^g$估计精度与非线性优化相当。同时值得注意的是，各种近似方法之间的差异也微不足道。
真实数据：在本实验中，相对旋转量 $R_{ij}$ 是通过图像对应关系利用 C2P [^29] 计算得出的，随后通过 RANSAC 算法剔除异常值。这种设置模拟了现实场景：旋转输入源自真实的视觉数据，因此会受到噪声和匹配误差的影响。在所有初始化实验中，平均异常值比例约为 0.15。
表 II 的最后几行总结了在所有 EuRoC 序列中，我们提出的闭式解与非线性优化方法所得到的陀螺仪偏差估计误差的平均值。表 III 则给出了采用上述四种策略进行陀螺仪偏差估计时的执行时间均值与标准差。相较于非线性基准方法，交换性近似算法可将运行时间缩短 $39 \times$ 倍，Avg. 近似算法可缩短 $147 \times$ 倍，而算术平均值近似算法则可缩短 $194 \times$ 倍。这一结果验证了我们所提近似算法的计算优势。表 III 所列的是陀螺仪偏差估计的独立运行时间，前提是相对旋转量 $R_{ij}$ 已经预先算出；而计算 $R_{ij}$ 所需的时间已包含在表 V 所报告的总初始化时间内。
综上所述，可以注意到在现实条件下，我们的所有方法都能达到相近的精度，只有在无噪声的理想情况下才会出现明显差异。由特征匹配产生的误差远大于我们所提近似算法带来的误差，而后者在实际应用中完全可以忽略不计。我们提出的三种近似算法精度相近，其中算术平均值近似算法效率最高，因此最适合用于实际实现。
### C. 完整状态初始化
触发策略评估：为了验证我们基于可观测性驱动的初始化方法，我们比较了三种不同的激活策略：(i) 在 25 帧后采用固定时间策略（$\approx$ 1.25 秒）；(ii) 当满足第一个可观测性条件（平移可观测性测试）时激活；(iii) 当满足第二个可观测性条件（完全可观测性测试）时激活。图 2 展示了该实验的结果，凸显出我们的两阶段策略相较于简单的固定窗口策略所具有的优势。在 25 帧后才开始初始化会导致误差更大且更分散，尤其是在重力估计方面，因为无法保证所选时间间隔内系统状态的可观测性。相比之下，一旦满足第一个平移可观测性测试条件就激活闭式完整解法，就能显著减小误差；而如果继续监测海森矩阵并在满足完全可观测性测试条件后再激活，则会获得更小的误差，这一结果在所有估计的状态变量上均成立。这些结果证明，我们基于可观测性的准则不仅能比固定时间方法实现更快的初始化，还能提供比固定窗口策略更精确、更可靠的估计值。在激励有限的情况下，加速度计偏差很难与重力分量分离。因此，只有当完全可观测性测试表明海森矩阵的条件良好时，才会求解公式 (18)，因为过早求解会导致估计值出现偏差（见图 2）。在单一线性系统中同时估计速度、重力和 $b_{a}$，无需依赖三维结构即可提升数值稳定性。
与基线方法的比较：我们将所提出的初始化器与三种具有代表性的基线方法进行比较：紧密耦合版本的DRT（记为DRT-tight）[^14]、VINS-Mono的初始化模块（记为VINS-Mono）[^1]，以及解析解方法[^30]，该方法对ORB-SLAM3的初始化过程进行了扩展[^11]（记为AS-MLE）。表四报告了在EuRoC数据集上各序列的初始化误差。在所有序列中取平均值后，我们的方法实现了最低的速度均方根误差（0.09 m/s）和重力方向误差（0.90度），性能优于DRT-tight（0.11 m/s，1.03度）、VINS-Mono（0.11 m/s，1.47度）以及AS-MLE（0.17 m/s，2.66度）。各序列的结果显示，我们的初始化器在大多数情况下无论是速度还是重力估计方面都表现最佳或次佳，仅有少数序列中DRT-tight仍具有竞争力（例如V102中的重力估计）。
---
TABLE IV
PER-SEQUENCE INITIALIZATION ERRORS ON EUROC (ATTEMPTS EVERY 0.5 s) FOR AS-MLE, VINS-MONO, DRT-TIGHT, AND OUR METHOD. BEST PER ROW IS SHOWN IN BOLD, SECOND-BEST IS $\underline{\text{UNDERLINED.}}$
| Seq. | Velocity Error [m/s] | Gravity Error [deg] | Accel. Bias Error [m/s $^{2}$] | Gyro. Bias Error [rad/s] |
| AS-MLE | VINS-Mono | DRT-tight | Ours | AS-MLE | VINS-Mono | DRT-tight | Ours | AS-MLE | VINS-Mono | DRT-tight | Ours | AS-MLE | VINS-Mono | DRT-tight | Ours |
| MH01 | 0.11 | 0.08 | 0.11 | 0.10 | 1.64 | 1.17 | 1.00 | 0.87 | 0.093 | 0.092 | - | 0.088 | 0.091 | 0.070 | - | 0.030 |
| MH02 | 0.09 | 0.08 | 0.11 | 0.09 | 1.36 | 1.11 | 0.97 | 0.65 | 0.092 | 0.093 | - | 0.091 | 0.090 | 0.080 | - | 0.033 |
| MH03 | 0.30 | 0.19 | 0.14 | 0.09 | 3.06 | 1.57 | 0.96 | 0.96 | 0.080 | 0.081 | - | 0.080 | 0.063 | 0.051 | - | 0.029 |
| MH04 | 0.25 | 0.14 | 0.19 | 0.11 | 1.87 | 1.23 | 0.99 | 0.84 | 0.087 | 0.088 | - | 0.086 | 0.068 | 0.060 | - | 0.045 |
| MH05 | 0.25 | 0.17 | 0.22 | 0.13 | 2.61 | 1.42 | 0.99 | 0.83 | 0.084 | 0.081 | - | 0.081 | 0.075 | 0.062 | - | 0.039 |
| V101 | 0.07 | 0.06 | 0.07 | 0.07 | 3.37 | - | 0.99 | 0.89 | 0.316 | 0.319 | - | 0.128 | 0.082 | - | - | 0.045 |
| V102 | 0.24 | 0.11 | 0.06 | 0.05 | 5.57 | 2.60 | 0.79 | 1.09 | 0.082 | 0.081 | - | 0.078 | 0.091 | 0.077 | - | 0.091 |
| V103 | - | - | 0.11 | 0.12 | - | - | 1.48 | 0.87 | 0.115 | 0.116 | - | 0.094 | 0.352 | 0.349 | - | 0.352 |
| V201 | 0.08 | 0.07 | 0.08 | 0.06 | 1.79 | 1.43 | 1.15 | 1.06 | 0.085 | 0.083 | - | 0.082 | 0.067 | 0.049 | - | 0.049 |
| V202 | 0.13 | 0.08 | 0.06 | 0.07 | 2.68 | 1.26 | 0.84 | 0.94 | 0.055 | 0.056 | - | 0.052 | 0.068 | 0.067 | - | 0.058 |
| V203 | - | - | 0.09 | 0.08 | - | - | 1.17 | 0.92 | 0.053 | 0.053 | - | 0.062 | 0.151 | 0.141 | - | 0.122 |
| Mean | 0.17 | 0.11 | 0.11 | 0.09 | 2.66 | 1.47 | 1.03 | 0.90 | 0.104 | 0.104 | - | 0.084 | 0.109 | 0.101 | - | 0.081 |
TABLE V
AVERAGE INITIALIZATION COST [MS] ON EUROC. RUNTIME BREAKDOWN AND COMPARISON AGAINST BASELINES.
| Module | AS-MLE | VINS-Mono | DRT-tight | Ours |
| SfM | 30.30 | 30.35 | - | - |
| $b^{g}$ Est. | 0.15 | 0.44 | 1.95 | 0.02 |
| Vel. & Grav. Est. | 0.08 | 0.14 | 2.81 | 0.52 |
| Point Tri. | 0.01 | 0.01 | 0.42 | 0.42 |
| Total | 30.54 | 30.94 | 5.18 | 0.96 |
除了速度和重力外，表四还明确报告了初始化过程中的惯性偏差误差。我们的方法在所有序列中始终能实现最低的陀螺仪偏差误差，其数值约为$10^{-2}$ rad/s，这一结果相比AS-MLE和VINS-Mono有显著改善。这凸显出在求解闭式初始化系统之前，显式估计并补偿陀螺仪偏差所带来的优势，进而可获得更精确的相对旋转结果。如表中所示，改进后的旋转估计直接转化为更一致的重力对齐效果以及更低的速度误差。对于加速度计偏差，我们的方法也取得了最低的平均均方根误差（0.084 m/s $^2$），表明初始化过程中能够可靠地捕捉到偏差效应。需要注意的是，DRT-tight在其初始化阶段并未显式建模惯性偏差，因此该方法未报告加速度计和陀螺仪偏差误差。与某些初始化方法不同，我们在初始化过程中并不强制设定固定的重力大小；相反，只有在检测到足够的可观测性后，才会估计与平移相关的状态，从而确保线性系统的条件良好。如图2和表四所示，这种基于可观测性感知的策略所产生的重力方向和速度误差，均低于那些过早强制设定重力大小的方法。
TABLE VI
运行时间：我们在EuRoC数据集上评估了我们的整体初始化运行时间与代表性基线方法的对比情况。表五按模块细分了完整状态初始化的时间。我们的方法在初始化过程中无需依赖三维结构，因此仅统计陀螺仪偏差估计以及速度-重力求解器的耗时。为了与依赖结构的方法进行公平比较，我们还额外报告了初始化之后执行的
INITIALIZATION WINDOW LENGTH AND ADAPTIVITY. WHEREAS BASELINES RELY ON FIXED/LONG WINDOWS, OUR TWO-STAGE CRITERION REDUCES INITIALIZATION DELAYS $4\times$.
| Method | Initialization time [s] | Adaptive? |
| Campos et al. [^10] | 2.16 $\pm$ 0.01 | ✓ |
| He et al. [^14] | 2.50 | ✗ |
| Ours | 0.48 $\pm$ 0.27 | ✓ |
可选的“点三角测量”阶段。我们的总初始化时间仅为0.96毫秒，分别比AS-MLE快31.8倍、比VINS-Mono快32.2倍，比DRT-tight快5.4倍。在模块层面，陀螺仪偏差估计的时间相比AS-MLE缩短了7.5倍，相比VINS-Mono缩短了22倍，相比DRT-tight的缩短倍数则未明确给出。我们的闭式速度-重力求解器比DRT-tight快5.4倍，但比AS-MLE/VINS-Mono慢3.7至6.5倍（后者利用了预先构建的结构）。值得注意的是，我们的流程省去了SfM阶段（在AS-MLE/VINS-Mono中约占30毫秒），而该阶段正是它们的主要耗时来源；若仅报告核心初始化部分（偏差+速度-重力），我们的时间仅为0.54毫秒，即比AS-MLE快56.6倍。
初始化延迟：表六对比了基线算法与我们的方法所使用的初始化时间窗口。以往的方法通常需要平均约两秒的时间窗口，无论是固定窗口还是自适应选择的窗口。相比之下，我们的两阶段准则仅需平均半秒的自适应窗口即可实现可靠的初始化。
近似鲁棒性分析：为了量化三种近似方法的有效范围，我们在图3中绘制了陀螺仪偏差估计误差随累积旋转幅度变化的曲线。对于每个有效样本，我们比较了以下四种情况下的偏差误差：(i) 交换律近似、(ii) 平均值近似、(iii) 算术平均值近似以及(iv) 非线性优化方法。在2698个有效样本中，当旋转幅度不超过1.5–2度时，并未观察到明显的性能退化，此时我们的近似方法仍保持极高的精度。一旦超过这一阈值，误差便会增大，且与非线性优化方法之间的差距也会持续存在。相比之下，非线性优化方法的表现则相对稳定，其误差趋势近乎平坦且数值更小。不过需要注意的是，在EuRoC数据集中，仅有15%的连续样本对其相对旋转角度超过了2度。这一观察结果支持了
---
Fig. 3. Gyroscope-bias error vs. accumulated rotation on EuRoC. Our three approximation variants (solid blue/green/orange curves) produce nearly identical results and appear superimposed. In contrast, the nonlinear optimization (dashed red curve) remains comparatively stable up to 20 deg, while approximation-based estimates exhibit increasing error and variability as rotation grows.
我们的近似方法在实际应用中的有效性，同时也与表二和表四中所报告的定量结果相一致。
### 七、结论
在这项工作中，我们提出了一种闭式初始化方法，无需迭代求解器即可恢复完整的视觉-惯性状态。通过利用小旋转和恒速近似，我们的方法在保持运动与惯性测量之间本质关联的同时，还具备简洁的分析形式。其中的核心部分是我们对陀螺仪偏差估计的快速近似算法，该算法用轻量级的求解器取代了耗时的优化过程，从而实现了即时且稳健的启动。在所有EuRoC序列中，我们的方法将相关基线算法的初始化误差降低了10–20%，初始化时间窗口缩短了4倍，计算时间则减少了5倍。重要的是，我们的解决方案在代数形式上也更加简单。由于所提出的方法针对的是短时间窗口和中等程度的运动，因此在极端动态条件下其近似前提可能会失效。在此类特殊情况下，基于非线性优化的较慢方法可能更为适用。尽管如此，我们认为这种闭式求解思路为资源受限的平台提供了一种实用的替代方案，也为现代视觉-惯性系统中的后续扩展奠定了坚实基础。
## 参考文献
[^1]: T. Qin, P. Li, and S. Shen, “VINS-Mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Trans. Robot., vol. 34, no. 4, pp. 1004–1020, Aug. 2018.
[^2]: C. Campos, R. Elvira, J. J. G. Rodríguez, J. M. M. Montiel, and J. D. Tardós, “ORB-SLAM3: An accurate open-source library for visual, visual-inertial, and multimap SLAM,” IEEE Trans. Robot., vol. 37, no. 6, pp. 1874–1890, Dec. 2021.
[^3]: S. Boche, J. Jung, S. B. Laina, and S. Leutenegger, “OKVIS2-X: Open keyframe-based visual-inertial SLAM configurable with dense depth or LiDAR, and GNSS,” IEEE Trans. Robot., vol. 41, pp. 6064–6083, 2025.
[^4]: L. Carlone and A. Censi, “From angular manifolds to the integer lattice: Guaranteed orientation estimation with application to pose graph optimization,” IEEE Trans. Robot., vol. 30, no. 2, pp. 475–492, Apr. 2014.
[^5]: L. Carlone, R. Tron, K. Daniilidis, and F. Dellaert, “Initialization techniques for 3D SLAM: A survey on rotation estimation and its use in pose graph optimization,” in Proc. IEEE Int. Conf. Robot. Automat., 2015, pp. 4597–4604.
[^6]: A. Martinelli, “Vision and IMU data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination,” IEEE Trans. Robot., vol. 28, no. 1, pp. 44–60, Feb. 2012.
[^7]: A. Martinelli, “Closed-form solution of visual-inertial structure from motion,” Int. J. Comput. Vis., vol. 106, no. 2, pp. 138–152, 2014.
[^8]: J. Kaiser, A. Martinelli, F. Fontana, and D. Scaramuzza, “Simultaneous state initialization and gyroscope bias calibration in visual inertial aided navigation,” IEEE Robot. Automat. Lett., vol. 2, no. 1, pp. 18–25, Jan. 2017.
[^9]: J. Domnguez-Conti, J. Yin, Y. Alami, and J. Civera, “Visual-inertial SLAM initialization: A general linear formulation and a gravity-observing non-linear optimization,” in Proc. IEEE Int. Symp. Mixed Augmented Reality, 2018, pp. 37–45.
[^10]: C. Campos, J. M. Montiel, and J. D. Tardós, “Fast and robust initialization for visual-inertial SLAM,” in Proc. Int. Conf. Robot. Automat., 2019, pp. 1288–1294.
[^11]: C. Campos, J. M. Montiel, and J. D. Tardós, “Inertial-only optimization for visual-inertial initialization,” in Proc. Int. Conf. Robot. Automat., 2020, pp. 51–57.
[^12]: S. H. Lee and J. Civera, "Rotation-only bundle adjustment," in Proc. IEEE/CVF Conf. Comput. Vis. Pattern Recognit., 2021, pp. 424–433.
[^13]: A. Concha, M. Burri, J. Briales, C. Forster, and L. Oth, “Instant visual odometry initialization for mobile AR,” IEEE Trans. Vis. Comput. Graph., vol. 27, no. 11, pp. 4226–4235, Nov. 2021.
[^14]: Y. He, B. Xu, Z. Ouyang, and H. Li, “A rotation-translation-decoupled solution for robust and efficient visual-inertial initialization,” in Proc. IEEE/CVF Conf. Comput. Vis. Pattern Recognit., 2023, pp. 739–748.
[^15]: R. Wang, X. Qian, X. Gao, and L. Chen, "EDI: An error-state Kalman filter based disjoint initialization framework for visual-inertial SLAM," IEEE Robot. Automat. Lett., vol. 8, no. 9, pp. 5905–5912, 2023.
[^16]: G. D. Evangelidis and B. Micušik, “Revisiting visual-inertial structure-from-motion for odometry and SLAM initialization,” in Proc. IEEE Int. Conf. Robot. Automat., 2020, pp. 1415–1422.
[^17]: L. Kneip and S. Lynen, “Direct optimization of frame-to-frame rotation,” in Proc. Int. Conf. Comput. Vis., 2013, pp. 2352–2359.
[^18]: J. Oliensis, “Exact two-image structure from motion,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 24, no. 12, pp. 1618–1633, Dec. 2002.
[^19]: S. H. Lee and J. Civera, “Closed-form optimal two-view triangulation based on angular errors,” in Proc. Int. Conf. Comput. Vis., 2019, pp. 2681–2689.
[^20]: F. Yu and D. Gallup, "3D reconstruction from accidental motion," in Proc. IEEE/CVF Conf. Comput. Vis. Pattern Recognit., 2014, pp. 3986–3993.
[^21]: J. Ventura, C. Arth, and V. Lepetit, “Approximated relative pose solvers for efficient camera motion estimation,” in Proc. Eur. Conf. Comput. Vis., 2014, pp. 180–193.
[^22]: C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual-inertial odometry,” IEEE Trans. Robot., vol. 33, no. 1, pp. 1–21, Feb. 2017.
[^23]: A. Fontan, L. Oliva, J. Civera, and R. Triebel, “Model for multi-view residual covariances based on perspective deformation,” IEEE Robot. Automat. Lett., vol. 7, no. 2, pp. 1960–1967, Apr. 2022.
[^24]: P. Geneva, K. Eckenhoff, W. Lee, Y. Yang, and G. Huang, “OpenVINS: A research platform for visual-inertial estimation,” in Proc. IEEE Int. Conf. Robot. Automat., 2020, pp. 4666–4672.
[^25]: S. Cerezo and J. Civera, “GNSS-Inertial state initialization using interEpoch baseline residuals,” IEEE Robot. Automat. Lett., vol. 11, no. 2, pp. 1458–1465, Feb. 2026.
[^26]: S. Cerezo and J. Civera, “Camera motion estimation from RGB-D-inertial scene flow,” in Proc. Conf. Comput. Vis. Pattern Recognit. Workshop, 2024, pp. 841–849.
[^27]: S. Agarwal, K. Mierle, and T. C. S. Team, "Ceres solver," 2023. [Online]. Available:
[^28]: M. Burri et al., “The EuRoC micro aerial vehicle datasets,” Int. J. Robot. Res., vol. 35, 2016, pp. 1157–1163.
[^29]: J. Tirado-Gariñ and J. Civera, “From correspondences to pose: Non-minimal certifiably optimal relative pose without disambiguation,” Proc. IEEE/CVF Conf. Comput. Vis. Pattern Recognit., 2024, pp. 403–412.
[^30]: D. Zuniga-Noël, F.-A. Moreno, and J. Gonzalez-Jimenez, “An analytical solution to the IMU initialization problem for visual-inertial systems,” IEEE Robot. Automat. Lett., vol. 6, no. 3, pp. 6116–6122, Jul. 2021.