Commit e9470ec2 by Jean-Yves TINEVEZ

### Finally found a good way to go to meaningful Euler angles from rotation matrix.

`and back.`
parent aea446a3
 function R = euleurZXZ2rot( E ) %% EULERZYZ2ROT Convert euler ZX'Z' angles to rotation matrix. % See https://www.geometrictools.com/Documentation/EulerAngles.pdf to have % a final answer to this madness. alpha = E( 1 ); beta = E( 2 ); gamma = E( 3 ); c1 = cos( alpha ); s1 = sin( alpha ); c2 = cos( beta ); s2 = sin( beta ); c3 = cos( gamma ); s3 = sin( gamma ); r11 = c1 * c3 - c2 * s1 * s3; r12 = - c1 * s3 - c2 * c3 * s1; r13 = s1 * s2; r21 = c3 * s1 + c1 * c2 * s3; r22 = c1 * c2 * c3 - s1 * s3; r23 = - c1 * s2; r31 = s2 * s3; r32 = c3 * s2; r33 = c2; R = [ r11 r12 r13 r21 r22 r23 r31 r32 r33 ]; end \ No newline at end of file
 ... ... @@ -12,7 +12,7 @@ function [ f, E ] = fit_ellipse( o ) % Fit a plane to these points. [ ~, ~, v ] = svd( p ); E = to_euler( v ); E = rot2eulerZXZ( v ); % Rotate the points into the principal axes frame. p = p * v; ... ... @@ -24,26 +24,6 @@ function [ f, E ] = fit_ellipse( o ) %% Subfunctions function E = to_euler( R ) if R( 1, 3 ) == 1 || R( 1, 3 ) == -1 E3 = 0; dlta = atan2(R(1,2),R(1,3)); if R(1,3) == -1 E2 = pi/2; E1 = E3 + dlta; else E2 = -pi/2; E1 = -E3 + dlta; end else E2 = - asin( R( 1, 3 ) ); E1 = atan2( R( 2, 3 ) / cos( E2 ), R( 3, 3 ) / cos( E2 ) ); E3 = atan2( R( 1, 2 ) / cos( E2 ), R( 1, 1 ) / cos( E2 ) ); end E = [ E1, E2, E3 ]; end function f = quadratic_to_cartesian( A ) % Equations taken from Wolfram website. ... ...
 function[E,E_deg]=rot2eulerZXZ(R) %ROT2EULERZXZ Convert rotation matrix to ZX'Z' Euler angles. r13 = R( 1, 3 ); r31 = R( 3, 1 ); r22 = R( 2, 2 ); r23 = R( 2, 3 ); r32 = R( 3, 2 ); r33 = R( 3, 3 ); if r22 < +1 if r22 > -1 thetaX = acos( r33 ); thetaZ0 = atan2( r13, -r23 ); thetaZ1 = atan2( r31, r32 ); else thetaX = pi; thetaZ0 = -atan2( -r12, r11 ); thetaZ1 = 0; end else thetaX = 0; thetaZ0 = atan2( -r12, r11 ); thetaZ1 = 0; end E = double( [ thetaZ0, thetaX, thetaZ1 ] ); E_deg = rad2deg( E ); end
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!