如何从旋转矩阵计算角度

Question

我正在使用单个对象的两个图像，该对象从其第一个图像旋转了一定程度。

我已经计算了每个图像的 POSE，并使用 Rodergues() 将旋转矢量转换为矩阵。现在我如何计算并查看它从第一个位置旋转了多少？

我尝试了很多方法，但答案都不是很接近

编辑：我的相机是固定的，只有物体在移动。

Answer 1

如果 R 是 (3x3) 旋转矩阵，则旋转角度将为 acos((tr(R)-1)/2)，其中 tr (R)是矩阵的迹（即对角线元素之和）。

这就是你所要求的；我估计有 90% 的可能性不是你想要的。

Answer 2

我们可以使用以下公式从旋转矩阵中得到欧拉角。

给定一个 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，其中包括六个不同阶欧拉角的实现。

Answer 3

让 R1c 和 R2c 成为您计算的 2 个旋转矩阵。它们分别表示从姿势 1 和 2 中的对象到相机帧的旋转（因此是第二个 c 后缀）。您想要的旋转矩阵是从姿势 1 到姿势 2，即 R12。要计算它，您必须在脑海中将物体从pose_1 旋转到camera，然后从camera-to-pose_2 旋转。后一种旋转是 R2c 表达的pose_2-to-camera的倒数，因此：

R12 = R1c * inv(R2c)

然后您可以从矩阵 R12 使用 Rodiguez 公式计算旋转角度和轴。

Answer 4

供您参考，此代码在 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 提供。欲了解更多信息，请参阅here

Answer 5

由于我正在解决同样的问题，因此想在这里做出贡献。我通过发布用于将 3-D 旋转矩阵 (3x3) 转换为相应的滚动 (Rx) 、俯仰 (Ry) 、偏航 (Rz) 角度的纯 python 实现来增加上述答案的价值。

参考伪代码：https://www.gregslabaugh.net/publications/euler.pdf（链接在某种程度上不再有效/在 2021 年被破坏......但为了完整起见，我将其包含在此处）

参考问题设置：假设我们有一个 3x3 旋转矩阵，我们想要提取欧拉角（以度为单位）。我将使 Python 实现尽可能“明显”，以便于理解脚本中发生的事情。相应的编码人员可以对其进行优化以供自己使用。

假设：我们首先围绕 x 轴旋转，然后是 y 轴，最后是 z 轴。当您调整此代码 sn-p 时，必须遵守此排序定义。

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

希望这对其他程序员有所帮助！

Answer 6

假设它主要围绕某个坐标轴旋转（一个人站在镜头前，左右旋转得到旋转矩阵），试试下面的代码：

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_;
}

您可以使用以下代码进行可视化：

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)

Answer 7

对于二维的情况，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()

Answer 8

我猜你想知道如何从旋转矩阵计算精确的角度。 首先，您应该决定订购（XYZ、ZYZ、ZXZ 等。） 根据旋转乘积的结果，您可以通过反正弦函数获取角度。 （使用您已经采用的旋转矩阵！）