Python 三维空间中的相对旋转

我有一个对象（XYZ坐标系，其中Z向上），它用相应的旋转矩阵从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旋转了多少

---

编辑：为绘图编写代码：

您可以使用来自的矩阵检查结果

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

pip3安装pytransform3d

，或者如果正在使用Jupyter笔记本，则可能需要在Jupyter笔记本中运行

！pip3安装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）

：

这似乎是个好兆头&可能是检查数学的一个好起点

---

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

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

当我们处理转换时，会有一些含糊不清的地方，这会使事情变得更加混乱。有两件事可能会让这里的代码有点混乱：

我从上面的例子开始：

将numpy导入为np
r_0=np.数组（[-0.02659679，-0.00281247,0.99964229]，
[ 0.76308514, -0.64603356,  0.01848528],
[ 0.64575048,  0.76330382,  0.01932857]])
r_1=np.数组（[[0.05114056，-0.03815443,0.99796237]，
[-0.30594799,  0.95062582,  0.05202294],
[-0.95067369, -0.30798506,  0.03694226]])

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

r0到r1=r1.dot（r0.T）
r0到r1

结果:

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

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

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

r\u 0.T

之后应用

r\u 1

。（如果

r\u 0

和

r\u 1

是实数，我们将编写

r\u 1-r\u 0

以获得一个将

r\u 0

转换为

r\u 1

的数字）

您可以验证

r0_到_r1

是否从

r_0

旋转到

r_1

：

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

来自numpy.testing导入断言数组几乎相等
#验证正确性：在r\u 0之后将r0\u应用于r\u r1
断言数组几乎相等（r1，r0到r1.dot（r0））
#如果测试失败，将引发错误

无论如何，内在约定也会起作用：

r0_到r1_内在=r_0.T.dot（r_1）
断言数组几乎相等（r1，r0.dot（r0到r1）

由于zabop介绍了pytransform3d，我还想澄清一下，scipy使用主动旋转矩阵和

pytransform3d.rotations.euler_xyz_from_matrix

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

旋转。要使用

从scipy.spatial.transform导入旋转为R
r=r.从矩阵（r0到r1）
euler_xyz_固有_活动_度=r.as_euler（'xyz'，度=真）
euler_xyz_固有_活动_度

结果：
数组（[-148.20762964，-3.6166255,3.30106818]）

您可以使用pytransform3d获得相同的结果（注意，我们通过
.T
获得被动旋转矩阵）：

导入pytransform3d.rotations作为pr
euler_xyz_固有_活动_弧度=pr.euler_xyz_自_矩阵（r0_至_r1.T）
np.rad2deg（欧拉弧度）

结果：
数组（[-148.20762951，-3.61662542,3.30106799]）

您还可以使用pytransform3d从euler角度获取旋转矩阵（注意，我们通过
.T
获取活动旋转矩阵）：

r0_到r1_从_euler=pr.matrix_从_euler_xyz（euler_xyz_固有的_活动的_弧度）.T
来自欧拉的r0到r1

结果:

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

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

您的
r\u 0
和
r\u 1
是否分别与
rot\u mat\u 0
和
rot\u mat\u 1
相同？是的，对不起。更正。
np.degrees（r.as_euler（'xyz'，degrees=True）
看起来不对劲。如果你能得到
as_euler（）
若要返回以度为单位的值，您不需要将结果转换为度！感谢引入pytransform3d。我想知道是否可以使用X、Y、Z坐标列表作为转换的输入。根据手册，它仅适用于轴。我添加了与旋转矩阵对应的绘图。您是否可以包括e制作情节的代码？：）由于角色限制，我不得不将代码外包，下面是链接：创建一个房间来讨论这个问题：