Revision history [back]

One option would be to use transformations.py http://wiki.ros.org/tf/TfUsingPython:

import tf.transformations as tr

R = tr.random_rotation_matrix()
# Note, in the transformations library conventions, even though the above
# method says it returns a rotation matrix, it actually returns a 4x4 SE(3)
# matrix with the rotation portion in the upper left 3x3 block.
q = tr.quaternion_from_matrix(R)

That is a well-tested library that will do exactly what you want.

FWIW, I'm almost 100% sure the issue with your code is that you've incorrectly converted Eigen's algorithm by misunderstanding the order they store the coefficients. They store the coefficients as [x,y,z,w] and it appears to me like you tried to convert using [w,x,y,z] without changing the index arithmetic in the else portion. Maybe this is what you were trying to do:

def rotationMatrixToQuaternion1(m):
    #q0 = qw
    t = np.matrix.trace(m)
    q = np.asarray([0.0, 0.0, 0.0, 0.0], dtype=np.float64)

    if(t > 0):
        t = np.sqrt(t + 1)
        q[3] = 0.5 * t
        t = 0.5/t
        q[0] = (m[2,1] - m[1,2]) * t
        q[1] = (m[0,2] - m[2,0]) * t
        q[2] = (m[1,0] - m[0,1]) * t

    else:
        i = 0
        if (m[1,1] > m[0,0]):
            i = 1
        if (m[2,2] > m[i,i]):
            i = 2
        j = (i+1)%3
        k = (j+1)%3

        t = np.sqrt(m[i,i] - m[j,j] - m[k,k] + 1)
        q[i] = 0.5 * t
        t = 0.5 / t
        q[3] = (m[k,j] - m[j,k]) * t
        q[j] = (m[j,i] + m[i,j]) * t
        q[k] = (m[k,i] + m[i,k]) * t

    return q