Presa-Mano/docs/superpowers/plans/2026-05-11-sam21-segmentation.md

# SAM21 segmentation in the grasp pipeline — Implementation Plan

> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.

**Goal:** Replace bbox-only point extraction with SAM21-segmented points in `/api/grasp/plan` and `/api/grasp/execute`, with automatic fallback to the legacy path when SAM21 is unreachable.

**Architecture:** New `Sam21Client` + `align_depth_to_rgb()` helper. The branch lives inside `_object_points_in_base()` in `grasp_routes.py`: if `grasp.use_sam21=true`, build an RGB-aligned depth image client-side, POST `(rgb, depth, intrinsics, bbox)` to SAM21, get points in OpenCV RGB-camera frame, transform to base. Any exception in this branch logs and falls back to the bbox path.

**Tech Stack:** Python 3.10 · FastAPI · `httpx` · `numpy` · `opencv-contrib-python-headless` (already deps) · `pytest` (added as dev dep).

**Design doc:** `docs/superpowers/specs/2026-05-11-sam21-segmentation-design.md`.

**Critical engineering facts** (verified by reading the SAM21 source):
- SAM21 expects RGB **PNG** base64 (not JPEG); cv2 round-trips JPEG→PNG cleanly because PNG stores channels in RGB order regardless of OpenCV's BGR memory layout.
- SAM21 expects depth as 16-bit single-channel PNG, values in mm, **same shape as RGB**.
- SAM21 returns `data.points` in **millimeters**, not meters — divide by 1000 before `transform_points(T_base_camera, ...)`.
- Nuwa intrinsics is a nested dict `{rgb: {fx,fy,cx,cy,width,height}, ir: {...}, extrinsics_ir_to_rgb: {...}}`. SAM21 wants only the `rgb` block.
- `CameraRegistration.from_nuwa()` (existing, in `perception/depth_to_rgb.py`) already parses both intrinsics blocks and the IR→RGB rigid transform — reuse it.

---

## File Structure

| File | Action | Responsibility |
|---|---|---|
| `pyproject.toml` | modify | Add `pytest` as optional `[dependency-groups] dev` dependency |
| `tests/__init__.py` | create | Empty package marker |
| `tests/conftest.py` | create | Pytest path setup so `presa_mano.*` imports work without install |
| `tests/test_depth_align.py` | create | Unit tests for `align_depth_to_rgb` |
| `tests/test_sam21_client.py` | create | Unit tests for `Sam21Client` (mocks `httpx.Client.post`) |
| `src/presa_mano/perception/depth_align.py` | create | `align_depth_to_rgb(depth_ir_mm, reg) -> depth_rgb_mm` |
| `src/presa_mano/clients/sam21.py` | create | `Sam21Client.segment_pointcloud(...) -> np.ndarray` (meters), `Sam21Unavailable`, `Sam21Error` |
| `src/presa_mano/clients/__init__.py` | modify | Re-export `Sam21Client`, `Sam21Unavailable`, `Sam21Error` |
| `src/presa_mano/api/_deps.py` | modify | Add `sam21()` factory |
| `config.yaml` | modify | Add `services.sam21.{base_url,timeout_s}` and `grasp.use_sam21` |
| `src/presa_mano/api/grasp_routes.py` | modify | Extract `_points_via_bbox`, add `_points_via_sam21`, insert SAM21 branch with fallback in `_object_points_in_base` |

---

## Task 0: Dev infrastructure (pytest scaffold)

**Files:**
- Modify: `pyproject.toml`
- Create: `tests/__init__.py`
- Create: `tests/conftest.py`

- [ ] **Step 1: Add pytest as optional dev dependency**

Edit `pyproject.toml`. Append after the `[build-system]` section (the file ends at line 22):

```toml
[dependency-groups]
dev = [
    "pytest>=8.0.0",
]
```

- [ ] **Step 2: Create tests directory with package marker**

Create `tests/__init__.py` with empty content (`""` is fine — write a single empty line so the file exists).

- [ ] **Step 3: Create conftest.py for src-layout import**

Create `tests/conftest.py`:

```python
"""Make `presa_mano.*` importable in tests without installing the package."""
from __future__ import annotations

import sys
from pathlib import Path

_SRC = Path(__file__).resolve().parent.parent / "src"
if str(_SRC) not in sys.path:
    sys.path.insert(0, str(_SRC))
```

- [ ] **Step 4: Install dev deps and verify pytest runs**

Run:

```bash
uv sync --group dev
uv run pytest --collect-only -q
```

Expected: pytest collects 0 tests, exits 0 (no errors). If you see `ModuleNotFoundError` or `error in conftest`, fix before moving on.

- [ ] **Step 5: Commit**

```bash
git add pyproject.toml tests/__init__.py tests/conftest.py uv.lock
git commit -m "test: add pytest dev dep + tests scaffold

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>"
```

---

## Task 1: `align_depth_to_rgb` (TDD)

**Files:**
- Create: `tests/test_depth_align.py`
- Create: `src/presa_mano/perception/depth_align.py`

The function vectorises the standard "rs.align(color)" operation: forward-project every IR pixel with a depth value into the RGB pixel grid, keeping the closest point per RGB pixel.

- [ ] **Step 1: Write the failing test**

Create `tests/test_depth_align.py`:

```python
"""Unit tests for align_depth_to_rgb."""
from __future__ import annotations

import numpy as np

from presa_mano.perception.depth_align import align_depth_to_rgb
from presa_mano.perception.depth_to_rgb import CameraRegistration


def _identity_reg(W: int = 64, H: int = 48) -> CameraRegistration:
    """Same K for RGB and IR, no extrinsic offset."""
    K = np.array([[60.0, 0.0, W / 2.0],
                  [0.0,  60.0, H / 2.0],
                  [0.0,  0.0,  1.0]], dtype=np.float64)
    return CameraRegistration(
        K_rgb=K.copy(), K_ir=K.copy(),
        R_ir_to_rgb=np.eye(3, dtype=np.float64),
        t_ir_to_rgb=np.zeros(3, dtype=np.float64),
        rgb_width=W, rgb_height=H,
        ir_width=W, ir_height=H,
    )


def test_identity_alignment_preserves_depth():
    """K_ir == K_rgb and no extrinsic → every IR pixel maps to itself."""
    reg = _identity_reg()
    depth_ir = np.full((reg.ir_height, reg.ir_width), 1000, dtype=np.uint16)
    depth_rgb = align_depth_to_rgb(depth_ir, reg)
    assert depth_rgb.shape == (reg.rgb_height, reg.rgb_width)
    assert depth_rgb.dtype == np.uint16
    nonzero = depth_rgb[depth_rgb > 0]
    # Coverage close to full (rounding can drop a few border pixels).
    assert nonzero.size >= 0.95 * depth_rgb.size, f"coverage too low: {nonzero.size}/{depth_rgb.size}"
    # All filled pixels equal the input depth (±1 mm rounding).
    assert np.all(np.abs(nonzero.astype(int) - 1000) <= 1)


def test_zero_depth_pixels_stay_zero():
    """IR pixels with depth=0 must not produce any RGB output."""
    reg = _identity_reg()
    depth_ir = np.zeros((reg.ir_height, reg.ir_width), dtype=np.uint16)
    depth_rgb = align_depth_to_rgb(depth_ir, reg)
    assert depth_rgb.shape == (reg.rgb_height, reg.rgb_width)
    assert int(depth_rgb.sum()) == 0


def test_sparse_pixels_preserved():
    """Two isolated IR depths produce exactly two filled RGB pixels (identity reg)."""
    reg = _identity_reg(W=64, H=48)
    depth_ir = np.zeros((reg.ir_height, reg.ir_width), dtype=np.uint16)
    depth_ir[0, 0] = 1000
    depth_ir[reg.ir_height - 1, reg.ir_width - 1] = 1000
    depth_rgb = align_depth_to_rgb(depth_ir, reg)
    assert (depth_rgb > 0).sum() == 2


def test_z_buffer_keeps_closer_point():
    """When two IR pixels collide on the same RGB pixel, the smaller depth wins.

    Force a collision with K_ir = 2 × K_rgb (same resolution), so adjacent IR
    pixels round to the same RGB pixel."""
    W = H = 16
    K_rgb = np.array([[60.0, 0.0, W / 2.0],
                      [0.0,  60.0, H / 2.0],
                      [0.0,  0.0,  1.0]], dtype=np.float64)
    K_ir = K_rgb.copy()
    K_ir[0, 0] *= 2.0   # double focal length → two IR pixels collapse to one RGB pixel
    K_ir[1, 1] *= 2.0
    reg = CameraRegistration(
        K_rgb=K_rgb, K_ir=K_ir,
        R_ir_to_rgb=np.eye(3, dtype=np.float64),
        t_ir_to_rgb=np.zeros(3, dtype=np.float64),
        rgb_width=W, rgb_height=H, ir_width=W, ir_height=H,
    )
    depth_ir = np.zeros((H, W), dtype=np.uint16)
    # Two IR pixels (5, 5) and (6, 5) both project to the same RGB pixel near (4, 5)
    # (when projected through K_ir with depth 1m and re-projected through K_rgb).
    depth_ir[5, 5] = 1500
    depth_ir[5, 6] = 800
    depth_rgb = align_depth_to_rgb(depth_ir, reg)
    nonzero = depth_rgb[depth_rgb > 0]
    # At least one RGB pixel must receive the closer (800) value; nothing further
    # may be ≥ 1500 if a collision actually occurred. If they didn't collide, we
    # see exactly two values [800, 1500]; if they did, we see only [800].
    assert nonzero.size in (1, 2)
    assert int(nonzero.min()) == 800
    if nonzero.size == 1:
        assert int(nonzero[0]) == 800   # collision happened, closer point won
```

- [ ] **Step 2: Run tests to verify they fail**

Run:

```bash
uv run pytest tests/test_depth_align.py -v
```

Expected: all 4 tests FAIL with `ModuleNotFoundError: No module named 'presa_mano.perception.depth_align'`.

- [ ] **Step 3: Implement `align_depth_to_rgb`**

Create `src/presa_mano/perception/depth_align.py`:

```python
"""Forward-project an IR depth image into RGB pixel coordinates.

Vectorised equivalent of librealsense `rs.align(stream.color)`. Each IR pixel
with valid depth is back-projected to 3D in the IR camera frame, transformed to
the RGB camera frame via the rigid extrinsics, and projected with RGB
intrinsics. On collisions the closest point wins (z-buffer).

Output is a uint16 array in millimeters, shape (rgb_height, rgb_width), with 0
on pixels that received no IR projection.
"""
from __future__ import annotations

import numpy as np

from .depth_to_rgb import CameraRegistration

_SENTINEL = np.iinfo(np.uint32).max


def align_depth_to_rgb(
    depth_ir_mm: np.ndarray,
    reg: CameraRegistration,
) -> np.ndarray:
    """Forward-project IR depth → RGB pixel grid.

    Parameters
    ----------
    depth_ir_mm : (H_ir, W_ir) uint16
        Depth in millimeters, IR camera frame. Zero means "no depth".
    reg : CameraRegistration
        IR and RGB intrinsics + IR→RGB rigid transform (translation in meters).

    Returns
    -------
    (reg.rgb_height, reg.rgb_width) uint16
        Depth in millimeters in RGB pixel coordinates; 0 where no IR pixel
        projected.
    """
    if depth_ir_mm.ndim != 2:
        raise ValueError(f"depth_ir_mm must be 2D, got shape {depth_ir_mm.shape}")
    if depth_ir_mm.shape != (reg.ir_height, reg.ir_width):
        raise ValueError(
            f"depth_ir_mm shape {depth_ir_mm.shape} ≠ IR resolution "
            f"({reg.ir_height}, {reg.ir_width}) from registration"
        )

    H_rgb, W_rgb = reg.rgb_height, reg.rgb_width
    H_ir, W_ir = reg.ir_height, reg.ir_width

    valid = depth_ir_mm > 0
    if not valid.any():
        return np.zeros((H_rgb, W_rgb), dtype=np.uint16)

    v_idx, u_idx = np.where(valid)
    z_ir_m = depth_ir_mm[v_idx, u_idx].astype(np.float64) / 1000.0

    # Back-project IR pixel → 3D point in IR camera frame.
    K_ir_inv = np.linalg.inv(reg.K_ir)
    pixels_h = np.vstack([u_idx.astype(np.float64),
                          v_idx.astype(np.float64),
                          np.ones_like(u_idx, dtype=np.float64)])
    rays_ir = K_ir_inv @ pixels_h
    P_ir = rays_ir * z_ir_m

    # IR camera frame → RGB camera frame.
    P_rgb = reg.R_ir_to_rgb @ P_ir + reg.t_ir_to_rgb.reshape(3, 1)

    z_rgb = P_rgb[2]
    in_front = z_rgb > 1e-6
    P_rgb = P_rgb[:, in_front]
    if P_rgb.shape[1] == 0:
        return np.zeros((H_rgb, W_rgb), dtype=np.uint16)

    z_rgb = P_rgb[2]
    u_rgb = reg.K_rgb[0, 0] * P_rgb[0] / z_rgb + reg.K_rgb[0, 2]
    v_rgb = reg.K_rgb[1, 1] * P_rgb[1] / z_rgb + reg.K_rgb[1, 2]

    u_int = np.round(u_rgb).astype(np.int64)
    v_int = np.round(v_rgb).astype(np.int64)
    z_mm = np.round(z_rgb * 1000.0).astype(np.int64)

    in_bounds = (
        (u_int >= 0) & (u_int < W_rgb)
        & (v_int >= 0) & (v_int < H_rgb)
        & (z_mm > 0) & (z_mm < 65535)
    )
    u_int = u_int[in_bounds]
    v_int = v_int[in_bounds]
    z_mm = z_mm[in_bounds].astype(np.uint32)
    if z_mm.size == 0:
        return np.zeros((H_rgb, W_rgb), dtype=np.uint16)

    # Z-buffer: initialise to sentinel, reduce-min, then sentinel→0.
    zbuf = np.full(H_rgb * W_rgb, _SENTINEL, dtype=np.uint32)
    flat_idx = v_int * W_rgb + u_int
    np.minimum.at(zbuf, flat_idx, z_mm)
    zbuf[zbuf == _SENTINEL] = 0
    return zbuf.reshape(H_rgb, W_rgb).astype(np.uint16)
```

- [ ] **Step 4: Run tests to verify they pass**

Run:

```bash
uv run pytest tests/test_depth_align.py -v
```

Expected: 4 PASS.

- [ ] **Step 5: Commit**

```bash
git add tests/test_depth_align.py src/presa_mano/perception/depth_align.py
git commit -m "feat(perception): vectorised IR→RGB depth alignment

Forward-project IR depth pixels into the RGB pixel grid with a z-buffer.
Output uint16 mm at RGB resolution, ready to feed SAM21.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>"
```

---

## Task 2: `Sam21Client` (TDD)

**Files:**
- Create: `tests/test_sam21_client.py`
- Create: `src/presa_mano/clients/sam21.py`

The client posts to `/api/segment_pointcloud`, unwraps the `{success, data, error}` envelope, normalises errors into two exception types, and returns an `(N, 3)` array in **meters**.

- [ ] **Step 1: Write the failing test**

Create `tests/test_sam21_client.py`:

```python
"""Unit tests for Sam21Client."""
from __future__ import annotations

import json
from unittest.mock import MagicMock, patch

import httpx
import numpy as np
import pytest

from presa_mano.clients.sam21 import Sam21Client, Sam21Error, Sam21Unavailable


def _intrinsics():
    return {"fx": 600.0, "fy": 600.0, "cx": 320.0, "cy": 240.0,
            "width": 640, "height": 480}


def _ok_response(points_mm):
    body = {
        "success": True,
        "data": {
            "n_points": len(points_mm),
            "n_pixels_in_mask": len(points_mm),
            "n_pixels_with_valid_depth": len(points_mm),
            "elapsed_ms": {"segment": 100.0, "project": 5.0, "total": 105.0},
            "points": [list(p) + [0.0, 0.0, 0.0] for p in points_mm],
        },
        "error": None,
    }
    return httpx.Response(200, json=body)


def _err_response(status: int, code: str, message: str):
    body = {"success": False, "data": None,
            "error": {"code": code, "message": message}}
    return httpx.Response(status, json=body)


def test_segment_pointcloud_happy_path():
    points_mm = [(100.0, 200.0, 1000.0), (110.0, 200.0, 1010.0)]
    response = _ok_response(points_mm)
    client = Sam21Client("http://sam21:8002", timeout_s=5.0)
    with patch("httpx.Client.post", return_value=response) as mock_post:
        out = client.segment_pointcloud(
            rgb_png=b"FAKE_PNG", depth_png=b"FAKE_DEPTH",
            intrinsics=_intrinsics(), bbox=(10, 20, 100, 200),
        )
    assert mock_post.call_count == 1
    posted_url = mock_post.call_args.args[0]
    assert posted_url.endswith("/api/segment_pointcloud")
    body = mock_post.call_args.kwargs["json"]
    assert body["bbox"] == [10, 20, 100, 200]
    assert body["depth_scale_mm"] == 1.0
    assert body["return_format"] == "json"
    assert body["return_mask"] is False
    assert isinstance(body["rgb_png_base64"], str) and len(body["rgb_png_base64"]) > 0
    assert isinstance(body["depth_png_base64"], str) and len(body["depth_png_base64"]) > 0
    # Output is in meters: mm/1000.
    assert out.shape == (2, 3)
    assert out.dtype == np.float64
    assert np.allclose(out, np.array([[0.1, 0.2, 1.0], [0.11, 0.2, 1.01]]))


def test_segment_pointcloud_connect_error_raises_unavailable():
    client = Sam21Client("http://sam21:8002")
    with patch("httpx.Client.post", side_effect=httpx.ConnectError("refused")):
        with pytest.raises(Sam21Unavailable):
            client.segment_pointcloud(
                rgb_png=b"x", depth_png=b"y",
                intrinsics=_intrinsics(), bbox=(0, 0, 10, 10),
            )


def test_segment_pointcloud_timeout_raises_unavailable():
    client = Sam21Client("http://sam21:8002")
    with patch("httpx.Client.post", side_effect=httpx.TimeoutException("timeout")):
        with pytest.raises(Sam21Unavailable):
            client.segment_pointcloud(
                rgb_png=b"x", depth_png=b"y",
                intrinsics=_intrinsics(), bbox=(0, 0, 10, 10),
            )


def test_segment_pointcloud_model_not_ready_raises_unavailable():
    """503 MODEL_NOT_READY is transient → treated as unavailable (fallback)."""
    response = _err_response(503, "MODEL_NOT_READY", "loading")
    client = Sam21Client("http://sam21:8002")
    with patch("httpx.Client.post", return_value=response):
        with pytest.raises(Sam21Unavailable):
            client.segment_pointcloud(
                rgb_png=b"x", depth_png=b"y",
                intrinsics=_intrinsics(), bbox=(0, 0, 10, 10),
            )


def test_segment_pointcloud_segmentation_empty_raises_error():
    """422 SEGMENTATION_EMPTY is a semantic outcome, not transport."""
    response = _err_response(422, "SEGMENTATION_EMPTY", "no mask")
    client = Sam21Client("http://sam21:8002")
    with patch("httpx.Client.post", return_value=response):
        with pytest.raises(Sam21Error) as info:
            client.segment_pointcloud(
                rgb_png=b"x", depth_png=b"y",
                intrinsics=_intrinsics(), bbox=(0, 0, 10, 10),
            )
    assert "SEGMENTATION_EMPTY" in str(info.value)


def test_segment_pointcloud_no_valid_depth_raises_error():
    response = _err_response(422, "NO_VALID_DEPTH", "mask found but no depth")
    client = Sam21Client("http://sam21:8002")
    with patch("httpx.Client.post", return_value=response):
        with pytest.raises(Sam21Error) as info:
            client.segment_pointcloud(
                rgb_png=b"x", depth_png=b"y",
                intrinsics=_intrinsics(), bbox=(0, 0, 10, 10),
            )
    assert "NO_VALID_DEPTH" in str(info.value)


def test_segment_pointcloud_empty_points_returns_empty_array():
    """Service returned success but 0 points — return (0, 3) array."""
    response = _ok_response([])
    client = Sam21Client("http://sam21:8002")
    with patch("httpx.Client.post", return_value=response):
        out = client.segment_pointcloud(
            rgb_png=b"x", depth_png=b"y",
            intrinsics=_intrinsics(), bbox=(0, 0, 10, 10),
        )
    assert out.shape == (0, 3)
```

- [ ] **Step 2: Run tests to verify they fail**

Run:

```bash
uv run pytest tests/test_sam21_client.py -v
```

Expected: all tests FAIL with `ModuleNotFoundError: No module named 'presa_mano.clients.sam21'`.

- [ ] **Step 3: Implement `Sam21Client`**

Create `src/presa_mano/clients/sam21.py`:

```python
"""Client for the SAM21 segmentation service (port 8002)."""
from __future__ import annotations

import base64
from typing import Any

import httpx
import numpy as np


class Sam21Unavailable(RuntimeError):
    """Transport-level failure (connection refused, timeout, model loading)."""


class Sam21Error(RuntimeError):
    """Semantic failure surfaced by the service (e.g. empty segmentation)."""

    def __init__(self, code: str, message: str):
        super().__init__(f"{code}: {message}")
        self.code = code
        self.message = message


_TRANSIENT_CODES = {"MODEL_NOT_READY"}


class Sam21Client:
    """Thin wrapper around POST /api/segment_pointcloud.

    Returns points in **meters**, OpenCV RGB-camera frame (X-right, Y-down,
    Z-forward) — directly compatible with `T_base_camera`.
    """

    def __init__(self, base_url: str, timeout_s: float = 10.0):
        self.base_url = base_url.rstrip("/")
        self.timeout_s = timeout_s

    def segment_pointcloud(
        self,
        rgb_png: bytes,
        depth_png: bytes,
        intrinsics: dict[str, Any],
        bbox: tuple[int, int, int, int],
        depth_scale_mm: float = 1.0,
    ) -> np.ndarray:
        body = {
            "rgb_png_base64": base64.b64encode(rgb_png).decode("ascii"),
            "depth_png_base64": base64.b64encode(depth_png).decode("ascii"),
            "intrinsics": {
                "fx": float(intrinsics["fx"]),
                "fy": float(intrinsics["fy"]),
                "cx": float(intrinsics["cx"]),
                "cy": float(intrinsics["cy"]),
                "width": int(intrinsics["width"]),
                "height": int(intrinsics["height"]),
            },
            "bbox": [int(bbox[0]), int(bbox[1]), int(bbox[2]), int(bbox[3])],
            "depth_scale_mm": float(depth_scale_mm),
            "return_format": "json",
            "return_mask": False,
        }
        url = f"{self.base_url}/api/segment_pointcloud"
        try:
            with httpx.Client(timeout=self.timeout_s) as c:
                r = c.post(url, json=body)
        except (httpx.ConnectError, httpx.TimeoutException, httpx.NetworkError) as e:
            raise Sam21Unavailable(f"sam21 unreachable: {e}") from e

        # Try to parse the envelope even on non-2xx, because SAM21 always
        # returns its `{success, data, error}` shape.
        try:
            payload = r.json()
        except ValueError as e:
            raise Sam21Unavailable(f"sam21 returned non-JSON (status {r.status_code}): {e}") from e

        if not isinstance(payload, dict):
            raise Sam21Unavailable(f"sam21 returned unexpected payload type: {type(payload)}")

        if payload.get("success") is True:
            data = payload.get("data") or {}
            points = data.get("points") or []
            if not points:
                return np.empty((0, 3), dtype=np.float64)
            arr_mm = np.asarray(points, dtype=np.float64)
            if arr_mm.ndim != 2 or arr_mm.shape[1] < 3:
                raise Sam21Unavailable(f"unexpected points shape: {arr_mm.shape}")
            return arr_mm[:, :3] / 1000.0

        err = payload.get("error") or {}
        code = str(err.get("code") or "UNKNOWN")
        msg = str(err.get("message") or f"http {r.status_code}")
        if code in _TRANSIENT_CODES:
            raise Sam21Unavailable(f"{code}: {msg}")
        raise Sam21Error(code, msg)
```

- [ ] **Step 4: Run tests to verify they pass**

Run:

```bash
uv run pytest tests/test_sam21_client.py -v
```

Expected: 7 PASS.

- [ ] **Step 5: Commit**

```bash
git add tests/test_sam21_client.py src/presa_mano/clients/sam21.py
git commit -m "feat(clients): Sam21Client wrapper for /api/segment_pointcloud

Posts (rgb_png, depth_png, intrinsics, bbox) and returns (N,3) points in
meters in OpenCV RGB-camera frame. Normalises transport vs semantic errors
into Sam21Unavailable / Sam21Error.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>"
```

---

## Task 3: Wire `Sam21Client` into the orchestrator

**Files:**
- Modify: `src/presa_mano/clients/__init__.py`
- Modify: `src/presa_mano/api/_deps.py`
- Modify: `config.yaml`

- [ ] **Step 1: Re-export Sam21 symbols**

Edit `src/presa_mano/clients/__init__.py` — change the imports and `__all__` to:

```python
"""HTTP clients for upstream microservices."""

from .g1 import G1Client, G1StatusError, G1Unavailable
from .marker import MarkerClient
from .nuwa import NuwaClient, NuwaError
from .sam21 import Sam21Client, Sam21Error, Sam21Unavailable
from .yolo import YoloClient

__all__ = [
    "G1Client", "G1StatusError", "G1Unavailable",
    "MarkerClient", "NuwaClient", "NuwaError",
    "Sam21Client", "Sam21Error", "Sam21Unavailable",
    "YoloClient",
]
```

- [ ] **Step 2: Add `sam21()` factory to `_deps.py`**

Edit `src/presa_mano/api/_deps.py`. Update the imports at line 9:

```python
from ..clients import G1Client, MarkerClient, NuwaClient, Sam21Client, YoloClient
```

Append a new factory after the existing `yolo()` function (around line 30, before `active_robot_key`):

```python
def sam21() -> Sam21Client:
    c = _svc(get_config(), "sam21")
    return Sam21Client(
        base_url=c.get("base_url", "http://localhost:8002"),
        timeout_s=float(c.get("timeout_s", 10.0)),
    )
```

- [ ] **Step 3: Add config entries**

Edit `config.yaml`. Inside the existing `services:` block (after the `h1:` entry, before `aruco:`), insert:

```yaml
  sam21:
    base_url: http://localhost:8002
    timeout_s: 10.0
```

Inside the existing `grasp:` block, add a new key after `pointcloud_max_depth_mm`:

```yaml
  use_sam21: true
```

- [ ] **Step 4: Smoke-test the factory**

Run:

```bash
uv run python -c "from presa_mano.api._deps import sam21; c = sam21(); print(c.base_url, c.timeout_s)"
```

Expected output: `http://localhost:8002 10.0`. If you get a `KeyError` or wrong values, fix before moving on.

- [ ] **Step 5: Commit**

```bash
git add src/presa_mano/clients/__init__.py src/presa_mano/api/_deps.py config.yaml
git commit -m "feat: wire Sam21Client into _deps and config

Adds services.sam21 (base_url + timeout_s) and grasp.use_sam21 toggle.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>"
```

---

## Task 4: Refactor `_object_points_in_base` (extract `_points_via_bbox`)

Pure refactor — no behaviour change. Splits the existing body into a helper so Task 5 can add the SAM21 branch cleanly.

**Files:**
- Modify: `src/presa_mano/api/grasp_routes.py`

- [ ] **Step 1: Read the current function**

Open `src/presa_mano/api/grasp_routes.py`. The current `_object_points_in_base` is at lines 43–88. Keep its public signature exactly the same.

- [ ] **Step 2: Replace the function with the refactored split**

Replace lines 43–88 (the whole `_object_points_in_base` function) with:

```python
def _object_points_in_base(conf: float | None, class_name: str | None, sw: Stopwatch
                           ) -> tuple[np.ndarray, dict[str, Any]]:
    cfg = get_config()
    nuwa = _deps.nuwa()
    yolo = _deps.yolo()

    side = cfg.get("robot", {}).get("wrist_side", "right")
    T_base_cam = CalibrationStore(_deps.calibration_file_path(side)).T_base_camera()
    if T_base_cam is None:
        raise HTTPException(status_code=412, detail=f"calibration missing for side='{side}': run /api/calibration/solve first")

    detector = ObjectDetector(yolo, nuwa)
    try:
        with sw.step("yolo"):
            dets = detector.detect(conf=conf)
    except HTTPException:
        raise
    except Exception as e:
        raise HTTPException(status_code=502, detail=f"YOLO detect failed: {e}")
    if class_name:
        dets = [d for d in dets if (d.get("class") or "").lower() == class_name.lower()]
    if not dets:
        raise HTTPException(status_code=404, detail="no object detected")
    dets.sort(key=lambda d: d["confidence"], reverse=True)
    top = dets[0]
    bbox = _bbox_to_roi(top["bbox"])

    pts_base = _points_via_bbox(nuwa, T_base_cam, bbox, sw)
    meta = {
        "detection": top,
        "T_base_camera": T_base_cam.tolist(),
        "n_points": int(len(pts_base)),
        "source": "bbox_roi",
    }
    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.

    Raises HTTPException(422) on <10 valid depth points (existing behaviour).
    """
    cfg = get_config()
    u_min, v_min, u_max, v_max = bbox
    try:
        with sw.step("pointcloud_roi"):
            pc, _meta = nuwa.pointcloud_roi(
                u_min, v_min, u_max, v_max,
                step=int(cfg.get("grasp", {}).get("pointcloud_step", 1)),
                max_depth_mm=int(cfg.get("grasp", {}).get("pointcloud_max_depth_mm", 2000)),
            )
    except Exception as e:
        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)}")
    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
```

Note: `_bbox_to_roi` (imported from `perception_routes` at the top of the file) returns `(u_min, v_min, u_max, v_max)`; the refactor stores that tuple in `bbox` and passes it to `_points_via_bbox`. The semantics are unchanged.

- [ ] **Step 3: Smoke-test that the orchestrator still imports cleanly**

Run:

```bash
uv run python -c "from presa_mano.api.grasp_routes import _object_points_in_base, _points_via_bbox; print('ok')"
```

Expected: `ok`.

- [ ] **Step 4: Commit**

```bash
git add src/presa_mano/api/grasp_routes.py
git commit -m "refactor(grasp): extract _points_via_bbox helper

Pure refactor in preparation for the SAM21 branch. _object_points_in_base
keeps its signature and the bbox→ROI→base-frame logic moves into the
private helper. Meta now includes source='bbox_roi'.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>"
```

---

## Task 5: Add SAM21 branch with automatic fallback

**Files:**
- Modify: `src/presa_mano/api/grasp_routes.py`

- [ ] **Step 1: Add imports and the fallback sentinel**

In `src/presa_mano/api/grasp_routes.py`, near the existing imports (after the `from ..clients import G1Unavailable` line), add:

```python
import base64
import logging

import cv2

from ..clients import Sam21Error, Sam21Unavailable
from ..perception.depth_align import align_depth_to_rgb
from ..perception.depth_to_rgb import CameraRegistration

log = logging.getLogger(__name__)


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."""
```

Place the `_Sam21Fallback` class definition just above the `_object_points_in_base` function (so it's in scope without forward references).

The codes that signal "the request was structurally wrong" (presa-mano bug) and deserve `ERROR` level rather than `WARNING`:

```python
_SAM21_BUG_CODES = frozenset({"SHAPE_MISMATCH", "BBOX_OUT_OF_BOUNDS",
                              "IMAGE_DECODE_ERROR", "INVALID_INPUT"})
```

Place this constant right after the `_Sam21Fallback` class.

- [ ] **Step 2: Add the `_points_via_sam21` helper**

Add this helper right after `_points_via_bbox`:

```python
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 any failure that
    should trigger the bbox path; lets unexpected exceptions propagate as
    _Sam21Fallback with repr(e).
    """
    try:
        with sw.step("nuwa_grab"):
            rgb_jpeg = nuwa.rgb_jpeg()
            depth_png_in = nuwa.depth_png()
            intr_full = nuwa.intrinsics()
        rgb_intr = intr_full.get("rgb")
        if not rgb_intr:
            raise _Sam21Fallback("nuwa intrinsics missing 'rgb' block")

        # 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. Verified by inspection.
        bgr = cv2.imdecode(np.frombuffer(rgb_jpeg, np.uint8), cv2.IMREAD_COLOR)
        if bgr is None:
            raise _Sam21Fallback("could not decode Nuwa RGB JPEG")
        ok, rgb_png_buf = cv2.imencode(".png", bgr)
        if not ok:
            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:
                raise _Sam21Fallback("could not decode Nuwa depth PNG")
            if depth_ir.dtype != np.uint16:
                raise _Sam21Fallback(f"depth PNG dtype expected uint16, got {depth_ir.dtype}")
            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:
                raise _Sam21Fallback("could not encode aligned depth as PNG")
            depth_png_out = bytes(depth_png_buf)
    except _Sam21Fallback:
        raise
    except Exception as 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=rgb_intr,
                bbox=bbox,
                depth_scale_mm=1.0,
            )
        except Sam21Unavailable as 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)
            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:
        raise _Sam21Fallback(f"too few points: {pts_cv.shape[0]}")
    return pts_cv
```

- [ ] **Step 3: Insert the SAM21 branch into `_object_points_in_base`**

Replace the body of `_object_points_in_base` (the version produced in Task 4) so that the SAM21 path runs before the bbox path. The full replacement of `_object_points_in_base`:

```python
def _object_points_in_base(conf: float | None, class_name: str | None, sw: Stopwatch
                           ) -> tuple[np.ndarray, dict[str, Any]]:
    cfg = get_config()
    nuwa = _deps.nuwa()
    yolo = _deps.yolo()

    side = cfg.get("robot", {}).get("wrist_side", "right")
    T_base_cam = CalibrationStore(_deps.calibration_file_path(side)).T_base_camera()
    if T_base_cam is None:
        raise HTTPException(status_code=412, detail=f"calibration missing for side='{side}': run /api/calibration/solve first")

    detector = ObjectDetector(yolo, nuwa)
    try:
        with sw.step("yolo"):
            dets = detector.detect(conf=conf)
    except HTTPException:
        raise
    except Exception as e:
        raise HTTPException(status_code=502, detail=f"YOLO detect failed: {e}")
    if class_name:
        dets = [d for d in dets if (d.get("class") or "").lower() == class_name.lower()]
    if not dets:
        raise HTTPException(status_code=404, detail="no object detected")
    dets.sort(key=lambda d: d["confidence"], reverse=True)
    top = dets[0]
    bbox = _bbox_to_roi(top["bbox"])
    meta: dict[str, Any] = {"detection": top, "T_base_camera": T_base_cam.tolist()}

    if cfg.get("grasp", {}).get("use_sam21", True):
        try:
            pts_cv = _points_via_sam21(nuwa, 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:
            log.warning("SAM21 path failed, falling back to bbox: %s", e)
            meta["sam21_fallback_reason"] = str(e)

    pts_base = _points_via_bbox(nuwa, T_base_cam, bbox, sw)
    meta["source"] = "bbox_roi"
    meta["n_points"] = int(len(pts_base))
    return pts_base, meta
```

Note the **critical frame convention difference**: the SAM21 branch does NOT call `nuwa_to_cv_frame` (SAM21 already returns OpenCV-frame points), whereas `_points_via_bbox` still calls it (legacy behaviour, preserves the existing path exactly).

- [ ] **Step 4: Smoke-test imports + happy reload**

```bash
uv run python -c "from presa_mano.api.grasp_routes import _object_points_in_base, _points_via_sam21, _points_via_bbox, _Sam21Fallback; print('ok')"
```

Expected: `ok`.

- [ ] **Step 5: Commit**

```bash
git add src/presa_mano/api/grasp_routes.py
git commit -m "feat(grasp): SAM21 segmentation branch with bbox fallback

When grasp.use_sam21=true (default), _object_points_in_base feeds the YOLO
bbox to SAM21 and uses the segmented cloud directly. Any failure
(transport, segmentation empty, no valid depth, payload bug) logs and falls
back to the legacy bbox path. Response meta includes source ∈ {sam21,
bbox_roi} and sam21_fallback_reason when applicable.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>"
```

---

## Task 6: Live integration verification

**Files:** none modified (verification only). At the end, optionally rebuild the Docker image.

- [ ] **Step 1: Make sure SAM21 is up**

```bash
docker compose -f /opt/docker/robotserver/docker-compose.yml up -d segmentation-sam21
docker compose -f /opt/docker/robotserver/docker-compose.yml ps segmentation-sam21
curl -fsS http://localhost:8002/api/health | jq .
```

Expected: `model_loaded: true`, `device` is `cuda` or `cpu`.

- [ ] **Step 2: Rebuild and start presa-mano with the new code**

```bash
docker compose -f /opt/docker/robotserver/docker-compose.yml up -d --build presa-mano
```

Wait for the healthcheck:

```bash
until curl -fsS http://localhost:8890/health >/dev/null 2>&1; do sleep 2; done
echo "presa-mano healthy"
```

- [ ] **Step 3: Verify the SAM21 happy path**

With an object visible to the camera:

```bash
curl -s -X POST http://localhost:8890/api/grasp/plan \
  -H 'Content-Type: application/json' -d '{}' | jq '{source, n_points, sam21_fallback_reason, timings_ms}'
```

Expected:
- `source: "sam21"`
- `n_points > 100` (typical for a hand-sized object)
- `sam21_fallback_reason` absent
- `timings_ms.sam21 < 1000`

- [ ] **Step 4: Verify the fallback path**

```bash
docker compose -f /opt/docker/robotserver/docker-compose.yml stop segmentation-sam21
curl -s -X POST http://localhost:8890/api/grasp/plan \
  -H 'Content-Type: application/json' -d '{}' | jq '{source, n_points, sam21_fallback_reason}'
docker compose -f /opt/docker/robotserver/docker-compose.yml start segmentation-sam21
```

Expected:
- `source: "bbox_roi"`
- `sam21_fallback_reason: "unreachable: ..."`
- `n_points` still reasonable (legacy path produces ROI cloud as before)

- [ ] **Step 5: Verify the empty-mask fallback**

Aim the camera at a featureless surface or move the object outside the frame, then:

```bash
curl -s -X POST http://localhost:8890/api/grasp/plan \
  -H 'Content-Type: application/json' -d '{"class_name":"hammer"}' | jq '{source, sam21_fallback_reason}'
```

If YOLO still detects something but SAM21 can't segment it, expect `source: "bbox_roi"`, `sam21_fallback_reason: "SEGMENTATION_EMPTY: ..."`. If YOLO detects nothing the orchestrator returns 404 (existing behaviour); that's fine, just confirm it doesn't crash.

- [ ] **Step 6: Tail logs and confirm no ERROR-level lines**

```bash
docker compose -f /opt/docker/robotserver/docker-compose.yml logs --since=2m presa-mano | grep -E 'ERROR|Traceback' || echo "no errors"
```

Expected: `no errors`. If you see any `ERROR` in SAM21-related lines (other than the explicit ones from `_SAM21_BUG_CODES`), investigate before declaring done.

- [ ] **Step 7: Final commit (only if anything new emerged)**

If verification revealed anything that needed a fix (e.g. config typo, missing import), commit those fixes here. Otherwise, this task ends with no commit and the work is done.

---

## Done criteria

- All pytest tests pass: `uv run pytest -v` → green.
- `/api/grasp/plan` returns `source: "sam21"` with SAM21 up.
- `/api/grasp/plan` returns `source: "bbox_roi"` + `sam21_fallback_reason` with SAM21 down.
- No `ERROR` lines in `presa-mano` logs during normal operation.
- The 7 commits from Tasks 0–6 are on the branch.