Adriano/Presa-Mano
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

docs: design work-plane calibration + below-plane filter

Browse Source 
Repurpose the "Servo Visuale" tab into "Calibrazione Piano". Reuses the
existing WorkPlane + fit_plane_svd infrastructure (moved out of
grasp/visual_servo.py into perception/work_plane.py), extends WorkPlane
with offset_mm, splits fit (preview) from save (commit), and applies a
below-plane + offset filter to every object cloud (sam21_bridge,
_points_via_bbox, _object_cloud_cv bbox branch). Visual-servo feature
removed entirely; the only survivors are the plane bits.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
Adriano Dal PastroandClaude Opus 4.7 committed 2026-05-11 16:27:39 +02:00
1 parent 3b96589bf8
commit f7661a782a
1 file changed
+311
Download Patch File Download Diff File Expand all files Collapse all files
docs/superpowers/specs/2026-05-11-work-plane-filter-design.md
+311
View File
@@ -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 │
│ │ <pre> JSON </pre> │
└──────────────────────────────────────────┴─────────────────────────────┘
```
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.