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

## 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
- [x] \`pytest tests/base/test_transforms2d.py\` -- 20 passed
- [x] New \`test_icp2d_returns_valid_se2\` (seed fixed for determinism)
confirmed to fail against the pre-fix implementation and pass with the
fix
- [x] Verified \`RTB\`'s \`PoseGraph.scanmatch(100, 101)\` on the real
killian.g2o lidar dataset, which surfaced this, now runs end-to-end

Author: petercorke

spatialmath/base/transforms2d.py

@@ -1312,7 +1312,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:

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)