From 3b0a65554a5d614ae7a14105e3400b5e5769de88 Mon Sep 17 00:00:00 2001 From: Adriano Dal Pastro Date: Mon, 11 May 2026 16:52:17 +0200 Subject: [PATCH] feat: apply work-plane filter at all three cloud sites points_via_sam21, _points_via_bbox, and _object_cloud_cv (bbox branch) now call apply_below_plane_filter on the CV-frame cloud before the <10-points guard. Responses carry plane_applied + n_points_before_plane. Without a saved plane the filter is a no-op (plane_applied=false). Co-Authored-By: Claude Opus 4.7 (1M context) --- src/presa_mano/api/grasp_routes.py | 29 ++++++++++++++++------- src/presa_mano/api/perception_routes.py | 20 ++++++++++++---- src/presa_mano/perception/sam21_bridge.py | 18 ++++++++++---- 3 files changed, 50 insertions(+), 17 deletions(-) diff --git a/src/presa_mano/api/grasp_routes.py b/src/presa_mano/api/grasp_routes.py index 8792914..7d36d32 100644 --- a/src/presa_mano/api/grasp_routes.py +++ b/src/presa_mano/api/grasp_routes.py @@ -25,6 +25,7 @@ from ..grasp.visual_servo import ( ) from ..perception import ObjectDetector, filter_outliers, nuwa_to_cv_frame from ..perception.sam21_bridge import Sam21Fallback, points_via_sam21 +from ..perception.work_plane import apply_below_plane_filter, get_work_plane from .perception_routes import _bbox_to_roi from ..timing import Stopwatch from . import _deps @@ -71,27 +72,32 @@ def _object_points_in_base(conf: float | None, class_name: str | None, sw: Stopw if cfg.get("grasp", {}).get("use_sam21", True): try: - pts_cv = points_via_sam21(nuwa, _deps.sam21(), bbox, sw) + pts_cv, plane_info = points_via_sam21(nuwa, _deps.sam21(), bbox, sw) with sw.step("to_base_frame"): pts_base = transform_points(T_base_cam, pts_cv) meta["source"] = "sam21" meta["n_points"] = int(len(pts_base)) + meta["plane_applied"] = plane_info["plane_applied"] + meta["n_points_before_plane"] = plane_info["n_points_before_plane"] return pts_base, meta except Sam21Fallback as e: # Logging happens inside points_via_sam21 with the right level. meta["sam21_fallback_reason"] = str(e) - pts_base = _points_via_bbox(nuwa, T_base_cam, bbox, sw) + pts_base, plane_info = _points_via_bbox(nuwa, T_base_cam, bbox, sw) meta["source"] = "bbox_roi" meta["n_points"] = int(len(pts_base)) + meta["plane_applied"] = plane_info["plane_applied"] + meta["n_points_before_plane"] = plane_info["n_points_before_plane"] return pts_base, meta def _points_via_bbox(nuwa, T_base_cam: np.ndarray, bbox: tuple[int, int, int, int], - sw: Stopwatch) -> np.ndarray: - """Legacy path: bbox → Nuwa ROI cloud → filter outliers → base frame. + sw: Stopwatch) -> tuple[np.ndarray, dict]: + """Legacy path: bbox → Nuwa ROI cloud → outlier filter → plane filter → base frame. - Raises HTTPException(422) on <10 valid depth points (existing behaviour). + Raises HTTPException(422) on <10 valid depth points (after both outlier + filter and below-plane filter). Returns (pts_base, plane_info). """ cfg = get_config() u_min, v_min, u_max, v_max = bbox @@ -106,12 +112,17 @@ def _points_via_bbox(nuwa, T_base_cam: np.ndarray, bbox: tuple[int, int, int, in raise HTTPException(status_code=502, detail=f"Nuwa ROI pointcloud failed: {e}") with sw.step("filter_outliers"): cropped = filter_outliers(pc) - if len(cropped) < 10: - raise HTTPException(status_code=422, detail=f"too few valid depth points in bbox: {len(cropped)}") + pts_cv = nuwa_to_cv_frame(cropped[:, :3]) if len(cropped) else np.empty((0, 3), dtype=np.float64) + pts_cv, plane_info = apply_below_plane_filter(pts_cv, get_work_plane()) + if len(pts_cv) < 10: + raise HTTPException( + status_code=422, + detail=f"too few valid depth points after plane filter: {len(pts_cv)}" + f" (before plane: {plane_info['n_points_before_plane']})" + ) with sw.step("to_base_frame"): - pts_cv = nuwa_to_cv_frame(cropped[:, :3]) pts_base = transform_points(T_base_cam, pts_cv) - return pts_base + return pts_base, plane_info @router.post("/plan") diff --git a/src/presa_mano/api/perception_routes.py b/src/presa_mano/api/perception_routes.py index 4bf8b62..be91d83 100644 --- a/src/presa_mano/api/perception_routes.py +++ b/src/presa_mano/api/perception_routes.py @@ -12,6 +12,7 @@ from ..config import get_config from ..geometry import transform_points from ..perception import ObjectDetector, filter_outliers, nuwa_to_cv_frame from ..perception.sam21_bridge import Sam21Fallback, points_via_sam21 +from ..perception.work_plane import apply_below_plane_filter, get_work_plane from ..timing import Stopwatch from . import _deps @@ -110,6 +111,9 @@ def scan(req: ScanReq) -> dict[str, Any]: out["source"] = source if extra.get("sam21_fallback_reason"): out["sam21_fallback_reason"] = extra["sam21_fallback_reason"] + if "plane_applied" in extra: + out["plane_applied"] = extra["plane_applied"] + out["n_points_before_plane"] = extra["n_points_before_plane"] out["n_points"] = int(len(pts_cv)) out["points_camera"] = pts_cv.tolist() if len(pts_cv) else [] @@ -153,6 +157,9 @@ def object_points(req: DetectReq) -> dict[str, Any]: } if extra.get("sam21_fallback_reason"): out["sam21_fallback_reason"] = extra["sam21_fallback_reason"] + if "plane_applied" in extra: + out["plane_applied"] = extra["plane_applied"] + out["n_points_before_plane"] = extra["n_points_before_plane"] T_base_cam = _load_calibration() if T_base_cam is not None and len(pts_cv) > 0: @@ -177,7 +184,9 @@ def _object_cloud_cv(nuwa, bbox: tuple[int, int, int, int], max_points: int, extra: dict[str, Any] = {} if cfg.get("grasp", {}).get("use_sam21", True): try: - pts_cv = points_via_sam21(nuwa, _deps.sam21(), bbox, sw) + pts_cv, plane_info = points_via_sam21(nuwa, _deps.sam21(), bbox, sw) + extra["plane_applied"] = plane_info["plane_applied"] + extra["n_points_before_plane"] = plane_info["n_points_before_plane"] if len(pts_cv) > max_points: idx = np.random.choice(len(pts_cv), max_points, replace=False) pts_cv = pts_cv[idx] @@ -198,10 +207,13 @@ def _object_cloud_cv(nuwa, bbox: tuple[int, int, int, int], max_points: int, raise HTTPException(status_code=502, detail=f"Nuwa pointcloud ROI failed: {e}") with sw.step("filter_outliers"): cropped = filter_outliers(pc) - if len(cropped) > max_points: - idx = np.random.choice(len(cropped), max_points, replace=False) - cropped = cropped[idx] pts_cv = nuwa_to_cv_frame(cropped[:, :3]) if len(cropped) else np.empty((0, 3), dtype=np.float64) + pts_cv, plane_info = apply_below_plane_filter(pts_cv, get_work_plane()) + extra["plane_applied"] = plane_info["plane_applied"] + extra["n_points_before_plane"] = plane_info["n_points_before_plane"] + if len(pts_cv) > max_points: + idx = np.random.choice(len(pts_cv), max_points, replace=False) + pts_cv = pts_cv[idx] return pts_cv, "bbox_roi", extra diff --git a/src/presa_mano/perception/sam21_bridge.py b/src/presa_mano/perception/sam21_bridge.py index c488b60..7efdbfc 100644 --- a/src/presa_mano/perception/sam21_bridge.py +++ b/src/presa_mano/perception/sam21_bridge.py @@ -17,6 +17,7 @@ from ..clients import NuwaError, Sam21Error, Sam21Unavailable from ..timing import Stopwatch from .depth_align import align_depth_to_rgb from .depth_to_rgb import CameraRegistration +from .work_plane import apply_below_plane_filter, get_work_plane log = logging.getLogger(__name__) @@ -31,13 +32,17 @@ SAM21_BUG_CODES = frozenset({"SHAPE_MISMATCH", "BBOX_OUT_OF_BOUNDS", def points_via_sam21(nuwa, sam21, bbox: tuple[int, int, int, int], - sw: Stopwatch) -> np.ndarray: + sw: Stopwatch) -> tuple[np.ndarray, dict]: """SAM21 path: bbox → segmented point cloud in RGB-camera/OpenCV frame. Returns points in **meters**. Raises Sam21Fallback on failures that should trigger the bbox path. Re-raises HTTPException(502) for Nuwa failures (the bbox path would fail too — better to fail fast). All SAM21-specific logging happens here; callers do not log on fallback. + + After the SAM21 segmentation the work-plane filter is applied (no-op if no + plane is calibrated). Returns (points, plane_info) where plane_info has + `plane_applied` and `n_points_before_plane`. """ try: with sw.step("nuwa_grab"): @@ -120,7 +125,12 @@ def points_via_sam21(nuwa, sam21, bbox: tuple[int, int, int, int], log.error("Unexpected SAM21 error: %r", e) raise Sam21Fallback(f"unexpected: {e!r}") from e + pts_cv, plane_info = apply_below_plane_filter(pts_cv, get_work_plane()) if pts_cv.shape[0] < 10: - log.warning("SAM21 returned %d points (<10), falling back to bbox", pts_cv.shape[0]) - raise Sam21Fallback(f"too few points: {pts_cv.shape[0]}") - return pts_cv + log.warning("SAM21 cloud has %d points after plane filter (<10), falling back to bbox", + pts_cv.shape[0]) + raise Sam21Fallback( + f"too few points after plane filter: {pts_cv.shape[0]}" + f" (before plane: {plane_info['n_points_before_plane']})" + ) + return pts_cv, plane_info