Skip to content

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

Merged
taughz merged 1 commit into
rai-opensource:masterfrom
petercorke:fix/icp2d-trnorm2
Oct 13, 2026
Merged

taughz merged 1 commit into
rai-opensource:masterfrom
petercorke:fix/icp2d-trnorm2

Conversation

@petercorke

Copy link
Copy Markdown
Collaborator

Summary

  • `ICP2d`'s final `T` accumulates ordinary floating-point roundoff through the loop's repeated `T = T @ new_T` composition. Traced this on a real scan-matching case (RTB's `PoseGraph.scanmatch`, killian dataset): deviation from orthonormality grows monotonically from ~1e-16 at iteration 1 to ~7.3e-15 after 15 iterations -- just over `ishom2()`'s `20*eps ≈ 4.44e-15` tolerance.
  • When that check fails, downstream `SE2(T)` construction doesn't raise -- `arghandler()` returns `False` and execution falls through to the branch meant for `[x, y, theta]`-style list/tuple arguments, since a (3,3) array also satisfies `len(x) == 3`. This produces a confusing `TypeError: only 0-dimensional arrays can be converted to Python scalars` deep in `math.cos()`, nowhere near the real problem. (That fallthrough behavior is a separate, pre-existing `SE2.init` issue -- not addressed in this PR, flagging it here for visibility.)
  • Fix: normalize the returned `T` with `trnorm2()` before returning, which is exactly the situation its own docstring describes ("prevent finite word length arithmetic causing transforms to become 'unnormalized'").

Test plan

  • `pytest tests/base/test_transforms2d.py` -- 20 passed
  • New `test_icp2d_returns_valid_se2` (seed fixed for determinism) confirmed to fail against the pre-fix implementation and pass with the fix
  • Verified `RTB`'s `PoseGraph.scanmatch(100, 101)` on the real killian.g2o lidar dataset, which surfaced this, now runs end-to-end

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.
@codecov-commenter

Copy link
Copy Markdown

⚠️ Please install the 'codecov app svg image' to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

✅ All modified and coverable lines are covered by tests.

📢 Thoughts on this report? Let us know!

@taughz taughz mentioned this pull request Oct 11, 2026
7 tasks done
@taughz
taughz self-requested a review October 13, 2026 17:04
@taughz
taughz merged commit 3899801 into rai-opensource:master Oct 13, 2026
13 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

3 participants