From 9ac32e71dd9bcfa0e7a6144eb2548717257bf56a Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Sun, 9 Aug 2026 19:45:54 +1000 Subject: [PATCH] fix: SO3.RotatedVector returned identity for antipodal vectors SO3.RotatedVector(v1, v2) computes the rotation axis as v1 x v2. That cross product is zero whenever v1 and v2 are (anti)parallel, and the existing singularity check treated both cases identically -- returning identity regardless. Correct only when v1 and v2 point the same way; when they're antipodal (v2 == -v1), the correct answer is a 180 degree rotation about any axis perpendicular to v1, not identity. Distinguish the two cases via the sign of v1.v2 at the singularity. The antipodal case uses the closed form for a 180 degree rotation about a unit axis u (R = 2*u*u^T - I), with u found by crossing v1 against whichever world axis it's least aligned with. Found while building spatialgeometry's Arrow.FromTo(start, end), which needs exactly this "rotate a reference axis to point along an arbitrary direction" operation and hit the bug immediately for an arrow pointing straight backward relative to its own rest orientation. --- spatialmath/pose3d.py | 19 ++++++++++++++++++- tests/test_pose3d.py | 13 +++++++++++++ 2 files changed, 31 insertions(+), 1 deletion(-) diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py index b8d8d5de..03641b92 100644 --- a/spatialmath/pose3d.py +++ b/spatialmath/pose3d.py @@ -769,7 +769,24 @@ def RotatedVector(cls, v1: ArrayLike3, v2: ArrayLike3, tol=20) -> Self: v = smb.cross(v1, v2) s = smb.norm(v) if abs(s) < tol * np.finfo(float).eps: - return cls(np.eye(3), check=False) + c = np.dot(v1, v2) + if c > 0: + # v1 and v2 already (anti)parallel in the same direction + return cls(np.eye(3), check=False) + # v1 and v2 point in opposite directions -- the formula below + # is singular here too (it divides by s**2), but unlike the + # c > 0 case the answer isn't identity: any 180 degree + # rotation about an axis perpendicular to v1 takes v1 to v2. + # Closed form for a 180 degree rotation about unit axis u: + # R = 2*u*u^T - I (Rodrigues at theta=pi, sin=0, cos=-1). + # Pick u by crossing v1 with whichever world axis it's least + # aligned with, so the cross product is never itself + # degenerate. + axis = np.zeros(3) + axis[np.argmin(np.abs(v1))] = 1.0 + u = smb.unitvec(smb.cross(v1, axis)) + R = 2 * np.outer(u, u) - np.eye(3) + return cls(R, check=False) else: c = np.dot(v1, v2) V = smb.skew(v) diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py index cdb80fbd..7b0d2414 100755 --- a/tests/test_pose3d.py +++ b/tests/test_pose3d.py @@ -726,6 +726,19 @@ def test_rotatedvector(self): Re = SO3.RotatedVector(v1, v1) np.testing.assert_almost_equal(Re, np.eye(3)) + # Antipodal case: v1 and v2 point in exactly opposite directions. + # The cross product used to find the rotation axis is zero here + # too, same as the parallel case above, but the correct answer is + # a 180 degree flip, not identity -- regression test for a bug + # where this silently returned identity instead. + for v1 in ([1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 2, 3], [-3, 1, 2]): + v1 = unitvec(v1) + v2 = [-x for x in v1] + Re = SO3.RotatedVector(v1, v2) + np.testing.assert_almost_equal(np.asarray(Re * v1).flatten(), v2) + # must actually be a 180 degree rotation, not identity + assert not np.allclose(Re.A, np.eye(3)) + R = SO3() # identity matrix case # Check log and exponential map