如何从旋转矩阵计算角度

Question

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

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

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

编辑：我的相机仅在物体移动时固定。

Answer 1

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

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

Answer 2

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

这就是你要求的;我估计有90％的可能性不是你想要的。

Answer 3

供您参考，此代码计算MATLAB中的Euler角度：

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 4

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

Answer 5

设R1c和R2c为您计算的2个旋转矩阵。它们分别表示从姿势1和2中的物体到相机帧的旋转（因此是第二个c后缀）。你想要的旋转矩阵是从姿势1到姿势2，即R12。要计算它，您必须在脑海中旋转pose_1到相机，然后从相机到姿势_2。后一个旋转是由R2c表示的pose_2到相机的反转，因此：

R12 = R1c * inv(R2c)

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

Answer 6

在我处理相同问题时，想在这里做些贡献。我发布了一个纯Python实现，将3-D旋转矩阵（3x3）转换为相应的横摇（Rx），俯仰（Ry）和偏航（Rz）角，从而为上述答案增加了价值。

参考伪代码： https://www.gregslabaugh.net/publications/euler.pdf

参考问题设置：假设我们有一个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] 

希望这可以帮助其他编码人员！

Answer 7

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

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 8

对于 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()