Reimplement Eigen rotation matrix conversion to quaternions in Python

Question

for consistency, I want to reimplement the conversion from a rotation matrix to quaternions of the Cpp Eigen library in python.

The Cpp implementation can be found here and below you can find my python implementation:

def rotationMatrixToQuaternion3(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[0] = 0.5 * t
        t = 0.5/t
        q[1] = (m[2,1] - m[1,2]) * t
        q[2] = (m[0,2] - m[2,0]) * t
        q[3] = (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[0] = (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

Here are some example results:

rotation matrix: 
[[-0.00998882  0.01194957 -0.99987871]
 [ 0.49223613 -0.87032691 -0.01531875]
 [-0.8704044  -0.49232944  0.00281153]]
python implemnation - qw, qx, qy, qz:
[-0.68145553 -0.18496647  0.68613542  0.        ]
eigen:
-0.686135  -0.174997 -0.681456 0.184966



rotation matrix: 
[[ 0.01541426  0.02293597 -0.9996181 ]
 [ 0.49081359 -0.87117607 -0.01242048]
 [-0.87112825 -0.49043469 -0.02468582]]
python implemnation - qw, qx, qy, qz:
[-0.17288173  0.18580601 -0.67658628  0.        ]
eigen:
-0.686135  -0.174997 -0.681456 0.184966



rotation matrix: 
[[ 0.03744363 -0.01068005 -0.99924167]
 [ 0.48694091 -0.87299945  0.02757743]
 [-0.87263195 -0.48760425 -0.02748771]]
python implemnation - qw, qx, qy, qz:
[-0.18503815  0.17105894 -0.67232212  0.        ]
eigen:
-0.672322  -0.185038 -0.696048 0.171059

Help would be highly appreciated! Thanks, Johannes

Answer 1

It seems that the order used by Eigen is [x, y, z, w], see same source file that you base your implementation on.

So the indices that you use should be changed in the following way:

def rotationMatrixToQuaternion3(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

And the returned quaternion is [x, y, z, w].

Running the modified code did not produce the same results that you report for Eigen. Unfortunately, I do not know how to reproduce the Eigen results.

However, there is a scipy implementation of quaternion-to-matrix, which gives the same results as the above implementation (up to multiplication by of the vector by -1 which is an inherent ambiguity of the quaternion and is thus implementation-dependent):

from scipy.spatial import transform
mat = np.array([[-0.00998882,  0.01194957, -0.99987871],
         [ 0.49223613, -0.87032691, -0.01531875,],
         [-0.8704044,  -0.49232944,  0.00281153]])
quat_sp = transform.Rotation.from_matrix(mat).as_quat()

So I think the above implementation is correct, and the problem was in the invocation of Eigen.