views:

505

answers:

1

I'm trying to convert a 3D rotation described in term of euler angles into a matrix and then back, using .NET/C#. My conventions are:

  • left handed system (x right, y top, z forward)
  • order of rotations: heading around y, pitch around x, bank around z
  • rotations are positive using the left hand rule (thumb pointing to +infinity)

My trial is:

Euler to matrix (I've removed the x,y,z translation part for simplification)

Matrix3D matrix = new Matrix3D() {
    M11 =   cosH * cosB - sinH * sinP * sinB,
    M12 = - sinB * cosP,
    M13 =   sinH * cosB + cosH * sinP * sinB,
    M21 =   cosH * sinB + sinH * sinP * cosB,
    M22 =   cosB * cosP,
    M23 =   sinB * sinH - cosH * sinP * cosB,
    M31 = - sinH * cosP,
    M32 = - sinP,
    M33 =   cosH * cosP,
};

Matrix to Euler

const double RD_TO_DEG = 180 / Math.PI;            
double h, p, b; // angles in degrees

// extract pitch
double sinP = -matrix.M23;            
if (sinP >= 1) {
    p = 90; }       // pole
else if (sinP <= -1) {
    p = -90; } // pole
else {
    p = Math.Asin(sinP) * RD_TO_DEG; }             

// extract heading and bank
if (sinP < -0.9999 || sinP > 0.9999) { // account for small angle errors
    h = Math.Atan2(-matrix.M31, matrix.M11) * RD_TO_DEG;
    b = 0; }
else {
    h = Math.Atan2(matrix.M13, matrix.M33) * RD_TO_DEG;
    b = Math.Atan2(matrix.M21, matrix.M22) * RD_TO_DEG; }

It must be wrong. If I take 3 angles, convert them into a matrix and convert the matrix back into angles, the result if different than the intial values.

I have browsed several sites with different formulas, starting with euclideanspace.com, but I'm now completely lost, and can't find the right computations. I' appreciate a little help. Is there a mathematician onboard?

+1  A: 

Firstly, should:

sinP = -matrix.M32

EDIT: Full solution follows

My derivation:

Rx(P)=| 1      0       0 |
      | 0  cos P  -sin P |
      | 0  sin P   cos P |

Ry(H)=|  cos H  0  sin H |
      |      0  1      0 |
      | -sin H  0  cos H |

Rz(B)=| cos B  -sin B  0 |
      | sin B   cos B  0 |
      |     0       0  1 |

Multiplied with your ordering:

R = Ry(H)*Rx(P)*Rz(B)
  = | cos H*cos B+sin H*sin P*sin B  cos B*sin H*sin P-sin B*cos H  cos P*sin H |
    |                   cos P*sin B                    cos B*cos P       -sin P |
    | sin B*cos H*sin P-sin H*cos B  sin H*sin B+cos B*cos H*sin P  cos P*cos H |

Which gives reverse derivations:

tan B = M12/M22

sin P = -M32

tan H = M31/M33

fd
I tried, but still not working. Currently in the process euler->matrix->euler, if I use a single angle in HPB (say h,0,0), then the sign is change in the result (-h,0,0). I wonder if I could debug each conversion separately, for instance is there a page on the net with examples of conversion and the value of the matrix elements. I found some, but not in a left handed system / left handed rotations.
Mike
I derived the original rotation matrix and I did come up with a different answer tbh, I assumed my maths was wrong (or just equivalent) though:| cosH*cosB+sinH*sinP*sinB | cosB*sinH*sinP-sinB*cosH | cosP*sinH || cosP*sinB | cosB*cosP | -sinP || sinB*cosH*sinP-sinH*cosB | sinH*sinB+cosB*cosH*sinP | cosP*cosH |
fd
From the matrix I have in my above comment, the reversal follows as:tan B = M12/M22, sin P = -M32, tan H = M31/M33
fd
Yes, that's perfect! I'm not a mathematician, though I did understand the principles, but this is definitely not easy for me to sort out the different handeness and variations and reuse code not written for those I use. But you did come within a couple of minites with the exact answer that needed only a copy-paste on my side. Thanks a lot sir! I'm sincerely grateful for that.
Mike
I've redone my answer to include the above commentary. Glad it both helped and was correct (been a few years since my Maths degree) :)
fd
Sorry to bother again... could you help me in switching from atan(M12/M22) to atan2(?, ?). This is a general question about obtaining a canonic form, but something I've never used. atan2 Wikipedia article is not easy to read for me.
Mike
Should just be B = atan2(M12,M22) and H = atan2(M31,M33). Which should give you a better answer for the angles than atan.
fd
When I said your solution worked, I was using your matrix but still my unmodified routine for matrix->euler. I changed to use your formulas for this conversion. It stopped working. I see your matrix is the transpose of my faulty matrix. However, it seems your formulas are wrong (pardon me!) and the ones I used are ok. M13/M33 = tan H, M21/M22 = tan B, and -M23 = sin P (with your matrix). OTH, the code when looking at poles in my solution is odd. -M31/M11 = 1 / cos H when B==0, not tan H as I would have expected (not sure).
Mike
Sounds like maybe we're using different numbering systems for the rows/columns. I was using M{col}{row}, so M31=cos P*sin H.
fd
Correct, I use .NET Matrix3D which has properties for members numbered "M row col". When I borrowed my code, it seems I picked one conversion right, and one wrong. I didn't noticed first because despite the error the code seemed to worked with the test data I had (as I said the sign was reversed, but in my chain it was reversed an even number of time).
Mike