## 四元数矩阵与 so(3) 左右雅可比

2018-05-22

${\bf q}({\boldsymbol \phi}) = [ \sin\frac{\phi}{2} {\bf a}, \, \cos \frac{\phi}{2}]’$, in which $\phi = | \boldsymbol \phi|, \, {\bf a} = \frac{\boldsymbol \phi}{\phi}$

${\bf q}({\boldsymbol \phi}) = [ \frac{1}{2} {\bf \boldsymbol \phi}, \, 1]'$

### 四元数矩阵

${\bf q} \otimes {\bf p} = \begin{bmatrix} q_w p_x - q_z p_y + q_y p_z + q_x p_w \\ q_z p_x + q_w p_y - q_x p_z + q_y p_w \\ -q_y p_x + q_x p_y + q_w p_z + q_z p_w \\ -q_x p_x - q_y p_y - q_z p_z + q_w p_w \\ \end{bmatrix} = {\bf Q}_l({\bf q)p} = {\bf Q}_r{\bf (p)q}$

${\bf Q}_l({\bf q}) = \begin{bmatrix} q_w {\bf I}_3 + {\bf q}_{1:3}^\wedge & {\bf q}_{1:3}\\ - {\bf q}_{1:3}' & q_w \end{bmatrix} , {\bf Q}_r ({\bf q}) = \begin{bmatrix} q_w {\bf I}_3 - {\bf q}_{1:3}^\wedge & {\bf q}_{1:3}\\ - {\bf q}_{1:3}' & q_w \end{bmatrix}$

$\frac{\partial{\bf q \otimes p}}{\partial\bf p} = {\bf Q}_l({\bf q}), \,\frac{\partial{\bf q \otimes p}}{\partial\bf q} = {\bf Q}_r ({\bf p})$

${\bf e}_{\rm qt} = 2( \tilde {\bf q}^{-1}\otimes {\bf q})_{1:3}$

\begin{aligned} \frac{\partial {\bf e}_{\rm qt}}{\partial \boldsymbol \alpha} &= 2\frac{\partial (\tilde{\bf q}^{-1}\otimes{\bf q}\otimes {\bf q}({\boldsymbol \alpha}))}{\partial \boldsymbol \alpha}\Bigg | _{(1:3, :)} \\ &=2\frac{\partial (\tilde{\bf q}^{-1}\otimes{\bf q}\otimes {\bf q}({\boldsymbol \alpha}))}{\partial {\bf q}({\boldsymbol \alpha})} \frac{\partial {\bf q}({\boldsymbol \alpha})}{\partial \boldsymbol \alpha}\Bigg | _{(1:3, :)} \\ &={\bf Q}_l(\tilde{\bf q}^{-1}\otimes{\bf q}) \begin{bmatrix} {\bf I}_3 \\ {\bf 0_{1\times 3}} \end{bmatrix}\Bigg | _{(1:3, :)} \\ &={\bf Q}_l(\tilde{\bf q}^{-1}\otimes{\bf q}) _{(1:3, 1:3)} \end{aligned}

### so(3) 雅可比

${\bf e}_{\rm lie} = {\rm Log}( \tilde {\bf C}' {\bf C})$

\begin{aligned} \frac{\partial {\bf e}_{\rm lie}}{\partial \boldsymbol \alpha} &= \frac{\partial{\rm Log}(\tilde{\bf C}'{\bf C} \,{\rm Exp} ({\boldsymbol \alpha}))}{\partial \boldsymbol \alpha} \\ & = {\rm J}_r({\rm Log(\tilde{\bf C}'{\bf C} )})^{-1}\\ & = {\rm J}_r({\bf e}_{\rm lie})^{-1} \end{aligned}

### 讨论

\begin{aligned} \frac{\partial {\bf e}_ {\rm qt}}{\partial \boldsymbol \alpha} &={\bf Q}_l[{\bf q}({\bf e}_{\rm qt})] _{(1:3, 1:3)}\\ &=q_w({\bf e}_{\rm qt}) {\bf I}_3 + {\bf q}_{1:3}^\wedge({\bf e}_{\rm qt}) \end{aligned}

$\frac{\partial {\bf e}_{\rm qt}}{\partial \boldsymbol \alpha} \approx {\bf I}_3 + \frac{1}{2}{\bf e}_{\rm qt}^\wedge$

$\frac{\partial {\bf e}_{\rm lie}}{\partial \boldsymbol \alpha} = {\rm J}_r({\bf e}_{\rm lie})^{-1} = \frac{\phi_{e}}{2}\cot\frac{\phi_{e}}{2} {\bf I}+(1-\frac{\phi_{e}}{2}\cot\frac{\phi_{e}}{2}){\bf a_{\it e}a_{\it e}}' + \frac{\phi_{e}}{2}{\bf a}_{e}^\wedge$

$\frac{\partial {\bf e}_{\rm lie}}{\partial \boldsymbol \alpha} \approx {\bf I}+\frac{\phi_{e}}{2}{\bf a}_{e}^\wedge={\bf I} + \frac{1}{2}{\bf e}_{\rm lie}^\wedge 。$

### 尾声

$\frac{\partial\boldsymbol \phi}{\partial{\boldsymbol \alpha}}={\bf J}_r({\boldsymbol\phi})^{-1},\quad \frac{\partial{\bf q}_{1:3}}{\partial{\boldsymbol \alpha}}={\bf Q}_l({\bf q}({\boldsymbol\phi}))_{(1:3,1:3)}$

${\bf J}_r({\boldsymbol\phi})^{-1} = \frac{\partial\boldsymbol \phi}{\partial{\bf q}_{1:3}} \,{\bf Q}_l({\bf q}({\boldsymbol\phi}))_{(1:3,1:3)}$

### 参考文献

[1] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, P. Furgale, “Keyframe-based visual-inertial odometry using nonlinear optimization”, Int. Journal of Robotics Research (IJRR), 2014.

[2] C. Forster, L. Carlone, F. Dellaert and D. Scaramuzza, “On-Manifold Preintegration for Real-Time Visual–Inertial Odometry,” in IEEE Transactions on Robotics, vol. 33, no. 1, pp. 1-21, Feb. 2017.

[4] T. Barfoot, State Estimation for Robotics.