feat: initial commit — orchestratore presa-mano con AI assistant + dual-robot
Pipeline completa per calibrazione hand-eye e grasp planning su robot Unitree G1/H1 con camera depth Nuwa-HP60C. Esposta come FastAPI + GUI vanilla JS, con assistente AI (OpenRouter + tool calling curato) per pilotare il sistema in linguaggio naturale. Architettura: - FastAPI orchestrator (porta da .env, default 8890) che coordina 4 microservizi upstream: Nuwa-HP60C, marker-finder, YOLO, robot bridge. - Dual-robot: services.g1 e services.h1 in config.yaml; switchabili a runtime via PUT /api/home/robot. - Calibrazione hand-eye eye-to-hand split per-side (right/left). - Marker pose: marker-finder riceve gli intrinseci RGB della Nuwa al boot, PnP server-side con quei dati. Nessun re-PnP locale richiesto. - Depth-to-RGB: registrazione iterativa IR↔RGB per back-projection 3D corretta (Nuwa /api/measure usa coords IR; il nostro /api/perception/ measure_rgb prende pixel RGB e ritorna 3D nel frame RGB OpenCV). - Correzione affine sul depth bias (calibrabile via marker come ground-truth) — riduce errore Z da ~6% a ~1.5% nei test. - AI assistant: 16 tool curati (move_wrist_*, hand_action, get_marker[_3d], detect_object, calibration_*, set_active_robot…), system prompt dinamico che riflette robot attivo + marker config + addendum customizzabile da GUI. Temperature di default 0.1 per evitare invenzioni. GUI (7 tab): - Home: stato comunicazione device + selettore robot attivo G1/H1 - Assistente AI: chat con OpenRouter, history persistita lato browser, task riutilizzabili (prompt salvati), trace tool calls - Impostazioni AI: modello da combo curata, temperature, max iterations, prompt addendum, anteprima system prompt - Calibrazione, Auto-Calibrazione, TrovaUtensile, Servo Visuale, Vista 3D, Movimento EE — invariate dalle versioni precedenti Setup: - Variabili in presa-mano/.env (template in .env.example) - OPENROUTER_API_KEY richiesta solo per il tab Assistente AI - PRESA_MANO_PORT configurabile Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
Adriano Dal PastroandClaude Opus 4.7 committed
commit
ac573505b7
121 files changed
+67199
No files matched your search
@@ -0,0 +1,4 @@
|
||||
from .detector import ObjectDetector
|
||||
from .pointcloud import crop_bbox, filter_outliers, nuwa_to_cv_frame
|
||||
|
||||
__all__ = ["ObjectDetector", "crop_bbox", "filter_outliers", "nuwa_to_cv_frame"]
|
||||
@@ -0,0 +1,97 @@
|
||||
"""Local ArUco pose estimation with Nuwa RGB intrinsics.
|
||||
|
||||
The marker-finder service bakes D435i intrinsics into its solvePnP, which
|
||||
produces systematic pose errors on Nuwa images. We recompute the pose from the
|
||||
2D corners (still trustworthy) using the real Nuwa intrinsics, disambiguating
|
||||
the planar-marker ambiguity via IPPE_SQUARE + smallest reprojection error.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
|
||||
def nuwa_intrinsics_to_matrix(intr: dict[str, Any]
|
||||
) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""Accept the `data` payload from Nuwa /api/intrinsics and return
|
||||
(K_3x3, dist_5x1) for RGB. Distortion fields default to 0 if absent."""
|
||||
rgb = intr.get("rgb") if isinstance(intr, dict) else None
|
||||
if not isinstance(rgb, dict):
|
||||
raise ValueError(f"Nuwa intrinsics missing 'rgb' block: {intr!r}")
|
||||
K = np.array([
|
||||
[float(rgb["fx"]), 0.0, float(rgb["cx"])],
|
||||
[0.0, float(rgb["fy"]), float(rgb["cy"])],
|
||||
[0.0, 0.0, 1.0],
|
||||
], dtype=np.float64)
|
||||
d = rgb.get("distortion") or {}
|
||||
dist = np.array([
|
||||
float(d.get("k1", 0.0)),
|
||||
float(d.get("k2", 0.0)),
|
||||
float(d.get("p1", 0.0)),
|
||||
float(d.get("p2", 0.0)),
|
||||
float(d.get("k3", 0.0)),
|
||||
], dtype=np.float64)
|
||||
return K, dist
|
||||
|
||||
|
||||
def nuwa_intrinsics_to_opencv_payload(intr: dict[str, Any]) -> dict[str, list]:
|
||||
"""Build the wire-format dict that marker-finder's /detect (and /intrinsics
|
||||
POST) expects: `{camera_matrix: 3x3, dist_coeffs: [k1,k2,p1,p2,k3]}`.
|
||||
Plain Python lists so it serialises through JSON."""
|
||||
K, dist = nuwa_intrinsics_to_matrix(intr)
|
||||
return {"camera_matrix": K.tolist(), "dist_coeffs": dist.tolist()}
|
||||
|
||||
|
||||
def solve_square_marker(corners_px: np.ndarray, size_m: float,
|
||||
K: np.ndarray, dist: np.ndarray
|
||||
) -> dict[str, Any]:
|
||||
"""Recover (R, t) for a square planar marker of side `size_m` from its
|
||||
4 image corners in the order TL, TR, BR, BL (ArUco convention).
|
||||
|
||||
Uses solvePnPGeneric with SOLVEPNP_IPPE_SQUARE, which returns both planar
|
||||
solutions; we pick the one with smaller reprojection error. The runner-up
|
||||
error is also returned so the caller can detect ambiguous detections
|
||||
(both errors similar → the marker is nearly fronto-parallel and the pose
|
||||
is fundamentally underdetermined by a single view).
|
||||
"""
|
||||
s = size_m / 2.0
|
||||
obj = np.array([
|
||||
[-s, s, 0.0],
|
||||
[ s, s, 0.0],
|
||||
[ s, -s, 0.0],
|
||||
[-s, -s, 0.0],
|
||||
], dtype=np.float64)
|
||||
img = np.asarray(corners_px, dtype=np.float64).reshape(-1, 2)
|
||||
if img.shape[0] != 4:
|
||||
raise ValueError(f"expected 4 corners, got shape {img.shape}")
|
||||
|
||||
retval, rvecs, tvecs, errs = cv2.solvePnPGeneric(
|
||||
obj, img, K, dist, flags=cv2.SOLVEPNP_IPPE_SQUARE
|
||||
)
|
||||
if not retval or rvecs is None or len(rvecs) == 0:
|
||||
raise RuntimeError("solvePnP (IPPE_SQUARE) failed to converge")
|
||||
|
||||
errs_arr = np.asarray(errs).flatten() if errs is not None else np.array([0.0])
|
||||
best = int(np.argmin(errs_arr)) if errs_arr.size > 0 else 0
|
||||
R, _ = cv2.Rodrigues(rvecs[best])
|
||||
t = np.asarray(tvecs[best]).flatten()
|
||||
|
||||
# runner-up error for ambiguity reporting
|
||||
if errs_arr.size > 1:
|
||||
runner = float(np.partition(errs_arr, 1)[1])
|
||||
else:
|
||||
runner = float("inf")
|
||||
best_err = float(errs_arr[best])
|
||||
# ratio close to 1 means the two solutions are almost equally plausible.
|
||||
ambiguity = float(best_err / runner) if runner > 0 else 0.0
|
||||
return {
|
||||
"R": R,
|
||||
"t": t,
|
||||
"reprojection_error_px": best_err,
|
||||
"runnerup_error_px": runner,
|
||||
"ambiguity_ratio": ambiguity,
|
||||
"n_solutions": int(len(rvecs)),
|
||||
}
|
||||
@@ -0,0 +1,136 @@
|
||||
"""Per-installation depth-bias correction for the Nuwa-HP60C.
|
||||
|
||||
The Nuwa depth sensor exhibits an offset on `depth_mm` that doesn't fit a
|
||||
simple multiplicative scale (see /api/perception/measure_rgb experiments
|
||||
on a known marker — bias varies with distance). We model the bias as an
|
||||
affine map:
|
||||
|
||||
depth_raw = scale * depth_true + offset_mm (sensor)
|
||||
depth_true = (depth_raw - offset_mm) / scale (correction)
|
||||
|
||||
and fit (scale, offset_mm) from N≥2 samples where `depth_true` is taken
|
||||
from a trusted source (typically a PnP-derived Z on a known-size ArUco
|
||||
marker held at varying distances).
|
||||
|
||||
State (params + samples) lives in a single JSON file so it can be
|
||||
inspected/edited by hand:
|
||||
|
||||
data/depth_correction.json
|
||||
{
|
||||
"enabled": true,
|
||||
"scale": 0.871,
|
||||
"offset_mm": 185.0,
|
||||
"fit_meta": {"n_samples": 4, "rms_residual_mm": 2.1, ...},
|
||||
"samples": [
|
||||
{"depth_raw_mm": 1030, "depth_true_mm": 970, "note": "...", "added_at": ...},
|
||||
...
|
||||
]
|
||||
}
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def default_state() -> dict[str, Any]:
|
||||
return {
|
||||
"enabled": False,
|
||||
"scale": 1.0,
|
||||
"offset_mm": 0.0,
|
||||
"fit_meta": None,
|
||||
"samples": [],
|
||||
}
|
||||
|
||||
|
||||
def load(path: Path) -> dict[str, Any]:
|
||||
if not path.exists():
|
||||
return default_state()
|
||||
try:
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = json.load(f)
|
||||
if not isinstance(data, dict):
|
||||
return default_state()
|
||||
# Fill in any missing keys with defaults.
|
||||
merged = default_state()
|
||||
merged.update(data)
|
||||
return merged
|
||||
except json.JSONDecodeError:
|
||||
return default_state()
|
||||
|
||||
|
||||
def save(path: Path, state: dict[str, Any]) -> None:
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with open(path, "w", encoding="utf-8") as f:
|
||||
json.dump(state, f, ensure_ascii=False, indent=2)
|
||||
|
||||
|
||||
def apply_correction(depth_raw_mm: int, state: dict[str, Any] | None) -> int:
|
||||
"""Convert a raw sensor depth (mm) to the corrected depth (mm). Returns
|
||||
the input unchanged if correction is disabled or state is None."""
|
||||
if not state or not state.get("enabled") or depth_raw_mm <= 0:
|
||||
return depth_raw_mm
|
||||
scale = float(state.get("scale") or 1.0)
|
||||
offset = float(state.get("offset_mm") or 0.0)
|
||||
if scale <= 0:
|
||||
return depth_raw_mm
|
||||
corrected = (depth_raw_mm - offset) / scale
|
||||
return max(0, int(round(corrected)))
|
||||
|
||||
|
||||
def fit_affine(samples: list[dict[str, Any]]) -> dict[str, Any]:
|
||||
"""Least-squares fit `depth_raw = scale * depth_true + offset_mm`.
|
||||
|
||||
Needs ≥2 samples with non-degenerate range of depth_true. With exactly
|
||||
2 samples the fit is exact and rms_residual_mm = 0. With more samples
|
||||
the residual is meaningful for assessing fit quality.
|
||||
"""
|
||||
if len(samples) < 2:
|
||||
raise ValueError("need at least 2 samples to fit affine model")
|
||||
raw = np.array([float(s["depth_raw_mm"]) for s in samples], dtype=np.float64)
|
||||
true = np.array([float(s["depth_true_mm"]) for s in samples], dtype=np.float64)
|
||||
if true.max() - true.min() < 50:
|
||||
raise ValueError("samples cover too narrow a range of depth_true (<50 mm)")
|
||||
A = np.column_stack([true, np.ones_like(true)])
|
||||
sol, *_ = np.linalg.lstsq(A, raw, rcond=None)
|
||||
scale = float(sol[0])
|
||||
offset = float(sol[1])
|
||||
pred = scale * true + offset
|
||||
residuals = pred - raw
|
||||
rms = float(np.sqrt(np.mean(residuals ** 2)))
|
||||
max_abs = float(np.max(np.abs(residuals)))
|
||||
return {
|
||||
"scale": scale,
|
||||
"offset_mm": offset,
|
||||
"n_samples": len(samples),
|
||||
"rms_residual_mm": rms,
|
||||
"max_abs_residual_mm": max_abs,
|
||||
"depth_true_min_mm": float(true.min()),
|
||||
"depth_true_max_mm": float(true.max()),
|
||||
"fitted_at": time.time(),
|
||||
}
|
||||
|
||||
|
||||
def add_sample(state: dict[str, Any], depth_raw_mm: int, depth_true_mm: int,
|
||||
note: str = "") -> dict[str, Any]:
|
||||
samples = list(state.get("samples") or [])
|
||||
samples.append({
|
||||
"depth_raw_mm": int(depth_raw_mm),
|
||||
"depth_true_mm": int(depth_true_mm),
|
||||
"note": note,
|
||||
"added_at": time.time(),
|
||||
})
|
||||
new_state = dict(state)
|
||||
new_state["samples"] = samples
|
||||
return new_state
|
||||
|
||||
|
||||
def clear_samples(state: dict[str, Any]) -> dict[str, Any]:
|
||||
new_state = dict(state)
|
||||
new_state["samples"] = []
|
||||
return new_state
|
||||
@@ -0,0 +1,189 @@
|
||||
"""Back-project an RGB pixel to 3D using the depth sensor, with proper
|
||||
IR↔RGB registration.
|
||||
|
||||
Why this exists
|
||||
---------------
|
||||
Nuwa-HP60C is RGB-D with two physically distinct cameras (RGB + IR) that
|
||||
have different intrinsics and a small baseline:
|
||||
|
||||
fx_rgb=593.6 fx_ir=445.8 cx_rgb=348.1 cx_ir=316.6
|
||||
t_ir_to_rgb ≈ (-12.05, +0.08, -0.03) mm
|
||||
|
||||
Nuwa's `/api/measure?u=&v=` interprets `(u,v)` as **IR/depth-image** pixel
|
||||
coords and back-projects with IR intrinsics. So if we have a pixel `(u,v)`
|
||||
in the **RGB** image (e.g. a YOLO bbox center, an ArUco corner) and we want
|
||||
the actual 3D point in the RGB camera frame, we need to handle the
|
||||
registration ourselves.
|
||||
|
||||
Algorithm (single fixed-point iteration is enough at typical robot ranges):
|
||||
1. Initial guess depth z0 from sampling depth[v_rgb, u_rgb] (depth is in
|
||||
IR coords — at small baselines it's already close).
|
||||
2. Hypothesise the 3D point on the RGB ray at distance z0.
|
||||
3. Project that 3D point into the IR image → (u_ir, v_ir).
|
||||
4. Sample depth at (u_ir, v_ir) → z1.
|
||||
5. Use (u_ir, v_ir, z1) to back-project with IR intrinsics → P_ir.
|
||||
6. Transform P_ir → P_rgb via the IR→RGB extrinsics. Done.
|
||||
|
||||
Two iterations of step 2-4 reduce residual error to sub-mm even with
|
||||
mediocre initial guesses; we do two unconditionally because they're cheap.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Any
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
from ..clients import NuwaClient
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraRegistration:
|
||||
"""Cached intrinsics+extrinsics for a Nuwa-HP60C-style RGB-D camera."""
|
||||
K_rgb: np.ndarray # 3x3
|
||||
K_ir: np.ndarray # 3x3
|
||||
R_ir_to_rgb: np.ndarray # 3x3
|
||||
t_ir_to_rgb: np.ndarray # (3,) in **meters**
|
||||
rgb_width: int
|
||||
rgb_height: int
|
||||
ir_width: int
|
||||
ir_height: int
|
||||
|
||||
@classmethod
|
||||
def from_nuwa(cls, intr: dict[str, Any]) -> "CameraRegistration":
|
||||
rgb = intr.get("rgb") or {}
|
||||
ir = intr.get("ir") or {}
|
||||
ext = intr.get("extrinsics_ir_to_rgb") or {}
|
||||
if not rgb or not ir:
|
||||
raise ValueError(f"Nuwa intrinsics must contain 'rgb' and 'ir' blocks: {intr!r}")
|
||||
K_rgb = np.array([[rgb["fx"], 0, rgb["cx"]],
|
||||
[0, rgb["fy"], rgb["cy"]],
|
||||
[0, 0, 1]], dtype=np.float64)
|
||||
K_ir = np.array([[ir["fx"], 0, ir["cx"]],
|
||||
[0, ir["fy"], ir["cy"]],
|
||||
[0, 0, 1]], dtype=np.float64)
|
||||
R = np.asarray(ext.get("rotation") or [1, 0, 0, 0, 1, 0, 0, 0, 1], dtype=np.float64)
|
||||
if R.size == 9:
|
||||
R = R.reshape(3, 3)
|
||||
elif R.shape != (3, 3):
|
||||
raise ValueError(f"extrinsics rotation must be 9 elements or 3x3, got {R.shape}")
|
||||
# Translation in Nuwa is in mm; we convert to meters once.
|
||||
t_mm = np.asarray(ext.get("translation") or [0, 0, 0], dtype=np.float64).flatten()
|
||||
t = t_mm / 1000.0
|
||||
return cls(
|
||||
K_rgb=K_rgb, K_ir=K_ir,
|
||||
R_ir_to_rgb=R, t_ir_to_rgb=t,
|
||||
rgb_width=int(rgb.get("width") or 640),
|
||||
rgb_height=int(rgb.get("height") or 480),
|
||||
ir_width=int(ir.get("width") or 640),
|
||||
ir_height=int(ir.get("height") or 480),
|
||||
)
|
||||
|
||||
|
||||
def fetch_depth_mm(nuwa: NuwaClient) -> np.ndarray:
|
||||
"""Fetch the current depth frame as an int32 array (mm), shape (H, W).
|
||||
Zero values mean 'no depth' — caller should mask them out."""
|
||||
png = nuwa.depth_png()
|
||||
arr = np.frombuffer(png, dtype=np.uint8)
|
||||
img = cv2.imdecode(arr, cv2.IMREAD_UNCHANGED)
|
||||
if img is None:
|
||||
raise RuntimeError("could not decode Nuwa depth PNG")
|
||||
if img.dtype != np.uint16:
|
||||
# Some configurations return 8-bit (color preview); we need raw mm.
|
||||
raise RuntimeError(f"expected 16-bit depth PNG, got dtype={img.dtype}")
|
||||
return img.astype(np.int32)
|
||||
|
||||
|
||||
def _sample_depth(depth_mm: np.ndarray, u: float, v: float, radius: int = 4) -> int:
|
||||
"""Nearest valid (non-zero) depth value within `radius` of (u,v) on the
|
||||
depth image. Returns 0 if nothing found."""
|
||||
h, w = depth_mm.shape
|
||||
u0, v0 = int(round(u)), int(round(v))
|
||||
if u0 < 0 or v0 < 0 or u0 >= w or v0 >= h:
|
||||
return 0
|
||||
z = int(depth_mm[v0, u0])
|
||||
if z > 0:
|
||||
return z
|
||||
# Spiral / box search outward to `radius`.
|
||||
for r in range(1, radius + 1):
|
||||
u_lo, u_hi = max(0, u0 - r), min(w, u0 + r + 1)
|
||||
v_lo, v_hi = max(0, v0 - r), min(h, v0 + r + 1)
|
||||
patch = depth_mm[v_lo:v_hi, u_lo:u_hi]
|
||||
valid = patch[patch > 0]
|
||||
if valid.size:
|
||||
return int(valid.min()) # nearest along ray = smallest depth
|
||||
return 0
|
||||
|
||||
|
||||
def back_project_rgb_pixel(
|
||||
u_rgb: float, v_rgb: float,
|
||||
depth_mm: np.ndarray,
|
||||
reg: CameraRegistration,
|
||||
*, sample_radius: int = 4, n_iters: int = 2,
|
||||
depth_correction: dict[str, Any] | None = None,
|
||||
) -> dict[str, Any]:
|
||||
"""Back-project an RGB pixel using the registered depth image.
|
||||
|
||||
If `depth_correction` is supplied (dict with `enabled`, `scale`,
|
||||
`offset_mm`), each raw depth sample is corrected before back-projection.
|
||||
See `depth_correction.apply_correction`.
|
||||
|
||||
Returns a dict:
|
||||
{
|
||||
"ok": bool,
|
||||
"rgb_pixel": [u, v],
|
||||
"ir_pixel": [u_ir, v_ir],
|
||||
"depth_mm_raw": int, # what the sensor reported
|
||||
"depth_mm": int, # corrected (== raw if correction off)
|
||||
"point_rgb_m": [X, Y, Z], # in RGB camera frame, OpenCV convention
|
||||
"point_ir_m": [X, Y, Z], # in IR camera frame
|
||||
}
|
||||
"""
|
||||
from .depth_correction import apply_correction
|
||||
|
||||
out: dict[str, Any] = {"ok": False, "rgb_pixel": [float(u_rgb), float(v_rgb)]}
|
||||
K_rgb_inv = np.linalg.inv(reg.K_rgb)
|
||||
|
||||
ray_rgb = K_rgb_inv @ np.array([u_rgb, v_rgb, 1.0])
|
||||
|
||||
z_raw_first = _sample_depth(depth_mm, u_rgb, v_rgb, radius=sample_radius)
|
||||
if z_raw_first <= 0:
|
||||
out["error"] = f"no valid depth near RGB pixel ({u_rgb:.0f},{v_rgb:.0f})"
|
||||
return out
|
||||
|
||||
z_guess = apply_correction(z_raw_first, depth_correction)
|
||||
P_rgb = (z_guess / 1000.0) * ray_rgb
|
||||
|
||||
u_ir = v_ir = None
|
||||
z_raw = z_raw_first
|
||||
z_corr = z_guess
|
||||
for _ in range(max(1, n_iters)):
|
||||
P_ir = reg.R_ir_to_rgb.T @ (P_rgb - reg.t_ir_to_rgb)
|
||||
if P_ir[2] <= 0:
|
||||
out["error"] = "RGB ray hits behind IR camera"
|
||||
return out
|
||||
u_ir = reg.K_ir[0, 0] * P_ir[0] / P_ir[2] + reg.K_ir[0, 2]
|
||||
v_ir = reg.K_ir[1, 1] * P_ir[1] / P_ir[2] + reg.K_ir[1, 2]
|
||||
z_raw = _sample_depth(depth_mm, u_ir, v_ir, radius=sample_radius)
|
||||
if z_raw <= 0:
|
||||
break
|
||||
z_corr = apply_correction(z_raw, depth_correction)
|
||||
K_ir_inv = np.linalg.inv(reg.K_ir)
|
||||
P_ir_new = (z_corr / 1000.0) * (K_ir_inv @ np.array([u_ir, v_ir, 1.0]))
|
||||
P_rgb = reg.R_ir_to_rgb @ P_ir_new + reg.t_ir_to_rgb
|
||||
|
||||
if z_raw <= 0:
|
||||
out["error"] = f"no valid depth near IR pixel ({u_ir:.0f},{v_ir:.0f})"
|
||||
return out
|
||||
|
||||
out.update({
|
||||
"ok": True,
|
||||
"ir_pixel": [float(u_ir), float(v_ir)],
|
||||
"depth_mm_raw": int(z_raw),
|
||||
"depth_mm": int(z_corr),
|
||||
"point_rgb_m": P_rgb.tolist(),
|
||||
"point_ir_m": (reg.R_ir_to_rgb.T @ (P_rgb - reg.t_ir_to_rgb)).tolist(),
|
||||
})
|
||||
return out
|
||||
@@ -0,0 +1,64 @@
|
||||
"""Wrap YOLO detection + parse response."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
from ..clients import NuwaClient, YoloClient
|
||||
|
||||
|
||||
class ObjectDetector:
|
||||
def __init__(self, yolo: YoloClient, nuwa: NuwaClient):
|
||||
self.yolo = yolo
|
||||
self.nuwa = nuwa
|
||||
|
||||
def detect(self, conf: float | None = None) -> list[dict[str, Any]]:
|
||||
"""Returns list of {class, confidence, bbox: {x1,y1,x2,y2}} from current Nuwa RGB."""
|
||||
raw = self.yolo.detect_url(self.nuwa.rgb_url(), conf=conf)
|
||||
return _normalize(raw)
|
||||
|
||||
|
||||
def _normalize(payload: Any) -> list[dict[str, Any]]:
|
||||
"""YOLO responses vary; try to extract a list of detections uniformly.
|
||||
|
||||
Supports:
|
||||
- [ {...}, {...} ] # plain list
|
||||
- {"detections": [...]} # flat
|
||||
- {"success": true, "data": {"detections":[...]}}# envelope used by this yolo-server
|
||||
"""
|
||||
# Unwrap {success, data, error} envelope
|
||||
if isinstance(payload, dict) and isinstance(payload.get("data"), dict):
|
||||
payload = payload["data"]
|
||||
|
||||
items: Any = payload
|
||||
if isinstance(payload, dict):
|
||||
items = []
|
||||
for key in ("detections", "predictions", "results"):
|
||||
if isinstance(payload.get(key), list):
|
||||
items = payload[key]
|
||||
break
|
||||
|
||||
out: list[dict[str, Any]] = []
|
||||
for it in items or []:
|
||||
bbox = it.get("bbox") or it.get("box") or it
|
||||
x1, y1, x2, y2 = _xyxy(bbox)
|
||||
out.append({
|
||||
"class": it.get("class") or it.get("label") or it.get("name"),
|
||||
"confidence": float(it.get("confidence") or it.get("conf") or it.get("score") or 0.0),
|
||||
"bbox": {"x1": x1, "y1": y1, "x2": x2, "y2": y2},
|
||||
})
|
||||
return out
|
||||
|
||||
|
||||
def _xyxy(b: Any) -> tuple[float, float, float, float]:
|
||||
if isinstance(b, dict):
|
||||
if "x1" in b:
|
||||
return float(b["x1"]), float(b["y1"]), float(b["x2"]), float(b["y2"])
|
||||
if "xmin" in b:
|
||||
return float(b["xmin"]), float(b["ymin"]), float(b["xmax"]), float(b["ymax"])
|
||||
if "x" in b and "w" in b:
|
||||
x, y, w, h = float(b["x"]), float(b["y"]), float(b["w"]), float(b["h"])
|
||||
return x, y, x + w, y + h
|
||||
if isinstance(b, (list, tuple)) and len(b) >= 4:
|
||||
return float(b[0]), float(b[1]), float(b[2]), float(b[3])
|
||||
raise ValueError(f"cannot parse bbox: {b}")
|
||||
@@ -0,0 +1,65 @@
|
||||
"""Point cloud utilities: crop to bbox in image space + outlier filtering.
|
||||
|
||||
Nuwa /api/pointcloud returns points in camera frame; we need to project them
|
||||
back to pixel space to keep only those inside a YOLO bbox.
|
||||
|
||||
Frame note:
|
||||
Nuwa publishes points in X-right / Y-up / Z-back (Z<0 in front of camera).
|
||||
`cv2.solvePnP` (used by marker-finder to produce T_cam_marker, hence
|
||||
T_base_camera after hand-eye) uses X-right / Y-down / Z-forward (Z>0 in front).
|
||||
Before applying T_base_camera we flip Y and Z via `nuwa_to_cv_frame`.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def nuwa_to_cv_frame(points_xyz: np.ndarray) -> np.ndarray:
|
||||
"""Flip Y and Z to convert Nuwa camera points to OpenCV convention."""
|
||||
pts = np.asarray(points_xyz, dtype=np.float64).copy()
|
||||
pts[:, 1] *= -1.0
|
||||
pts[:, 2] *= -1.0
|
||||
return pts
|
||||
|
||||
|
||||
def project(points_cam: np.ndarray, K: np.ndarray) -> np.ndarray:
|
||||
"""Project Nx3 camera-frame points to Nx2 pixels via pinhole."""
|
||||
pts = np.asarray(points_cam, dtype=np.float64)
|
||||
z = pts[:, 2]
|
||||
z = np.where(z == 0, 1e-9, z)
|
||||
u = K[0, 0] * pts[:, 0] / z + K[0, 2]
|
||||
v = K[1, 1] * pts[:, 1] / z + K[1, 2]
|
||||
return np.stack([u, v], axis=1)
|
||||
|
||||
|
||||
def crop_bbox(points_cam: np.ndarray, bbox_xyxy: tuple[float, float, float, float],
|
||||
K: np.ndarray) -> np.ndarray:
|
||||
"""Keep only 3D points whose projection falls inside bbox (in pixel coords).
|
||||
|
||||
Works regardless of depth-sign convention (OpenCV: Z>0 in front;
|
||||
Nuwa/OpenGL-like: Z<0 in front) — the server-provided point cloud already
|
||||
obeys u = fx·x/z + cx with its own convention.
|
||||
"""
|
||||
if points_cam.size == 0:
|
||||
return points_cam[:0, :]
|
||||
xyz = points_cam[:, :3]
|
||||
uv = project(xyz, K)
|
||||
x1, y1, x2, y2 = bbox_xyxy
|
||||
mask = (
|
||||
(uv[:, 0] >= x1) & (uv[:, 0] <= x2) &
|
||||
(uv[:, 1] >= y1) & (uv[:, 1] <= y2) &
|
||||
(np.abs(xyz[:, 2]) > 1e-6)
|
||||
)
|
||||
return points_cam[mask]
|
||||
|
||||
|
||||
def filter_outliers(points: np.ndarray, n_std: float = 2.0) -> np.ndarray:
|
||||
"""Remove points whose distance from centroid exceeds n_std * std."""
|
||||
if len(points) < 10:
|
||||
return points
|
||||
xyz = points[:, :3]
|
||||
c = xyz.mean(axis=0)
|
||||
d = np.linalg.norm(xyz - c, axis=1)
|
||||
keep = d < (d.mean() + n_std * d.std())
|
||||
return points[keep]
|
||||
Reference in new issue
Block a user