From 8b01ae123fca49f663637a0179eb0575e26436de Mon Sep 17 00:00:00 2001 From: Adriano Date: Wed, 3 Jun 2026 12:15:59 +0200 Subject: [PATCH] feat(pose_3d): GET /tags multi-camera, rimuovi /front_tags Co-Authored-By: Claude Sonnet 4.6 --- src/presa_mano/api/pose_3d_routes.py | 198 +++++++++++++-------------- tests/test_pose_3d_tags.py | 75 ++++++++++ 2 files changed, 174 insertions(+), 99 deletions(-) create mode 100644 tests/test_pose_3d_tags.py diff --git a/src/presa_mano/api/pose_3d_routes.py b/src/presa_mano/api/pose_3d_routes.py index c5b7e1e..174dd9e 100644 --- a/src/presa_mano/api/pose_3d_routes.py +++ b/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 --- diff --git a/tests/test_pose_3d_tags.py b/tests/test_pose_3d_tags.py new file mode 100644 index 0000000..944706b --- /dev/null +++ b/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