3

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

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

4

2 回答 2

3

您可以使用matrix_from_euler_xyz本教程中的内容来检查您的结果。

（您可能需要在pip3 install pytransform3d运行 Python 代码的终端中运行，或者!pip3 install pytransform3d如果您正在使用 Jupyter Notebook，则可能需要在终端中运行。）

准备数据：

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

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

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

于 2020-07-22T18:35:43.600 回答
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_0r_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)

结果：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]])
于 2020-12-30T12:47:15.610 回答