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 ..config import get_config, set_config_value, update_config
-from ..geometry import normalize_quaternion
+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)

tests/test_pose_3d_align.py

@@ -0,0 +1,85 @@
+"""Test POST /api/pose_3d/align: fit Umeyama + persistenza config."""
+from __future__ import annotations
+
+from unittest.mock import patch, MagicMock
+
+import numpy as np
+from fastapi.testclient import TestClient
+
+from presa_mano.main import app
+from presa_mano.config import get_config
+
+client = TestClient(app)
+
+_CORNERS_PX = [[100, 100], [140, 100], [140, 140], [100, 140]]
+
+
+def _cam_with_points(points_by_pixel):
+    cam = MagicMock()
+    cam.intrinsics.return_value = {"rgb": {"fx": 600.0, "fy": 600.0, "cx": 320.0, "cy": 240.0}}
+    cam.rgb_url.return_value = "http://cam/rgb.png"
+
+    def _measure(u, v, radius=2):
+        key = (int(u), int(v))
+        if key in points_by_pixel:
+            x, y, z = points_by_pixel[key]
+        else:
+            xs = [p[0] for p in points_by_pixel.values()]
+            ys = [p[1] for p in points_by_pixel.values()]
+            zs = [p[2] for p in points_by_pixel.values()]
+            x, y, z = sum(xs)/len(xs), sum(ys)/len(ys), sum(zs)/len(zs)
+        return {"point_m": {"x": x, "y": y, "z": z}, "depth_mm": int(z * 1000)}
+    cam.measure.side_effect = _measure
+    return cam
+
+
+def _marker(found_ids):
+    mk = MagicMock()
+    mk.detect_url.return_value = {
+        "found": [{"id": i, "corners": _CORNERS_PX} for i in found_ids],
+        "not_found": [],
+    }
+    return mk
+
+
+def test_align_fits_and_persists():
+    nuwa_pts = {
+        (100, 100): (0.10, 0.10, 0.30),
+        (140, 100): (0.14, 0.10, 0.30),
+        (140, 140): (0.14, 0.14, 0.30),
+        (100, 140): (0.10, 0.14, 0.30),
+    }
+    d435_pts = {k: (v[0]*2, v[1]*2, v[2]*2) for k, v in nuwa_pts.items()}
+    with patch("presa_mano.api.pose_3d_routes._deps.nuwa") as nuwa_f, \
+         patch("presa_mano.api.pose_3d_routes._deps.front_camera") as front_f, \
+         patch("presa_mano.api.pose_3d_routes._deps.marker") as mk_f:
+        nuwa_f.return_value = _cam_with_points(nuwa_pts)
+        front_f.return_value = _cam_with_points(d435_pts)
+        mk_f.return_value = _marker([7])
+        r = client.post("/api/pose_3d/align?ids=0-9&align_id=7")
+        assert r.status_code == 200
+        body = r.json()
+        assert body["found_in_both"] is True
+        assert abs(body["scale"] - 2.0) < 1e-3
+        assert body["align_id"] == 7
+        cc = get_config()["pose_3d"]["cross_camera"]
+        assert abs(cc["scale"] - 2.0) < 1e-3
+        assert cc["align_id"] == 7
+        assert len(cc["R"]) == 3 and len(cc["t"]) == 3
+
+
+def test_align_tag_missing_in_one_camera_does_not_save():
+    pts = {(100,100):(0.1,0.1,0.3),(140,100):(0.14,0.1,0.3),
+           (140,140):(0.14,0.14,0.3),(100,140):(0.1,0.14,0.3)}
+    with patch("presa_mano.api.pose_3d_routes._deps.nuwa") as nuwa_f, \
+         patch("presa_mano.api.pose_3d_routes._deps.front_camera") as front_f, \
+         patch("presa_mano.api.pose_3d_routes._deps.marker") as mk_f:
+        nuwa_f.return_value = _cam_with_points(pts)
+        front_f.return_value = _cam_with_points(pts)
+        mk_f.return_value = _marker([1])  # nessun tag id=99
+        r = client.post("/api/pose_3d/align?ids=0-9&align_id=99")
+        assert r.status_code == 200
+        body = r.json()
+        assert body["found_in_both"] is False
+        assert body.get("error")
+        assert "cross_camera" not in get_config().get("pose_3d", {})