Compare commits
17
Commits
b4922fe01b
...
054ad103df
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
054ad103df | ||
|
|
7b4dc6ce8c | ||
|
|
cf726b479e | ||
|
|
a88a064aa8 | ||
|
|
8084b87261 | ||
|
|
0ee3ef244b | ||
|
|
d9213ca19a | ||
|
|
47ae8a6b2c | ||
|
|
746662d534 | ||
|
|
2f0dc69cdd | ||
|
|
edc4430c67 | ||
|
|
79f587567e | ||
|
|
5ab808accd | ||
|
|
8b01ae123f | ||
|
|
77fb0f8fc0 | ||
|
|
9189700f34 | ||
|
|
80625cc281 |
No files matched your search
@@ -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
@@ -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
|
||||||
|
|||||||
@@ -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).
|
||||||
|
|
||||||
|
|||||||
@@ -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.
|
||||||
@@ -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,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")
|
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:
|
continue
|
||||||
continue
|
pts3d: list[list[float]] = []
|
||||||
pts3d: list[list[float]] = []
|
depths: list[Optional[int]] = []
|
||||||
depths: list[Optional[int]] = []
|
try:
|
||||||
try:
|
for (u, v) in corners:
|
||||||
for (u, v) in corners:
|
md = client.measure(int(round(u)), int(round(v)), radius=2)
|
||||||
md = front.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):
|
pts3d.append([float("nan")] * 3)
|
||||||
pts3d.append([float("nan")] * 3)
|
depths.append(None)
|
||||||
depths.append(None)
|
else:
|
||||||
else:
|
pts3d.append([float(pt["x"]), float(pt["y"]), float(pt["z"])])
|
||||||
pts3d.append([float(pt["x"]), float(pt["y"]), float(pt["z"])])
|
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 = client.measure(int(round(cu)), int(round(cv)), radius=4)
|
||||||
cmd = front.measure(int(round(cu)), int(round(cv)), radius=4)
|
cpt = (cmd or {}).get("point_m") or {}
|
||||||
cpt = (cmd or {}).get("point_m") or {}
|
center3d = ([float(cpt["x"]), float(cpt["y"]), float(cpt["z"])]
|
||||||
center3d = (
|
if cpt else [float("nan")] * 3)
|
||||||
[float(cpt["x"]), float(cpt["y"]), float(cpt["z"])]
|
except Exception as e:
|
||||||
if cpt else [float("nan")] * 3
|
log.warning("tags(%s): measure failed for id=%s: %s",
|
||||||
)
|
frame_label, m.get("id"), e)
|
||||||
except Exception as e:
|
continue
|
||||||
log.warning("front_tags: measure failed for id=%s: %s", m.get("id"), e)
|
|
||||||
continue
|
|
||||||
|
|
||||||
# Outlier rejection sulla depth: il pixel di un corner sul bordo
|
valid_d = [d for d in depths if d is not None]
|
||||||
# del marker spesso prende la depth del background (es. 1358mm
|
if len(valid_d) >= 3:
|
||||||
# mentre gli altri 3 sono a 320mm). Rifiutiamo corner con depth
|
sd = sorted(valid_d)
|
||||||
# > 20% off dalla mediana → li trattiamo come NaN nel calcolo edge.
|
median_d = (sd[len(sd)//2 - 1] + sd[len(sd)//2]) / 2 if len(sd) % 2 == 0 else sd[len(sd)//2]
|
||||||
valid_d = [d for d in depths if d is not None]
|
for i, d in enumerate(depths):
|
||||||
if len(valid_d) >= 3:
|
if d is None:
|
||||||
# mediana semplice (4 elementi → media dei 2 centrali)
|
continue
|
||||||
sd = sorted(valid_d)
|
if abs(d - median_d) / median_d > 0.20:
|
||||||
median_d = (sd[len(sd)//2 - 1] + sd[len(sd)//2]) / 2 if len(sd) % 2 == 0 else sd[len(sd)//2]
|
pts3d[i] = [float("nan")] * 3
|
||||||
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
|
|
||||||
|
|
||||||
# 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]:
|
valid_edges = [e for e in edges_mm if e is not None]
|
||||||
if any(x != x for x in a + b): # NaN check
|
edge_mean = sum(valid_edges) / len(valid_edges) if valid_edges else None
|
||||||
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)]
|
distance_m: Optional[float] = None
|
||||||
valid_edges = [e for e in edges_mm if e is not None]
|
if all(x == x for x in center3d):
|
||||||
edge_mean = sum(valid_edges) / len(valid_edges) if valid_edges else None
|
distance_m = (sum(c ** 2 for c in center3d)) ** 0.5
|
||||||
|
|
||||||
distance_m: Optional[float] = None
|
tags_out.append({
|
||||||
if all(x == x for x in center3d):
|
"id": int(m["id"]),
|
||||||
distance_m = (sum(c ** 2 for c in center3d)) ** 0.5
|
"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 = {
|
# Map: camera key → (frame_label, _deps attribute name).
|
||||||
"tags": tags_out,
|
# Usiamo il nome stringa dell'attributo su _deps e lo risolviamo via getattr
|
||||||
"not_found_count": int(len(det.get("not_found", []))),
|
# a ogni richiesta: cosi' patch("...pose_3d_routes._deps.nuwa") viene visto.
|
||||||
"frame": "d435_rgb_optical", # OpenCV: X right, Y down, Z forward
|
_TAG_CAMERAS: dict[str, tuple[str, str]] = {
|
||||||
"timings_ms": sw.as_dict(),
|
"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 ---
|
# --- 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_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
|
||||||
@@ -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"
|
||||||
|
|||||||
@@ -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}
|
||||||
@@ -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")
|
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
|
||||||
@@ -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
|
||||||
@@ -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
@@ -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}`; pill.title = `build ${r.build_time}`; }
|
||||||
|
} catch {}
|
||||||
|
}
|
||||||
|
refreshAppVersion();
|
||||||
|
|
||||||
// ---------- init ----------
|
// ---------- init ----------
|
||||||
(async () => {
|
(async () => {
|
||||||
try {
|
try {
|
||||||
|
|||||||
+8
-1
@@ -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>
|
||||||
|
|||||||
+163
-42
@@ -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 (pts.parent !== parent) parent.add(pts);
|
if (camKey === 'nuwa') {
|
||||||
applyCloudTransform(camKey);
|
applyNuwaCloudPlacement();
|
||||||
|
} else {
|
||||||
|
if (pts.parent !== parent) parent.add(pts);
|
||||||
|
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);
|
||||||
|
|||||||
Reference in new issue
Block a user