VINS是由香港科技大学秦通等人,于2018年提出的一种强大且通用的单目视觉惯性状态估计器,是初学者接触VIO 比较好的开源学习资料,作者的论文条理清晰,分为五个主要部分对该系统展开描述:测量预处理、初始化、后端非线性优化、闭环检测、闭环优化。本节主要对原始论文进行翻译。

VINS-Mono

摘要

摘要:由相机和低成本惯性测量单元 (IMU) 组成的单目视觉惯性系统 (VINS) 构成了用于度量六自由度 (DOF) 状态估计的最小传感器套件。然而,缺乏直接距离测量,在 IMU 处理、估计器初始化、外部校准和非线性优化方面提出了重大挑战。在这项工作中,我们介绍了 VINS-Mono:一种强大且通用的单目视觉惯性状态估计器。我们的方法从估计器初始化和故障恢复的稳健程序开始。通过融合预积分的 IMU 测量和特征观测,使用一种基于紧耦合非线性优化的方法来获得高精度的视觉惯性里程计。回环检测模块与我们的紧密耦合公式相结合,可以以最小的计算开销实现重新定位。我们还执行了四个自由度位姿图优化,以强制执行全局一致性。我们在公共数据集和真实世界的实验中验证了我们系统的性能,并与其他最先进的算法进行了比较。我们还在 MAV 平台上执行机载闭环自主飞行,并将算法移植到基于 iOS 的演示中。我们强调,所提出的工作是一个可靠、完整且通用的系统,适用于需要高精度定位的不同应用。我们为 PC$^1$ 和 iOS $^2$ 移动设备开源了我们的实现。

$^1$https://github.com/HKUST-Aerial-Robotics/VINS-Mono

$^2$ https://github.com/HKUST-Aerial-Robotics/VINS-Mobile

索引词——单目视觉惯性系统、状态估计、传感器融合、同时定位和建图 。

一、引言

状态估计无疑是机器人导航、自动驾驶、虚拟现实和增强现实 (AR) 等广泛应用的最基本模块。仅使用单目相机的方法由于其体积小、成本低和易于硬件设置而在该领域获得了极大的兴趣[1]-[5]。然而,单目视觉系统无法恢复公制尺度,因此限制了它们在现实世界机器人应用中的使用。最近,我们看到了用低成本惯性测量单元(IMU)辅助单目视觉系统的增长趋势。这种单目视觉惯性系统 (VINS) 的主要优点是公制刻度,以及滚动和俯仰角可观。这使得需要度量状态估计的导航任务成为可能。此外,IMU 测量的集成可以通过弥补由于光照变化、无纹理区域或运动模糊造成的视觉轨迹损失之间的差距,从而显着提高运动跟踪性能。单目VINS不仅在地面机器人和无人机上广泛使用,而且在移动设备上也很实用。它在尺寸、重量和功耗方面对自我和环境感知都有很大的优势。

但是,有几个问题会影响单目 VINS 的使用。第一个是严格的初始化。由于缺乏直接的距离测量,很难将单目视觉结构与惯性测量直接融合。同样认识到 VINS 是高度非线性的这一事实,我们看到了估计器初始化方面的重大挑战。在大多数情况下,系统应该从已知的静止位置启动,并在开始时缓慢而小心地移动,这限制了其在实践中的使用。另一个问题是,对于视觉惯性里程计 (VIO),长期漂移是不可避免的。为了消除漂移,必须开发回环检测、重新定位和全局优化。除了这些关键问题外,对地图保存和重用的需求正在增长。

为了解决所有这些问题,我们提出了 VINS-Mono,这是一种强大且通用的单目视觉惯性状态估计器,它是我们之前三项工作的组合和扩展 [6]-[8]。 VINS-Mono 包含以下功能:

  • 能够从未知初始状态引导系统的稳健初始化过程;
  • 紧耦合、基于优化的单目 VIO 与相机-IMU 外部校准和 IMU 偏差校正;
  • 在线重定位和四自由度(DOF)全局位姿图优化;
  • 位姿图复用,可以保存、加载和合并多个局部位姿图。

在这些功能中,稳健的初始化、重定位和位姿图重用是我们的技术贡献,这些贡献来自我们之前的工作 [6]-[8]。工程贡献包括开源系统集成、无人机导航实时演示和移动应用。整个系统已成功应用于小规模 AR 场景、中规模无人机导航和大规模状态估计任务,如图 1 所示。

image-20220516104928638

(a) 轨迹(蓝色)和特征位置(红色)

image-20220516105005304

(b) 用谷歌地图覆盖的轨迹以进行视觉比较

图 1 所提出的单目视觉惯性状态估计器的室外实验结果。数据是在正常行走条件下由手持单目相机-IMU 设置收集的。它包括场地内的两个完整圆和附近车道上的两个半圆。总轨迹长度为 2.5 公里。实验视频可在多媒体附件中找到。

本文的其余部分的结构如下:在第二节中,我们讨论了相关的文献。在第三节中,我们对完整的系统框架进行了概述。在第四节中,给出了视觉的预处理和IMU测量值的预积分步骤。在第五节中,我们讨论了估计器的初始化过程。在第六节中提出了一种紧耦合、自标定、非线性优化的单目VIO。第七节和第八节分别给出了紧耦合重定位和全局位姿图优化。实施细节和实验结果见第九节。最后,第十节本文对研究方向进行了探讨和展望。

二、相关工作

关于基于单目视觉的状态估计/里程计SLAM的学术工作非常广泛。值得注意的方法包括PTAM[1]、SVO[2]、LSD-SLAM[3]、DSO[5]和ORB-SLAM[4]。显然,尝试对任何方法进行全面回顾都无法完整。然而,在这一节中,我们跳过了关于只使用视觉的方法的讨论,而只专注于关于单目视觉惯性状态估计的最相关的结果。

处理视觉和惯性测量的最简单方法是松耦合传感器融合 [9]、[10],其中 IMU 被视为一个独立模块,以辅助从运动的视觉结构中获得的仅视觉姿态估计。融合通常由扩展卡尔曼滤波器 (EKF) 完成,其中 IMU 用于状态传播,而视觉位姿用于更新。此外,紧耦合的视觉惯性算法要么基于 EKF[11]-[13],,要么基于图优化 [14]-[19],,其中相机和IMU测量值是从原始测量水平联合优化的。一种流行的基于 EKF 的 VIO 方法MSCKF [11]、[12]。 MSCKF 在状态向量中维护几个先前的相机位姿,并使用跨多个相机视图的相同特征的视觉测量来形成多约束更新。 SR-ISWF [20]、[21] 是 MSCKF 的扩展。它采用平方根($squareroot$)形式[14]实现单精度表示,避免了较差的数值性质。该方法采用逆滤波器进行迭代再线性化,使其与基于优化的算法相当。批量图优化或集束调整技术($Batch\ graph\ optimization
or\ bundle\ adjustment\ techniques\ ,BA$)维护和优化所有测量值以获得最优状态估计。为了达到恒定的处理时间,流行的基于图的VIO方法[15]、[17]、[18]通常采用边缘化过去的状态和测量来优化最近状态的有界滑动窗口。由于对非线性系统迭代求解的计算要求很高,很少有基于图的非线性系统能够在资源受限的平台(如手机上)实现实时性能。

对于视觉测量处理,根据视觉残差模型的定义,算法可分为直接法间接法。直接法 [2] [3] [22]最小化光度误差,而间接法最小化几何位移。直接法因其吸引区域小,需要很好的初始估计,而间接法 [12]、[15]、[17]在提取和匹配特征时需要额外的计算资源。间接法由于其成熟性和鲁棒性,在实际工程部署中得到了广泛的应用。然而,直接法更容易扩展到稠密建图,因为它们是直接在像素级别上操作的。

在实践中,IMU 通常以比相机高得多的速率获取数据。已经提出了不同的方法来处理高速 IMU 测量。最直接的方法是在基于 EKF 的方法 [9]、[11] 中使用 IMU 进行状态传播。在图优化公式中,为了避免重复的IMU重复积分,提出了一种有效的方法,即IMU预积分($IMU\ pre-integration$)。这种方法在[23]中首次提出的,它用欧拉角来参数化旋转误差。[16] 使用连续时间误差状态动力学推导出协方差传播。然而IMU偏置被忽略了。在 [19] 和 [24]中通过增加后验IMU偏置校正,进一步改进了预积分理论。

精确的初始值对于引导任何单目VINS是至关重要的。在 [17] 和 [25] 中提出了一种利用短期IMU预积分相对旋转的线性估计器初始化方法。但是,该方法不对陀螺仪偏置进行建模,也不能模拟原始投影方程中的传感器噪声。在实际应用中,当视觉特性远离传感器套件时,这会导致不可靠的初始化。在[26]中给出了单目视觉惯性初始化问题的一种封闭解。随后,在[27]中提出了通过添加陀螺仪偏置校准来扩展这种封闭形式的解决方案。这些方法无法对惯性积分中的不确定性进行建模,因为它们依赖于长时间内 IMU 测量的双重积分。在[28]中,提出了一种基于SVO [2]的重新初始化和故障恢复算法。需要一个额外的向下距离传感器来恢复公制刻度。 [18] 中介绍了一种建立在流行的 ORB-SLAM [4] 之上的初始化算法。据研究,尺度收敛所需的时间可以超过 10 s。这可能会给机器人导航任务带来问题,这些任务一开始就需要规模估计。

里程计方法,无论它们所依赖的基础数学公式如何,都会在全局平移和方向上长期漂移。为此,闭环在长期运营中发挥着重要作用。 ORB-SLAM [4] 能够闭合回环 并重用地图,这利用了词袋 [29]。一个 7-DOF [30](位置、方向和比例)位姿图优化跟随循环检测。

三、概述

所提出的单目视觉惯性状态估计器的结构如图 2 所示。该系统从测量预处理开始(参见第 IV 节),其中提取和跟踪特征,并对两个连续帧之间的 IMU 测量进行预积分。初始化过程(参见第 V 节)提供了所有必要的值,包括姿态、速度、重力矢量、陀螺仪偏差和三维 (3-D) 特征位置,用于引导后续基于非线性优化的 VIO。带有重定位(参见第 VII 节)模块的 VIO(参见第 VI 节)紧密融合了预积分的 IMU 测量、特征观察。最后,位姿图优化模块(参见第 VIII 节)采用经过几何验证的重定位结果,并执行全局优化以消除漂移。它还实现了位姿图重用。 VIO 和位姿图优化模块在单独的线程中同时运行。

与适用于立体相机的最先进的 VIO 算法 OKVIS [15] 相比,我们的算法是专门为单目相机设计的。因此,我们特别提出了初始化程序、关键帧选择标准,并使用和处理大视场 (FOV) 相机以获得更好的跟踪性能。此外,我们的算法提出了一个完整的系统,具有闭环和位姿图重用模块。

image-20220516110348319

图 2. 一个框图,说明了所提出的单目视觉惯性状态估计器的完整流程。

我们现在定义我们在整篇论文中使用的符号和坐标系定义。我们将 $(·)^w$ 视为世界坐标系。重力方向与世界坐标系的 $z$ 轴对齐。$(·)^b$是body 坐标系,我们定义它和IMU 坐标系一样。$(·)^c$ 是相机坐标系。我们使用旋转矩阵 $R$ 和 Hamilton 四元数 $q$ 来表示旋转。我们主要在状态向量中使用四元数,但旋转矩阵也用于方便地旋转三维向量。 $q^w_b , p^w_b$ 是从本体坐标系到世界坐标系的旋转和平移。$b_k$ 是拍摄第 $k$ 张图像时的本体坐标系。 $c_k$ 是拍摄第 $k$ 张图像时的相机帧。 $\otimes$ 表示两个四元数之间的乘法运算。$g^w = [0, 0, g]^T$ 是世界坐标系中的重力矢量。最后,我们将 $\hat{(·)}$ 表示为某个量的噪声测量或估计。

四、测量预处理

本节介绍惯性和单目视觉测量的预处理步骤。对于视觉测量,我们跟踪连续帧之间的特征并检测最新帧中的新特征。对于 IMU 测量,我们将它们预积分在两个连续的帧之间。请注意,我们使用的低成本 IMU 的测量值会受到偏差和噪声的影响。因此,我们在 IMU 预积分过程中特别考虑了偏差。

A. 视觉处理前端

对于每一幅新图像,KLT 稀疏光流算法对现有特征进行跟踪[29]。同时,检测新的角点特征[30]以保证每个图像特征的最小数目 (100-300)。该检测器通过设置两个相邻特征之间像素的最小间隔强制实现均匀的特征分布。二维特征首先是不失真的,然后在通过剔除异常点后,投影到一个单位球面上。利用基础矩阵模型的RANSAC算法进行异常点剔除。

在此步骤中还选择了关键帧。我们有两个关键帧选择标准。第一是与上一个关键帧的平均视差。如果在当前帧和最新关键帧之间跟踪的特征点的平均视差超出某个特定阈值,则将该帧视为新的关键帧。请注意,不仅平移,旋转也会产生视差。然而,特征点无法在纯旋转运动中三角化。为了避免这种情况,在计算视差时我们使用陀螺仪测量值的短时积分来补偿旋转。请注意,此旋转补偿仅用于关键帧选择,而不涉及VINS公式中的旋转计算。为此,即使陀螺仪含有较大的噪声或存在偏置,也只会导致次优的关键帧选择结果,不会直接影响估计质量。另一个标准是跟踪质量。如果跟踪的特征数量低于某一阈值,我们将此帧视为新的关键帧。这个标准是为了避免跟踪特征完全丢失。

B. IMU 预积分

我们遵循我们之前基于连续时间四元数的 IMU 预积分推导 [16],并将 IMU 偏差的处理包括为 [19] 和 [24]。我们注意到,我们当前的 IMU 预积分过程与 [19] 和 [24] 共享几乎相同的数值结果,但使用不同的推导。所以,我们在这里只做一个简单的介绍。有关基于四元数的推导的详细信息,请参见附录 A。

(1)IMU 噪声和偏置:IMU 测量,在本体坐标系中测量,结合了反重力的力和平台动力学,并受到加速度偏差 $b_a$、陀螺仪偏差 $b_w$ 和附加噪声的影响。IMU 的原始陀螺仪和加速度计测量值 $\hat{w}$ 和 $\hat{a}$ 由下式给出:

$$ \begin{align*} \hat{a}_t&=a_t+b_{a_t}+R^t_wg^w+n_a\\ \hat{w}_t&=w_t+b_{w_t}+n_w \end{align*}\tag{1} $$

I我们假设加速度和陀螺仪测量中的附加噪声是高斯噪声,$n_a\sim\mathcal{N}(0,\sigma^2_a),n_w\sim\mathcal{N}(0,\sigma^2_w)$。加速度偏差和陀螺仪偏差被建模为随机游走,其导数为高斯,$n_{b_a}\sim\mathcal{N}(0, \sigma^2_{b_a}),n_{b_w}\sim\mathcal{N}(0, \sigma^2_{b_w})$:

$$ \dot{b}_{a_t}=n_{b_a},\ \ \ \ \ \ \dot{b}_{w_t}=n_{b_w}\tag{2} $$

(2)预积分:对于两个时间连续帧 $b_k$ 和 $b_{k+1}$,在时间间隔 $[t_k , t_{k+1}]$ 内存在多个惯性测量。给定偏差估计,我们将它们整合到局部框架 $b_k$ 中:

$$ \begin{align*} \alpha^{b_k}_{b_{k+1} }&=\iint_{t\in[t_k, t_{k+1}]}\mathbf{R}^{b_k}_t(\hat{a}_t-b_{a_t})dt^2\\ \beta ^{b_k}_{b_{k+1} }&=\int_{t\in[t_k, t_{k+1}]}\mathbf{R}^{b_k}_t(\hat{a}_t-b_{a_t})dt\\ \gamma^{b_k}_{b_{k+1} }&= \int_{t\in[t_k, t_{k+1}]} \frac{1}{2}\Omega(\hat{w}_t-b_{w_t})\gamma^{b_k}_tdt \end{align*}\tag{3} $$

其中,

$$ \Omega(w)= \begin{bmatrix} -w^{\land}&w\\-w^T&0 \end{bmatrix}, w^{\land}=\begin{bmatrix} 0&-w_z&w_y\\ w_z&0&-w_x\\ -w_y&w_x&0 \end{bmatrix}\tag{4} $$

$α、β$ 和 $γ$ 的协方差 $\mathbf{P}^{b_k}_{b_{k+1} }$ 也相应地传播。可以看出,预积分项 $(3)$ 可以通过将 $b_k$ 作为给定偏差的参考系单独使用 IMU 测量来获得。

(3)偏差校正:如果偏差的估计变化很小,我们通过它们关于偏差的一阶近似来调整 $\alpha^{b_k}_{b_{k+1} }$ 、$\beta^{b_k}_{b_{k+1} }$和 $\gamma^{b_k}_{b_{k+1} }$ 为:

$$ \begin{align*} \alpha^{b_k}_{b_{k+1} }&\approx \hat{\alpha}^{b_k}_{b_{k+1} }+\mathbf{J}^{\alpha}_{b_a}\delta b_{a_k}+\mathbf{J}^{\alpha}_{b_w}\delta b_{w_k} \\ \beta^{b_k}_{b_{k+1} }&\approx \hat{\beta}^{b_k}_{b_{k+1} }+\mathbf{J}^{\beta}_{b_a}\delta b_{a_k}+\mathbf{J}^{\beta}_{b_w}\delta b_{w_k} \\ \gamma^{b_k}_{b_{k+1} }&\approx \hat{\gamma}^{b_k}_{b_{k+1} }\otimes \begin{bmatrix}1\\\frac{1}{2}\mathbf{J}^{\gamma}_{b_w}\delta b_{w_k} \end{bmatrix} \end{align*}\tag{5} $$

否则,当偏差的估计发生显着变化时,我们会在新的偏差估计下进行重新传播。这种策略为基于优化的算法节省了大量的计算资源,因为我们不需要重复传播 IMU 测量。

五、估计器初始化

单目紧耦合 VIO 是一个高度非线性的系统,一开始就需要一个准确的初始值。我们通过将 IMU 预集成与纯视觉结构松耦合对齐来获得必要的初始值

A. 滑动窗口纯视觉 SfM

初始化过程从纯视觉 SfM 开始,以估计最大比例的相机姿势和特征位置图。

我们保持了帧的滑动窗口来限制计算复杂度。首先,我们检查了最新帧与之前所有帧之间的特征对应关系。如果我们能在滑动窗口中的最新帧和任何其他帧之间,找到稳定的特征跟踪 (超过30个跟踪特征) 和足够的视差 (超过20个的旋转补偿像素),我们使用五点法 [33] 恢复这两个帧之间的相对旋转和尺度平移。否则,我们将最新的帧保存在窗口中,并等待新的帧。如果五点算法成功的话,我们任意设置尺度,并对这两个帧中观察到的所有特征进行三角化。基于这些三角特征,采用PnP[35]来估计窗口中所有其他帧的姿态。最后,应用全局光束平差法 (BA) [36]最小化所有特征观测的重投影误差。由于我们还没有关于世界坐标系的任何知识,我们将第一个相机坐标系 $(·)^{c_0}$ 设置为 SfM 的参考坐标系。所有帧的位姿 $(\overline {\mathbf{p} }_{c_k}^{c_0},{\mathbf{q} }_{c_k}^{c_0})$ 和特征位置表示相对于 $(·)^{c_0}$ 。假设摄像机和IMU之间有一个粗略测量的外部参数 $({\mathbf{p} }_c^b,{\mathbf{q} }_c^b)$,我们可以将姿态从相机坐标系转换到物体 (IMU) 坐标系。

$$ \begin{align*} {\mathbf{q} }_{b_k}^{c_0}&={\mathbf{q} }_{c_k}^{c_0}\otimes({\mathbf{q} }_c^b)^{-1}\\ s\overline {\mathbf{p} }_{b_k}^{c_0}&=s\overline {\mathbf{p} }_{c_k}^{c_0}-{\mathbf{R} }_{b_k}^{c_0}{\mathbf{p} }_c^b \end{align*}\tag{6} $$

其中 $s$ 将视觉结构与公制比例对齐的缩放参数。解决这个缩放参数是实现初始化成功的关键。

B. 视觉惯性对齐

视觉惯性对齐的示意图如图 3 所示。基本思想是将按比例放大的视觉结构与 IMU 预积分相匹配。

image-20220517085904500

图 3. 估计器初始化的视觉惯性对齐过程示意图。其基本思想是用IMU预积分匹配up-to-scale的视觉结构。

(1)陀螺仪偏置标定:考虑窗口中连续两帧 $b_k$ 和 $b_{k+1}$,我们从视觉 SfM 中得到旋转 ${\mathbf{q} }_{b_k}^{c_0}$ 和 ${\mathbf{q} }_{b_{k+1} }^{c_0}$ ,从IMU预积分得到的相对约束 $\hat{\gamma}^{b_k}_{b_{k+1} }$。我们对陀螺仪偏置求IMU预积分项的线性化,并最小化以下代价函数:

$$ \underset{\delta b_w}{\min}\sum_{k\in\mathcal{B} }\parallel { {\mathbf{q} }_{b_{k+1} }^{c_0} }^{-1}\otimes{\mathbf{q} }_{b_k}^{c_0}\otimes \gamma^{b_k}_{b_{k+1} } \parallel^2\\ \gamma^{b_k}_{b_{k+1} }\approx\hat{\gamma}^{b_k}_{b_{k+1} }\otimes\begin{bmatrix}1\\\frac{1}{2}\mathbf{J}^{\gamma}_{b_w}\delta b_{w} \end{bmatrix}\tag{7} $$

其中 $\mathcal{B}$ 是索引窗口中的所有帧。利用第四部分推导出的偏置雅可比矩阵,给出了 $\hat{\gamma}^{b_k}_{b_{k+1} }$ 对陀螺仪偏置的一阶近似。这样,我们得到了陀螺仪偏置 $b_w$ 的初始校准。然后我们用新的陀螺仪偏置重新传递所有的IMU预积分量 $ \hat{\alpha}^{b_k}_{b_{k+1} },\hat{\beta}^{b_k}_{b_{k+1} },\hat{\gamma}^{b_k}_{b_{k+1} }$。

(2)速度、重力向量和尺度初始化:在陀螺仪偏置初始化后,我们继续初始化导航的其他基本状态,即速度、重力向量和尺度:

$$ \mathcal{X}_I=[\mathbf{v}^{b_0}_{b_0},\mathbf{v}^{b_1}_{b_1},···,\mathbf{v}^{b_n}_{b_n},g^{c_0},s]\tag{8} $$

其中 $\mathbf{v}^{b_k}_{b_k}$ 是拍摄第 $k$ 张图像时本体坐标系中的速度,$g^{c_0}$ 是 $c_0$ 帧中的重力矢量,$s$ 将单目 SfM 缩放为公制单位。

考虑窗口中连续的两个帧 $b_k$ 和 $b_{k+1}$,我们有以下等式:

$$ \begin{align*} \alpha^{b_k}_{b_{k+1} }&=\mathbf{R}^{b_k}_{c_0}(s(\overline{\mathbf{p} }_{b_{k+1} }^{c_0}-\overline{\mathbf{p} }_{b_{k} }^{c_0})+\frac{1}{2}g^{c_0}\Delta t^2_k-\mathbf{R}_{b_k}^{c_0}\mathbf{v}^{b_k}_{b_k}\Delta t_k)\\ \beta^{b_k}_{b_{k+1} }&=\mathbf{R}^{b_k}_{c_0}(\mathbf{R}_{b_{k+1} }^{c_0}\mathbf{v}^{b_{k+1} }_{b_{k+1} }+g^{c_0}\Delta t_k-\mathbf{R}_{b_k}^{c_0}\mathbf{v}^{b_k}_{b_k}) \end{align*}\tag{9} $$

我们可以将 $(6)$ 和 $(9)$ 组合成以下线性测量模型:

$$ \hat{\mathbf{z} }^{b_k}_{b_{k+1} }= \begin{bmatrix} \hat{\alpha}^{b_k}_{b_{k+1} }-\mathbf{p}^b_c+\mathbf{R}^{b_k}_{c_0}\mathbf{R}^{c_0}_{b_{k+1} }\mathbf{p}^b_c\\ \hat{\beta}^{b_k}_{b_{k+1} } \end{bmatrix}= \mathbf{H}^{b_k}_{b_{k+1} }\mathcal{X}_I+\mathbf{n}^{b_k}_{b_{k+1} }\tag{10} $$

其中,

$$ \mathbf{H}^{b_k}_{b_{k+1} }= \begin{bmatrix} -\mathbf{I}\Delta t_k&0&\frac{1}{2}\mathbf{R}^{b_k}_{c_0}\Delta t_k^2&\mathbf{R}^{b_k}_{c_0}(\overline {\mathbf{p} }_{c_{k+1} }^{c_0}-\overline {\mathbf{p} }_{c_k}^{c_0})\\ -\mathbf{I}&\mathbf{R}^{b_k}_{c_0}\mathbf{R}^{c_0}_{b_{k+1} }&\mathbf{R}^{b_k}_{c_0}\Delta t_k&0 \end{bmatrix}\tag{11} $$

可以看出,$\mathbf{R}^{b_k}_{c_0},\ \mathbf{R}^{c_0}_{b_{k+1} },\ \overline {\mathbf{p} }_{c_k}^{c_0}$ 和 $\overline {\mathbf{p} }_{c_{k+1} }^{c_0}$ 是从up-to-scale 单目视觉SfM 中得到的。 $\Delta t_k$ 是两个连续帧之间的时间间隔。通过解决这个线性最小二乘问题:

$$ \underset{\mathcal{X}_I}{\min}\sum_{k\in\mathcal{B} }\parallel\hat{\mathbf{z} }^{b_k}_{b_{k+1} }-\mathbf{H}^{b_k}_{b_{k+1} }\mathcal{X}_I \parallel^2\tag{12} $$

我们可以获得窗口中每一帧本体坐标系的速度、视觉参考帧 $(·)^{c_0}$ 中的重力矢量以及比例参数。

(3)重力细化:通过约束量值,可以对原线性初始化步骤得到的重力向量进行细化。在大多数情况下,重力向量的大小是已知的。这导致重力向量只剩2个自由度。因此,我们在其切线空间上用两个变量重新参数化重力。我们的重力向量受到 $g(\overline{\hat{g} } + \delta g)$ 的扰动,$\delta g = w_1b_1 + w_2b_2$,其中 $g$ 是已知的重力大小,$\overline{\hat{g} }$ 是表示重力方向的单位向量。 $b_1$ 和 $b_2$ 是跨越切平面的两个正交基,如图 4 所示。 $w_1$ 和 $w_2$ 分别是对 $b_1$ 和 $b_2$ 的二维扰动。我们可以任意找到任何旋转切线空间的 $b_1$ 和 $b_2$ 集合。然后,我们用 $g(\overline{\hat{g} } + \delta g)$ 将 $g$ 代入 $(9)$,并与其他状态变量一起求解 维 $δg$。这个过程迭代了几次,直到 $\hat{g}$ 收敛。

image-20220521200811442

图 4. 2自由度重力扰动示意图。由于重力的大小是已知的,$g$ 位于半径 $g ≈ 9.81 m/s^2$ 的球体上。重力在当前估计附近受到扰动,因为 $g(\overline{\hat{g} } + \delta g)$ ,$\delta g = w_1b_1 + w_2b_2$, 其中 $b_1$ 和 $b_2$ 是跨越切线空间的两个正交基。

(3)完成初始化:对重力向量进行细化后,我们可以通过将重力旋转到 $z$ 轴得到世界坐标系和相机坐标系 $c_0$ 之间的旋转 $\mathbf{q}^w_{c_0}$。然后我们将所有变量从参考坐标系 $(·)^{c_0}$ 旋转到世界坐标系 $(·)^w$ 。本体坐标系速度也将旋转到世界坐标系。来自视觉 SfM 的平移分量将被缩放为公制单位。至此,初始化过程完成,所有这些度量值将被馈送到紧密耦合的单目 VIO。

六、紧耦合单目VIO

在估计器初始化之后,我们继续使用基于滑动窗口的紧密耦合单目 VIO,以实现高精度和稳健的状态估计。滑动窗口公式的说明如图 5 所示。

image-20220516150129486

图 5. 带有重定位的滑动窗口单目 VIO 示意图。滑动窗口中存在多个相机位姿、IMU 测量和视觉测量。它是与 IMU、视觉和闭环检测紧密耦合的公式。

A.公式

滑动窗口中的全状态向量定义为:

$$ \begin{align*} \mathcal{X}&=[\mathbf{x}_0,\mathbf{x}_1,···,\mathbf{x}_n,\mathbf{x}^b_c,\lambda_0,\lambda_1,···,\lambda_m]\\ \mathbf{x}_k&=[\mathbf{p}^w_{b_k},\mathbf{v}^w_{b_k},\mathbf{q}^w_{b_k},\mathbf{b}_a,\mathbf{b}_g],k\in[0,n]\\ \mathbf{x}^b_c&=[\mathbf{p}^b_c,\mathbf{q}^b_c] \end{align*}\tag{13} $$

其中 $\mathbf{x}_k$ 是捕获第 $k$ 个图像时的 IMU 状态。它包含 IMU 在世界坐标系中的位置、速度和方向,以及 IMU 主体框架中的加速度偏差和陀螺仪偏差。 $n$ 是关键帧的总数,$m$ 是滑动窗口中的特征总数。 $λ_l$ 是第 $l$ 个特征与其第一次观察的逆深度。

我们使用 BA 公式。我们最小化所有测量残差的先验和马氏距离之和,以获得最大后验估计为:

$$ \underset{\mathcal{X} }{\min} \left\{ \parallel \mathbf{r}_p-\mathbf{H}_p\mathcal{X} \parallel^2+ \sum_{k\in\mathcal{B} }\parallel\mathbf{r}_{\mathcal{B} }(\hat{\mathbf{z} }^{b_k}_{b_{k+1} },\mathcal{X}) \parallel^2_{\mathbf{P}^{b_k}_{b_{k+1} } } +\sum_{(l,j)\in\mathcal{C} }\rho(\parallel\mathbf{r}_\mathcal{C}(\hat{\mathbf{z} }^{c_j}_{l},\mathcal{X}) \parallel^2_{\mathbf{P}^{c_j}_l }) \right\}\tag{14} $$

其中 Huber 范数 [37] 定义为:

$$ \rho(s)= \left\{ \begin{array}{l} s\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ s\leq1\\ 2\sqrt{s}-1\ \ \ \ \ \ s>1\\ \end{array} \right. \tag{15} $$

其中 $\mathbf{r}_{\mathcal{B} }(\hat{\mathbf{z} }^{b_k}_{b_{k+1} },\mathcal{X})$ 和 $\mathbf{r}_\mathcal{C}(\hat{\mathbf{z} }^{c_j}_{l},\mathcal{X}) $ 分别是 IMU 和视觉测量的残差。剩余残差的详细定义将在第 VI-B 和 VI-C 节中介绍。 $\mathcal{B}$ 是所有 IMU 测量值的集合,$\mathcal{C}$ 是在当前滑动窗口中至少观察到两次的特征集合。 $\{\mathbf{r}_p, \mathbf{H}_p\}$ 是来自边缘化的先验信息。 Ceres 求解器 [38] 用于解决这个非线性问题。

B. IMU 测量残差

考虑滑动窗口中两个连续帧 $b_k$ 和 $b_{k+1}$ 内的 IMU 测量值,预积分 IMU 测量值的残差可以定义为:

$$ \mathbf{r}_{\mathcal{B} }(\hat{\mathbf{z} }^{b_k}_{b_{k+1} },\mathcal{X})= \begin{bmatrix} \delta {\alpha}^{b_k}_{b_{k+1} }\\\delta {\beta}^{b_k}_{b_{k+1} }\\ \delta {\theta}^{b_k}_{b_{k+1} }\\ \delta {b}_{a}\\\delta {b}_{g} \end{bmatrix}= \begin{align*} \begin{bmatrix} \mathbf{R}^{b_k}_w(\mathbf{p}^w_{b_{k+1} }-\mathbf{p}^w_{b_k}+ \frac{1}{2}g^w\Delta t_k^2-\mathbf{v}^w_{b_k}\Delta t_k)-\hat{\alpha}^{b_k}_{b_{k+1} }\\ \mathbf{R}^{b_k}_w(\mathbf{v}^w_{b_{k+1} }-\mathbf{v}^w_{b_k}+g^w\Delta t_k)-\hat{\beta}^{b_k}_{b_{k+1} }\\ 2[{\mathbf{q}^{b_k}_w}^{-1}\otimes\mathbf{q}^w_{b_{k+1} } \otimes(\hat{\gamma}^{b_k}_{b_{k+1} })^{-1}]_{xyz}\\ \mathbf{b}_{ab_{k+1} }-\mathbf{b}_{ab_k}\\ \mathbf{b}_{wb_{k+1} }-\mathbf{b}_{wb_k}\\ \end{bmatrix} \end{align*}\tag{16} $$

其中,$[·]_{xyz}$ 是提取四元数 $\mathbf{q}$ 的向量部分,以进行误差状态表示。$\delta {\theta}^{b_k}_{b_{k+1} }$ 是四元数的三维误差状态表示。 $[ \hat{\alpha}^{b_k}_{b_{k+1} },\hat{\beta}^{b_k}_{b_{k+1} },\hat{\gamma}^{b_k}_{b_{k+1} }]$ 是在两个连续图像帧的间隔时间内使用仅包含噪声的加速度计和陀螺仪测量值预积分的IMU测量项。加速度计和陀螺仪偏置也包括在在线校正的残差项中。

C. 视觉测量残差

与在广义图像平面上定义重投影误差的传统针孔相机模型相比,我们在单位球面上定义相机测量残差。几乎所有类型的相机(包括广角相机、鱼眼相机或全向相机)的光学器件都可以建模为连接单位球体表面的单位射线。考虑在第 $i$ 个图像中首先观察到的第 $l$ 个特征,第 $j$ 个图像中特征观察的残差定义为:

$$ \begin{align*} \mathbf{r}_\mathcal{C}(\hat{\mathbf{z} }^{c_j}_{l},\mathcal{X})&=[\mathbf{b}_1,\mathbf{b}_2]^T\cdot(\hat{\overline{\mathcal{P} } }^{c_j}_l-\frac{\mathcal{P}^{c_j}_l}{\parallel\mathcal{P}^{c_j}_l \parallel^2})\\ \hat{\overline{\mathcal{P} } }^{c_j}_l&=\pi^{-1}_c \left(\begin{bmatrix}\hat{u}^{c_j}_l\\\hat{v}^{c_j}_l \end{bmatrix}\right)\\ \mathcal{P}^{c_j}_l &=\mathbf{R}^c_b \left(\mathbf{R}^{b_j}_w \left(\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l}\pi^{-1}_c \left(\begin{bmatrix}\hat{u}^{c_i}_l\\\hat{v}^{c_i}_l \end{bmatrix} \right)+\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right)-\mathbf{p}^{b}_{c} \right) \end{align*}\tag{17} $$

其中 $[\hat{u}^{c_i}_l,\hat{v}^{c_i}_l]$ 是第 $i$ 个图像中发生的第 $l$个特征的第一次观察。 $[\hat{u}^{c_j}_l,\hat{v}^{c_j}_l]$ 是对第 j 个图像中相同特征的观察。 $\pi^{-1}_c$ 是反投影函数,它使用相机内在参数将像素位置转换为单位向量。由于视觉残差的自由度为 2,我们将残差向量投影到切平面上。 $b_1$ 和 $b_2$ 是两个任意选择的正交基,它们跨越 $\hat{\overline{\mathcal{P} } }^{c_j}_l$ 的切平面,如图 6 所示。在 $(14)$ 中使用的方差 $\mathbf{P}^{c_j}_l$ 也从像素坐标传播到单位球体上.

image-20220521200846809

图 6. 单位球面上的视觉残差图。 $\hat{\overline{\mathcal{P} } }^{c_j}_l$ 是在第 $j$ 帧中观察第 $l$ 个特征的单位向量。$\mathcal{P}^{c_j}_l$ 是通过将其在第 $i$ 帧中的第一个观察值转换为第 $j$ 帧来预测的单位球体上的特征测量。残差定义在 $\hat{\overline{\mathcal{P} } }^{c_j}_l$ 的切平面上

D. 边缘化

为了限制我们基于优化的 VIO 的计算复杂性,加入了边缘化。我们从滑动窗口中选择性地边缘化 IMU 状态 $\mathbf{x}_k$ 和特征 $λ_l$,同时将对应于边缘化状态的测量值转换为先验。

如图 7 所示,当第二个最新帧是关键帧时,它将停留在窗口中,并且最旧的帧与其相应的测量值一起被边缘化。否则,如果第二个最新帧是非关键帧,我们会抛出视觉测量并保留连接到该非关键帧的 IMU 测量。为了保持系统的稀疏性,我们不会边缘化所有非关键帧的测量值。我们的边缘化方案旨在将空间分离的关键帧保留在窗口中。这确保了特征三角测量有足够的视差,并最大限度地提高了在大激励下保持加速度计测量的概率。

image-20220521200953625

图 7. 我们的边缘化策略说明。如果第二个最新帧是关键帧,我们会将其保留在窗口中,并边缘化最旧的帧及其相应的视觉和惯性测量值。边缘化的测量值变成了先验值。如果第二个最新帧不是关键帧,我们将简单地删除该帧及其所有相应的视觉测量。但是,对于非关键帧会保留预积分惯性测量值,并且预积分过程会继续到下一帧。

边缘化是利用Schur补[39]进行的。我们基于与移除状态相关的所有边缘化测量值构造一个新的先验。新的先验项被添加到现有的先验项中。

我们确实注意到,边缘化导致了线性化点的早期固定,这可能导致次优估计结果。然而,由于小型漂移对于VIO来说是可以接受的,我们认为边缘化所造成的负面效果并不重要。

E. 摄像机速率状态估计的纯运动视觉惯性优化

对于计算能力较低的设备如手机,由于对非线性优化的计算要求很高,紧耦合单目VIO无法实现摄像机速率输出。为此,我们采用了一种轻量级的纯运动视觉惯性优化,以提升状态估计速率到相机速率(30Hz)。

纯运动单目视觉惯性BA的代价函数与 $(14)$ 中单目VIO的代价函数相同。然而我们只对固定数量的最新IMU状态的姿态和速度进行了优化,而不是对滑动窗口中的所有状态进行优化。我们将特征深度、外部参数、偏置和旧的IMU状态这些不希望优化的状态作为常量来处理。我们使用所有的视觉和惯性测量来进行纯运动的优化。这导致了比单帧PnP方法更平滑的状态估计。图8显示了提出方法的插图。与在最先进的嵌入式计算机上可能导致超过 $50ms$ 的完全紧耦合单目VIO不同,这种纯运动的视觉惯性优化只需大约 $5ms$ 来计算。这使得低延迟的相机频率进行位姿估计对无人机和AR应用特别有利。

image-20220521201107818

图 8. 相机速率输出的仅运动优化图示。

F. IMU前向传递以达到IMU速率状态估计

IMU 测量的速度比视觉测量高得多。尽管我们的 VIO 的频率受到图像捕获频率的限制,但我们仍然可以使用最近的 IMU 测量值直接传播最新的 VIO 估计值,以实现 IMU 速率性能。高频状态估计可以用作闭环闭合的状态反馈。 IX-C 节介绍了利用该 IMU 速率状态估计的自主飞行实验。

七、重定位

我们的滑动窗口和边缘化方案限制了计算复杂度,但它也为系统引入了累积漂移。为了消除漂移,提出了一种与单目 VIO 无缝集成的紧密耦合重定位模块。重新定位过程从一个闭环检测模块开始,该模块识别已经访问过的地方。然后建立闭环候选和当前帧之间的特征级连接。这些特征对应紧密集成到单目 VIO 模块中,从而以最少的计算量实现无漂移状态估计。多个特征的多次观察直接用于重定位,从而获得更高的精度和更好的状态估计平滑度。图 9(a) 显示了重定位过程的图解说明。

image-20220521201129052

(a) 重新定位程序。它从仅 VIO 的姿势估计开始(蓝色)。记录过去的状态(绿色)。如果检测到最新关键帧的闭环(参见第 VII-A 节),如第二个图中的红线所示,则发生了重新定位。请注意,由于使用特征级对应进行重定位,我们能够合并来自多个过去关键帧的闭环约束(参见第 VII-C 节),如最后三个图中所示。

image-20220521201235377

(b) 全局位姿图优化。当关键帧从滑动窗口边缘化时,它被添加到姿势图中。如果此关键帧与任何其他过去的关键帧之间存在循环,则表示为 4-DOF 相对刚体变换的闭环约束也将添加到位姿图中。位姿图在一个单独的线程中使用所有相对位姿约束(参见第 VIII-C 节)进行优化,并且重新定位模块始终相对于最新的位姿图配置运行。

图 9. 说明重定位和位姿图优化过程的图表。

A. 回环检测

我们利用 DBoW2 [29],一种最先进的词袋位置识别方法,用于回环检测。除了用于单目 VIO 的角点特征外,BRIEF 描述符 [40] 还检测和描述了 500 个角点。

额外的角点特征用于在循环检测中实现更好的召回率。描述符被视为视觉词来查询视觉数据库。 DBoW2 在时间和几何一致性检查后返回闭环候选。我们保留所有用于特征检索的简要描述符,但丢弃原始图像以减少内存消耗。

B. 特征检索

当检测到闭环时,通过检索特征对应关系来建立局部滑动窗口和闭环候选之间的连接。通过简要描述符匹配找到对应关系。描述符匹配可能会导致一些错误的匹配对。为此,我们使用两步几何异常值剔除,如图 10 所示。

image-20220521201302610

图 10. 闭环期间特征检索的描述符匹配和异常值去除。 (a) 简要描述符匹配结果。 (b) 第一步:2-D-2-D 异常值剔除结果。 (c) 第二步:3-D-2-D 异常值剔除结果。

1、2D-2D:RANSAC[33]的基本矩阵检验。我们利用当前图像中检索到的特征的二维观测和回环候选图像进行基本矩阵检验。
2、3D-2D:RANSAC的 PnP 检验。基于特征在局部滑动窗口中已知的三维位置,以及回环候选图像中的二维观测,进行 PnP 检验。

在异常值剔除后,我们将此候选者视为正确的闭环检测并执行重新定位。

C. 紧耦合重定位

重新定位过程有效地将当前滑动窗口与过去的姿势对齐。在重新定位期间,我们将所有闭环帧的姿势视为常数。我们使用所有 IMU 测量、局部视觉测量和检索到的特征对应来共同优化滑动窗口。我们可以很容易地编写由闭环框架 $v$ 观察到的检索特征的视觉测量模型,使其与 VIO 中的视觉测量相同,如 $(17)$。唯一的区别是,取自 位姿图(参见第 VIII 节),或直接取自 过去的里程计输出(如果这是第一次重新定位)的闭环帧的位姿 $(\hat{\mathbf{q} }^w_v, \hat{\mathbf{p} }^w_v)$ 被处理作为常数。为此,我们可以在 $(14)$ 中稍微修改非线性代价函数,增加回环项:

$$ \begin{align*} \underset{\mathcal{X} }{\min} \parallel \mathbf{r}_p-\mathbf{H}_p\mathcal{X} \parallel^2 &+\sum_{k\in\mathcal{B} }\parallel\mathbf{r}_{\mathcal{B} }(\hat{\mathbf{z} }^{b_k}_{b_{k+1} },\mathcal{X}) \parallel^2_{\mathbf{P}^{b_k}_{b_{k+1} } } +\sum_{(l,j)\in\mathcal{C} }\rho(\parallel\mathbf{r}_\mathcal{C}(\hat{\mathbf{z} }^{c_j}_{l},\mathcal{X}) \parallel^2_{\mathbf{P}^{c_j}_l })\\ &+\underbrace{ \sum_{(l,v)\in\mathcal{L} } \rho(\parallel\mathbf{r}_\mathcal{C}(\hat{\mathbf{z} }^{v}_{l},\mathcal{X},\hat{\mathbf{q} }^w_v, \hat{\mathbf{p} }^w_v) \parallel^2_{\mathbf{P}^{c_v}_l }) }_{闭环帧中的重投影误差} \end{align*}\tag{18} $$

其中 $\mathcal{L}$ 是闭环帧中检索到的特征的观察集。 $(l, v) $表示在回环帧 $v$ 中观察到的第 $l$ 个特征。请注意,尽管代价函数与 $(14)$ 略有不同,但要解决的状态的维数保持不变,因为回环帧的位姿被视为常数。当使用当前滑动窗口建立多个闭环时,我们同时使用来自所有帧的所有闭环特征对应进行优化。这为重新定位提供了多视图约束,从而获得更高的准确性和更好的平滑度。重新定位后保持一致性的全局优化将在第 VIII 节中讨论。

八、 全局位姿图优化

重新定位后,开发了额外的位姿图优化步骤,以确保将一组过去的位姿注册到全局一致的配置中。

A. 四个累积漂移方向

得益于重力的惯性测量,在 VINS 中可以完全观察到滚动角 (roll) 和俯仰角 (pitch)。如图 11 所示,随着物体的移动,3-D 位置和旋转相对于参考系发生了变化。但是,我们可以通过重力矢量确定水平面,这意味着我们始终观察绝对的滚动角 (roll) 和俯仰角 (pitch)。因此,滚动角 (roll) 和俯仰角 (pitch) 是世界坐标系中的绝对状态,而 $x、y、z$ 和 yaw 是相对于参考坐标系的相对估计。累积漂移仅发生在 $x、y、z$ 和偏航角中。为了充分利用有效信息并有效地纠正漂移,我们修复了无漂移滚动角 (roll) 和俯仰角 (pitch),并且仅在 4-DOF 中执行位姿图优化。

image-20220521201318125

图 11. 四个漂移方向示意图。随着物体的移动,$x、y、z$ 和偏航角相对于参考系发生了相对变化。绝对滚动角 (roll) 和俯仰角 (pitch) 可以由重力矢量的水平面确定。

B. 将关键帧添加到姿势图中

在 VIO 过程之后,关键帧被添加到位姿图中。每个关键帧都作为位姿图中的一个顶点,它通过两种类型的边与其他顶点连接,如图 12 所示。

image-20220521201330728

图 12. 姿态图说明。关键帧作为位姿图中的一个顶点,它通过顺序边和循环边连接其他顶点。每条边代表相对平移和相对偏航 (yaw)。

1)顺序边(Sequential Edge):一个关键帧为其先前的关键帧建立了几个顺序边。顺序边表示两个关键帧之间的相对变换,直接取自 VIO。考虑到关键帧 $i$ 及其先前的关键帧 $j$ 之一,顺序边仅包含相对位置 $\hat{\mathbf{p} }^i_{ij}$ 和偏航角 $\hat{\psi}_{ij}$ 。

$$ \begin{align*} \hat{\mathbf{p} }^i_{ij}&={\hat{\mathbf{R} }^w_{i} }^{-1}(\hat{\mathbf{p} }^w_{j}-\hat{\mathbf{p} }^w_{i})\\ \hat{\psi}_{ij}&=\hat{\psi}_{j}-\hat{\psi}_{i} \end{align*}\tag{19} $$

2)回环边(Loop Closure Edge):如果新边缘化的关键帧有一个回环连接,它将与回环帧通过一个回环边在位姿图图中连接。同样,闭环边缘只包含与 $(19)$ 相同定义的四自由度相对位姿变换。回环边的值由重定位结果得出。

C. 四自由度位姿图优化

我们将帧 $i$ 和 $j$ 之间的边缘残差最小定义为:

$$ \mathbf{r}_{i,j}(\mathbf{p}^w_i,\psi_i,\mathbf{p}^w_j,\psi_j)= \begin{bmatrix} \mathbf{R}(\hat{\phi_i},\hat{\theta}_i,\hat{\psi}_i)^{-1}(\mathbf{p}^w_j-\mathbf{p}^w_i)-\hat{\mathbf{p} }^i_{ij}\\ \psi_j-\psi_i-\hat{\psi}_{ij} \end{bmatrix}\tag{20} $$

其中 $\hat{\phi_i}$ 和 $\hat{\theta}_i$ 是滚转角和俯仰角的固定估计值,它们是从单目 VIO 获得的。

通过最小化以下代价函数来优化顺序边和闭环边的整个图:

$$ \underset{\mathbf{p},\psi}{\,min}\left\{ \sum_{(i,j)\in\mathcal{S} }\parallel\mathbf{r}_{i,j}\parallel^2 +\sum_{(i,j)\in\mathcal{L} }\rho(\parallel\mathbf{r}_{i,j}\parallel^2) \right\}\tag{21} $$

其中 $\mathcal{S}$ 是所有连续边的集合, $\mathcal{L}$ 是所有闭环边的集合。尽管紧耦合的重定位已经有助于消除错误的闭合回环,但我们添加了另一个 Huber 范数 $ρ(·) $ 以进一步减少任何可能的错误闭环的影响。相比之下,我们没有对顺序边使用任何稳健的规范,因为这些边是从 VIO 中提取的,它已经包含足够的异常值拒绝机制。

位姿图优化和重新定位(参见第 VII-C 节)在两个单独的线程中异步运行。这使得只要可用,就可以立即使用最优化的位姿图进行重新定位。同样,即使当前的位姿图优化尚未完成,仍然可以使用现有的位姿图配置进行重新定位。该过程如图 9(b) 所示。

D. 位姿图合并

位姿图不仅可以优化当前地图,还可以将当前地图与之前构建的地图合并。如果我们加载了之前构建的地图并检测到两个地图之间的闭环连接,我们可以将它们合并在一起。由于所有边都是相对约束,因此位姿图优化通过循环连接自动将两个地图合并在一起。如图 13 所示,当前地图被循环边缘拉入前一个地图。每个顶点和每条边都是相对变量,因此,我们只需要固定位姿图中的第一个顶点。

image-20220521201348827

图 13. 地图合并示意图。黄色的图是之前建好的地图。蓝色图为当前地图。根据闭环连接合并两个地图。

E. 位姿图保存

我们的位姿图的结构非常简单。我们只需要保存顶点和边,以及每个关键帧(顶点)的描述符。原始图像被丢弃以减少内存消耗。具体来说,我们为第 $i$ 个关键帧保存的状态是:

$$ [i,\hat{\mathbf{p} }^w_i,\hat{\mathbf{q} }^w_i,v,\hat{\mathbf{p} }^i_{iv},\hat{\psi}_{iv},\mathbf{D}(u,v,des)]\tag{22} $$

其中 $i$ 是帧索引,$\hat{\mathbf{p} }^w_i$ 和 $\hat{\mathbf{q} }^w_i$ 分别是来自 VIO 的位置和方向。如果这个帧有一个闭环帧,$v$ 是闭环帧的索引。 $\hat{\mathbf{p} }^i_{iv}$ 和 $\hat{\psi}_{iv}$ 是这两帧之间通过重定位得到的相对位置和偏航角。$\mathbf{D}(u,v,des)$ 是特征集。每个特征都包含二维位置及其简要描述符。

F 位姿图加载

我们使用相同的保存格式来加载关键帧。每个关键帧都是位姿图中的一个顶点。顶点的初始位姿是 $\hat{\mathbf{p} }^w_i$ 和 $\hat{\mathbf{q} }^w_i$ 。闭环边缘直接由回环信息 $\hat{\mathbf{p} }^i_{iv}$ 和 $\hat{\psi}_{iv}$ 建立。每个关键帧与其相邻关键帧建立几个连续的边,如 $(19)$。加载位姿图后,我们立即执行一次全局 4自由度位姿图。位姿图保存和加载的速度与位姿图的大小呈线性相关。

九、实验

十、结论和未来的工作

在本文中,我们提出了一种鲁棒且通用的单目视觉惯性估计器。我们的方法在 IMU 预积分、估计器初始化、在线外部校准、紧耦合 VIO、重定位和高效全局优化方面具有最先进和新颖的解决方案。通过与其他最先进的开源实现进行比较,我们展示了卓越的性能。为了社区的利益,我们开源了 PC 和 iOS 实现。

尽管基于特征的 VINS 估计器已经达到了实际部署的成熟度,但我们仍然看到了未来研究的许多方向。根据运动和环境的不同,单目 VINS 可能会达到弱可观测甚至退化的条件。我们对评估单目 VINS 可观察性特性的在线方法以及在线生成运动计划以恢复可观察性感兴趣。另一个研究方向涉及单目 VINS 在各种消费设备上的大规模部署,例如 Android 手机。该应用需要在线校准几乎所有传感器的内部和外部参数,以及校准质量的在线识别。最后,我们有兴趣根据单目 VINS 的结果生成密集地图。我们在无人机导航中应用的单目视觉惯性稠密建图的第一个结果在 [47] 中提出。但是,仍然需要进行广泛的研究以进一步提高系统的准确性和鲁棒性。

附录:基于四元数的 IMU 预积分

给定对应于图像帧 $b_k$ 和 $b_{k+1}$ 的两个时刻,位置、速度和方向状态可以在世界帧中的时间间隔 $[t_k, t_{k+1}]$ 期间通过惯性测量传播:

$$ \begin{align*} \mathbf{p}^w_{b_{k+1} }&=\mathbf{p}^w_{b_k}+\mathbf{v}^w_{b_k}\Delta t_k+\iint_{t\in[t_k, t_{k+1}]}(\mathbf{R}^w_t(\hat{a}_t-b_{a_t}-n_a)-g^w)dt^2\\ \mathbf{v}^w_{b_{k+1} }&=\mathbf{v}^w_{b_k}+\int_{t\in[t_k, t_{k+1}]}(\mathbf{R}^w_t(\hat{a}_t-b_{a_t}-n_a)-g^w)dt\\ \mathbf{q}^w_{b_{k+1} }&=\mathbf{q}^w_{b_k}\otimes\int_{t\in[t_k, t_{k+1}]} \frac{1}{2}\Omega(\hat{w}_t-b_{w_t}-n_w)\mathbf{q}^{b_k}_tdt \end{align*}\tag{23} $$

其中,

$$ \Omega(w)= \begin{bmatrix} -w^{\land}&w\\-w^T&0 \end{bmatrix}, w^{\land}=\begin{bmatrix} 0&-w_z&w_y\\ w_z&0&-w_x\\ -w_y&w_x&0 \end{bmatrix}\tag{24} $$

$\Delta t_k$ 是时间间隔 $[t_k, t_{k+1}]$ 之间的持续时间。

可以看出,IMU状态传播需要帧 $b_k$ 的旋转、位置和速度。当这些起始状态发生变化时,我们需要重新传播 IMU 测量值。特别是在基于优化的算法中,每次调整姿势时,我们都需要在它们之间重新传播 IMU 测量值。这种传播策略在计算上要求很高。为了避免重复传播,我们采用预积分算法。

将参考坐标系从世界坐标系变为局部坐标系 $b_k$ 后,我们只能对与线加速度$\hat a$ 和角速度 $\hat w$ 相关的部分进行预积分如下:

$$ \begin{align*} \mathbf{R}^{b_k}_w\mathbf{p}^w_{b_{k+1} }&=\mathbf{R}^{b_k}_w(\mathbf{p}^w_{b_k}+\mathbf{v}^w_{b_k}\Delta t_k- \frac{1}{2}g^w\Delta t_k^2)+\alpha^{b_k}_{b_{k+1} }\\ \mathbf{R}^{b_k}_w\mathbf{v}^w_{b_{k+1} }&= \mathbf{R}^{b_k}_w(\mathbf{v}^w_{b_k}-g^w\Delta t_k)+\beta^{b_k}_{b_{k+1} }\\ \mathbf{q}^{b_k}_w\mathbf{q}^w_{b_{k+1} }&=\gamma^{b_k}_{b_{k+1} } \end{align*}\tag{25} $$

其中,

$$ \begin{align*} \alpha^{b_k}_{b_{k+1} }&=\iint_{t\in[t_k, t_{k+1}]}\mathbf{R}^{b_k}_t(\hat{a}_t-b_{a_t}-n_a)dt^2\\ \beta ^{b_k}_{b_{k+1} }&=\int_{t\in[t_k, t_{k+1}]}\mathbf{R}^{b_k}_t(\hat{a}_t-b_{a_t}-n_a)dt\\ \gamma^{b_k}_{b_{k+1} }&= \int_{t\in[t_k, t_{k+1}]} \frac{1}{2}\Omega(\hat{w}_t-b_{w_t}-n_w)\gamma^{b_k}_tdt \end{align*}\tag{26} $$

可以看出,预积分项 $(26)$ 可以仅通过以 $b_k$ 作为参考系的 IMU 测量来获得。 $\alpha^{b_k}_{b_{k+1} },\beta^{b_k}_{b_{k+1} },\gamma^{b_k}_{b_{k+1} }$ 仅与IMU偏差有关,与 $b_k$ 和 $b_{k+1}$ 中的其他状态无关。当偏差的估计发生变化时,如果变化很小,我们通过它们对偏差的一阶近似来调整 $\alpha^{b_k}_{b_{k+1} },\beta^{b_k}_{b_{k+1} },\gamma^{b_k}_{b_{k+1} }$ ,否则我们进行重新传播。这种策略为基于优化的算法节省了大量的计算资源,因为我们不需要重复传播 IMU 测量。

对于离散时间的实现,可以应用不同的数值积分方法,例如欧拉、中点、RK4 积分。这里选择欧拉积分来演示过程,以便于理解(我们在实现代码中使用了中点积分)。

一开始,$\alpha^{b_k}_{b_k},\beta^{b_k}_{b_k}$ 为0,$\gamma^{b_k}_{b_k}$ 为单位四元数。 $(26)$ 中的 $\alpha、\beta、\gamma$ 的平均值按如下方式逐步传播。请注意,增加的噪声项 $n_a、n_w$ 是未知的,在实现中被视为零。这得到了预积分项的估计值,用 $\hat{(·)}$ 标记:

$$ \begin{align*} \hat{\alpha}^{b_k}_{i+1}&=\hat{\alpha}^{b_k}_{i}+\hat{\beta}^{b_k}_{i}\delta t^2+\frac{1}{2}\mathbf{R}(\hat{\gamma}^{b_k}_{i})(\hat{a}_i-b_{a_i})\delta t^2\\ \hat{\beta}^{b_k}_{i+1}&=\hat{\beta}^{b_k}_{i}+\mathbf{R}(\hat{\gamma}^{b_k}_{i})(\hat{a}_i-b_{a_i})\delta t\\ \hat{\gamma}^{b_k}_{i+1}&=\hat{\gamma}^{b_k}_{i}\otimes \begin{bmatrix}1\\\frac{1}{2}(\hat{w}_i-b_{w_i})\delta t\end{bmatrix} \\ \end{align*}\tag{27} $$

$i$ 是在 $[t_k, t_{k+1}]$ 中IMU测量值对应的离散时刻。 $\delta t$ 是两次 IMU 测量 $i$ 和 $i + 1$ 之间的时间间隔。

然后我们处理协方差传播。由于四维旋转四元数 $\gamma^{b_k}_t$ 被过度参数化,我们将其误差项定义为围绕其均值的扰动:

$$ \gamma^{b_k}_t\approx\hat{\gamma}^{b_k}_t\otimes\begin{bmatrix}1\\\frac{1}{2}\delta \theta^{b_k}_t\end{bmatrix}\tag{28} $$

其中 $\theta^{b_k}_t$ 是三维小扰动。

我们可以推导出 $(26)$ 的误差项的连续时间线性化方程:

$$ \begin{align*} \begin{bmatrix} \delta \dot{\alpha}^{b_k}_t\\\delta \dot{\beta}^{b_k}_t\\ \delta \dot{\theta}^{b_k}_t\\\delta \dot{b}_{a_t}\\\delta \dot{b}_{w_t}\\ \end{bmatrix}&= \begin{bmatrix} 0&\mathbf{I}&0&0&0\\ 0&0&-\mathbf{R}^{b_k}_t[\hat{a}_t-b_{a_t}]^{\land}&-\mathbf{R}^{b_k}_t&0\\ 0&0&-[\hat{w}_t-b_{w_t}]^{\land}&0&-\mathbf{I}\\ 0&0&0&0&0\\ 0&0&0&0&0\\ \end{bmatrix} \begin{bmatrix} \delta {\alpha}^{b_k}_t\\\delta {\beta}^{b_k}_t\\ \delta {\theta}^{b_k}_t\\\delta {b}_{a_t}\\\delta {b}_{w_t}\\ \end{bmatrix}+ \begin{bmatrix} 0&0&0&0\\ -\mathbf{R}^{b_k}_t&0&0&0\\ 0&-\mathbf{I}&0&0\\ 0&0&\mathbf{I}&0\\ 0&0&0&\mathbf{I}\\ \end{bmatrix} \begin{bmatrix}\mathbf{n}_a\\\mathbf{n}_w\\\mathbf{n}_{b_a}\\\mathbf{n}_{b_w}\end{bmatrix}\\ &=\mathbf{F}_t\delta\mathbf{z}^{b_k}_t+\mathbf{G}_t\mathbf{n}_t \end{align*}\tag{29} $$

在零阶保持离散化中,$\mathbf{F}_t$ 在积分期间是恒定的,因此对于给定的时间步长 $\delta t$,$\mathbf{F}_d=\exp(\mathbf{F}_t\delta t)$ 。通过展开指数级数并省略高阶项,我们得到 $\mathbf{F}_d \approx I + \mathbf{F}_t\delta t$。使用连续时间噪声协方差矩阵$ \mathbf{Q}_t = diag(\sigma^2_a, \sigma^2_w, \sigma^2_{b_a}, \sigma^2_{b_w})$,离散时间噪声协方差矩阵计算为:

$$ \begin{align*} \mathbf{Q}_d&=\int^{\delta t}_0 \mathbf{F}_d(\tau)\mathbf{G}_t\mathbf{Q}_t\mathbf{G}_t^T\mathbf{F}_d(\tau)^T\\ &=\delta t\mathbf{F}_d\mathbf{G}_t\mathbf{Q}_t\mathbf{G}_t^T\mathbf{F}_d^T\\ &\approx\delta t\mathbf{G}_t\mathbf{Q}_t\mathbf{G}_t^T\\ \end{align*}\tag{30} $$

协方差 $\mathbf{P}^{b_k}_{b_{k+1} }$ 可以通过初始协方差 $\mathbf{P}^{b_k}_{b_k}= 0$ 传播如下:

$$ \mathbf{P}^{b_k}_{t+\delta t}= (\mathbf{I}+\mathbf{F}_t\delta t)\mathbf{P}^{b_k}_{t}(\mathbf{I}+\mathbf{F}_t\delta t)^T+ \delta t\mathbf{G}_t\mathbf{Q}_t\mathbf{G}_t^T,t\in[k,k+1]\tag{31} $$

同时,一阶雅可比矩阵也可以通过初始雅可比 $\mathbf{J}_{b_k}=\mathbf{I}$ 进行递归计算:

$$ \mathbf{J}_{t+\delta t}=(\mathbf{I}+\mathbf{F}_t\delta t)\mathbf{J}_t,t\in[k,k+1]\tag{32} $$

使用这个递归公式,我们得到协方差矩阵 $\mathbf{P}^{b_k}_{b_{k+1} }$ 和雅可比 $\mathbf{J}_{b_{k+1} }$ 。 $\alpha^{b_k}_{b_{k+1} },\beta^{b_k}_{b_{k+1} },\gamma^{b_k}_{b_{k+1} }$ 关于偏差的一阶近似可以写成:

$$ \begin{align*} \alpha^{b_k}_{b_{k+1} }&\approx \hat{\alpha}^{b_k}_{b_{k+1} }+\mathbf{J}^{\alpha}_{b_a}\delta b_{a_k}+\mathbf{J}^{\alpha}_{b_w}\delta b_{w_k} \\ \beta^{b_k}_{b_{k+1} }&\approx \hat{\beta}^{b_k}_{b_{k+1} }+\mathbf{J}^{\beta}_{b_a}\delta b_{a_k}+\mathbf{J}^{\beta}_{b_w}\delta b_{w_k} \\ \gamma^{b_k}_{b_{k+1} }&\approx \hat{\gamma}^{b_k}_{b_{k+1} }\otimes \begin{bmatrix}1\\\frac{1}{2}\mathbf{J}^{\gamma}_{b_w}\delta b_{w_k} \end{bmatrix} \end{align*}\tag{33} $$

其中 $\mathbf{J}^{\alpha}_{b_a}$ 和 是 $\mathbf{J}^{\alpha}_{b_{k+1} }$ 中的子块矩阵,其位置对应于 $\frac{\delta\alpha^{b_k}_{b_{k+1} } }{\delta b_{a_k} }$ 。 $\mathbf{J}^{\alpha}_{b_w}、\mathbf{J}^{\beta}_{b_a}、\mathbf{J}^{\beta}_{b_w}、\mathbf{J}^{\gamma}_{b_w}$ 也使用相同的含义。当偏差的估计发生轻微变化时,我们使用 $(33)$ 来近似地校正预积分结果,而不是重新传播。

现在我们可以写下 IMU 测量模型及其对应的协方差 $\mathbf{P}^{b_k}_{b_{k+1} }$ :

$$ \begin{align*} \begin{bmatrix} \hat{\alpha}^{b_k}_{b_{k+1} }\\\hat{\beta}^{b_k}_{b_{k+1} }\\\hat{\gamma}^{b_k}_{b_{k+1} }\\0\\0 \end{bmatrix}=\begin{bmatrix} \mathbf{R}^{b_k}_w(\mathbf{p}^w_{b_{k+1} }-\mathbf{p}^w_{b_k}+ \frac{1}{2}g^w\Delta t_k^2-\mathbf{v}^w_{b_k}\Delta t_k)\\ \mathbf{R}^{b_k}_w(\mathbf{v}^w_{b_{k+1} }-\mathbf{v}^w_{b_k}+g^w\Delta t_k)\\ {\mathbf{q}^{b_k}_w}^{-1}\otimes\mathbf{q}^w_{b_{k+1} }\\ \mathbf{b}_{ab_{k+1} }-\mathbf{b}_{ab_k}\\ \mathbf{b}_{wb_{k+1} }-\mathbf{b}_{wb_k}\\ \end{bmatrix} \end{align*}\tag{34} $$

🧐 本文作者:
😉 本文链接:https://lukeyalvin.site/archives/55.html
😊 版权说明:本博客所有文章除特别声明外,均默认采用 CC BY-NC-SA 4.0 许可协议。
最后修改:2022 年 06 月 10 日
赏杯咖啡