"""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}")