Compare commits

..
18 Commits
Author SHA1 Message Date
AdrianoandClaude Opus 4.8 c383fd3143 feat(version): mostra build time nel badge (non solo tooltip); bump 1.0.2
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 14:49:22 +02:00
AdrianoandClaude Opus 4.8 054ad103df chore: bump VERSION 1.0.0 -> 1.0.1 (deploy)
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 14:44:25 +02:00
AdrianoandClaude Opus 4.8 7b4dc6ce8c docs: build.sh deploy workflow + badge versione (CLAUDE.md, README)
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 14:43:07 +02:00
AdrianoandClaude Opus 4.8 cf726b479e feat: badge versione build in GUI + build.sh con auto-bump
- VERSION (1.0.0) + build.sh: bump patch/minor/major e deploy via docker compose
- Dockerfile: ARG/ENV APP_VERSION + BUILD_TIME (iniettati come build-arg)
- version.build_info(): env in Docker, fallback file VERSION + '(local)' in dev
- GET /api/version; badge #appVersionPill nell'header (tooltip = build time)
- test_version.py: env vs fallback

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 14:40:52 +02:00
AdrianoandClaude Opus 4.8 a88a064aa8 docs: spec badge versione build in GUI
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 14:40:52 +02:00
AdrianoandClaude Opus 4.8 8084b87261 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>
2026-06-03 13:58:06 +02:00
AdrianoandClaude Opus 4.8 0ee3ef244b chore(pose_3d): tieni config.yaml allineato a main (drift runtime non versionato)
Il commit della Task 1 aveva incluso per errore (via git add -u) drift runtime
di config.yaml (cloud_params size/flip) persistito dall'app live. Lo rimuovo dal
branch: config.yaml resta uguale a main. I valori live restano come modifica
non committata nel working tree.

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 12:43:34 +02:00
AdrianoandClaude Sonnet 4.6 d9213ca19a docs(pose_3d): tags multi-camera + calibrazione cross-camera
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-06-03 12:41:12 +02:00
AdrianoandClaude Opus 4.8 47ae8a6b2c feat(pose_3d): doAlign + persistenza/caricamento calibrazione cross-camera
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 12:38:25 +02:00
AdrianoandClaude Opus 4.8 746662d534 feat(pose_3d): piazzamento Nuwa allineato nel frame D435i (matrice)
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 12:35:30 +02:00
AdrianoandClaude Sonnet 4.6 2f0dc69cdd feat(pose_3d): rinomina sezione Tags + controlli Allinea cross-camera
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-06-03 12:33:55 +02:00
AdrianoandClaude Sonnet 4.6 edc4430c67 feat(pose_3d): overlay tag su entrambe le camere (Nuwa rosso, D435i ciano)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-06-03 12:31:16 +02:00
AdrianoandClaude Sonnet 4.6 79f587567e feat(pose_3d): /state espone blocco cross_camera
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-06-03 12:28:52 +02:00
AdrianoandClaude Sonnet 4.6 5ab808accd feat(pose_3d): POST /align fit Umeyama cross-camera + persistenza
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-06-03 12:28:05 +02:00
AdrianoandClaude Sonnet 4.6 8b01ae123f feat(pose_3d): GET /tags multi-camera, rimuovi /front_tags
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-06-03 12:20:04 +02:00
AdrianoandClaude Sonnet 4.6 77fb0f8fc0 feat(geometry): umeyama_similarity (rot+trasl+scala uniforme)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-06-03 12:13:25 +02:00
AdrianoandClaude Opus 4.8 9189700f34 docs(pose_3d): piano implementazione allineamento cross-camera
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 12:06:25 +02:00
AdrianoandClaude Opus 4.8 80625cc281 docs(pose_3d): spec allineamento cross-camera + overlay tag multi-camera
Design per: detection tag su tutte le camere attive (Nuwa rosso + D435i),
rinomina "Tag camera frontale" -> "Tags", calibrazione persistente
Nuwa->frontale via tag condiviso (Umeyama similarity), flag+pulsante
"Allinea" nella sezione Cloud cross-camera.

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-06-03 12:02:26 +02:00
20 changed files with 2001 additions and 100 deletions

No files matched your search

+5 -1
View File
@@ -9,6 +9,8 @@ Build & run (Docker):
docker compose up -d presa-mano # serves GUI + API on http://localhost:8890 docker compose up -d presa-mano # serves GUI + API on http://localhost:8890
``` ```
**Deploy changes — use `./build.sh`.** `src/` and `web/` are baked into the image (only `config.yaml` + `data/` are bind-mounted), so **code edits are invisible until the image is rebuilt**. `./build.sh` auto-bumps `VERSION` (patch by default; `./build.sh minor|major` for the others), injects version + build time as build-args, rebuilds, and restarts the container. The running version is shown as a badge in the GUI header (and at `GET /api/version`) so a stale deployment is visible at a glance. The `VERSION` bump is left uncommitted — commit it with your changes.
Local dev (uses `uv`, same as the Dockerfile): Local dev (uses `uv`, same as the Dockerfile):
```bash ```bash
uv sync # install deps from pyproject.toml uv sync # install deps from pyproject.toml
@@ -17,13 +19,14 @@ uv run uvicorn presa_mano.main:app --host 0.0.0.0 --port 8890 --reload
Useful endpoints (all under `http://localhost:8890`): Useful endpoints (all under `http://localhost:8890`):
- `GET /apidocs` — Swagger UI; `GET /redoc` — ReDoc. - `GET /apidocs` — Swagger UI; `GET /redoc` — ReDoc.
- `GET /health` — liveness; `GET /api/version` — `{version, build_time}` baked at build by `build.sh` (falls back to the `VERSION` file + `(local)` under `uv run`).
- `GET/PUT /api/config` — read/patch runtime config (deep-merged into `config.yaml` and persisted). - `GET/PUT /api/config` — read/patch runtime config (deep-merged into `config.yaml` and persisted).
- Calibration: `POST /api/calibration/sample`, `GET/DELETE /api/calibration/samples`, `POST /api/calibration/solve`, `GET /api/calibration/result`, `POST /api/calibration/preview`. - Calibration: `POST /api/calibration/sample`, `GET/DELETE /api/calibration/samples`, `POST /api/calibration/solve`, `GET /api/calibration/result`, `POST /api/calibration/preview`.
- Perception: `POST /api/perception/detect`, `POST /api/perception/object_points`, `GET /api/perception/measure`. - Perception: `POST /api/perception/detect`, `POST /api/perception/object_points`, `GET /api/perception/measure`.
- Grasp: `POST /api/grasp/plan`, `POST /api/grasp/execute` (set `dry_run` to gate G1 mutations). - 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}`. - 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}`. - 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. Tests live under `tests/` and run with `uv run pytest`. No linter is configured.
@@ -94,6 +97,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 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 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 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)`. - 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()`. - 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. - Italian is fine in user-facing copy (GUI, README, comments) — it's the project default.
+8 -1
View File
@@ -12,13 +12,20 @@ COPY pyproject.toml README.md ./
COPY src/ src/ COPY src/ src/
RUN uv sync --no-dev RUN uv sync --no-dev
COPY config.yaml ./ COPY config.yaml VERSION ./
COPY web/ web/ COPY web/ web/
ENV PRESA_MANO_CONFIG=/app/config.yaml ENV PRESA_MANO_CONFIG=/app/config.yaml
ENV PRESA_MANO_DATA=/app/data ENV PRESA_MANO_DATA=/app/data
ENV PRESA_MANO_PORT=8890 ENV PRESA_MANO_PORT=8890
# Versione build: iniettata da build.sh come --build-arg. Cambia ad ogni build
# (il valore diverso invalida solo questi due layer finali, non le dipendenze).
ARG APP_VERSION=dev
ARG BUILD_TIME=unknown
ENV APP_VERSION=${APP_VERSION}
ENV BUILD_TIME=${BUILD_TIME}
EXPOSE 8890 EXPOSE 8890
# Shell form so $PRESA_MANO_PORT viene espanso a runtime: cambi la porta in # Shell form so $PRESA_MANO_PORT viene espanso a runtime: cambi la porta in
+15
View File
@@ -35,6 +35,18 @@ docker compose up -d presa-mano
# Apri http://localhost:8890 # Apri http://localhost:8890
``` ```
## Build & deploy delle modifiche
`src/` e `web/` sono inclusi (baked) nell'immagine: le modifiche al codice **non si vedono finché non ricostruisci l'immagine**. Usa lo script:
```bash
./build.sh # patch++ (1.0.1 -> 1.0.2), rebuild + restart
./build.sh minor # 1.0.2 -> 1.1.0
./build.sh major # 1.1.0 -> 2.0.0
```
Lo script bumpa il file `VERSION`, inietta versione + ora di build nell'immagine e riavvia il container. La versione attiva è mostrata nel **badge in alto a destra** nella GUI (tooltip con l'ora di build) ed è esposta da `GET /api/version` — così capisci subito se il container gira l'ultima build. Il bump di `VERSION` resta non committato: committalo con le tue modifiche.
## GUI ## GUI
Tab principali: Tab principali:
@@ -56,6 +68,9 @@ Tab principali:
- **Pose 3D**: viewport Three.js con URDF G1 specchiato sui giunti reali - **Pose 3D**: viewport Three.js con URDF G1 specchiato sui giunti reali
(`/diagnostics`), point cloud Nuwa + D435i, modello 3D camera sul polso, (`/diagnostics`), point cloud Nuwa + D435i, modello 3D camera sul polso,
gizmo traslazione/rotazione, editor pose (Apply/Jog), named poses salvabili. 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, La tab resta utilizzabile anche senza robot (mostra il modello URDF, config,
pose salvate). pose salvate).
+1
View File
@@ -0,0 +1 @@
1.0.2
Executable
+40
View File
@@ -0,0 +1,40 @@
#!/usr/bin/env bash
# Build + deploy di presa-mano con auto-bump della versione.
#
# ./build.sh → patch++ (1.0.1 -> 1.0.2)
# ./build.sh minor → minor++ (1.0.2 -> 1.1.0)
# ./build.sh major → major++ (1.1.0 -> 2.0.0)
#
# La nuova versione + l'ora di build vengono iniettate nell'immagine come
# build-arg e mostrate nel badge in GUI. Il file VERSION viene riscritto ma NON
# committato automaticamente (lo committi tu con le altre modifiche).
set -euo pipefail
SCRIPT_DIR="$(cd "$(dirname "$0")" && pwd)"
COMPOSE_DIR="$(dirname "$SCRIPT_DIR")" # docker-compose.yml sta un livello sopra
VERSION_FILE="$SCRIPT_DIR/VERSION"
[ -f "$VERSION_FILE" ] || echo "1.0.0" > "$VERSION_FILE"
cur="$(tr -d '[:space:]' < "$VERSION_FILE")"
IFS='.' read -r major minor patch <<< "$cur"
major="${major:-1}"; minor="${minor:-0}"; patch="${patch:-0}"
case "${1:-patch}" in
major) major=$((major + 1)); minor=0; patch=0 ;;
minor) minor=$((minor + 1)); patch=0 ;;
patch) patch=$((patch + 1)) ;;
*) echo "uso: $0 [major|minor|patch]" >&2; exit 1 ;;
esac
new="${major}.${minor}.${patch}"
echo "$new" > "$VERSION_FILE"
build_time="$(date -u +"%Y-%m-%d %H:%M UTC")"
echo "→ build V${new} (${build_time})"
cd "$COMPOSE_DIR"
docker compose build \
--build-arg APP_VERSION="$new" \
--build-arg BUILD_TIME="$build_time" \
presa-mano
docker compose up -d presa-mano
echo "✓ deployed V${new} (${build_time})"
File diff suppressed because it is too large. Load diff
@@ -0,0 +1,153 @@
# Cross-camera tag overlay + Nuwa→frontale alignment calibration
**Date:** 2026-06-03
**Status:** approved (design)
**Area:** Pose 3D tab — `web/pose_3d.js`, `web/index.html`, `src/presa_mano/api/pose_3d_routes.py`, `src/presa_mano/geometry.py`
## Problem
The Pose 3D tab detects ArUco tags only on the **front D435i** camera (`GET /api/pose_3d/front_tags`) and draws them as an overlay. We want:
1. Tag detection on **all active cameras** (Nuwa wrist eye-in-hand + D435i front), not just the front one. Nuwa tags drawn in a **distinct red** color; D435i keeps its current color.
2. The "Tag camera frontale" UI block renamed to **"Tags"** (it now covers every active camera).
3. A **cross-camera alignment calibration**: a configurable `align_id` tag that must be seen by **both** cameras lets us fit a similarity transform that maps the Nuwa point cloud onto the D435i cloud (rotation + translation + uniform scale). This is a **persistent calibration** (saved, reused across sessions), gated by an enable/disable flag in the "Cloud cross-camera" section.
## Decisions (locked)
- **Recompute trigger:** dedicated **"Allinea"** button. The fitted matrix is frozen until the button is pressed again. Acquisisci does *not* recompute it.
- **Persistence:** the matrix is a **persistent calibration**, saved to `config.yaml > pose_3d.cross_camera` and reloaded on startup (like a hand-eye calibration). Session memory mirrors it.
- **Transform type:** **Umeyama similarity** — rotation + translation + single uniform scale.
- **Tag detection scope:** when "Mostra tag" is on, detection runs on **both** cameras always (independent of which clouds are shown).
- **Fallback when the align tag is not seen by both cameras at calibration time:** the `POST /align` call returns `found_in_both:false` and an error and **does not overwrite** the saved matrix. At render time, if the flag is ON but there is no valid saved matrix, the Nuwa cloud falls back to normal URDF/hand-eye placement and a warning is shown.
- **`align_id` default:** `0`. It must be a scene/table tag visible to both the wrist (Nuwa) and torso (D435i) cameras simultaneously — not a robot-mounted tag.
- **`/front_tags` removed** (the GUI is the only consumer) and replaced by the generalized endpoints below.
## Architecture
### Backend — `src/presa_mano/`
**`geometry.py` — new pure helper**
```python
def umeyama_similarity(src: np.ndarray, dst: np.ndarray
) -> tuple[float, np.ndarray, np.ndarray, float]:
"""Fit dst ≈ s·R·src + t (Umeyama 1991, with scale).
src, dst: (N,3) corresponding points. Returns (scale, R(3x3), t(3,), rmse_meters).
Raises ValueError if N < 3 or the point set is degenerate (rank-deficient covariance)."""
```
Numpy-only, unit-testable in isolation. Standard Umeyama: center both sets, `H = dst_c.T @ src_c / N`, SVD `U,S,Vt = svd(H)`, `D = diag(1,1,sign(det(U@Vt)))` to forbid reflections, `R = U @ D @ Vt`, `scale = trace(diag(S)@D) / var(src_c)`, `t = mean(dst) - scale·R·mean(src)`. `rmse` = RMS residual of `s·R·src + t` vs `dst`.
**`pose_3d_routes.py` — generalize detection**
Extract the per-camera body of the current `get_front_tags` into a camera-agnostic helper:
```python
def _detect_tags_on(client, frame_label: str, ids: str,
probe_size_mm: float = 40.0) -> list[dict]:
"""Detect ArUco on `client` RGB + measure 3D corners/center via `client.measure`.
`client` is any NuwaClient (Nuwa wrist cam or D435i). Returns the per-tag list
(id, corners_pixel, corners_3d_optical_m, corner_depths_mm, center_3d_optical_m,
edge_mm_per_side, edge_mm, distance_m) — identical schema to today's front_tags."""
```
The current intrinsics→K override, corner measure, depth-outlier rejection, edge/distance math move verbatim into this helper.
**`GET /api/pose_3d/tags?ids=0-49&probe_size_mm=40`** — detect on both cameras:
```json
{ "cameras": {
"nuwa": { "tags": [ ...tag schema... ], "frame": "nuwa_rgb_optical" },
"d435": { "tags": [ ... ], "frame": "d435_rgb_optical" } },
"timings_ms": { "nuwa": ..., "d435": ..., "total": ... } }
```
Each camera detected independently; a per-camera failure yields `{ "tags": [], "error": "<msg>", "frame": ... }` instead of a 500, so one offline camera doesn't kill the other (mirrors the `/clouds` graceful pattern).
**`POST /api/pose_3d/align?ids=0-49&align_id=0&probe_size_mm=40`** — compute + persist the calibration:
1. `_detect_tags_on` both cameras.
2. Find `align_id` in both `nuwa` and `d435` tag lists. If missing in either → return `{ "found_in_both": false, "error": "tag <id> non visto da <cam(s)>", ... }` **without touching the saved matrix**.
3. Build the 4 corner correspondences (`corners_3d_optical_m`), dropping any corner that is NaN in *either* camera. Require ≥3 valid corners else `found_in_both:false`, error.
4. `scale, R, t, rmse = umeyama_similarity(nuwa_corners, d435_corners)`.
5. Persist via `update_config({"pose_3d": {"cross_camera": {...}}})`:
```json
{ "align_id": 0, "scale": 0.98, "R": [[...3x3...]], "t": [x,y,z],
"rmse_mm": 3.2, "n_corners": 4 }
```
The `enabled` flag lives in the same `cross_camera` block but is written by the JS save path (see below), not overwritten here.
6. Return the same object plus `found_in_both:true`, `timings_ms`. Frame note: maps **Nuwa optical → D435 optical** (both OpenCV X-right/Y-down/Z-forward).
**`GET /api/pose_3d/state`** — extend the returned dict with `cross_camera`: the saved `{ enabled, align_id, scale, R, t, rmse_mm, n_corners }` (or `null` if never calibrated). The frontend uses it to repopulate the UI and apply the matrix on load.
### Frontend — `web/pose_3d.js` + `web/index.html`
**Tag overlay generalization.** `state.front_tags` → `state.tags = { nuwa: {group:null}, d435: {group:null} }`. Add `NUWA_TAG_COLOR = 0xff3b30` (red); D435i keeps `FRONT_TAG_COLOR`.
- `pollFrontTags()` → `pollTags()`: `GET /api/pose_3d/tags?ids=...`, then for each camera `renderTags(camKey, tags, color)`.
- `renderFrontTags(tags)` → `renderTags(camKey, tags, color)`: same geometry/label logic, but `clearTagsGroup(camKey)`, parent = `getCloudParent(camKey)` (so Nuwa tags ride the Nuwa cloud node, D435i tags the D435i node), color parameterized.
- `addOneFrontTag(grp, t)` → `addOneTag(grp, t, color)`.
- `clearFrontTagsGroup()` → `clearTagsGroup(camKey)` (per-camera).
- Trigger points unchanged in spirit: on the "Mostra tag" checkbox toggle-on, and at the end of `acquireClouds()` when the flag is on (the loop in `pollState()` stays removed). The IDs field `#p3FrontTagIds` is reused (the single shared range for both cameras).
**Cross-camera alignment.** New `state.align = { enabled:false, id:0, matrix:null /*THREE.Matrix4*/, info:null }`.
UI added to the existing **"Cloud cross-camera"** section in `index.html`:
```
[ ] Allinea Nuwa→frontale tag id [ 0 ] [Allinea]
<status line: scala / rmse / errore>
```
- `#p3AlignEnabled` (checkbox), `#p3AlignTagId` (number), `#p3AlignBtn` (button), `#p3AlignStatus` (status div).
Handlers:
- **`doAlign()`** (`#p3AlignBtn`): `POST /api/pose_3d/align?ids=<p3FrontTagIds>&align_id=<p3AlignTagId>`. On `found_in_both`: build `THREE.Matrix4` from `scale,R,t` → `state.align.matrix`, status shows `scala=… rmse=…mm`, then `applyNuwaCloudPlacement()`. On failure: leave `state.align.matrix` as-is, status shows the error in the warning color.
- **`#p3AlignEnabled` change**: update `state.align.enabled`, persist (see below), `applyNuwaCloudPlacement()`.
- On load, `applyCrossCameraFromCfg(s.cross_camera)` (called from `pollState`, guarded like `applyCloudParamsFromCfg` so it doesn't clobber in-progress edits): sets the checkbox/id inputs and, if `R/t/scale` present, builds `state.align.matrix`.
**Nuwa cloud placement.** Refactor the Nuwa branch of `applyCloudTransform`/`buildOrUpdateCloud` into `applyNuwaCloudPlacement()`:
- **Aligned mode** (`state.align.enabled && state.align.matrix`): re-parent the Nuwa `THREE.Points` to `getCloudParent('d435')` (same parent as the D435i cloud), set `matrixAutoUpdate=false`, and set
`nuwa.matrix = D435LocalMatrix · AlignMatrix`,
where `D435LocalMatrix` is exactly the matrix the D435i cloud uses relative to that parent (hand-eye `M·S`, or URDF `T(offset)·R(rpy)·S(scaleVec)` — extracted into a shared `d435LocalMatrix()` helper so both clouds stay in lockstep). A raw Nuwa optical point `p` then renders at `parent · D435Local · Align · p`, i.e. mapped into D435i optical space and pushed through D435i's own pipeline → guaranteed overlay regardless of URDF/hand-eye error.
- **Normal mode** (flag off, or no matrix): current behavior — parent `right_wrist_yaw_link`, `T·R·S` from `getCloudTransform('nuwa')` + `readCloudScaleVec('nuwa')`. If the flag is ON but `matrix` is null, also surface a warning in `#p3AlignStatus`.
`applyNuwaCloudPlacement()` is invoked from `buildOrUpdateCloud('nuwa')`, after `doAlign()`, and on the enable-flag toggle.
**Persistence (JS).** Extend `readCloudParamsFromUI`/`saveCloudParamsToConfig` path (or a sibling `saveCrossCameraFlag()`) to PUT `{ pose_3d: { cross_camera: { enabled: <bool>, align_id: <int> } } }`. The matrix fields are written server-side by `/align`; JS only owns `enabled` + `align_id`.
## Config schema
`config.yaml > pose_3d.cross_camera`:
```yaml
pose_3d:
cross_camera:
enabled: false
align_id: 0
scale: 1.0 # written by POST /align
R: [[1,0,0],[0,1,0],[0,0,1]]
t: [0.0, 0.0, 0.0]
rmse_mm: null
n_corners: 0
```
Reads tolerate a missing block (defaults). Writes go through `update_config` deep-merge.
## Error handling
- One camera offline → `/tags` and `/align` still report the other; `/align` with the dead camera missing the tag → `found_in_both:false`, no save.
- `umeyama_similarity` raises `ValueError` on <3 / degenerate corners → `/align` catches → `found_in_both:false`, error string.
- All new endpoints keep the `timings_ms` convention via `Stopwatch().step(...)`.
- Render-time: flag ON + null matrix → URDF fallback + visible warning (never a blank/crashed cloud).
## Testing
- **`umeyama_similarity`** (`tests/test_geometry.py` or new): apply a known `(s,R,t)` to random points, recover within tolerance (scale ~1e-6, R ~1e-6, t ~1e-6); collinear/<3 points → `ValueError`.
- **`POST /api/pose_3d/align`** (mock marker + both camera clients): tag in both → matrix returned + persisted; tag missing in one → `found_in_both:false`, saved matrix untouched. Use the **config snapshot/restore fixture** (conftest) because `update_config` reformats `config.yaml`.
- **`GET /api/pose_3d/tags`** (mock): both cameras' tags returned with correct `frame` labels; one camera raising → that camera reports `error`, the other still returns tags.
- **`GET /api/pose_3d/state`**: includes `cross_camera` (saved or null).
## Out of scope (YAGNI)
- Per-axis/anisotropic scale (rejected — uniform only).
- Continuous/live re-alignment (rejected — button-driven).
- Aligning D435i onto Nuwa (direction is fixed Nuwa→frontale).
- A clear/reset-calibration button (can be added later; not required now).
```
@@ -0,0 +1,95 @@
# GUI build-version badge
**Date:** 2026-06-03
**Status:** approved (design)
**Area:** `VERSION`, `build.sh`, `Dockerfile`, `src/presa_mano/version.py`, `src/presa_mano/main.py`, `web/index.html`, `web/app.js`
## Problem
`src/` and `web/` are baked into the Docker image (no bind-mount), so code changes only take effect after an image rebuild. There is no visible indicator of which build the running container serves, so a stale deployment is invisible — exactly the confusion behind "non vedo modifiche alla GUI". We want a version shown in the GUI header that increments on every build, plus the build time.
## Decisions (locked)
- **Version string:** `V<major>.<minor>.<patch>` (e.g. `V1.0.1`), **patch auto-increments every build**; build time shown too.
- **Injection:** a build wrapper script passes the version + build time as Docker build args.
- **Placement:** a pill badge in the GUI header, build time in its tooltip.
## Components
### `VERSION` (tracked file, repo root `presa-mano/VERSION`)
Plain text `major.minor.patch`, initialized to `1.0.0`. Single source of truth for the number; persists across builds via the file on disk. The build script rewrites it (left as an uncommitted working-tree edit — the script does NOT auto-commit).
### `build.sh` (repo root, executable)
```
1. cd to script dir; ensure VERSION exists (else seed 1.0.0)
2. parse current major.minor.patch
3. bump: default patch++; arg `minor` → minor++/patch=0; `major` → major++/minor=patch=0
4. write new VERSION
5. BUILD_TIME = $(date -u +"%Y-%m-%d %H:%M UTC")
6. (cd compose dir) docker compose build --build-arg APP_VERSION=<new> --build-arg BUILD_TIME="<ts>" presa-mano
7. docker compose up -d presa-mano
8. echo deployed version
```
The compose file lives one level up (`/opt/docker/robotserver/docker-compose.yml`); the script resolves that dir and runs compose there. Usage: `./build.sh [patch|minor|major]` (default patch).
### `Dockerfile`
Add late (before `CMD`), so changing them doesn't bust the dependency layers:
```
ARG APP_VERSION=dev
ARG BUILD_TIME=unknown
ENV APP_VERSION=${APP_VERSION}
ENV BUILD_TIME=${BUILD_TIME}
```
Also `COPY VERSION ./` (cheap, makes the file fallback work inside the container too).
### `src/presa_mano/version.py`
```python
def build_info() -> dict:
"""{'version', 'build_time'}. Prefer env (set at Docker build); fall back to
the VERSION file + '(local)' for `uv run` dev."""
```
- `version` = `APP_VERSION` env if set, else `<VERSION file contents> (local)` (or `0.0.0 (local)` if the file is unreadable).
- `build_time` = `BUILD_TIME` env if set, else `(local)`.
- `VERSION` file resolved as `Path(__file__).resolve().parents[2] / "VERSION"`.
### `src/presa_mano/main.py`
```python
from .version import build_info
@app.get("/api/version", tags=["meta"])
def version() -> dict:
return build_info()
```
### `web/index.html`
In the header, next to `#activeRobotPill`, add:
```html
<span class="active-robot-pill" id="appVersionPill" title="versione build">V—</span>
```
(reuses the existing pill styling).
### `web/app.js`
Add and call once at startup (alongside `refreshHomeRobotSelector()`):
```javascript
async function refreshAppVersion() {
try {
const r = await api('GET', '/api/version');
const pill = $('appVersionPill');
if (pill) { pill.textContent = `V${r.version}`; pill.title = `build ${r.build_time}`; }
} catch {}
}
refreshAppVersion();
```
## Testing
- Unit test `tests/test_version.py` for `build_info()`:
- env set (`APP_VERSION`, `BUILD_TIME` via monkeypatch) → returns exactly those.
- env unset (monkeypatch.delenv) → `version` ends with `(local)` and contains the `VERSION` file content; `build_time == "(local)"`.
- Frontend: manual — after `./build.sh` the badge increments and the tooltip shows the new build time.
## Out of scope (YAGNI)
- git SHA / dirty flag (user chose version+time only).
- Auto-commit of the VERSION bump.
- Frontend-vs-backend version mismatch warning.
+123 -57
View File
@@ -10,12 +10,13 @@ from pathlib import Path
from typing import Any, Literal, Optional from typing import Any, Literal, Optional
import httpx import httpx
import numpy as np
from fastapi import APIRouter, HTTPException from fastapi import APIRouter, HTTPException
from pydantic import BaseModel, Field from pydantic import BaseModel, Field
from ..clients import G1Unavailable 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 ..perception.pointcloud import nuwa_to_cv_frame
from ..timing import Stopwatch from ..timing import Stopwatch
from . import _deps from . import _deps
@@ -104,6 +105,7 @@ def get_state() -> dict[str, Any]:
"right_wrist_camera": cfg.get("right_wrist_camera") or {}, "right_wrist_camera": cfg.get("right_wrist_camera") or {},
"clouds": cfg.get("clouds") or {}, "clouds": cfg.get("clouds") or {},
"cloud_params": cfg.get("cloud_params") or {}, "cloud_params": cfg.get("cloud_params") or {},
"cross_camera": cfg.get("cross_camera") or {},
"timings_ms": sw.as_dict(), "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 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]: def _parse_id_range(spec: str) -> list[int]:
"""Accetta "0-49" o "1,10,20,33".""" """Accetta "0-49" o "1,10,20,33"."""
@@ -271,27 +279,18 @@ def _parse_id_range(spec: str) -> list[int]:
return sorted(set(out)) 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() 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 {} rgb = (intr or {}).get("rgb") or {}
if not rgb or "fx" not in rgb: 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 = { K = {
"camera_matrix": [ "camera_matrix": [
[float(rgb["fx"]), 0.0, float(rgb["cx"])], [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") 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) id_list = _parse_id_range(ids)
markers = [{"id": int(i), "size_mm": float(probe_size_mm)} for i in id_list] 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]] = [] tags_out: list[dict[str, Any]] = []
with sw.step("measure_corners"):
for m in det.get("found", []): for m in det.get("found", []):
corners = m.get("corners") or [] corners = m.get("corners") or []
if len(corners) != 4: 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]] = [] depths: list[Optional[int]] = []
try: try:
for (u, v) in corners: 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 {} pt = (md or {}).get("point_m") or {}
d_mm = (md or {}).get("depth_mm") d_mm = (md or {}).get("depth_mm")
if not pt or d_mm in (None, 0): 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)) depths.append(int(d_mm))
cu = sum(c[0] for c in corners) / 4.0 cu = sum(c[0] for c in corners) / 4.0
cv = sum(c[1] 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 {} 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: 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 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] valid_d = [d for d in depths if d is not None]
if len(valid_d) >= 3: if len(valid_d) >= 3:
# mediana semplice (4 elementi → media dei 2 centrali)
sd = sorted(valid_d) 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] 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): for i, d in enumerate(depths):
if d is None: if d is None:
continue continue
if abs(d - median_d) / median_d > 0.20: 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] 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 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, "edge_mm": edge_mean,
"distance_m": distance_m, "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"),
} }
return result
@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),
}
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 --- # --- Tag locator: piazza crocetta nella scena 3D e salva la posa in pelvis ---
+38
View File
@@ -142,3 +142,41 @@ def quaternion_xyzw_to_rpy_deg(quat_xyzw) -> tuple[float, float, float]:
rpy_rad = R_to_euler_xyz(R) rpy_rad = R_to_euler_xyz(R)
rpy_deg = np.degrees(rpy_rad) rpy_deg = np.degrees(rpy_rad)
return float(rpy_deg[0]), float(rpy_deg[1]), float(rpy_deg[2]) 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
+6
View File
@@ -25,6 +25,7 @@ from .api import (
) )
from .clients import G1StatusError from .clients import G1StatusError
from .config import load_config from .config import load_config
from .version import build_info
logging.basicConfig(level=logging.INFO, format="%(asctime)s %(levelname)s %(name)s: %(message)s") logging.basicConfig(level=logging.INFO, format="%(asctime)s %(levelname)s %(name)s: %(message)s")
@@ -102,6 +103,11 @@ def health() -> dict:
return {"status": "ok"} return {"status": "ok"}
@app.get("/api/version", tags=["meta"])
def version() -> dict:
return build_info()
@app.get("/", include_in_schema=False) @app.get("/", include_in_schema=False)
def index() -> HTMLResponse: def index() -> HTMLResponse:
idx = WEB_DIR / "index.html" idx = WEB_DIR / "index.html"
+35
View File
@@ -0,0 +1,35 @@
"""Versione build dell'app, mostrata nel badge in GUI.
In Docker i valori arrivano da env (`APP_VERSION`, `BUILD_TIME`) iniettati come
build-arg da `build.sh`. In dev locale (`uv run`) gli env non ci sono: si
ripiega sul contenuto del file `VERSION` nella root del repo + suffisso
`(local)`.
"""
from __future__ import annotations
import os
from pathlib import Path
_VERSION_FILE = Path(__file__).resolve().parents[2] / "VERSION"
def _file_version() -> str:
try:
return _VERSION_FILE.read_text(encoding="utf-8").strip() or "0.0.0"
except OSError:
return "0.0.0"
def build_info() -> dict:
"""Ritorna {'version', 'build_time'}.
Preferisce gli env settati al build dell'immagine; in loro assenza ripiega
sul file VERSION marcando la versione come `(local)`.
"""
version = os.environ.get("APP_VERSION")
build_time = os.environ.get("BUILD_TIME")
if not version:
version = f"{_file_version()} (local)"
if not build_time:
build_time = "(local)"
return {"version": version, "build_time": build_time}
+49
View File
@@ -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)
+85
View File
@@ -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", {})
+10
View File
@@ -208,3 +208,13 @@ def test_unlock_proxies_to_bridge():
r = client.post("/api/pose_3d/unlock") r = client.post("/api/pose_3d/unlock")
assert r.status_code == 200 assert r.status_code == 200
assert r.json()["ok"] is True 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
+75
View File
@@ -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
+25
View File
@@ -0,0 +1,25 @@
"""Unit test per build_info(): env (Docker) vs fallback VERSION file (local)."""
from __future__ import annotations
from pathlib import Path
from presa_mano.version import build_info
_VERSION_FILE = Path(__file__).resolve().parents[1] / "VERSION"
def test_build_info_prefers_env(monkeypatch):
monkeypatch.setenv("APP_VERSION", "1.2.3")
monkeypatch.setenv("BUILD_TIME", "2026-06-03 14:22 UTC")
info = build_info()
assert info == {"version": "1.2.3", "build_time": "2026-06-03 14:22 UTC"}
def test_build_info_falls_back_to_version_file(monkeypatch):
monkeypatch.delenv("APP_VERSION", raising=False)
monkeypatch.delenv("BUILD_TIME", raising=False)
info = build_info()
file_ver = _VERSION_FILE.read_text(encoding="utf-8").strip()
assert file_ver in info["version"]
assert info["version"].endswith("(local)")
assert info["build_time"] == "(local)"
+10
View File
@@ -742,6 +742,16 @@ async function refreshHomeRobotSelector() {
refreshHomeRobotSelector(); refreshHomeRobotSelector();
startHomePoll(); startHomePoll();
// Badge versione build (header). Si aggiorna ad ogni rebuild via build.sh.
async function refreshAppVersion() {
try {
const r = await api('GET', '/api/version');
const pill = $('appVersionPill');
if (pill) { pill.textContent = `V${r.version} · ${r.build_time}`; pill.title = `versione ${r.version} · build ${r.build_time}`; }
} catch {}
}
refreshAppVersion();
// ---------- init ---------- // ---------- init ----------
(async () => { (async () => {
try { try {
+8 -1
View File
@@ -20,6 +20,7 @@
</nav> </nav>
<div class="status-strip" id="statusStrip"></div> <div class="status-strip" id="statusStrip"></div>
<span class="active-robot-pill" id="activeRobotPill" title="Robot attualmente collegato">robot: —</span> <span class="active-robot-pill" id="activeRobotPill" title="Robot attualmente collegato">robot: —</span>
<span class="active-robot-pill" id="appVersionPill" title="versione build">V—</span>
</header> </header>
<main> <main>
@@ -449,6 +450,12 @@
<input id="p3D435ScaleXY" type="number" min="0.1" max="3" step="0.01" value="0.65" style="width:60px"> <input id="p3D435ScaleXY" type="number" min="0.1" max="3" step="0.01" value="0.65" style="width:60px">
</label> </label>
</div> </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"> <div class="p3-row">
<label>step <input id="p3CloudStep" type="number" min="1" max="32" value="8" style="width:50px"></label> <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> <label>max <input id="p3CloudMaxDepth" type="number" min="100" max="5000" step="100" value="2000" style="width:70px"> mm</label>
@@ -511,7 +518,7 @@
</div> </div>
<div class="p3-section"> <div class="p3-section">
<h3>Tag camera frontale</h3> <h3>Tags</h3>
<div class="p3-row"> <div class="p3-row">
<label><input type="checkbox" id="p3ShowFrontTags"> Mostra tag visibili (quad + dim + dist)</label> <label><input type="checkbox" id="p3ShowFrontTags"> Mostra tag visibili (quad + dim + dist)</label>
</div> </div>
+161 -40
View File
@@ -222,13 +222,15 @@ const state = {
joint_ucses: [], // array di THREE.Group, UCS per ogni link URDF (debug) joint_ucses: [], // array di THREE.Group, UCS per ogni link URDF (debug)
clouds_cfg: null, // {nuwa:{...}, d435:{...}} dal config clouds_cfg: null, // {nuwa:{...}, d435:{...}} dal config
cloud_points: { nuwa: null, d435: null }, // THREE.Points obj per camera 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 tag_locator: { // sezione "Tag locator" nella sidebar Pose 3D
marker: null, // THREE.LineSegments (crocetta XYZ) o null marker: null, // THREE.LineSegments (crocetta XYZ) o null
position_m: null, // [x,y,z] in pelvis frame della crocetta corrente 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) 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(); updateNuwaInputsFromCfg();
updateD435InputsFromCfg(); updateD435InputsFromCfg();
applyCloudParamsFromCfg(s.cloud_params); applyCloudParamsFromCfg(s.cloud_params);
applyCrossCameraFromCfg(s.cross_camera);
applyCalibratedD435UCS(); applyCalibratedD435UCS();
// Applica TUTTI i giunti reali sul modello URDF (29 motori G1) // Applica TUTTI i giunti reali sul modello URDF (29 motori G1)
state.joints = s.joints || null; // snapshot per tag locator save 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)}]`, `Y[${ymin.toFixed(2)},${ymax.toFixed(2)}]`,
`Z[${zmin.toFixed(2)},${zmax.toFixed(2)}]`); `Z[${zmin.toFixed(2)},${zmax.toFixed(2)}]`);
// (Ri-)attacca al parent corretto e applica transform optical→link // (Ri-)attacca al parent corretto e applica transform optical→link
if (camKey === 'nuwa') {
applyNuwaCloudPlacement();
} else {
if (pts.parent !== parent) parent.add(pts); if (pts.parent !== parent) parent.add(pts);
applyCloudTransform(camKey); applyCloudTransform(camKey);
}
applyCloudVisibility(); applyCloudVisibility();
return pts; return pts;
} }
@@ -1620,6 +1627,24 @@ function applyCloudVisibility() {
// Applica position+rpy+mirror sui punti del cloud `camKey`. Il mirror (sign // 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 // 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). // (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) { function applyCloudTransform(camKey) {
const pts = state.cloud_points[camKey]; const pts = state.cloud_points[camKey];
if (!pts) return; if (!pts) return;
@@ -1651,6 +1676,34 @@ function applyCloudTransform(camKey) {
pts.scale.set(sv[0], sv[1], sv[2]); 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) { function readCloudFlip(camKey) {
const tag = camKey === 'nuwa' ? 'Nuwa' : 'D435'; const tag = camKey === 'nuwa' ? 'Nuwa' : 'D435';
return [ return [
@@ -1767,7 +1820,7 @@ async function acquireClouds() {
saveCloudParamsToConfig(); saveCloudParamsToConfig();
// Nuova immagine acquisita: se l'overlay tag e' attivo ri-esegui la // Nuova immagine acquisita: se l'overlay tag e' attivo ri-esegui la
// detection una volta sola, sul frame appena acquisito. // detection una volta sola, sul frame appena acquisito.
if ($('p3ShowFrontTags')?.checked) pollFrontTags(); if ($('p3ShowFrontTags')?.checked) pollTags();
} catch (e) { } catch (e) {
$('p3CloudStatus').textContent = `errore: ${e.message}`; $('p3CloudStatus').textContent = `errore: ${e.message}`;
$('p3CloudStatus').style.color = 'var(--danger)'; $('p3CloudStatus').style.color = 'var(--danger)';
@@ -1795,6 +1848,8 @@ function clearClouds() {
// camera in modalita' URDF link e in modalita' hand-eye. // camera in modalita' URDF link e in modalita' hand-eye.
const FRONT_TAG_COLOR = 0x4dd0e1; 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'; const FRONT_TAG_LABEL_COLOR = '#4dd0e1';
// Label compatta auto-sized: canvas largo quanto il testo, font piccolo, // 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]); 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) return;
if (grp.parent) grp.parent.remove(grp); if (grp.parent) grp.parent.remove(grp);
grp.traverse(o => { grp.traverse(o => {
@@ -1865,78 +1920,137 @@ function clearFrontTagsGroup() {
else o.material.dispose(); 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; const c = t.corners_3d_optical_m;
if (!Array.isArray(c) || c.length !== 4) return; if (!Array.isArray(c) || c.length !== 4) return;
if (c.some(p => !p || p.some(v => !Number.isFinite(v)))) return; if (c.some(p => !p || p.some(v => !Number.isFinite(v)))) return;
// LineLoop (5 verts = close)
const v = new Float32Array(5 * 3); 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]; } 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]; v[12] = c[0][0]; v[13] = c[0][1]; v[14] = c[0][2];
const geom = new THREE.BufferGeometry(); const geom = new THREE.BufferGeometry();
geom.setAttribute('position', new THREE.BufferAttribute(v, 3)); 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 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 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 sprite = makeCompactLabel(`id=${t.id} · ${edge_s} · d=${dist_s}`);
const ctr = t.center_3d_optical_m; 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); grp.add(sprite);
} }
function renderFrontTags(tags) { function renderTags(camKey, tags, color) {
clearFrontTagsGroup(); clearTagsGroup(camKey);
if (!tags || tags.length === 0) return; 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(); const grp = new THREE.Group();
grp.name = 'front_tags_group'; grp.name = `${camKey}_tags_group`;
applyCloudLikeTransform(grp, 'd435'); applyCloudLikeTransform(grp, camKey);
parent.add(grp); 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 ids = ($('p3FrontTagIds')?.value || '0-49').trim();
const status = $('p3FrontTagStatus'); const status = $('p3FrontTagStatus');
try { 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) { } catch (e) {
if (status) status.textContent = `errore: ${e?.message || 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() { function bindFrontTagFinder() {
const cb = $('p3ShowFrontTags'); const cb = $('p3ShowFrontTags');
if (!cb) return; if (!cb) return;
cb.addEventListener('change', () => { cb.addEventListener('change', () => {
if (cb.checked) { if (cb.checked) {
// Refresh immediato senza aspettare il prossimo tick di pollState pollTags();
pollFrontTags();
} else { } else {
clearFrontTagsGroup(); clearTagsGroup('nuwa');
clearTagsGroup('d435');
const s = $('p3FrontTagStatus'); const s = $('p3FrontTagStatus');
if (s) s.textContent = '—'; if (s) s.textContent = '—';
} }
@@ -1946,6 +2060,13 @@ function bindFrontTagFinder() {
function bindCloudControls() { function bindCloudControls() {
$('p3CloudAcquireBtn')?.addEventListener('click', acquireClouds); $('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); $('p3CloudClearBtn')?.addEventListener('click', clearClouds);
$('p3ShowNuwaCloud')?.addEventListener('change', applyCloudVisibility); $('p3ShowNuwaCloud')?.addEventListener('change', applyCloudVisibility);
$('p3ShowD435Cloud')?.addEventListener('change', applyCloudVisibility); $('p3ShowD435Cloud')?.addEventListener('change', applyCloudVisibility);