extract roll pitch yaw rotation angles from 3D transform

34 次查看（过去 30 天）
Alan Turing
Alan Turing 2021-9-29
编辑： David Goodmanson 2021-10-9
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 个评论
David Goodmanson
David Goodmanson 2021-10-1
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).

请先登录，再进行评论。

请先登录，再回答此问题。

回答（3 个）

Bruno Luong
Bruno Luong 2021-10-1
编辑：Bruno Luong 2021-10-1
See if you can find the formula your are looking for
https://www.geometrictools.com/Documentation/EulerAngles.pdf
Matt J
Matt J 2021-10-1
编辑：Matt J 2021-10-1
Ran in:
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
  4 个评论
显示 2更早的评论
Bruno Luong
Bruno Luong 2021-10-1
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
>>

请先登录，再进行评论。

David Goodmanson
David Goodmanson 2021-10-9
编辑：David Goodmanson 2021-10-9
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 . . ]

类别

在 Help Center 和 File Exchange 中查找有关 Surface and Mesh Plots 的更多信息

标签

产品

Community Treasure Hunt

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

Start Hunting!

Translated by