本系列参考自 Quaternions for Computer Graphics, 四元数与三维旋转 和 视觉SLAM十四讲(第二版)
四元数到旋转矩阵的转换
\[ \begin{align*} \boldsymbol{q_{a}}\boldsymbol{q_{b}} = L(q_{a})q_{b} &=\begin{bmatrix} s_{a} &-x_{a} &-y_{a} &-z_{a} \\ x_{a} &s_{a} &-z_{a} &y_{a} \\ y_{a} &z_{a} &s_{a} &-x_{a} \\ z_{a} &-y_{a} &x_{a} &s_{a} \end{bmatrix} \begin{bmatrix}s_{b}\\ x_{b}\\ y_{b}\\ z_{b}\end{bmatrix} \\ \boldsymbol{q_{a}}\boldsymbol{q_{b}} = R(q_{b})q_{a} &=\begin{bmatrix} s_{b} &-x_{b} &-y_{b} &-z_{b} \\ x_{b} &s_{b} &z_{b} &-y_{b} \\ y_{b} &-z_{b} &s_{b} &x_{b} \\ z_{b} &y_{b} &-x_{b} &s_{b} \end{bmatrix} \begin{bmatrix}s_{a}\\ x_{a}\\ y_{a}\\ z_{a}\end{bmatrix} \end{align*} \]
由此可得:
\[ \begin{align*} pvp^{-1} &=R(p^{-1})L(p)v \\ &=\begin{bmatrix} s &x &y &z \\ -x &s &-z &y \\ -y &z &s &-x \\ -z &-y &x &s \end{bmatrix} \begin{bmatrix} s &-x &-y &-z \\ x &s &-z &y \\ y &z &s &-x \\ z &-y &x &s \end{bmatrix} \begin{bmatrix}0\\ x_{v}\\ y_{v}\\ z_{v}\end{bmatrix}\\ &= \begin{bmatrix} 1 &0 &0 &0 \\ 0 &1-2(y^{2}+z^{2}) &2(xy-sz) &2(xz+sy) \\ 0 &2(xy+sz) &1-2(x^{2}+z^{2}) &2(yz-sx) \\ 0 &2(xz-sy) &2(yz+sx) &1-2(x^{2}+y^{2}) \end{bmatrix} \begin{bmatrix}0\\ x_{v}\\ y_{v}\\ z_{v}\end{bmatrix} \end{align*} \]
忽略第一行和第一列就得到了旋转矩阵:
\[ R=\begin{bmatrix} 1-2(y^{2}+z^{2}) &2(xy-sz) &2(xz+sy) \\ 2(xy+sz) &1-2(x^{2}+z^{2}) &2(yz-sx) \\ 2(xz-sy) &2(yz+sx) &1-2(x^{2}+y^{2}) \end{bmatrix} \]
旋转矩阵到四元数的转换
\[ R=\begin{bmatrix} 1-2(y^{2}+z^{2}) &2(xy-sz) &2(xz+sy) \\ 2(xy+sz) &1-2(x^{2}+z^{2}) &2(yz-sx) \\ 2(xz-sy) &2(yz+sx) &1-2(x^{2}+y^{2}) \end{bmatrix}=\begin{bmatrix} a_{11} & a_{12} & a_{13} \\ a_{21} & a_{22} & a_{23} \\ a_{31} & a_{32} & a_{33} \end{bmatrix} \]
首先很容易观察到: \(a_{11}+a_{22}+a_{33}=3-4(x^{2}+y^{2}+z^{2})=3-4\sin^{2}{\alpha}\), 从而
\[ \begin{align*} s &=\cos{\alpha} \\ &=\pm\sqrt{1-\sin^{2}{\alpha}} \\ &=\pm\sqrt{1-\frac{3-(a_{11}+a_{22}+a_{33})}{4}} \\ &=\pm\sqrt{\frac{1+(a_{11}+a_{22}+a_{33})}{4}} \end{align*} \]
观察可得:
\[ \begin{align*} x=\frac{a_{32}-a_{23}}{4s}\\ y=\frac{a_{13}-a_{31}}{4s}\\ z=\frac{a_{21}-a_{12}}{4s} \end{align*} \]
欧拉角到四元数的转换
通过旋转矩阵中转, 很容易得到欧拉角到四元数的转换.
四元数到旋转向量的转换
前面的推导就是基于旋转向量的, 要反推回去很容易:
\[ \begin{align*} \alpha&=2\arccos{s} \\ \boldsymbol{k} &= \frac{\begin{bmatrix}x,&y,&z\end{bmatrix}^{T}}{\sin{\frac{\alpha}{2}}} \end{align*} \]