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

feat(perception): extend SAM21 to /scan and /object_points

Browse Source 
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) <noreply@anthropic.com>
This commit is contained in:
Adriano Dal PastroandClaude Opus 4.7 committed 2026-05-11 16:06:00 +02:00
1 parent ba0259aa90
commit 3b96589bf8
3 files changed
+186 -163
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

src/presa_mano/api/grasp_routes.py
+5 -118
View File
@@ -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()
src/presa_mano/api/perception_routes.py
+55 -45
View File
@@ -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: