Getting Eigen3 Matrix3f::eulerAngles() values similar to tf2 Matrix3x3f::getRPY() roll, pitch, yaw
I'm extracting euler angles from a Matrix3x3 based off a quaternion, but am having trouble with getting euler from Eigen that has the same behaviour as tf2::Matrix3x3::getRPY().
Using the following code:
Eigen::Quaternionf acOrient(0.991, -0.019, 0.037, 0.127);
Eigen::Vector3f acEuler = Eigen::Matrix3f(acOrient).eulerAngles(0,1,2);
tf2::Quaternion q( -0.019, 0.037, 0.127, 0.991);
tf2::Matrix3x3 m(q);
double r, p, y;
m.getRPY(r, p, y);
ROS_INFO("AC Y %.3f ACtf %.3f",angles.yaw, acEuler.z(), y );
And the Output is:
[ INFO] [1557815455.909538774]: AC Y -2.885 ACtf 0.254
These should the be same, my question is what modifications do I need to do to the euler extraction from Eigen to achieve this, ie is it a matter of different axis ordering etc.
Thanks.
Asked by PeterMilani on 2019-05-14 01:52:08 UTC
Answers
I believe you're looking for the second solution (from getRPY):
@param solution_number Which solution of two possible solutions ( 1 or 2) are possible values
Eigen::Quaternionf acOrient(0.991, -0.019, 0.037, 0.127);
Eigen::Vector3f acEuler = Eigen::Matrix3f(acOrient).eulerAngles(0,1,2);
tf2::Quaternion q( -0.019, 0.037, 0.127, 0.991);
tf2::Matrix3x3 m(q);
double r, p, y;
m.getRPY(r, p, y, 2);
// ^
ROS_INFO("AC Y % .3f ACtf % .3f", /*angles.yaw,*/ acEuler.z(), y ); // AC Y -2.885 ACtf -2.888
ROS_INFO("AC P % .3f ACtf % .3f", /*angles.yaw,*/ acEuler.y(), p ); // AC P 3.073 ACtf 3.063
ROS_INFO("AC R % .3f ACtf % .3f", /*angles.yaw,*/ acEuler.x(), r ); // AC R 3.094 ACtf 3.113
You had the multiples of 360° with configuration 1.
Asked by aPonza on 2019-05-15 03:21:48 UTC
Comments
Hi @aPonza, thanks, but I'm happy with the output from tf2::Matrix3x3::getRPY. I want to know the config to get an output from Eigen that is the same. I would prefer to use Eigen over the tf2 libraries for linear geometric transforms. Unfortunately this operation is preventing that.
Asked by PeterMilani on 2019-05-15 19:54:26 UTC
Yeah, I inverted the idea, you're right. As you saw from your comment link the conversion you want should be there with #include <unsupported/Eigen/EulerAngles>
but I can't use it without a source install (?). If that doesn't work or is too much trouble I'd use the same conversion that's applied in the body of the tf2::getEulerYPR
function.
Asked by aPonza on 2019-05-16 06:35:53 UTC
Thanks, I've been following this up on eigen bugzilla. I dont really like the Eigen/EulerAngles setup as it creates a new class for euler angles which are indeterminate unless the axes of rotation is given. I'd much prefer the use of quaternions or Matrix3f holding the rotation info, an then extracted with perhaps some extra specification as to the output angle ranges. This is the main problem is that the first angle output from eulerAngles() is limited to [0, pi] which doesnt' work for yaw. At this stage I'm going to just keep using the tf2 transformations for extracting the euler angles.
Asked by PeterMilani on 2019-05-22 18:46:35 UTC
Comments
This behaviour is similar to bug reported on eigen http://eigen.tuxfamily.org/bz/show_bug.cgi?id=947. As a feature request to switch to tait-bryan angles which apparently start in range [-pi: pi]. currently the eigen implementation works in [0:pi]. This gives errors if assuming a ZYX rotation order as the output yaw is [0:pi]
Asked by PeterMilani on 2019-05-15 20:09:41 UTC