diff --git a/docs/superpowers/specs/2026-06-03-cross-camera-tag-alignment-design.md b/docs/superpowers/specs/2026-06-03-cross-camera-tag-alignment-design.md new file mode 100644 index 0000000..3853d64 --- /dev/null +++ b/docs/superpowers/specs/2026-06-03-cross-camera-tag-alignment-design.md @@ -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": "", "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 non visto da ", ... }` **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] + +``` +- `#p3AlignEnabled` (checkbox), `#p3AlignTagId` (number), `#p3AlignBtn` (button), `#p3AlignStatus` (status div). + +Handlers: +- **`doAlign()`** (`#p3AlignBtn`): `POST /api/pose_3d/align?ids=&align_id=`. 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: , align_id: } } }`. 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). +```