Roll pitch and yaw from Rotation matrix with Eigen Library

Question

I need to extract the roll pitch yaw angles from a rotation matrix and I want to be sure that what I do is correct.

    Eigen::Matrix< simFloat, 3, 1> rpy         


        

Answer 1

I think this is what you are looking for. Depending on how we use m.eulerAngles(0, 1, 2); Here's the code which get rotx, roty, rotz that is reconstructed with rotx*roty*rotz

Matrix3f m;

m = AngleAxisf(0.25*M_PI, Vector3f::UnitX())
  * AngleAxisf(0.5*M_PI, Vector3f::UnitY())
  * AngleAxisf(0.33*M_PI, Vector3f::UnitZ());

cout << "original rotation:" << endl;
cout << m << endl << endl;

Vector3f ea = m.eulerAngles(0, 1, 2); 
cout << "to Euler angles:" << endl;
cout << ea << endl << endl;

Matrix3f n;
n = AngleAxisf(ea[0], Vector3f::UnitX())
  * AngleAxisf(ea[1], Vector3f::UnitY())
  * AngleAxisf(ea[2], Vector3f::UnitZ()); 

cout << "recalc original rotation:" << endl;
cout << n << endl;

Thank you for your reference! I also firstly use Eigen. It's simply save a lot of work!