Convert from rotation matrix to quaternion

Tweet

It is well known that rotation matrix can be represented by quaternion. This page explains how to calculate the quaternions when the rotation matrix is given.

Quaternion q=(q0,q1,q2,q3) where |q|=1

Quaternion to rotation matrix.

( 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 )

Rotation matrix to quaternion. Following holds.

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

Therefore,

( 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 )

By solving this,

( 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 )

Now let's calculate the sign. q and -q represent same rotation, thus,

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

Other sign will be as follows.

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} )

Another choice is to do as follows.

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} )

or

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} )

or

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

Source code: Convert rotation matrix to quaternion.

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;

Back