extract roll pitch yaw rotation angles from 3D transform

I am looking for a way to extract rotation angles (roll, pitch and yaw) from a transform matrix returned by:

t = imregtform(moving, fixed, 'affine', optimizer, metric);

So far I have tried the function

tform2eul(t.T)

but neither the 'ZYX' or 'ZYZ' or 'XYZ' (i.e. like tform2eul(t.T,'ZY'X'), etc) seem to return the expected results.

is there a built-in function like 'getRollPitchYaw' or something similar? if not what would you suggest ?

not that the matrices a 4x4 in homogeneous coordinates (not 3x3 matrices !)

[David Goodmanson]

Do you have any prior knowledge of the axes of rotation and of the order of multiplication of the rotation matrices? (which I would think are the upper left 3x3 submatrix).

Answer 1

[Bruno Luong]

See if you can find the formula your are looking for

https://www.geometrictools.com/Documentation/EulerAngles.pdf

Answer 2

[Matt J]

The quadricFit class in this File Exchange package

https://www.mathworks.com/matlabcentral/fileexchange/87584-object-oriented-tools-for-fitting-conics-and-quadrics

has static methods that will do it. Example.

Tyaw=quadricFit.R3d(45,[0,0,1])   %45 degree yaw transform

Tyaw = 3×3

0.7071 -0.7071 0 0.7071 0.7071 0 0 0 1.0000

Tpitch=quadricFit.R3d(20,[-1,1,0]) %20 degree pitch transform

Tpitch = 3×3

0.9698 -0.0302 0.2418 -0.0302 0.9698 0.2418 -0.2418 -0.2418 0.9397

ypr=quadricFit.rot2ypr(Tpitch*Tyaw) %recover the yaw/pitch/roll angles

ypr = 1×3

45 20 0

[Bruno Luong]

Why the pitch axis is entered as [-1,1,0]?

Normally it's y-axis http://planning.cs.uiuc.edu/node102.html

[Matt J]

These are intrinsic rotations. The y-axis moves because of the 45 degree yaw to [-1,1,0].

[Bruno Luong]

Well OK, but why make thing complicated?

Tyaw=quadricFit.R3d(45,[0,0,1])

Tyaw =

0.7071   -0.7071         0
0.7071    0.7071         0
0         0    1.0000

Tpitch=quadricFit.R3d(20,[0,1,0])

Tpitch =

0.9397         0    0.3420
0    1.0000         0
-0.3420         0    0.9397

ypr=quadricFit.rot2ypr(Tyaw*Tpitch)

ypr =

45    20     0

>> 

[Matt J]

Because I didn't know you could do that!

Answer 3

[David Goodmanson]

Hello "AT"

fwiw, here is a version of finding three rotation angles th1,th2,th3 from a 3d rotation matrix defined below.

Angles th1 and th3 run the entire angular range from -180 to 180 but th2 is restricted to -90<th2<90 for tait-bryan angles, 0<th2<180 for euler angles. Without that restriction there are two sets of solution angles for a given R, rather than one set.

The code does not directly address the singular cases when th2 is at the ends of its range. I believe those are the gimbal lock situations (Apollo 13!). However, if you construct a matrix R with known angles and see how well the answer agrees, to have disagreement >10^-9 on any angle then th2 has to be within one or the other end of its range by something on the order of 10^-4 degrees.

function theta = anglesR(R,str)

% solve a 3d rotation matrix R of the form Ra(th1)*Rb(th2)*Rc(th3)
% for angles th1,th2,th3; where each of a,b c are one of x,y,z.
% input str is a three-letter string of rotation axes, such as 'yzx'.
% consecutive rotations along the same axis such as 'xxy' are not allowed.
% 1st and 3rd rotations along different axes such as 'yzx' are tait-bryan,
% along the same axis such as 'xzx' are euler.  12 possibilities in all.
% Output is the vector [th1 th2 th3] and angles are in DEGREES,
% with -180<(th1,th3)<180;  -90<th2<90 (tait-bryan), 0<th2<180 (euler).  
% see below for angle restrictions and matrices Rx,Ry,Rz

theta = zeros(1,3);
% similarity transform matrices
By = [0 0 1;0 -1 0;1 0 0];     % x<-->z  y(th)-->y(-th)
Ry90 = [0 0 1;0 1 0; -1 0 0];  % y rotation by 90 deg
C = [0 0 1;1 0 0; 0 1 0;];     % cycic, x-->y-->z-->x
signy = 1;
% tranform to RaRyRb
if str(2) == 'x'
    R = C*R/C;
elseif str(2) == 'z'
    R = C\R*C;
end
% tait-bryan, transform to RxRyRz 
if all(str=='xzy')|all(str=='yxz')|all(str=='zyx')
    R = By*R/By;
    signy = -1;
end
% euler, transform to RxRyRx 
if all(str=='xzx')|all(str=='yxy')|all(str=='zyz')
    R = Ry90*R/Ry90;
end  
if str(1)~=str(3)                 % tait-bryan
    theta(2) = signy*asind(R(1,3));
    theta(1) = atan2d(-R(2,3),R(3,3));
    theta(3) = atan2d(-R(1,2),R(1,1));  
else                              % euler
    theta(2) = acosd(R(1,1));
    theta(1) = atan2d(R(2,1),-R(3,1));
    theta(3) = atan2d(R(1,2), R(1,3));
end
end

% Rotation of an object's coordinates.  Counterclockwise
% rotation looking down at the rotation axis coming up out of the page.
%
%        [1 0  0         [c 0 s          [c -s 0        
%  Rx =   0 c -s    Ry =  0 1 0    Rz =   s  c 0
%         0 s  c]        -s 0 c]          0  0 1]
%
%  tait-bryan RxRyRz
%  [c2c3  -c2s3  s2
%    .      .   -s1c2
%    .      .    c1c2]
% 
% euler RxRyRx
%  [c2    s2s3  s2c3
%   s1s2   .     .
%  -c1s2   .     .  ]