Converting a rotation matrix to quaternions in python

noetic

asked 2021-10-07 04:35:20 -0500

biermann
3 ●1 ●3 ●4

updated 2021-10-07 13:36:49 -0500

Hi!

How can I convert a rotation matrix to quaternions in python to work with ROS, TF and Eigen?

I tried several packages and implemented the algorithm Eigen is using in python, but all are changing the original orientation.

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[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

This is how the poses should look like:

This is how they look with tr.quaternion_from_matrix()

Here are some example calculations:

R:
[ 0.11783312 -0.11989661  0.98576882  0.        ]
 [ 0.68971437  0.72405973  0.0056211   0.        ]
 [-0.71442945  0.67923657  0.16801263  0.        ]
 [ 0.          0.          0.          1.        ]
python:
[0.23757144 0.5996278  0.28553449 0.70885568]
cpp:
-0.313485  -0.630221 -0.322927 0.632666

R:
[[ 0.11783312 -0.11989661  0.98576882  0.        ]
 [ 0.68971437  0.72405973  0.0056211   0.        ]
 [-0.71442945  0.67923657  0.16801263  0.        ]
 [ 0.          0.          0.          1.        ]]
python:
0.23757144 0.5996278  0.28553449 0.70885568
cpp:
-0.316639  -0.626235 -0.328723 0.632068

Any help or suggestion would be highly appreciated!

Thanks! Johannes

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2021-10-07 10:00:59 -0500

jarvisschultz

9031 ●58 ●128 ●143 https://www.linkedin.c...

One option would be to use transformations.pyhttp://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

edit flag offensive delete link

Comments

Hi Jarvis! Thanks for your suggestion, but I tried already the tf package and it is also changing the original orientation.

I kept the original indices of the Eigen implementation.

What I am doing now is to send the rotation matrix via ros msg to my main c++ node and converting there to quaternions also using the tf library. This works, but I really do not understand the difference between my python and the Eigen implementation.

biermann ( 2021-10-07 11:42:06 -0500 )edit

transformations.py has been around for a long time. Age does not make perfect per se, but I'd be surprised if there wouldn't be more reports about quaternion_from_matrix(..) not working as it should.

I'm not saying you're not seeing what you're seeing, but with code of this age and history of use, personally I would doubt my own use of it before something else.

gvdhoorn ( 2021-10-07 12:00:05 -0500 )edit

Hi gvdhoorn! I totally agree with you. Probably I made a stupid mistake along the way, but I can not think of one.

I edited the question.

biermann ( 2021-10-07 13:27:38 -0500 )edit

Without seeing what you did in your C++ code and how you are sending your transforms it's impossible to guess at what you might be doing wrong. However, I will say that Eigen natively seems to agree perfectly with Python's result in the numerical example you added to the original question. https://replit.com/@JarvisSchultz/Tru...:

$ ./main
Rotation Matrix: 
 0.117833 -0.119897  0.985769
 0.689714   0.72406 0.0056211
-0.714429  0.679237  0.168013
Quaternion: 
0.237571
0.599628
0.285534
0.708856

jarvisschultz ( 2021-10-07 17:09:02 -0500 )edit

Jarvis you are right! I was very sure that the problem lies in the calculation of the quaternions and the rest of the code is spread through different functions and nodes, which would make it difficult to post it here.

Still thank you a lot for your help!

biermann ( 2021-10-08 03:01:04 -0500 )edit

add a comment

answered 2021-10-08 03:19:23 -0500

biermann
3 ●1 ●3 ●4

updated 2021-10-08 03:20:14 -0500

With the help of Jarvis I found the problem. I am using 3 directional vectors to construct my rotational matrix vx, vy and vz. As their dimensions are [0,3] transposing them has no effect because the first dimension is zero.

    vx = ndarray([1, 2, 3])
    print(vx.T)
>>> [1, 2, 3]

So I built my rotation matrix wrong:

vx = ndarray([1, 2, 3])
vy = ndarray([4, 5, 6])
vz = ndarray([7, 8, 9])

R = np.asarray([vx.T, vy.T, vz.T], dtype=np.float64)
>>> [[1, 2, 3]
     [4, 5, 6]
     [7, 8, 9]

One solution would be:

R = np.asarray(np.column_stack((vx, vy, vz)), dtype=np.float64)
>>> [[1, 4, 7]
     [2, 5, 8]
     [3, 6, 9]

As the rotation matrix was wrong every algorithm calculated different quaternions with different orientations, which led me think that the problem lies there.

This took me more than a day to figure out, ouch! Anyways big thanks for your help! Without I would still try to debug the conversion algorithm.

Cheers! Johannes

edit flag offensive delete link

Comments

Glad you figured it out!

jarvisschultz ( 2021-10-08 10:27:14 -0500 )edit

add a comment

Converting a rotation matrix to quaternions in python

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Converting a rotation matrix to quaternions in python edit

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Converting a rotation matrix to quaternions in python