Skip to content

Fix numerical precision issues in quaternion to RPY conversion #811

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 9 commits into
base: rolling
Choose a base branch
from
Open

Fix numerical precision issues in quaternion to RPY conversion #811

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
77aad90
Fix numerical precision issues in quaternion to RPY conversion
Jul 25, 2025
f65a5d2
Adjust numerical precision threshold to prevent test failures
Jul 25, 2025
dfddeba
Fix code style issues in utils.hpp
Jul 25, 2025
d553db2
Delete .gitignore
Jul 28, 2025
3697b41
Merge branch 'ros2:rolling' into rolling
Aug 11, 2025
6a8e975
Add tests for gimbal lock scenarios and improve numerical stability i…
Aug 11, 2025
e69faa4
Create .gitignore
Aug 12, 2025
e6fa0bd
Update .gitignore
ahcorde Aug 18, 2025
a9b147d
Restored gitignore
ahcorde Aug 18, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 0 additions & 3 deletions .gitignore
View file
Open in desktop

This file was deleted.

54 changes: 40 additions & 14 deletions tf2/include/tf2/LinearMath/Matrix3x3.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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)
Expand Down
55 changes: 48 additions & 7 deletions tf2/include/tf2/impl/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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;
Expand All @@ -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);
}
}
}

Expand All @@ -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;
Expand All @@ -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;
}
Expand Down