diff --git a/CLAUDE.md b/CLAUDE.md index 95f3915..28e678b 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -23,7 +23,7 @@ Useful endpoints (all under `http://localhost:8890`): - Grasp: `POST /api/grasp/plan`, `POST /api/grasp/execute` (set `dry_run` to gate G1 mutations). - Wrist/EE: `GET /api/wrist`, `POST /api/wrist/{absolute,relative,hand,emergency_stop,unlock}`. - Right arm: `GET /api/right_arm/state`, `POST /api/right_arm/detect_{front,wrist}`, `POST /api/right_arm/move_toward`, `POST /api/right_arm/{home,custom_pose}`. -- Pose 3D: `GET /api/pose_3d/state` (wrist + joints + config), `POST /api/pose_3d/apply`, `GET /api/pose_3d/clouds` (Nuwa + D435i point clouds), `CRUD /api/pose_3d/poses/{name}`. +- Pose 3D: `GET /api/pose_3d/state` (wrist + joints + config, incl. `cross_camera` calibration), `POST /api/pose_3d/apply`, `GET /api/pose_3d/clouds` (Nuwa + D435i point clouds), `GET /api/pose_3d/tags` (ArUco su Nuwa polso + D435i frontale), `POST /api/pose_3d/align?align_id=N` (fit Umeyama Nuwa→frontale, persiste in `pose_3d.cross_camera`), `CRUD /api/pose_3d/poses/{name}`. Tests live under `tests/` and run with `uv run pytest`. No linter is configured. @@ -94,6 +94,7 @@ Marker-finder caveat: it uses **D435i intrinsics baked into its config** that do - **Pose 3D graceful offline:** `GET /api/pose_3d/state` returns 200 even when G1 is unreachable — `wrist` and `joints` are `null`, `robot_error` carries the message. The tab must remain usable for editing config / named poses / viewing the URDF model without a live robot. - **Pose 3D clouds:** `GET /api/pose_3d/clouds` acquires from Nuwa (eye-in-hand, polso DX) and D435i (front camera, torso) in parallel. Both are returned in **OpenCV optical frame** (X right, Y down, Z forward). The client attaches them to URDF links with configurable offset/rpy from `config.yaml > pose_3d.clouds`. - **Pose 3D joints mirroring:** `G1Client.diagnostics()` → `motors[].{name, q}` gives real joint positions in radians. The frontend uses these to pose the URDF model directly, bypassing client-side IK. +- **Pose 3D cross-camera alignment:** `POST /api/pose_3d/align` fits an Umeyama similarity (rotation + translation + uniform scale) mapping Nuwa-optical → D435-optical from a tag (`align_id`) seen by both cameras, and persists it to `config.yaml > pose_3d.cross_camera`. The frontend renders the Nuwa cloud through the D435i transform pipeline (`d435LocalMatrix()` · alignMatrix) when `cross_camera.enabled`. If the tag isn't seen by both cameras, the saved matrix is NOT overwritten and the Nuwa cloud falls back to URDF placement. Tag detection (`GET /tags`) runs on both cameras; the GUI draws Nuwa tags red, D435i tags cyan. - Endpoints return `{ ..., "timings_ms": {step: ms, ..., total: ms} }`. New endpoints should follow this with `Stopwatch().step(name)`. - Calibration paths in config (`calibration.file`, `calibration.samples_file`) are resolved against `PRESA_MANO_DATA` when relative — see `_deps.calibration_file_path()` / `samples_file_path()`. - Italian is fine in user-facing copy (GUI, README, comments) — it's the project default. diff --git a/README.md b/README.md index d0838a3..f7525b6 100644 --- a/README.md +++ b/README.md @@ -56,6 +56,9 @@ Tab principali: - **Pose 3D**: viewport Three.js con URDF G1 specchiato sui giunti reali (`/diagnostics`), point cloud Nuwa + D435i, modello 3D camera sul polso, gizmo traslazione/rotazione, editor pose (Apply/Jog), named poses salvabili. + Overlay tag ArUco su entrambe le camere (Nuwa = rosso, D435i = ciano). + Calibrazione cross-camera: pulsante *Allinea* esegue fit Umeyama Nuwa→D435i + da un tag comune, persiste in `config.yaml > pose_3d.cross_camera`. La tab resta utilizzabile anche senza robot (mostra il modello URDF, config, pose salvate). diff --git a/src/presa_mano/api/pose_3d_routes.py b/src/presa_mano/api/pose_3d_routes.py index c5b7e1e..1e2f4e7 100644 --- a/src/presa_mano/api/pose_3d_routes.py +++ b/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 @@ -104,6 +105,7 @@ def get_state() -> dict[str, Any]: "right_wrist_camera": cfg.get("right_wrist_camera") or {}, "clouds": cfg.get("clouds") or {}, "cloud_params": cfg.get("cloud_params") or {}, + "cross_camera": cfg.get("cross_camera") or {}, "timings_ms": sw.as_dict(), } @@ -249,11 +251,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 +279,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 +302,159 @@ 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(), +# 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()} + + +@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), } - return result + 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 --- diff --git a/src/presa_mano/geometry.py b/src/presa_mano/geometry.py index b58eeeb..f62934c 100644 --- a/src/presa_mano/geometry.py +++ b/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 diff --git a/tests/test_geometry_umeyama.py b/tests/test_geometry_umeyama.py new file mode 100644 index 0000000..0a5adb8 --- /dev/null +++ b/tests/test_geometry_umeyama.py @@ -0,0 +1,49 @@ +"""Unit test per umeyama_similarity (absolute orientation con scala).""" +from __future__ import annotations + +import numpy as np +import pytest + +from presa_mano.geometry import umeyama_similarity, rpy_deg_to_quaternion_xyzw, quat_xyzw_to_R + + +def _known_transform(): + # scala 1.5, rotazione nota, traslazione nota + R = quat_xyzw_to_R(rpy_deg_to_quaternion_xyzw(10.0, -20.0, 35.0)) + s = 1.5 + t = np.array([0.3, -0.1, 0.2]) + return s, R, t + + +def test_recovers_known_similarity(): + s, R, t = _known_transform() + rng = np.random.default_rng(0) + src = rng.normal(size=(8, 3)) + dst = (s * (R @ src.T)).T + t + + s_hat, R_hat, t_hat, rmse = umeyama_similarity(src, dst) + + assert abs(s_hat - s) < 1e-6 + assert np.allclose(R_hat, R, atol=1e-6) + assert np.allclose(t_hat, t, atol=1e-6) + assert rmse < 1e-6 + + +def test_forbids_reflection(): + rng = np.random.default_rng(1) + src = rng.normal(size=(5, 3)) + dst = rng.normal(size=(5, 3)) + _, R_hat, _, _ = umeyama_similarity(src, dst) + assert np.linalg.det(R_hat) > 0 + + +def test_raises_on_too_few_points(): + with pytest.raises(ValueError): + umeyama_similarity(np.zeros((2, 3)), np.zeros((2, 3))) + + +def test_raises_on_degenerate_points(): + src = np.ones((4, 3)) + dst = np.ones((4, 3)) * 2.0 + with pytest.raises(ValueError): + umeyama_similarity(src, dst) diff --git a/tests/test_pose_3d_align.py b/tests/test_pose_3d_align.py new file mode 100644 index 0000000..a541c11 --- /dev/null +++ b/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", {}) diff --git a/tests/test_pose_3d_routes.py b/tests/test_pose_3d_routes.py index bf219a1..de8393b 100644 --- a/tests/test_pose_3d_routes.py +++ b/tests/test_pose_3d_routes.py @@ -208,3 +208,13 @@ def test_unlock_proxies_to_bridge(): r = client.post("/api/pose_3d/unlock") assert r.status_code == 200 assert r.json()["ok"] is True + + +def test_state_includes_cross_camera_block(): + with patch("presa_mano.api.pose_3d_routes._deps.g1") as g1_factory: + g1 = MagicMock() + g1.wrist.return_value = _fake_wrist_pair() + g1_factory.return_value = g1 + r = client.get("/api/pose_3d/state") + assert r.status_code == 200 + assert "cross_camera" in r.json() # {} se mai calibrato 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 diff --git a/web/index.html b/web/index.html index 86564cc..ddd91dd 100644 --- a/web/index.html +++ b/web/index.html @@ -449,6 +449,12 @@ +
+ + + +
+
—
@@ -511,7 +517,7 @@
-

Tag camera frontale

+

Tags

diff --git a/web/pose_3d.js b/web/pose_3d.js index 06f8d0e..3ad8f1c 100644 --- a/web/pose_3d.js +++ b/web/pose_3d.js @@ -222,13 +222,15 @@ const state = { joint_ucses: [], // array di THREE.Group, UCS per ogni link URDF (debug) clouds_cfg: null, // {nuwa:{...}, d435:{...}} dal config cloud_points: { nuwa: null, d435: null }, // THREE.Points obj per camera + align: { enabled: false, id: 0, matrix: null /* THREE.Matrix4 */ }, tag_locator: { // sezione "Tag locator" nella sidebar Pose 3D marker: null, // THREE.LineSegments (crocetta XYZ) o null position_m: null, // [x,y,z] in pelvis frame della crocetta corrente joints_at_place: null, // snapshot giunti al click (può essere null se G1 offline) }, - front_tags: { // overlay tag visibili dalla camera D435i frontale - group: null, // THREE.Group child del d435 parent (cloud-like transform) + tags: { // overlay tag per camera (gruppi THREE child del cloud node) + nuwa: { group: null }, + d435: { group: null }, }, }; @@ -782,6 +784,7 @@ async function pollState() { updateNuwaInputsFromCfg(); updateD435InputsFromCfg(); applyCloudParamsFromCfg(s.cloud_params); + applyCrossCameraFromCfg(s.cross_camera); applyCalibratedD435UCS(); // Applica TUTTI i giunti reali sul modello URDF (29 motori G1) state.joints = s.joints || null; // snapshot per tag locator save @@ -1604,8 +1607,12 @@ function buildOrUpdateCloud(camKey, pointsFlat) { `Y[${ymin.toFixed(2)},${ymax.toFixed(2)}]`, `Z[${zmin.toFixed(2)},${zmax.toFixed(2)}]`); // (Ri-)attacca al parent corretto e applica transform optical→link - if (pts.parent !== parent) parent.add(pts); - applyCloudTransform(camKey); + if (camKey === 'nuwa') { + applyNuwaCloudPlacement(); + } else { + if (pts.parent !== parent) parent.add(pts); + applyCloudTransform(camKey); + } applyCloudVisibility(); return pts; } @@ -1620,6 +1627,24 @@ function applyCloudVisibility() { // Applica position+rpy+mirror sui punti del cloud `camKey`. Il mirror (sign // flip per asse) avviene IN FRAME OTTICO grazie all'ordine T·R·S di Three.js // (lo scale e' la prima operazione applicata al punto locale). +// Matrix4 locale (rispetto al parent getCloudParent('d435')) usata dal cloud +// D435i: hand-eye (M·S) oppure URDF (T·R·S). Riusata per piazzare la Nuwa +// allineata nello stesso frame del D435i. +function d435LocalMatrix() { + const sv = readCloudScaleVec('d435'); + const S = new THREE.Matrix4().makeScale(sv[0], sv[1], sv[2]); + if (useD435Handeye()) { + const M = getD435HandeyeMatrix(); + if (M) return M.clone().multiply(S); + } + const t = getCloudTransform('d435'); + const T = new THREE.Matrix4().makeTranslation(t.offset_m[0], t.offset_m[1], t.offset_m[2]); + const e = new THREE.Euler( + t.rpy_deg[0]*Math.PI/180, t.rpy_deg[1]*Math.PI/180, t.rpy_deg[2]*Math.PI/180, 'XYZ'); + const R = new THREE.Matrix4().makeRotationFromEuler(e); + return T.multiply(R).multiply(S); +} + function applyCloudTransform(camKey) { const pts = state.cloud_points[camKey]; if (!pts) return; @@ -1651,6 +1676,34 @@ function applyCloudTransform(camKey) { pts.scale.set(sv[0], sv[1], sv[2]); } +// Piazza la nuvola Nuwa: in modalità allineata la renderizza nel frame del +// D435i (parent + pipeline D435i · matrice di allineamento); altrimenti usa il +// piazzamento URDF normale. +function applyNuwaCloudPlacement() { + const pts = state.cloud_points.nuwa; + if (!pts) return; + const aligned = state.align.enabled && state.align.matrix; + if (aligned) { + const parent = getCloudParent('d435'); + if (parent) { + if (pts.parent !== parent) parent.add(pts); + pts.matrixAutoUpdate = false; + pts.matrix.copy(d435LocalMatrix()).multiply(state.align.matrix); + pts.matrixWorldNeedsUpdate = true; + return; + } + } + if (state.align.enabled && !state.align.matrix) { + const s = $('p3AlignStatus'); + if (s) { s.textContent = 'nessuna calibrazione: premi Allinea'; s.style.color = 'var(--dim)'; } + } + // fallback URDF normale + const parent = getCloudParent('nuwa'); + if (parent && pts.parent !== parent) parent.add(pts); + pts.matrixAutoUpdate = true; + applyCloudTransform('nuwa'); +} + function readCloudFlip(camKey) { const tag = camKey === 'nuwa' ? 'Nuwa' : 'D435'; return [ @@ -1767,7 +1820,7 @@ async function acquireClouds() { saveCloudParamsToConfig(); // Nuova immagine acquisita: se l'overlay tag e' attivo ri-esegui la // detection una volta sola, sul frame appena acquisito. - if ($('p3ShowFrontTags')?.checked) pollFrontTags(); + if ($('p3ShowFrontTags')?.checked) pollTags(); } catch (e) { $('p3CloudStatus').textContent = `errore: ${e.message}`; $('p3CloudStatus').style.color = 'var(--danger)'; @@ -1795,6 +1848,8 @@ function clearClouds() { // camera in modalita' URDF link e in modalita' hand-eye. const FRONT_TAG_COLOR = 0x4dd0e1; +const NUWA_TAG_COLOR = 0xff3b30; // rosso: tag visti dalla camera Nuwa (polso) +const TAG_COLORS = { nuwa: NUWA_TAG_COLOR, d435: FRONT_TAG_COLOR }; const FRONT_TAG_LABEL_COLOR = '#4dd0e1'; // Label compatta auto-sized: canvas largo quanto il testo, font piccolo, @@ -1854,8 +1909,8 @@ function applyCloudLikeTransform(obj, camKey) { obj.scale.set(sv[0], sv[1], sv[2]); } -function clearFrontTagsGroup() { - const grp = state.front_tags.group; +function clearTagsGroup(camKey) { + const grp = state.tags[camKey].group; if (!grp) return; if (grp.parent) grp.parent.remove(grp); grp.traverse(o => { @@ -1865,78 +1920,137 @@ function clearFrontTagsGroup() { else o.material.dispose(); } }); - state.front_tags.group = null; + state.tags[camKey].group = null; } -function addOneFrontTag(grp, t) { +function addOneTag(grp, t, color) { const c = t.corners_3d_optical_m; if (!Array.isArray(c) || c.length !== 4) return; if (c.some(p => !p || p.some(v => !Number.isFinite(v)))) return; - - // LineLoop (5 verts = close) const v = new Float32Array(5 * 3); for (let i = 0; i < 4; i++) { v[3*i]=c[i][0]; v[3*i+1]=c[i][1]; v[3*i+2]=c[i][2]; } v[12] = c[0][0]; v[13] = c[0][1]; v[14] = c[0][2]; const geom = new THREE.BufferGeometry(); geom.setAttribute('position', new THREE.BufferAttribute(v, 3)); - const mat = new THREE.LineBasicMaterial({ color: FRONT_TAG_COLOR }); - grp.add(new THREE.Line(geom, mat)); - - // Label "id=X · NNmm · d=DDDmm" leggermente offset dal centro + grp.add(new THREE.Line(geom, new THREE.LineBasicMaterial({ color }))); const edge_s = Number.isFinite(t.edge_mm) ? `${t.edge_mm.toFixed(1)}mm` : '?'; const dist_s = Number.isFinite(t.distance_m) ? `${(t.distance_m * 1000).toFixed(0)}mm` : '?'; const sprite = makeCompactLabel(`id=${t.id} · ${edge_s} · d=${dist_s}`); const ctr = t.center_3d_optical_m; - if (ctr && ctr.every(Number.isFinite)) { - // offset in -Y ottico = sopra (Y down nella convenzione OpenCV) - sprite.position.set(ctr[0], ctr[1] - 0.04, ctr[2]); - } + if (ctr && ctr.every(Number.isFinite)) sprite.position.set(ctr[0], ctr[1] - 0.04, ctr[2]); grp.add(sprite); } -function renderFrontTags(tags) { - clearFrontTagsGroup(); +function renderTags(camKey, tags, color) { + clearTagsGroup(camKey); if (!tags || tags.length === 0) return; - const parent = getCloudParent('d435'); - if (!parent) { - console.warn('[pose_3d] front_tags: parent d435 non disponibile'); - return; - } + const parent = getCloudParent(camKey); + if (!parent) { console.warn(`[pose_3d] tags: parent ${camKey} non disponibile`); return; } const grp = new THREE.Group(); - grp.name = 'front_tags_group'; - applyCloudLikeTransform(grp, 'd435'); + grp.name = `${camKey}_tags_group`; + applyCloudLikeTransform(grp, camKey); parent.add(grp); - state.front_tags.group = grp; - for (const t of tags) addOneFrontTag(grp, t); + state.tags[camKey].group = grp; + for (const t of tags) addOneTag(grp, t, color); } -async function pollFrontTags() { +async function pollTags() { const ids = ($('p3FrontTagIds')?.value || '0-49').trim(); const status = $('p3FrontTagStatus'); try { - const r = await jsonFetch(`/api/pose_3d/front_tags?ids=${encodeURIComponent(ids)}`); - const tags = r.tags || []; - renderFrontTags(tags); - if (status) { - const ms = r.timings_ms?.total; - status.textContent = tags.length - ? `${tags.length} tag · ids: ${tags.map(t => t.id).join(',')}${ms != null ? ' · ' + ms.toFixed(0) + 'ms' : ''}` - : 'nessun tag visibile'; + const r = await jsonFetch(`/api/pose_3d/tags?ids=${encodeURIComponent(ids)}`); + const cams = r.cameras || {}; + const parts = []; + for (const camKey of ['nuwa', 'd435']) { + const c = cams[camKey] || {}; + renderTags(camKey, c.tags || [], TAG_COLORS[camKey]); + if (c.error) parts.push(`${camKey}: err`); + else parts.push(`${camKey}: ${(c.tags || []).length}`); } + if (status) status.textContent = parts.join(' · '); } catch (e) { if (status) status.textContent = `errore: ${e?.message || e}`; } } +// Costruisce una THREE.Matrix4 (row-major via .set) da {scale,R,t}: p' = s·R·p + t. +function matrixFromAlign(cc) { + if (!cc || !Array.isArray(cc.R) || cc.R.length !== 3 || !Array.isArray(cc.t)) return null; + const s = Number.isFinite(cc.scale) ? cc.scale : 1; + const R = cc.R, t = cc.t; + return new THREE.Matrix4().set( + s*R[0][0], s*R[0][1], s*R[0][2], t[0], + s*R[1][0], s*R[1][1], s*R[1][2], t[1], + s*R[2][0], s*R[2][1], s*R[2][2], t[2], + 0, 0, 0, 1, + ); +} + +async function doAlign() { + const ids = ($('p3FrontTagIds')?.value || '0-49').trim(); + const alignId = parseInt($('p3AlignTagId')?.value, 10) || 0; + const status = $('p3AlignStatus'); + if (status) { status.textContent = 'allineamento…'; status.style.color = 'var(--dim)'; } + try { + const r = await jsonFetch( + `/api/pose_3d/align?ids=${encodeURIComponent(ids)}&align_id=${alignId}`, + { method: 'POST' }); + if (!r.found_in_both) { + if (status) { status.textContent = r.error || 'tag non visto da entrambe'; status.style.color = 'var(--danger)'; } + return; + } + state.align.matrix = matrixFromAlign(r); + state.align.id = r.align_id; + if (status) { + status.textContent = `scala=${r.scale.toFixed(3)} · rmse=${r.rmse_mm.toFixed(1)}mm · n=${r.n_corners}`; + status.style.color = 'var(--ok)'; + } + applyNuwaCloudPlacement(); + } catch (e) { + if (status) { status.textContent = `errore: ${e?.message || e}`; status.style.color = 'var(--danger)'; } + } +} + +// Persistenza del solo flag + align_id (la matrice la salva il backend in /align). +async function saveCrossCameraFlag() { + try { + await jsonFetch('/api/config', { + method: 'PUT', + headers: { 'Content-Type': 'application/json' }, + body: JSON.stringify({ pose_3d: { cross_camera: { + enabled: $('p3AlignEnabled')?.checked === true, + align_id: parseInt($('p3AlignTagId')?.value, 10) || 0, + } } }), + }); + } catch (e) { console.warn('[pose_3d] save cross_camera failed:', e?.message || e); } +} + +// Ripopola UI + matrice dalla config (guardato come applyCloudParamsFromCfg). +function applyCrossCameraFromCfg(cc) { + if (state.cross_camera_populated) return; + if (!cc || Object.keys(cc).length === 0) { state.cross_camera_populated = true; return; } + const en = $('p3AlignEnabled'); if (en) en.checked = cc.enabled === true; + const id = $('p3AlignTagId'); if (id && Number.isFinite(cc.align_id)) id.value = String(cc.align_id); + state.align.enabled = cc.enabled === true; + state.align.id = cc.align_id || 0; + const m = matrixFromAlign(cc); + if (m) state.align.matrix = m; + const status = $('p3AlignStatus'); + if (status && Number.isFinite(cc.rmse_mm)) { + status.textContent = `caricata · scala=${(cc.scale||1).toFixed(3)} · rmse=${cc.rmse_mm.toFixed(1)}mm`; + } + state.cross_camera_populated = true; +} + function bindFrontTagFinder() { const cb = $('p3ShowFrontTags'); if (!cb) return; cb.addEventListener('change', () => { if (cb.checked) { - // Refresh immediato senza aspettare il prossimo tick di pollState - pollFrontTags(); + pollTags(); } else { - clearFrontTagsGroup(); + clearTagsGroup('nuwa'); + clearTagsGroup('d435'); const s = $('p3FrontTagStatus'); if (s) s.textContent = '—'; } @@ -1946,6 +2060,13 @@ function bindFrontTagFinder() { function bindCloudControls() { $('p3CloudAcquireBtn')?.addEventListener('click', acquireClouds); + $('p3AlignBtn')?.addEventListener('click', doAlign); + $('p3AlignEnabled')?.addEventListener('change', () => { + state.align.enabled = $('p3AlignEnabled').checked === true; + saveCrossCameraFlag(); + applyNuwaCloudPlacement(); + }); + $('p3AlignTagId')?.addEventListener('change', saveCrossCameraFlag); $('p3CloudClearBtn')?.addEventListener('click', clearClouds); $('p3ShowNuwaCloud')?.addEventListener('change', applyCloudVisibility); $('p3ShowD435Cloud')?.addEventListener('change', applyCloudVisibility);