预积分学习

作者: 陈瓜瓜_ARPG | 来源:发表于2019-05-19 23:04 被阅读0次

以前学习预积分的时候没有整理笔记，现在把一些知识点写下以防忘记。预积分是避免在优化中重复积分IMU的策略。本篇文章基于论文<IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation>分析，对其中一些重要概念进行解读，难点公式进行推导。

一些概念

manifold(流形)：来源于wiki https://en.wikipedia.org/wiki/Manifold 。流行的数学定义其实不好理解，涉及到拓扑学的一些定义。但是可以有比较形象的说法。比如线和圆圈是一维流形，平面圆面是二维流形。不标准的说，我们熟悉的欧几里得空间是"四四方方"的空间，而流形则一般是"弯弯曲曲"的空间。
流形的小区域可以视为欧几里得空间。好比地球表面其实近似为球面(流形)，但我们平常看到的周围像平的一样(欧几里得空间)。论文<IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation>里说道 $SO(3)$ 群形成了一个平滑的流形。再看论文里的Fig2.就大概知道 $SO(3)$ 群形成了一个弯曲的空间，而这个空间某一点的局部空间或者说正切空间(tangent space)就是欧几里得空间，记为 $so(3)$ 。整个流形的正切空间也被称为流形的李代数。正切空间可以用skew symmetric matrix表示，而每一个skew symmetric matrix可以用一个向量表示(论文公式一)
注意理清楚他们之间的关系。
Lifting: 把应该在 $SO(3)$ 上解决的优化问题变为在 $so(3)$ 对应的向量上解决，称之为lifting。论文公式(14)。
Retraction:把结果的向量变换到 $so(3)$ 上(变为skew symmetric matrix)再重新映射到 $SO(3)$ 上称之为Retraction。对应论文式子(6)的Exp，注意是大写的Exp。小写的exp是把skew symmetric matrix的映射到 $SO(3)$ ，而大写的Exp是把 $so(3)$ 对应的向量映射到 $SO(3)$ ，本质是一样的。

部分公式推导

论文里的式28，这里我自己先写出来(MD这个公式写起来可真复杂)
$\begin{align} \Delta R_{ij} & \simeq \prod_{k=i}^{j-1}[\mathrm{Exp}((\tilde{\boldsymbol{\omega}}_k - \mathrm{\textbf{b}}_i^g)\Delta t )\mathrm{Exp}(-J_r^k\boldsymbol{\eta}_k^{gd}\Delta t)] \\ & = \Delta \tilde R_{ij} \prod_{k=i}^{j-1} \mathrm{Exp}(- \Delta \tilde{ R}_{k+1j}^T J_r^k\boldsymbol{\eta}_k^{gd}\Delta t) \\ & = \Delta \tilde R_{ij} \mathrm{Exp}(\delta \phi_{ij}) \end{align}$
第一行式子由论文式7推出很好理解，第一行到第二行就相对难了，论文里写的根据式(11)，具体是怎么根据11推出的呢？我们先把第一行的右边式子展开
$\begin{align} &\prod_{k=i}^{j-1}[\mathrm{Exp}((\tilde{\boldsymbol{\omega}}_k - \mathrm{\textbf{b}}_i^g)\Delta t )\mathrm{Exp}(-J_r^k\boldsymbol{\eta}_k^{gd}\Delta t)] \\ & = \mathrm{Exp}((\tilde{\boldsymbol{\omega}}_i - \mathrm{\textbf{b}}_i^g)\Delta t )\mathrm{Exp}(-J_r^i\boldsymbol{\eta}_i^{gd}\Delta t) * \mathrm{Exp}((\tilde{\boldsymbol{\omega}}_{i+1} - \mathrm{\textbf{b}}_{i+1}^g)\Delta t )\mathrm{Exp}(-J_r^{i+1}\boldsymbol{\eta}_{i+1}^{gd} \Delta t) \\ & * \mathrm{Exp}((\tilde{\boldsymbol{\omega}}_{i+2} - \mathrm{\textbf{b}}_{i+2}^g)\Delta t )\mathrm{Exp}(-J_r^{i+2}\boldsymbol{\eta}_{i+2}^{gd} \Delta t) ... * \mathrm{Exp}((\tilde{\boldsymbol{\omega}}_{j-2} - \mathrm{\textbf{b}}_{j-2}^g)\Delta t )\mathrm{Exp}(-J_r^{j-2} \boldsymbol{\eta}_{j-2}^{gd} \Delta t) \\ & * \mathrm{Exp}((\tilde{\boldsymbol{\omega}}_{j-1} - \mathrm{\textbf{b}}_{j-1}^g)\Delta t )\mathrm{Exp}(-J_r^{j-1} \boldsymbol{\eta}_{j-1}^{gd} \Delta t) \end{align} \tag{1}$
大家不要被这个长式子吓到，其实就是把连乘的原来的式子展开来写出了 $k=i,i+1,i+2...j-2,j-1$ 五项而已。乘号可有可无的，只是为了区分每一项。为了方便观察，我们令
$\begin{align} A_i \doteq \tilde {\boldsymbol{\omega}}_i - \mathrm{\textbf{b}}_i^g \Delta t \\ B_i \doteq -J_r^k\boldsymbol{\eta}_k^{gd}\Delta t \end{align} \tag{2}$
( $\doteq$ 表示定义，右式定义为左式)。这样这篇文章的式1就可以简写为
$\mathrm{Exp}(A_i)\mathrm{Exp}({B_i})*\mathrm{Exp}(A_{i+1})\mathrm{Exp}({B_{i+1}})*\mathrm{Exp}(A_{i+2})\mathrm{Exp}({B_{i+2}})...*\mathrm{Exp}(A_{j-2})\mathrm{Exp}({B_{j-2}}) * \mathrm{Exp}(A_{j-1})\mathrm{Exp}({B_{j-1}}) \tag{3}$
注意 $\mathrm{Exp}(\bullet)$ 这个操作是把向量 $\bullet$ 映射到 $SO(3)$ ，所以映射完成后的东西本来也就表示一个旋转，我们可以进一步令 $R_{A_i} \doteq \mathrm{Exp}(A_i)$ 。这样式子3就可以进一步简写为(进一步省略了最后两项)
$R_{A_i}\mathrm{Exp}({B_i}) * R_{A_{i+1}}\mathrm{Exp}({B_{i+1}}) * R_{A_{i+2}}\mathrm{Exp}({B_{i+2}}) * R_{A_{i+3}}\mathrm{Exp}({B_{i+3}})... \tag{4}$
注意这儿我们当然也可以令 $R_{B_i} \doteq \mathrm{Exp}({B_i})$ 之类的，但是我们没有这样做。后面的推导会显示出原因。现在就是原论文式(11)大显神通的时候了。注意这篇文章式(4)第二和第三项，即 $\mathrm{Exp}({B_i}) * R_{A_{i+1}}$ ，根据论文式(11)，我们可以把它写为 $R_{A_{i+1}}\mathrm{Exp}(R_{A_{i+1}}^TB_i)$ ，放回式(4)我们就可以得到
$R_{A_i}R_{A_{i+1}}\mathrm{Exp}(R_{A_{i+1}}^TB_i)R_{A_{i+2}}\mathrm{Exp}({B_{i+2}})\mathrm{Exp}({B_{i+3}})... \tag{5}$
同时又根据论文式11, $\mathrm{Exp}(R_{A_{i+1}}^TB_i)R_{A_{i+2}} = R_{A_{i+2}} \mathrm{Exp}(R_{A_{i+2}}^T R_{A_{i+1}}^T B_i)$ ，放回式(5)，我们又可以得到
$R_{A_i}R_{A_{i+1}}R_{A_{i+2}}\mathrm{Exp}(R_{A_{i+2}}^T R_{A_{i+1}}^TB_i)\mathrm{Exp}({B_{i+2}})R_{A_{i+3}}\mathrm{Exp}({B_{i+3}})... \tag{6}$
大家可以看到，我们可以利用论文式11不断把带有 $\mathrm{Exp}({B_{i}})$ 部分往后移动，或者说把 $R_{A_i}$ 部分往前移动。再利用两次论文式11去转变式6，把 $R_{A_{i+3}}$ 往前移动两次
$\begin{align} & R_{A_i}R_{A_{i+1}}R_{A_{i+2}}\mathrm{Exp}(R_{A_{i+2}}^T R_{A_{i+1}}^TB_i)\mathrm{Exp}({B_{i+2}})R_{A_{i+3}}\mathrm{Exp}({B_{i+3}})... \\ & = R_{A_i}R_{A_{i+1}}R_{A_{i+2}}\mathrm{Exp}(R_{A_{i+2}}^T R_{A_{i+1}}^TB_i)R_{A_{i+3}} \mathrm{Exp}(R_{A_{i+3}}^T{B_{i+2}}) \mathrm{Exp}({B_{i+3}})... \\ & = R_{A_i}R_{A_{i+1}}R_{A_{i+2}}R_{A_{i+3}}\mathrm{Exp}(R_{A_{i+3}}^T R_{A_{i+2}}^T R_{A_{i+1}}^TB_i) \mathrm{Exp}(R_{A_{i+3}}^T{B_{i+2}}) \mathrm{Exp}({B_{i+3}})... \\ \end{align}$
所有重复利用论文式(11)，我们可以把 $R_{A_{i+4}},R_{A_{i+5}}...$ 等往前移动,最终得到 $R_{A_i}R_{A_{i+1}} ...R_{A_{j-1}}$ ，这即是论文中的 $\Delta \tilde{R}_{ij}$ 。同时 $\mathrm{Exp}$ 部分我们会有
$\mathrm{Exp}(R_{A_{j-1}}^T... R_{A_{i+1}}^TB_{i})* \mathrm{Exp}(R_{A_{j-1}}^T... R_{A_{i+2}}^TB_{i+1}) * \mathrm{Exp}(R_{A_{j-1}}^T... R_{A_{i+3}}^TB_{i+2})...$ 这即是论文中 $\prod_{k=i}^{j-1} \mathrm{Exp}(- \Delta \tilde{ R}_{k+1j}^T J_r^k\boldsymbol{\eta}_k^{gd}\Delta t)$ 。最终结合为原论文的式(28)的第二行。
这个式子的推导不是一眼就能看出来的，望大家细细斟酌。

未完待续...

预积分学习

一些概念

部分公式推导

相关文章

网友评论

延伸阅读

深度阅读

栏目导航

热点阅读