diff --git a/docs/superpowers/specs/2026-05-11-work-plane-filter-design.md b/docs/superpowers/specs/2026-05-11-work-plane-filter-design.md new file mode 100644 index 0000000..0b0acf3 --- /dev/null +++ b/docs/superpowers/specs/2026-05-11-work-plane-filter-design.md @@ -0,0 +1,311 @@ +# Work-plane calibration + below-plane filter + +**Status:** approved (brainstorming → ready for implementation plan) +**Date:** 2026-05-11 +**Scope:** `presa-mano` orchestrator + GUI + +## Goal + +Calibrate the work-plane (table top) from a user-drawn rectangle on the RGB +image, persist it with a configurable `offset_mm`, and use it to drop points +below the plane from every object point cloud produced by the grasp / +perception pipeline. The current "Servo Visuale" tab is repurposed into a +"Calibrazione Piano" tab focused exclusively on this calibration. + +## Non-goals + +- Replace `fit_plane_svd` or the SVD-based plane-fitting maths. +- Persist multiple work planes (a single `data/work_plane.json` per + deployment). +- Per-side calibration. The work plane is a property of the world, not the + robot side. +- Keep the marker-relative visual-servoing feature — removed entirely. + +## Pre-existing infrastructure (kept) + +The repo already has most of the plane machinery (currently inside +`grasp/visual_servo.py`). This design moves it to `perception/work_plane.py` +and extends it with `offset_mm`. Kept verbatim: + +- `WorkPlane` dataclass (`normal`, `offset`, `centroid`, `n_inliers`, + `fit_rms_mm`, `roi`, `created_at`). +- `fit_plane_svd(points_xyz)` — SVD-based plane fit. +- `set_work_plane(wp)` / `get_work_plane()` — in-memory singleton with + disk-backed lazy load. +- Disk persistence at `data/work_plane.json` via + `_save_work_plane_to_disk` / `_load_work_plane_from_disk`. + +## Removed (visual servo, entirely) + +The marker-relative servoing feature is removed: + +- GUI tab "Servo Visuale" (the buttons "Plana", "Step verso oggetto", + "Rileva/ricalcola", marker rvec controls, etc.). +- `web/servo.js` (replaced by `web/work_plane.js`). +- Endpoints `POST /api/grasp/visual_servo/plan`, `/step`, and the existing + `/plane/fit`, `/plane`, `DELETE /plane`. +- `_servo_plan_from_req`, `_detect_object_point_cam` and the surrounding + visual-servo logic in `grasp_routes.py`. +- The bulk of `grasp/visual_servo.py` (everything except the plane bits, + which move to `perception/work_plane.py`). + +What survives the removal: +- `WorkPlane`, `fit_plane_svd`, `set/get_work_plane`, disk persistence — + moved to `perception/work_plane.py`. + +## Data model + +`WorkPlane` is extended with one new field: + +```python +@dataclass +class WorkPlane: + normal: list[float] # unit vector in CV frame, points toward camera + offset: float # plane equation: normal·X + offset = 0 + centroid: list[float] # centroid of the ROI inliers, CV frame + n_inliers: int # count of points used in the fit + fit_rms_mm: float # RMS distance of inliers from the fitted plane + roi: list[int] # [u_min, v_min, u_max, v_max] + offset_mm: float # NEW: filter tolerance, default 5.0 + created_at: float # unix timestamp +``` + +On-disk JSON format mirrors the dataclass; loaders that encounter an old +file without `offset_mm` default it to `5.0`. + +## Endpoints + +Four new endpoints under `/api/work_plane/`. They replace the existing +`/api/grasp/visual_servo/plane*` set. + +| Method | Path | Body | Effect | +|---|---|---|---| +| POST | `/api/work_plane/fit` | `{u_min, v_min, u_max, v_max, offset_mm?}` | Compute & return a candidate `WorkPlane`. **Does not write to disk.** Also returns a downsampled cloud (≤5000 points) for GUI 3D preview. | +| POST | `/api/work_plane/save` | `{plane: WorkPlane}` | Persist the given plane to `data/work_plane.json` via `set_work_plane`. | +| GET | `/api/work_plane` | — | `{plane: WorkPlane | null}` | +| DELETE | `/api/work_plane` | — | Remove `data/work_plane.json` and clear the in-memory singleton. | + +## Data flow + +### Calibration (preview → save) + +``` +[GUI: shift+drag on RGB image] + │ + ▼ +roi = (u_min, v_min, u_max, v_max), offset_mm + │ + ▼ +POST /api/work_plane/fit { roi, offset_mm } + │ + ▼ +[backend] + pc = nuwa.pointcloud_roi(roi) + pts_cv = nuwa_to_cv_frame(pc[:, :3]) + pts_cv = filter_outliers(pts_cv) + normal, offset, centroid, rms_mm = fit_plane_svd(pts_cv) + candidate = WorkPlane(normal, offset, centroid, n_inliers, rms_mm, + list(roi), offset_mm, time.time()) + preview_points = subsample(pts_cv, 5000) + return { plane: candidate, preview: true, + points_camera: preview_points, timings_ms } + │ + ▼ +[GUI: render plane mesh + colored scatter in 3D] + │ + │ user adjusts offset_mm slider, colors recompute client-side + │ user clicks "Salva piano" + ▼ +POST /api/work_plane/save { plane: candidate } + │ + ▼ +[backend: set_work_plane(wp) → write data/work_plane.json] + │ + ▼ +return { ok: true, plane: wp, saved: true } +``` + +### Below-plane filter applied to object clouds + +A single helper in `perception/work_plane.py` does the actual filtering: + +```python +def apply_below_plane_filter( + pts_cv: np.ndarray, # (N, 3) in CV camera frame, meters + wp: WorkPlane | None, +) -> tuple[np.ndarray, dict]: + """Returns (filtered_pts_cv, info). info has: + plane_applied: bool — true iff wp is not None and filter ran + n_points_before_plane: int + """ + info = {"plane_applied": False, + "n_points_before_plane": int(len(pts_cv))} + if wp is None or len(pts_cv) == 0: + return pts_cv, info + n_vec = np.asarray(wp.normal, dtype=np.float64) + d_off = float(wp.offset) + offset_m = float(wp.offset_mm) / 1000.0 + signed = pts_cv @ n_vec + d_off # (N,), positive = above plane + keep = signed >= -offset_m + info["plane_applied"] = True + return pts_cv[keep], info +``` + +This helper is called from **three call sites**, each immediately after the +raw cloud is in CV frame and before any `<10` check or `max_points` +subsample: + +1. **`points_via_sam21`** (sam21_bridge.py) — after `Sam21Client.segment_pointcloud` + returns. On `<10` post-filter, raises `Sam21Fallback("too few points + after plane filter")`. The caller falls back to the bbox path, which + applies the same filter and may end up with `<10` too → `422` from the + bbox helper, which is the right user-facing error. +2. **`_points_via_bbox`** (grasp_routes.py) — after `nuwa_to_cv_frame` and + before the existing `<10` check. The existing 422 message stays; the + meaning is now "after outlier + plane filter". +3. **Bbox branch of `_object_cloud_cv`** (perception_routes.py) — after + `nuwa_to_cv_frame`, before the `max_points` subsample. If the filter + drops the cloud below `<10`, return `(np.empty((0,3)), 'bbox_roi', + extra)` and let the caller surface an empty `n_points`; `/scan` is + tolerant of empty clouds today (no 422). + +Each call site populates `out["plane_applied"]` and +`out["n_points_before_plane"]` from the helper's `info` dict. + +### Response enrichment + +`/api/grasp/plan`, `/api/grasp/execute`, `/api/perception/scan`, +`/api/perception/object_points` add three new meta keys: + +- `plane_applied: bool` — true if a `WorkPlane` was loaded and the filter + ran (even if it kept all points). +- `n_points_before_plane: int` — count before the filter, useful for + debugging "I drew the ROI badly". +- `n_points` — keeps current meaning (count after every filter). + +## Frame conventions + +- `WorkPlane.normal` and `WorkPlane.offset` live in **CV/OpenCV RGB-camera + frame** (the same frame the cloud uses post-`nuwa_to_cv_frame` flip / + SAM21 output). No base-frame conversion needed for the filter. +- The fit ensures `normal` points "toward the camera" (negative Z in + OpenCV), i.e. "up" relative to the table. Objects sitting on the table + have positive `signed_distance`. +- The filter keeps `signed_distance >= -offset_m`, so `offset_mm > 0` + preserves a buffer below the plane. `offset_mm = 0` is an exact cut at + the plane; `offset_mm = 10` keeps points up to 10 mm below the plane. + +## File structure + +| File | Action | Responsibility | +|---|---|---| +| `src/presa_mano/perception/work_plane.py` | new | `WorkPlane` dataclass (extended with `offset_mm`), `fit_plane_svd`, `set/get_work_plane`, disk persistence. Moved from `grasp/visual_servo.py`. | +| `src/presa_mano/grasp/visual_servo.py` | delete | Visual servo removed. | +| `src/presa_mano/api/work_plane_routes.py` | new | Router with the four new endpoints (`/fit`, `/save`, `GET`, `DELETE`). | +| `src/presa_mano/api/grasp_routes.py` | modify | Remove all visual-servo endpoints and helpers. Apply the below-plane filter inside `_points_via_bbox`. | +| `src/presa_mano/perception/sam21_bridge.py` | modify | Apply the below-plane filter on `pts_cv` before the `<10` guard. | +| `src/presa_mano/api/perception_routes.py` | modify | Surface `plane_applied`/`n_points_before_plane` in `/scan` and `/object_points` responses. The filter itself runs inside the bridge / bbox helpers (already shared), so no logic change here. | +| `src/presa_mano/main.py` | modify | Register the new `work_plane_routes` router; update the cache-bust list (`servo.js` → `work_plane.js`). | +| `web/index.html` | modify | Rename the "Servo Visuale" tab to "Calibrazione Piano"; replace the panel content with the new layout. | +| `web/servo.js` | delete | Replaced by `web/work_plane.js`. | +| `web/work_plane.js` | new | ROI drawing on RGB, fit-preview call, 3D rendering of plane + colored scatter, offset slider, save / clear. | +| `web/style.css` | no change expected | Reuse existing `ctrl-block`, `kv compact`, `actions`, `info-box`, `marker-info` selectors. | +| `tests/test_work_plane_filter.py` | new | Unit test: synthetic plane + cloud, verify the filter keeps the right points for various offsets. | + +## GUI layout (Calibrazione Piano tab) + +``` +┌────────────────────────────────────────────────────────────────────────┐ +│ Tab: Calibrazione Piano status: piano salvato │ +├──────────────────────────────────────────┬─────────────────────────────┤ +│ LEFT │ RIGHT │ +│ │ │ +│ ┌─ RGB live (pan/zoom) ──────────────┐ │ ┌─ Info ─────────────────┐ │ +│ │ IMG + overlay rettangolo viola │ │ │ Calibra il piano: │ │ +│ │ shift+drag = disegna ROI │ │ │ shift+drag su RGB per │ │ +│ │ drag = pan · wheel = zoom │ │ │ definire una zona del │ │ +│ │ doppio click = reset │ │ │ tavolo (no oggetti), │ │ +│ └────────────────────────────────────┘ │ │ poi "Calcola piano". │ │ +│ │ └────────────────────────┘ │ +│ Vista 3D (camera frame) │ │ +│ ┌─ canvas Three.js ──────────────────┐ │ ROI: u=132,v=220 → 540,420 │ +│ │ • piano viola translucido │ │ offset (mm): [ 5.0 ] │ +│ │ • punti ROI: │ │ │ +│ │ verde = inliers │ │ [Calcola piano (preview)] │ +│ │ giallo = sopra plane + offset │ │ [Salva piano] │ +│ │ (filtro tiene) │ │ [Rimuovi piano salvato] │ +│ │ rosso = sotto plane − offset │ │ │ +│ │ (filtro scarta) │ │ Stato │ +│ │ grigio = buffer │ │ piano: saved (RMS 1.8mm) │ +│ │ drag = ruota · wheel = zoom │ │ n_inliers: 1240 │ +│ │ RMS fit: 1.8 mm │ │ preview not saved ⚠ │ +│ └────────────────────────────────────┘ │ │ +│ │ Dettagli piano │ +│ │
JSON│ +└──────────────────────────────────────────┴─────────────────────────────┘ +``` + +Button states: + +| State | Calcola | Salva | Rimuovi | +|---|---|---|---| +| No ROI | disabled | disabled | enabled iff a saved plane exists | +| ROI drawn, no preview | enabled | disabled | (same) | +| Preview computed, not saved | enabled (re-fit) | enabled | (same) | +| Plane saved | enabled (re-calibrate) | disabled (preview == saved) | enabled | + +The 3D viewer re-colours the scatter client-side when the offset slider +changes — no new backend call. The viewer initialises on tab open: if a +saved plane exists, it renders the plane mesh alone (no scatter, because the +ROI cloud is not persisted). + +## Error handling + +| Origin | Cause | Behaviour | +|---|---|---| +| Nuwa ROI fetch fails | camera down | 502 from `/work_plane/fit`, GUI shows error toast | +| `fit_plane_svd` `<10` inliers | ROI too small / depth mostly zero | 422 from `/work_plane/fit` with "ROI too sparse" | +| Negative `offset_mm` | client bug | 422 from `/work_plane/fit` and `/save` | +| ROI rectangle outside image | client bug | 422 (Pydantic validation) | +| `get_work_plane()` returns `None` | no plane calibrated | filter is a no-op, response has `plane_applied: false` | +| Filter drops `<10` points | bad calibration / object too small | `Sam21Fallback("too few points after plane filter")` for SAM21 path, `HTTPException(422)` for bbox path | + +## Testing + +**Unit** (`tests/test_work_plane_filter.py`): +- Synthetic plane at z=1.0 m with normal `[0, 0, -1]` (camera looking down). + Synthetic cloud of 100 points sampled in the band `0.95 ≤ z ≤ 1.05`. + - `offset_mm=0`: keeps exactly the points with `z ≤ 1.0` (above plane in + the camera-facing convention). + - `offset_mm=20`: keeps everything (band is ±50 mm but filter window is + ±20 mm and the plane sits at z=1.0). + - `offset_mm=100`: keeps everything (filter wider than the band). +- `get_work_plane()` with no file → `None`. +- `save` then `get` round-trip preserves `offset_mm`. +- Loading an old JSON without `offset_mm` field returns `WorkPlane` with + `offset_mm = 5.0`. + +**Live integration** (manual, against the running stack): +1. Open GUI tab "Calibrazione Piano", draw ROI on table, click "Calcola + piano". 3D shows green inliers tight along the plane mesh, RMS reported. +2. Adjust offset to 0; verify yellow points appear above the plane in + real-time without a new backend call. +3. Click "Salva piano". `data/work_plane.json` exists, content matches the + preview. +4. Switch to "TrovaUtensile", click "Pianifica grasp". Response has + `plane_applied: true`, `n_points_before_plane > n_points`. Scatter views + no longer show table-surface points. +5. Click "Rimuovi piano salvato". File is gone. `/api/grasp/plan` returns + `plane_applied: false`, `n_points_before_plane == n_points`. + +## Out-of-scope follow-ups (logged, not done) + +- The work plane lives in CV camera frame. If we ever need base-frame + reasoning ("filter below z=table in base frame"), we can add a + base-frame projection — but the current camera-frame filter is sufficient + because the object cloud lives in CV frame too. +- The current GUI does not visualise the *object* cloud post-filter in the + Calibrazione Piano tab. To see that, the user opens the TrovaUtensile + tab. Adding a "Test filter on object" button could be done later. +- The deleted `visual_servo` code remains in git history at the commit + before the removal, so a future re-introduction is a `git revert` away.