3D 中的相对旋转

Question

I have an object (XYZ-coordinate system where Z is up) that rotates from t0 to t1 with the corresponding rotation matrices:

import numpy as np
from scipy.spatial.transform import Rotation as R
r_0 = np.array([[-0.02659679, -0.00281247,  0.99964229],
                [ 0.76308514, -0.64603356,  0.01848528],
                [ 0.64575048,  0.76330382,  0.01932857]])

r_1 = np.array([[ 0.05114056, -0.03815443,  0.99796237],
               [-0.30594799,  0.95062582,  0.05202294],
               [-0.95067369, -0.30798506,  0.03694226]])

# Calculate the relative rotation matrix from t0 to t1
rot_mat_rel = np.matmul(np.transpose(r_0), r_1)
r = R.from_maxtrix(rot_mat_rel)
# Obtain angles
print(r.as_euler('xyz', degrees=True)

# Result
array([  -1.52028392,   -1.55242217, -148.10677483])

The problem is, that the relative angles look wrong to me but I can't find my mistake. What I wanted to know is how much the object rotated along x, y and z.

Edit: Code to for plots: https://codeshare.io/GA9zK8

Answer 1

You can use matrix_from_euler_xyz from this tutorial to check your results.

(You might need to run pip3 install pytransform3d in your terminal where you are running your python code from, or !pip3 install pytransform3d from Jupyter Notebook if you are using that.)

Preparing the data:

import numpy as np
from scipy.spatial.transform import Rotation as R
r_0 = np.array([[-0.02659679, -0.00281247,  0.99964229],
                [ 0.76308514, -0.64603356,  0.01848528],
                [ 0.64575048,  0.76330382,  0.01932857]])

r_1 = np.array([[ 0.05114056, -0.03815443,  0.99796237],
                [-0.30594799,  0.95062582,  0.05202294],
                [-0.95067369, -0.30798506,  0.03694226]])

# Calculate the relative rotation matrix from t0 to t1
rot_mat_rel = np.matmul(np.transpose(r_0), r_1)
r = R.from_matrix(rot_mat_rel)

Let's plot what the rotation r means in practice:

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from pytransform3d.rotations import *

ax = plot_basis(R=np.eye(3), ax_s=1)

p = np.array([0, 0, 0])

R = matrix_from_euler_xyz(r.as_euler('xyz'))
plot_basis(ax, R, p, alpha = 0.5)

plt.show()

We obtain this plot:

You can check if this is what you expected or not.

Check the rotation matrix which the pytransform3d module calculated from Euler angles r :

matrix_from_euler_xyz(r.as_euler('xyz'))

Giving ouput:

array([[-0.84872253, -0.52814402,  0.02709157],
       [ 0.52754172, -0.84911505, -0.02652111],
       [ 0.03701082, -0.00821713,  0.99928108]])

which is exactly the traspose of np.matmul(np.transpose(r_0), r_1) :

array([[-0.84872253,  0.52754172,  0.03701082],
       [-0.52814402, -0.84911505, -0.00821714],
       [ 0.02709157, -0.02652111,  0.99928109]])

Which seems a good sign & may be a good starting point for checking your math.

As I don't see what you would expect to get, I suggest you experiment with plotting your results with the tools outlined here, and check step by step that what you have is what you have expected to have.

Answer 2

I'm probably a bit late and zabop's answer already points to the right direction. I just want to clarify two things.

There are several ambiguities when we work with transformations that can make things more confusing. The two things that might make the code here a bit confusing are:

• active vs. passive rotation
• intrinsic vs. extrinsic rotation

I'm starting from your example above:

import numpy as np
r_0 = np.array([[-0.02659679, -0.00281247,  0.99964229],
                [ 0.76308514, -0.64603356,  0.01848528],
                [ 0.64575048,  0.76330382,  0.01932857]])
r_1 = np.array([[ 0.05114056, -0.03815443,  0.99796237],
                [-0.30594799,  0.95062582,  0.05202294],
                [-0.95067369, -0.30798506,  0.03694226]])

The way I would calculate a rotation matrix that rotates r_0 to r_1 is the following (different from your code:):

r0_to_r1 = r_1.dot(r_0.T)
r0_to_r1

Result:

array([[ 0.99635252,  0.08212126,  0.0231898 ],
       [ 0.05746796, -0.84663889,  0.52905579],
       [ 0.06308011, -0.52579339, -0.84827012]])

I use the extrinsic convention for concatenation of rotation matrices, that is, r_1 is applied after r_0.T . (If r_0 and r_1 were real numbers, we would write r_1 - r_0 to obtain a number that transforms r_0 to r_1 .)

You can verify that r0_to_r1 rotates from r_0 to r_1 :

from numpy.testing import assert_array_almost_equal
# verify correctness: apply r0_to_r1 after r_0
assert_array_almost_equal(r_1, r0_to_r1.dot(r_0))
# would raise an error if test fails

Anyway, the intrinsic convention would also work:

r0_to_r1_intrinsic = r_0.T.dot(r_1)
assert_array_almost_equal(r_1, r_0.dot(r0_to_r1_intrinsic))

Since zabop introduced pytransform3d, I would also like to clarify that scipy uses active rotation matrices and the rotation matrix that pytransform3d.rotations.euler_xyz_from_matrix produces is a passive rotation matrix. This wasn't documented so clearly in previous versions. You can transform an active rotation matrix to a passive rotation matrix and vice versa with the matrix transpose. Both pytransform3d's function and scipy's Rotation.to_euler("xyz", ...) use the intrinsic concatenation convention .

from scipy.spatial.transform import Rotation as R
r = R.from_matrix(r0_to_r1)
euler_xyz_intrinsic_active_degrees = r.as_euler('xyz', degrees=True)
euler_xyz_intrinsic_active_degrees

Result: array([-148.20762964, -3.6166255, 3.30106818])

You can obtain the same result with pytransform3d (note that we obtain the passive rotation matrix by .T ):

import pytransform3d.rotations as pr
euler_xyz_intrinsic_active_radians = pr.euler_xyz_from_matrix(r0_to_r1.T)
np.rad2deg(euler_xyz_intrinsic_active_radians)

Result: array([-148.20762951, -3.61662542, 3.30106799])

You can also obtain the rotation matrix from euler angles with pytransform3d (note that we obtain the active rotation matrix by .T ):

r0_to_r1_from_euler = pr.matrix_from_euler_xyz(euler_xyz_intrinsic_active_radians).T
r0_to_r1_from_euler

Result:

array([[ 0.99635251,  0.08212125,  0.0231898 ],
       [ 0.05746796, -0.84663889,  0.52905579],
       [ 0.06308011, -0.52579339, -0.84827013]])