# computing angles from combined rotation matrixes

I use the following code to build a rotation matrix

 function C = build3Drot(yaw,pitch,roll)

X = [[1,0,0];[0,cos(roll),-sin(roll)];[0,sin(roll),cos(roll)]]

Y = [[cos(pitch),0,sin(pitch)];[0,1,0];[-sin(pitch),0,cos(pitch)]]

Z = [[cos(yaw),-sin(yaw),0];[sin(yaw),cos(yaw),0];[0,0,1]]
C = Z * Y * X;


and the inverse function to get euleur angles given a rotation matrix

function [yaw,pitch,roll] = unbuild3DRot(X)
yaw = atan2(X(2,1),X(1,1));
pitch = atan(-X(3,1) / sqrt(X(3,2) * X(3,2) + X(3,3) * X(3,3)));
roll = atan2(X(3,2),X(3,3));


The two functions look to be correct, but I get very, very weird results when extracting angles from combined matrixes. For example

A= build3Drot(0.052041,0.663198,-0.014)
A =
0.7870   -0.0606    0.6140
0.0410    0.9981    0.0460
-0.6156   -0.0110    0.7880

B = build3Drot(0.085,0.737,0.049)
B =
0.7378   -0.0520    0.6730
0.0629    0.9980    0.0082
-0.6721    0.0363    0.7396
C = A * B
C =
0.1642   -0.0791    0.9833
0.0621    0.9956    0.0698
-0.9845    0.0496    0.1684

[y,p,r] = unbuild3Drot(C)
y =
0.3615
p =
1.3944
r =
0.2864


So I started from realtively small yaw and roll angles for both matrixes A and B and ended up with a final yaw of (0.3615 * 180 /pi) = 20.7 degrees and a roll of (0.2864 * 180 / pi) = 16.4 degrees. Is this normal ? It looks very strange to me, since initial angles in degrees for A were yaw = 2.9817 pitch = 37.9984 roll = -0.8021 and for B yaw = 4.8701 pitch = 42.2270 roll = 2.8075.

I also wonder why, if I chain/multiply several matrixes together at a certain point the pitch decreases. For example

[y,p,r]=unbuild3Drot(A * A * A)

[y,p,r]
=
3.1174    1.1478    2.9611

while
[y,p,r]=unbuild3Drot(A * A * A * A)
y,p,r]
=
-3.0863    0.4847    2.9904


The resulting pitch of A * A * A * A is smaller than that of A

I feel like I'm doing something horribly wrong...but can't understand what.. thanks.

-
A question before going into the details of this: Do you really have to do this? Rotations can be handled more gracefully using quaternions, unless there's some exterior requirement in your case that prevents that. – joriki Oct 4 '11 at 10:33
On the question itself: Why are you using such messy numbers for the example? It would be easier both for you and for us if you find an example with as few non-zero values as possible, with fewer decimal digits (ideally one each). Also, could you explain more why you think that this result may be wrong? – joriki Oct 4 '11 at 10:37
Numbers come out from a simple pano stitcher that I'm trying to code. The numbers look wrong to me because I can't see such enormous yawn and pitch in my input images. I also find strange the A * A * A * A example I just added in my question – tagomago Oct 4 '11 at 10:51
I don't understand what you mean by "numbers come out from". You seem to be quoting statements that you enter, building and "unbuilding" rotation matrices, and it seems you could also enter statements with more manageable numbers? What's forcing you to present us with this mess of digits? – joriki Oct 4 '11 at 11:01
Sorry if numbers looked too complicated. I quoted my statemets just becuase it was some simple matlab code. Here's a simple example. Suppose I create a rotation matrix R with angles (0,0.5,0). If I multiply R * R I expect to get a final pitch of 1.0 which is true, same goes for the pitch of R * R * R where I expect 1.5. But if I try to get the pitch from R * R * R *R I get yaw = 3.1416, pitch = 1.1416, roll = 3.1416 when I would have expected yaw = 0, pitch = 2.0, roll = 0 – tagomago Oct 4 '11 at 11:13

I can't speak to what's happening in the $A$, $B$, $C$ example, but you're right that the powers of $A$ point to an error in the pitch calculation. The problem is that by taking the square root you're losing the sign of the cosine of the pitch, and this is causing the angles to be reflected at $\pi/2$. Since you have the roll, you can use that to undo the roll without a square root:

roll = atan2(X(3,2),X(3,3));
pitch = atan(-X(3,1) / (X(3,2) * sin (roll) + X(3,3) * cos (roll)));


This should cause the pitch to cycle through to $-\pi/2$ instead of being reflected at $\pi/2$, which is correct, since the yaw and roll are jumping by $\pi$ at the same time. If you don't understand that equivalence, you might want to have a look at this: How can I find equivalent Euler angles?

Note that the division can cause problems if the denominator becomes zero; to avoid that, you could use the atan2 function as for the other two angles and discard the spurious extra information on the pitch that it yields.

But I can't bear to write this answer without repeating that if at all possible you should try to avoid all these complications by working with quaternions instead.

-
@jorik...thanks for your answer, after looking at the other post you linked and googling a bit, maybe all my problems come from the fact that Euler angles are not unique ? I wish I was better in maths... Would resorting to quaternions (I'm already writing some code to transform between matrixes and quaternions and to multiply quaternions) help me in getting rid of this problem ? Also could I then get "correct/unique" euleur angles from quaternions ? I have seen that one can get euler angles from a quaternions, but I somehow suspect, that I will fall again into the problem... – tagomago Oct 4 '11 at 17:47
@Alfio: No, your problems don't all come from the fact that Euler angles are not unique. The angles reflected at $\pi/2$ describe the wrong rotation; they're not equivalent to the correct angles. Equivalent angles would all yield the same rotation when you plug them into build3Drot; whereas these angles would lead to the wrong sign on the cosine. Yes, resorting to quaternions would help you get rid of this problem. Yes, you could then get correct Euler (not "euleur") angles from the quaternions (if you still need them). No, you couldn't get unique Euler angles, since there's no such thing. – joriki Oct 4 '11 at 23:50