docs(pose_3d): spec allineamento cross-camera + overlay tag multi-camera
Design per: detection tag su tutte le camere attive (Nuwa rosso + D435i), rinomina "Tag camera frontale" -> "Tags", calibrazione persistente Nuwa->frontale via tag condiviso (Umeyama similarity), flag+pulsante "Allinea" nella sezione Cloud cross-camera. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
This commit is contained in:
AdrianoandClaude Opus 4.8 committed
1 parent
b4922fe01b
commit
80625cc281
1 file changed
+153
@@ -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).
|
||||
```
|
||||
Reference in new issue
Block a user