Wednesday, December 4, 2013

Eigen rotation conversions

Eigen::Matrix4f pose;
pose = SOME_INPUT;

/* Euler representation */
Eigen::Vector3f Eangle = Eigen::Isometry3f(pose.rotation().eulerAngles(0,1,2);
Eangle = Eangle*(180.0f/M_PI);
float e0 = Eangle(0); float e1 = Eangle(1); float e2 = Eangle(2);

/* AngleAxis representation */
Eigen::Matrix3f rotation_mat;
rotation_mat = pose.topLeftCorner(3,3);
Eigen::AngleAxisf rotation_aa(rotation_mat);
float Aangle = rotation_aa.angle()*(180.0f/M_PI);
Eigen::Vector3f Aaxis = rotation_aa.axis();
float a0 = Aaxis(0); float a1 = Aaxis(1); float a2 = Aaxis(2);

No comments:

Post a Comment