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) <noreply@anthropic.com>
This commit is contained in:
Adriano Dal PastroandClaude Opus 4.7 committed 2026-05-11 16:52:17 +02:00
1 parent 28ba0dd9f2
commit 3b0a65554a
3 files changed
+50 -17

No files matched your search

+20 -9
View File
@@ -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")
+16 -4
View File
@@ -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