基于 $\mathbf{S}{\mathbf{E}}_{\mathbf{2}}(\mathbf{3})$ 的精确 IMU 预积分

$\mathbf{S}{\mathbf{E}}_{\mathbf{2}}(\mathbf{3})$ Based Exact IMU Pre-Integration

文献¹基于 $\mathbf{S}{\mathbf{E}}_{\mathbf{2}}(\mathbf{3})$ 自同构指数函数引入了一种新的 IMU 预积分公式。该方法确保了 IMU 数据的高精度整合，可以有效增强快速旋转运动和延长积分时间下 SLAM 系统的跟踪性能。

1. IMU 固有补偿 - IMU intrinsic compensation

文献¹作者首先要在 IMU 预积分阶段之前添加 IMU 固有补偿。当在视觉惯性 SLAM 系统中使用 IMU 信息时，通常会将原始加速度计和陀螺仪测量值建模为

$$\begin{array}{}\text{(1.a)}& {\check{\mathbf{a}}}_t& ={\mathbf{a}}_t+{\mathbf{b}}_{a_t}+{\mathbf{n}}_{a_t}\text{(1.b)}& {\check{\mathit{\omega }}}_t& ={\mathit{\omega }}_t+{\mathbf{b}}_{{\omega }_t}+{\mathbf{n}}_{{\omega }_t}\end{array}$$

仅考虑测量噪声密度（即 $\mathbf{n}$）和偏差 (bias) $\mathbf{b}$。受加速度偏置 ${\mathbf{b}}_{a_t}$ 和陀螺仪偏置 ${\mathbf{b}}_{{\omega }_t}$ 以及加性噪声 ${\mathbf{n}}_a$ 和 ${\mathbf{n}}_{\omega }$ 的影响，模型 $\text{(1.a)}\text{(1.b)}$ 简单且有用，可为具有工厂校准内在特性的器件提供良好的近似值。然而，对于低成本的消费级惯性传感器来说，它可能会产生有损校准的结果，因为这些传感器会表现出明显的轴对齐误差和比例因子误差。受²的启发，通过引入 IMU 内在建模来扩展 IMU 模型 $\text{(1.a)}\text{(1.b)}$ ，

$$\begin{array}{}\text{(2.a)}& {\check{\mathbf{a}}}_t& ={\mathbf{S}}_{\alpha }{\mathbf{M}}_{\alpha }{\mathbf{a}}_t+{\mathbf{b}}_{a_t}+{\mathbf{n}}_{a_t}\text{(2.b)}& {\check{\mathit{\omega }}}_t& ={\mathbf{S}}_{\omega }{\mathbf{M}}_{\alpha }{\mathit{\omega }}_t^{\prime }+{\mathbf{A}}_{\omega }{\mathbf{a}}_t+{\mathbf{b}}_{{\omega }_t}+{\mathbf{n}}_{{\omega }_t}\end{array}$$

其中，${\mathit{\omega }}_t^{\prime }={\mathbf{C}}_{\omega }{\mathit{\omega }}_t$ 是加速度计和陀螺仪之间的旋转矩阵。我们将 ${\mathbf{S}}_{\alpha }$ 和 ${\mathbf{S}}_{\omega }$ 定义为缩放效应的对角矩阵，${\mathbf{M}}_{\alpha }$ 和 ${\mathbf{M}}_{\omega }$ 是对应于失准小角度的下单位三角矩阵，并令 ${\mathbf{A}}_{\omega }$ 为完全填充的倾斜矩阵。上述所有特性都可以通过执行广泛采用的 Kalibr ³来获得。扩展模型的使用可以参考这里 how to use scale-misalignment result ?

2. IMU 预积分 - IMU pre-integration

按照上一节的方法，我们对 IMU 轴偏差和比例因子进行补偿，并使用符号 ${\overline{\mathit{\omega }}}_t$ 和 ${\overline{\mathbf{a}}}_t$ 表示有偏差但无噪声、无歪斜和比例校正的 IMU 测量值。在以下章节中，我们去掉或保留下标 $t$ ，以简化符号，或强调变量的时间依赖性。

现在继续介绍核心贡献：一种新颖、精确的 IMU 预积分公式，基于矩阵李群 $\mathbf{S}{\mathbf{E}}_{\mathbf{2}}(\mathbf{3})$ 自同构的指数函数。我们首先对系统的无噪声运动学进行建模如下： $$\begin{array}{}\text{(3)}& \{\begin{array}{lll}\dot{\mathbf{R}}& =& \mathbf{R}(\overline{\mathit{\omega }}-{\mathbf{b}}_{\omega }{)}^{\wedge }\\ \dot{\mathbf{p}}& =& \mathbf{v}\\ \dot{\mathbf{v}}& =& \mathbf{R}(\overline{\mathbf{a}}-{\mathbf{b}}_a)+\mathbf{g}\end{array}\end{array}$$ 其中 $(\dot{\mathbf{R}},\dot{\mathbf{p}},\dot{\mathbf{v}})$ 是 IMU 坐标系的旋转、位置和速度的一阶导数，$\mathbf{g}$ 表示重力矢量，两者均相对于世界固定坐标系表示。我们还定义了偏差 ${\dot{\mathit{b}}}_{\omega }={\mathit{\tau }}_{\omega }$ 和 ${\dot{\mathbf{b}}}_a={\mathit{\tau }}_a$ 的随机游走。我们可以使用 $\mathbf{S}{\mathbf{E}}_{\mathbf{2}}(\mathbf{3})$ 李群中的扩展位姿来表示 IMU 位姿，使得状态为 $\mathit{\xi }=\left[\begin{array}{ccc}\mathbf{R}& \mathbf{p}& \mathbf{v}\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$ 并且状态 $\mathit{\xi }$ 的运动学可以使用 $\mathbf{S}{\mathbf{E}}_{\mathbf{2}}(\mathbf{3})$​ 的自同构来表示⁴ $$\begin{array}{}\text{(4)}& \dot{\mathit{\xi }}=(\mathbf{G}-\mathbf{D})\mathit{\xi }+\mathit{\xi }(\mathbf{U}-\mathbf{B}+\mathbf{D})\end{array}$$ 其中， $$\begin{array}{rlrl}\mathbf{U}& =\left[\begin{array}{ccc}{\mathit{\omega }}^{\wedge }& 0& \mathbf{a}\\ 0& 0& 0\\ 0& 0& 0\end{array}\right],& \mathbf{G}& =\left[\begin{array}{ccc}0& 0& \mathbf{g}\\ 0& 0& 0\\ 0& 0& 0\end{array}\right],\\ \mathbf{D}& =\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 1& 0\end{array}\right],& \mathbf{B}& =\left[\begin{array}{ccc}({\mathbf{b}}_{\omega }{)}^{\wedge }& 0& {\mathbf{b}}_a\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\end{array}$$

假设 $t_{i-1}$ 是上一帧图像 ${\mathcal{F}}_{i-1}$ 的预积分开始时间。我们定义 $t_j$ 为帧 ${\mathcal{F}}_{i-1}$ 和帧 ${\mathcal{F}}_i$ 之间任意 IMU 测量的时间戳，并让 $t_{j^{\prime }}$ 为其后续 IMU 测量的时间戳，这样小积分时间 $\delta =t_{j^{\prime }}-t_j$ 。设 ${\xi }_{t_j}$ 为时间瞬间 $t_j$ 的扩展姿态。给定 $\text{(4)}$ 并假设 $\mathbf{U}$ 和 $\mathbf{B}$ 在积分时间 $\delta$ 内恒定，则精确积分为 $$\begin{array}{}\text{(5)}& {\xi }_{t_{j^{\prime }}}=\exp (\delta (\mathbf{G}-\mathbf{D})){\xi }_{t_j}\exp (\delta (\mathbf{U}-\mathbf{B}+\mathbf{D}))\end{array}$$ 假设在时间 $t_j$ 和 $t_{i-1}$ 之间有 $N$ 组 IMU 测量值，我们可以得出 $$\begin{array}{}\text{(6)}& {\xi }_{t_j}=\exp ((\sum _{s=0}^N{\delta }_s)(\mathbf{G}-\mathbf{D})){\xi }_{t_{i-1}}\prod _{s=0}^N\exp ({\delta }_s({\mathbf{U}}_s-{\mathbf{B}}_s+\mathbf{D}))\end{array}$$ 定义 $\mathcal{T}=\sum _{s=0}^N{\delta }_s$ ，并重新排列 $\text{(6)}$ 得到 $$\begin{array}{}\text{(7)}& {\xi }_{t_{i-1}}^{-1}\exp (-\mathcal{T}(\mathbf{G}-\mathbf{D})){\xi }_{t_j}=\prod _{s=0}^N\exp ({\delta }_s({\mathbf{U}}_s-{\mathbf{B}}_s+\mathbf{D}))\end{array}$$ 左边的指数等于 $$\begin{array}{}\text{(8)}& \exp (-\mathcal{T}(\mathbf{G}-\mathbf{D}))=\left[\begin{array}{ccc}\mathbf{I}& -\frac{1}{2}{\mathcal{T}}^2\mathbf{g}& -\mathcal{T}\mathbf{g}\\ 0& 1& 0\\ 0& \mathcal{T}& 1\end{array}\right]\end{array}$$ 将 $\text{(8)}$ 代入 $\text{(7)}$ ， $\text{(7)}$ 的左侧成分与 ⁵ 中方程 (33) 的中间部分完全相同，右侧是我们新推导出的预积分项，其中每个指数都可以展开为 $$\begin{array}{}\text{(9)}& \exp ({\delta }_s({\mathbf{U}}_s-{\mathbf{B}}_s+\mathbf{D}))=\left[\begin{array}{ccc}\exp ({\delta }_s({\mathit{\omega }}_s-{\mathbf{b}}_{{\omega }_s}{)}^{\wedge })& {\mathbf{J}}_2({\mathbf{a}}_s-{\mathbf{b}}_{a_s})& {\mathbf{J}}_1({\mathbf{a}}_s-{\mathbf{b}}_{a_s})\\ 0& 1& 0\\ 0& {\delta }_s& 1\end{array}\right]\end{array}$$ 其中，参考⁶ 中 $\mathrm{S}\mathrm{O}(3)$ 中雅可比的闭式解， $$\begin{array}{}\text{(10)}& \theta & =\Vert \tilde{\mathit{\omega }}\Vert ,\tilde{\mathit{\omega }}={\mathit{\omega }}_s-{\mathbf{b}}_{{\omega }_s}\text{(11)}& {\mathbf{J}}_1& ={\delta }_s\mathbf{I}+\frac{1}{{\theta }^2}(1-\cos ({\delta }_s\theta )){\tilde{\mathit{\omega }}}^{\wedge }+\frac{1}{{\theta }^3}({\delta }_s\theta -\sin ({\delta }_s\theta ))({\tilde{\mathit{\omega }}}^{\wedge }{)}^2\text{(12)}& {\mathbf{J}}_2& =\frac{1}{2}{\delta }_s^2\mathbf{I}+\frac{1}{{\theta }^3}({\delta }_s\theta -\sin ({\delta }_s\theta )){\tilde{\mathit{\omega }}}^{\wedge }+\frac{1}{{\theta }^4}(\frac{1}{2}{\delta }_s^2{\theta }^2+\cos ({\delta }_s\theta )-1)({\tilde{\mathit{\omega }}}^{\wedge }{)}^2\end{array}$$ 迭代预积分的最终结果为 $$\begin{array}{}\text{(13)}& \{\begin{array}{lll}\mathrm{\Delta }{\mathbf{R}}_{t_{j^{\prime }}}^{t_i}& =& \mathrm{\Delta }{\mathbf{R}}_{t_j}^{t_i}\exp ({\delta }_j(\mathit{\omega }-{\mathbf{b}}_{{\omega }_j}{)}^{\wedge })\\ \mathrm{\Delta }{\mathbf{p}}_{t_{j^{\prime }}}^{t_i}& =& \mathrm{\Delta }{\mathbf{p}}_{t_j}^{t_i}+{\delta }_j\mathrm{\Delta }{\mathbf{v}}_{t_j}^{t_i}+\mathrm{\Delta }{\mathbf{R}}_{t_j}^{t_i}{\mathbf{J}}_2({\mathbf{a}}_j-{\mathbf{b}}_{a_j})\\ \mathrm{\Delta }{\mathbf{v}}_{t_{j^{\prime }}}^{t_i}& =& \mathrm{\Delta }{\mathbf{v}}_{t_j}^{t_i}+\mathrm{\Delta }{\mathbf{R}}_{t_j}^{t_i}{\mathbf{J}}_1({\mathbf{a}}_j-{\mathbf{b}}_{a_j})\end{array}\end{array}$$ 请注意，在欧拉积分方案下，雅可比项 ${\mathbf{J}}_1$ 和 ${\mathbf{J}}_2$ 简化为 ${\mathbf{J}}_1={\delta }_s\mathbf{I}$ 和 ${\mathbf{J}}_2=\frac{1}{2}{\delta }_s^2\mathbf{I}$ 。

在实际应用中，无噪声项是不可用的，因此用相应的估计值代替，用 $\widehat{(\cdot )}$ 符号表示。然后，考虑到前一个估计值的不确定性和测量噪声，需要将处理协方差传播问题。误差项定义如下

$$\begin{array}{}\text{(14)}& {\mathbf{e}}_{i{j}^{\prime }}& =[({\mathbf{e}}_{i{j}^{\prime }}^{\mathrm{\Delta }\mathbf{R}}{)}^{\mathrm{\top }},({\mathbf{e}}_{i{j}^{\prime }}^{\mathrm{\Delta }\mathbf{p}}{)}^{\mathrm{\top }},({\mathbf{e}}_{i{j}^{\prime }}^{\mathrm{\Delta }\mathbf{v}}{)}^{\mathrm{\top }},({\mathbf{e}}_{i{j}^{\prime }}^{{\mathbf{b}}_{\omega }}{)}^{\mathrm{\top }},({\mathbf{e}}_{i{j}^{\prime }}^{{\mathbf{b}}_a}{)}^{\mathrm{\top }}{]}^{\mathrm{\top }}\text{(15.a)}& {\mathbf{e}}_{i{j}^{\prime }}^{\mathrm{\Delta }\mathbf{R}}& =\log (\mathrm{\Delta }{\mathbf{R}}_{t_{j^{\prime }}}^{t_i}{}^{\mathrm{\top }}\mathrm{\Delta }{\hat{\mathbf{R}}}_{t_{j^{\prime }}}^{t_i}{)}^{\vee }\sim \mathcal{N}(0,{\mathrm{\Sigma }}_{i{j}^{\prime }}^{\mathbf{R}})\in {\mathbb{R}}^3\text{(15.b)}& {\mathbf{e}}_{i{j}^{\prime }}^{\mathrm{\Delta }\mathbf{p}}& =\mathrm{\Delta }{\hat{\mathbf{p}}}_{t_{j^{\prime }}}^{t_i}-\mathrm{\Delta }{\mathbf{p}}_{t_{j^{\prime }}}^{t_i}\sim \mathcal{N}(0,{\mathrm{\Sigma }}_{i{j}^{\prime }}^{\mathbf{p}})\in {\mathbb{R}}^3\text{(15.c)}& {\mathbf{e}}_{i{j}^{\prime }}^{\mathrm{\Delta }\mathbf{v}}& =\mathrm{\Delta }{\hat{\mathbf{v}}}_{t_{j^{\prime }}}^{t_i}-\mathrm{\Delta }{\mathbf{v}}_{t_{j^{\prime }}}^{t_i}\sim \mathcal{N}(0,{\mathrm{\Sigma }}_{i{j}^{\prime }}^{\mathbf{v}})\in {\mathbb{R}}^3\text{(15.d)}& {\mathbf{e}}_{i{j}^{\prime }}^{\mathrm{\Delta }{\mathbf{b}}_{\omega }}& ={\hat{\mathbf{b}}}_{{\omega }_j}-{\mathbf{b}}_{{\omega }_j}\sim \mathcal{N}(0,{\mathrm{\Sigma }}_{i{j}^{\prime }}^{{\mathbf{b}}_{\omega }})\in {\mathbb{R}}^3\text{(15.e)}& {\mathbf{e}}_{i{j}^{\prime }}^{\mathrm{\Delta }{\mathbf{b}}_a}& ={\hat{\mathbf{b}}}_{a_j}-{\mathbf{b}}_{a_j}\sim \mathcal{N}(0,{\mathrm{\Sigma }}_{i{j}^{\prime }}^{{\mathbf{b}}_a})\in {\mathbb{R}}^3\end{array}$$

由此推导出预积分项演化的矩阵表示

$$\begin{array}{}\text{(16)}& {\mathbf{e}}_{i{j}^{\prime }}={\mathbf{A}}_{j^{\prime }}{\mathbf{e}}_{ij}+{\mathbf{B}}_{j^{\prime }}{\mathbf{n}}_{j^{\prime }}\end{array}$$

其中，

$$\begin{array}{}\text{(17)}& \begin{array}{c}\mathbf{A}=\left[\begin{array}{ccccc}\exp (-{\delta }_{j^{\prime }}({\hat{\mathit{\omega }}}_{j^{\prime }}-{\hat{\mathbf{b}}}_{w_{j^{\prime }}}{)}^{\wedge })& 0& 0& -{\delta }_{j^{\prime }}\mathbf{I}& 0\\ -\mathrm{\Delta }{\hat{\mathbf{R}}}_{t_j}^{t_i}({\hat{\mathbf{J}}}_2({\hat{\mathbf{a}}}_{j^{\prime }}-{\hat{\mathbf{b}}}_{a_{j^{\prime }}}){)}^{\wedge }& \mathbf{I}& {\delta }_{j^{\prime }}\mathbf{I}& {\mathbf{D}}_1& -\mathrm{\Delta }{\hat{\mathbf{R}}}_{t_j}^{t_i}{\hat{\mathbf{J}}}_2\\ -\mathrm{\Delta }{\hat{\mathbf{R}}}_{t_j}^{t_i}({\hat{\mathbf{J}}}_1({\hat{\mathbf{a}}}_{j^{\prime }}-{\hat{\mathbf{b}}}_{a_{j^{\prime }}}){)}^{\wedge }& 0& \mathbf{I}& -{\mathbf{D}}_2& -\mathrm{\Delta }{\hat{\mathbf{R}}}_{t_j}^{t_i}{\hat{\mathbf{J}}}_1\\ 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& \mathbf{I}\end{array}\right]\\ \mathbf{B}=\left[\begin{array}{cccc}{\delta }_{j^{\prime }}\mathbf{I}& 0& 0& 0\\ -{\mathbf{D}}_1& \mathrm{\Delta }{\hat{\mathbf{R}}}_{t_j}^{t_i}{\hat{\mathbf{J}}}_2& 0& 0\\ {\mathbf{D}}_2& \mathrm{\Delta }{\hat{\mathbf{R}}}_{t_j}^{t_i}{\hat{\mathbf{J}}}_1& 0& 0\\ 0& 0& -{\delta }_{j^{\prime }}\mathbf{I}& 0\\ 0& 0& 0& -{\delta }_{j^{\prime }}\mathbf{I}\end{array}\right],{\mathbf{n}}_{j^{\prime }}=\left[\begin{array}{c}{\mathbf{n}}_{{\omega }_{j^{\prime }}}\\ {\mathbf{n}}_{a_{j^{\prime }}}\\ {\mathit{\tau }}_{{\omega }_{j^{\prime }}}\\ {\mathit{\tau }}_{a_{j^{\prime }}}\end{array}\right]\end{array}\end{array}$$

有

$$\begin{array}{rl}{\mathbf{D}}_1& =\mathrm{\Delta }{\hat{\mathbf{R}}}_{t_j}^{t_i}(\frac{{\delta }_{j^{\prime }}{}^2}{\Vert {\hat{\mathit{\omega }}}_{j^{\prime }}-{\hat{\mathbf{b}}}_{{\omega }_{j^{\prime }}}{\Vert }^2}((({\hat{\mathit{\omega }}}_{j^{\prime }}-{\hat{\mathbf{b}}}_{{\omega }_{j^{\prime }}}{)}^{\wedge }({\hat{\mathbf{a}}}_{j^{\prime }}-{\hat{\mathbf{b}}}_{a_{j^{\prime }}}){)}^{\wedge }+({\hat{\mathit{\omega }}}_{j^{\prime }}-{\hat{\mathbf{b}}}_{{\omega }_{j^{\prime }}}{)}^{\wedge }({\hat{\mathbf{a}}}_{j^{\prime }}-{\hat{\mathbf{b}}}_{a_{j^{\prime }}}{)}^{\wedge })\\ \text{(18)}& & +\frac{2{\delta }_{j^{\prime }}{}^2}{\Vert {\hat{\mathit{\omega }}}_{j^{\prime }}-{\hat{\mathbf{b}}}_{{\omega }_{j^{\prime }}}{\Vert }^4}((({\hat{\mathit{\omega }}}_{j^{\prime }}-{\hat{\mathbf{b}}}_{{\omega }_{j^{\prime }}}{)}^{\wedge }{)}^2({\hat{\mathbf{a}}}_{j^{\prime }}-{\hat{\mathbf{b}}}_{a_{j^{\prime }}})({\hat{\mathit{\omega }}}_{j^{\prime }}-{\hat{\mathbf{b}}}_{{\omega }_{j^{\prime }}}{)}^{\mathrm{\top }}))\text{(19)}& {\mathbf{D}}_2& =\mathrm{\Delta }{\hat{\mathbf{R}}}_{t_j{}^{\prime }}^{t_i}\Vert {\hat{\mathit{\omega }}}_{j^{\prime }}-{\hat{\mathbf{b}}}_{{\omega }_{j^{\prime }}}{\Vert }^4{\delta }_{j^{\prime }}{}^2({\hat{\mathit{\omega }}}_{j^{\prime }}-{\hat{\mathbf{b}}}_{{\omega }_{j^{\prime }}}{)}^{\wedge }({\hat{\mathbf{a}}}_{j^{\prime }}-{\hat{\mathbf{b}}}_{a_{j^{\prime }}})({\hat{\mathit{\omega }}}_{j^{\prime }}-{\hat{\mathbf{b}}}_{{\omega }_{j^{\prime }}}{)}^{\mathrm{\top }}\end{array}$$

这里的 $\hat{\mathit{\omega }}$ 和 $\hat{\mathbf{a}}$ 是有偏差、有噪声和内在补偿的 IMU 测量值。

3. 工程实现

根据上文中的公式与推到，易将具有 IMU 固有补偿 (scale-misalignment 模型) 和自同构指数函数的 IMU 预积分公式移植到 VINS 系列开源算法中。个人的代码实现见 LSXiang/VINS-Fusion : This is an extended version of VINS-Fusion and migrates to ROS2, adding RGB-D mode, IMU preintegration in the form of SE2(3), line features and gpu acceleration 。

Reference