tf2::Matrix3x3::getEulerYPR issue

[DobbyPeng]

In my case, I used the getEulerYPR() function to get the RPY from the quaternion like marcoarruda did. When the pitch is -90 degrees, the value of roll is weird. So I read the source code and found something in the link below.

https://github.com/ros/geometry2/blob/c73b5939723db078c9bbe18523230ad54f859682/tf2/include/tf2/LinearMath/Matrix3x3.h#L315

When the pitch is 90 or -90 degrees and the yaw is set to 0 degrees, the roll should be tf2Atan2(-m_el[0].y(), -m_el[0].z()). Because m_el[2].y() and m_el[2].z() are zero.

[DobbyPeng]

I think it should be modified to the following.

// From difference of angles formula
if (m[2].x() < 0)  //gimbal locked down
{
  tf2Scalar delta = tf2Atan2(m[0].y(), m[0].z());
  euler_out.pitch = TF2SIMD_PI / tf2Scalar(2.0);
  euler_out2.pitch = TF2SIMD_PI / tf2Scalar(2.0);
  euler_out.yaw = delta;
  euler_out2.yaw = delta;
}
else // gimbal locked up
{
  tf2Scalar delta = tf2Atan2(-m[0].y(), -m[0].z());
  euler_out.pitch = -TF2SIMD_PI / tf2Scalar(2.0);
  euler_out2.pitch = -TF2SIMD_PI / tf2Scalar(2.0);
  euler_out.yaw = delta;
  euler_out2.yaw = delta;
}

[Naoki-Hiraoka]

I faced the same trouble.

#include <tf2/LinearMath/Matrix3x3.h>
#include <iostream>

int main(void){
  tf2::Matrix3x3 m(0.0, -1.0, 0.0,
                   0.0, 0.0, -1.0,
                   1.0, 0.0, 0.0);
  double r, p, y;
  m.getEulerYPR(y,p,r);
  std::cout << r << std::endl; // 0.0 (wrong. expected 1.57035)
  std::cout << p << std::endl; // -1.57035
  std::cout << y << std::endl; // 0.0
}

As the author of this issue pointed out, I think the cause of this is this line. https://github.com/ros/geometry2/blob/c73b5939723db078c9bbe18523230ad54f859682/tf2/include/tf2/LinearMath/Matrix3x3.h#L315

[tfoote]

This is a fork of the Bullet Physics linear math library and we generally do not want to diverge from their behavior.

The latest version is here: https://github.com/bulletphysics/bullet3/blob/f0f2a952e146f016096db6f85cf0c44ed75b0b9a/src/LinearMath/btMatrix3x3.h#L526

It appears to use the same math still. If they accept a change we can consider backporting it, but otherwise our policy is to stick with what Bullet's implementation does instead of trying to second guess it.

In general I recommend using your own linear math library not the built in one here. The interface is exposed but was mostly designed for internal use.

[Naoki-Hiraoka]

Thank you for your response.

I am using Eigen instead of tf2::Matrix3x3::getEulerYPR now.

Eigen::Matrix3d m = ...;
Eigen::Vector3d ypr = m.eulerAngles(2,1,0);