Fix numerical precision issues in quaternion to RPY conversion #811
+146
−21
Conversation
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Addresses numerical precision errors that occur during quaternion-to-matrix conversion, particularly near gimbal lock singularities. When matrix elements contain small numerical errors (around 1e-12), the atan2 function can return incorrect angles, such as 45° instead of 0° for roll values. Changes: - Add epsilon thresholding (1e-10) to matrix elements before RPY computation - Improve singularity detection to handle near-singular cases - Add safety checks to prevent division by near-zero cosine values - Protect atan2 calls when both arguments are very small - Apply fixes to both Matrix3x3::getEulerYPR() and tf2::impl utilities - Update .gitignore with comprehensive build and temporary file patterns This resolves issues where tf2_echo and other tf2 tools would report incorrect RPY angles due to floating-point precision limitations.
Changes the epsilon threshold from 1e-10 to 1e-8 to be more conservative and avoid interfering with legitimate small values in normal quaternion conversions. The 1e-8 threshold still effectively catches the numerical precision errors that cause incorrect RPY results while ensuring existing tests continue to pass. Analysis showed that quaternion-to-matrix conversion preserves error magnitude, so a threshold of 1e-8 provides adequate safety margin above typical floating-point precision errors without affecting normal operations.
- Reorder headers to follow cpplint convention (C++ system headers first) - Remove trailing whitespace from empty lines - Addresses cpplint and uncrustify test failures
ahcorde
requested changes
Jul 28, 2025
Signed-off-by: Roman Wu <[email protected]>
mjcarroll
reviewed
Aug 8, 2025
mjcarroll
requested changes
Aug 12, 2025
Signed-off-by: Roman Wu <[email protected]>
ahcorde
requested changes
Aug 18, 2025
Signed-off-by: Alejandro Hernández Cordero <[email protected]>
Signed-off-by: Alejandro Hernandez Cordero <[email protected]>
ahcorde
approved these changes
Aug 18, 2025
|
Pulls: #811
|
ahcorde
requested changes
Aug 19, 2025
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Description
Fixes #796
Addresses numerical precision errors that occur during quaternion-to-matrix conversion, particularly near gimbal lock singularities. When matrix elements contain small numerical errors (around 1e-12), the atan2 function can return incorrect angles, such as 45° instead of 0° for roll values.
Changes:
This resolves issues where tf2_echo and other tf2 tools would report incorrect RPY angles due to floating-point precision limitations.
Is this user-facing behavior change?
precision issues
- Before: RPY (degree) [45.000, -90.000, 0.000] (incorrect)
- After: RPY (degree) [0.000, -90.000, 0.000] (correct)
- Near gimbal lock singularities
- When quaternions come from tf2 lookupTransform operations
- When dealing with transformations that should have zero roll/yaw but show small numerical errors
Did you use Generative AI?
No.
Additional Information