## Can anyone help in the conversion of matrices?

Asked By 10 points N/A Posted on -

I need help to convert rotation matrix to quaternion. The computation of the covariance of image projection, can be done in which method. Can be the bundle adjustment more efficient by using the MEX file? Is SBA can be used in commercial application?

SHARE
Answered By 0 points N/A #100464

## Can anyone help in the conversion of matrices?

Hi Albert,

You can try to use the code below to convert rotation matrix to quaternion.

inline floatSIGN(float x) {return (x >= 0.0f) ? +1.0f : -1.0f;}
inline floatNORM(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 errorn");
}
r = NORM(q0, q1, q2, q3);
q0 /= r;
q1 /= r;
q2 /= r;
q3 /= r;

Aristono