Difference between 2 orientations

I have a sensor in an orientation - let's call it the target orientation. The orientation is in euler angles in degrees (pitch = 50, roll = 50 and yaw = 50). I move the sensor about then I try to return it to the target orientation. The 'attempt' orientation is in euler angles (pitch = 70, roll = 70 and yaw = 70). I am interested in the difference between the 2 orientations (how close is the 'attempt' to the 'target' orientation?).

3 suggested methods have been proposed:

1. Subtract the orientations to yield a difference of 20 degrees pitch, 20 degrees roll and 20 degrees yaw.
2. Compute the resultant orientation between the 2 orientations by converting euler angles to rotation matrices, multipling 2 rotation matrices and then extracting the euler angles (this gives 19 degrees pitch, 12 degrees roll and 4 degrees yaw.
3. Converting the euler angles to quaternions and multipling the 2 qw components, then extracting an rotation from this (this gives 105 degrees).

I am interested in the 'correct way' are reporting the error between the attempt and the target. Any help gratefully recieved.

I post the code for clarity. Notes for the code - euler angles with an a-suffix are the target orientation, b-suffix are the attempt orientation. Is it written with for loops for when time series data are added.

hdga = deg2rad(50);

pitcha = deg2rad(50);

rolla = deg2rad(50);

hdgb = deg2rad(70);

pitchb = deg2rad(70);

rollb = deg2rad(70);

ch = cos(hdga); sh = sin(hdga);

cp = cos(pitcha); sp = sin(pitcha);

cr = cos(rolla); sr = sin(rolla);

qxa = (sin(rolla*0.5) * cos(pitcha*0.5) * cos(hdga*0.5)) - (cos(rolla*0.5) * sin(pitcha*0.5) *sin(hdga*0.5));

qya = (cos(rolla*0.5) * sin(pitcha*0.5) * cos(hdga*0.5)) + (sin(rolla*0.5) * cos(pitcha*0.5) * sin(hdga*0.5));

qza = (cos(rolla*0.5) * cos(pitcha*0.5) * sin(hdga*0.5)) - (sin(rolla*0.5) * sin(pitcha*0.5) * cos(hdga*0.5));

qwa = (cos(rolla*0.5) * cos(pitcha*0.5) * cos(hdga*0.5)) + (sin(rolla*0.5) * sin(pitcha*0.5) * sin(hdga*0.5));

len = length(hdga);

Rrpha = zeros(len,9);

Rrpha(:,1:3) = [cp.*ch, -sp, cp.*sh];

Rrpha(:,4:6) = [cr.*sp.*ch + -sr.*-sh, cr.*cp, cr.*sp.*sh + -sr.*ch];

Rrpha(:,7:9) = [sr.*sp.*ch + cr.*-sh, sr.*cp, sr.*sp.*sh + cr.*ch];

%%

ch = cos(hdgb); sh = sin(hdgb);

cp = cos(pitchb); sp = sin(pitchb);

cr = cos(rollb); sr = sin(rollb);

qxb = (sin(rollb*0.5) * cos(pitchb*0.5) * cos(hdgb*0.5)) - (cos(rollb*0.5) * sin(pitchb*0.5) *sin(hdgb*0.5));

qyb = (cos(rollb*0.5) * sin(pitchb*0.5) * cos(hdgb*0.5)) + (sin(rollb*0.5) * cos(pitchb*0.5) * sin(hdgb*0.5));

qzb = (cos(rollb*0.5) * cos(pitchb*0.5) * sin(hdgb*0.5)) - (sin(rollb*0.5) * sin(pitchb*0.5) * cos(hdgb*0.5));

qwb = (cos(rollb*0.5) * cos(pitchb*0.5) * cos(hdgb*0.5)) + (sin(rollb*0.5) * sin(pitchb*0.5) * sin(hdgb*0.5));

Rrphb = zeros(len,9);

Rrphb(:,1:3) = [cp.*ch, -sp, cp.*sh];

Rrphb(:,4:6) = [cr.*sp.*ch + -sr.*-sh, cr.*cp, cr.*sp.*sh + -sr.*ch];

Rrphb(:,7:9) = [sr.*sp.*ch + cr.*-sh, sr.*cp, sr.*sp.*sh + cr.*ch];

for n= 1: len

dca{n}=[Rrpha(n,1) Rrpha(n,2) Rrpha(n,3); Rrpha(n,4) Rrpha(n,5) Rrpha(n,6); Rrpha(n,7) Rrpha(n,8) Rrpha(n,9)];

end

for n= 1: len

dcb{n}=[Rrphb(n,1) Rrphb(n,2) Rrphb(n,3); Rrphb(n,4) Rrphb(n,5) Rrphb(n,6); Rrphb(n,7) Rrphb(n,8) Rrphb(n,9)];

end

for n= 1: len

R{n}=(dca{n}'* dcb{n});

roll(n) = (atan2(R{n}(3,2),R{n}(2,2)))/pi*180

heading(n) = (atan2(R{n}(3,1),R{n}(1,1)))/pi*180

pitch(n) = -asin(R{n}(1,2))/pi*180

end

qwR = qwa*qwb;

qwRES = rad2deg(2*acos(qwR))

[Jonathan Williams]

Some great options offered. The code now uses the inbuilt functions to provide solutions and the multiplication has been corrected.

The solutions match those offered by James Tursa, David Goodmanson in his function and Bruno Luong.

What I am struggling to understand is why method 1 is not acceptable.

If I was using a single rotation and the target was 50deg pitch and the attempt was 70 deg pitch, the 'error' defined as the difference between the attempt and the target is 20 degrees, isn't it? However this is not the case when using rotation matrices, for example if pitchb = 70 and rollb = 50 and hdgb = 50. It is only the case if using pitchb = 50, rollb = 50 and hdgb = 70. I am afraid I do not understand this.

If I were to have the target orientation pitcha, rolla, hdga all 50 degrees and the attempt orientation to be pitchb= 50, rollb = 70 and hdgb =70, what is the error? According to my code the 'error' for XYZ sequence is -18.9deg pitch, -6.7deg roll, -21.1deg hdg. But the pitch angle was the same between the target and the attempt. Any help gratefully recieved.

clear all
hdga = deg2rad(50);
pitcha = deg2rad(50);
rolla = deg2rad(50);
hdgb = deg2rad(70);
pitchb = deg2rad(70);
rollb = deg2rad(70);

%% using quaternions
Ea = [pitcha rolla hdga];
QaXYZ = eul2quat(Ea,'XYZ');
Eb = [pitchb rollb hdgb];
QbXYZ = eul2quat(Eb,'XYZ');
QResXYZ = quatmultiply(QaXYZ,quatinv(QbXYZ));
if( QResXYZ(1) < 0 )
    QResXYZ = -QResXYZ;
end
angle_quatXYZ = 2*acosd(QResXYZ(1))

Ea = [pitcha rolla hdga];
QaZYX = eul2quat(Ea,'ZYX');
Eb = [pitchb rollb hdgb];
QbZYX = eul2quat(Eb,'ZYX');
QResZYX = quatmultiply(QaZYX,quatinv(QbZYX));
if( QResZYX(1) < 0 )
    QResZYX = -QResZYX;
end
angle_quatZYX = 2*acosd(QResZYX(1))

Ea = [pitcha rolla hdga];
QaZYZ = eul2quat(Ea,'ZYZ');
Eb = [pitchb rollb hdgb];
QbZYZ = eul2quat(Eb,'ZYZ');
QResZYZ = quatmultiply(QaZYZ,quatinv(QbZYZ));
if( QResZYZ(1) < 0 )
    QResZYZ = -QResZYZ;
end
angle_quatZYZ = 2*acosd(QResZYZ(1))


%% Using rotation matrices
ErotmataXYZ = eul2rotm(Ea,'XYZ');
ErotmatbXYZ = eul2rotm(Eb,'XYZ');
ErotmatResXYZ= inv(ErotmatbXYZ) * ErotmataXYZ;
EResRotMatCalcXYZ = rotm2eul(ErotmatResXYZ,'XYZ');
EResRotMatCalcdegXYZ = rad2deg(EResRotMatCalcXYZ)

ErotmataZYX = eul2rotm(Ea,'ZYX');
ErotmatbZYX = eul2rotm(Eb,'ZYX');
ErotmatResZYX= inv(ErotmatbZYX) * ErotmataZYX;
EResRotMatCalcZYX = rotm2eul(ErotmatResZYX,'ZYX');
EResRotMatCalcdegZYX = rad2deg(EResRotMatCalcZYX)

ErotmataZYZ = eul2rotm(Ea,'ZYZ');
ErotmatbZYZ = eul2rotm(Eb,'ZYZ');
ErotmatResZYZ= inv(ErotmatbZYZ) * ErotmataZYZ;
EResRotMatCalcZYZ = rotm2eul(ErotmatResZYZ,'ZYZ');
EResRotMatCalcdegZYZ = rad2deg(EResRotMatCalcZYZ)

[Bruno Luong]

To illustrate option 1 is not fully correct is similar to the siluation where you find the distance between 2 points on the earth. If you have longitude and latitude of both points, it is tempting to make the difference of longitudes and latitudes then somehow combine them as "distance". This is NOT correct since the true distance is the separation on great circle passing through both points.

Why it is not correct? I believe the triangular inequality is not respected if you do the first method for point closes to the poles. So it NOT a distances. Stuffs like that will end up having non robust code if you don't use correct mathematical definition with proper properties.

[Jonathan Williams]

So grateful for this. If I may...How would it ever be possible to simply compare orientation of 2 different 'arms'? If an IMU was placed on an arm (mechanical or human) and oriented itself to an orientation of say RPY 50, 50 50 and I wanted to compare this to the orientation of another arm, based on the reasoning you propose this would need to be done through rotation matrices (or similar). So how do people compare range of motion between individuals? Say the range of motion of the arm was 0 to 50 degree in all planes (RPY) and someone else's range of motion was 0 to 60 degrees in all planes (RPY 60, 60, 60). Is it possible to deduce one has moved further by 10 degrees in each plane, or must this be calculated via rotation matrices also?

One final query - if the set up was one IMU on the thorax and one on the arm, range of motion determined through the resultant angle of the two sensors using rotation matrices. Could these 'resultant angles', be compared directly with another individuals i.e., participant A moved 50, 50, 50 and B 60, 60, 60 therefore B moved 10 degrees more in each plane?

[Paul]

Is it possible to deduce one has moved further by 10 degrees in each plane,

Let's start with two arms perfectly aligned with each other. To make it simple, we will assume that the coordinate axes are fixed to each arm are aligned with our world axes.

The first arm rotates through a 30 deg roll and the second arm rotates through a 50 deg roll. These two angles are diretcly comparable becasue they are both rotations around the same axes, namely the x-axis of the world frame (which happens to also be the x-axis of each arm frame). So a 20 deg difference. Now, apply a pitch rotation to each arm of 30 degrees. These rotations are not comparable, because they are each a rotation around a different axis. Arm-1 is rotating about its y-axis and arm-2 is rotating about its y-axis. But these axes aren't aligned with each other because the preceding roll angles were different, so the pitch rotations represent different motion when viewed relative to the world (try to picture what happens on the pitch rotations when arm-1 roll is 0 and arm-2 roll is 90). And then the same issue with the yaw rotation. Basically, unless the rotations are around the same axis, they can't be subtracted from (or added to) each other.

Maybe your really asking this question ...

Define the DCM from the world frame to the arm-1 frame:

world2arm1 = angle2dcm(50*pi/180,40*pi/180,30*pi/180,'XYZ');

And similarly for arm-2

world2arm1 = angle2dcm(60*pi/180,60*pi/180,60*pi/180,'XYZ');

Do you want then determine the delta angles relative to (50,40,30) that result in the arm-2 orientation, i.e., solve this equation for deltar, deltap, deltay?

% world2arm2 == angle2dcm((50+deltar)*pi/180,(40+deltay)*pi/180,(30+detay)*pi/180,'XYZ'); 

In theory, this might be doable, but I'm not sure how meaningful the result would be.

[Bruno Luong]

@Jonathan Williams It is just me but but I read through your many questions in the last comment few times, I still don't understand what you are asking.

[Jonathan Williams]

So sorry if I wasn't making myself clear. I am very grateful for the onging discussion into this fascinating world. I really was interested in direct comparison (simple subtractions) and their application to the measurement of human bodies. With a single sensor it seems that 2 peoples' angles derived from RPY are not compariable in the obvious way to a clinician. Many thanks to Paul for the crystal clear explanation above. However with an additional sensor on the thorax providing orientation of the 'rigid body' of the thorax used as the reference is direct comparison it now possible? One sensor on the arm providing euler angles across time, converted to a rotation matrix (or quaternion) and multiplied by the (thorax) reference sensor's rotation matrix (or quaternion) would result in change in orientation relative to the thorac reference frame.This is the common approach to measuring range of motion (change in orientation across time). Therefore my question - if this approach was completed for 2 different individuals, are the 2 sets of resultant euler angles (one for each individual) directly comparable with simple subtraction?

[James Tursa]

3D rotations in general are never directly comparible with simple subtractions of the Euler angles, as shown in the answers below. The only time you could do that calculation and have it make sense is if the angles in question are only planar (motion taking place all in one plane), or are very small are you are doing a very small angle approximation to the rotations. But that is not what you are doing.

[Bruno Luong]

Further more euler angles are not uniquely defined for a given orientation, generally there is 2 solutions (not counting the fact that each angle can be wrapped independently on 360 degrees)

http://eecs.qmul.ac.uk/~gslabaugh/publications/euler.pdf

When comparing 2 orientations so between 2 x 2 sets of euler angles, which difference (out of 4 possibility) is "right" ?

Topogically the orientation space SO(3) is different than Euler-angles which is SO(1)^3. Therefore it must have a discontinuity when mapping orientation to Euler angles, meaning it must have two orientation arbitrary close BUT their respective Euler angles are not close to each other (this point is like the two poles on earch which does not have continuity of lon/lat coordinates).

That's why Euler angles cannot be used consistently for any calculation of orientation.

[Bruno Luong]

To illustrate the discontinuity of euler angles with a concrete numerical example, see the result of the bellow code. So clearly why it should NOT be used to evaluate the difference between two orientations:

% Generate the first orientation
A = [1 0 0;
     0 1 0;
     1e-12 1e-6 1];
% Orthonormalize
[A,~] = qr(A);
% Make sure axis direction are as I wanted (close to Identity)
for k=1:3
    if A(k,k)<0
        A(:,k) = -A(:,k);
    end
end
A

A = 3×3

1.0000 -0.0000 -0.0000 0 1.0000 -0.0000 0.0000 0.0000 1.0000

[PhiA, ThetaA, PsiA] = RotMat3Eu(A)

PhiA = 1.0000e-06

ThetaA = 1.0000e-06

PsiA = 3.1416

% Generate the second orientation
B = [1 0 0;
     0 1 0;
     -1e-12 -1e-6 1];
[B,~] = qr(B);
for k=1:3
    if B(k,k)<0
        B(:,k) = -B(:,k);
    end
end
B

B = 3×3

1.0000 -0.0000 0.0000 0 1.0000 0.0000 -0.0000 -0.0000 1.0000

 [PhiB, ThetaB, PsiB] = RotMat3Eu(B)

PhiB = -3.1416

ThetaB = 1.0000e-06

PsiB = -1.0000e-06

 % small showing A ~ B
norm(A-B)

ans = 2.0000e-06

 % geodesic distance SO(3), David's method
lbd = eig(B'*A);
dAB = angle(lbd(find(imag(lbd)~=0,1)))

dAB = 2.0000e-06

% Difference two of the Euler angles (Phi and Psi), ~ PI
% so the delta does NOT reflect the fact that A ~ B
dEul = [PhiA, ThetaA, PsiA] - [PhiB, ThetaB, PsiB]

dEul = 1×3

3.1416 0 3.1416

function [Phi, Theta, Psi] = RotMat3Eu(A)
% Compute Euler angles from rotation matrix
% https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms
% Rotation matrix → Euler angles (z-x-z extrinsic), just to illustrate for
% this specific convention, but the same phenomenon occurs for ANY convention
Phi = atan2(A(3,1),A(3,2));
Theta = acos(A(3,3));
Psi = -atan2(A(1,3),A(2,3)); % EDIT after Paul spots the sign error
end

[Paul]

Shouldn't the equation for Psi be (with negative sign):

Psi = -atan2(A(1,3),A(2,3));

[Bruno Luong]

Yeah good catch @Paul, I correct it. The conclusion is the same.

[David Goodmanson]

Hi Jonathan,

Comparing rotation matrices A and B, each expressed by three euler angles, can be tricky as you can see. That's why, instead of comparing two sets of three euler angles I like the idea of expressing B\A as a single rotation about some particular axis, with the angle of rotation (or a function of it such as the energy-like 8*sin(theta/2)^2 ) expressing the disagreement between A and B. It doesn't tell you how to further rotate B to make it agree with A, but at least it's a single consistent criterion for the difference between them.

Answer 1

[James Tursa]

I would do your method #3 involving quaternions. This would give you the "smallest" rotation between the two attitudes. But you need to multiply the inverse of one times the other, not just a simple multiply (this is analagous to transposing one of your rotation matrices). It isn't clear what the intended order of these rotations are, so you may have to adjust the code below, but just using 'XYZ' order for example gives:

% Using functions from the Aerospace Toolbox
hdga = deg2rad(50);
pitcha = deg2rad(50);
rolla = deg2rad(50);
hdgb = deg2rad(70);
pitchb = deg2rad(70);
rollb = deg2rad(70);
% Using quaternions
qa = eul2quat([rolla,pitcha,hdga],'XYZ');
qb = eul2quat([rollb,pitchb,hdgb],'XYZ');
qr = quatmultiply(qa,quatinv(qb));
if( qr(1) < 0 )
    qr = -qr;
end
angle_quat = 2*acosd(qr(1))

angle_quat = 43.4416

% Using rotation matrices
ma = eul2rotm([rolla,pitcha,hdga],'XYZ');
mb = eul2rotm([rollb,pitchb,hdgb],'XYZ');
mr = inv(mb) * ma;
q = rotm2quat(mr);
if( q(1) < 0 )
    q = -q;
end
angle_rotm = 2*acosd(q(1))

angle_rotm = 43.4416

If you don't have the Aerospace Toolbox, then you would have to write your own low level code for these conversions as you have done in your original post.

[Paul]

@James Tursa

Any particular reason to prefer

inv(mb) * ma  % (1)

instead of

mb \ ma  % (2)

if wanting to use the inverse,

or using the inverse property of rotation matrices?

mb.' * ma % (3)

In this particular instance it makes no difference, just asking if you prefer (1) over (2) or (3) for some technical or philosophical reason.

[James Tursa]

The only reason was to make it look similar to the quaternion code for comparison (you see the 'inv' in both). Nothing else. Of course all your points are correct, and I could have used conjugate for the quaternion and transpose for the rotation matrix and got the same result. And my code above assumes the quaternions and rotation matrices produced by the MATLAB functions are proper so no checks or adjustments are made. But if the quaternions or rotation matrices came from some other source one may want to perform checks.

[James Tursa]

And, after thinking more about this in terms of coordinate system transformations, switching which variables gets the inv( ) might make more sense to keep the coordinate systems consistent. E.g., supposing intertial and body frames:

% Using functions from the Aerospace Toolbox
hdga = deg2rad(50);
pitcha = deg2rad(50);
rolla = deg2rad(50);
hdgb = deg2rad(70);
pitchb = deg2rad(70);
rollb = deg2rad(70);
% Using quaternions
qa = eul2quat([rolla,pitcha,hdga],'XYZ'); % INERTIAL -> BODY1
qb = eul2quat([rollb,pitchb,hdgb],'XYZ'); % INERTIAL -> BODY2
qr = quatmultiply(quatconj(qa),qb); % B1->I * I->B2 = BODY1 -> BODY2
if( qr(1) < 0 )
    qr = -qr;
end
angle_quat = 2*acosd(qr(1))

angle_quat = 43.4416

angle_quat = 43.4416

angle_quat = 43.4416

% Using rotation matrices
ma = eul2rotm([rolla,pitcha,hdga],'XYZ'); % INERTIAL -> BODY1
mb = eul2rotm([rollb,pitchb,hdgb],'XYZ'); % INERTIAL -> BODY2
mr = mb * ma'; % I->B2 * B1->I = BODY1 -> BODY2
q = rotm2quat(mr);
if( q(1) < 0 )
    q = -q;
end
angle_rotm = 2*acosd(q(1))

angle_rotm = 43.4416

[Paul]

I understand the sentiment, but eul2rotm is defined such that its output is premultiplied by a row vector. So if we a have a row vector resolved in inertial coordinates, that same vector resolved in body1 and body2 coordinates would be

xrow_body1 = xrow_inertial*ma

xrow_body2 = xrow_inertial*mb

Combining

xrow_body2 = xrow_body1*tanspose(ma)*mb

which was the original solution.

If using the Aerospace toolbox function angle2dcm it would go the other way because that function returns the DCM to post-multiply by a column vector resolved in inertial coordinates to get that same vector resolved in body coordinates.

[James Tursa]

@Paul Yikes! I rarely use Euler Angles myself so I missed that in the doc. It's hard to keep up with the different conventions in the Aerospace, Navigation, and Robotics toolboxes, and the generic quaternion( ) class. To your point ...

r = deg2rad(20); p = deg2rad(40); y = deg2rad(60); eul = [r p y];
eul2rotm(eul,'XYZ')

ans = 3×3

0.3830 -0.6634 0.6428 0.9237 0.2795 -0.2620 -0.0058 0.6941 0.7198

angle2dcm(r,p,y,'XYZ')

ans = 3×3

0.3830 0.9237 -0.0058 -0.6634 0.2795 0.6941 0.6428 -0.2620 0.7198

Answer 2

[David Goodmanson]

Hello Jonathan,

You have two 3x3 rotation matrices, each expressed in terms of euler angles,

Rrpha(:,1:3) = [cp.*ch,  -sp,  cp.*sh];
Rrpha(:,4:6) = [cr.*sp.*ch + -sr.*-sh,  cr.*cp,  cr.*sp.*sh + -sr.*ch];
Rrpha(:,7:9) = [sr.*sp.*ch + cr.*-sh,  sr.*cp,  sr.*sp.*sh + cr.*ch];

and similarly for Rrphb. I will call these A and B for short. Ideally these two matrices equal each other, so

inv(B)*A = B\A = I, and ideally (B\A -I) = 0 

In actuality, D = (B\A-I) is not zero, and

D = B\A -eye(3);
then E = sum(D.*D,'all')    

is a reasonable measure of the difference between A and B. You can express that quantity algebraically in terms of a bunch of euler angles for A and B, but since you are looking for a numerical result it seems easiest to just calculate A and B with the known euler angles and then find E.

If you just need a measure of the difference between A and B, and you don't need the rotation matrix B\A in terms of more euler angles, then I don't see the need to convert A or B to quaternions. If you do need B\A in terms of angles, quaternions are a good way to go, but for getting B\A in terms of euler angles one way is by the code down at the end.

Every 3d rotation can be expressed in terms of rotation by an angle theta about a particular unit vector w in 3d space. It can be shown that the quantity E is simply

E = 8*sin(theta/2)^2

regardless of the direction of w. So calculation of E can give you the single euler-type rotation angle, within a sign. The smaller the angle, the closer A is to B.

The quantity E has a geometric interpretation. Let

D = [a b c
     d e f
     g h k]        

If ux is the unit vector in the x direction, ux = [1 0 0]'

then D*ux = [a d g]'             

and the squared distance between ux and D*ux is (a-1)^2 + d^2 + g^2 which is three of the terms making up E. Similarly for uy and uz so overall you get the sum of the squared distances between what you have and what you would like to get.

You can find w and theta, within a sign, by looking at

[v lam] = eig(B\A)

As long as B\A is not the identity, you should get one eigenvalue = 1 and two eigenalues that are complex conjugates on the unit circle, exp(i*theta) and exp(-i*theta). Theta is the rotation angle, same one as determined by E.

The column of v that corresponds to eigenvalue 1 is the rotation axis w. [ eig probably produces a vector w that has real components, but there is no guarantee of that. w might possibly be multiplied by an overall phase constant exp(i*phi). To get rid of it, you can do

[~,ind] = max(abs(w))
phi = angle(w(ind))
w = real(w*exp(-i*phi)) ]

There are two eigenvectors corresponding to the eigenvalues exp(+-i*theta), and those eigenvectors are complex conjugates of each other. Taking one of them and denoting it by v1, then let

w1 = real(v1), w2 = imag(v1)

and w, w1, w2 are a real orthonormal set of vectors. The rotation is about w, in the w1,w2 plane,

To express B\A in terms of euler angles:

function theta = anglesR(R,str)

% solve a 3d rotation matrix R in the form Ra(th1)*Rb(th2)*Rc(th3)
% for angles th1,th2,th3.
% str is a three-letter input string such as 'xyz'
% that provides the rotation axes a,b,c respectively.
% consecutive rotations along the same axis such as 'xxy' are not allowed.
% 1st and 3rd rotations along different axes are tait-bryan,
% along the same axis are euler.  3x2x2 = 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) 
%
% theta = anglesR(R,str)

% David Goodmanson

theta = zeros(1,3);
% useful 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;
% transform to RdRyRe
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') % transformed str is 'zyx'
    R = By*R/By;
    signy = -1;
end
% euler, transform to RxRyRx 
if all(str=='xzx')|all(str=='yxy')|all(str=='zyz') % transformed str is 'zyz'
    R = Ry90*R/Ry90;
end  
if str(1)~=str(3)                 % tait-bryan RxRyRz  -90 <= th2 <= 90
    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 RxRyRx    0 <= th2 <= 180
    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.
% ccw rotation looking down at rotation axis coming up out of 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       (elements represented by . are more complicated)
%  [c2c3  -c2s3  s2
%    .      .   -s1c2
%    .      .    c1c2]
% 
% euler RxRyRx
%  [c2    s2s3  s2c3
%   s1s2   .     .
%  -c1s2   .     .  ]

Answer 3

[Bruno Luong]

I recall a long discussion with Roger, James on this topic on the newsgroup long time ago.

At the time our conclusion is quaternion is the best, thank you James. The EIG method as David's solution is not accurate numerically in some extrem cases.

Since then I have tested several other methods of computing the rotation vector (rotation angle * unit vector of a rotation) and the Cayley method beats even the quaternion in term of robustness.

Using James's example

% Using functions from the Aerospace Toolbox
hdga = deg2rad(50);
pitcha = deg2rad(50);
rolla = deg2rad(50);
hdgb = deg2rad(70);
pitchb = deg2rad(70);
rollb = deg2rad(70);

% Using rotation matrices
Q1 = eul2rotm([rolla,pitcha,hdga],'XYZ');
Q2 = eul2rotm([rollb,pitchb,hdgb],'XYZ');

For two frames Q1 and Q2, call

[theta, u] = CayleyMethod(Q1*Q2');
rad2deg(theta)

ans = 43.4416

will give theta the rotation angle (in radian); one can consider abs(theta) as the "distance" between Q1 and Q2, since Q1 can obtained by rotation of Q2 about u of +/-abs(theta). One can prove this is most economical rotation to map Q1 to Q2 (and vice versa) since abs(theta) is minimal.

%%
function [theta, u] = CayleyMethod(A)
% Ref: "A Survey on the Computation of Quaternions from Rotation Matrices",
% Soheil Sarabandi, Federico Thomas
% J. Mechanisms Robotics. Apr 2019
p = size(A,3);
R = reshape(A,9,p);
C = [R(6,:)-R(8,:);
    R(7,:)-R(3,:);
    R(2,:)-R(4,:)];
D = [R(6,:)+R(8,:);
    R(7,:)+R(3,:);
    R(2,:)+R(4,:)];
C2      = C.^2;
D2      = D.^2;
E0      = (R(1,:)+R(5,:)+R(9,:)+1).^2 + C2(1,:) + C2(2,:) + C2(3,:);
Exyz2   = [ ...
    C2(1,:) + D2(2,:) + D2(3,:) + (R(1,:)-R(5,:)-R(9,:)+1).^2;
    C2(2,:) + D2(3,:) + D2(1,:) + (R(5,:)-R(9,:)-R(1,:)+1).^2;
    C2(3,:) + D2(1,:) + D2(2,:) + (R(9,:)-R(1,:)-R(5,:)+1).^2 
    ];
c           = sqrt(E0);
s           = sqrt(sum(Exyz2,1));
theta       = 2*atan2(s, c);
u           = sign(C).*sqrt(Exyz2)./s;
isId        = s == 0;
nId         = sum(isId,2);
u(:,isId)   = repmat([1; 0; 0], 1, nId);
theta(isId) = 0;
end