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