Merge: allineamento cross-camera + overlay tag multi-camera (Pose 3D)

- umeyama_similarity (geometry)
- GET /api/pose_3d/tags (Nuwa + D435i), rimosso /front_tags
- POST /api/pose_3d/align: fit Umeyama + persistenza pose_3d.cross_camera
- /state espone cross_camera
- overlay tag su entrambe le camere (Nuwa rosso, D435i ciano)
- UI Allinea cross-camera + render Nuwa nel frame D435i
- docs

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>

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.

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).
 

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
@@ -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 ----
+# --- Multi-camera tag finder: detect + misura indipendente via depth ----
-# Scansiona ArUco DICT_4X4_50 sulla RGB del D435i frontale e per ogni tag
+# Scansiona ArUco DICT_4X4_50 su tutte le camere attive (Nuwa polso DX e
-# trovato misura i 4 corner 3D usando il depth sensor (independent da pnp).
+# D435i frontale). Per ogni tag trovato misura i 4 corner 3D via depth sensor
-# Output in frame ottico OpenCV (X right, Y down, Z forward) — il client
+# (indipendente da solvePnP). Output in frame ottico OpenCV per ciascuna camera.
-# lo agganciera' al link URDF del d435 nella scena 3D.
+
+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 _detect_tags_on(client, frame_label: str, ids: str,
-def get_front_tags(ids: str = "0-49", probe_size_mm: float = 40.0) -> dict[str, Any]:
+                    probe_size_mm: float = 40.0) -> list[dict]:
-    """Detect ArUco markers via front D435i + misura geometria 3D reale.
+    """Detect ArUco markers su `client` (qualsiasi NuwaClient) e misura i 4
-
+    corner + centro in 3D via depth sensor (indipendente da solvePnP).
-    Per ogni tag trovato chiama /api/measure ai 4 corner per ottenere i punti
+    Ritorna una lista di dict per tag (id, corners_pixel, corners_3d_optical_m,
-    3D direttamente dal depth sensor (indipendente da solvePnP). Cosi' la
+    corner_depths_mm, center_3d_optical_m, edge_mm_per_side, edge_mm, distance_m).
-    dimensione misurata non dipende dalla size assunta in input.
+    `frame_label` è solo per i log; i punti sono nel frame ottico OpenCV della camera."""
-
-    `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()
     mk = _deps.marker()
-
+    intr = client.intrinsics()
-    # 1) intrinsics RGB del D435i → passate a marker-finder per pose corrette
-    with sw.step("intrinsics"):
-        intr = front.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,16 +302,11 @@ 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=client.rgb_url(), intrinsics=K)
-        det = mk.detect_url(markers=markers, image_url=front.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:
@@ -321,7 +315,7 @@ def get_front_tags(ids: str = "0-49", probe_size_mm: float = 40.0) -> dict[str,
         depths: list[Optional[int]] = []
         try:
             for (u, v) in corners:
-                    md = front.measure(int(round(u)), int(round(v)), radius=2)
+                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):
@@ -332,38 +326,26 @@ def get_front_tags(ids: str = "0-49", probe_size_mm: float = 40.0) -> dict[str,
                     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)
+            cmd = client.measure(int(round(cu)), int(round(cv)), radius=4)
             cpt = (cmd or {}).get("point_m") or {}
-                center3d = (
+            center3d = ([float(cpt["x"]), float(cpt["y"]), float(cpt["z"])]
-                    [float(cpt["x"]), float(cpt["y"]), float(cpt["z"])]
+                        if cpt else [float("nan")] * 3)
-                    if cpt else [float("nan")] * 3
-                )
         except Exception as e:
-                log.warning("front_tags: measure failed for id=%s: %s", m.get("id"), 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
+                    pts3d[i] = [float("nan")] * 3
 
-            # 3D edge lengths (mm) — robusti a un corner senza depth (NaN salta)
+        edges_mm = [_edge_len_mm(pts3d[i], pts3d[(i + 1) % 4]) for i in range(4)]
-            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 = [_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
 
@@ -381,14 +363,98 @@ def get_front_tags(ids: str = "0-49", probe_size_mm: float = 40.0) -> dict[str,
             "edge_mm": edge_mean,
             "distance_m": distance_m,
         })
+    return tags_out
 
-    result = {
+
-        "tags": tags_out,
+# Map: camera key → (frame_label, _deps attribute name).
-        "not_found_count": int(len(det.get("not_found", []))),
+# Usiamo il nome stringa dell'attributo su _deps e lo risolviamo via getattr
-        "frame": "d435_rgb_optical",  # OpenCV: X right, Y down, Z forward
+# a ogni richiesta: cosi' patch("...pose_3d_routes._deps.nuwa") viene visto.
-        "timings_ms": sw.as_dict(),
+_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 ---

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

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)

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", {})

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

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

web/index.html

@@ -449,6 +449,12 @@
             <input id="p3D435ScaleXY" type="number" min="0.1" max="3" step="0.01" value="0.65" style="width:60px">
           </label>
         </div>
+        <div class="p3-row" title="Calibrazione persistente Nuwa→frontale via tag condiviso (Umeyama). Premi Allinea con il tag visto da entrambe le camere.">
+          <label><input type="checkbox" id="p3AlignEnabled"> Allinea Nuwa→frontale</label>
+          <label>tag id <input id="p3AlignTagId" type="number" min="0" max="999" value="0" style="width:55px"></label>
+          <button id="p3AlignBtn">Allinea</button>
+        </div>
+        <div class="marker-info" id="p3AlignStatus">—</div>
         <div class="p3-row">
           <label>step <input id="p3CloudStep" type="number" min="1" max="32" value="8" style="width:50px"></label>
           <label>max <input id="p3CloudMaxDepth" type="number" min="100" max="5000" step="100" value="2000" style="width:70px"> mm</label>
@@ -511,7 +517,7 @@
       </div>
 
       <div class="p3-section">
-        <h3>Tag camera frontale</h3>
+        <h3>Tags</h3>
         <div class="p3-row">
           <label><input type="checkbox" id="p3ShowFrontTags"> Mostra tag visibili (quad + dim + dist)</label>
         </div>

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
+  tags: {                  // overlay tag per camera (gruppi THREE child del cloud node)
-    group: null,           //   THREE.Group child del d435 parent (cloud-like transform)
+    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 (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() {
+function clearTagsGroup(camKey) {
-  const grp = state.front_tags.group;
+  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, new THREE.LineBasicMaterial({ color })));
-  grp.add(new THREE.Line(geom, mat));
-
-  // Label "id=X · NNmm · d=DDDmm" leggermente offset dal centro
   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)) {
+  if (ctr && ctr.every(Number.isFinite)) sprite.position.set(ctr[0], ctr[1] - 0.04, ctr[2]);
-    // offset in -Y ottico = sopra (Y down nella convenzione OpenCV)
-    sprite.position.set(ctr[0], ctr[1] - 0.04, ctr[2]);
-  }
   grp.add(sprite);
 }
 
-function renderFrontTags(tags) {
+function renderTags(camKey, tags, color) {
-  clearFrontTagsGroup();
+  clearTagsGroup(camKey);
   if (!tags || tags.length === 0) return;
-  const parent = getCloudParent('d435');
+  const parent = getCloudParent(camKey);
-  if (!parent) {
+  if (!parent) { console.warn(`[pose_3d] tags: parent ${camKey} non disponibile`); return; }
-    console.warn('[pose_3d] front_tags: parent d435 non disponibile');
-    return;
-  }
   const grp = new THREE.Group();
-  grp.name = 'front_tags_group';
+  grp.name = `${camKey}_tags_group`;
-  applyCloudLikeTransform(grp, 'd435');
+  applyCloudLikeTransform(grp, camKey);
   parent.add(grp);
-  state.front_tags.group = grp;
+  state.tags[camKey].group = grp;
-  for (const t of tags) addOneFrontTag(grp, t);
+  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 r = await jsonFetch(`/api/pose_3d/tags?ids=${encodeURIComponent(ids)}`);
-    const tags = r.tags || [];
+    const cams = r.cameras || {};
-    renderFrontTags(tags);
+    const parts = [];
-    if (status) {
+    for (const camKey of ['nuwa', 'd435']) {
-      const ms = r.timings_ms?.total;
+      const c = cams[camKey] || {};
-      status.textContent = tags.length
+      renderTags(camKey, c.tags || [], TAG_COLORS[camKey]);
-        ? `${tags.length} tag · ids: ${tags.map(t => t.id).join(',')}${ms != null ? ' · ' + ms.toFixed(0) + 'ms' : ''}`
+      if (c.error) parts.push(`${camKey}: err`);
-        : 'nessun tag visibile';
+      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
+      pollTags();
-      pollFrontTags();
     } 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);