我正在使用单个对象的两个图像，该对象从其第一个图像旋转了一定程度。
我已经计算了每个图像的 POSE，并使用 Rodergues() 将旋转矢量转换为矩阵。现在我如何计算并查看它从第一个位置旋转了多少？
我尝试了很多方法，但答案不是很接近
编辑：我的相机是固定的，只有物体在移动。
问问题
80413 次
8 回答
73
我们可以使用以下公式从旋转矩阵中得到欧拉角。
给定一个 3×3 的旋转矩阵
3 个欧拉角是
这里 atan2 是相同的反正切函数，带有象限检查，您通常在 C 或 Matlab 中找到。
注意：如果绕 y 轴的角度正好是 +/-90°，则必须小心。在这种情况下，第一列和最后一行中的所有元素，除了下角的元素，即 1 或 -1，都将为 0 (cos(1)=0)。一种解决方案是将绕 x 轴的旋转固定为 180°，并从以下位置计算绕 z 轴的角度：atan2(r_12, -r_22)。
另请参阅https://www.geometrictools.com/Documentation/EulerAngles.pdf，其中包括六个不同阶欧拉角的实现。
于 2013-02-22T17:01:31.470 回答
21
如果R是 (3x3) 旋转矩阵，则旋转角度将为 acos((tr( R )-1)/2)，其中 tr( R ) 是矩阵的轨迹（即对角线元素之和） ）。
这就是你所要求的；我估计有 90% 的可能性不是你想要的。
于 2013-02-22T15:50:05.370 回答
6
想在这里做出贡献，因为我正在解决同样的问题。我通过发布用于将 3-D 旋转矩阵 (3x3) 转换为相应的滚动 (Rx) 、俯仰 (Ry) 、偏航 (Rz) 角度的纯 python 实现来增加上述答案的价值。
参考伪代码： https ://www.gregslabaugh.net/publications/euler.pdf （链接在某种程度上不再有效/在 2021 年被破坏......但我仍然将其包含在此处以保持完整性）
参考问题设置：假设我们有一个 3x3 旋转矩阵，我们想要提取欧拉角的度数。我将使 Python 实现尽可能“明显”，以便于理解脚本中发生的事情。相应的编码人员可以对其进行优化以供自己使用。
假设：我们首先围绕 x 轴旋转，然后是 y 轴，最后是 z 轴。当您调整此代码片段时，必须遵守此排序定义。
"""
Illustration of the rotation matrix / sometimes called 'orientation' matrix
R = [
R11 , R12 , R13,
R21 , R22 , R23,
R31 , R32 , R33
]
REMARKS:
1. this implementation is meant to make the mathematics easy to be deciphered
from the script, not so much on 'optimized' code.
You can then optimize it to your own style.
2. I have utilized naval rigid body terminology here whereby;
2.1 roll -> rotation about x-axis
2.2 pitch -> rotation about the y-axis
2.3 yaw -> rotation about the z-axis (this is pointing 'upwards')
"""
from math import (
asin, pi, atan2, cos
)
if R31 != 1 and R31 != -1:
pitch_1 = -1*asin(R31)
pitch_2 = pi - pitch_1
roll_1 = atan2( R32 / cos(pitch_1) , R33 /cos(pitch_1) )
roll_2 = atan2( R32 / cos(pitch_2) , R33 /cos(pitch_2) )
yaw_1 = atan2( R21 / cos(pitch_1) , R11 / cos(pitch_1) )
yaw_2 = atan2( R21 / cos(pitch_2) , R11 / cos(pitch_2) )
# IMPORTANT NOTE here, there is more than one solution but we choose the first for this case for simplicity !
# You can insert your own domain logic here on how to handle both solutions appropriately (see the reference publication link for more info).
pitch = pitch_1
roll = roll_1
yaw = yaw_1
else:
yaw = 0 # anything (we default this to zero)
if R31 == -1:
pitch = pi/2
roll = yaw + atan2(R12,R13)
else:
pitch = -pi/2
roll = -1*yaw + atan2(-1*R12,-1*R13)
# convert from radians to degrees
roll = roll*180/pi
pitch = pitch*180/pi
yaw = yaw*180/pi
rxyz_deg = [roll , pitch , yaw]
希望这可以帮助其他编码人员！
于 2020-10-13T13:28:36.080 回答
3
供您参考，此代码在 MATLAB 中计算欧拉角：
function Eul = RotMat2Euler(R)
if R(1,3) == 1 | R(1,3) == -1
%special case
E3 = 0; %set arbitrarily
dlta = atan2(R(1,2),R(1,3));
if R(1,3) == -1
E2 = pi/2;
E1 = E3 + dlta;
else
E2 = -pi/2;
E1 = -E3 + dlta;
end
else
E2 = - asin(R(1,3));
E1 = atan2(R(2,3)/cos(E2), R(3,3)/cos(E2));
E3 = atan2(R(1,2)/cos(E2), R(1,1)/cos(E2));
end
Eul = [E1 E2 E3];
代码由 Graham Taylor、Geoff Hinton 和 Sam Roweis 提供。有关更多信息，请参见此处
于 2017-09-28T03:33:43.653 回答
1
让 R1c 和 R2c 成为您计算的 2 个旋转矩阵。它们分别表示从姿势 1 和 2 中的对象到相机帧的旋转（因此是第二个 c 后缀）。你想要的旋转矩阵是从pose 1到pose 2，即R12。要计算它，您必须在脑海中将物体从pose_1 旋转到camera，然后从camera-to-pose_2 旋转。后一个旋转是 R2c 表达的pose_2-to-camera的倒数，因此：
R12 = R1c * inv(R2c)
然后，您可以从矩阵 R12 使用 Rodiguez 的公式计算角度和旋转轴。
于 2013-02-24T18:44:59.710 回答
1
对于 2D 的情况，python 代码。
import numpy as np
import matplotlib.pyplot as plt
def get_random_a(r = 3, centre_x = 5, centre_y = 5):
angle = np.random.uniform(low=0.0, high=2 * np.pi)
x = np.cos(angle) * r
y = np.sin(angle) * r
x += centre_x
y += centre_y
return x, y
def norm(x):
return np.sqrt(x[0] ** 2 + x[1] ** 2)
def normalize_vector(x):
return x / norm(x)
def rotate_A_onto_B(vector_a, vector_b ):
A = normalize_vector(vector_a)
B = normalize_vector(vector_b)
cos_theta = np.dot(A, B)
sin_theta = np.cross(A, B)
theta = np.arctan2(sin_theta, cos_theta)
M = np.array ( [[np.cos(theta ), -np.sin(theta)],
[np.sin(theta), np.cos(theta) ]
])
M_dash = np.array( [ [cos_theta, -sin_theta],
[sin_theta, cos_theta]
])
print( f" np all close of M and M_dash : {np.allclose(M, M_dash)}" )
vector_a = vector_a[:, np.newaxis]
rotated_vector_a = np.dot(M, vector_a)
return rotated_vector_a.squeeze()
#--------------
#----------------
centre_x, centre_y = 5, 5
r = 3
b = (centre_x, centre_y - r)
vector_b = np.array ( ( b[0] - centre_x, b[1] - centre_y ) )
x, y = get_random_a(r, centre_x, centre_y)
vector_a = np.array ( ( x - centre_x, y - centre_y ) )
rotated_vector_a = rotate_A_onto_B(vector_a, vector_b)
print("is the answer corrent ? ", np.allclose(rotated_vector_a, vector_b))
print(rotated_vector_a)
# plot
plt.plot( [centre_x, x], [ centre_y, y ] )
plt.plot( [centre_x, b[0]], [centre_y, b[1]] )
plt.scatter( [centre_x, x, b[0] ], [ centre_y, y, b[1] ], c = "r")
plt.text(centre_x, centre_y, f"centre : {centre_x, centre_y}")
plt.text(x, y, f"a : {x, y}")
plt.text(b[0], b[1], f"b : {b[0], b[1]}")
plt.xlim(left = centre_x - r - 1, right = centre_x + r + 1 )
plt.ylim(bottom= centre_y -r - 1 , top = centre_y + r +1 )
plt.show()
于 2021-05-24T17:35:27.240 回答
1
我想你想知道如何从旋转矩阵计算精确的角度。首先，您应该根据旋转乘积的结果决定订购（XYZ、ZYZ、ZXZ 等），您可以通过反正弦函数获取角度。（使用您已经采用的旋转矩阵！）
于 2021-07-20T00:55:46.360 回答
1
Suppose it mainly rotates around a certain coordinate axis(A person stands in front of the camera and rotates around to get the rotation matrix), try the following code:
float calc_angle(Eigen::Matrix3f &R_, int axis_)
{
//! the coordinate system is consistent with "vedo"
//! suppose it mainly rotates around a certain coordinate axis(X/Y/Z)
Eigen::Vector3f aX_(1.0f, 0.0f, 0.0f);
Eigen::Vector3f aY_(0.0f, 1.0f, 0.0f);
Eigen::Vector3f aZ_(0.0f, 0.0f, 1.0f);
Eigen::Vector3f v0_, v1_;
int axis_contrary_[2];
switch (axis_)
{
case 0 /* x */:
axis_contrary_[0] = 1;
axis_contrary_[1] = 2;
v0_ = aY_;
v1_ = aZ_;
break;
case 1 /* y */:
axis_contrary_[0] = 0;
axis_contrary_[1] = 2;
v0_ = aX_;
v1_ = aZ_;
break;
case 2 /* z */:
axis_contrary_[0] = 0;
axis_contrary_[1] = 1;
v0_ = aX_;
v1_ = aY_;
break;
}
Eigen::Vector3f v0_new_ = R_ * v0_; //R_.col(axis_contrary_[0]);
v0_new_(axis_) = 0.0f;
v0_new_.normalize();
Eigen::Vector3f v1_new_ = R_ * v1_; //R_.col(axis_contrary_[1]);
v1_new_(axis_) = 0.0f;
v1_new_.normalize();
Eigen::Vector3f v2_new_0_ = v0_.cross(v0_new_);
Eigen::Vector3f v2_new_1_ = v1_.cross(v1_new_);
bool is_reverse = ((v2_new_0_[axis_] + v2_new_1_[axis_]) / 2.0f < 0.0f);
float cos_theta_0_ = v0_new_(axis_contrary_[0]);
float cos_theta_1_ = v1_new_(axis_contrary_[1]);
float theta_0_ = std::acos(cos_theta_0_) / 3.14f * 180.0f;
float theta_1_ = std::acos(cos_theta_1_) / 3.14f * 180.0f;
// std::cout << "theta_0_: " << theta_0_ << std::endl;
// std::cout << "theta_1_: " << theta_1_ << std::endl;
float theta_ = (theta_0_ + theta_1_) / 2.0f;
float deg_;
if (!is_reverse)
{
deg_ = theta_;
}
else
{
deg_ = 360.0f - theta_;
}
return deg_;
}
and you can visualize with the following code:
import numpy as np
from glob import glob
from vedo import *
path_folder = ".../data/20210203_175550/res_R"
path_R_ALL = sorted(glob(path_folder + "/*_R.txt"))
path_t_ALL = sorted(glob(path_folder + "/*_t.txt"))
o = np.array([0, 0, 0])
x = np.mat([1, 0, 0]).T
y = np.mat([0, 1, 0]).T
z = np.mat([0, 0, 1]).T
vp = Plotter(axes=4)
vp += Box((0, 0, 0), 3, 3, 3, alpha=0.1)
for i, (path_R, path_t) in enumerate(zip(path_R_ALL, path_t_ALL)):
R = np.loadtxt(path_R)
R = np.mat(R.reshape(3, 3)).T
# t = np.loadtxt(path_t)
# t = np.mat(t).T
Ax = Line(o, R*x, c="r")
Ay = Line(o, R*y, c="g")
Az = Line(o, R*z, c="b")
vp += Ax
vp += Ay
vp += Az
vp.show(interactive=1)
vp -= Ax
vp -= Ay
vp -= Az
x_new = R*x
x_new[1] = 0
x_new = x_new / np.linalg.norm(x_new)
# print("x_new:", x_new)
z_new = R*z
z_new[1] = 0
z_new = z_new / np.linalg.norm(z_new)
# print("z_new:", z_new)
cos_thetaX = x.T * x_new
thetaX = np.arccos(cos_thetaX) / 3.14 * 180
cos_thetaZ = z.T * z_new
thetaZ = np.arccos(cos_thetaZ) / 3.14 * 180
# print(x, x_new)
tmpX = np.cross(x.T, x_new.T)
# print("tmpX:", tmpX)
if tmpX[0][1] < 0:
thetaX = 360 - thetaX
tmpZ = np.cross(z.T, z_new.T)
# print("tmpZ:", tmpZ)
if tmpZ[0][1] < 0:
thetaZ = 360 - thetaZ
# print(i, tmpX, tmpZ)
print(i, thetaX, thetaZ)
于 2021-05-13T03:32:53.603 回答