From 3b96589bf8f19eca230c8b7ad1beff2b770eefce Mon Sep 17 00:00:00 2001 From: Adriano Dal Pastro Date: Mon, 11 May 2026 16:06:00 +0200 Subject: [PATCH] feat(perception): extend SAM21 to /scan and /object_points MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Extract the SAM21 wiring out of grasp_routes.py into a shared module perception/sam21_bridge.py (Sam21Fallback + points_via_sam21). Both /api/perception/scan and /api/perception/object_points now try SAM21 first and fall back to the bbox ROI path on any Sam21Fallback. Response now carries 'source' (sam21 | bbox_roi) and optional 'sam21_fallback_reason'. Side-effect: 'points_camera' on both endpoints is now in OpenCV RGB-camera frame (same as 'points_base' minus the T_base_camera transform), regardless of which path produced the cloud. Previously the bbox path stored points in Nuwa-native frame here. The web GUI plots points_base when available, so the scatter views automatically pick up the SAM21-segmented cloud. Verified live: /scan → source=sam21 n=2414 sam21=111ms; /object_points → source=sam21 n=2347 sam21=118ms; /grasp/plan regression → source=sam21 n=2390 sam21=119ms; /scan with SAM21 down → source=bbox_roi fallback='unreachable: ...' Co-Authored-By: Claude Opus 4.7 (1M context) --- src/presa_mano/api/grasp_routes.py | 123 +-------------------- src/presa_mano/api/perception_routes.py | 100 +++++++++-------- src/presa_mano/perception/sam21_bridge.py | 126 ++++++++++++++++++++++ 3 files changed, 186 insertions(+), 163 deletions(-) create mode 100644 src/presa_mano/perception/sam21_bridge.py diff --git a/src/presa_mano/api/grasp_routes.py b/src/presa_mano/api/grasp_routes.py index d613841..8792914 100644 --- a/src/presa_mano/api/grasp_routes.py +++ b/src/presa_mano/api/grasp_routes.py @@ -2,11 +2,8 @@ from __future__ import annotations -import logging from typing import Any, Optional -import cv2 -import httpx import numpy as np import time from dataclasses import asdict @@ -14,7 +11,7 @@ from fastapi import APIRouter, HTTPException from pydantic import BaseModel, Field from ..calibration import CalibrationStore -from ..clients import G1Unavailable, NuwaError, Sam21Error, Sam21Unavailable +from ..clients import G1Unavailable from ..config import get_config from ..geometry import transform_points from ..grasp import GraspPlanner, compute_axes @@ -27,27 +24,14 @@ from ..grasp.visual_servo import ( set_work_plane, ) from ..perception import ObjectDetector, filter_outliers, nuwa_to_cv_frame -from ..perception.depth_align import align_depth_to_rgb -from ..perception.depth_to_rgb import CameraRegistration +from ..perception.sam21_bridge import Sam21Fallback, points_via_sam21 from .perception_routes import _bbox_to_roi from ..timing import Stopwatch from . import _deps -log = logging.getLogger(__name__) - router = APIRouter() -class _Sam21Fallback(Exception): - """Internal sentinel: the SAM21 path could not produce a usable cloud. - Never escapes _object_points_in_base — caught and turned into a log line - + legacy bbox path.""" - - -_SAM21_BUG_CODES = frozenset({"SHAPE_MISMATCH", "BBOX_OUT_OF_BOUNDS", - "IMAGE_DECODE_ERROR", "INVALID_INPUT"}) - - class PlanReq(BaseModel): conf: Optional[float] = None class_name: Optional[str] = None @@ -87,14 +71,14 @@ 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, bbox, sw) + pts_cv = 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)) return pts_base, meta - except _Sam21Fallback as e: - # Logging happens inside _points_via_sam21 with the right level. + 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) @@ -130,103 +114,6 @@ def _points_via_bbox(nuwa, T_base_cam: np.ndarray, bbox: tuple[int, int, int, in return pts_base -def _points_via_sam21(nuwa, bbox: tuple[int, int, int, int], sw: Stopwatch) -> np.ndarray: - """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; the caller does not log on fallback. - """ - # Nuwa I/O: if it fails the bbox path will also fail, so fail fast with 502 - # rather than logging a deceptive "falling back to bbox" warning. - try: - with sw.step("nuwa_grab"): - rgb_jpeg = nuwa.rgb_jpeg() - depth_png_in = nuwa.depth_png() - intr_full = nuwa.intrinsics() - except (NuwaError, httpx.RequestError) as e: - raise HTTPException(status_code=502, detail=f"Nuwa unreachable: {e}") - - if not intr_full.get("rgb"): - log.warning("SAM21 path skipped: nuwa intrinsics missing 'rgb' block") - raise _Sam21Fallback("nuwa intrinsics missing 'rgb' block") - - try: - # JPEG → PNG. cv2.imdecode gives BGR; cv2.imencode(".png", bgr) writes - # PNG bytes with standard R,G,B color order (libpng handles the swap), - # which SAM21's PIL-based decoder reads as RGB. - bgr = cv2.imdecode(np.frombuffer(rgb_jpeg, np.uint8), cv2.IMREAD_COLOR) - if bgr is None: - log.warning("SAM21 path skipped: could not decode Nuwa RGB JPEG") - raise _Sam21Fallback("could not decode Nuwa RGB JPEG") - h_rgb, w_rgb = bgr.shape[:2] - ok, rgb_png_buf = cv2.imencode(".png", bgr) - if not ok: - log.warning("SAM21 path skipped: could not re-encode RGB as PNG") - raise _Sam21Fallback("could not re-encode RGB as PNG") - rgb_png = bytes(rgb_png_buf) - - with sw.step("depth_align"): - depth_ir = cv2.imdecode(np.frombuffer(depth_png_in, np.uint8), - cv2.IMREAD_UNCHANGED) - if depth_ir is None: - log.warning("SAM21 path skipped: could not decode Nuwa depth PNG") - raise _Sam21Fallback("could not decode Nuwa depth PNG") - if depth_ir.dtype != np.uint16: - log.warning("SAM21 path skipped: depth PNG dtype %s (expected uint16)", - depth_ir.dtype) - raise _Sam21Fallback(f"depth PNG dtype expected uint16, got {depth_ir.dtype}") - h_ir, w_ir = depth_ir.shape - # Nuwa /api/intrinsics omits width/height — inject the real ones - # from the decoded images so CameraRegistration and SAM21 agree on - # frame sizes. - intr_full = { - **intr_full, - "rgb": {**intr_full["rgb"], "width": w_rgb, "height": h_rgb}, - "ir": {**intr_full.get("ir", {}), "width": w_ir, "height": h_ir}, - } - reg = CameraRegistration.from_nuwa(intr_full) - depth_aligned = align_depth_to_rgb(depth_ir, reg) - ok, depth_png_buf = cv2.imencode(".png", depth_aligned) - if not ok: - log.warning("SAM21 path skipped: could not encode aligned depth as PNG") - raise _Sam21Fallback("could not encode aligned depth as PNG") - depth_png_out = bytes(depth_png_buf) - except _Sam21Fallback: - raise - except Exception as e: - log.error("Unexpected SAM21 prep error: %r", e) - raise _Sam21Fallback(f"prep failed: {e!r}") from e - - with sw.step("sam21"): - try: - pts_cv = _deps.sam21().segment_pointcloud( - rgb_png=rgb_png, - depth_png=depth_png_out, - intrinsics=intr_full["rgb"], - bbox=bbox, - depth_scale_mm=1.0, - ) - except Sam21Unavailable as e: - log.warning("SAM21 unreachable, falling back to bbox: %s", e) - raise _Sam21Fallback(f"unreachable: {e}") from e - except Sam21Error as e: - if e.code in _SAM21_BUG_CODES: - log.error("SAM21 rejected payload (%s): %s", e.code, e.message) - else: - log.warning("SAM21 returned %s: %s — falling back to bbox", e.code, e.message) - raise _Sam21Fallback(f"{e.code}: {e.message}") from e - except Exception as e: - log.error("Unexpected SAM21 error: %r", e) - raise _Sam21Fallback(f"unexpected: {e!r}") from e - - 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 - - @router.post("/plan") def plan(req: PlanReq) -> dict[str, Any]: sw = Stopwatch() diff --git a/src/presa_mano/api/perception_routes.py b/src/presa_mano/api/perception_routes.py index 37236d9..4bf8b62 100644 --- a/src/presa_mano/api/perception_routes.py +++ b/src/presa_mano/api/perception_routes.py @@ -11,6 +11,7 @@ from pydantic import BaseModel 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 ..timing import Stopwatch from . import _deps @@ -103,34 +104,18 @@ def scan(req: ScanReq) -> dict[str, Any]: out["timings_ms"] = sw.as_dict() return out - bbox = target["bbox"] - cfg = get_config() - step = int(cfg.get("grasp", {}).get("pointcloud_step", 1)) - max_depth = int(cfg.get("grasp", {}).get("pointcloud_max_depth_mm", 2000)) - u_min, v_min, u_max, v_max = _bbox_to_roi(bbox) - try: - with sw.step("pointcloud_roi"): - pc, _meta = nuwa.pointcloud_roi(u_min, v_min, u_max, v_max, - step=step, max_depth_mm=max_depth) - except HTTPException: - raise - except Exception as e: - raise HTTPException(status_code=502, detail=f"Nuwa pointcloud ROI failed: {e}") - - with sw.step("filter_outliers"): - cropped = filter_outliers(pc) - if len(cropped) > req.max_points: - idx = np.random.choice(len(cropped), req.max_points, replace=False) - cropped = cropped[idx] - + bbox = _bbox_to_roi(target["bbox"]) + pts_cv, source, extra = _object_cloud_cv(nuwa, bbox, req.max_points, sw) out["selected"] = target - out["n_points"] = int(len(cropped)) - out["points_camera"] = cropped[:, :3].tolist() if len(cropped) else [] + out["source"] = source + if extra.get("sam21_fallback_reason"): + out["sam21_fallback_reason"] = extra["sam21_fallback_reason"] + out["n_points"] = int(len(pts_cv)) + out["points_camera"] = pts_cv.tolist() if len(pts_cv) else [] T_base_cam = _load_calibration() - if T_base_cam is not None and len(cropped) > 0: + if T_base_cam is not None and len(pts_cv) > 0: with sw.step("to_base_frame"): - pts_cv = nuwa_to_cv_frame(cropped[:, :3]) pts_base = transform_points(T_base_cam, pts_cv) out["points_base"] = pts_base.tolist() out["T_base_camera"] = T_base_cam.tolist() @@ -157,12 +142,52 @@ def object_points(req: DetectReq) -> dict[str, Any]: raise HTTPException(status_code=404, detail="no object detected") dets.sort(key=lambda d: d["confidence"], reverse=True) top = dets[0] - bbox = top["bbox"] + bbox = _bbox_to_roi(top["bbox"]) + pts_cv, source, extra = _object_cloud_cv(nuwa, bbox, req.max_points, sw) + out: dict[str, Any] = { + "detection": top, + "source": source, + "n_points": int(len(pts_cv)), + "points_camera": pts_cv.tolist() if len(pts_cv) else [], + } + if extra.get("sam21_fallback_reason"): + out["sam21_fallback_reason"] = extra["sam21_fallback_reason"] + + T_base_cam = _load_calibration() + if T_base_cam is not None and len(pts_cv) > 0: + with sw.step("to_base_frame"): + pts_base = transform_points(T_base_cam, pts_cv) + out["points_base"] = pts_base.tolist() + out["T_base_camera"] = T_base_cam.tolist() + out["timings_ms"] = sw.as_dict() + return out + + +def _object_cloud_cv(nuwa, bbox: tuple[int, int, int, int], max_points: int, + sw: Stopwatch) -> tuple[np.ndarray, str, dict[str, Any]]: + """Get the object's point cloud in OpenCV RGB-camera frame (meters). + + Tries SAM21 first (if grasp.use_sam21 is true); on any non-fatal failure + falls back to the bbox ROI cloud + nuwa_to_cv_frame flip. Returns + `(pts_cv, source, extra)` where source ∈ {'sam21','bbox_roi'} and extra + may carry `sam21_fallback_reason`. Subsamples to `max_points` if needed. + """ cfg = get_config() + extra: dict[str, Any] = {} + if cfg.get("grasp", {}).get("use_sam21", True): + try: + pts_cv = points_via_sam21(nuwa, _deps.sam21(), bbox, sw) + 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, "sam21", extra + except Sam21Fallback as e: + extra["sam21_fallback_reason"] = str(e) + + u_min, v_min, u_max, v_max = bbox step = int(cfg.get("grasp", {}).get("pointcloud_step", 1)) max_depth = int(cfg.get("grasp", {}).get("pointcloud_max_depth_mm", 2000)) - u_min, v_min, u_max, v_max = _bbox_to_roi(bbox) try: with sw.step("pointcloud_roi"): pc, _meta = nuwa.pointcloud_roi(u_min, v_min, u_max, v_max, @@ -171,28 +196,13 @@ def object_points(req: DetectReq) -> dict[str, Any]: raise except Exception as e: raise HTTPException(status_code=502, detail=f"Nuwa pointcloud ROI failed: {e}") - with sw.step("filter_outliers"): cropped = filter_outliers(pc) - if len(cropped) > req.max_points: - idx = np.random.choice(len(cropped), req.max_points, replace=False) + if len(cropped) > max_points: + idx = np.random.choice(len(cropped), max_points, replace=False) cropped = cropped[idx] - - out: dict[str, Any] = { - "detection": top, - "n_points": int(len(cropped)), - "points_camera": cropped[:, :3].tolist(), - } - - T_base_cam = _load_calibration() - if T_base_cam is not None and len(cropped) > 0: - with sw.step("to_base_frame"): - pts_cv = nuwa_to_cv_frame(cropped[:, :3]) - pts_base = transform_points(T_base_cam, pts_cv) - out["points_base"] = pts_base.tolist() - out["T_base_camera"] = T_base_cam.tolist() - out["timings_ms"] = sw.as_dict() - return out + pts_cv = nuwa_to_cv_frame(cropped[:, :3]) if len(cropped) else np.empty((0, 3), dtype=np.float64) + return pts_cv, "bbox_roi", extra def _load_calibration() -> np.ndarray | None: diff --git a/src/presa_mano/perception/sam21_bridge.py b/src/presa_mano/perception/sam21_bridge.py new file mode 100644 index 0000000..c488b60 --- /dev/null +++ b/src/presa_mano/perception/sam21_bridge.py @@ -0,0 +1,126 @@ +"""Bridge: bbox → SAM21-segmented point cloud in OpenCV RGB-camera frame. + +Shared by /api/grasp/* and /api/perception/* endpoints. The helper is +stateless and pure HTTP-call wiring; callers pass in the Nuwa and SAM21 +clients built per-request via api/_deps.py. +""" +from __future__ import annotations + +import logging + +import cv2 +import httpx +import numpy as np +from fastapi import HTTPException + +from ..clients import NuwaError, Sam21Error, Sam21Unavailable +from ..timing import Stopwatch +from .depth_align import align_depth_to_rgb +from .depth_to_rgb import CameraRegistration + +log = logging.getLogger(__name__) + + +class Sam21Fallback(Exception): + """The SAM21 path could not produce a usable cloud. Callers should + fall back to the non-SAM21 path (bbox ROI cloud).""" + + +SAM21_BUG_CODES = frozenset({"SHAPE_MISMATCH", "BBOX_OUT_OF_BOUNDS", + "IMAGE_DECODE_ERROR", "INVALID_INPUT"}) + + +def points_via_sam21(nuwa, sam21, bbox: tuple[int, int, int, int], + sw: Stopwatch) -> np.ndarray: + """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. + """ + try: + with sw.step("nuwa_grab"): + rgb_jpeg = nuwa.rgb_jpeg() + depth_png_in = nuwa.depth_png() + intr_full = nuwa.intrinsics() + except (NuwaError, httpx.RequestError) as e: + raise HTTPException(status_code=502, detail=f"Nuwa unreachable: {e}") + + if not intr_full.get("rgb"): + log.warning("SAM21 path skipped: nuwa intrinsics missing 'rgb' block") + raise Sam21Fallback("nuwa intrinsics missing 'rgb' block") + + try: + # JPEG → PNG. cv2.imdecode gives BGR; cv2.imencode(".png", bgr) writes + # PNG bytes with standard R,G,B color order (libpng handles the swap), + # which SAM21's PIL-based decoder reads as RGB. + bgr = cv2.imdecode(np.frombuffer(rgb_jpeg, np.uint8), cv2.IMREAD_COLOR) + if bgr is None: + log.warning("SAM21 path skipped: could not decode Nuwa RGB JPEG") + raise Sam21Fallback("could not decode Nuwa RGB JPEG") + h_rgb, w_rgb = bgr.shape[:2] + ok, rgb_png_buf = cv2.imencode(".png", bgr) + if not ok: + log.warning("SAM21 path skipped: could not re-encode RGB as PNG") + raise Sam21Fallback("could not re-encode RGB as PNG") + rgb_png = bytes(rgb_png_buf) + + with sw.step("depth_align"): + depth_ir = cv2.imdecode(np.frombuffer(depth_png_in, np.uint8), + cv2.IMREAD_UNCHANGED) + if depth_ir is None: + log.warning("SAM21 path skipped: could not decode Nuwa depth PNG") + raise Sam21Fallback("could not decode Nuwa depth PNG") + if depth_ir.dtype != np.uint16: + log.warning("SAM21 path skipped: depth PNG dtype %s (expected uint16)", + depth_ir.dtype) + raise Sam21Fallback(f"depth PNG dtype expected uint16, got {depth_ir.dtype}") + h_ir, w_ir = depth_ir.shape + # Nuwa /api/intrinsics omits width/height — inject the real ones + # from the decoded images so CameraRegistration and SAM21 agree on + # frame sizes. + intr_full = { + **intr_full, + "rgb": {**intr_full["rgb"], "width": w_rgb, "height": h_rgb}, + "ir": {**intr_full.get("ir", {}), "width": w_ir, "height": h_ir}, + } + reg = CameraRegistration.from_nuwa(intr_full) + depth_aligned = align_depth_to_rgb(depth_ir, reg) + ok, depth_png_buf = cv2.imencode(".png", depth_aligned) + if not ok: + log.warning("SAM21 path skipped: could not encode aligned depth as PNG") + raise Sam21Fallback("could not encode aligned depth as PNG") + depth_png_out = bytes(depth_png_buf) + except Sam21Fallback: + raise + except Exception as e: + log.error("Unexpected SAM21 prep error: %r", e) + raise Sam21Fallback(f"prep failed: {e!r}") from e + + with sw.step("sam21"): + try: + pts_cv = sam21.segment_pointcloud( + rgb_png=rgb_png, + depth_png=depth_png_out, + intrinsics=intr_full["rgb"], + bbox=bbox, + depth_scale_mm=1.0, + ) + except Sam21Unavailable as e: + log.warning("SAM21 unreachable, falling back to bbox: %s", e) + raise Sam21Fallback(f"unreachable: {e}") from e + except Sam21Error as e: + if e.code in SAM21_BUG_CODES: + log.error("SAM21 rejected payload (%s): %s", e.code, e.message) + else: + log.warning("SAM21 returned %s: %s — falling back to bbox", e.code, e.message) + raise Sam21Fallback(f"{e.code}: {e.message}") from e + except Exception as e: + log.error("Unexpected SAM21 error: %r", e) + raise Sam21Fallback(f"unexpected: {e!r}") from e + + 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