diff --git a/test_tf2/test/test_utils.cpp b/test_tf2/test/test_utils.cpp index c374de565..4b6e8be21 100644 --- a/test_tf2/test/test_utils.cpp +++ b/test_tf2/test/test_utils.cpp @@ -13,12 +13,14 @@ // limitations under the License. #include +#include #include #include #include #include #include +#include #include #include @@ -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; diff --git a/tf2/include/tf2/LinearMath/Matrix3x3.hpp b/tf2/include/tf2/LinearMath/Matrix3x3.hpp index 34de10554..17d17580a 100644 --- a/tf2/include/tf2/LinearMath/Matrix3x3.hpp +++ b/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) diff --git a/tf2/include/tf2/impl/utils.hpp b/tf2/include/tf2/impl/utils.hpp index 03f688b66..4a591f32d 100644 --- a/tf2/include/tf2/impl/utils.hpp +++ b/tf2/include/tf2/impl/utils.hpp @@ -15,6 +15,9 @@ #ifndef TF2__IMPL__UTILS_HPP_ #define TF2__IMPL__UTILS_HPP_ +#include +#include + #include #include #include @@ -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; }