Skip to content

fix: normalize ICP2d's returned transform to exact orthonormality #204

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

Merged
taughz merged 1 commit into from
Oct 13, 2026
Merged

fix: normalize ICP2d's returned transform to exact orthonormality #204

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
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.
  • Loading branch information
@petercorke
petercorke committed Aug 16, 2026
commit b9c144d8439d2ab0504bd6dad93b6762e1fd4af7
6 changes: 5 additions & 1 deletion spatialmath/base/transforms2d.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
16 changes: 16 additions & 0 deletions tests/base/test_transforms2d.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
Loading