logo标签列表

在3D中相对旋转

pythonangle
4

我有一个物体（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轴旋转了多少。

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

你的 r_0 和 r_1 是否分别等同于 rot_mat_0 和 rot_mat_1？ - zabop
1
是的，抱歉。已更正。 - Christine
np.degrees(r.as_euler('xyz', degrees=True) 看起来不太对。如果你能让 as_euler() 返回角度值，那么你就不需要再将结果转换为角度了！ - jasonharper
2个回答
3
你可以使用这个教程中的matrix_from_euler_xyz函数来检查你的结果。

（如果你正在运行Python代码的终端中，可能需要运行pip3 install pytransform3d。如果你正在使用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()

我们得到这张图：

enter image description here

您可以检查这是否是您预期的结果。
检查 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]])

这似乎是一个好的迹象，也可能是检查您的数学的良好起点。由于我看不到您期望得到什么，建议您使用此处概述的工具绘制结果，并逐步检查您所拥有的是否与您所期望的相符。
感谢介绍pytransform3d。我想知道是否可以使用X、Y、Z坐标列表作为变换的输入。根据手册，它只能使用轴。 - Christine
我添加了对应于旋转矩阵的图表。 - Christine
1
你能把生成图表的代码也包含进来吗？ :) - zabop
我因为字符限制不得不外包代码，这是链接：https://codeshare.io/GA9zK8 - Christine
我仍然无法确定错误在哪里。从视觉上看，角度变化应该是大约30度的z轴，而不是-148度。 - Christine
1
创建了一个讨论这个问题的房间：https://chat.stackoverflow.com/rooms/218527/pythonic-rotations - zabop
2
我可能有点晚了，zabop的回答已经指向了正确的方向。我只想澄清两件事。
当我们使用变换时，存在几个不确定性，这可能会使事情更加混乱。可能会让这里的代码有点混乱的两件事是：

active vs. passive rotation (主动旋转与被动旋转)
intrinsic vs. extrinsic rotation (本质旋转与外在旋转)

我从您上面的示例开始：
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_0.T再应用r_1。（如果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 使用的是主动旋转矩阵，而pytransform3d.rotations.euler_xyz_from_matrix生成的旋转矩阵是一个被动旋转矩阵！以前的版本没有这么清楚地记录下来。您可以通过矩阵转置将活动旋转矩阵转换为被动旋转矩阵，反之亦然。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)

结果：数组（[-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]])
网页内容由stack overflow 提供, 点击上面的
可以查看英文原文，
原文链接