3D 中的相对旋转

Question

我有一个对象（Z 向上的 XYZ 坐标系），它使用相应的旋转矩阵从 t0 旋转到 t1：

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])

问题是，相对角度对我来说看起来是错误的，但我找不到我的错误。我想知道的是物体沿 x、y 和 z 旋转了多少。

编辑：绘图代码：https://codeshare.io/GA9zK8

Answer 1

您可以使用

本教程

中的matrix_from_euler_xyz来检查您的结果。

（您可能需要在运行 Python 代码的终端中运行

pip3 install pytransform3d

，或者如果您正在使用 Jupyter Notebook，则需要从 Jupyter Notebook 中运行

!pip3 install pytransform3d

。）

准备数据：

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)

让我们绘制一下旋转

r

在实践中的含义：

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()

我们得到这个图：

您可以检查这是否是您所期望的。

检查

pytransform3d

模块根据欧拉角

r

计算出的旋转矩阵：

matrix_from_euler_xyz(r.as_euler('xyz'))

给出输出：

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

这正是

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]])

这似乎是一个好兆头，并且可能是检查你的数学的一个很好的起点。

由于我没有看到您期望得到什么，我建议您尝试使用此处概述的工具绘制结果，并逐步检查您所拥有的是否是您所期望的。

Answer 2

我可能有点晚了，zabop 的答案已经指出了正确的方向。我只想澄清两件事。

当我们处理转换时，存在一些歧义，这会让事情变得更加混乱。可能使这里的代码有点混乱的两件事是：

• 主动旋转与被动旋转
• 内在旋转与外在旋转

我从上面的例子开始：

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]])

我计算将

r_0

旋转到

r_1

的旋转矩阵的方法如下（与您的代码不同！）：

r0_to_r1 = r_1.dot(r_0.T)
r0_to_r1

结果：

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

我使用外部约定来连接旋转矩阵，即在

r_1

之后应用

r_0.T

。 （如果

r_0

和

r_1

是实数，我们可以写

r_1 - r_0

来获得将

r_0

转换为

r_1

的数字。）

您可以验证

r0_to_r1

从

r_0

旋转到

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

无论如何，内在约定也是有效的：

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

自从zabop引入了pytransform3d，我还想澄清一下，scipy使用active旋转矩阵，而

pytransform3d.rotations.euler_xyz_from_matrix

产生的旋转矩阵是passive旋转矩阵！在以前的版本中，这一点没有记录得那么清楚。您可以使用矩阵转置将主动旋转矩阵转换为被动旋转矩阵，反之亦然。 pytransform3d 的函数和 scipy 的

Rotation.to_euler("xyz", ...)

都使用 内在串联约定。

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

结果：

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

使用 pytransform3d 可以获得相同的结果（注意，我们通过

.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)

结果：

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

您还可以使用 pytransform3d 从欧拉角获取旋转矩阵（注意，我们通过

.T

获取活动旋转矩阵）：

r0_to_r1_from_euler = pr.matrix_from_euler_xyz(euler_xyz_intrinsic_active_radians).T
r0_to_r1_from_euler

结果：

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