三維空間轉換與李理論（二）

先複習以前寫過的相關文章：

• 三維空間旋轉
  
• 關於三維空間轉換的更多筆記
• Quaternion 四元數簡介 
• 李群、李代數與三維空間旋轉 Lie Group & Lie Algebra
• SO(3) 上的李代數求導數 
• 三維空間轉換與李理論（一）
  

本文將以參考資料 [1, 2] 的內容為基礎整理出一份完整的筆記。

利用 Pose 轉換一個三維空間中的點

假設 pose 為 $\mathbf{p}$、點為 $\mathbf{a'}$，則轉換後的點 $\mathbf{a} = \mathbf{p}\oplus  \mathbf{a'}$。

1. 3D+YPR pose：先轉換成 $4 \times 4$ 矩陣再計算新的點 $\mathbf{a}$。
   
2. 3D+YPR pose covariance：(Eq. 7) $$cov(\mathbf{a})= \frac{\partial f(\mathbf{p_6}, \mathbf{a})}{\partial \mathbf{p_6}} cov(\mathbf{p_6}) \frac{\partial f(\mathbf{p_6}, \mathbf{a})}{\partial \mathbf{p_6}}^T + \frac{\partial f(\mathbf{p_6}, \mathbf{a})}{\partial \mathbf{a}} cov(\mathbf{a'}) \frac{\partial f(\mathbf{p_6}, \mathbf{a})}{\partial \mathbf{a}}^T \\ \frac{\partial f(\mathbf{p_6}, \mathbf{a})}{\partial \mathbf{p_6}}_{3 \times 6} =\begin{pmatrix} I_3 & J \end{pmatrix} \\ \frac{\partial f(\mathbf{p_6}, \mathbf{a})}{\partial \mathbf{a}}_{3 \times 3}=R(\phi,\chi,\psi)$$ 其中 $f(\mathbf{p_6}, \mathbf{a})$ 為 $\mathbf{p}\oplus  \mathbf{a'}$，$J$ 的細節在 [1] 的式 (3.3) 中。上面的第三式其實就是 $\mathbf{a} = R \mathbf{a'} + t$，再對 $\mathbf{a'}$ 偏微分的結果。
   
3. 3D+Quat pose：定義 $\mathbf{p_7}=[x, y, z, q_r, q_x, q_y, q_z]$，且 $\mathbf{a}= \mathbf{p_7}\oplus  \mathbf{a'} = f(\mathbf{p_7}, \mathbf{a'})$。(Eq. 8) $$f(\mathbf{p_7}, \mathbf{a'}) = \begin{bmatrix} x+a_x'+2[-(q_y^2+q_z^2)a_x'+(q_xq_y-q_rq_z)a_y'+(q_rq_y+q_xq_z)a_z']\\ y+a_y'+2[(q_rq_z+q_xq_y)a_x'-(q_x^2+q_z^2)a_y'+(q_yq_z-q_rq_x)a_z']\\ z+a_z'+2[(q_xq_z-q_rq_y)a_x'+(q_rq_x+q_yq_z)a_y'-(q_x^2+q_y^2)a_z'] \end{bmatrix}$$
   
4. 3D+Quat pose covariance：(Eq. 9)，過程即為將 (Eq. 8) 計算 jacobian。 $$cov(\mathbf{a})= \frac{\partial f(\mathbf{p_7}, \mathbf{a})}{\partial \mathbf{p_7}} cov(\mathbf{p_7}) \frac{\partial f(\mathbf{p_7}, \mathbf{a})}{\partial \mathbf{p_7}}^T + \frac{\partial f(\mathbf{p_7}, \mathbf{a})}{\partial \mathbf{a}} cov(\mathbf{a'}) \frac{\partial f(\mathbf{p_7}, \mathbf{a})}{\partial \mathbf{a}}^T \\ \frac{\partial f(\mathbf{p_7}, \mathbf{a})}{\partial \mathbf{p_7}}_{3 \times 7} = \begin{bmatrix} 1 & 0 & 0 & \\ 0 & 1 & 0 & \frac{\partial f(\mathbf{p_7}, \mathbf{a})}{\partial [q_r, q_x, q_y, q_z]}\\ 0 & 0 & 1 & \end{bmatrix} \\ \frac{\partial f(\mathbf{p_7}, \mathbf{a})}{\partial [q_r, q_x, q_y, q_z]} = \frac{\partial f(\mathbf{p_7}, \mathbf{a})}{\partial \mathbf{q'}} \frac{\partial \mathbf{q'}}{\partial \mathbf{q}}$$ 其中 $\frac{\partial f(\mathbf{p_7}, \mathbf{a})}{\partial \mathbf{q'}}$ 於 [1] 中的式 (3.9) 有結果， $\frac{\partial f(\mathbf{p_7}, \mathbf{a})}{\partial \mathbf{a}}$ 於 [1] 中的式 (3.10) 有結果。

利用 Pose 轉換反推一個三維空間中的點

前一段介紹的是 $\mathbf{a} = \mathbf{p}\oplus  \mathbf{a'}$，而本節為相反的操作 $\mathbf{a'} = \mathbf{a}\ominus  \mathbf{p}$。大致上內容與前一段差不多，細節都在 [1] 中的第四章。

參考資料

[1] A tutorial on SE(3) transformation parameterizations and on-manifold optimization

[2] A micro Lie theory for state estimation in robotics