Fix numerical precision issues in quaternion to RPY conversion

.gitignore

@@ -1,3 +1,84 @@
-*~
+# Build directories
+build/
+install/
+log/
+devel/
+
+# CMake generated files
+CMakeCache.txt
+CMakeFiles/
+cmake_install.cmake
+CTestTestfile.cmake
+Makefile
+*.cmake
+
+# Compiled Object files
+*.o
+*.obj
+*.slo
+*.lo
+*.a
+*.la
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Compiled Python files
 *.pyc
-\#*
+*.pyo
+__pycache__/
+*.egg-info/
+dist/
+
+# Test results
+test_results/
+Testing/
+
+# IDE files
+.vscode/
+.idea/
+*.swp
+*.swo
+
+# OS generated files
+.DS_Store
+.DS_Store?
+._*
+.Spotlight-V100
+.Trashes
+ehthumbs.db
+Thumbs.db
+
+# Temporary files
+*.tmp
+*.bak
+*.log
+
+# ROS specific
+*.launch.xml
+*.bag
+.catkin_workspace
+
+# Coverage files
+*.gcno
+*.gcda
+*.gcov
+coverage.info
+coverage/
+
+# Test executables and temporary test files
+test_*
+*test_output*
+gtest_*.xml
+
+# Editor backup files
+*~
+*.orig
+*.rej
+\#*
+
+# Doxygen output
+html/
+latex/

tf2/include/tf2/LinearMath/Matrix3x3.hpp

@@ -296,16 +296,24 @@ 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
+		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 +330,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,8 @@ 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
+  const double epsilon = 1e-8;
   double sqw;
   double sqx;
   double sqy;
@@ -115,18 +120,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 +167,8 @@ inline
 double getYaw(const tf2::Quaternion & q)
 {
   double yaw;
+  // Use a conservative threshold to handle numerical errors from quaternion computations
+  const double epsilon = 1e-8;
 
   double sqw;
   double sqx;
@@ -155,12 +184,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;
 }