How to calculate the angle from rotation matrix

Question 1

We can get Euler angles from rotation matrix using following formula.

Given a 3×3 rotation matrix

enter image description here

The 3 Euler angles are

enter image description here

Here atan2 is the same arc tangent function, with quadrant checking, you typically find in C or Matlab.

Note: Care must be taken if the angle around the y-axis is exactly +/-90°. In that case all elements in the first column and last row, except the one in the lower corner, which is either 1 or -1, will be 0 (cos(1)=0). One solution would be to fix the rotation around the x-axis at 180° and compute the angle around the z-axis from: atan2(r_12, -r_22).

See also https://www.geometrictools.com/Documentation/EulerAngles.pdf, which includes implementations for six different orders of Euler angles.

Question 2

If R is the (3x3) rotation matrix, then the angle of rotation will be acos((tr(R)-1)/2), where tr(R) is the trace of the matrix (i.e. the sum of the diagonal elements).

That is what you asked for; I estimate a 90% chance that it is not what you want.

Question 3

Would like to put a contribution here as I was working on the same problem. I add value to the above answers by posting a pure python implementation for converting a 3-D rotation matrix (3x3) to the corresponding roll (Rx) , pitch (Ry) , yaw (Rz) angles.

Reference pseudocode: https://www.gregslabaugh.net/publications/euler.pdf (link is somewhat no longer valid / is broken in 2021... but i include it here for completeness nonetheless)

Reference problem setup: Say we have a 3x3 rotation matrix and we want to extract the Euler angles in degrees. I will make the Python implementation as 'obvious' as possible to make it easy to decipher what is going on in the script. Respective coders can optimize it for their own use.

Assumptions: We rotate the first about the x-axis, followed by the y-axis, and finally the z-axis. This ordering-definition must be respected when you are adapting this code snippet.

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

Hope this helps fellow coders out there!

Question 4

For your reference, this code computes the Euler angles in 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];

Code provided by Graham Taylor, Geoff Hinton and Sam Roweis. For more information, see here

Question 5

Let R1c and R2c be the 2 rotation matrices you have computed. These express the rotations from the object in poses 1 and 2 respectively to the camera frame (hence the second c suffix). The rotation matrix you want is from pose 1 to pose 2, i.e. R12. To compute it you must rotate, in your mind, the object from pose_1-to-camera, then from the camera-to-pose_2. The latter rotation is the inverse of the pose_2-to-camera espressed by R2c, hence:

R12 = R1c * inv(R2c)

From matrix R12 you can then compute the angle and axis of rotation using Rodiguez's formula.

Question 6

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)

Question 7

For the case of 2D, python code.

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

Question 8

I guess you wanna know how compute exact angle from rotation matrix. first of all, you should decide to order (XYZ, ZYZ, ZXZ, etc..) from the result of product of rotation's, you can take angles by inverse sinusoidal function. (using rotation matrix you've already took!)