feat(pose_3d): POST /align fit Umeyama cross-camera + persistenza

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

src/presa_mano/api/pose_3d_routes.py

@@ -10,12 +10,13 @@ from pathlib import Path
 from typing import Any, Literal, Optional
 
 import httpx
+import numpy as np
 from fastapi import APIRouter, HTTPException
 from pydantic import BaseModel, Field
 
 from ..clients import G1Unavailable
-from ..config import get_config, set_config_value
-from ..geometry import normalize_quaternion
+from ..config import get_config, set_config_value, update_config
+from ..geometry import normalize_quaternion, umeyama_similarity
 from ..perception.pointcloud import nuwa_to_cv_frame
 from ..timing import Stopwatch
 from . import _deps
@@ -391,6 +392,70 @@ def get_tags(ids: str = "0-49", probe_size_mm: float = 40.0) -> dict[str, Any]:
     return {"cameras": cameras, "timings_ms": sw.as_dict()}
 
 
+@router.post("/align")
+def post_align(ids: str = "0-49", align_id: int = 0,
+               probe_size_mm: float = 40.0) -> dict[str, Any]:
+    """Calibrazione cross-camera: trova `align_id` su Nuwa e D435i, fitta una
+    similarita' (rot+trasl+scala) Nuwa-optical -> D435-optical e la PERSISTE in
+    config.yaml > pose_3d.cross_camera. Se il tag non e' visto da entrambe, NON
+    sovrascrive la matrice salvata."""
+    sw = Stopwatch()
+    found: dict[str, Optional[dict]] = {}
+    cam_errors: dict[str, str] = {}
+    for key, (frame_label, dep_name) in _TAG_CAMERAS.items():
+        try:
+            factory = getattr(_deps, dep_name)
+            with sw.step(key):
+                tags = _detect_tags_on(factory(), frame_label, ids, probe_size_mm)
+            found[key] = next((t for t in tags if int(t["id"]) == int(align_id)), None)
+        except Exception as e:
+            log.warning("align: camera %s failed: %s", key, e)
+            found[key] = None
+            cam_errors[key] = f"{type(e).__name__}: {e}"
+
+    missing = [k for k, v in found.items() if v is None]
+    if missing:
+        if cam_errors:
+            detail = "; ".join(f"{k}: {msg}" for k, msg in cam_errors.items())
+            err = f"camera non disponibile -> {detail}"
+        else:
+            err = f"tag {align_id} non visto da: {', '.join(missing)}"
+        return {"found_in_both": False, "align_id": int(align_id),
+                "error": err, "timings_ms": sw.as_dict()}
+
+    # I corner sono accoppiati posizionalmente: stesso marker-finder + stessa
+    # geometria ArUco -> ordine corner deterministico e identico tra le camere.
+    src, dst = [], []
+    for cn, cd in zip(found["nuwa"]["corners_3d_optical_m"],
+                      found["d435"]["corners_3d_optical_m"]):
+        if any(x != x for x in cn) or any(x != x for x in cd):
+            continue
+        src.append(cn)
+        dst.append(cd)
+    if len(src) < 3:
+        return {"found_in_both": False, "align_id": int(align_id),
+                "error": f"corner validi insufficienti ({len(src)}/4)",
+                "timings_ms": sw.as_dict()}
+
+    try:
+        with sw.step("umeyama"):
+            scale, R, t, rmse = umeyama_similarity(np.array(src), np.array(dst))
+    except ValueError as e:
+        return {"found_in_both": False, "align_id": int(align_id),
+                "error": f"fit fallito: {e}", "timings_ms": sw.as_dict()}
+
+    payload = {
+        "align_id": int(align_id),
+        "scale": float(scale),
+        "R": [[float(x) for x in row] for row in R],
+        "t": [float(x) for x in t],
+        "rmse_mm": float(rmse * 1000.0),
+        "n_corners": len(src),
+    }
+    update_config({"pose_3d": {"cross_camera": payload}})
+    return {"found_in_both": True, **payload, "timings_ms": sw.as_dict()}
+
+
 # --- Tag locator: piazza crocetta nella scena 3D e salva la posa in pelvis ---
 # Workflow utente (vedi web/pose_3d.js sezione "tag locator"):
 #   1) seleziona tag da dropdown (id+size+label)