From 77fb0f8fc08441461124fb6370f7af455dc02d2d Mon Sep 17 00:00:00 2001
From: Adriano <adriano.dalpastro@laboticsitalia.com>
Date: Wed, 3 Jun 2026 12:11:11 +0200
Subject: [PATCH] feat(geometry): umeyama_similarity (rot+trasl+scala uniforme)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
---
 config.yaml                    |  8 +++---
 src/presa_mano/geometry.py     | 38 ++++++++++++++++++++++++++
 tests/test_geometry_umeyama.py | 49 ++++++++++++++++++++++++++++++++++
 3 files changed, 91 insertions(+), 4 deletions(-)
 create mode 100644 tests/test_geometry_umeyama.py

diff --git a/config.yaml b/config.yaml
index 4285386..e44cc88 100644
--- a/config.yaml
+++ b/config.yaml
@@ -147,13 +147,13 @@ pose_3d:
   cloud_params:
     step: 2
     max_depth_mm: 1000
-    nuwa_size_mm: 5
-    d435_size_mm: 5
+    nuwa_size_mm: 3
+    d435_size_mm: 3
     d435_scale_xy: 1
     flip_nuwa:
     - false
-    - false
-    - false
+    - true
+    - true
     flip_d435:
     - false
     - false
diff --git a/src/presa_mano/geometry.py b/src/presa_mano/geometry.py
index b58eeeb..f62934c 100644
--- a/src/presa_mano/geometry.py
+++ b/src/presa_mano/geometry.py
@@ -142,3 +142,41 @@ def quaternion_xyzw_to_rpy_deg(quat_xyzw) -> tuple[float, float, float]:
     rpy_rad = R_to_euler_xyz(R)
     rpy_deg = np.degrees(rpy_rad)
     return float(rpy_deg[0]), float(rpy_deg[1]), float(rpy_deg[2])
+
+
+def umeyama_similarity(src: np.ndarray, dst: np.ndarray) -> tuple[float, np.ndarray, np.ndarray, float]:
+    """Fit dst ≈ s·R·src + t (Umeyama 1991, con scala uniforme).
+
+    src, dst: (N,3) array di punti corrispondenti.
+    Ritorna (scale: float, R: (3,3), t: (3,), rmse: float in unità di dst).
+    Solleva ValueError se N<3 o il set è degenere (varianza nulla).
+    """
+    src = np.asarray(src, dtype=np.float64).reshape(-1, 3)
+    dst = np.asarray(dst, dtype=np.float64).reshape(-1, 3)
+    if src.shape != dst.shape:
+        raise ValueError(f"shape mismatch: {src.shape} vs {dst.shape}")
+    n = src.shape[0]
+    if n < 3:
+        raise ValueError(f"servono almeno 3 punti, ricevuti {n}")
+
+    mu_src = src.mean(axis=0)
+    mu_dst = dst.mean(axis=0)
+    src_c = src - mu_src
+    dst_c = dst - mu_dst
+
+    var_src = (src_c ** 2).sum() / n
+    if var_src < 1e-12:
+        raise ValueError("punti src degeneri (varianza nulla)")
+
+    cov = (dst_c.T @ src_c) / n
+    U, D, Vt = np.linalg.svd(cov)
+    S = np.eye(3)
+    if np.linalg.det(U) * np.linalg.det(Vt) < 0:
+        S[2, 2] = -1.0  # vieta riflessioni
+    R = U @ S @ Vt
+    scale = float((D * np.diag(S)).sum() / var_src)
+    t = mu_dst - scale * (R @ mu_src)
+
+    pred = (scale * (R @ src.T)).T + t
+    rmse = float(np.sqrt(((pred - dst) ** 2).sum(axis=1).mean()))
+    return scale, R, t, rmse
diff --git a/tests/test_geometry_umeyama.py b/tests/test_geometry_umeyama.py
new file mode 100644
index 0000000..0a5adb8
--- /dev/null
+++ b/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)