feat(geometry): umeyama_similarity (rot+trasl+scala uniforme)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

tests/test_geometry_umeyama.py

@@ -0,0 +1,49 @@
+"""Unit test per umeyama_similarity (absolute orientation con scala)."""
+from __future__ import annotations
+
+import numpy as np
+import pytest
+
+from presa_mano.geometry import umeyama_similarity, rpy_deg_to_quaternion_xyzw, quat_xyzw_to_R
+
+
+def _known_transform():
+    # scala 1.5, rotazione nota, traslazione nota
+    R = quat_xyzw_to_R(rpy_deg_to_quaternion_xyzw(10.0, -20.0, 35.0))
+    s = 1.5
+    t = np.array([0.3, -0.1, 0.2])
+    return s, R, t
+
+
+def test_recovers_known_similarity():
+    s, R, t = _known_transform()
+    rng = np.random.default_rng(0)
+    src = rng.normal(size=(8, 3))
+    dst = (s * (R @ src.T)).T + t
+
+    s_hat, R_hat, t_hat, rmse = umeyama_similarity(src, dst)
+
+    assert abs(s_hat - s) < 1e-6
+    assert np.allclose(R_hat, R, atol=1e-6)
+    assert np.allclose(t_hat, t, atol=1e-6)
+    assert rmse < 1e-6
+
+
+def test_forbids_reflection():
+    rng = np.random.default_rng(1)
+    src = rng.normal(size=(5, 3))
+    dst = rng.normal(size=(5, 3))
+    _, R_hat, _, _ = umeyama_similarity(src, dst)
+    assert np.linalg.det(R_hat) > 0
+
+
+def test_raises_on_too_few_points():
+    with pytest.raises(ValueError):
+        umeyama_similarity(np.zeros((2, 3)), np.zeros((2, 3)))
+
+
+def test_raises_on_degenerate_points():
+    src = np.ones((4, 3))
+    dst = np.ones((4, 3)) * 2.0
+    with pytest.raises(ValueError):
+        umeyama_similarity(src, dst)