先複習以前寫過的相關文章：
本文將以參考資料 [1, 2] 的內容為基礎整理出一份完整的筆記。
基本定義
- 所有可逆的 \(3 \times 3\) 矩陣構成 general linear group \(\mathbf{GL}(3,\mathbb{R})\)。
- Orthogonal 的 \(3 \times 3\) 矩陣且行列式為正負 1 構成 orthogonal group \(\mathbf{O}(3)\)。
- \(\mathbf{SO}(3)\)：Special orthogonal group，由 3×3 方陣 \(R\) 組成的群，必須滿足線性關係、\(R R^T=R^T R=I\)、以及行列式為 1。
- SE(3)：\(\{g = (R,T)|R\in \mathbf{SO}(3), T \in \mathbb{R}^3\}\)
常見表示法
- 3D+YPR：3D 位移與 yaw-pitch-roll。這是最直觀的表示法，但是會有 degenerate 的情形，也就是 gimbal lock，在三維空間旋轉文中已經介紹過。
- 3D+Quat：3D 位移與 quaternion。細節在Quaternion 四元數簡介文中已經介紹過。
- 4x4 變換矩陣
- (Eq. 1)：Normalization of a quaternion \[ \mathbf{q'}(\mathbf{q})=\frac{\mathbf{q}}{\left | \mathbf{q} \right |}=\frac{1}{(q_r^2+q_x^2+q_y^2+q_z^2)^{1/2}}\begin{bmatrix} q_r\\ q_x\\ q_y\\ q_z \end{bmatrix} \]
- (Eq. 2)：Jacobian of quaternion normalization \[ \frac{\partial \mathbf{q'}}{\partial \boldsymbol{\mathbf{}q}}=\\ \frac{1}{(q_r^2+q_x^2+q_y^2+q_z^2)^{3/2}} \begin{bmatrix} q_x^2+q_y^2+q_z^2 & -q_r q_x & -q_r q_y & -q_r q_z\\ -q_x q_r & q_r^2+q_y^2 + q_z^2 & -q_x q_y & -q_x q_z\\ -q_yq_r & -q_yq_x & q_r^2+q_x^2+q_z^2 & -q_yq_z\\ -q_zq_r & -q_zq_x & -q_zq_y & q_r^2+q_x^2+q_y^2 \end{bmatrix} \]
表示法之間的轉換
這一章節注重的是表示法之間的轉換以及其對應的 covariance matrix 的轉換。一般來說當一個隨機變數 \(x\) 為常態分布 \(N(\bar{x},\Sigma_x )\) 時，若經由函數 f 轉換成 y：\(y=f(x)\)，則 y 會為常態分布 \(N(\bar{y},\Sigma_y )\)，其中： \[\Sigma_y=\frac{\partial f(x)}{\partial x}_{x=\bar{x}}\Sigma_x\frac{\partial f(x)}{\partial x}_{x=\bar{x}}^T\]
- 3D+YPR to 3D+Quat：(Eq. 3) yaw(\(\phi\)), pitch(\(\chi\)), roll(\(\psi\)) \[ \mathbf{q}(\phi ,\chi ,\psi ):R^3\rightarrow R^4 =\begin{bmatrix} cos\frac{\psi}{2}cos\frac{\chi}{2}cos\frac{\phi}{2}+sin\frac{\psi}{2}sin\frac{\chi}{2}sin\frac{\phi}{2}\\ sin\frac{\psi}{2}cos\frac{\chi}{2}cos\frac{\phi}{2}-cos\frac{\psi}{2}sin\frac{\chi}{2}sin\frac{\phi}{2}\\ cos\frac{\psi}{2}sin\frac{\chi}{2}cos\frac{\phi}{2}+sin\frac{\psi}{2}cos\frac{\chi}{2}sin\frac{\phi}{2}\\ cos\frac{\psi}{2}cos\frac{\chi}{2}sin\frac{\phi}{2}-sin\frac{\psi}{2}sin\frac{\chi}{2}cos\frac{\phi}{2} \end{bmatrix} \]
- 3D+YPR to 3D+Quat covariance：(Eq. 4)
\[
cov(\mathbf{p_7})=\frac{\partial \mathbf{p_7} }{\partial \mathbf{p_6}}cov(\mathbf{p_6})\frac{\partial \mathbf{p_7} }{\partial \mathbf{p_6}}^T
\\
\frac{\partial \mathbf{p_7} }{\partial \mathbf{p_6}}=\begin{pmatrix}
I_3 & 0_{3 \times 3}\\
0_{4 \times 3} & \frac{\partial \mathbf{q}(\phi ,\chi ,\psi )}{\partial \{\phi ,\chi ,\psi\}}_{4 \times 3}
\end{pmatrix}_{7 \times 6}
\]
細節在 [1] 的式 (2.9b)。
- 3D+Quat to 3D+YPR：(Eq. 5)，首先先算 \(\Delta =q_rq_y-q_xq_z\)，若 \(\Delta\) 的絕對值小於 0.5 時：
\[
\left\{\begin{matrix}
\phi = tan^{-1}(2\frac{q_rq_z+q_xq_y}{1-2(q_y^2+q_z^2)})\\
\chi = sin^{-1}(2 \Delta)\\
\psi = tan^{-1}(2\frac{q_rq_x+q_yq_z}{1-2(q_x^2+q_y^2)})
\end{matrix}\right.
\]
而當 \(\Delta\) 為 0.5 或 -0.5 時請參考 [1] 的式 (2.11)。
- 3D+Quat to 3D+YPR covariance：(Eq. 6)
\[
cov(\mathbf{p_6})=\frac{\partial \mathbf{p_6} }{\partial \mathbf{p_7}}cov(\mathbf{p_7})\frac{\partial \mathbf{p_6} }{\partial \mathbf{p_7}}^T
\\
\frac{\partial \mathbf{p_6} }{\partial \mathbf{p_7}}=
\begin{pmatrix}
I_3 & 0_{3 \times 4}\\
0_{3 \times 3} & \frac{\delta(\phi,\chi,\psi)(q_r, q_x, q_y, q_z)}{\delta \{q_r, q_x, q_y, q_z\}}
\end{pmatrix}
\\
\frac{\delta(\phi,\chi,\psi)(q_r, q_x, q_y, q_z)}{\delta \{q_r, q_x, q_y, q_z\}} =
\frac{\partial \delta(\phi,\chi,\psi)(q_r, q_x, q_y, q_z)}{\partial \mathbf{q'}}
\frac{\partial \mathbf{q'}}{\partial \mathbf{q}}
\]上式的第一項為 (Eq. 5) 的 jacobian，而第二項即為 (Eq. 1)。當 quaternion 已經 normalized 時則此項為 \(I_4\)。
- 3D+YPR to matrix：請參考 [1] 式 (2.15)。
- 3D+Quat to matrix：請參考 [1] 式 (2.20)。
- Matrix to 3D+YPR：請參考 [1] 2.5 節，值得一提的是由於變換矩陣的最後一列皆為已知數，所以 \(4 \times 4\) 的變換矩陣自由度為 12 而不是 16。
沒有留言:
張貼留言