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>
This commit is contained in:
AdrianoandClaude Opus 4.8 committed
commit
8084b87261
10 files changed
+598
-144
No files matched your search
@@ -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.
|
||||
@@ -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).
|
||||
|
||||
|
||||
@@ -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 ---
|
||||
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -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", {})
|
||||
@@ -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
|
||||
@@ -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
|
||||
+7
-1
@@ -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>
|
||||
|
||||
+163
-42
@@ -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);
|
||||
|
||||
Reference in new issue
Block a user