回転行列からクォータニオン（四元数）に戻す

Tweet

クォータニオン（四元数）を使って回転行列を表すことができますが，ここでは逆に，回転行列が与えられたとき，そこからクォータニオンを計算する方法を説明します．

クォータニオン（４元数, quaternion） q=(q0,q1,q2,q3) ただし |q|=1

クォータニオンから回転行列は以下のように計算できる．

( r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ) = ( q 0 2 + q 1 2 − q 2 2 − q 3 2 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 1 q 2 + q 0 q 3 ) q 0 2 − q 1 2 + q 2 2 − q 3 2 2 ( q 2 q 3 − q 0 q 1 ) 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) q 0 2 − q 1 2 − q 2 2 + q 3 2 ) left ( matrix{r_{11} # r_{12} # r_{13}## r_{21} # r_{22}# r_{23} ## r_{31} # r_{32}# r_{33}} right ) = left ( matrix{ q^2_0+q^2_1-q^2_2-q^2_3 # 2( q_1 q_2-q_0 q_3) # 2( q_1 q_3+q_0 q_2)## 2( q_1 q_2+q_0 q_3) # q^2_0-q^2_1+q^2_2-q^2_3 # 2( q_2 q_3-q_0 q_1)## 2( q_1 q_3-q_0 q_2) # 2( q_2 q_3+q_0 q_1) # q^2_0-q^2_1-q^2_2+q^2_3 } right )

回転行列からクォータニオンに戻す場合．以下が成り立っている．

q 0 2 + q 1 2 + q 2 2 + q 3 2 = 1 q^2_0 + q^2_1 +q^2_2 +q^2_3 =1

よって，以下が成り立つ．

( 1 1 − 1 − 1 1 − 1 1 − 1 1 − 1 − 1 1 1 1 1 1 ) ( q 0 2 q 1 2 q 2 2 q 3 2 ) = ( r 11 r 22 r 33 1 ) left ( matrix{ 1 # 1 # -1 # -1 ## 1 # -1 # 1 # -1 ## 1 # -1 # -1 # 1 ## 1 # 1 # 1 # 1 } right ) left ( matrix{ q^2_0 ## q^2_1 ## q^2_2 ## q^2_3 } right ) = left ( matrix{ r_{11} ## r_{22} ## r_{33} ## 1 } right )

これを解くと，

( q 0 2 q 1 2 q 2 2 q 3 2 ) = 1 4 ( 1 1 1 1 1 − 1 − 1 1 − 1 1 − 1 1 − 1 − 1 1 1 ) ( r 11 r 22 r 33 1 ) left ( matrix{ q^2_0 ## q^2_1 ## q^2_2 ## q^2_3 } right ) = {1} over {4} left ( matrix{ 1 # 1 # 1 # 1 ## 1 # -1 # -1 # 1 ## -1 # 1 # -1 # 1 ## -1 # -1 # 1 # 1 } right ) left ( matrix{ r_{11} ## r_{22} ## r_{33} ## 1 } right )

あとは符号を求める．qと-qは同じ回転を表すので，

sgn ( q 0 ) = + 1 func sgn ( q_{0} ) = +1

とする．

他の符号は以下の通り．

sgn ( q 1 ) = sgn ( r 32 − r 23 ) sgn ( q 2 ) = sgn ( r 13 − r 31 ) sgn ( q 3 ) = sgn ( r 21 − r 12 ) func sgn ( q_{1} ) = func sgn ( r_{32}-r_{23} ) newline func sgn ( q_{2} ) = func sgn ( r_{13}-r_{31} ) newline func sgn ( q_{3} ) = func sgn ( r_{21}-r_{12} )

あるいは以下のようにしてもいい．

sgn ( q 0 ) = sgn ( r 32 − r 23 ) sgn ( q 1 ) = + 1 sgn ( q 2 ) = sgn ( r 21 + r 12 ) sgn ( q 3 ) = sgn ( r 13 + r 31 ) "" func sgn ( q_{0} ) = func sgn ( r_{32}-r_{23} ) newline "" func sgn ( q_{1} )=+1 newline "" func sgn ( q_{2} ) = func sgn ( r_{21}+r_{12} ) newline "" func sgn ( q_{3} ) = func sgn ( r_{13}+r_{31} )

または

sgn ( q 0 ) = sgn ( r 13 − r 31 ) sgn ( q 1 ) = sgn ( r 21 + r 12 ) sgn ( q 2 ) = + 1 sgn ( q 3 ) = sgn ( r 32 + r 23 ) func sgn ( q_{0} ) = func sgn ( r_{13}-r_{31} ) newline func sgn ( q_{1} ) = func sgn ( r_{21}+r_{12} ) newline func sgn ( q_{2} )=+1 newline func sgn ( q_{3} ) = func sgn ( r_{32}+r_{23} )

または

sgn ( q 0 ) = sgn ( r 21 − r 12 ) sgn ( q 1 ) = sgn ( r 13 + r 31 ) sgn ( q 2 ) = sgn ( r 32 + r 23 ) sgn ( q 3 ) = + 1 func sgn ( q_{0} ) = func sgn ( r_{21}-r_{12} ) newline func sgn ( q_{1} ) = func sgn ( r_{13}+r_{31} ) newline func sgn ( q_{2} ) = func sgn ( r_{32}+r_{23} ) newline func sgn ( q_{3} )=+1

回転行列からクォータニオンに戻すプログラム．

inline float SIGN(float x) {return (x >= 0.0f) ? +1.0f : -1.0f;}
inline float NORM(float a, float b, float c, float d) {return sqrt(a * a + b * b + c * c + d * d);}

q0 = ( r11 + r22 + r33 + 1.0f) / 4.0f;
q1 = ( r11 - r22 - r33 + 1.0f) / 4.0f;
q2 = (-r11 + r22 - r33 + 1.0f) / 4.0f;
q3 = (-r11 - r22 + r33 + 1.0f) / 4.0f;
if(q0 < 0.0f) q0 = 0.0f;
if(q1 < 0.0f) q1 = 0.0f;
if(q2 < 0.0f) q2 = 0.0f;
if(q3 < 0.0f) q3 = 0.0f;
q0 = sqrt(q0);
q1 = sqrt(q1);
q2 = sqrt(q2);
q3 = sqrt(q3);
if(q0 >= q1 && q0 >= q2 && q0 >= q3) {
    q0 *= +1.0f;
    q1 *= SIGN(r32 - r23);
    q2 *= SIGN(r13 - r31);
    q3 *= SIGN(r21 - r12);
} else if(q1 >= q0 && q1 >= q2 && q1 >= q3) {
    q0 *= SIGN(r32 - r23);
    q1 *= +1.0f;
    q2 *= SIGN(r21 + r12);
    q3 *= SIGN(r13 + r31);
} else if(q2 >= q0 && q2 >= q1 && q2 >= q3) {
    q0 *= SIGN(r13 - r31);
    q1 *= SIGN(r21 + r12);
    q2 *= +1.0f;
    q3 *= SIGN(r32 + r23);
} else if(q3 >= q0 && q3 >= q1 && q3 >= q2) {
    q0 *= SIGN(r21 - r12);
    q1 *= SIGN(r31 + r13);
    q2 *= SIGN(r32 + r23);
    q3 *= +1.0f;
} else {
    printf("coding error\n");
}
r = NORM(q0, q1, q2, q3);
q0 /= r;
q1 /= r;
q2 /= r;
q3 /= r;

もどる