Search code examples
c++scipyeigenquaternionseuler-angles

Calculating Euler angles from Rotation matrix in C++

I have this code in Python:

from scipy.spatial.transform import Rotation   

r =  Rotation.from_matrix(R)
angles = r.as_euler("zyx",degrees=True)
print(f'angles = {angles}')

Where R is 3x3 rotation matrix.

And I have this code in C++ which is supposed to do exactly the same thing as Python bit:

Eigen::Map<Eigen::Matrix<float, 3, 3, Eigen::RowMajor>> eigen_R(R.ptr<float>());

Eigen::Quaternionf q(eigen_R);
Eigen::Vector3f euler = q.toRotationMatrix().eulerAngles(0, 1, 2);

double angle_x = euler[0];
double angle_y = euler[1];
double angle_z = euler[2];

// Convert angles to degrees
//double deg_factor = 180.0 / M_PI;
double deg_factor = 1.0;

float half_circle = M_PI * deg_factor;
float yaw_dim = angle_y > 0 ? half_circle : -half_circle;

_headPose[0] = (angle_y * deg_factor);      // yaw
_headPose[1] = (angle_x * deg_factor);      // pitch
_headPose[2] = (angle_z * deg_factor);      // roll

The issue: if I run both Python and C++ codes with this R matrix:

R = [[ 0.99640366 -0.05234712  0.06662979]
 [ 0.05348801  0.99844889 -0.01545452]
 [-0.06571744  0.01896283  0.99765807]]

They give the same results which is

Yaw = 3.82043
Pitch = 0.887488
Roll = 3.00733

But if R matrix equals to

R = [[ 0.99321075 -0.08705603  0.07715995]
 [ 0.08656997  0.99619924  0.00962846]
 [-0.0777049  -0.00288336  0.99697223]]

Then Python code outputs:

yaw = 4.425338029132474, pitch = -0.5533284872549677, roll = 5.0092369943283375

When C++ outputs:

Yaw = 175.575
Pitch = 179.447
Roll = -174.991

In reality I am tracking the angle of a moving object and when there is a subtle change from (0, 0, 0) degrees point, angles in C++ become reversed for some reason. And in order to get the right angles I need to do something like:

_headPose[0] = yaw_dim - (angle_y * deg_factor);        // yaw
_headPose[1] = (angle_x * deg_factor) - half_circle;            // pitch
_headPose[2] = (angle_z * deg_factor) - half_circle;            // roll

My knowledge on angles are very rusty and I do not understand why such a subtle change result in completely differently oriented values. Please advise how to fix it

Solution

  • From the intended answer in Python (and the slightly ambiguous documentation on scipy) the sequence "zyx" appears to mean extrinsic rotations, first about z, then y, then x. The matrix for this would be Rx.Ry.Rz in terms of elemental matrices.

    "Extrinsic" (I prefer "active") rotations are rotations of the object, but keeping the coordinate frame fixed. "Intrinsic" (I prefer "passive") rotations are those where the object stays constant but the coordinate reference frame is rotated.

    The matrix product Rx.Ry.Rz is enter image description here

    From this you can pick out the angles one by one. Y (the rotation about the y-axis) comes from the isolated value for sin(Y) in the top corner. This should give a unique solution for Y in the range [-90,90] degrees. (This is where your code appears to be wrong). Then, provided cos(Y) is not zero (i.e. Y is not either -90 degrees or 90 degrees) you can get an unambiguous X from the bottom two elements of the last column, whilst you can get an unambiguous Z from the first two elements of the top row. If Y is +-90 degrees you get a nasty ambiguity called gimbal lock - in this case, solutions are not unique and you can only predict either X-Z or X+Z. (Your test cases do not fall into this pit.)

    The following code produces the intended answers for the two test cases. Note that I have given explicit rotations about the different axes, rather than your particular definition of "yaw", "pitch" and "roll" - I would never live it down amongst my engineering colleagues if I use your convention, which is definitely not what is used for a ship or an aeroplane.

    On a really pedantic note you are actually using "Tait-Bryan angles", rather than "Euler angles" - all your three axes of rotation are different.

    #include <iostream>
#include <vector>
#include <cmath>
using namespace std;

const double PI = 4.0 * atan( 1.0 ), RADTODEG = 180.0 / PI;

//=====================================================================

vector<double> getAngles( const string &seq, const vector<vector<double>> &R, bool asDegrees )
{
   const double EPS = 1.0e-6;
   double X{}, Y{}, Z{};

   if ( seq == "zyx" )                           // Active rotation; z-axis, followed by y-axis, followed by x-axis
   {                                             //    Matrix would be Rx.Ry.Rz
      Y = asin( R[0][2] );                       // Unique angle in [-pi/2,pi/2]

      if ( abs( abs( R[0][2] ) - 1.0 ) < EPS )   // Yuk! Gimbal lock. Infinite choice of X and Z
      {
         X = atan2( R[2][1], R[1][1] );          // One choice amongst many
         Z = 0.0;
      }
      else                                       // Unique solutions in (-pi,pi]
      {
         X = atan2( -R[1][2], R[2][2] );         // atan2 gives correct quadrant and unique solutions
         Z = atan2( -R[0][1], R[0][0] );
      }
   }
   else
   {
      cout << "This sequence of rotations not coded yet.\n";
   }
   
   if ( asDegrees ) { X *= RADTODEG;   Y *= RADTODEG;   Z *= RADTODEG; }
   
   return { X, Y, Z };
}

//=====================================================================

int main()
{
   vector<vector<double>> R1 = { {  0.99640366, -0.05234712,  0.06662979 },
                                 {  0.05348801,  0.99844889, -0.01545452 },
                                 { -0.06571744,  0.01896283,  0.99765807 } };

   vector<vector<double>> R2 = { {  0.99321075, -0.08705603,  0.07715995 },
                                 {  0.08656997,  0.99619924,  0.00962846 },
                                 {  -0.0777049, -0.00288336,  0.99697223 } };

   auto angles = getAngles( "zyx", R1, true );
   cout << "Case 1: in order:\n";
   cout << "Rotation about z-axis: " << angles[2] << '\n'
        << "Rotation about y-axis: " << angles[1] << '\n'
        << "Rotation about x-axis: " << angles[0] << '\n';

   angles = getAngles( "zyx", R2, true );
   cout << "\nCase 2: in order:\n";
   cout << "Rotation about z-axis: " << angles[2] << '\n'
        << "Rotation about y-axis: " << angles[1] << '\n'
        << "Rotation about x-axis: " << angles[0] << '\n';
}

    Output:

    Case 1: in order:
Rotation about z-axis: 3.00733
Rotation about y-axis: 3.82044
Rotation about x-axis: 0.887486

Case 2: in order:
Rotation about z-axis: 5.00924
Rotation about y-axis: 4.42534
Rotation about x-axis: -0.553328