How to calculate the angle from rotation matrix
Solution 1
We can get Euler angles from rotation matrix using following formula.
Given a 3×3 rotation matrix
The 3 Euler angles are
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.
Solution 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.
Solution 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!
Solution 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
Solution 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.
N.J
Updated on July 20, 2021Comments
-
N.J almost 3 years
I am using two image of the single object the object is roated certain degree from its first image.
I have calculated the POSE of each image and converted the rotational vector to Matrix using Rodergues(). Now how do I calculate and see how much it is rotated from its first position?
I have tried many ways but answers was no were close
EDIT: My camera is fixed only the object is moving.
-
N.J over 11 yearsThank you so much for your help.. +1
-
x squared almost 10 yearsIs there also a way to determine in which order these rotations have to be applied to achieve this matrix?
-
Saravanabalagi Ramachandran over 6 yearsthis gives exactly one scalar value corresponding to the angle of rotation along which axis?
-
Beta over 6 years@SaravanabalagiRamachandran: The eigenvector of the matrix, of course. Just solve (R-I)X= 0 for X (where X and 0 are vectors).
-
Patrick Stalph about 6 yearsHere, the order of rotation is Rx, then Ry, then Rz. Thus RzRyRx=R
-
Бојан Матовски about 5 yearsI wanted to try it, so I wrote a Python version: import numpy as np def rot_mat_to_euler(r): if (r[0, 2] == 1) | (r[0, 2] == -1): # special case e3 = 0 # set arbitrarily dlt = np.arctan2(r[0, 1], r[0, 2]) if r[0, 2] == -1: e2 = np.pi/2 e1 = e3 + dlt else: e2 = -np.pi/2 e1 = -e3 + dlt else: e2 = -np.arcsin(r[0, 2]) e1 = np.arctan2(r[1, 2]/np.cos(e2), r[2, 2]/np.cos(e2)) e3 = np.arctan2(r[0, 1]/np.cos(e2), r[0, 0]/np.cos(e2)) return [e1, e2, e3]
-
mrgloom over 4 yearsWhat is
angle of rotation
in this case? -
John Ernest over 4 yearsHow was the cosine part of the pitch calculation obtained (sqrt of r_32^2 + r_33^3)? Most literature I read only gives an arc sine calculation for the pitch. On a ZYX matrix I can also get the same via sqrt of r_00^2+r_10^2.
-
AsthaUndefined almost 3 yearsPlease add more details to your answer.