Added direct quaternion to euler transformation and solved issue #53

[evbernardes]

Brief description

I recently published an article about a direct formula for the conversion between a quaternion variable to Euler angles in any sequence, which can be read here (Open Access).

Compared to either having 12 separate formulas (or 24, taking into account both intrinsic and extrinsic rotations) or using the well known quaternion-to-matrix-to-euler method, this has 3 main advantages:

1. Numerically, it is up to 30 times faster than the previous quaternion-to-matrix-to-euler method (used both originally in SciPy and in transforms3d, for example).
2. It is a lot simpler to implement, debug and maintain than both methods.
3. It provides the simple formulas that can be used for theorerical work.

Because of points 1 and 2, it has already been merged into Scipy. Because point 3, it has also been merged into SymPy.

I truly believe it's the best possible way to do this conversion, so I'm trying to contribute it to as many open source projects I can!

Other comments

Apart from the execution time advantages, this also has other numerical advantages. The main one is that the only inverse trigonometric function used is arctan2 instead of arccos and asin, which is always preferred when both sin and cos are known (arccos, for example, can give much higher errors near 0).

It also fixes issue #53.

I left the other method (plus a correction as explained in #53) for testing purposes, but I'd argue that in the future this should be removed.

What is not implemented

I don't know exactly how to to use the axes checking functions already existing in transforms3d, so if someone helps me with that, we could also add that at the beginning!

[matthew-brett]

Thanks for this. Yes, in general, that sounds like a good idea. Could you say more about what you mean for the axes checking functions? Sorry to be slow - I haven't touched this code in a long time.

[evbernardes]

Thanks for this. Yes, in general, that sounds like a good idea. Could you say more about what you mean for the axes checking functions? Sorry to be slow - I haven't touched this code in a long time.

Thanks for your reply! It's a very silly thing at the beginning: the test to check if the input axes string is correct or not.

For example, 'rxyz' gets accepted, but 'rxxz' or 'rxym' should throw errors.

[matthew-brett]

I think that would just be a case of checking axes in _AXES2TUPLE .

[evbernardes]

I think that would just be a case of checking axes in _AXES2TUPLE .

Alright, just changed it! It just tries to access it to have a KeyError, instead of checking axes in _AXES2TUPLE and throwing something else, for consistency with the other method.

Also, sorry if the code looks too much like the one from SciPy, it's because I wrote both!

[kopytjuk]

@evbernardes did you take a look into the ROS Stack ROS used in robotics? Its tf2 library heavily uses quarternions, maybe you can include your method there?

[evbernardes]

@evbernardes did you take a look into the ROS Stack ROS used in robotics? Its tf2 library heavily uses quarternions, maybe you can include your method there?

Hey there! I didnt yet, but I am planing to! Are you familiar with where exactly it's implemented in the Ros stack?

[kopytjuk]

@evbernardes did you take a look into the ROS Stack ROS used in robotics? Its tf2 library heavily uses quarternions, maybe you can include your method there?

Hey there! I didnt yet, but I am planing to! Are you familiar with where exactly it's implemented in the Ros stack?

You transform a quarternion q to euler angles with (source):

tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);

The implementation is in Matrix3x3::getEulerYPR here: https://github.com/ros2/geometry2/blob/9139e0c266cfd579d55e2ef36b786fb334b15665/tf2/include/tf2/LinearMath/Matrix3x3.h#L286