回転の相互変換

一覧

オイラー角 → 回転行列 回転行列 → オイラー角
オイラー角 → クォータニオン クォータニオン → オイラー角
クォータニオン → 回転行列 回転行列 → クォータニオン

オイラー角 → 回転行列 EulerToMatrix

EulerToMatrix

入力
eulerAngle LREAL[3] オイラー角(ラジアン)
order keisan\eEulerOrder 回転順

出力
mat LREAL[3,3] 回転行列

【 例 】
入力
eulerAngle [\(pi/9, pi/6, pi/4\)] (20度,30度,45度)
order XYZ
出力
mat \begin{bmatrix} 0.612& -0.612& 0.5 \\ 0.785& 0.544& -0.296 \\ -0.09& 0.574& 0.814 \end{bmatrix}

【 order 列挙体 】
\\keisan\eEulerOrder#
XYZ
XZY
YXZ
YZX
ZXY
ZYX

【 計算式 】
回転順 XYZ \begin{align} &R_{xyz}=R_xR_yR_z \\ &= \begin{bmatrix} cos\theta_y cos\theta_z & -cos\theta_y sin\theta_z & sin\theta_y \\ sin\theta_x sin\theta_y cos\theta_z+cos\theta_x sin\theta_z & -sin\theta_x sin\theta_y sin\theta_z+cos\theta_xcos\theta_z & -sin\theta_x cos\theta_y \\ -cos\theta_x sin\theta_y cos\theta_z+sin\theta_x sin\theta_z & cos\theta_x sin\theta_y sin\theta_z+sin\theta_x cos\theta_z & cos\theta_x cos\theta_y \end{bmatrix} \end{align}
回転順 XZY \begin{align} &R_{xzy}=R_xR_zR_y \\ &= \begin{bmatrix} cos\theta_y cos\theta_z & -sin\theta_z & sin\theta_y cos\theta_z \\ cos\theta_x cos\theta_y sin\theta_z+sin\theta_x sin\theta_y & cos\theta_x cos\theta_z & cos\theta_x sin\theta_y sin\theta_z-sin\theta_x cos\theta_y \\ sin\theta_x cos\theta_y sin\theta_z-cos\theta_x sin\theta_y & sin\theta_x cos\theta_z & sin\theta_x sin\theta_y sin\theta_z+cos\theta_x cos\theta_y \end{bmatrix} \end{align}
回転順 YXZ \begin{align} &R_{yxz}=R_yR_xR_z \\ &= \begin{bmatrix} sin\theta_x sin\theta_y sin\theta_z+cos\theta_y cos\theta_z & sin\theta_x sin\theta_y sin\theta_z-cos\theta_y sin\theta_z & cos\theta_x sin\theta_y \\ cos\theta_x sin\theta_z & cos\theta_x cos\theta_z & -sin\theta_z \\ sin\theta_x cos\theta_y sin\theta_z-sin\theta_y cos\theta_z & sin\theta_x cos\theta_y cos\theta_z+sin\theta_y sin\theta_z & cos\theta_x cos\theta_y \end{bmatrix} \end{align}
回転順 YZX \begin{align} &R_{yzx}=R_yR_zR_x \\ &= \begin{bmatrix} cos\theta_y cos\theta_z & -cos\theta_x cos\theta_y sin\theta_z+sin\theta_x sin\theta_y & sin\theta_x cos\theta_y sin\theta_z+cos\theta_x sin\theta_y \\ sin\theta_x & cos\theta_x cos\theta_z & -sin\theta_x cos\theta_z \\ -sin\theta_y cos\theta_z & cos\theta_x sin\theta_y sin\theta_z+sin\theta_x cos\theta_y & -sin\theta_x sin\theta_y sin\theta_z+cos\theta_x cos\theta_y \end{bmatrix} \end{align}
回転順 ZXY \begin{align} &R_{zxy}=R_zR_xR_y \\ &= \begin{bmatrix} -sin\theta_x sin\theta_y sin\theta_z+cos\theta_y cos\theta_z & -cos\theta_x sin\theta_z+ & sin\theta_x cos\theta_y sin\theta_z+sin\theta_y cos\theta_z \\ sin\theta_x sin\theta_y cos\theta_z+cos\theta_y sin\theta_z & cos\theta_x cos\theta_z & -sin\theta_x cos\theta_y cos\theta_z+sin\theta_y sin\theta_z \\ -cos\theta_x sin\theta_y & sin\theta_x & cos\theta_x cos\theta_y \end{bmatrix} \end{align}
回転順 ZYX \begin{align} &R_{zyx}=R_zR_yR_x \\ &= \begin{bmatrix} cos\theta_y cos\theta_z & sin\theta_x sin\theta_y cos\theta_z-cos\theta_x sin\theta_z & cos\theta_x sin\theta_y cos\theta_z+sin\theta_x sin\theta_z \\ cos\theta_y sin\theta_z & sin\theta_x sin\theta_y sin\theta_z+cos\theta_x cos\theta_z & cos\theta_x sin\theta_y sin\theta_z+sin\theta_x cos\theta_z \\ -sin\theta_y & sin\theta_x cos\theta_y & cos\theta_x cos\theta_y \end{bmatrix} \end{align}


回転行列 → オイラー角 MatrixToEuler

MatrixToEuler

入力
mat LREAL[3,3] 回転行列
order keisan\eEulerOrder 回転順

出力
eulerAngle LREAL[3] オイラー角(ラジアン)

【 例 】
入力
mat \begin{bmatrix} 0.612& -0.612& 0.5 \\ 0.785& 0.544& -0.296 \\ -0.09& 0.574& 0.814 \end{bmatrix}
order XYZ
出力
eulerAngle [\(pi/9, pi/6, pi/4\)] (20度,30度,45度)

【 order 列挙体 】
こちらを参照

【 計算式 】
回転行列の各要素は以下のように表します \[ \begin{bmatrix} m_{00}& m_{01}& m_{02} \\ m_{10}& m_{11}& m_{12} \\ m_{20}& m_{21}& m_{22} \\ \end{bmatrix} \]
回転順 XYZ \[\large \begin{align} \theta_x &= \begin{cases} \arctan \left( -\frac{m_{12}}{m_{22}} \right) &(\cos \theta_y \neq 0) \\ \arctan \left( \frac{m_{21}}{m_{11}} \right) &(otherwise) \end{cases} \\ \theta_y &= \arcsin (m_{02}) \\ \theta_z &= \begin{cases} \arctan \left( -\frac{m_{01}}{m_{00}} \right) &(\cos \theta_y \neq 0) \\ 0 &(otherwise) \end{cases} \\ \end{align} \]
回転順 XZY \[\large \begin{align} \theta_x &= \begin{cases} \arctan \left( \frac{m_{21}}{m_{11}} \right) &(\cos \theta_z \neq 0) \\ \arctan \left( -\frac{m_{12}}{m_{22}} \right) &(otherwise) \end{cases} \\ \theta_y &= \begin{cases} \arctan \left( \frac{m_{02}}{m_{00}} \right) &(\cos \theta_z \neq 0) \\ 0 &(otherwise) \end{cases} \\ \theta_z &= \arcsin (-m_{01}) \\ \end{align} \]
回転順 YXZ \[\large \begin{align} \theta_x &= \arcsin (-m_{12}) \\ \theta_y &= \begin{cases} \arctan \left( \frac{m_{02}}{m_{22}} \right) &(\cos \theta_x \neq 0) \\ \arctan \left( -\frac{m_{20}}{m_{00}} \right) &(otherwise) \end{cases} \\ \theta_z &= \begin{cases} \arctan \left( \frac{m_{10}}{m_{11}} \right) &(\cos \theta_x \neq 0) \\ 0 &(otherwise) \end{cases} \\ \end{align} \]
回転順 YZX \[\large \begin{align} \theta_x &= \begin{cases} \arctan \left( -\frac{m_{12}}{m_{11}} \right) &(\cos \theta_z \neq 0) \\ 0 &(otherwise) \end{cases} \\ \theta_y &= \begin{cases} \arctan \left( -\frac{m_{20}}{m_{00}} \right) &(\cos \theta_z \neq 0) \\ \arctan \left( \frac{m_{02}}{m_{22}} \right) &(otherwise) \\ \end{cases} \\ \theta_z &= \arcsin (m_{10}) \\ \end{align} \]
回転順 ZXY \[\large \begin{align} \theta_x &= \arcsin (m_{21}) \\ \theta_y &= \begin{cases} \arctan \left( -\frac{m_{20}}{m_{22}} \right) &(\cos \theta_x \neq 0) \\ 0 &(otherwise) \end{cases} \\ \theta_z &= \begin{cases} \arctan \left( -\frac{m_{01}}{m_{11}} \right) &(\cos \theta_x \neq 0) \\ \arctan \left( \frac{m_{10}}{m_{00}} \right) &(otherwise) \\ \end{cases} \\ \end{align} \]
回転順 ZYX \[\large \begin{align} \theta_x &= \begin{cases} \arctan \left( \frac{m_{21}}{m_{22}} \right) &(\cos \theta_y \neq 0) \\ 0 &(otherwise) \end{cases} \\ \theta_y &= \arcsin (-m_{20}) \\ \theta_z &= \begin{cases} \arctan \left( \frac{m_{10}}{m_{00}} \right) &(\cos \theta_y \neq 0) \\ \arctan \left( -\frac{m_{01}}{m_{11}} \right) &(otherwise) \\ \end{cases} \\ \end{align} \]


オイラー角 → クォータニオン EulerToQuaternion

EulerToQuaternion

入力
eulerAngle LREAL[3] オイラー角(ラジアン)
order keisan\eEulerOrder 回転順

出力
q keisan\sQuaternion クォータニオン

【 例 】
入力
eulerAngle [\(pi/9, pi/6, pi/4\)] (20度,30度,45度)
order XYZ
出力
q \begin{bmatrix} 0.253 \\ 0.171 \\ 0.406 \\ 0.862 \end{bmatrix}

【 order 列挙体 】
こちらを参照

【 計算式 】
クォータニオンは以下のように表します \[ q = \begin{bmatrix} q_x \\ q_y \\ q_z \\ q_w \\ \end{bmatrix} \]
回転順 XYZ \[\large \begin{bmatrix} \cos \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\sin \frac{\theta_z}{2} + \sin \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\cos \frac{\theta_z}{2} \\ -\sin \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\sin \frac{\theta_z}{2} + \cos \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\cos \frac{\theta_z}{2} \\ \cos \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\sin \frac{\theta_z}{2} + \sin \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\cos \frac{\theta_z}{2} \\ -\sin \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\sin \frac{\theta_z}{2} + \cos \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\cos \frac{\theta_z}{2} \\ \end{bmatrix} \]
回転順 XZY \[\large \begin{bmatrix} -\cos \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\sin \frac{\theta_z}{2} + \sin \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\cos \frac{\theta_z}{2} \\ \cos \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\cos \frac{\theta_z}{2} - \sin \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\sin \frac{\theta_z}{2} \\ \sin \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\cos \frac{\theta_z}{2} + \cos \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\sin \frac{\theta_z}{2} \\ \sin \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\sin \frac{\theta_z}{2} + \cos \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\cos \frac{\theta_z}{2} \\ \end{bmatrix} \]
回転順 YXZ \[\large \begin{bmatrix} \cos \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\sin \frac{\theta_z}{2} + \sin \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\cos \frac{\theta_z}{2} \\ -\sin \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\sin \frac{\theta_z}{2} + \cos \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\cos \frac{\theta_z}{2} \\ \cos \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\sin \frac{\theta_z}{2} - \sin \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\cos \frac{\theta_z}{2} \\ \sin \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\sin \frac{\theta_z}{2} + \cos \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\cos \frac{\theta_z}{2} \\ \end{bmatrix} \]
回転順 YZX \[\large \begin{bmatrix} \sin \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\cos \frac{\theta_z}{2} + \cos \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\sin \frac{\theta_z}{2} \\ \sin \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\sin \frac{\theta_z}{2} + \cos \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\cos \frac{\theta_z}{2} \\ -\sin \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\cos \frac{\theta_z}{2} + \cos \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\sin \frac{\theta_z}{2} \\ -\sin \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\sin \frac{\theta_z}{2} + \cos \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\cos \frac{\theta_z}{2} \\ \end{bmatrix} \]
回転順 ZXY \[\large \begin{bmatrix} -\cos \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\sin \frac{\theta_z}{2} + \sin \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\cos \frac{\theta_z}{2} \\ \cos \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\cos \frac{\theta_z}{2} + \sin \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\sin \frac{\theta_z}{2} \\ \sin \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\cos \frac{\theta_z}{2} + \cos \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\sin \frac{\theta_z}{2} \\ -\sin \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\sin \frac{\theta_z}{2} + \cos \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\cos \frac{\theta_z}{2} \\ \end{bmatrix} \]
回転順 ZYX \[\large \begin{bmatrix} \sin \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\cos \frac{\theta_z}{2} - \cos \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\sin \frac{\theta_z}{2} \\ \sin \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\sin \frac{\theta_z}{2} + \cos \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\cos \frac{\theta_z}{2} \\ -\sin \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\cos \frac{\theta_z}{2} + \cos \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\sin \frac{\theta_z}{2} \\ \sin \frac{\theta_x}{2}\sin \frac{\theta_y}{2}\sin \frac{\theta_z}{2} + \cos \frac{\theta_x}{2}\cos \frac{\theta_y}{2}\cos \frac{\theta_z}{2} \\ \end{bmatrix} \]


クォータニオン → オイラー角 QuaternionToEuler

QuaternionToEuler

入力
q keisan\sQuaternion クォータニオン
order keisan\eEulerOrder 回転順

出力
eulerAngle LREAL[3] オイラー角(ラジアン)

【 例 】
入力
q \begin{bmatrix} 0.253 \\ 0.171 \\ 0.406 \\ 0.862 \end{bmatrix}
order XYZ
出力
eulerAngle [\(pi/9, pi/6, pi/4\)] (20度,30度,45度)

【 order 列挙体 】
こちらを参照

【 計算式 】
クォータニオンは以下のように表します \[ q = \begin{bmatrix} q_x \\ q_y \\ q_z \\ q_w \\ \end{bmatrix} \]
回転順 XYZ \[\large \begin{align} \theta_x &= \begin{cases} \arctan \left( -\frac{2q_yq_z-2q_xq_w}{2q_w^2+2q_z^2-1} \right) &(\cos \theta_y \neq 0) \\ \arctan \left( \frac{2q_yq_z+2q_xq_w}{2q_w^2+2q_y^2-1} \right) &(otherwise) \end{cases} \\ \theta_y &= \arcsin (2q_xq_z+2q_yq_w) \\ \theta_z &= \begin{cases} \arctan \left( -\frac{2q_xq_y-2q_zq_w}{2q_w^2+2q_x^2-1} \right) &(\cos \theta_y \neq 0) \\ 0 &(otherwise) \end{cases} \\ \end{align} \]
回転順 XZY \[\large \begin{align} \theta_x &= \begin{cases} \arctan \left( \frac{2q_yq_z+2q_xq_w}{2q_w^2+2q_y^2-1} \right) &(\cos \theta_z \neq 0) \\ \arctan \left( -\frac{2q_yq_z-2q_xq_w}{2q_w^2+2q_z^2-1} \right) &(otherwise) \\ \end{cases} \\ \theta_y &= \begin{cases} \arctan \left( \frac{2q_xq_z+2q_yq_w}{2q_w^2+2q_x^2-1} \right) &(\cos \theta_z \neq 0) \\ 0 &(otherwise) \end{cases} \\ \theta_z &= \arcsin (-(2q_xq_y-2q_zq_w)) \\ \end{align} \]
回転順 YXZ \[\large \begin{align} \theta_x &= \arcsin (-(2q_yq_z-2q_xq_w)) \\ \theta_y &= \begin{cases} \arctan \left( \frac{2q_xq_z+2q_yq_w}{2q_w^2+2q_z^2-1} \right) &(\cos \theta_x \neq 0) \\ \arctan \left( -\frac{2q_xq_z-2q_yq_w}{2q_w^2+2q_z^2-1} \right) &(otherwise) \\ \end{cases} \\ \theta_z &= \begin{cases} \arctan \left( \frac{2q_xq_y+2q_zq_w}{2q_w^2+2q_y^2-1} \right) &(\cos \theta_x \neq 0) \\ 0 &(otherwise) \end{cases} \\ \end{align} \]
回転順 YZX \[\large \begin{align} \theta_x &= \begin{cases} \arctan \left( -\frac{2q_yq_z-2q_xq_w}{2q_w^2+2q_y^2-1} \right) &(\cos \theta_z \neq 0) \\ 0 &(otherwise) \end{cases} \\ \theta_y &= \begin{cases} \arctan \left( -\frac{2q_xq_z-2q_yq_w}{2q_w^2+2q_x^2-1} \right) &(\cos \theta_z \neq 0) \\ \arctan \left( \frac{2q_xq_z+2q_yq_w}{2q_w^2+2q_z^2-1} \right) &(otherwise) \\ \end{cases} \\ \theta_z &= \arcsin (-(2q_xq_y+2q_zq_w)) \\ \end{align} \]
回転順 ZXY \[\large \begin{align} \theta_x &= \arcsin (2q_yq_+2q_xq_w) \\ \theta_y &= \begin{cases} \arctan \left( -\frac{2q_xq_z-2q_yq_w}{2q_w^2+2q_z^2-1} \right) &(\cos \theta_x \neq 0) \\ 0 &(otherwise) \end{cases} \\ \theta_z &= \begin{cases} \arctan \left( -\frac{2q_xq_y-2q_zq_w}{2q_w^2+2q_y^2-1} \right) &(\cos \theta_x \neq 0) \\ \arctan \left( \frac{2q_xq_y+2q_zq_w}{2q_w^2+2q_x^2-1} \right) &(otherwise) \\ \end{cases} \\ \end{align} \]
回転順 ZYX \[\large \begin{align} \theta_x &= \begin{cases} \arctan \left( \frac{2q_yq_z+2q_xq_w}{2q_w^2+2q_z^2-1} \right) &(\cos \theta_y \neq 0) \\ 0 &(otherwise) \end{cases} \\ \theta_y &= \arcsin (-(2q_xq_z-2q_yq_w)) \\ \theta_z &= \begin{cases} \arctan \left( \frac{2q_xq_y+2q_zq_w}{2q_w^2+2q_x^2-1} \right) &(\cos \theta_y \neq 0) \\ \arctan \left( -\frac{2q_xq_y-2q_zq_w}{2q_w^2+2q_y^2-1} \right) &(otherwise) \\ \end{cases} \\ \end{align} \]


クォータニオン → 回転行列 QuaternionToMatrix

QuaternionToMatrix

入力
q keisan\sQuaternion クォータニオン

出力
mat LREAL[3,3] 回転行列

【 例 】
入力
q \[ \begin{bmatrix} q_x \\ q_y \\ q_z \\ q_w \end{bmatrix} = \begin{bmatrix} 1 \\ 0 \\ 0 \\ 0 \end{bmatrix} \]
出力
mat \begin{bmatrix} 1& 0& 0& \\ 0& -1& 0 \\ 0& 0& -1 \end{bmatrix}

【 計算式 】
\[\large \begin{bmatrix} 2q_w^2+2q_x^2-1& 2q_xq_y-2q_zq_w& 2q_xq_z+2q_yq_w \\ 2q_xq_y+2q_zq_w& 2q_w^2+2q_y^2-1& 2q_yq_z-2q_xq_w \\ 2q_xq_z-2q_yq_w& 2q_yq_z+2q_xq_w& 2q_w^2+2q_z^2-1 \\ \end{bmatrix} \]


回転行列 → クォータニオン MatrixToQuaternion

MatrixToQuaternion

入力
mat LREAL[3,3] 回転行列
conv UINT 変換候補 0-3
回転行列からクォータニオンへの変換には複数の変換候補があるるため安定して解が求まる候補を選択します

出力
q keisan\sQuaternion クォータニオン


【 例 】
入力
mat \begin{bmatrix} 1& 0& 0& \\ 0& -1& 0 \\ 0& 0& -1 \end{bmatrix}
conv 0
出力
q \[ \begin{bmatrix} q_x \\ q_y \\ q_z \\ q_w \end{bmatrix} = \begin{bmatrix} 1 \\ 0 \\ 0 \\ 0 \end{bmatrix} \]

【 計算式 】
conv=0 \[\Large q= \begin{bmatrix} \frac{\sqrt{m_{00}-m_{11}-m_{22}+1}}{2} \\ \frac{m_{10}-m_{01}}{2\sqrt{m_{00}-m_{11}-m_{22}+1}} \\ \frac{m_{02}-m_{20}}{2\sqrt{m_{00}-m_{11}-m_{22}+1}} \\ \frac{m_{21}-m_{12}}{2\sqrt{m_{00}-m_{11}-m_{22}+1}} \\ \end{bmatrix} \] conv=1 \[\Large q= \begin{bmatrix} \frac{m_{10}-m_{01}}{2\sqrt{-m_{00}+m_{11}-m_{22}+1}} \\ \frac{\sqrt{-m_{00}+m_{11}-m_{22}+1}}{2} \\ \frac{m_{21}-m_{12}}{2\sqrt{-m_{00}+m_{11}-m_{22}+1}} \\ \frac{m_{02}-m_{20}}{2\sqrt{-m_{00}+m_{11}-m_{22}+1}} \\ \end{bmatrix} \] conv=2 \[\Large q= \begin{bmatrix} \frac{m_{02}-m_{20}}{2\sqrt{-m_{00}-m_{11}+m_{22}+1}} \\ \frac{m_{21}-m_{12}}{2\sqrt{-m_{00}-m_{11}+m_{22}+1}} \\ \frac{\sqrt{-m_{00}-m_{11}+m_{22}+1}}{2} \\ \frac{m_{10}-m_{01}}{2\sqrt{-m_{00}-m_{11}+m_{22}+1}} \\ \end{bmatrix} \] conv=3 \[\Large q= \begin{bmatrix} \frac{m_{21}-m_{12}}{2\sqrt{m_{00}+m_{11}+m_{22}+1}} \\ \frac{m_{02}-m_{20}}{2\sqrt{m_{00}+m_{11}+m_{22}+1}} \\ \frac{m_{10}-m_{01}}{2\sqrt{m_{00}+m_{11}+m_{22}+1}} \\ \frac{\sqrt{m_{00}+m_{11}+m_{22}+1}}{2} \\ \end{bmatrix} \]


参考URL

Quita 回転行列、クォータニオン、オイラー角の相互変換 https://qiita.com/aa_debdeb/items/3d02e28fb9ebfa357eaf