预积分和流形

论文：IMU Preintegration on Manifold for Effificient Visual-Inertial Maximum-a-Posteriori Estimation

引言

Recent results in monocular visual-inertial navigation (VIN) have shown that optimization-based approaches outperform filtering methods in terms of accuracy due to their capability to relinearize past states. However, the improvement comes at the cost of increased computational complexity. In this paper, we address this issue by preintegrating inertial measurements between selected keyframes. The preintegration allows us to accurately summarize hundreds of inertial measurements into a single relative motion constraint. Our first contribution is a preintegration theory that properly addresses the manifold structure of the rotation group and carefully deals with uncertainty propagation. The measurements are integrated in a local frame, which eliminates the need to repeat the integration when the linearization point changes while leaving the opportunity for belated bias corrections. The second contribution is to show that the preintegrated IMU model can be seamlessly integrated in a visual-inertial pipeline under the unifying framework of factor graphs. This enables the use of a structureless model for visual measurements, further accelerating the computation. The third contribution is an extensive evaluation of our monocular VIN pipeline: experimental results confirm that our system is very fast and demonstrates superior accuracy with respect to competitive state-of-the-art filtering and optimization algorithms, including off-the-shelf systems such as Google Tango [1].

引言

本文提出了一个使用增量平滑（incremental smoothing）快速计算最大后验估计(MAP)的系统。

第一项贡献是发展出了一种新颖的预积分理论。预积分IMU测量的使用是在[26]中首次提出的，包括将两个关键帧之间的许多惯性测量组合成一个相对运动约束。 本文在此工作的基础上提出了一个预积分理论，该理论恰当地解决了旋转群的流形结构，并允许对所有雅可比矩阵进行解析推导。

[26]采用欧拉角作为旋转的全局参数化。Using Euler angles and applying the usual averaging and smoothing techniques of Euclidean spaces for state propagation and covariance estimation is not properly invariant under the action of rigid transformations [27]。此外，已知欧拉角具有奇点。在第五节中的理论推导也推进了之前[10,12,13,25]这些工作使用了预积分测量，但没有发展出不确定性传播和后验bias校正的相应理论。本文受益于[26]开创性见解: 在局部坐标系中进行积分，当线性化点发生变化时不需要重复积分。

第二项贡献，我们将预积分的IMU模型框定为一个因子图视角（we frame our preintegrated IMU model into a factor graph perspective）。这使得基于iSAM2 [24]的恒定时间VIN管道设计成为可能。我们提出的增量平滑解决方案避免了线性化误差的积累，并提供了一个具有吸引力的替代方案，以使用自适应支持窗口进行优化[10]。

受[20,28]的启发，本文采用了一种无结构的视觉测量模型，该模型允许在增量平滑过程中消除大量变量(即所有3D点)，进一步加速计算。

第三个贡献是对系统进行了有效的实施和广泛的评估。实验结果表明，后端需要平均10毫秒的CPU时间来计算full MAP估计，并与SOTA方法相比获得了更高的准确性。论文附有补充材料[29]。此外，本文在GTSAM 4.0优化工具箱中发布了IMU预积分和无结构视觉因子的实现。

预备知识

黎曼几何记号

本节回顾与特殊正交群SO(3)和特殊欧氏群SE(3)相关的有用概念，基于[31,32]。

SO(3)

SO(3)描述了一组三维旋转矩阵，定义为 SO(3)≡{R∈R3×3:RTR=I,det(R)=1}\text{SO}(3) \equiv \{ \bold{R} \in \mathbb{R}^{3\times 3} : \bold{R}^{T} \bold{R} = \bold{I}, \text{det}(\bold{R}) = 1 \}SO(3)≡{R∈R3×3:RTR=I,det(R)=1} 。群运算是一般的矩阵乘法，逆是矩阵转置。 SO(3)也形成光滑流形，流形(在恒等处)的切空间（tangent space）表示为so(3)，这也称为李代数，并与3×3反对称矩阵的空间相一致。可以用hat符号在 R3\mathbb{R}^3R3 中标识每个带有向量的反对称矩阵:
ω∧=[ω1ω2ω3]∧=[0−ω3ω2ω30−ω1−ω2ω10]∈so(3)(1)\boldsymbol{\omega}^{\wedge}=\left[\begin{array}{l} \omega_{1} \\ \omega_{2} \\ \omega_{3} \end{array}\right]^{\wedge}=\left[\begin{array}{ccc} 0 & -\omega_{3} & \omega_{2} \\ \omega_{3} & 0 & -\omega_{1} \\ -\omega_{2} & \omega_{1} & 0 \end{array}\right] \in \mathfrak{s o}(3) \tag{1} ω∧=⎣⎡ω1ω2ω3⎦⎤∧=⎣⎡0ω3−ω2−ω30ω1ω2−ω10⎦⎤∈so(3)(1)
可以用vee运算符将反对称矩阵映射到 R3\mathbb{R}^3R3 中的向量：对于反对称矩阵 S=ω∧\bold{S} = \boldsymbol{\omega}^{\wedge}S=ω∧ ，有： S∨=ω\bold{S}^{\vee}=\boldsymbol{\omega}S∨=ω 。反对称矩阵的一个性质是:
a∧b=−b∧a,∀a,b∈R3(2)\mathbf{a}^{\wedge} \mathbf{b}=-\mathbf{b}^{\wedge} \mathbf{a}, \quad \forall \mathbf{a}, \mathbf{b} \in \mathbb{R}^{3} \tag{2} a∧b=−b∧a,∀a,b∈R3(2)

指数映射exp： so(3)→SO(3)\text{so}(3) \to \text{SO}(3)so(3)→SO(3) 将一个李代数元素关联到一个旋转：
exp⁡(ϕ∧)=I+sin⁡(∥ϕ∥)∥ϕ∥ϕ∧+1−cos⁡(∥ϕ∥)∥ϕ∥2(ϕ∧)2(3)\exp \left(\phi^{\wedge}\right)=\mathbf{I}+\frac{\sin (\|\phi\|)}{\|\phi\|} \phi^{\wedge}+\frac{1-\cos (\|\phi\|)}{\|\phi\|^{2}}\left(\phi^{\wedge}\right)^{2} \tag{3} exp(ϕ∧)=I+∥ϕ∥sin(∥ϕ∥)ϕ∧+∥ϕ∥21−cos(∥ϕ∥)(ϕ∧)2(3)
指数映射的一阶近似为:
exp⁡(ϕ∧)≈I+ϕ∧(4)\exp \left(\phi^{\wedge}\right) \approx \mathbf{I}+\phi^{\wedge} \tag{4} exp(ϕ∧)≈I+ϕ∧(4)

对数映射将一个SO(3)旋转矩阵关联到一个反对称矩阵：
log⁡(R)=φ⋅(R−R⊤)2sin⁡(φ)with φ=cos⁡−1(tr⁡(R)−12)(5)\log (\mathrm{R})=\frac{\varphi \cdot\left(\mathrm{R}-\mathrm{R}^{\top}\right)}{2 \sin (\varphi)} \text { with } \varphi=\cos ^{-1}\left(\frac{\operatorname{tr}(\mathrm{R})-1}{2}\right) \tag{5} log(R)=2sin(φ)φ⋅(R−R⊤) with φ=cos−1(2tr(R)−1)(5)
注意有 log⁡(R)∨=aφ\log(\bold{R})^{\vee}=\bold{a}\varphilog(R)∨=aφ ，a\bold{a}a 是旋转轴， φ\varphiφ 是旋转角。

如果限制在开球 ∥ϕ∥<π\Vert \phi \Vert < \pi∥ϕ∥<π ，指数映射是一个双射，对应的逆是对数映射。然而，如果不限制定域，指数映射就会变成满射，因为每个向量 ϕ=(φ+2kπ)a,k∈Z\boldsymbol{\phi}=( \varphi + 2k\pi )\bold{a},k\in \mathbb{Z}ϕ=(φ+2kπ)a,k∈Z 都是R的一个对数。

为了便于标记，采用指数和对数映射的“向量化”版本:
Exp⁡:R3∋ϕ→exp⁡(ϕ∧)∈SO(3)log⁡:SO(3)∋R→log⁡(R)∨∈R3(6)\begin{array}{cccccccc} \operatorname{Exp}: & \mathbb{R}^{3} & \ni & \phi & \rightarrow & \exp \left(\phi^{\wedge}\right) & \in & \mathrm{SO}(3) \\ \log : & \mathrm{SO}(3) & \ni & \mathrm{R} & \rightarrow & \log (R)^{\vee} & \in & \mathbb{R}^{3} \end{array} \tag{6} Exp:log:R3SO(3)∋∋ϕR→→exp(ϕ∧)log(R)∨∈∈SO(3)R3(6)
它们直接作用于向量，而不是so(3)。

使用以下一阶近似:
Exp⁡(ϕ+δϕ)≈Exp⁡(ϕ)Exp⁡(Jr(ϕ)δϕ)(7)\operatorname{Exp}(\phi+\delta \phi) \approx \operatorname{Exp}(\boldsymbol{\phi}) \operatorname{Exp}\left(\mathrm{J}_{r}(\boldsymbol{\phi}) \delta \boldsymbol{\phi}\right) \tag{7} Exp(ϕ+δϕ)≈Exp(ϕ)Exp(Jr(ϕ)δϕ)(7)
Jr(ϕ)\bold{J}_r(\boldsymbol{\phi})Jr(ϕ) 是SO(3)的右雅可比，其将tangent空间中的加性增量与应用于右侧的乘法增量联系起来:
Jr(ϕ)=I−1−cos⁡(∥ϕ∥)∥ϕ∥2ϕ∧+∥ϕ∥−sin⁡(∥ϕ∥)∥ϕ3∥(ϕ∧)2(8)\mathrm{J}_{r}(\phi)=\mathbf{I}-\frac{1-\cos (\|\phi\|)}{\|\phi\|^{2}} \phi^{\wedge}+\frac{\|\phi\|-\sin (\|\phi\|)}{\left\|\phi^{3}\right\|}\left(\phi^{\wedge}\right)^{2} \tag{8} Jr(ϕ)=I−∥ϕ∥21−cos(∥ϕ∥)ϕ∧+∥ϕ3∥∥ϕ∥−sin(∥ϕ∥)(ϕ∧)2(8)
类似的一阶近似也适用于对数:
log⁡(Exp⁡(ϕ)Exp⁡(δϕ))≈ϕ+Jr−1(ϕ)δϕ(9)\log (\operatorname{Exp}(\boldsymbol{\phi}) \operatorname{Exp}(\delta \phi)) \approx \boldsymbol{\phi}+\mathrm{J}_{r}^{-1}(\boldsymbol{\phi}) \delta \boldsymbol{\phi} \tag{9} log(Exp(ϕ)Exp(δϕ))≈ϕ+Jr−1(ϕ)δϕ(9)
右雅可比矩阵逆的显式表达式在补充材料[29]中给出。 Jr(ϕ)J_r(\boldsymbol{\phi})Jr(ϕ) reduces to the identity for ∥ϕ∥=0\Vert \boldsymbol{\phi} \Vert=0∥ϕ∥=0 。

指数映射的另一个有用的性质是关于伴随矩阵的：
RExp⁡(ϕ)R⊤=exp⁡(Rϕ∧R⊤)=Exp⁡(Rϕ)⇔Exp⁡(ϕ)R=RExp⁡(R⊤ϕ)(11)\begin{aligned} \mathrm{R} \operatorname{Exp}(\phi) \mathrm{R}^{\top} &=\exp \left(\mathrm{R} \phi^{\wedge} \mathrm{R}^{\top}\right)=\operatorname{Exp}(\mathrm{R} \phi) \\ \Leftrightarrow \quad \operatorname{Exp}(\phi) \mathrm{R} &=\mathrm{R} \operatorname{Exp}\left(\mathrm{R}^{\top} \boldsymbol{\phi}\right) \end{aligned} \tag{11} RExp(ϕ)R⊤⇔Exp(ϕ)R=exp(Rϕ∧R⊤)=Exp(Rϕ)=RExp(R⊤ϕ)(11)

SE(3)

SE(3)描述了三维刚体运动群，定义为 SE(3)≡{(R,p):R∈SO(3),p∈R3}\text{SE}(3) \equiv \{ (\bold{R},\bold{p}) : \bold{R} \in \text{SO(3)} , \bold{p} \in \mathbb{R}^3 \}SE(3)≡{(R,p):R∈SO(3),p∈R3} 。群运算是 T1⋅T2=(R1R2,R1p2+R1)\bold{T}_1 \cdot \bold{T}_2 = (\bold{R}_1 \bold{R}_2,\bold{R}_1 \bold{p}_2 + \bold{R}_1)T1⋅T2=(R1R2,R1p2+R1) ，其逆为 T1−1=(R1T,−R1Tp1)\bold{T}_1^{-1}=(\bold{R}_1^{T},-\bold{R}_1^{T} \bold{p}_1)T1−1=(R1T,−R1Tp1) ， SE(3)的指数映射和对数映射在[32]中定义。但是，本文不需要这些，原因将在第II-C节中说明。

SO(3)中不确定的描述

SO(3)中不确定度的定义：tangent空间中的一个分布，然后通过指数映射(6)将其映射到SO(3) [32-34]:
R~=RExp⁡(ϵ),ϵ∼N(0,Σ)(12)\tilde{\mathrm{R}}=\mathrm{R} \operatorname{Exp}(\epsilon), \quad \epsilon \sim \mathcal{N}(0, \Sigma) \tag{12} R~=RExp(ϵ),ϵ∼N(0,Σ)(12)
其中 R\bold{R}R 是一个给定的无噪声旋转(均值), ϵ\epsilonϵ 为均值为零的小的正态分布扰动。

随机变量 R~∈SO(3)\tilde{\bold{R}} \in \text{SO}(3)R~∈SO(3) 的分布可以被显式计算，如[33]一样，得到：
p(R~)=β(R~)e−12∥Log(R−1R~)∥Σ2(13)p(\tilde{\mathrm{R}})=\beta(\tilde{\mathrm{R}}) e^{-\frac{1}{2}\left\| \text{Log} \left(\mathrm{R}^{-1} \tilde{\mathrm{R}}\right)\right\|_{\Sigma}^{2}} \tag{13} p(R~)=β(R~)e−21∥Log(R−1R~)∥Σ2(13)
其中 β(R~)\beta(\tilde{\bold{R}})β(R~) 是归一化因子，近似计算： β(R~)≃1/2πdet(Σ)\beta(\tilde{\bold{R}}) \simeq 1 / \sqrt{ 2 \pi \text{det}(\Sigma) }β(R~)≃1/2πdet(Σ) ，其中 Σ\SigmaΣ 很小。如果将 β\betaβ 近似为一个常数，那么给定一个测量 R~\tilde{\bold{R}}R~ ，旋转 R\bold{R}R 的负对数似然为：
L(R)=12∥Log(R−1R~)∥Σ2+const=12∥Log(R~−1R)∥Σ2+const(14)\mathcal{L}(\mathrm{R})=\frac{1}{2}\left\| \text{Log} \left(\mathrm{R}^{-1} \tilde{\mathrm{R}}\right)\right\|_{\Sigma}^{2}+ \text{const} =\frac{1}{2}\left\| \text{Log} \left(\tilde{\mathrm{R}}^{-1} \mathrm{R}\right)\right\|_{\Sigma}^{2}+ \text{const} \tag{14} L(R)=21∥∥Log(R−1R~)∥∥Σ2+const=21∥∥Log(R~−1R)∥∥Σ2+const(14)

流形上的高斯牛顿方法

关于流形：流形优化： Manifold Optimization 的全网最通俗版本详解 (一）

考虑一个优化问题 min⁡x∈Mf(x)\min_{x \in \mathcal{M}} f(x)minx∈Mf(x) ，其中 f(⋅)f(\cdot)f(⋅) 是给定的代价函数，变量 xxx 属于流形 M\mathcal{M}M ; 为了简单起见，考虑一个单一变量，因而描述可以很容易地一般化。

流形优化的一种标准方法[35,36]包括定义一个retraction Rx\mathcal{R}_xRx ，它是tangent空间(在 xxx 处)的元素 δxδxδx 与 x∈Mx∈\mathcal{M}x∈M 的邻域之间的一个双射映射。使用retraction，可以重新参数化问题如下:
min⁡x∈Mf(x)⇒min⁡δx∈Rnf(Rx(δx))\min _{x \in \mathcal{M}} f(x) \Rightarrow \min _{\delta x \in \mathbb{R}^{n}} f\left(\mathcal{R}_{x}(\delta x)\right) x∈Mminf(x)⇒δx∈Rnminf(Rx(δx))
重新参数化通常称为提升（lifting）[35]。粗略地说，我们在当前估计定义的tangent空间中工作，它在局部表现为欧氏空间。现在可以将标准优化技术应用于(14)中右边的问题。在GN框架中，将cost围绕当前估计进行平方计算。然后求解二次逼近得到在正切空间中向量 δx⋆\delta x ^{\star}δx⋆ 。最后，流形上的当前猜测被更新为 x^←Rx^(δx⋆)\hat{x} \leftarrow \mathcal{R}_{\hat{x}} (\delta x ^{\star})x^←Rx^(δx⋆) 。

一个可能的retraction是指数映射。在计算上，这可能不是最方便的，参见[37]。

对于SE(3)，使用下面的retraction，在 T=(R,p)\bold{T} = (\bold{R},\bold{p})T=(R,p) ：
RT(δϕ,δp)=(RExp⁡(δϕ),p+Rδp),[δϕδp]∈R6(15)\mathcal{R}_{\mathrm{T}}(\delta \boldsymbol{\phi}, \delta \mathbf{p})=(\mathrm{R} \operatorname{Exp}(\delta \boldsymbol{\phi}), \mathbf{p}+\mathrm{R} \delta \mathbf{p}), \quad\left[\begin{array}{ll}\delta \boldsymbol{\phi} & \delta \mathbf{p}\end{array}\right] \in \mathbb{R}^{6} \tag{15} RT(δϕ,δp)=(RExp(δϕ),p+Rδp),[δϕδp]∈R6(15)
这解释了为什么在第II-A节中，只定义了SO(3)的指数映射:有了这种选择就不需要计算SE(3)的指数映射。

视觉惯性状态估计的MAP

系统和假设：考虑一个VIN问题，在这个问题中想要跟踪一个装有IMU和单目相机的传感系统 (例如，一个移动机器人或手持设备) 的状态。假设IMU帧“B”与想要跟踪的body帧重合，并且相机和IMU之间的变换（外参）是固定的，且预先标定了（图3）。假设SLAM前端提供未知位置的3D路标的像素测量。且得到了一个称为关键帧[16]的图像子集，对其计算位姿估计。

状态：系统在 iii 时刻的状态用IMU的方向、位置、速度和bias来描述: xi≡[Ri,pi,vi,bi]\bold{x}_{i} \equiv [ \bold{R}_i,\bold{p}_i,\bold{v}_i , \bold{b}_i ]xi≡[Ri,pi,vi,bi] ，其中 (Ri,pi)∈SE(3)(\bold{R}_i,\bold{p}_i) \in \text{SE(3)}(Ri,pi)∈SE(3) ，速度在向量空间中即 vi∈R3\bold{v}_i \in \mathbb{R}^3vi∈R3 ，IMU bias可以写成 bi=[big,bia]∈R6\bold{b}_i = [\bold{b}_i^{g},\bold{b}_i^{a}] \in \mathbb{R}^6bi=[big,bia]∈R6 ，其中 big,bia∈R3\bold{b}_i^g ,\bold{b}_i^a \in \mathbb{R}^3big,bia∈R3 是陀螺仪和加速度计的bias。

记 Kk\mathcal{K}_kKk 为到时刻 kkk 为止的所有关键帧集合。这里估计所有关键帧的状态:
Xk≐{xi}i∈Kk(16)\mathcal{X}_{k} \doteq\left\{\mathbf{x}_{i}\right\}_{i \in \mathcal{K}_{k}} \tag{16} Xk≐{xi}i∈Kk(16)
这里采用了一种无结构的方法(参见第六节)，因此3D路标不属于待估计变量的一部分。

测量：系统输入是来自相机和IMU的测量值。 Ci\mathcal{C}_iCi 为在关键帧 iii 的相机测量，在 iii 时刻相机可以观察到多个路标 lll ，因此 Ci\mathcal{C}_iCi 包含多个像素的观测 zil\bold{z}_{il}zil 。用 Iij\mathcal{I}_{ij}Iij 表示在两个连续关键帧 iii 和 jjj 之间获得的IMU测量集合。通常每个 Iij\mathcal{I}_{ij}Iij 包含数百个IMU测量。到时间 kkk 收集到的测量集合是
Zk≐{Ci,Iij}(i,j)∈Kk(17)\mathcal{Z}_{k} \doteq\left\{\mathcal{C}_{i}, \mathcal{I}_{i j}\right\}(i, j) \in \mathcal{K}_{k} \tag{17} Zk≐{Ci,Iij}(i,j)∈Kk(17)

因子图和MAP估计：给定可用测量 Zk\mathcal{Z}_kZk 和先验 p(X0)p(\mathcal{X}_0)p(X0) ，因子图编码了变量 Xk\mathcal{X}_kXk 的后验概率：
p(Xk∣Zk)∝p(X0)p(Zk∣Xk)=p(X0)∏(i,j)∈Kkp(Ci,Iij∣Xk)=p(X0)∏(i,j)∈Kkp(Iij∣xi,xj)∏i∈Kk∏l∈Cip(zil∣xi)(18)p\left(\mathcal{X}_{k} \mid \mathcal{Z}_{k}\right) \propto p\left(\mathcal{X}_{0}\right) p\left(\mathcal{Z}_{k} \mid \mathcal{X}_{k}\right)=p\left(\mathcal{X}_{0}\right) \prod_{(i, j) \in \mathcal{K}_{k}} p\left(\mathcal{C}_{i}, \mathcal{I}_{i j} \mid \mathcal{X}_{k}\right)\\ =p\left(\mathcal{X}_{0}\right) \prod_{(i, j) \in \mathcal{K}_{k}} p\left(\mathcal{I}_{i j} \mid \mathbf{x}_{i}, \mathbf{x}_{j}\right) \prod_{i \in \mathcal{K}_{k}} \prod_{l \in \mathcal{C}_{i}} p\left(\mathbf{z}_{i l} \mid \mathbf{x}_{i}\right) \tag{18} p(Xk∣Zk)∝p(X0)p(Zk∣Xk)=p(X0)(i,j)∈Kk∏p(Ci,Iij∣Xk)=p(X0)(i,j)∈Kk∏p(Iij∣xi,xj)i∈Kk∏l∈Ci∏p(zil∣xi)(18)
因式分解(18)中的项称为因子（factors）。图4给出了问题的因子图的连通性示意图(无结构视觉因子的连通性将在第VI节中阐明)。

MAP估计对应于公式(18)的最大值，或者等价于负对数后验的最小值。在零均值高斯噪声的假设下，负对数后验可以写成残差平方和:
Xk⋆≐arg⁡min⁡Xk−log⁡ep(Xk∣Zk)=arg⁡min⁡Xk∥r0∥Σ02+∑(i,j)∈Kk∥rIij∥Σij2+∑i∈Kk∑l∈Ci∥rCil∥ΣC2(19)\mathcal{X}_{k}^{\star} \doteq \arg \min _{\mathcal{X}_{k}}-\log _{e} p\left(\mathcal{X}_{k} \mid \mathcal{Z}_{k}\right) \\ =\arg \min _{\mathcal{X}_{k}}\left\|\mathbf{r}_{0}\right\|_{\Sigma_{0}}^{2}+\sum_{(i, j) \in \mathcal{K}_{k}}\left\|\mathbf{r}_{\mathcal{I}_{i j}}\right\|_{\Sigma_{i j}}^{2}+\sum_{i \in \mathcal{K}_{k}} \sum_{l \in \mathcal{C}_{i}}\left\|\mathbf{r}_{\mathcal{C}_{i l}}\right\|_{\Sigma_{\mathcal{C}}}^{2} \tag{19} Xk⋆≐argXkmin−logep(Xk∣Zk)=argXkmin∥r0∥Σ02+(i,j)∈Kk∑∥∥rIij∥∥Σij2+i∈Kk∑l∈Ci∑∥rCil∥ΣC2(19)
其中 r0\bold{r}_0r0 , rIij\bold{r}_{\mathcal{I}_{ij}}rIij , rCil\bold{r}_{\mathcal{C}_{il}}rCil 是关于测量的残差项， Σ0\bold{\Sigma}_{0}Σ0 , Σij\bold{\Sigma}_{ij}Σij , ΣC\bold{\Sigma}_{\mathcal{C}}ΣC 是对应的协方差矩阵。

IMU模型和运动积分

IMU测量相对于惯性系的传感器的转速（rotate rate）和加速度。测量记为 Ba~(t)_{B}\tilde{\bold{a}}(t)Ba~(t) , Bω~WB(t)_{B}\tilde{\boldsymbol{\omega}}_{WB}(t)Bω~WB(t) ，受到加性白噪声 η\boldsymbol{\eta}η 和缓慢变化的传感器bias b\bold{b}b 的影响。
Bω~WB(t)=BωWB(t)+bg(t)+ηg(t)(20){ }_{\mathrm{B}} \tilde{\boldsymbol{\omega}}_{\mathrm{WB}}(t)={ }_{\mathrm{B}} \boldsymbol{\omega}_{\mathrm{WB}}(t)+\mathbf{b}^{g}(t)+\boldsymbol{\eta}^{g}(t) \tag{20} Bω~WB(t)=BωWB(t)+bg(t)+ηg(t)(20)

Ba~(t)=RwB⊤(t)(wa(t)−wg)+ba(t)+ηa(t)(21){ }_{\mathrm{B}} \tilde{\mathbf{a}}(t)=\mathrm{R}_{\mathrm{wB}}^{\top}(t) \left( { }_{\mathrm{w}} \mathbf{a}(t)-{ }_{\mathrm{w}} \mathbf{g}\right)+\mathbf{b}^{a}(t)+\boldsymbol{\eta}^{a}(t) \tag{21} Ba~(t)=RwB⊤(t)(wa(t)−wg)+ba(t)+ηa(t)(21)

前缀B表示对应的量在坐标系B中表示(c.f.，图3)。 IMU的位姿由变换 RWB,Wp{\bold{R}_{WB}, _{W}\bold{p}}RWB,Wp 描述，它将一个点从传感器坐标系B映射到W。向量 Bω~WB(t)∈R3_{B}\tilde{\boldsymbol{\omega}}_{WB}(t) \in \mathbb{R}^3Bω~WB(t)∈R3 为B相对于W的瞬时角速度，在坐标系B中表示； Ba~(t)∈R3_{B}\tilde{\bold{a}}(t) \in \mathbb{R}^3Ba~(t)∈R3 传感器的加速度； Wg_{W}\bold{g}Wg 是世界坐标中的重力向量。这里忽略了地球自转的影响，这相当于假设W是一个惯性系。

现在的目标是从IMU测量中推断系统的运动。为此，引入以下运动学模型[38,39]:
R˙WB=RWBBωWB∧,wv˙=wa,wp˙=wv(22)\dot{\mathrm{R}}_{\mathrm{WB}}=\mathrm{R}_{\mathrm{WB}} ~~{ }_{\mathrm{B}} \boldsymbol{\omega}_{\mathrm{WB}}^{\wedge}, \quad{ }_{\mathrm{w}} \dot{\mathbf{v}}={ }_{\mathrm{w}} \mathbf{a}, \quad{ }_{\mathrm{w}} \dot{\mathbf{p}}={ }_{\mathrm{w}} \mathbf{v} \tag{22} R˙WB=RWB BωWB∧,wv˙=wa,wp˙=wv(22)
这描述了B的姿态和速度的演变。

t+∆tt+∆tt+∆t 时刻的状态由式(22)积分得到。应用欧拉积分，假设 Wa_W\bold{a}Wa 和 BωWB_B\boldsymbol{\omega}_{WB}BωWB 在区间 [t,t+∆t][t, t +∆t][t,t+∆t] 中保持恒定，得到:
RWB(t+Δt)=RWB(t)Exp⁡(BωWB(t)Δt)wv(t+Δt)=wv(t)+wa(t)Δtwp(t+Δt)=wp(t)+wv(t)Δt+12wa(t)Δt2.(23)\begin{aligned} \mathrm{R}_{\mathrm{WB}}(t+\Delta t) &=\mathrm{R}_{\mathrm{WB}}(t) \operatorname{Exp}\left({ }_{\mathrm{B}} \boldsymbol{\omega}_{\mathrm{WB}}(t) \Delta t\right) \\ {}_{\mathrm{w}} \mathbf{v}(t+\Delta t) &={ }_{\mathrm{w}} \mathbf{v}(t)+{ }_{\mathrm{w}} \mathbf{a}(t) \Delta t \\ {}_{\mathrm{w}} \mathbf{p}(t+\Delta t) &={ }_{\mathrm{w}} \mathbf{p}(t)+{ }_{\mathrm{w}} \mathbf{v}(t) \Delta t+\frac{1}{2}{ }_{\mathrm{w}} \mathbf{a}(t) \Delta t^{2} . \end{aligned} \tag{23} RWB(t+Δt)wv(t+Δt)wp(t+Δt)=RWB(t)Exp(BωWB(t)Δt)=wv(t)+wa(t)Δt=wp(t)+wv(t)Δt+21wa(t)Δt2.(23)
使用公式 (20)-(21)，根据IMU测量可以计算 Wa_W\bold{a}Wa 和 BωWB_B\boldsymbol{\omega}_{WB}BωWB ，因此(23)为：
R(t+Δt)=R(t)Exp⁡((ω~(t)−bg(t)−ηgd(t))Δt)v(t+Δt)=v(t)+gΔt+R(t)(a~(t)−ba(t)−ηad(t))Δtp(t+Δt)=p(t)+v(t)Δt+12gΔt2+12R(t)(a~(t)−ba(t)−ηad(t))Δt2(24)\begin{aligned} \mathrm{R}(t+\Delta t) &=\mathrm{R}(t) \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}(t)-\mathbf{b}^{g}(t)-\boldsymbol{\eta}^{g d}(t)\right) \Delta t\right) \\ \mathbf{v}(t+\Delta t) &=\mathbf{v}(t)+\mathbf{g} \Delta t+\mathrm{R}(t)\left(\tilde{\mathbf{a}}(t)-\mathbf{b}^{a}(t)-\boldsymbol{\eta}^{a d}(t)\right) \Delta t \\ \mathbf{p}(t+\Delta t) &=\mathbf{p}(t)+\mathbf{v}(t) \Delta t+\frac{1}{2} \mathbf{g} \Delta t^{2} \\ &+\frac{1}{2} \mathrm{R}(t)\left(\tilde{\mathbf{a}}(t)-\mathbf{b}^{a}(t)-\boldsymbol{\eta}^{a d}(t)\right) \Delta t^{2} \end{aligned} \tag{24} R(t+Δt)v(t+Δt)p(t+Δt)=R(t)Exp((ω~(t)−bg(t)−ηgd(t))Δt)=v(t)+gΔt+R(t)(a~(t)−ba(t)−ηad(t))Δt=p(t)+v(t)Δt+21gΔt2+21R(t)(a~(t)−ba(t)−ηad(t))Δt2(24)
为了可读性去掉了坐标帧的下标。离散时间噪声 ηgd\boldsymbol{\eta}^{gd}ηgd 的协方差是采样率的函数，与连续时间谱噪声 ηg\boldsymbol{\eta}^gηg 有关： Cov(ηgd(t))=1ΔtCov(ηg(t))\text{Cov}(\boldsymbol{\eta}^{gd}(t)) = \frac{1}{\Delta t} \text{Cov}(\boldsymbol{\eta}^{g}(t))Cov(ηgd(t))=Δt1Cov(ηg(t)) ； ηad\boldsymbol{\eta}^{ad}ηad 也一样，参考附录。

在流形上的IMU预积分

本节包含本文的第一个关键贡献。虽然Eq.(24)可以很容易地被看作因子图中的概率约束，但它需要在因子图中以较高的速率包含状态。在这里，表明在 k=ik = ik=i 和 k=jk = jk=j 时刻两个关键帧之间的所有测量可以整合为一个单一的复合测量，称为预积分IMU测量，它约束了连续关键帧之间的运动。这个概念首先在[26]中提出（使用欧拉角），本文通过发展一个适用于流形上预积分的理论来扩展它。

假设IMU与相机同步，并在离散时间 kkk 提供测量(参见图5) 。

对于 k=ik = ik=i 和 k=jk = jk=j 之间的所有 ∆t∆t∆t 区间，迭代IMU积分(24)(c.f，图5)，发现:
Rj=Ri∏k=ij−1Exp⁡((ω~k−bkg−ηkgd)Δt)vj=vi+gΔtij+∑k=ij−1Rk(a~k−bka−ηkad)Δtpj=pi+∑k=ij−1vkΔt+12gΔtij2+12∑k=ij−1Rk(a~k−bka−ηkad)Δt2(25)\mathrm{R}_{j}=\mathrm{R}_{i} \prod_{k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{k}^{g}-\boldsymbol{\eta}_{k}^{g d}\right) \Delta t\right) \\ \mathbf{v}_{j}=\mathbf{v}_{i}+\mathbf{g} \Delta t_{i j}+\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{p}_{j}=\mathbf{p}_{i}+\sum_{k=i}^{j-1} \mathbf{v}_{k} \Delta t+\frac{1}{2} \mathbf{g} \Delta t_{i j}^{2}+\frac{1}{2} \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^{2} \tag{25} Rj=Rik=i∏j−1Exp((ω~k−bkg−ηkgd)Δt)vj=vi+gΔtij+k=i∑j−1Rk(a~k−bka−ηkad)Δtpj=pi+k=i∑j−1vkΔt+21gΔtij2+21k=i∑j−1Rk(a~k−bka−ηkad)Δt2(25)
引入缩写： Δtij≡∑k=ijΔt\Delta t_{ij} \equiv \sum_{k=i}^{j} \Delta tΔtij≡∑k=ijΔt ， (⋅)i≡(⋅)(ti)(\cdot)_i \equiv (\cdot)(t_i)(⋅)i≡(⋅)(ti) 来增强可读性。

虽然Eq.(25)已经提供了时间 tit_iti 和 tjt_jtj 之间运动的估计，但它有一个缺点，即**(25)中的积分必须在时间 tit_iti 的线性化点发生变化时重新计算**（直观地说，旋转 Ri\bold{R}_iRi 的变化意味着所有未来的旋转 Rk,k=i,⋯,j−1\bold{R}_k,k=i,\cdots,j-1Rk,k=i,⋯,j−1 的变化，并且有必要重新计算(25)中的求和与乘积）。

本文的目标是避免重复积分。为此定义了以下相对运动增量，它们与 tit_iti 时刻的位姿和速度无关:
ΔRij≐Ri⊤Rj=∏k=ij−1Exp⁡((ω~k−bkg−ηkgd)Δt)Δvij≐Ri⊤(vj−vi−gΔtij)=∑k=ij−1ΔRik(a~k−bka−ηkad)ΔtΔpij≐Ri⊤(pj−pi−viΔtij−12gΔtij2)=∑k=ij−1[ΔvikΔt+12ΔRik(a~k−bka−ηkad)Δt2]=∑k=ij−1[32ΔRik(a~k−bka−ηkad)Δt2](26)\begin{aligned} \Delta \mathrm{R}_{i j} & \doteq \mathrm{R}_{i}^{\top} \mathrm{R}_{j}=\prod_{k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{k}^{g}-\boldsymbol{\eta}_{k}^{g d}\right) \Delta t\right) \\ \Delta \mathbf{v}_{i j} & \doteq \mathrm{R}_{i}^{\top}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \Delta t_{i j}\right)=\sum_{k=i}^{j-1} \Delta \mathrm{R}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{k}^{a}-\boldsymbol{\eta}_{k}^{a d}\right) \Delta t \\ \Delta \mathbf{p}_{i j} & \doteq \mathrm{R}_{i}^{\top}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \Delta t_{i j}-\frac{1}{2} \mathbf{g} \Delta t_{i j}^{2}\right) \\ &=\sum_{k=i}^{j-1}\left[\Delta \mathbf{v}_{i k} \Delta t+\frac{1}{2} \Delta \mathrm{R}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{k}^{a}-\boldsymbol{\eta}_{k}^{a d}\right) \Delta t^{2}\right] \\ &=\sum_{k=i}^{j-1}\left[\frac{3}{2} \Delta \mathrm{R}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{k}^{a}-\boldsymbol{\eta}_{k}^{a d}\right) \Delta t^{2}\right] \end{aligned} \tag{26} ΔRijΔvijΔpij≐Ri⊤Rj=k=i∏j−1Exp((ω~k−bkg−ηkgd)Δt)≐Ri⊤(vj−vi−gΔtij)=k=i∑j−1ΔRik(a~k−bka−ηkad)Δt≐Ri⊤(pj−pi−viΔtij−21gΔtij2)=k=i∑j−1[ΔvikΔt+21ΔRik(a~k−bka−ηkad)Δt2]=k=i∑j−1[23ΔRik(a~k−bka−ηkad)Δt2](26)
其中定义 ΔRik≡RiTRk\Delta \bold{R}_{ik} \equiv \bold{R}_{i}^{T} \bold{R}_{k}ΔRik≡RiTRk ， Δvik≡vkvi\Delta \bold{v}_{ik} \equiv \bold{v}_{k} \bold{v}_{i}Δvik≡vkvi 。

(26)中的求和与乘积仍然是bias的函数。分两步解决这个问题。在V-A节中，假设 bi\bold{b}_ibi 是已知的; 然后在V-C节中展示了当bias估计发生变化时如何避免重复积分。在本文的其余部分中，假设两个关键帧之间的bias保持不变：
big=bi+1g=…=bj−1g,bia=bi+1a=…=bj−1a(27)\mathbf{b}_{i}^{g}=\mathbf{b}_{i+1}^{g}=\ldots=\mathbf{b}_{j-1}^{g}, \quad \mathbf{b}_{i}^{a}=\mathbf{b}_{i+1}^{a}=\ldots=\mathbf{b}_{j-1}^{a} \tag{27} big=bi+1g=…=bj−1g,bia=bi+1a=…=bj−1a(27)

A.预积分IMU测量

Exp函数的线性化：
Exp⁡(ϕ+δϕ)≈Exp⁡(ϕ)Exp⁡(Jr(ϕ)δϕ)(7)\operatorname{Exp}(\phi+\delta \phi) \approx \operatorname{Exp}(\boldsymbol{\phi}) \operatorname{Exp}\left(\mathrm{J}_{r}(\boldsymbol{\phi}) \delta \boldsymbol{\phi}\right) \tag{7} Exp(ϕ+δϕ)≈Exp(ϕ)Exp(Jr(ϕ)δϕ)(7)
Jr(ϕ)\bold{J}_r(\boldsymbol{\phi})Jr(ϕ) 是SO(3)的右雅可比，其将tangent空间中的加性增量与应用于右侧的乘法增量联系起来:
Jr(ϕ)=I−1−cos⁡(∥ϕ∥)∥ϕ∥2ϕ∧+∥ϕ∥−sin⁡(∥ϕ∥)∥ϕ3∥(ϕ∧)2(8)\mathrm{J}_{r}(\phi)=\mathbf{I}-\frac{1-\cos (\|\phi\|)}{\|\phi\|^{2}} \phi^{\wedge}+\frac{\|\phi\|-\sin (\|\phi\|)}{\left\|\phi^{3}\right\|}\left(\phi^{\wedge}\right)^{2} \tag{8} Jr(ϕ)=I−∥ϕ∥21−cos(∥ϕ∥)ϕ∧+∥ϕ3∥∥ϕ∥−sin(∥ϕ∥)(ϕ∧)2(8)

SO(3)伴随矩阵的性质：
RExp⁡(ϕ)R⊤=exp⁡(Rϕ∧R⊤)=Exp⁡(Rϕ)⇔Exp⁡(ϕ)R=RExp⁡(R⊤ϕ)(11)\begin{aligned} \mathrm{R} \operatorname{Exp}(\phi) \mathrm{R}^{\top} &=\exp \left(\mathrm{R} \phi^{\wedge} \mathrm{R}^{\top}\right)=\operatorname{Exp}(\mathrm{R} \phi) \\ \Leftrightarrow \quad \operatorname{Exp}(\phi) \mathrm{R} &=\mathrm{R} \operatorname{Exp}\left(\mathrm{R}^{\top} \boldsymbol{\phi}\right) \end{aligned} \tag{11} RExp(ϕ)R⊤⇔Exp(ϕ)R=exp(Rϕ∧R⊤)=Exp(Rϕ)=RExp(R⊤ϕ)(11)

在本节中假设 tit_iti 时刻的bias是已知的，现在想要分离(26)中的噪声。从旋转增量 ∆Rij∆\bold{R}_{ij}∆Rij 开始，使用一阶近似公式(7)（旋转噪声是“小”的）并重新排列项，使用式(11)将噪声“移动”到最后:
KaTeX parse error: Expected 'EOF', got '&' at position 71: …\mathrm{R}_{j} &̲=\prod_{k=i}^{j…
公式变形：
ΔRij=(eq.7)∏k=ij−1[Exp⁡((ω~k−big)Δt)Exp⁡(−JrkηkgdΔt)]=(eq.11)ΔR~ij∏k=ij−1Exp⁡(−ΔR~k+1j⊤JrkηkgdΔt)≐ΔR~ijExp⁡(−δϕij)(28)\begin{aligned} \Delta \mathrm{R}_{i j} & \stackrel{\text{(eq.7)}}{=} \prod_{k=i}^{j-1}\left[\operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right) \operatorname{Exp}\left(-\mathrm{J}_{r}^{k} \boldsymbol{\eta}_{k}^{g d} \Delta t\right)\right] \\ & \stackrel{\text{(eq.11)}}{=} \Delta \tilde{\mathrm{R}}_{i j} \prod_{k=i}^{j-1} \operatorname{Exp}\left(-\Delta \tilde{\mathrm{R}}_{k+1 j}^{\top} \mathrm{J}_{r}^{k} \boldsymbol{\eta}_{k}^{g d} \Delta t\right) \\ & \doteq \Delta \tilde{\mathrm{R}}_{i j} \operatorname{Exp}\left(-\delta \boldsymbol{\phi}_{i j}\right) \end{aligned} \tag{28} ΔRij=(eq.7)k=i∏j−1[Exp((ω~k−big)Δt)Exp(−JrkηkgdΔt)]=(eq.11)ΔR~ijk=i∏j−1Exp(−ΔR~k+1j⊤JrkηkgdΔt)≐ΔR~ijExp(−δϕij)(28)
其中 Jrk≡Jrk(ω~k−big)\bold{J}^k_r \equiv \bold{J}^k_r ( \tilde{\boldsymbol{\omega}}_k - \bold{b}_i^g )Jrk≡Jrk(ω~k−big) 。定义预积分旋转测量 ΔR~ij≡∏k=ij−1Exp((ω~k−big)Δt)\Delta \tilde{\bold{R}}_{ij} \equiv \prod_{k=i}^{j-1} \text{Exp}( (\tilde{\boldsymbol{\omega}}_k - \bold{b}_i^g) \Delta t )ΔR~ij≡∏k=ij−1Exp((ω~k−big)Δt) ，其噪声 δϕij\delta \boldsymbol{\phi}_{ij}δϕij 将在下一节中进一步分析。

反对称矩阵的性质：
a∧b=−b∧a,∀a,b∈R3(2)\mathbf{a}^{\wedge} \mathbf{b}=-\mathbf{b}^{\wedge} \mathbf{a}, \quad \forall \mathbf{a}, \mathbf{b} \in \mathbb{R}^{3} \tag{2} a∧b=−b∧a,∀a,b∈R3(2)
指数映射到一阶近似：
exp⁡(ϕ∧)≈I+ϕ∧(4)\exp \left(\phi^{\wedge}\right) \approx \mathbf{I}+\phi^{\wedge} \tag{4} exp(ϕ∧)≈I+ϕ∧(4)

将(28)带入到(26)，使用(4)对于 Exp(−δϕij)\text{Exp}(-\delta \boldsymbol{\phi}_{ij})Exp(−δϕij) 使用一阶近似，并扔掉高阶的噪声项，可以获得：
Δvij≐Ri⊤(vj−vi−gΔtij)=∑k=ij−1ΔRik(a~k−bka−ηkad)Δt=∑k=ij−1(ΔRik(a~k−bka)Δt−ΔRikηkadΔt)代入(28)=∑k=ij−1(ΔR~ijExp⁡(−δϕij)(a~k−bka)Δt−ΔR~ijExp⁡(−δϕij)ηkadΔt)\Delta \mathbf{v}_{i j} \doteq \mathrm{R}_{i}^{\top}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \Delta t_{i j}\right) \\ =\sum_{k=i}^{j-1} \Delta \mathrm{R}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{k}^{a}-\boldsymbol{\eta}_{k}^{a d}\right) \Delta t \\ =\sum_{k=i}^{j-1} (\Delta \mathrm{R}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{k}^{a} \right) \Delta t - \Delta \mathrm{R}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t )\\ \text{代入(28)} \\ =\sum_{k=i}^{j-1} (\Delta \tilde{\mathrm{R}}_{i j} \operatorname{Exp}\left(-\delta \boldsymbol{\phi}_{i j}\right) \left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{k}^{a} \right) \Delta t - \Delta \tilde{\mathrm{R}}_{i j} \operatorname{Exp}\left(-\delta \boldsymbol{\phi}_{i j}\right) \boldsymbol{\eta}_{k}^{a d} \Delta t ) Δvij≐Ri⊤(vj−vi−gΔtij)=k=i∑j−1ΔRik(a~k−bka−ηkad)Δt=k=i∑j−1(ΔRik(a~k−bka)Δt−ΔRikηkadΔt)代入(28)=k=i∑j−1(ΔR~ijExp(−δϕij)(a~k−bka)Δt−ΔR~ijExp(−δϕij)ηkadΔt)
变形：
Δvij≃eq.(4) ∑k=ij−1ΔR~ik(I−δϕik∧)(a~k−bia)Δt−ΔR~ikηkadΔt=eq.(2) Δv~ij+∑k=ij−1[ΔR~ik(a~k−bia)∧δϕikΔt−ΔR~ikηkadΔt]≐Δv~ij−δvij(29)\Delta \mathbf{v}_{i j} \stackrel{\text { eq.(4) }}{\simeq} \sum_{k=i}^{j-1} \Delta \tilde{\mathrm{R}}_{i k}\left(\mathbf{I}-\delta \boldsymbol{\phi}_{i k}^{\wedge}\right)\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right) \Delta t-\Delta \tilde{\mathrm{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t \\ \stackrel{\text { eq.(2) }}{=} \Delta \tilde{\mathbf{v}}_{i j}+\sum_{k=i}^{j-1}\left[\Delta \tilde{\mathrm{R}}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \delta \phi_{i k} \Delta t-\Delta \tilde{\mathrm{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t\right] \\ \doteq \Delta \tilde{\mathbf{v}}_{i j}-\delta \mathbf{v}_{i j} \tag{29} Δvij≃ eq.(4) k=i∑j−1ΔR~ik(I−δϕik∧)(a~k−bia)Δt−ΔR~ikηkadΔt= eq.(2) Δv~ij+k=i∑j−1[ΔR~ik(a~k−bia)∧δϕikΔt−ΔR~ikηkadΔt]≐Δv~ij−δvij(29)
其中定义了预积分速度测量 Δv~ij≡∑k=ij−1ΔR~ik(a~k−bia)Δt\Delta \tilde{\bold{v}}_{ij} \equiv \sum_{k=i}^{j-1} \Delta \tilde{\bold{R}}_{ik} ( \tilde{\bold{a}}_{k} - \bold{b}_i^a )\Delta tΔv~ij≡∑k=ij−1ΔR~ik(a~k−bia)Δt 和它的噪声 δvij\delta \bold{v}_{ij}δvij 。

同理，将(28)代入(26)中 Δpij\Delta \bold{p}_{ij}Δpij 的表达式中，利用一阶近似(4)可得:
Δpij=∑k=ij−1[32ΔRik(a~k−bka−ηkad)Δt2]代入(28)Δpij=∑k=ij−1[32ΔR~ijExp⁡(−δϕij)(a~k−bka−ηkad)Δt2]\Delta \mathbf{p}_{i j} = \sum_{k=i}^{j-1}\left[\frac{3}{2} \Delta \mathrm{R}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{k}^{a}-\boldsymbol{\eta}_{k}^{a d}\right) \Delta t^{2}\right] \\ \text{代入(28)}\\ \Delta \mathbf{p}_{i j} = \sum_{k=i}^{j-1}\left[\frac{3}{2} \Delta \tilde{\mathrm{R}}_{i j} \operatorname{Exp}\left(-\delta \boldsymbol{\phi}_{i j}\right) \left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{k}^{a}-\boldsymbol{\eta}_{k}^{a d}\right) \Delta t^{2}\right] Δpij=k=i∑j−1[23ΔRik(a~k−bka−ηkad)Δt2]代入(28)Δpij=k=i∑j−1[23ΔR~ijExp(−δϕij)(a~k−bka−ηkad)Δt2]
变形：
Δpij≃eq.(4)∑k=ij−132ΔR~ik(I−δϕik∧)(a~k−bia)Δt2−∑k=ij−132ΔR~ikηkadΔt2=eq.(2) Δp~ij+∑k=ij−1[32ΔR~ik(a~k−bia)∧δϕikΔt2−32ΔR~ikηkadΔt2]≐Δp~ij−δpij(30)\Delta \mathbf{p}_{i j} \stackrel{\text { eq.(4)}}{\simeq} \sum_{k=i}^{j-1} \frac{3}{2} \Delta \tilde{\mathrm{R}}_{i k}\left(\mathbf{I}-\delta \boldsymbol{\phi}_{i k}^{\wedge}\right)\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right) \Delta t^{2}-\sum_{k=i}^{j-1} \frac{3}{2} \Delta \tilde{\mathrm{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t^{2} \\ \stackrel{\text { eq.(2) }}{=} \Delta \tilde{\mathbf{p}}_{i j}+\sum_{k=i}^{j-1}\left[\frac{3}{2} \Delta \tilde{\mathrm{R}}_{i k}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \delta \boldsymbol{\phi}_{i k} \Delta t^{2}-\frac{3}{2} \Delta \tilde{\mathrm{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t^{2}\right] \\ \doteq \Delta \tilde{\mathbf{p}}_{i j}-\delta \mathbf{p}_{i j} \tag{30} Δpij≃ eq.(4)k=i∑j−123ΔR~ik(I−δϕik∧)(a~k−bia)Δt2−k=i∑j−123ΔR~ikηkadΔt2= eq.(2) Δp~ij+k=i∑j−1[23ΔR~ik(a~k−bia)∧δϕikΔt2−23ΔR~ikηkadΔt2]≐Δp~ij−δpij(30)
其中定义了预积分位置测量 Δp~ij\Delta \tilde{\bold{p}}_{ij}Δp~ij 和它的噪声 δpij\delta \bold{p}_{ij}δpij 。

将(28),(29),(30)代入(26)，最终得到预积分测量模型：
ΔR~ij=Ri⊤RjExp⁡(δϕij)Δv~ij=Ri⊤(vj−vi−gΔtij)+δvijΔp~ij=Ri⊤(pj−pi−viΔtij−12gΔtij2)+δpij(31)\begin{aligned} \Delta \tilde{\mathrm{R}}_{i j} &=\mathrm{R}_{i}^{\top} \mathrm{R}_{j} \operatorname{Exp}\left(\delta \phi_{i j}\right) \\ \Delta \tilde{\mathbf{v}}_{i j} &=\mathrm{R}_{i}^{\top}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \Delta t_{i j}\right)+\delta \mathbf{v}_{i j} \\ \Delta \tilde{\mathbf{p}}_{i j} &=\mathrm{R}_{i}^{\top}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \Delta t_{i j}-\frac{1}{2} \mathbf{g} \Delta t_{i j}^{2}\right)+\delta \mathbf{p}_{i j} \end{aligned} \tag{31} ΔR~ijΔv~ijΔp~ij=Ri⊤RjExp(δϕij)=Ri⊤(vj−vi−gΔtij)+δvij=Ri⊤(pj−pi−viΔtij−21gΔtij2)+δpij(31)
其中复合测量被写成(待估计)状态“加上”随机噪声的函数，由随机向量描述 [δϕijT,δvijT,δpijT]T[\delta \boldsymbol{\phi}_{ij}^T,\delta \bold{v}_{ij}^T,\delta \bold{p}_{ij}^T]^T[δϕijT,δvijT,δpijT]T 。下一节将讨论噪声项的性质。

流形上的预积分（上）相关推荐

H5实现多图片预览上传，可点击可拖拽控件介绍
在做图片上传时发现一个蛮好用的控件,支持多张图片同时上传,可以点击选择图片,也可以将图片拖拽到上传框直接上传,方便,好用,接口也简单,基本可以直接放到项目里使用. 先看看他的样式: 选择图片后: $( ...
ROS上同时预览depth,IR,RGB 调试记录
ROS上同时预览depth,IR,RGB 调试记录用rviz同时显示RGB,IR,DEPTH(验证设备:astraprosm,canglong2,deeyea) 1.编译libuvc库 cd lib ...
Make GNN Great Again：图神经网络上的预训练和自监督学习
来源:RUC AI Box本文约6500字,建议阅读13分钟本文梳理近年来 GNN预训练和自监督学习/对比学习的相关工作. 1 引言近些年来,对图神经网络(GNN)的研究如火如荼.通过设计基于 GN ...
Angular6自定义指令实现多图片上传预览
在做移动端开发多时候经常会遇到用户图片上传的需求,有单图片上传预览的需求,也有多图片上传预览的需求.自己刚遇到这个需求的时候有踩到各种个样到坑.经过多番尝试,下面将本人成功的一个案例分享出来(公司对外 ...
php 图片上传预览(转)
网上找的图片上传预览: <!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http:/ ...
js实现图片上传预览及进度条
js实现图片上传预览及进度条原文js实现图片上传预览及进度条最近在做图片上传的时候,由于产品设计的比较fashion,上网找了比较久还没有现成的,因此自己做了一个,实现的功能如下: 1:去除浏览器 ...
JAVA实现一个图片上传预览功能
这个小项目主要使用java实现了一个简单的图片上传预览功能,废话不多说,先上实现成果 ^ _ ^
java 图片上传预览 demo_图片上传预览
[实例简介] 实现图片上传预览,可以增加新的空数组,并上传和替换.还有删除功能:提交的时候,还可以判断是否有空的img [实例截图] [核心代码] 613ddc50-96b8-4197-ba2e-1e ...

流形上的预积分（上）