Сортировочная ячейка Isaac Sim: CV-пайплайн и меши товаров
Замкнутый контур "поток -> CV -> механика": товары идут по конвейеру с шагом 700 мм, класс определяется стереопайплайном во время движения, пушер и плуг реагируют физически. Состав: * control_test/ - ячейка и CV. run_sorting_cv.py + cv_worker.py (два процесса, потому что torch внутри Isaac роняет сцену), cell.py (физика лент, плуга, пушера), measure_plane.py (замер габаритов), README.md и .memory.md с замерами, проблемами и ловушками * robozon_sorter/ - модули симуляции, scripts/ - утилиты, scene/ - сцены * assets/ - меши товаров, плуг, объекты Objaverse Бейзлайн CV: DEFOM-Stereo vitl, вход 480, iters 24, кроп зоны осмотра, без сегментации. На потоке 700 мм - классы 8/9, габариты MAE 32.8 мм, 469 мс на товар при такте 700 мс. Веса моделей (4.5 ГБ) и пропсы конвейера NVIDIA (274 МБ) не включены - источники и команды скачивания в MODELS.md. Выход прогонов (captures/, runtime/) не включён: воспроизводится. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
This commit is contained in:
dasha_f
@@ -0,0 +1,215 @@
|
||||
"""SUPERSEDED - kept for history only.
|
||||
|
||||
SUPERSEDED by measure_roi.py. Isolated the object by 3D cropping and RGB
|
||||
background subtraction; both were measuring the belt. Use the ROI pipeline.
|
||||
"""
|
||||
|
||||
#!/usr/bin/env python3
|
||||
"""STAGE 2 (standalone via python.sh - NOT inside the running app, so the renderer keeps
|
||||
its GPU): CREStereo on each rig's pair -> metric depth -> world cloud -> merge the rigs ->
|
||||
dimensions, against the labelled ground truth.
|
||||
|
||||
Merging uses the CALIBRATED extrinsics and nothing else. A previous study on this cell
|
||||
measured RANSAC/trimmed-ICP re-registration making it worse in every case (11.9 mm ->
|
||||
40.5 mm): the three views see largely different surfaces, so ICP aligns non-corresponding
|
||||
geometry and drags cameras off their correct poses. Accuracy comes from calibration, not
|
||||
from re-registration - so RANSAC is deliberately not used here.
|
||||
|
||||
Dimensions use voxel-resample + robust 2/98 percentile extents on PCA axes, which the same
|
||||
study found best (11.9 mm) against raw (17.6), SOR (19.1) and convex-hull OBB (27.4).
|
||||
"""
|
||||
import glob, json, os, sys
|
||||
import numpy as np, cv2, torch, torch.nn.functional as F
|
||||
|
||||
sys.path.insert(0, "/home/dasha/isaac_assets/cv/crestereo")
|
||||
from nets import Model
|
||||
|
||||
CFG = sys.argv[1] if len(sys.argv) > 1 else "A_original"
|
||||
BASE = f"/home/dasha/robozon-sorter/control_test/captures/{CFG}"
|
||||
MAN = json.load(open(f"{BASE}/manifest.json"))
|
||||
SCALE = 3.0 # scene is 1/3 of real size; labels are real mm
|
||||
TARGET = np.array(MAN["target"], dtype=np.float64)
|
||||
BELT_Z = TARGET[2]
|
||||
|
||||
dev = "cuda" if torch.cuda.is_available() else "cpu"
|
||||
model = Model(max_disp=256, mixed_precision=False, test_mode=True)
|
||||
model.load_state_dict(torch.load("/home/dasha/isaac_assets/cv/crestereo/models/crestereo_eth3d.pth",
|
||||
map_location="cpu"), strict=True)
|
||||
model.to(dev).eval()
|
||||
print(f"CREStereo on {dev} config={CFG}", flush=True)
|
||||
|
||||
|
||||
def infer(L, R, n=20):
|
||||
iL = torch.tensor(np.ascontiguousarray(L.transpose(2, 0, 1)[None]).astype("float32")).to(dev)
|
||||
iR = torch.tensor(np.ascontiguousarray(R.transpose(2, 0, 1)[None]).astype("float32")).to(dev)
|
||||
h, w = iL.shape[2], iL.shape[3]
|
||||
iLd = F.interpolate(iL, size=(h // 2, w // 2), mode="bilinear", align_corners=True)
|
||||
iRd = F.interpolate(iR, size=(h // 2, w // 2), mode="bilinear", align_corners=True)
|
||||
with torch.inference_mode():
|
||||
f0 = model(iLd, iRd, iters=n, flow_init=None)
|
||||
f = model(iL, iR, iters=n, flow_init=f0)
|
||||
return torch.squeeze(f[:, 0, :, :]).cpu().numpy()
|
||||
|
||||
|
||||
def object_mask(fL, fbg, thr=6):
|
||||
"""exact mask by background subtraction.
|
||||
|
||||
Order matters and the first attempt got it backwards. A ray-traced render differs from
|
||||
its background mostly on TEXTURED pixels, so the raw difference is a speckle field, not
|
||||
a solid silhouette: 13199 differing pixels came out as 86 fragments whose largest was
|
||||
178 px, and picking the largest component then measured a speck instead of the object.
|
||||
CLOSE first to bridge the speckle into one region, THEN open to drop stray noise, then
|
||||
take the largest blob and fill its holes.
|
||||
"""
|
||||
a = cv2.imread(fL).astype(np.int16)
|
||||
b = cv2.imread(fbg).astype(np.int16)
|
||||
d = np.abs(a - b).max(axis=2).astype(np.uint8)
|
||||
m = (d > thr).astype(np.uint8)
|
||||
m = cv2.morphologyEx(m, cv2.MORPH_CLOSE, np.ones((15, 15), np.uint8))
|
||||
m = cv2.morphologyEx(m, cv2.MORPH_OPEN, np.ones((5, 5), np.uint8))
|
||||
n, lab, stats, _ = cv2.connectedComponentsWithStats(m, 8)
|
||||
if n <= 1:
|
||||
return m.astype(bool), 0
|
||||
k = 1 + int(np.argmax(stats[1:, cv2.CC_STAT_AREA]))
|
||||
blob = (lab == k).astype(np.uint8)
|
||||
# fill interior holes so untextured patches inside the object are not lost
|
||||
ff = blob.copy()
|
||||
h, w = ff.shape
|
||||
cv2.floodFill(ff, np.zeros((h + 2, w + 2), np.uint8), (0, 0), 1)
|
||||
blob = blob | (1 - ff)
|
||||
return blob.astype(bool), int(blob.sum())
|
||||
|
||||
|
||||
def rig_cloud(fL, fR, K, b, use_mask=False, fbg=None):
|
||||
L = cv2.cvtColor(cv2.imread(fL), cv2.COLOR_BGR2RGB)
|
||||
R = cv2.cvtColor(cv2.imread(fR), cv2.COLOR_BGR2RGB)
|
||||
H, W = L.shape[:2]
|
||||
sw = 640; sh = int(round(H * sw / W / 8)) * 8
|
||||
d = infer(cv2.resize(L, (sw, sh)), cv2.resize(R, (sw, sh)))
|
||||
disp = cv2.resize(d, (W, H)) * (W / sw)
|
||||
depth = np.where(disp > 0.5, K["fx"] * b / np.maximum(disp, 1e-6), np.nan)
|
||||
fx, fy, cx, cy = K["fx"], K["fy"], K["cx"], K["cy"]
|
||||
vs, us = np.mgrid[0:H, 0:W]
|
||||
m = np.isfinite(depth) & (depth > 1e-3) & (depth < 3.0)
|
||||
npix = 0
|
||||
if use_mask and fbg is not None and os.path.exists(fbg):
|
||||
om, npix = object_mask(fL, fbg)
|
||||
m &= om
|
||||
u, v, z = us[m], vs[m], depth[m]
|
||||
if len(z) == 0:
|
||||
return np.zeros((0, 3)), npix, float("nan")
|
||||
Pc = np.stack([(u - cx) * z / fx, -(v - cy) * z / fy, -z, np.ones_like(z)], 1)
|
||||
return (Pc @ np.array(K["M"], dtype=np.float64))[:, :3], npix, float(np.median(z))
|
||||
|
||||
|
||||
RADIUS = 0.16 # scene metres around the inspection point (~0.48 m real)
|
||||
|
||||
|
||||
def belt_reference(rig, K, b):
|
||||
"""reconstructed belt height for THIS rig, from its own background pair.
|
||||
|
||||
A fixed z-threshold does not work: CREStereo places the belt a few mm above its true
|
||||
height, and the offset differs per rig, so a global cut either keeps a belt patch (the
|
||||
first attempt measured 1700x1550 mm for every item - the crop window, not the object)
|
||||
or removes the object's base. Measuring the belt from the background pair calibrates
|
||||
that bias away per rig. RGB background subtraction was tried instead and is worse: the
|
||||
object's SHADOW differs from the background too, so the mask swallows the belt.
|
||||
"""
|
||||
fbg = MAN.get("background", {}).get(f"{rig}_Left")
|
||||
fbgR = MAN.get("background", {}).get(f"{rig}_Right")
|
||||
if not (fbg and fbgR and os.path.exists(fbg) and os.path.exists(fbgR)):
|
||||
return None
|
||||
P, _, _ = rig_cloud(fbg, fbgR, K, b)
|
||||
m = ((np.abs(P[:, 0] - TARGET[0]) < RADIUS) & (np.abs(P[:, 1] - TARGET[1]) < RADIUS))
|
||||
if m.sum() < 200:
|
||||
return None
|
||||
return float(np.percentile(P[m][:, 2], 90)) # top of the reconstructed belt
|
||||
|
||||
|
||||
def isolate(P, belt_z):
|
||||
z0 = (belt_z if belt_z is not None else BELT_Z) + 0.004
|
||||
m = ((np.abs(P[:, 0] - TARGET[0]) < RADIUS) & (np.abs(P[:, 1] - TARGET[1]) < RADIUS)
|
||||
& (P[:, 2] > z0) & (P[:, 2] < BELT_Z + 0.30))
|
||||
return P[m]
|
||||
|
||||
|
||||
def voxel(P, s=0.002):
|
||||
if len(P) == 0:
|
||||
return P
|
||||
keys = np.floor(P / s).astype(np.int64)
|
||||
_, idx = np.unique(keys, axis=0, return_index=True)
|
||||
return P[idx]
|
||||
|
||||
|
||||
def dims_mm(P):
|
||||
"""robust extents on PCA axes, in real mm"""
|
||||
if len(P) < 30:
|
||||
return None
|
||||
Q = voxel(P)
|
||||
c = Q.mean(0)
|
||||
X = Q - c
|
||||
_, _, V = np.linalg.svd(X, full_matrices=False)
|
||||
A = X @ V.T
|
||||
lo = np.percentile(A, 2, axis=0)
|
||||
hi = np.percentile(A, 98, axis=0)
|
||||
return sorted(((hi - lo) * 1000.0 * SCALE), reverse=True)
|
||||
|
||||
|
||||
BELTREF = {}
|
||||
rows = []
|
||||
for name, rec in MAN["items"].items():
|
||||
gt = sorted(rec["gt"]["dims_mm"], reverse=True)
|
||||
per_rig, merged = {}, []
|
||||
for rig in ("RealSense_D435", "Orbbec_Gemini305", "Orbbec_Gemini345"):
|
||||
kL = MAN["calib"][f"{rig}_Left"]
|
||||
fL, fR = rec["files"][f"{rig}_Left"], rec["files"][f"{rig}_Right"]
|
||||
if not (os.path.exists(fL) and os.path.exists(fR)):
|
||||
continue
|
||||
try:
|
||||
if rig not in BELTREF:
|
||||
BELTREF[rig] = belt_reference(rig, kL, kL["baseline"])
|
||||
P, npix, zmed = rig_cloud(fL, fR, kL, kL["baseline"])
|
||||
P = isolate(P, BELTREF[rig])
|
||||
except Exception as e:
|
||||
print(f" {name}/{rig}: {e}"); continue
|
||||
per_rig[rig] = dict(n=len(P), dims=dims_mm(P), mask_px=npix, z_med_mm=
|
||||
round(zmed * 1000, 1) if zmed == zmed else None)
|
||||
if len(P):
|
||||
merged.append(P)
|
||||
M = np.vstack(merged) if merged else np.zeros((0, 3))
|
||||
dm = dims_mm(M)
|
||||
err = None
|
||||
if dm:
|
||||
err = float(np.mean([abs(a - b) for a, b in zip(dm, gt)]))
|
||||
rows.append(dict(item=name, cls=rec["gt"]["cls"], gt=gt,
|
||||
merged=[round(v, 1) for v in dm] if dm else None,
|
||||
n_merged=int(len(M)),
|
||||
mae_mm=round(err, 1) if err is not None else None,
|
||||
per_rig={k: dict(n=v["n"], mask_px=v["mask_px"],
|
||||
z_med_mm=v["z_med_mm"],
|
||||
dims=[round(x, 1) for x in v["dims"]] if v["dims"] else None,
|
||||
mae=round(float(np.mean([abs(a - b) for a, b in
|
||||
zip(v["dims"], gt)])), 1)
|
||||
if v["dims"] else None)
|
||||
for k, v in per_rig.items()}))
|
||||
print(f" {name:>18} {rec['gt']['cls']} gt={[round(g) for g in gt]} "
|
||||
f"merged={[round(v) for v in dm] if dm else None} MAE={err:.1f} mm"
|
||||
if dm else f" {name:>18}: no cloud", flush=True)
|
||||
|
||||
out = f"{BASE}/measure.json"
|
||||
json.dump(dict(config=CFG, rows=rows), open(out, "w"), indent=1)
|
||||
ok = [r for r in rows if r["mae_mm"] is not None]
|
||||
print(f"\n=== {CFG} ===")
|
||||
print(f" items measured: {len(ok)}/{len(rows)}")
|
||||
if ok:
|
||||
print(f" merged MAE: {np.mean([r['mae_mm'] for r in ok]):.1f} mm")
|
||||
for c in ("B", "C", "D"):
|
||||
s = [r["mae_mm"] for r in ok if r["cls"] == c]
|
||||
if s:
|
||||
print(f" class {c}: {np.mean(s):6.1f} mm (n={len(s)})")
|
||||
for rig in ("RealSense_D435", "Orbbec_Gemini305", "Orbbec_Gemini345"):
|
||||
s = [r["per_rig"][rig]["mae"] for r in ok
|
||||
if rig in r["per_rig"] and r["per_rig"][rig]["mae"] is not None]
|
||||
if s:
|
||||
print(f" single rig {rig:>18}: {np.mean(s):6.1f} mm (n={len(s)})")
|
||||
print(f" -> {out}")
|
||||
Reference in New Issue
Block a user