From b9c144d8439d2ab0504bd6dad93b6762e1fd4af7 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Sun, 16 Aug 2026 14:22:13 +1000 Subject: [PATCH] fix: normalize ICP2d's returned transform to exact orthonormality T accumulates ordinary floating-point roundoff through the loop's repeated T = T @ new_T composition. After enough iterations this can drift just outside ishom2()'s orthonormality tolerance, causing downstream SE2(T) construction to silently misinterpret the matrix as an [x, y, theta] vector instead of raising a clear error (a separate SE2.__init__ issue, not addressed here). Restore exact orthonormality with trnorm2() before returning, as its own docstring describes for exactly this situation. --- spatialmath/base/transforms2d.py | 6 +++++- tests/base/test_transforms2d.py | 16 ++++++++++++++++ 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index 50e35d02..ab451998 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -1240,7 +1240,11 @@ def _FindCorrespondences( num_iter += 1 - return T + # T accumulates roundoff error through repeated composition + # (T = T @ new_T) over the iterations above, and can drift just + # outside ishom2()'s orthonormality tolerance -- restore exact + # orthonormality before returning. + return trnorm2(T) if _matplotlib_exists: diff --git a/tests/base/test_transforms2d.py b/tests/base/test_transforms2d.py index 6a406006..bf578b22 100755 --- a/tests/base/test_transforms2d.py +++ b/tests/base/test_transforms2d.py @@ -174,6 +174,22 @@ def test_icp2d(self): T2 = ICP2d(p2, p1, T=xyt2tr([1, 2, 0.2])) nt.assert_almost_equal(T, T2) + def test_icp2d_returns_valid_se2(self): + # noisy correspondences plus a forced iteration count accumulate + # roundoff error in the T = T @ new_T composition -- the result + # must still pass ishom2(check=True), ie. be exactly orthonormal. + # seed fixed: this configuration reliably triggers the drift. + np.random.seed(0) + p1 = np.random.uniform(size=(2, 30)) + T = xyt2tr([1, 2, 0.2]) + + p2 = homtrans(T, p1) + np.random.normal(scale=0.01, size=(2, 30)) + k = np.random.permutation(p2.shape[1]) + p2 = p2[:, k] + + T2 = ICP2d(p2, p1, max_iter=100, min_delta_err=0) + self.assertTrue(ishom2(T2, check=True)) + def test_print2(self): T = transl2(1, 2) @ trot2(0.3)