[Python] Possible problem of precision in scipy Rotation

I am using a python script based on scipy for comparing 3x3 rotation matrices, more specifically to find the rotation angle(s) to go from one matrix (let's say "origin") to another matrix (let's say "target").

#!/usr/bin/env python

import math
import numpy as np
from scipy.spatial.transform import Rotation

from optparse import OptionParser

def compare_orientations(R1, R2, debug):
"""
Compares two orientations.

Computes:
- The relative rotation matrix.
- The rotation axis (a unit vector).
- The rotation angle in degrees.
"""
# Ensure inputs are valid rotation matrices (orthogonal with det=1)
if not np.allclose(np.linalg.det(R1), 1.0) or not np.allclose(R1 @ R1.T, np.eye(3)):
raise ValueError("R1 is not a valid rotation matrix.")
if not np.allclose(np.linalg.det(R2), 1.0) or not np.allclose(R2 @ R2.T, np.eye(3)):
raise ValueError("R2 is not a valid rotation matrix.")

# Calculate the relative rotation matrix.
# This matrix transforms the orientation of target into the orientation of orig.
R_relative = R1.T @ R2

# Convert the relative rotation matrix to axis-angle representation.
# This provides a more intuitive measure of the difference.
relative_rotation = Rotation.from_matrix(R_relative)

# Get the rotation axis and angle. The axis is a unit vector.
# The angle is in radians by default.
axis_angle = relative_rotation.as_rotvec()

# The magnitude of the rotation vector is the angle.
angle_rad = np.linalg.norm(axis_angle)

# # The direction of the rotation vector is the axis.
# if angle_rad == 0:
# axis = np.array([0, 0, 1]) # Arbitrary axis for no rotation
# else:
# axis = axis_angle / angle_rad
angle_deg = np.degrees(angle_rad)

if debug:
print(f"Rotation Matrix (orig):\n{R1}\n")
print(f"Rotation Matrix (target):\n{R2}\n")
print(f"Relative Rotation Matrix (target to orig):\n{R_relative}\n")
print(f"Relative Rotation Angle:\n{angle_deg}\n")

return angle_deg # R_relative, axis, angle_deg

#####################
def main():
#####################

target_matrix = np.array([[ 0.80709068, 0., -0.59042749],
[ 0.59042749, 0., 0.80709068],
[ 0., -1., 0. ]])

print('===============================')

orig_matrix1 = np.array([[-8.26982397e-01, 0.00000000e+00, -5.62227814e-01],
[-5.62227814e-01, -6.93889390e-18, 8.26982397e-01],
[ 0.00000000e+00, 1.00000000e+00, 4.85722573e-17]])
R1 = Rotation.from_matrix(orig_matrix1)

print(f"Euler ZYX:\n{R1.as_euler('zyx', degrees=True)}\n")

compare_orientations(orig_matrix1, target_matrix, True)

print('===============================')

# orig_matrix2 differs from orig_matrix1 by 2 deg
orig_matrix2 = np.array([[-8.06857473e-01, 0.00000000e+00, 5.90746154e-01],
[-5.90746154e-01, 2.08166817e-17, -8.06857473e-01],
[ 0.00000000e+00, -1.00000000e+00, -3.46944695e-17]])

R2 = Rotation.from_matrix(orig_matrix2)

print(f"Euler ZYX:\n{R2.as_euler('zyx', degrees=True)}\n")

compare_orientations(orig_matrix2, target_matrix, True)

print('===============================')

##################################
if __name__ == "__main__":
main()


In few cases (see the above code), although I start with two "origin" matrices which are different, the rotation angle found by the script is the same. This occurs although the two origin matrices are represented with the expected difference (e.g. what is returned by the as_euler() function).

Is this due to some regularization of the matrices performed inside the Rotation module?

Do you have suggestions on how to improve the code?

Continue reading...