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

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

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