3D 中的相對旋轉

Question

我有一個 object （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])

問題是，相對角度對我來說看起來不對，但我找不到我的錯誤。 我想知道的是 object 沿 x、y 和 z 旋轉了多少。

編輯：繪圖代碼： https://codeshare.io/GA9zK8

Answer 1

您可以使用本教程中的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)

讓我們 plot 旋轉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()

我們得到這個 plot：

您可以檢查這是否是您所期望的。

檢查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使用主動旋轉矩陣，而pytransform3d.rotations.euler_xyz_from_matrix產生的旋轉矩陣是被動旋轉矩陣。 這在以前的版本中沒有如此清楚地記錄。 您可以使用矩陣轉置將主動旋轉矩陣轉換為被動旋轉矩陣，反之亦然。 pytransform3d 的 function 和 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]])