extract roll pitch yaw rotation angles from 3D transform

110 views (last 30 days)
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 !)
  1 Comment
David Goodmanson
David Goodmanson on 1 Oct 2021
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).

Sign in to comment.

Answers (3)

Bruno Luong
Bruno Luong on 1 Oct 2021
Edited: Bruno Luong on 1 Oct 2021
See if you can find the formula your are looking for

Matt J
Matt J on 1 Oct 2021
Edited: Matt J on 1 Oct 2021
The quadricFit class in this File Exchange package
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
  4 Comments
Bruno Luong
Bruno Luong on 1 Oct 2021
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
>>

Sign in to comment.


David Goodmanson
David Goodmanson on 9 Oct 2021
Edited: David Goodmanson on 9 Oct 2021
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 . . ]

Categories

Find more on Eigenvalues in Help Center and File Exchange

Tags

Products

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!