先複習以前寫過的相關文章：
- 三維空間旋轉
- 關於三維空間轉換的更多筆記
- Quaternion 四元數簡介
- 李群、李代數與三維空間旋轉 Lie Group & Lie Algebra
- SO(3) 上的李代數求導數
- 三維空間轉換與李理論（一）
本文將以參考資料 [1, 2] 的內容為基礎整理出一份完整的筆記。
利用 Pose 轉換一個三維空間中的點
假設 pose 為 \(\mathbf{p}\)、點為 \(\mathbf{a'}\)，則轉換後的點 \(\mathbf{a} = \mathbf{p}\oplus \mathbf{a'}\)。
- 3D+YPR pose：先轉換成 \(4 \times 4\) 矩陣再計算新的點 \(\mathbf{a}\)。
- 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'}\) 偏微分的結果。
- 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}
\]
- 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] 中的第四章。
參考資料
[2] A micro Lie theory for state estimation in robotics
沒有留言:
張貼留言