# Converting a rotation matrix to quaternions in python

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