Fix numerical precision issues in quaternion to RPY conversion

test_tf2/test/test_utils.cpp

@@ -13,12 +13,14 @@
 // limitations under the License.
 
 #include <gtest/gtest.h>
+#include <cmath>
 
 #include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/quaternion_stamped.hpp>
 #include <geometry_msgs/msg/transform.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <tf2/LinearMath/Quaternion.hpp>
+#include <tf2/LinearMath/Scalar.hpp>
 #include <tf2/utils.hpp>
 #include <tf2_kdl/tf2_kdl.hpp>
 
@@ -74,6 +76,50 @@ TEST(tf2Utils, yaw)
   }
 }
 
+TEST(tf2Utils, singularityGimbalLock)
+{
+  double yaw, pitch, roll;
+  double test_epsilon = 1e-6;
+
+  // Test gimbal lock at positive 90 degrees pitch
+  // This corresponds to quaternion with specific values that produce sarg ≈ 1.0
+  tf2::Quaternion q_pos_gimbal;
+  q_pos_gimbal.setRPY(0.1, TF2SIMD_HALF_PI, 0.2);  // Roll=0.1, Pitch=90°, Yaw=0.2
+
+  tf2::getEulerYPR(q_pos_gimbal, yaw, pitch, roll);
+  EXPECT_NEAR(pitch, TF2SIMD_HALF_PI, test_epsilon);
+  // At gimbal lock, roll and yaw combine - exact values depend on implementation
+  EXPECT_TRUE(std::isfinite(yaw) && std::isfinite(roll));
+
+  // Test gimbal lock at negative 90 degrees pitch  
+  tf2::Quaternion q_neg_gimbal;
+  q_neg_gimbal.setRPY(0.3, -TF2SIMD_HALF_PI, 0.4);  // Roll=0.3, Pitch=-90°, Yaw=0.4
+
+  tf2::getEulerYPR(q_neg_gimbal, yaw, pitch, roll);
+  EXPECT_NEAR(pitch, -TF2SIMD_HALF_PI, test_epsilon);
+  EXPECT_TRUE(std::isfinite(yaw) && std::isfinite(roll));
+
+  // Test near-gimbal lock cases (just under threshold)
+  tf2::Quaternion q_near_gimbal;
+  q_near_gimbal.setRPY(0.1, TF2SIMD_HALF_PI - 1e-7, 0.2);
+
+  tf2::getEulerYPR(q_near_gimbal, yaw, pitch, roll);
+  EXPECT_TRUE(std::isfinite(yaw) && std::isfinite(pitch) && std::isfinite(roll));
+  EXPECT_FALSE(std::isnan(yaw) || std::isnan(pitch) || std::isnan(roll));
+
+  // Test quaternions that should produce very small angles (near epsilon threshold)
+  tf2::Quaternion q_small_angles;
+  q_small_angles.setRPY(1e-9, 1e-9, 1e-9);  // Very small rotations
+
+  tf2::getEulerYPR(q_small_angles, yaw, pitch, roll);
+  EXPECT_NEAR(yaw, 0.0, test_epsilon);
+  EXPECT_NEAR(pitch, 0.0, test_epsilon);
+  EXPECT_NEAR(roll, 0.0, test_epsilon);
+
+  // Verify getYaw works correctly for these cases too
+  EXPECT_NEAR(tf2::getYaw(q_small_angles), 0.0, test_epsilon);
+}
+
 TEST(tf2Utils, identity)
 {
   geometry_msgs::msg::Transform t;

tf2/include/tf2/LinearMath/Matrix3x3.hpp

@@ -296,16 +296,28 @@ class Matrix3x3 {
 		Euler euler_out2; //second solution
 		//get the pointer to the raw data
 
-		// Check that pitch is not at a singularity
-  		// Check that pitch is not at a singularity
-		if (tf2Fabs(m_el[2].x()) >= 1)
+		// Apply epsilon thresholding to matrix elements to handle numerical precision issues.
+		// Use a conservative threshold to handle numerical errors from quaternion-to-matrix conversion.
+		// 1e-8 is chosen as a balance between numerical stability and precision:
+		// - More conservative than FLT_EPSILON (~1.19e-7) to handle single-precision input
+		// - Less restrictive than DBL_EPSILON (~2.22e-16) to avoid false positives
+		// - Prevents numerical instabilities near gimbal lock singularities
+		tf2Scalar threshold = tf2Scalar(1e-8);
+		tf2Scalar m20 = tf2Fabs(m_el[2].x()) < threshold ? tf2Scalar(0.0) : m_el[2].x();
+		tf2Scalar m21 = tf2Fabs(m_el[2].y()) < threshold ? tf2Scalar(0.0) : m_el[2].y();
+		tf2Scalar m22 = tf2Fabs(m_el[2].z()) < threshold ? tf2Scalar(0.0) : m_el[2].z();
+		tf2Scalar m10 = tf2Fabs(m_el[1].x()) < threshold ? tf2Scalar(0.0) : m_el[1].x();
+		tf2Scalar m00 = tf2Fabs(m_el[0].x()) < threshold ? tf2Scalar(0.0) : m_el[0].x();
+
+		// Check that pitch is not at a singularity (improved detection)
+		if (tf2Fabs(m20) >= tf2Scalar(1.0) - threshold)
 		{
 			euler_out.yaw = 0;
 			euler_out2.yaw = 0;
 
 			// From difference of angles formula
-			tf2Scalar delta = tf2Atan2(m_el[2].y(),m_el[2].z());
-			if (m_el[2].x() < 0)  //gimbal locked down
+			tf2Scalar delta = tf2Atan2(m21, m22);
+			if (m20 < 0)  //gimbal locked down
 			{
 				euler_out.pitch = TF2SIMD_PI / tf2Scalar(2.0);
 				euler_out2.pitch = TF2SIMD_PI / tf2Scalar(2.0);
@@ -322,18 +334,36 @@ class Matrix3x3 {
 		}
 		else
 		{
-			euler_out.pitch = - tf2Asin(m_el[2].x());
+			euler_out.pitch = - tf2Asin(m20);
 			euler_out2.pitch = TF2SIMD_PI - euler_out.pitch;
 
-			euler_out.roll = tf2Atan2(m_el[2].y()/tf2Cos(euler_out.pitch), 
-				m_el[2].z()/tf2Cos(euler_out.pitch));
-			euler_out2.roll = tf2Atan2(m_el[2].y()/tf2Cos(euler_out2.pitch), 
-				m_el[2].z()/tf2Cos(euler_out2.pitch));
+			tf2Scalar cos_pitch1 = tf2Cos(euler_out.pitch);
+			tf2Scalar cos_pitch2 = tf2Cos(euler_out2.pitch);
 
-			euler_out.yaw = tf2Atan2(m_el[1].x()/tf2Cos(euler_out.pitch), 
-				m_el[0].x()/tf2Cos(euler_out.pitch));
-			euler_out2.yaw = tf2Atan2(m_el[1].x()/tf2Cos(euler_out2.pitch), 
-				m_el[0].x()/tf2Cos(euler_out2.pitch));
+			// Check for near-zero cosine values to avoid division by very small numbers
+			if (tf2Fabs(cos_pitch1) < threshold)
+			{
+				// Handle singularity case
+				euler_out.yaw = 0;
+				euler_out.roll = tf2Atan2(m21, m22);
+			}
+			else
+			{
+				euler_out.roll = tf2Atan2(m21 / cos_pitch1, m22 / cos_pitch1);
+				euler_out.yaw = tf2Atan2(m10 / cos_pitch1, m00 / cos_pitch1);
+			}
+
+			if (tf2Fabs(cos_pitch2) < threshold)
+			{
+				// Handle singularity case
+				euler_out2.yaw = 0;
+				euler_out2.roll = tf2Atan2(m21, m22);
+			}
+			else
+			{
+				euler_out2.roll = tf2Atan2(m21 / cos_pitch2, m22 / cos_pitch2);
+				euler_out2.yaw = tf2Atan2(m10 / cos_pitch2, m00 / cos_pitch2);
+			}
 		}
 
 		if (solution_number == 1)

tf2/include/tf2/impl/utils.hpp

@@ -15,6 +15,9 @@
 #ifndef TF2__IMPL__UTILS_HPP_
 #define TF2__IMPL__UTILS_HPP_
 
+#include <limits>
+#include <cmath>
+
 #include <tf2/convert.hpp>
 #include <tf2/transform_datatypes.hpp>
 #include <tf2/LinearMath/Quaternion.hpp>
@@ -102,6 +105,12 @@ inline
 void getEulerYPR(const tf2::Quaternion & q, double & yaw, double & pitch, double & roll)
 {
   const double pi_2 = 1.57079632679489661923;
+  // Use a conservative threshold to handle numerical errors from quaternion computations.
+  // 1e-8 is chosen as a balance between numerical stability and precision:
+  // - More conservative than FLT_EPSILON (~1.19e-7) to handle single-precision input
+  // - Less restrictive than DBL_EPSILON (~2.22e-16) to avoid false positives
+  // - Prevents numerical instabilities near gimbal lock singularities
+  const double epsilon = 1e-8;
   double sqw;
   double sqx;
   double sqy;
@@ -115,18 +124,40 @@ void getEulerYPR(const tf2::Quaternion & q, double & yaw, double & pitch, double
   // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
   // normalization added from urdfom_headers
   double sarg = -2 * (q.x() * q.z() - q.w() * q.y()) / (sqx + sqy + sqz + sqw);
-  if (sarg <= -0.99999) {
+
+  // Apply epsilon thresholding to handle numerical precision issues
+  double threshold_high = 0.99999 - epsilon;
+  double threshold_low = -0.99999 + epsilon;
+
+  if (sarg <= threshold_low) {
     pitch = -0.5 * pi_2;
     roll = 0;
     yaw = -2 * atan2(q.y(), q.x());
-  } else if (sarg >= 0.99999) {
+  } else if (sarg >= threshold_high) {
     pitch = 0.5 * pi_2;
     roll = 0;
     yaw = 2 * atan2(q.y(), q.x());
   } else {
     pitch = asin(sarg);
-    roll = atan2(2 * (q.y() * q.z() + q.w() * q.x()), sqw - sqx - sqy + sqz);
-    yaw = atan2(2 * (q.x() * q.y() + q.w() * q.z()), sqw + sqx - sqy - sqz);
+
+    // Apply epsilon thresholding to arguments before atan2 calls
+    double roll_y = 2 * (q.y() * q.z() + q.w() * q.x());
+    double roll_x = sqw - sqx - sqy + sqz;
+    double yaw_y = 2 * (q.x() * q.y() + q.w() * q.z());
+    double yaw_x = sqw + sqx - sqy - sqz;
+
+    // Zero out very small values to prevent atan2 from returning incorrect angles
+    if (std::abs(roll_y) < epsilon && std::abs(roll_x) < epsilon) {
+      roll = 0;
+    } else {
+      roll = atan2(roll_y, roll_x);
+    }
+
+    if (std::abs(yaw_y) < epsilon && std::abs(yaw_x) < epsilon) {
+      yaw = 0;
+    } else {
+      yaw = atan2(yaw_y, yaw_x);
+    }
   }
 }
 
@@ -140,6 +171,12 @@ inline
 double getYaw(const tf2::Quaternion & q)
 {
   double yaw;
+  // Use a conservative threshold to handle numerical errors from quaternion computations.
+  // 1e-8 is chosen as a balance between numerical stability and precision:
+  // - More conservative than FLT_EPSILON (~1.19e-7) to handle single-precision input
+  // - Less restrictive than DBL_EPSILON (~2.22e-16) to avoid false positives
+  // - Prevents numerical instabilities near gimbal lock singularities
+  const double epsilon = 1e-8;
 
   double sqw;
   double sqx;
@@ -155,12 +192,24 @@ double getYaw(const tf2::Quaternion & q)
   // normalization added from urdfom_headers
   double sarg = -2 * (q.x() * q.z() - q.w() * q.y()) / (sqx + sqy + sqz + sqw);
 
-  if (sarg <= -0.99999) {
+  // Apply epsilon thresholding to handle numerical precision issues
+  double threshold_high = 0.99999 - epsilon;
+  double threshold_low = -0.99999 + epsilon;
+
+  if (sarg <= threshold_low) {
     yaw = -2 * atan2(q.y(), q.x());
-  } else if (sarg >= 0.99999) {
+  } else if (sarg >= threshold_high) {
     yaw = 2 * atan2(q.y(), q.x());
   } else {
-    yaw = atan2(2 * (q.x() * q.y() + q.w() * q.z()), sqw + sqx - sqy - sqz);
+    double yaw_y = 2 * (q.x() * q.y() + q.w() * q.z());
+    double yaw_x = sqw + sqx - sqy - sqz;
+
+    // Zero out very small values to prevent atan2 from returning incorrect angles
+    if (std::abs(yaw_y) < epsilon && std::abs(yaw_x) < epsilon) {
+      yaw = 0;
+    } else {
+      yaw = atan2(yaw_y, yaw_x);
+    }
   }
   return yaw;
 }