0

I am reading article on IMU preintegration: https://rpg.ifi.uzh.ch/docs/TRO16_forster.pdf. I am not too strong in math and I fail to understand how some derivations were created. One of such equations is eq.35 (screenshot for convenience) for rotation increment. It using some of the exponential map properties, which are also listed in the paper. Even so, I still fail to understand how this derivation was performed. Anyone could expand this derivation with intermediate steps and comment on how it should be performed? Some resources with the required math (eg textbooks) related to topic would be also welcomed.

enter image description here

aquila
  • 105

1 Answers1

0

$\def\op#1{\operatorname{#1}}$

Using the first-order approximation $$ \op{Exp} \left[ \phi + \delta \phi \right] \simeq \op{Exp} (\phi) \op{Exp} \left[ \mathbf{J}_r(\phi) \delta \phi \right] $$ it holds \begin{eqnarray} \op{Exp} \left[ \phi_k - \eta_k \Delta t \right] &\simeq & \op{Exp} (\phi_k ) \op{Exp} \left[ - \mathbf{J}_r(\phi_k) \eta_k \Delta t \right] \end{eqnarray} To keep notations simple, let $\mathbf{c}_k = -\mathbf{J}_r(\phi_k) \eta_k \Delta t $ and $\phi_k= (\tilde{w}_k-\mathbf{b})\Delta t$.

It follows $$ \Delta \mathbf{R}_{ij} \simeq \ldots \op{Exp} (\phi_{j-2}) \color{red}{ \op{Exp} \left( \mathbf{c}_{j-2} \right) \op{Exp} (\phi_{j-1}) } \op{Exp} \left( \mathbf{c}_{j-1} \right) $$ But $ \op{Exp} (\phi_{j-1}) = \Delta \tilde{\mathbf{R}}_{j-1,j} $ (without noise) thus we can swap the red terms as in (11) of the linked article.

It follows \begin{eqnarray*} \Delta \mathbf{R}_{ij} &\simeq& \op{Exp} (\phi_{i}) \ldots \op{Exp} (\phi_{j-2}) \Delta \tilde{\mathbf{R}}_{j-1,j} \op{Exp} \left[ \Delta \tilde{\mathbf{R}}_{j-1,j}^T \mathbf{c}_{j-2}\right] \op{Exp} \left[ \mathbf{c}_{j-1} \right] \\ &\simeq & \op{Exp} (\phi_{i}) \ldots \Delta \tilde{\mathbf{R}}_{j-2,j} \prod_{k=j-2}^{j-1} \op{Exp} \left[ \Delta \tilde{\mathbf{R}}_{k+1,j}^T \mathbf{c}_{k} \right] \\ &\simeq & \Delta \tilde{\mathbf{R}}_{i,j} \prod_{k=i}^{j-1} \op{Exp} \left[ \Delta \tilde{\mathbf{R}}_{k+1,j}^T \mathbf{c}_{k} \right] \end{eqnarray*}

Steph
  • 3,665