Matrix To Quaternion

 

Maths - Conversion Matrix to Quaternion - Martin Baker

Matrix to Quaternion Calculator. Equations For a pure rotation, that is where: the matrix is orthogonal the matrix is special orthogonal which gives additional condition: det(matrix)= +1 Then the matrix can be converted to a quaternion using this basic form: qw= √(1 + m00 + m11 + m22) /2 qx = (m21 -...

www.euclideanspace.com

0. 기본적 계산

$q\ =\left(q_0,\ q_1,\ q_2,\ q_3\right)\ =\left(\cos \left(\frac{\theta }{2}\right),\ \sin \left(\frac{\theta }{2}\right)u\right)$q =(qx​, qy​, qz​, qw​) =(cos(θ2​), sin(θ2​)u)​

$=\ (\cos \left(\frac{\theta }{2}\right),\sin \left(\frac{\theta }{2}\right)\combi{\cdot u}_x,\ \sin \left(\frac{\theta }{2}\right)\cdot \combi{u}_y,\ \sin \left(\frac{\theta }{2}\right)\cdot \combi{u}_z)$= (cos(θ2​),sin(θ2​)·ux​, sin(θ2​)·uy​, sin(θ2​)·uz​)​

$R=\begin{bmatrix}\combi{\combi{q}_x}^2-\combi{\combi{q}_y}^2-\combi{\combi{q}_z}^2+\combi{\combi{q}_w}^2&2\left(\combi{q}_1\combi{q}_2-\combi{q}_0\combi{q}_3\right)&2\left(\combi{q}_x\combi{q}_z+\combi{q}_y\combi{q}_w\right)\\2\left(\combi{q}_1\combi{q}_2+\combi{q}_0\combi{q}_3\right)&\combi{\combi{-q}_x}^2+\combi{\combi{q}_y}^2-\combi{\combi{q}_z}^2+\combi{\combi{q}_w}^2&2\left(\combi{q}_x\combi{q}_w-\combi{q}_y\combi{q}_z\right)\\2\left(\combi{q}_x\combi{q}_z-\combi{q}_y\combi{q}_w\right)&2\left(\combi{q}_x\combi{q}_w+\combi{q}_y\combi{q}_z\right)&\combi{\combi{-q}_x}^2-\combi{\combi{q}_y}^2+\combi{\combi{q}_z}^2+\combi{\combi{q}_w}^2\end{bmatrix}$R=[

qx​2−qy​2−qz​2+qw​2	2(qx​qy​−qz​qw​)	2(qx​qz​+qy​qw​)
2(qz​qw​+qx​qy​)	−qx​2+qy​2−qz​2+qw​2	2(qy​qz​−qx​qw​)
2(qx​qz​−qy​qw​)	2(qx​qw​+qy​qz​)	−qx​2−qy​2+qz​2+qw​2

]​

q 는 quaternion 고 R 은 quaternion을 Column Major 행렬로 변환한 것이다.

q => R 은 이렇게 나타낼 수 있는데, R => Q 방향으로는 어떻게 알아낼까.

​

이때 주어진 matrix 가 Scale, Translate 이 없는 순수한 Rotation Matrix 라고 가정하자.

​

우선 행렬의 대각선을 다 더해서 w 를 알 수 있다.

$m_{11}\ +\ m_{22}+m_{33}\ =\ 3q_0^2-q_1^2-q_2^2-q_3^2\ \ $m11​ + m22​+m33​ = 3q2w​−q2x​−q2y​−q2z​  ​

$=\ 3\cdot \cos ^2\frac{\theta }{2}\ -\ \sin ^2\frac{\theta }{2}\ =\ 4\ \cdot \ \cos ^2\frac{\theta }{2}\ \ -\ 1$= 3·cos2θ2​ − sin2θ2​ = 4 · cos2θ2​  − 1​

$w=\cos \frac{\theta }{2}\ =\sqrt{\frac{\left(1\ +m_{11}\ +\ m_{22}+m_{33}\ \right)\ }{4}}$w=cosθ2​ =√(1 +m11​ + m22​+m33​ ) 4​​

$\star \left(if\ 1\ +q_0^2-q_1^2-q_2^2-q_3^2\ >\ 0\right)$⋆(if 3q2w​−q2x​−q2y​−q2z​ > 0)​

그리고 대각선을 중심으로 서로 대칭되는 것들을 빼서 나머지를 얻는다.

$m12\ +\ m21\ =\ 2\left(q_1q_2\ +\ q_0q_3\right)\ -\ 2\left(q_1q_2\ -\ q_0q_3\right)\ =\ 4\left(q_0q_3\right)$m12 + m21 = 2(q1​q2​ + q0​q3​) − 2(q1​q2​ − q0​q3​) = 4(q0​q3​)​

$q_3=\ \frac{m12\ +m21}{4q_0}$q3​= m12 +m214q0​​​

$=\frac{m12\ +m21}{2\sqrt{m_{11}+m_{22}+m_{33}\ +\ 1}}$=m12 +m212√m11​+m22​+m33​ + 1​​

$\star \left(if\ 1\ +q_0^2-q_1^2-q_2^2-q_3^2\ >\ 0\right)$⋆(if 3q2w​−q2x​−q2y​−q2z​ > 0)​

이때 루트쓸려면 내부가 양수라는 조건이 붙는다는 것을 명심하자.

​

그럼 만약 양수가 아닌 경우는 어떡하냐?

​

위에서 다 더했더니 q_w 이 3개가 나왔는데, 이번엔 다른걸 3개를 만드는 거임.

정규화되어 있기 때문에 한 경우는 무조건 양수가 된다.

​

$m_{11}\ -\ m_{22}-m_{33}\ =\ 3q_1^2-q_0^2-q_2^2-q_3^2\ \ $m11​ − m22​−m33​ = 3q2y​−q2x​−q2y​−q2w​  ​

$=\ 4q_1^2\ -\left(\cos ^2\frac{\theta }{2}\ -\ \sin ^2\frac{\theta }{2}\right)\ =\ 4\ \cdot \ q_1^2\ -\ 1$= 4q2y​ −(cos2θ2​ − sin2θ2​) = 4 · q2y​ − 1​

$q_1\ =\sqrt{\frac{\left(1\ +m_{11}\ -\ m_{22}-m_{33}\ \right)\ }{4}}$q1​ =√(1 +m11​ − m22​−m33​ ) 4​​

$\star \left(if\ m_{11}\ is\ biggest\ among\ trace\right)$⋆(if m11​ is biggest among trace)​

나머지 부분 구하는건 비슷하게 진행하면 됨.