Skip to content

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

Open
petercorke wants to merge 1 commit into
rai-opensource:masterfrom
petercorke:fix/icp2d-trnorm2
Open

fix: normalize ICP2d's returned transform to exact orthonormality#204
petercorke wants to merge 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!

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.

2 participants