From e9470ec26d964c31af61619cc57c6a9f10fe3149 Mon Sep 17 00:00:00 2001
From: Jean-Yves TINEVEZ <jean-yves.tinevez@pasteur.fr>
Date: Fri, 3 Jul 2020 16:37:37 +0200
Subject: [PATCH] Finally found a good way to go to meaningful Euler angles
from rotation matrix.
and back.
---
src/euleurZXZ2rot.m | 36 ++++++++++++++++++++++++++++++++++++
src/fit_ellipse.m | 22 +---------------------
src/rot2eulerZXZ.m | 30 ++++++++++++++++++++++++++++++
3 files changed, 67 insertions(+), 21 deletions(-)
create mode 100644 src/euleurZXZ2rot.m
create mode 100644 src/rot2eulerZXZ.m
diff --git a/src/euleurZXZ2rot.m b/src/euleurZXZ2rot.m
new file mode 100644
index 0000000..58ea885
--- /dev/null
+++ b/src/euleurZXZ2rot.m
@@ -0,0 +1,36 @@
+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
diff --git a/src/fit_ellipse.m b/src/fit_ellipse.m
index 0770443..2922dcc 100644
--- a/src/fit_ellipse.m
+++ b/src/fit_ellipse.m
@@ -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.
diff --git a/src/rot2eulerZXZ.m b/src/rot2eulerZXZ.m
new file mode 100644
index 0000000..7ef5820
--- /dev/null
+++ b/src/rot2eulerZXZ.m
@@ -0,0 +1,30 @@
+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
+
--
GitLab