8

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 = orientation.toRotationMatrix().eulerAngles(0,1,2);
    const double r = ((double)rpy(0));
    const double p = ((double)rpy(1));
    const double y = ((double)rpy(2));

Is that correct? Because I was reading here: http://eigen.tuxfamily.org/dox/group__Geometry__Module.html#gad118fececd448d7485ffea4858775e5a

And I was a bit confused when it says, at the end of the description, in which intervals are defined the angles.

desmond13
  • 2,106
  • 2
  • 21
  • 30

2 Answers2

12

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!

Michael Xu
  • 3
  • 3
TSL_
  • 1,879
  • 3
  • 32
  • 53
5

The answer by Shawn Le is correct but I think the line should be 

Vector3f ea = m.eulerAngles(2, 1, 0);

Then ea will hold the yaw pitch and roll value in that order. ZYX euler angle rotation is equivalent to XYZ fixed axis rotation which is nothing but roll pitch and yaw.

Rachit Bhargava
  • 153
  • 2
  • 10
  • 1
    Perhaps this could be explained better, because if that line is changed, then recalculating the rotation matrix yields a rotation matrix different from the originally calculated one. Unless the order order of the unit matrices is changed, I think that "m.eulerAngles(0, 1, 2);" is correct. – RStiltskin Jun 04 '19 at 18:23
  • No I'm quite sure it should be (2,1,0) the matrix multiplication above is not a conventional roll pitch yaw. The rotation should be R_yaw * R_pitch * R_roll not the other way around. – Rachit Bhargava Sep 20 '19 at 17:12