feat(pose_3d): GET /tags multi-camera, rimuovi /front_tags

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

src/presa_mano/api/pose_3d_routes.py

@@ -249,11 +249,17 @@ def get_clouds(step: int = 8, max_depth_mm: int = 2000) -> dict[str, Any]:
     return result
 
 
-# --- Front-camera tag finder: detect + misura indipendente via depth ----
-# Scansiona ArUco DICT_4X4_50 sulla RGB del D435i frontale e per ogni tag
-# trovato misura i 4 corner 3D usando il depth sensor (independent da pnp).
-# Output in frame ottico OpenCV (X right, Y down, Z forward) — il client
-# lo agganciera' al link URDF del d435 nella scena 3D.
+# --- Multi-camera tag finder: detect + misura indipendente via depth ----
+# Scansiona ArUco DICT_4X4_50 su tutte le camere attive (Nuwa polso DX e
+# D435i frontale). Per ogni tag trovato misura i 4 corner 3D via depth sensor
+# (indipendente da solvePnP). Output in frame ottico OpenCV per ciascuna camera.
+
+def _edge_len_mm(a: list[float], b: list[float]) -> "Optional[float]":
+    """Lunghezza 3D in mm tra due corner; None se un corner ha NaN."""
+    if any(x != x for x in a + b):  # NaN check
+        return None
+    return (sum((a[i] - b[i]) ** 2 for i in range(3))) ** 0.5 * 1000.0
+
 
 def _parse_id_range(spec: str) -> list[int]:
     """Accetta "0-49" o "1,10,20,33"."""
@@ -271,27 +277,18 @@ def _parse_id_range(spec: str) -> list[int]:
     return sorted(set(out))
 
 
-@router.get("/front_tags")
-def get_front_tags(ids: str = "0-49", probe_size_mm: float = 40.0) -> dict[str, Any]:
-    """Detect ArUco markers via front D435i + misura geometria 3D reale.
-
-    Per ogni tag trovato chiama /api/measure ai 4 corner per ottenere i punti
-    3D direttamente dal depth sensor (indipendente da solvePnP). Cosi' la
-    dimensione misurata non dipende dalla size assunta in input.
-
-    `probe_size_mm` viene passato a marker-finder solo per soddisfare la sua
-    API (richiede un size per ogni id) ma non influisce sulla misura.
-    """
-    sw = Stopwatch()
-    front = _deps.front_camera()
+def _detect_tags_on(client, frame_label: str, ids: str,
+                    probe_size_mm: float = 40.0) -> list[dict]:
+    """Detect ArUco markers su `client` (qualsiasi NuwaClient) e misura i 4
+    corner + centro in 3D via depth sensor (indipendente da solvePnP).
+    Ritorna una lista di dict per tag (id, corners_pixel, corners_3d_optical_m,
+    corner_depths_mm, center_3d_optical_m, edge_mm_per_side, edge_mm, distance_m).
+    `frame_label` è solo per i log; i punti sono nel frame ottico OpenCV della camera."""
     mk = _deps.marker()
-
-    # 1) intrinsics RGB del D435i → passate a marker-finder per pose corrette
-    with sw.step("intrinsics"):
-        intr = front.intrinsics()
+    intr = client.intrinsics()
     rgb = (intr or {}).get("rgb") or {}
     if not rgb or "fx" not in rgb:
-        raise HTTPException(500, detail=f"front camera intrinsics missing rgb: {intr!r}")
+        raise RuntimeError(f"intrinsics rgb mancanti: {intr!r}")
     K = {
         "camera_matrix": [
             [float(rgb["fx"]), 0.0, float(rgb["cx"])],
@@ -303,92 +300,95 @@ def get_front_tags(ids: str = "0-49", probe_size_mm: float = 40.0) -> dict[str,
             for k in ("k1", "k2", "p1", "p2", "k3")
         ],
     }
-
-    # 2) detect (passiamo intrinsics override per non usare il default Nuwa)
     id_list = _parse_id_range(ids)
     markers = [{"id": int(i), "size_mm": float(probe_size_mm)} for i in id_list]
-    with sw.step("detect"):
-        det = mk.detect_url(markers=markers, image_url=front.rgb_url(), intrinsics=K)
+    det = mk.detect_url(markers=markers, image_url=client.rgb_url(), intrinsics=K)
 
-    # 3) per ogni found: depth ai 4 corner + center
     tags_out: list[dict[str, Any]] = []
-    with sw.step("measure_corners"):
-        for m in det.get("found", []):
-            corners = m.get("corners") or []
-            if len(corners) != 4:
-                continue
-            pts3d: list[list[float]] = []
-            depths: list[Optional[int]] = []
-            try:
-                for (u, v) in corners:
-                    md = front.measure(int(round(u)), int(round(v)), radius=2)
-                    pt = (md or {}).get("point_m") or {}
-                    d_mm = (md or {}).get("depth_mm")
-                    if not pt or d_mm in (None, 0):
-                        pts3d.append([float("nan")] * 3)
-                        depths.append(None)
-                    else:
-                        pts3d.append([float(pt["x"]), float(pt["y"]), float(pt["z"])])
-                        depths.append(int(d_mm))
-                cu = sum(c[0] for c in corners) / 4.0
-                cv = sum(c[1] for c in corners) / 4.0
-                cmd = front.measure(int(round(cu)), int(round(cv)), radius=4)
-                cpt = (cmd or {}).get("point_m") or {}
-                center3d = (
-                    [float(cpt["x"]), float(cpt["y"]), float(cpt["z"])]
-                    if cpt else [float("nan")] * 3
-                )
-            except Exception as e:
-                log.warning("front_tags: measure failed for id=%s: %s", m.get("id"), e)
-                continue
+    for m in det.get("found", []):
+        corners = m.get("corners") or []
+        if len(corners) != 4:
+            continue
+        pts3d: list[list[float]] = []
+        depths: list[Optional[int]] = []
+        try:
+            for (u, v) in corners:
+                md = client.measure(int(round(u)), int(round(v)), radius=2)
+                pt = (md or {}).get("point_m") or {}
+                d_mm = (md or {}).get("depth_mm")
+                if not pt or d_mm in (None, 0):
+                    pts3d.append([float("nan")] * 3)
+                    depths.append(None)
+                else:
+                    pts3d.append([float(pt["x"]), float(pt["y"]), float(pt["z"])])
+                    depths.append(int(d_mm))
+            cu = sum(c[0] for c in corners) / 4.0
+            cv = sum(c[1] for c in corners) / 4.0
+            cmd = client.measure(int(round(cu)), int(round(cv)), radius=4)
+            cpt = (cmd or {}).get("point_m") or {}
+            center3d = ([float(cpt["x"]), float(cpt["y"]), float(cpt["z"])]
+                        if cpt else [float("nan")] * 3)
+        except Exception as e:
+            log.warning("tags(%s): measure failed for id=%s: %s",
+                        frame_label, m.get("id"), e)
+            continue
 
-            # Outlier rejection sulla depth: il pixel di un corner sul bordo
-            # del marker spesso prende la depth del background (es. 1358mm
-            # mentre gli altri 3 sono a 320mm). Rifiutiamo corner con depth
-            # > 20% off dalla mediana → li trattiamo come NaN nel calcolo edge.
-            valid_d = [d for d in depths if d is not None]
-            if len(valid_d) >= 3:
-                # mediana semplice (4 elementi → media dei 2 centrali)
-                sd = sorted(valid_d)
-                median_d = (sd[len(sd)//2 - 1] + sd[len(sd)//2]) / 2 if len(sd) % 2 == 0 else sd[len(sd)//2]
-                for i, d in enumerate(depths):
-                    if d is None:
-                        continue
-                    if abs(d - median_d) / median_d > 0.20:
-                        pts3d[i] = [float("nan")] * 3  # esclude da edge_mm
+        valid_d = [d for d in depths if d is not None]
+        if len(valid_d) >= 3:
+            sd = sorted(valid_d)
+            median_d = (sd[len(sd)//2 - 1] + sd[len(sd)//2]) / 2 if len(sd) % 2 == 0 else sd[len(sd)//2]
+            for i, d in enumerate(depths):
+                if d is None:
+                    continue
+                if abs(d - median_d) / median_d > 0.20:
+                    pts3d[i] = [float("nan")] * 3
 
-            # 3D edge lengths (mm) — robusti a un corner senza depth (NaN salta)
-            def _L(a: list[float], b: list[float]) -> Optional[float]:
-                if any(x != x for x in a + b):  # NaN check
-                    return None
-                return (sum((a[i] - b[i]) ** 2 for i in range(3))) ** 0.5 * 1000.0
+        edges_mm = [_edge_len_mm(pts3d[i], pts3d[(i + 1) % 4]) for i in range(4)]
+        valid_edges = [e for e in edges_mm if e is not None]
+        edge_mean = sum(valid_edges) / len(valid_edges) if valid_edges else None
 
-            edges_mm = [_L(pts3d[i], pts3d[(i + 1) % 4]) for i in range(4)]
-            valid_edges = [e for e in edges_mm if e is not None]
-            edge_mean = sum(valid_edges) / len(valid_edges) if valid_edges else None
+        distance_m: Optional[float] = None
+        if all(x == x for x in center3d):
+            distance_m = (sum(c ** 2 for c in center3d)) ** 0.5
 
-            distance_m: Optional[float] = None
-            if all(x == x for x in center3d):
-                distance_m = (sum(c ** 2 for c in center3d)) ** 0.5
+        tags_out.append({
+            "id": int(m["id"]),
+            "corners_pixel": [[float(c[0]), float(c[1])] for c in corners],
+            "corners_3d_optical_m": pts3d,
+            "corner_depths_mm": depths,
+            "center_3d_optical_m": center3d,
+            "edge_mm_per_side": edges_mm,
+            "edge_mm": edge_mean,
+            "distance_m": distance_m,
+        })
+    return tags_out
 
-            tags_out.append({
-                "id": int(m["id"]),
-                "corners_pixel": [[float(c[0]), float(c[1])] for c in corners],
-                "corners_3d_optical_m": pts3d,
-                "corner_depths_mm": depths,
-                "center_3d_optical_m": center3d,
-                "edge_mm_per_side": edges_mm,
-                "edge_mm": edge_mean,
-                "distance_m": distance_m,
-            })
 
-    result = {
-        "tags": tags_out,
-        "not_found_count": int(len(det.get("not_found", []))),
-        "frame": "d435_rgb_optical",  # OpenCV: X right, Y down, Z forward
-        "timings_ms": sw.as_dict(),
-    }
-    return result
+# Map: camera key → (frame_label, _deps attribute name).
+# Usiamo il nome stringa dell'attributo su _deps e lo risolviamo via getattr
+# a ogni richiesta: cosi' patch("...pose_3d_routes._deps.nuwa") viene visto.
+_TAG_CAMERAS: dict[str, tuple[str, str]] = {
+    "nuwa": ("nuwa_rgb_optical", "nuwa"),
+    "d435": ("d435_rgb_optical", "front_camera"),
+}
+
+
+@router.get("/tags")
+def get_tags(ids: str = "0-49", probe_size_mm: float = 40.0) -> dict[str, Any]:
+    """Detect ArUco su tutte le camere attive (Nuwa polso + D435i frontale).
+    Una camera offline riporta tags=[] + error, non fa fallire l'altra."""
+    sw = Stopwatch()
+    cameras: dict[str, Any] = {}
+    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)
+            cameras[key] = {"tags": tags, "frame": frame_label}
+        except Exception as e:
+            cameras[key] = {"tags": [], "frame": frame_label,
+                            "error": f"{type(e).__name__}: {e}"}
+    return {"cameras": cameras, "timings_ms": sw.as_dict()}
 
 
 # --- Tag locator: piazza crocetta nella scena 3D e salva la posa in pelvis ---

tests/test_pose_3d_tags.py

@@ -0,0 +1,75 @@
+"""Test del nuovo endpoint multi-camera GET /api/pose_3d/tags."""
+from __future__ import annotations
+
+from unittest.mock import patch, MagicMock
+
+from fastapi.testclient import TestClient
+
+from presa_mano.main import app
+
+client = TestClient(app)
+
+
+def _fake_camera(corner_depth_m=0.32):
+    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):
+        return {"point_m": {"x": u / 1000.0, "y": v / 1000.0, "z": corner_depth_m},
+                "depth_mm": int(corner_depth_m * 1000)}
+    cam.measure.side_effect = _measure
+    return cam
+
+
+def _fake_marker(found_ids):
+    mk = MagicMock()
+    found = [{
+        "id": i,
+        "corners": [[100, 100], [140, 100], [140, 140], [100, 140]],
+    } for i in found_ids]
+    mk.detect_url.return_value = {"found": found, "not_found": []}
+    return mk
+
+
+def test_tags_returns_both_cameras():
+    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 = _fake_camera()
+        front_f.return_value = _fake_camera()
+        mk_f.return_value = _fake_marker([5])
+        r = client.get("/api/pose_3d/tags?ids=0-9")
+        assert r.status_code == 200
+        body = r.json()
+        assert set(body["cameras"].keys()) == {"nuwa", "d435"}
+        assert body["cameras"]["nuwa"]["frame"] == "nuwa_rgb_optical"
+        assert body["cameras"]["d435"]["frame"] == "d435_rgb_optical"
+        assert body["cameras"]["nuwa"]["tags"][0]["id"] == 5
+        assert body["cameras"]["d435"]["tags"][0]["id"] == 5
+
+        tag = body["cameras"]["nuwa"]["tags"][0]
+        assert tag["edge_mm"] is not None
+        assert 35.0 < tag["edge_mm"] < 45.0          # ~40mm lato del quad
+        assert tag["distance_m"] is not None
+        assert abs(tag["distance_m"] - 0.362) < 0.02  # sqrt(.12^2+.12^2+.32^2)
+        assert all(len(c) == 3 for c in tag["corners_3d_optical_m"])
+
+
+def test_tags_one_camera_offline_reports_error():
+    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 = MagicMock()
+        nuwa.intrinsics.side_effect = RuntimeError("nuwa down")
+        nuwa_f.return_value = nuwa
+        front_f.return_value = _fake_camera()
+        mk_f.return_value = _fake_marker([5])
+        r = client.get("/api/pose_3d/tags?ids=0-9")
+        assert r.status_code == 200
+        body = r.json()
+        assert body["cameras"]["nuwa"]["tags"] == []
+        assert "error" in body["cameras"]["nuwa"]
+        assert body["cameras"]["d435"]["tags"][0]["id"] == 5