美文网首页
预积分学习

预积分学习

作者: 陈瓜瓜_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)的第二行。
    这个式子的推导不是一眼就能看出来的,望大家细细斟酌。

    未完待续...

    相关文章

      网友评论

          本文标题:预积分学习

          本文链接:https://www.haomeiwen.com/subject/ezifzqtx.html