diff --git a/src/presa_mano/api/pose_3d_routes.py b/src/presa_mano/api/pose_3d_routes.py index 174dd9e..2745bb5 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 @@ -391,6 +392,70 @@ def get_tags(ids: str = "0-49", probe_size_mm: float = 40.0) -> dict[str, Any]: return {"cameras": cameras, "timings_ms": sw.as_dict()} +@router.post("/align") +def post_align(ids: str = "0-49", align_id: int = 0, + probe_size_mm: float = 40.0) -> dict[str, Any]: + """Calibrazione cross-camera: trova `align_id` su Nuwa e D435i, fitta una + similarita' (rot+trasl+scala) Nuwa-optical -> D435-optical e la PERSISTE in + config.yaml > pose_3d.cross_camera. Se il tag non e' visto da entrambe, NON + sovrascrive la matrice salvata.""" + sw = Stopwatch() + found: dict[str, Optional[dict]] = {} + cam_errors: dict[str, str] = {} + for key, (frame_label, dep_name) in _TAG_CAMERAS.items(): + try: + factory = getattr(_deps, dep_name) + with sw.step(key): + tags = _detect_tags_on(factory(), frame_label, ids, probe_size_mm) + found[key] = next((t for t in tags if int(t["id"]) == int(align_id)), None) + except Exception as e: + log.warning("align: camera %s failed: %s", key, e) + found[key] = None + cam_errors[key] = f"{type(e).__name__}: {e}" + + missing = [k for k, v in found.items() if v is None] + if missing: + if cam_errors: + detail = "; ".join(f"{k}: {msg}" for k, msg in cam_errors.items()) + err = f"camera non disponibile -> {detail}" + else: + err = f"tag {align_id} non visto da: {', '.join(missing)}" + return {"found_in_both": False, "align_id": int(align_id), + "error": err, "timings_ms": sw.as_dict()} + + # I corner sono accoppiati posizionalmente: stesso marker-finder + stessa + # geometria ArUco -> ordine corner deterministico e identico tra le camere. + src, dst = [], [] + for cn, cd in zip(found["nuwa"]["corners_3d_optical_m"], + found["d435"]["corners_3d_optical_m"]): + if any(x != x for x in cn) or any(x != x for x in cd): + continue + src.append(cn) + dst.append(cd) + if len(src) < 3: + return {"found_in_both": False, "align_id": int(align_id), + "error": f"corner validi insufficienti ({len(src)}/4)", + "timings_ms": sw.as_dict()} + + try: + with sw.step("umeyama"): + scale, R, t, rmse = umeyama_similarity(np.array(src), np.array(dst)) + except ValueError as e: + return {"found_in_both": False, "align_id": int(align_id), + "error": f"fit fallito: {e}", "timings_ms": sw.as_dict()} + + payload = { + "align_id": int(align_id), + "scale": float(scale), + "R": [[float(x) for x in row] for row in R], + "t": [float(x) for x in t], + "rmse_mm": float(rmse * 1000.0), + "n_corners": len(src), + } + update_config({"pose_3d": {"cross_camera": payload}}) + return {"found_in_both": True, **payload, "timings_ms": sw.as_dict()} + + # --- Tag locator: piazza crocetta nella scena 3D e salva la posa in pelvis --- # Workflow utente (vedi web/pose_3d.js sezione "tag locator"): # 1) seleziona tag da dropdown (id+size+label) 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", {})