"""DINO+VLM multiview perception: GDINO broad detect → crop+tournament → SAM3 → 3D fusion. Canonical script for the ``perceiving-objects`` skill bundle. The generated subgraph references this as a ``type: script`` state. Internally it calls: - ``grounding-dino.detect`` (broad ``object.`` text prompt) - ``vlm.query`` in a single-elimination **pairwise tournament** to pick which DINO box matches the target - ``sam3.segment_box`` to segment the chosen box - ``geometry.mask_to_world_points`` to fuse depth into a world-frame cloud Why a tournament instead of a one-shot box pick: the targets here are small (20-40 px in an 800x512 frame), so a Set-of-Marks overlay asking the VLM for a single letter out of N is unreliable — it cannot resolve the boxes. Instead each detection is cropped and upscaled, and the target is found via binary "A or B?" comparisons of upscaled crop pairs. VLMs are dramatically more reliable 2-way than N-way (cf. CropVLM, arXiv:2511.19820). On the LIBERO-PosVar object-ID study this lifted accuracy from ~30% (Set-of-Marks) to 97%. The VLM prompt template lives in ``prompts/vlm_pairwise.md`` and is loaded at call time via :func:`gap.skills.load_prompt`. """ from __future__ import annotations import hashlib import logging import os import pickle import re import tempfile from dataclasses import dataclass from pathlib import Path from typing import Any, TypedDict import numpy as np from gap import NodeContext from gap.skills import load_prompt from gap_core.types import BoundingBox2D, CameraFrame, Mask, PointCloud, pose_to_matrix logger = logging.getLogger(__name__) # Print the LLM-credentials hint at most once per process. We don't swallow # the auth error — the not_found exit must still fire so the subgraph # routes correctly — but we do tag it with an actionable hint the first # time so the user knows which env var to set. _AUTH_HINT_FIRED = False _AUTH_HINT = ( "no LLM credentials for the vlm tool bundle — the VLM tournament will" " raise and the subgraph will route to its not_found exit. To enable" " VLM disambiguation, set one of:\n" " openrouter: export OPENROUTER_API_KEY=...\n" " vertex: gcloud auth application-default login + export GOOGLE_CLOUD_PROJECT=...\n" " (or pin the bundle's own provider via GAP_VLM_PROVIDER /" " GAP_VLM_PROJECT_ID / GAP_VLM_BASE_URL).\n" " Use `gap check` to see which providers are configured." ) def _looks_like_auth_error(exc: BaseException) -> bool: """True when the VLM call failed because no credentials were set. Provider-neutral: catches HTTP 401 / "unauthorized" from the openrouter (OpenAI-compatible) endpoint, and the authentication / credential errors the Vertex (google-genai) client raises when Application Default Credentials are not configured. """ msg = str(exc).lower() return ( " 401 " in f" {msg} " or "unauthorized" in msg or "authentication" in msg or "credential" in msg or "api key" in msg or "api_key" in msg ) def _maybe_warn_auth_once(exc: BaseException) -> None: """Tag the first auth-class VLM failure with the credentials hint.""" global _AUTH_HINT_FIRED if _AUTH_HINT_FIRED or not _looks_like_auth_error(exc): return _AUTH_HINT_FIRED = True logger.warning("VLM call failed (auth): %s\nhint: %s", exc, _AUTH_HINT) _LABELS = ["A", "B", "C", "D", "E", "F", "G", "H"] _INTERSECTION_DIST = 0.01 _MIN_INTERSECTION = 1 # --- Skill-level cache ------------------------------------------------------ # Caches the full perceive output (found/cloud/mask/score) keyed on the # input camera frames + args. A hit short-circuits the entire # DINO+tournament+SAM path. Bump _CACHE_VERSION when the algorithm changes # meaningfully. # v4: the vlm bundle moved to deterministic temperature-0 decoding and an # explicit YES/NO-first verify elicitation — v3 entries hold picks made # under nondeterministic sampling and a verify gate that mislabeled # affirmative prose as "no". # v5: robot-point exclusion in the cloud path. # v6: wrist-support cloud fusion on the verified path. # v7: vlm bundle now inherits GAP_VLM_* from GAP_LLM_*; key off the # RESOLVED provider/model so a user who exports only GAP_LLM_* hits the # same cache entry as one who exports the equivalent GAP_VLM_*. # v8: wrist-support fusion now filters to the intersected subset rather # than dragging in the whole wrist cloud (see _filter_wrist_to_anchor). # v9: object_description is now ignored (see run()). A wrong LLM appearance # hint ("chocolate pudding ... small cup or tub" for a flat box) was steering # both the tournament and the verify to the cup/tub-shaped alphabet-soup can, # so perception returned the wrong object. Name-only is the validated # 99.5%-ID behavior; ID keys on object_name only now. _CACHE_VERSION = "14" _CACHE_ENABLED = os.environ.get("GAP_PERCEPTION_CACHE", "1") == "1" # Default to checkout-local .llm_cache/perceiving-objects/ so cache is # per-checkout and easy to wipe. Override with GAP_PERCEPTION_CACHE_DIR. _REPO_ROOT = Path(__file__).resolve().parents[3] _CACHE_DIR = Path(os.environ.get( "GAP_PERCEPTION_CACHE_DIR", str(_REPO_ROOT / ".llm_cache" / "perceiving-objects"), )) def _hash_camera(cam: CameraFrame) -> bytes: h = hashlib.sha256() h.update((cam["name"] or "").encode("utf-8")) h.update(b"|rgb|") h.update(np.ascontiguousarray(cam["rgb"]).tobytes()) h.update(b"|depth|") h.update(np.ascontiguousarray(cam["depth"]).tobytes()) h.update(b"|intr|") h.update(np.ascontiguousarray(cam["intrinsics"]).tobytes()) h.update(b"|pose|") h.update(repr(cam["pose"]).encode("utf-8")) return h.digest() def _resolved_vlm_provider() -> str: """Mirrors :func:`tools.vlm._resolve_provider` for the cache key — kept in-script because skill bundles don't import the vlm tool bundle.""" return ( os.environ.get("GAP_VLM_PROVIDER", "").strip().lower() or os.environ.get("GAP_LLM_PROVIDER", "").strip().lower() or "openrouter" ) def _resolved_vlm_model() -> str: """Mirrors :func:`tools.vlm._resolve_model` for the cache key.""" return ( os.environ.get("GAP_VLM_MODEL", "").strip() or os.environ.get("GAP_LLM_MODEL", "").strip() or "gemini-3.1-flash-lite-preview" ) def _make_cache_key(cameras: list[CameraFrame], args: dict) -> str: h = hashlib.sha256() h.update(f"v{_CACHE_VERSION}".encode()) h.update(b"|model|") # Hash the RESOLVED provider/model (post-inheritance from GAP_LLM_*) # so two equivalent configs share a cache entry. Swapping models # still invalidates because the resolved value changes. h.update(_resolved_vlm_provider().encode("utf-8")) h.update(b"/") h.update(_resolved_vlm_model().encode("utf-8")) for cam in cameras: h.update(b"|cam|") h.update(_hash_camera(cam)) h.update(b"|args|") h.update(repr(sorted(args.items())).encode("utf-8")) return h.hexdigest() def _cache_load(key: str) -> Output | None: p = _CACHE_DIR / f"{key}.pkl" if not p.exists(): return None try: with p.open("rb") as f: d = pickle.load(f) return {"found": d["found"], "cloud": d["cloud"], "mask": d["mask"], "score": d["score"]} except Exception as exc: # noqa: BLE001 logger.warning("perceiving-objects cache: load failed for %s: %s", key[:12], exc) return None def _cache_store(key: str, out: Output) -> None: try: _CACHE_DIR.mkdir(parents=True, exist_ok=True) blob = pickle.dumps( { "found": out["found"], "cloud": out["cloud"], "mask": out["mask"], "score": out["score"], }, protocol=pickle.HIGHEST_PROTOCOL, ) with tempfile.NamedTemporaryFile( dir=str(_CACHE_DIR), delete=False, suffix=".tmp", ) as tf: tf.write(blob) tmp = tf.name os.replace(tmp, _CACHE_DIR / f"{key}.pkl") except Exception as exc: # noqa: BLE001 logger.warning("perceiving-objects cache: store failed for %s: %s", key[:12], exc) def _empty_cloud() -> PointCloud: return {"points": np.zeros((0, 3), dtype=np.float32)} def _empty_mask() -> Mask: return np.zeros((0, 0), dtype=np.uint8) _COLORS = [ (255, 0, 0), (0, 255, 0), (0, 0, 255), (255, 255, 0), (0, 255, 255), (255, 0, 255), (255, 128, 0), (128, 0, 255), ] def _box_xyxy(box: BoundingBox2D) -> tuple[int, int, int, int]: return int(box["x1"]), int(box["y1"]), int(box["x2"]), int(box["y2"]) # Size guard for containment-NMS. A genuine sub-region fragment (logo, # label, illustration) is small in absolute pixels; a legitimate object # box that merely happens to sit inside a larger over-capture box is # not. Only a contained box whose longest side is below this many pixels # is dropped -- so a tight basket box contained in a basket+table # over-capture box is KEPT. Validated by the 800-frame perception_study # sweep (2026-05-21): the size-bounded variant `tourn_nmsz` matched the # unbounded one on the orange-juice partial-mask fix (10/40 -> 0) AND # eliminated the posvar/all basket backfire (7 degraded container masks # -> 0). See [[project_perception_mask_quality_study]]. _NMS_FRAGMENT_PX = 80 def _drop_contained_detections( detections: list, containment_thresh: float = 0.7, max_small_side: int = _NMS_FRAGMENT_PX, ) -> list: """Size-bounded containment / Intersection-over-Smaller NMS. GroundingDINO with a generic prompt like ``"object."`` routinely emits *both* a whole-object box and a sub-region box (an iconic label, logo, or part of the object). The downstream VLM tournament sees the sub-region crop as a higher-signal "looks like X" than the whole-object crop -- and picks it -- which produces a tiny mask and a perceived 5cm-tall object instead of the real 14cm carton. Drop detection ``A`` whenever a *larger* detection ``B`` exists with ``inter(A, B) / area(A) >= containment_thresh`` AND ``A`` is itself small (longest side < ``max_small_side`` px) -- i.e. a genuine logo/label fragment. A legitimate large object box contained inside an over-capture box is KEPT: the earlier unbounded variant dropped the tight basket box in favour of a basket+table box, which skewed the container OBB and the place pose. Standard IoU NMS doesn't catch the fragment case (a small nested box has low IoU with its big parent); IoS / containment is the appropriate metric. """ def _area(b: BoundingBox2D) -> float: return max(0.0, b["x2"] - b["x1"]) * max(0.0, b["y2"] - b["y1"]) def _inter(a: BoundingBox2D, b: BoundingBox2D) -> float: x1 = max(a["x1"], b["x1"]) y1 = max(a["y1"], b["y1"]) x2 = min(a["x2"], b["x2"]) y2 = min(a["y2"], b["y2"]) return max(0.0, x2 - x1) * max(0.0, y2 - y1) kept: list = [] dropped: list[tuple[int, int]] = [] for i, di in enumerate(detections): ai = _area(di["box"]) if ai <= 0: continue contained_in_j = -1 for j, dj in enumerate(detections): if j == i: continue aj = _area(dj["box"]) if aj <= ai: continue if _inter(di["box"], dj["box"]) / ai >= containment_thresh: contained_in_j = j break # Size guard: only drop a *small* contained box (a true fragment). # A large legitimate box is kept even when it is contained. if contained_in_j >= 0: longest = max(di["box"]["x2"] - di["box"]["x1"], di["box"]["y2"] - di["box"]["y1"]) if longest >= max_small_side: contained_in_j = -1 if contained_in_j < 0: kept.append(di) else: dropped.append((i, contained_in_j)) if dropped: logger.info( "containment-NMS dropped %d sub-region boxes (kept %d/%d): %s", len(dropped), len(kept), len(detections), ", ".join(f"{i}<<{j}" for i, j in dropped), ) return kept def _crop_region(rgb: np.ndarray, box: BoundingBox2D, pad: float = 0.30): """Crop `box` from `rgb` with `pad` fraction of box size on each side, clamped to the image. Returns the crop (or None if degenerate).""" h, w = rgb.shape[:2] x1, y1, x2, y2 = _box_xyxy(box) bw, bh = max(x2 - x1, 1), max(y2 - y1, 1) px, py = int(pad * bw), int(pad * bh) cx1, cy1 = max(0, x1 - px), max(0, y1 - py) cx2, cy2 = min(w, x2 + px), min(h, y2 + py) if cx2 <= cx1 or cy2 <= cy1: return None return rgb[cy1:cy2, cx1:cx2].copy() def _upscale_letterbox(crop: np.ndarray, size: int = 384) -> np.ndarray: """Resize so the longest side == size, letterbox onto neutral gray. Upscaling the tiny detections is the whole point.""" import cv2 h, w = crop.shape[:2] s = size / max(h, w) nh, nw = max(1, int(round(h * s))), max(1, int(round(w * s))) interp = cv2.INTER_CUBIC if s > 1 else cv2.INTER_AREA r = cv2.resize(crop, (nw, nh), interpolation=interp) canvas = np.full((size, size, 3), 128, np.uint8) oy, ox = (size - nh) // 2, (size - nw) // 2 canvas[oy:oy + nh, ox:ox + nw] = r return canvas def _make_pair_sheet(crop_a: np.ndarray, crop_b: np.ndarray, tile: int = 384) -> np.ndarray: """Two upscaled crops side by side with A / B colored headers.""" import cv2 pad = 6 cell = tile + 2 * pad sheet = np.full((cell, 2 * cell, 3), 255, np.uint8) for i, cr in enumerate((crop_a, crop_b)): t = _upscale_letterbox(cr, tile) cv2.rectangle(t, (0, 0), (tile - 1, 34), _COLORS[i], -1) cv2.putText(t, _LABELS[i], (8, 27), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255, 255, 255), 3, cv2.LINE_AA) x = i * cell + pad sheet[pad:pad + tile, x:x + tile] = t return sheet def _tournament( ctx: Any, rgb_np: np.ndarray, detections: list, object_name: str, object_description: str, ) -> int: """Single-elimination pairwise binary discrimination over detection crops. Returns the winning index into `detections`, or -1.""" crops: list[np.ndarray] = [] keep: list[int] = [] for i, det in enumerate(detections[:len(_LABELS)]): cr = _crop_region(rgb_np, det["box"]) if cr is not None: crops.append(cr) keep.append(i) if not crops: return -1 if len(crops) == 1: return keep[0] bracket = list(range(len(crops))) while len(bracket) > 1: nxt: list[int] = [] for j in range(0, len(bracket), 2): if j + 1 >= len(bracket): nxt.append(bracket[j]) # bye continue a, b = bracket[j], bracket[j + 1] sheet = _make_pair_sheet(crops[a], crops[b]) prompt = load_prompt( __package__, "vlm_pairwise", object_name=object_name, object_description=object_description, ) # Let VLM transport/auth errors propagate. The dev-era swallow # that picked `a` on any exception silently turned a missing # API key into "tournament picks the first bracket entry every # round" — both target ("milk carton") and container ("basket") # then resolved to box 0 (alphabet soup), grasp and drop # collapsed to the same XY, and the run reported SUCCESS with # the wrong object. Raising routes the subgraph to its # ``on_error: not_found`` exit, which is the correct outcome. # We DO tag the first auth-class failure with a credentials # hint before re-raising — without that the user sees only an # opaque "Could not resolve authentication method..." trace. try: resp = ctx.tool("vlm.query", prompt=prompt, image=sheet) except Exception as exc: _maybe_warn_auth_once(exc) raise win = a if _parse_letter(resp["text"], 2) == 0 else b nxt.append(win) bracket = nxt logger.warning("perception tournament(baseline): %d candidates -> %d VLM calls", len(crops), len(crops)-1) return keep[bracket[0]] # --------------------------------------------------------------------------- # Goal 1 — difficulty-adaptive perception budget. # Before the pairwise tournament, pre-rank the GDINO candidate boxes against the # TARGET, prune to the top-k, and skip the tournament entirely when the pre-ranker # is confident. The number of VLM round-trips then scales with scene difficulty # (an unambiguous scene needs 0 calls; a cluttered one a few) instead of a fixed # ~N-1 calls per perceive node. This also fixes target-1-style failures: boxes # that match nothing the target names (the robot arm, the basket for "cream # cheese box") score ~0 and are pruned BEFORE the VLM ever has to disambiguate. # # Fully OPT-IN — byte-identical to baseline unless GAP_PERCEPTION_PRERANK is set. # GAP_PERCEPTION_PRERANK off (default) | dino | clip — target-match scorer # GAP_PERCEPTION_TOPK int, 0 = keep all (default) — candidates kept for tournament # GAP_PERCEPTION_SKIP_MARGIN float, 0 = never skip — skip if (top1-top2) >= this # GAP_PERCEPTION_SKIP_MIN float, default 0.35 — and top1 >= this (abs floor) # Pruning can only REMOVE low-target-match boxes; it never adds a box the # tournament could not have chosen. Scorer scales differ (dino score 0..1, # CLIP cosine ~0.15..0.35) — tune SKIP_* to the scorer; defaults suit `dino`. _PRERANK = os.environ.get("GAP_PERCEPTION_PRERANK", "off").strip().lower() _PRERANK_TOPK = int(os.environ.get("GAP_PERCEPTION_TOPK", "0") or "0") _PRERANK_SKIP_MARGIN = float(os.environ.get("GAP_PERCEPTION_SKIP_MARGIN", "0") or "0") _PRERANK_SKIP_MIN = float(os.environ.get("GAP_PERCEPTION_SKIP_MIN", "0.35") or "0.35") _CLIP_STATE: dict = {"bad": False} # --------------------------------------------------------------------------- # Goal 2 — repair the failed perception node instead of aborting. # Today a perceive that finds nothing returns found=False and the subgraph routes # to its not_found exit -> abort (arm home, episode wasted). Instead, when enabled, # re-run perception with ESCALATED RECALL before giving up: first with the specific # target-NAME prompt (grounding-dino may catch an object the broad "object." pass # missed), then with a very permissive broad pass. Reuses Goal-1's target scoring to # still disambiguate. OPT-IN (default off) so baseline is unchanged. Since this only # fires on the not_found path — which otherwise just aborts — the extra detect/VLM # calls are spent recovering a would-be failure, not on the common success path. # GAP_PERCEPTION_REPAIR off (default) | on — enable the repair ladder _REPAIR = os.environ.get("GAP_PERCEPTION_REPAIR", "off").strip().lower() not in ("", "0", "off", "false") # (tag, box_threshold, text_threshold, min_score); tag "target" => dino prompt = the # object name, "object" => broad "object." prompt. Each rung relaxes recall further. _REPAIR_LADDER = [ ("target", 0.12, 0.12, 0.25), ("object", 0.08, 0.08, 0.20), ] def _iou(a: BoundingBox2D, b: BoundingBox2D) -> float: ax1, ay1, ax2, ay2 = _box_xyxy(a) bx1, by1, bx2, by2 = _box_xyxy(b) ix1, iy1 = max(ax1, bx1), max(ay1, by1) ix2, iy2 = min(ax2, bx2), min(ay2, by2) iw, ih = max(0, ix2 - ix1), max(0, iy2 - iy1) inter = iw * ih ua = (max(0, ax2 - ax1) * max(0, ay2 - ay1) + max(0, bx2 - bx1) * max(0, by2 - by1) - inter) return inter / ua if ua > 0 else 0.0 def _target_scores_dino(ctx: Any, cam_rgb, detections: list, object_name: str): """Score each candidate by target-NAME match via a SECOND grounding-dino pass with the specific target text (the first pass uses the broad ``object.`` prompt for recall). A candidate inherits the score of the best target-conditioned detection it overlaps (IoU >= 0.3); boxes the target text doesn't describe score ~0. Returns None on failure so the caller falls back to the tournament.""" try: q = object_name.strip().rstrip(".") if not q: return None resp = ctx.tool("grounding-dino.detect", image=cam_rgb, query=f"{q}.", box_threshold=0.10, text_threshold=0.10) tdets = list(resp.get("detections", [])) except Exception as e: logger.warning("prerank: dino target re-detect failed: %s", e) return None scores = [] for det in detections: best = 0.0 for td in tdets: if _iou(det["box"], td["box"]) >= 0.3: best = max(best, float(td.get("score", 0.0))) scores.append(best) return scores def _target_scores_clip(rgb_np, detections: list, object_name: str): """CLIP crop<->text cosine as target-match score. Lazy-loads a small open_clip model once; returns None (caller falls back) if CLIP/torch is unavailable in this bundle — so it never hard-fails a run.""" if _CLIP_STATE.get("bad"): return None try: import torch # type: ignore import open_clip # type: ignore from PIL import Image # type: ignore if _CLIP_STATE.get("model") is None: dev = "cuda" if torch.cuda.is_available() else "cpu" model, _, preprocess = open_clip.create_model_and_transforms( "ViT-B-32", pretrained="openai") _CLIP_STATE.update(model=model.eval().to(dev), pre=preprocess, tok=open_clip.get_tokenizer("ViT-B-32"), dev=dev) model, pre, tok, dev = (_CLIP_STATE["model"], _CLIP_STATE["pre"], _CLIP_STATE["tok"], _CLIP_STATE["dev"]) batch = [] for det in detections: cr = _crop_region(rgb_np, det["box"]) if cr is None: return None batch.append(pre(Image.fromarray(cr)).unsqueeze(0)) with torch.no_grad(): im = model.encode_image(torch.cat(batch).to(dev)) tx = model.encode_text(tok([f"a photo of a {object_name}"]).to(dev)) im = im / im.norm(dim=-1, keepdim=True) tx = tx / tx.norm(dim=-1, keepdim=True) return (im @ tx.T).squeeze(-1).float().cpu().tolist() except Exception as e: logger.warning("prerank: CLIP unavailable (%s); using dino scorer", e) _CLIP_STATE["bad"] = True return None def _prerank_and_prune(ctx: Any, cam, rgb_np, detections: list, object_name: str, object_description: str): """Returns (skip_idx, detections_to_use). If skip_idx is not None the caller uses it directly (0 VLM calls); otherwise it runs the tournament over the returned (possibly pruned/reordered) detections. No-op unless enabled.""" if _PRERANK == "off" or len(detections) <= 1: return None, detections scores = None if _PRERANK == "clip": scores = _target_scores_clip(rgb_np, detections, object_name) if scores is None: # dino requested, or clip unavailable scores = _target_scores_dino(ctx, cam["rgb"], detections, object_name) if scores is None or len(scores) != len(detections): return None, detections # scoring failed -> baseline tournament order = sorted(range(len(detections)), key=lambda i: scores[i], reverse=True) top1 = scores[order[0]] top2 = scores[order[1]] if len(order) > 1 else 0.0 n0 = len(detections) if (_PRERANK_SKIP_MARGIN > 0 and top1 >= _PRERANK_SKIP_MIN and (top1 - top2) >= _PRERANK_SKIP_MARGIN): logger.warning("perception prerank(%s): confident skip for '%s' " "(top1=%.2f top2=%.2f) -> 0 VLM calls", _PRERANK, object_name, top1, top2) return order[0], detections kept = order if _PRERANK_TOPK <= 0 else order[:max(1, _PRERANK_TOPK)] pruned = [detections[i] for i in kept] logger.warning("perception prerank(%s): '%s' %d -> %d candidates " "(top1=%.2f), tournament = %d VLM calls", _PRERANK, object_name, n0, len(pruned), top1, max(0, len(pruned) - 1)) return None, pruned @dataclass class _CamResult: cloud: Any mask: Any score: float box: Any = None # selected GDINO box (None on segment_text path) def _verify_pick( ctx: Any, rgb_np: np.ndarray, box: Any, object_name: str, object_description: str, default: bool, ) -> bool: """Focused yes/no on the chosen crop — 'is this close-up actually a ?'. The gate signal for the `safe` wrist-fallback policy. `default` is returned on a missing box / infra error: - exterior pick -> default True (verify error must NOT trigger a spurious wrist fallback; keep the exterior pick = zero-regression) - wrist pick -> default False (never accept an unverified wrist) """ if box is None: return default crop = _crop_region(rgb_np, box, pad=0.35) if crop is None: return default img = _upscale_letterbox(crop) q = f'Is the main object in this close-up a "{object_name}"?' if object_description: # Anchor the judgment on the caller-provided appearance hints — # without the explicit "judge by shape/colors" instruction the VLM # falls back to its semantic prior for the category name and # rejects correct picks whose upscaled low-res crop reads as a # generic object (e.g. the LIBERO cream-cheese box scored "a small # book" and the false NO forced the degraded wrist fallback). q += (f" It should look like: {object_description}." " Judge by the described shape and colors; printed text may" " be illegible at this resolution.") try: resp = ctx.tool("vlm.query_yes_no", prompt=q, image=img) logger.info( "verify_pick('%s'): answer=%s text=%r", object_name, resp["answer"], str(resp["text"])[:200]) return bool(resp["answer"]) except Exception as first_exc: # query_yes_no may be unavailable in a non-canonical vlm bundle; # fall back to the lower-level vlm.query in that case. But if # query ALSO fails — auth, network, retries exhausted — let it # propagate. The dev-era silent ``return default`` masked missing # credentials by handing the safe gate a synthetic "yes," which # is exactly how the dev-era milk-vs-soup run logged SUCCESS with # the wrong object. logger.warning( "verify_pick: vlm.query_yes_no failed (perceiving-objects), " "falling back to vlm.query: %s", first_exc) resp = ctx.tool("vlm.query", prompt=q + " Answer YES or NO.", image=img) logger.info( "verify_pick('%s') [query fallback]: text=%r", object_name, str(resp["text"])[:200]) return resp["text"].strip().upper().startswith("Y") def _parse_letter(text: str, n: int) -> int: text = text.strip().upper() if len(text) == 1 and text in _LABELS[:n]: return _LABELS.index(text) last_idx = -1 for match in re.finditer(r"\b([A-H])\b", text): letter = match.group(1) idx = _LABELS.index(letter) if letter in _LABELS[:n] else -1 if idx >= 0: last_idx = idx return last_idx def _is_wrist(cam: CameraFrame) -> bool: return bool(cam["name"]) and "eye_in_hand" in cam["name"] def _perceive_single_camera( ctx: Any, cam: CameraFrame, object_name: str, text_prompts: list[str], min_score: float, min_points: int, box_threshold: float, text_threshold: float, dino_prompt: str, object_description: str, run_identify: bool | None = None, ) -> _CamResult | None: is_wrist = _is_wrist(cam) # When unspecified, preserve the historical behavior (identify on # every non-wrist cam, segment_text-only on the wrist). The `safe` # gate in run() passes this explicitly. do_identify = (not is_wrist) if run_identify is None else run_identify seg_mask = None seg_score = 0.0 sel_box = None if do_identify: gdino_detections: list = [] try: gdino_resp = ctx.tool( "grounding-dino.detect", image=cam["rgb"], query=dino_prompt, box_threshold=box_threshold, text_threshold=text_threshold, ) gdino_detections = list(gdino_resp["detections"]) except Exception as e: logger.warning("GDINO detect failed (perceiving-objects): %s", e) # Containment-NMS: when DINO emits both a whole-object box and a # sub-region (logo / label / illustration) for the same object, # the VLM tournament tends to pick the higher-signal sub-region # crop. Drop sub-regions before the tournament so the fragment # can't win. See `_drop_contained_detections` for the rationale. if gdino_detections: gdino_detections = _drop_contained_detections(gdino_detections) if gdino_detections: rgb_np = cam["rgb"] # Goal 1 — difficulty-adaptive perception budget: pre-rank the # candidates against the target, prune to top-k, and skip the # tournament when confident (see _prerank_and_prune). No-op unless # GAP_PERCEPTION_PRERANK is set, so baseline is unchanged. _skip_idx, gdino_detections = _prerank_and_prune( ctx, cam, rgb_np, gdino_detections, object_name, object_description, ) # Tournament errors (VLM auth/transport) MUST propagate — the # dev-era catch here swallowed the inner raise and dropped # through to segment_text, which produced a non-empty cloud # for the WRONG object and the run reported success. Only # catch sam3.segment_box errors (they have a legitimate # fallback to segment_text below). if _skip_idx is not None: selected_idx = _skip_idx else: selected_idx = _tournament( ctx, rgb_np, gdino_detections, object_name, object_description, ) logger.info( "VLM tournament selected box %d for '%s' from %d detections", selected_idx, object_name, min(len(gdino_detections), len(_LABELS)), ) if 0 <= selected_idx < len(gdino_detections): selected_det = gdino_detections[selected_idx] sel_box = selected_det["box"] try: seg_resp = ctx.tool( "sam3.segment_box", image=cam["rgb"], box=selected_det["box"], ) if seg_resp["masks"] and seg_resp["scores"]: seg_mask = seg_resp["masks"][0] seg_score = seg_resp["scores"][0] except Exception as e: logger.warning( "sam3.segment_box failed (perceiving-objects); " "falling back to segment_text: %s", e) if seg_mask is None or seg_score < min_score: for prompt in text_prompts: seg_resp = ctx.tool( "sam3.segment_text", image=cam["rgb"], query=prompt, ) if seg_resp["masks"] and seg_resp["scores"] and seg_resp["scores"][0] >= min_score: seg_mask = seg_resp["masks"][0] seg_score = seg_resp["scores"][0] break if seg_mask is None or seg_score < min_score: logger.debug("No acceptable mask for '%s' in cam '%s'", object_name, cam["name"]) return None cloud = ctx.tool( "geometry.mask_to_world_points", mask=seg_mask, depth=cam["depth"], intrinsics=cam["intrinsics"], camera_pose=cam["pose"], )["points"] # Strip robot-body points: when the target sits against the robot base # the segmentation mask bleeds onto robot pixels and the merged cloud # yields a wildly oversized OBB (e.g. basket + robot column fused into # a half-metre blob centred nowhere). FK-sphere exclusion removes them; # non-7-DOF arms pass through unchanged inside the tool. try: obs = ctx.tool("robot.get_observation") joints = obs["arms"][0]["joint_state"] before = len(cloud["points"]) cloud = ctx.tool( "geometry.exclude_robot_points", points=cloud, joint_positions=joints, )["points"] removed = before - len(cloud["points"]) if removed: logger.info( "perceive '%s' cam '%s': excluded %d robot-near points " "(%d remain)", object_name, cam["name"], removed, len(cloud["points"]), ) except Exception as exc: logger.warning("robot-point exclusion skipped: %s", exc) num_points = len(cloud["points"]) if num_points < min_points: logger.debug( "Too few points (%d < %d) for '%s' in cam '%s'", num_points, min_points, object_name, cam["name"], ) return None cloud = _extend_cloud_to_support(ctx, cloud, seg_mask, cam) return _CamResult(cloud=cloud, mask=seg_mask, score=seg_score, box=sel_box) # --------------------------------------------------------------------------- # Static-container anchoring (loop reperception hygiene) # --------------------------------------------------------------------------- # In a pack-all loop the container is re-perceived every pass, and the # items already placed in/on it bleed into its segmentation: the basket # OBB center drifted +2 cm then +4 cm over one episode, dragging every # subsequent drop toward the rim. The container is STATIC, so the first # clean sighting's XY envelope is authoritative: later sightings drop # cloud points outside it (+ margin). Keyed per-process like the # decide_next_item progress guard (one episode == one PID). _CONTAINER_WORDS = ( "basket", "bin", "box", "container", "tote", "crate", "bowl", "tray", "plate", ) _CONTAINER_ENV_MARGIN_M = 0.015 def _static_container_anchor(object_name: str, out): if not out.get("found"): return out name = (object_name or "").lower() if not any(w in name for w in _CONTAINER_WORDS): return out try: pts = np.asarray(out["cloud"]["points"], dtype=np.float64).reshape(-1, 3) if len(pts) < 30: return out safe = "".join(ch if ch.isalnum() else "_" for ch in name)[:40] path = f"/tmp/.gap_container_cloud_{os.getpid()}_{safe}.npz" centroid = pts.mean(axis=0) cached = None try: d = np.load(path) cached = d["points"] except Exception: pass if cached is not None: drift = float(np.linalg.norm( centroid[:2] - np.asarray(cached, dtype=np.float64).mean(axis=0)[:2] )) if drift < 0.06: # Same (static) container: reuse the FIRST clean sighting's # cloud verbatim. Contents placed in/on it bleed into later # masks — the downstream cluster filter then locks onto a # wall+pile SUBSET and the OBB center walks toward the pile # (+2 then +4 cm over one episode), dragging every drop # toward the rim. Point-level filtering cannot fix a subset # bias; cloud reuse can, and the 6 cm drift gate still # refreshes if the container was actually moved. anchored = dict(out) cloud = dict(out["cloud"]) cloud["points"] = np.asarray(cached, dtype=np.float32) cloud.pop("colors", None) anchored["cloud"] = cloud logger.info( "static-container anchor '%s': reusing first-sighting " "cloud (new sighting drifted %.0f mm)", object_name, drift * 1000, ) return anchored logger.info( "static-container anchor '%s': %.0f mm drift exceeds the " "moved-container gate; refreshing the cached cloud", object_name, drift * 1000, ) try: np.savez_compressed(path, points=pts.astype(np.float32)) except Exception: pass return out except Exception as exc: logger.warning("static-container anchor skipped: %s", exc) return out # --------------------------------------------------------------------------- # Amodal bottom completion (low-profile objects) # --------------------------------------------------------------------------- # A top-down view of a low object (the 2 cm cream-cheese / butter boxes) # yields a top-face-only cloud whose OBB has ~zero vertical extent; the # grasp then closes ABOVE the object and grabs air. When the object cloud # floats above the LOCAL support plane -- estimated from a dilated ring of # non-object pixels around the mask -- mirror the footprint down to the # support height so the OBB regains the object's true vertical extent. # Tall objects whose sides are observed are a no-op (their cloud already # reaches the support plane). _SUPPORT_GAP_MIN_M = 0.012 _SUPPORT_RING_PX = 12 _SUPPORT_MAX_SYNTH = 2000 def _extend_cloud_to_support(ctx, cloud, seg_mask, cam): try: pts = np.asarray(cloud["points"], dtype=np.float64).reshape(-1, 3) if len(pts) < 20: return cloud from scipy import ndimage m = np.asarray(seg_mask) > 0 ring = ndimage.binary_dilation(m, iterations=_SUPPORT_RING_PX) & ~m ring_resp = ctx.tool( "geometry.mask_to_world_points", mask=(ring.astype(np.uint8) * 255), depth=cam["depth"], intrinsics=cam["intrinsics"], camera_pose=cam["pose"], )["points"] ring_pts = np.asarray( ring_resp["points"], dtype=np.float64 ).reshape(-1, 3) if len(ring_pts) < 50: return cloud z = pts[:, 2] z_hi = float(np.percentile(z, 98.0)) z_lo = float(z.min()) # TOP SLAB: the one surface a tabletop camera sees completely, so # its footprint is an unbiased estimate of the object's # cross-section. A one-sided view biases the FULL cloud's centroid # toward the camera (a milk carton read +7 mm off-center with half # its true depth); the slab centroid stays true. slab_band = max(0.012, 0.15 * (z_hi - z_lo)) slab = pts[z > z_hi - slab_band] if len(slab) < 15: return cloud centroid_bias = float( np.linalg.norm(slab[:, :2].mean(axis=0) - pts[:, :2].mean(axis=0)) ) # Complete column + unbiased -> nothing to fix, skip the ring call. if centroid_bias < 0.004 and (z_hi - z_lo) > 0.03: return cloud from scipy import ndimage m = np.asarray(seg_mask) > 0 ring = ndimage.binary_dilation(m, iterations=_SUPPORT_RING_PX) & ~m ring_resp = ctx.tool( "geometry.mask_to_world_points", mask=(ring.astype(np.uint8) * 255), depth=cam["depth"], intrinsics=cam["intrinsics"], camera_pose=cam["pose"], )["points"] ring_pts = np.asarray( ring_resp["points"], dtype=np.float64 ).reshape(-1, 3) support_z = ( float(np.percentile(ring_pts[:, 2], 20.0)) if len(ring_pts) >= 50 else z_lo ) floating = (float(np.median(z)) - support_z) >= _SUPPORT_GAP_MIN_M if centroid_bias < 0.004 and not floating: return cloud sel_idx = np.arange(len(slab)) if len(sel_idx) > _SUPPORT_MAX_SYNTH: sel_idx = np.linspace( 0, len(slab) - 1, _SUPPORT_MAX_SYNTH ).astype(int) base = slab[sel_idx] lo_lv = support_z + 0.002 hi_lv = max(z_hi - 0.004, lo_lv + 0.001) n_lv = max(2, min(12, int((hi_lv - lo_lv) / 0.005))) # Extrude the slab footprint from the support plane up to the slab: # a connected prism (DBSCAN keeps it) that can only GROW the OBB # toward the true shape — complete clouds gain nothing. layers = [pts] for lv in np.linspace(lo_lv, hi_lv, n_lv): layer = base.copy() layer[:, 2] = lv layers.append(layer) out = dict(cloud) out["points"] = np.concatenate(layers, axis=0).astype(np.float32) if out.get("colors") is not None and len(out["colors"]) == len(pts): colors = np.asarray(out["colors"], dtype=np.float32).reshape(-1, 3) out["colors"] = np.concatenate( [colors] + [np.full((len(base), 3), colors.mean(axis=0), dtype=np.float32)] * n_lv, axis=0, ) logger.info( "amodal completion: slab-centroid bias %.1f mm, floating=%s -> " "extruded %d slab points across %d levels (support_z=%.3f)", centroid_bias * 1000, floating, len(base), n_lv, support_z, ) return out except Exception as exc: logger.warning("support-plane completion skipped: %s", exc) return cloud def _merge_multiview(results: list[_CamResult]) -> PointCloud: from scipy.spatial import cKDTree if len(results) < 2: return results[0].cloud clouds_np: list[np.ndarray] = [] for r in results: pts = np.asarray(r.cloud["points"], dtype=np.float64) if len(pts) > 0: clouds_np.append(pts.reshape(-1, 3)) else: clouds_np.append(np.zeros((0, 3))) pts_a, pts_b = clouds_np[0], clouds_np[1] do_merge = False if len(pts_a) > 0 and len(pts_b) > 0: tree = cKDTree(pts_a) dists, _ = tree.query(pts_b) intersection = int(np.sum(dists < _INTERSECTION_DIST)) logger.info( "Multiview intersection: %d points within %.0fmm " "(views have %d and %d points)", intersection, _INTERSECTION_DIST * 1000, len(pts_a), len(pts_b), ) do_merge = intersection >= _MIN_INTERSECTION if do_merge: all_points = np.concatenate( [np.asarray(r.cloud["points"], dtype=np.float32).reshape(-1, 3) for r in results], axis=0, ) merged: PointCloud = {"points": all_points} if all(r.cloud.get("colors") is not None for r in results): merged["colors"] = np.concatenate( [np.asarray(r.cloud["colors"], dtype=np.float32).reshape(-1, 3) for r in results], axis=0, ) return merged best = max(results, key=lambda r: r.score) logger.info( "Multiview disagreement: using single view (score=%.3f)", best.score, ) return best.cloud def _cloud_pts(cloud: PointCloud) -> np.ndarray: pts = np.asarray(cloud["points"], dtype=np.float64) return pts.reshape(-1, 3) if pts.size else np.zeros((0, 3)) def _clouds_intersect(anchor_pts: np.ndarray, cloud: PointCloud) -> bool: """Dev-era multiview fusion guard: the candidate cloud must share at least ``_MIN_INTERSECTION`` points within ``_INTERSECTION_DIST`` of the anchor cloud — i.e. both views observed the same physical surface.""" from scipy.spatial import cKDTree pts = _cloud_pts(cloud) if len(anchor_pts) == 0 or len(pts) == 0: return False dists, _ = cKDTree(anchor_pts).query(pts) n = int(np.sum(dists < _INTERSECTION_DIST)) logger.info("wrist-support intersection: %d points within %.0fmm", n, _INTERSECTION_DIST * 1000) return n >= _MIN_INTERSECTION def _filter_wrist_to_anchor( anchor_pts: np.ndarray, cloud: PointCloud, ) -> np.ndarray: """Return only the wrist points within ``_INTERSECTION_DIST`` of the anchor — the corrected fusion semantics. The original ``_collect_wrist_support`` used ``_clouds_intersect`` as a binary all-or-nothing gate: if *any* wrist point intersected the exterior cloud, the *entire* wrist cloud was fused. A low-score wrist mask (e.g. score 0.535 on LIBERO milk-carton) that grazed the exterior cloud could drag 1.6k off-target points (the table, a neighboring soup can) into the fused cloud, shifting the OBB centroid by 5+ cm in X and producing a top-down grasp that closed on empty air. Restricting fusion to the actually-intersecting subset keeps the wrist contribution where it agrees with the exterior view and drops contamination.""" from scipy.spatial import cKDTree pts = _cloud_pts(cloud) if len(anchor_pts) == 0 or len(pts) == 0: return np.zeros((0, 3), dtype=np.float64) dists, _ = cKDTree(anchor_pts).query(pts) return pts[dists < _INTERSECTION_DIST] def _segment_wrist_by_projection( ctx: Any, cam: CameraFrame, anchor_pts: np.ndarray, min_score: float, min_points: int, ) -> _CamResult | None: """Cross-view segmentation: seed ``sam3.segment_box`` on the wrist frame with the 2-98 percentile bbox of the verified exterior cloud projected into the wrist camera. Geometry-guided (no extra VLM call): the exterior cloud IS the verified object, so its wrist-frame projection bounds the object's visible wrist pixels; SAM grows the seed box to the full visible extent (the top face the exterior view cannot see).""" if len(anchor_pts) < 10: return None try: K = np.asarray(cam["intrinsics"], dtype=np.float64) T_world_to_cam = np.linalg.inv(pose_to_matrix(cam["pose"])) ph = np.hstack([anchor_pts, np.ones((len(anchor_pts), 1))]) pc = (T_world_to_cam @ ph.T).T[:, :3] z = pc[:, 2] ok = z > 1e-3 if int(ok.sum()) < 10: return None u = K[0, 0] * pc[ok, 0] / z[ok] + K[0, 2] v = K[1, 1] * pc[ok, 1] / z[ok] + K[1, 2] H, W = np.asarray(cam["depth"]).shape inb = (u >= 0) & (u < W) & (v >= 0) & (v < H) if int(inb.sum()) < 10: logger.info("wrist-support: object not visible in '%s' " "(%d projected px in bounds)", cam["name"], inb.sum()) return None u, v = u[inb], v[inb] x1, x2 = np.percentile(u, [2, 98]) y1, y2 = np.percentile(v, [2, 98]) pad = 0.15 * max(x2 - x1, y2 - y1) box: BoundingBox2D = { "x1": float(max(0.0, x1 - pad)), "y1": float(max(0.0, y1 - pad)), "x2": float(min(W - 1.0, x2 + pad)), "y2": float(min(H - 1.0, y2 + pad)), } seg = ctx.tool("sam3.segment_box", image=cam["rgb"], box=box) if not seg["masks"] or not seg["scores"] or seg["scores"][0] < min_score: return None mask, score = seg["masks"][0], seg["scores"][0] cloud = ctx.tool( "geometry.mask_to_world_points", mask=mask, depth=cam["depth"], intrinsics=cam["intrinsics"], camera_pose=cam["pose"], )["points"] try: obs = ctx.tool("robot.get_observation") cloud = ctx.tool( "geometry.exclude_robot_points", points=cloud, joint_positions=obs["arms"][0]["joint_state"], )["points"] except Exception as exc: # noqa: BLE001 logger.warning("wrist-support robot-point exclusion skipped: %s", exc) if len(cloud["points"]) < min_points: return None cloud = _extend_cloud_to_support(ctx, cloud, mask, cam) return _CamResult(cloud=cloud, mask=mask, score=score) except Exception as exc: # noqa: BLE001 logger.warning("wrist-support projection seeding failed: %s", exc) return None class Output(TypedDict): found: bool cloud: PointCloud mask: Mask score: float def run( ctx: NodeContext, cameras: list[CameraFrame], object_name: str, text_prompts: list[str] | None = None, min_points: int = 10, min_score: float = 0.3, use_multiview: bool = True, box_threshold: float = 0.20, text_threshold: float = 0.20, dino_prompt: str = "object.", object_description: str = "", ) -> Output: """Execute DINO+VLM perception with the `safe` wrist-fallback gate. Default to the exterior (e.g. agentview) identification; consult the wrist (eye-in-hand) view ONLY when the exterior pick fails its own close-up verify AND the wrist pick passes its own. On the LIBERO-PosVar object-ID study (4 PosVar suites, 200 frames, `scripts/analyze_wrist_regression.py`) this `safe` gate was the only zero-regression policy: +2.5% net accuracy (94.5% -> 97.0%), 0/189 regressions on the frames the exterior already got right — whereas "always verify->wrist" (-3%), "always fuse both" (-1%) and "wrist-only" (-20%) all regressed. The double gate is essential: never abandon a self-consistent exterior pick, never jump onto an unverified wrist. On the verified-exterior path the wrist views still contribute CLOUD GEOMETRY (never identity): each wrist cloud of the same object — validated by the dev-era multiview intersection guard — is fused into the output cloud so the downstream OBB sees the top face / far side a single front view misses. Without it, tall (13-15 cm) bottles/cartons perceive as a thin front-face sliver whose centre is biased toward the camera by half the object depth (measured 12-13 mm on the LIBERO grocery suite), and the resulting off-centre pinch slips during transport. The returned mask/score remain the exterior pick's. Skill-level caching is **on by default**: identical perceive calls (same cameras + args) short-circuit with the previously computed Output. Set ``GAP_PERCEPTION_CACHE=0`` to disable. Cache dir overridable with ``GAP_PERCEPTION_CACHE_DIR`` (default ``/.llm_cache/perceiving-objects``). """ # Ignore the caller-supplied object_description. It is an LLM-authored # appearance guess produced at graph-generation time and is frequently # WRONG for LIBERO-specific assets: e.g. "chocolate pudding" is a flat # thin box, but the generator described it as "likely a small cup or # tub". That bogus hint is injected into the pairwise tournament ("It # looks like: ...") and the close-up verify ("It should look like: ..."), # where it steers the VLM to a distractor that matches the hint — the # cylindrical alphabet-soup CAN — so perception returns a clean cloud of # the WRONG object and the grasp goes to the wrong place/height (measured: # OBB top 0.081 m @ the soup can vs the pudding's true 0.028 m; IoU vs the # pudding = 0.000). The description-free pipeline is the configuration the # perception GT study validated at 99.5% ID, and empirically restores # chocolate pudding from 1/6 -> 6/6 grasps with no regression on the other # objects (butter/soup/milk/cream-cheese/salad-dressing all 3/3). Passing # object_name alone is strictly safer than trusting a hallucinated shape. object_description = "" cache_key: str | None = None if _CACHE_ENABLED: cache_key = _make_cache_key(cameras, { "object_name": object_name, "text_prompts": sorted(text_prompts) if text_prompts else None, "min_points": min_points, "min_score": min_score, "use_multiview": use_multiview, "box_threshold": box_threshold, "text_threshold": text_threshold, "dino_prompt": dino_prompt, "object_description": object_description, }) hit = _cache_load(cache_key) if hit is not None: logger.info("perceiving-objects cache HIT key=%s found=%s score=%.3f", cache_key[:12], hit["found"], hit["score"]) return _static_container_anchor(object_name, hit) logger.info("perceiving-objects cache MISS key=%s", cache_key[:12]) out = _run_uncached( ctx, cameras, object_name, text_prompts, min_points, min_score, use_multiview, box_threshold, text_threshold, dino_prompt, object_description, ) # Goal 2 — repair instead of abort: on not_found, retry with escalated recall # before signalling failure. No-op unless GAP_PERCEPTION_REPAIR is set. if _REPAIR and not out.get("found"): for tag, bthr, tthr, mscore in _REPAIR_LADDER: dp = (f"{object_name.strip().rstrip('.')}." if tag == "target" else "object.") logger.warning("perception repair: not_found -> retry [%s] " "(dino='%s' box=%.2f text=%.2f min_score=%.2f)", tag, dp, bthr, tthr, mscore) out2 = _run_uncached( ctx, cameras, object_name, text_prompts, min_points, mscore, use_multiview, bthr, tthr, dp, "", ) if out2.get("found"): logger.warning("perception repair: RECOVERED via [%s] (score=%.3f) " "— abort avoided", tag, out2["score"]) out = out2 break else: logger.warning("perception repair: recall ladder exhausted, still not_found") if cache_key is not None and out["found"]: _cache_store(cache_key, out) return _static_container_anchor(object_name, out) def _run_uncached( ctx: NodeContext, cameras: list[CameraFrame], object_name: str, text_prompts: list[str] | None, min_points: int, min_score: float, use_multiview: bool, box_threshold: float, text_threshold: float, dino_prompt: str, object_description: str, ) -> Output: """Existing perceive body — kept callable directly for cache-bypass paths.""" if text_prompts is None: text_prompts = [object_name] def _collect(cams, run_identify): out: list[tuple[Any, _CamResult]] = [] for cam in cams: r = _perceive_single_camera( ctx, cam, object_name, text_prompts, min_score, min_points, box_threshold, text_threshold, dino_prompt, object_description, run_identify=run_identify, ) if r is not None: out.append((cam, r)) return out def _finish(results: list[_CamResult], anchor: _CamResult | None = None) -> Output: if not results: return {"found": False, "cloud": _empty_cloud(), "mask": _empty_mask(), "score": 0.0} cloud = (_merge_multiview(results) if use_multiview and len(results) > 1 else results[0].cloud) # When an anchor result is given (the verified exterior pick), its # mask/score stay authoritative — wrist masks live in a moving # camera frame and must never reach downstream consumers that # project masks through the static exterior camera (build_world). best = anchor if anchor is not None else max(results, key=lambda r: r.score) return {"found": True, "cloud": cloud, "mask": best.mask, "score": best.score} def _collect_wrist_support( wrist_cams_, ext_results: list[_CamResult], ) -> list[_CamResult]: """Dev-era multiview fusion, anchored on the verified exterior pick: gather wrist-view clouds of the SAME object so the fused cloud covers the top face / far side the exterior view cannot see (a single front view yields a sliver OBB whose centre is biased toward the camera by half the object depth — measured 12-13 mm on LIBERO tall bottles/cartons, enough to make the fingers pinch the edge and slip). A wrist result is fused ONLY when its cloud intersects the exterior cloud (the dev multiview guard), so a mis-segmented wrist view can never displace the verified pick.""" anchor_pts = np.concatenate( [_cloud_pts(r.cloud) for r in ext_results], axis=0, ) if ext_results else np.zeros((0, 3)) support: list[_CamResult] = [] for wcam in wrist_cams_: try: # Historical wrist behavior first: segment_text on the frame. r = None try: r = _perceive_single_camera( ctx, wcam, object_name, text_prompts, min_score, min_points, box_threshold, text_threshold, dino_prompt, object_description, run_identify=False, ) except Exception as exc: # noqa: BLE001 logger.warning( "wrist-support: segment_text path failed on '%s': %s", wcam.get("name"), exc) if r is None or not _clouds_intersect(anchor_pts, r.cloud): # Geometry-guided fallback: seed SAM with the exterior # cloud's projection into the wrist camera. r = _segment_wrist_by_projection( ctx, wcam, anchor_pts, min_score, min_points) if r is None: continue if _clouds_intersect(anchor_pts, r.cloud): # Fuse ONLY the intersected subset. See # `_filter_wrist_to_anchor` for the rationale — the # original "fuse the entire wrist cloud once any # intersection exists" let a low-score wrist mask # corrupt the OBB centroid (LIBERO milk-carton run # 20260616_012843: cloud X span 20 cm vs 8 cm true, # OBB offset → empty grasp). filtered_pts = _filter_wrist_to_anchor( anchor_pts, r.cloud) if len(filtered_pts) < _MIN_INTERSECTION: continue fused_cloud: PointCloud = { "points": filtered_pts.astype(np.float32), } logger.info( "wrist-support: fusing wrist view '%s' " "(%d/%d points after intersect filter, score=%.3f)", wcam["name"], len(filtered_pts), len(r.cloud["points"]), r.score) support.append(_CamResult( cloud=fused_cloud, mask=r.mask, score=r.score, box=r.box, )) except Exception as exc: # noqa: BLE001 — support is best-effort logger.warning( "wrist-support: skipping wrist view '%s': %s", wcam.get("name"), exc) return support wrist_cams = [c for c in cameras if _is_wrist(c)] ext_cams = [c for c in cameras if not _is_wrist(c)] # Non-LIBERO / single-view platforms: keep the historical per-camera # behavior unchanged (identify on non-wrist, segment_text on wrist). if not wrist_cams or not ext_cams: return _finish([r for _, r in _collect(cameras, None)]) # --- safe gate --- ext = _collect(ext_cams, True) if ext: bc, br = max(ext, key=lambda cr: cr[1].score) if _verify_pick(ctx, bc["rgb"], br.box, object_name, object_description, default=True): logger.info( "safe gate: exterior pick verified for '%s' -> exterior", object_name) ext_results = [r for _, r in ext] support = _collect_wrist_support(wrist_cams, ext_results) return _finish(ext_results + support, anchor=br) # Exterior pick rejected (or none) -> gated wrist fallback. Loud on # purpose: a wrist-only result is a single top-down view whose cloud # covers just the visible top face, so the downstream OBB loses its # height — if this fires on objects the exterior view identified # correctly, pass `object_description` (shape/appearance hints) so the # close-up verify can recognize the rendered asset. logger.warning( "safe gate: exterior pick rejected by close-up verify for '%s' " "(object_description=%r) -> trying wrist fallback", object_name, object_description) wr = _collect(wrist_cams, True) if wr: wc, wrr = max(wr, key=lambda cr: cr[1].score) if _verify_pick(ctx, wc["rgb"], wrr.box, object_name, object_description, default=False): logger.info("safe gate: wrist pick verified -> wrist") return _finish([wrr]) # Neither side verified: conservatively keep the exterior result if # any, else fall back to the legacy exterior segment_text net (never # fuse an unverified wrist — that is the regressing behavior). fallback = [r for _, r in ext] or [r for _, r in _collect(ext_cams, None)] logger.info("safe gate: no verified pick; using conservative exterior " "fallback (%d result(s))", len(fallback)) return _finish(fallback)