From 88c3a2d3a2bfc809d1589faa4897e70557f0a0fb Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 10 Jul 2014 13:19:35 +0530 Subject: [PATCH] Updated rotation matrix class method rot_mat2quat Updated method with better implementation for rotation matrix to quaternion conversion. The best method for this conversion is based on eigen vectors. This complex implementation will be added at a later stage after further analysis. signed-off-by: Ramasamy Change-Id: I9fe29d4f8e5ff3b5989a90ecba691d38adc9f3a8 --- .../standalone/util/rotation_matrix.cpp | 86 ++++++++++------------ 1 file changed, 37 insertions(+), 49 deletions(-) diff --git a/src/sensor_fusion/standalone/util/rotation_matrix.cpp b/src/sensor_fusion/standalone/util/rotation_matrix.cpp index aecce4f..3a1ef86 100644 --- a/src/sensor_fusion/standalone/util/rotation_matrix.cpp +++ b/src/sensor_fusion/standalone/util/rotation_matrix.cpp @@ -98,61 +98,49 @@ quaternion rot_mat2quat(rotation_matrix rm) { T q0, q1, q2, q3; - q0 = (rm.m_rot_mat.m_mat[0][0] + rm.m_rot_mat.m_mat[1][1] + - rm.m_rot_mat.m_mat[2][2] + (T) 1) / (T) QUAT_LEN; - q1 = (rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] - - rm.m_rot_mat.m_mat[2][2] + (T) 1) / (T) QUAT_LEN; - q2 = (-rm.m_rot_mat.m_mat[0][0] + rm.m_rot_mat.m_mat[1][1] - - rm.m_rot_mat.m_mat[2][2] + (T) 1) / (T) QUAT_LEN; - q3 = (-rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] + - rm.m_rot_mat.m_mat[2][2] + (T) 1) / (T) QUAT_LEN; - - if(q0 < (T) 0) - q0 = (T) 0; - if(q1 < (T) 0) - q1 = (T) 0; - if(q2 < (T) 0) - q2 = (T) 0; - if(q3 < (T) 0) - q3 = (T) 0; - - q0 = sqrt(q0); - q1 = sqrt(q1); - q2 = sqrt(q2); - q3 = sqrt(q3); - - if (q0 >= q1 && q0 >= q2 && q0 >= q3) - { - q0 *= (T) 1; - q1 *= get_sign(rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2]); - q2 *= get_sign(rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0]); - q3 *= get_sign(rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1]); - } - else if (q1 >= q0 && q1 >= q2 && q1 >= q3) - { - q0 *= get_sign(rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2]); - q1 *= (T) 1; - q2 *= get_sign(rm.m_rot_mat.m_mat[1][0] + rm.m_rot_mat.m_mat[0][1]); - q3 *= get_sign(rm.m_rot_mat.m_mat[0][2] + rm.m_rot_mat.m_mat[2][0]); - } - else if (q2 >= q0 && q2 >= q1 && q2 >= q3) + T diag_sum = rm.m_rot_mat.m_mat[0][0] + rm.m_rot_mat.m_mat[1][1] + rm.m_rot_mat.m_mat[2][2]; + + if ( diag_sum > 0 ) { - q0 *= get_sign(rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0]); - q1 *= get_sign(rm.m_rot_mat.m_mat[1][0] + rm.m_rot_mat.m_mat[0][1]); - q2 *= (T) 1; - q3 *= get_sign(rm.m_rot_mat.m_mat[2][1] + rm.m_rot_mat.m_mat[1][2]); + T val = (T) 0.5 / sqrt(diag_sum + (T) 1.0); + q0 = (T) 0.25 / val; + q1 = ( rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2] ) * val; + q2 = ( rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0] ) * val; + q3 = ( rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1] ) * val; } - else if(q3 >= q0 && q3 >= q1 && q3 >= q2) + else { - q0 *= get_sign(rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1]); - q1 *= get_sign(rm.m_rot_mat.m_mat[2][0] + rm.m_rot_mat.m_mat[0][2]); - q2 *= get_sign(rm.m_rot_mat.m_mat[2][1] + rm.m_rot_mat.m_mat[1][2]); - q3 *= (T) 1; + if ( rm.m_rot_mat.m_mat[0][0] > rm.m_rot_mat.m_mat[1][1] && rm.m_rot_mat.m_mat[0][0] > rm.m_rot_mat.m_mat[2][2] ) + { + T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] - rm.m_rot_mat.m_mat[2][2]); + q0 = (rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2] ) / val; + q1 = (T) 0.25 * val; + q2 = (rm.m_rot_mat.m_mat[0][1] + rm.m_rot_mat.m_mat[1][0] ) / val; + q3 = (rm.m_rot_mat.m_mat[0][2] + rm.m_rot_mat.m_mat[2][0] ) / val; + } + else if (rm.m_rot_mat.m_mat[1][1] > rm.m_rot_mat.m_mat[2][2]) + { + T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[1][1] - rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[2][2]); + q0 = (rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0] ) / val; + q1 = (rm.m_rot_mat.m_mat[0][1] + rm.m_rot_mat.m_mat[1][0] ) / val; + q2 = (T) 0.25 * val; + q3 = (rm.m_rot_mat.m_mat[1][2] + rm.m_rot_mat.m_mat[2][1] ) / val; + } + else + { + T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[2][2] - rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] ); + q0 = (rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1] ) / val; + q1 = (rm.m_rot_mat.m_mat[0][2] + rm.m_rot_mat.m_mat[2][0] ) / val; + q2 = (rm.m_rot_mat.m_mat[1][2] + rm.m_rot_mat.m_mat[2][1] ) / val; + q3 = (T) 0.25 * val; + } } - quaternion q(-q0, q1, q2, q3); + quaternion q(q0, q1, q2, q3); + + q.quat_normalize(); - return quat_normalize(q); + return q; } #endif /* _ROTATION_MATRIX_H */ -- 2.7.4