"""Named camera arrangements. The canonical one is E60 - see DEFAULT below. Each rig is described by (azimuth, elevation) of its CENTRE as seen from the inspection point, plus its native baseline. Both eyes of a rig always share one orientation and the right eye is offset along the camera's own +X - that is what keeps a pair rectified, which CREStereo needs (depth = fx*B/disp assumes parallel axes). """ import json, math import omni.usd from pxr import Gf, Usd, UsdGeom TARGET = Gf.Vec3d(-0.750, 0.0, 1.781) # HEIGHT drives the layout: every rig sits this far ABOVE the belt surface, and its # standoff follows from its elevation angle (standoff = HEIGHT / sin(elev)). At the old # 600 mm standoff / 400 mm height the vertical field at the target was +-320 mm and an # object taller than ~220 mm had its top outside four of the six frames - the height is # what buys headroom for the 450x320x320 envelope. HEIGHT = 0.70 # every rig this far above the belt surface STANDOFF = 0.60 # kept only for the SPLIT rig, which is deliberately low+sideways BASELINE = {"RealSense_D435": 0.0735, "Orbbec_Gemini305": 0.0265, "Orbbec_Gemini345": 0.1294} # azimuth measured in the belt plane (deg, 0 = +X downstream, 90 = +Y side), elevation # above the belt plane. The ORIGINAL rig measured out at ~90/208/330 deg azimuth and ~42 # deg elevation - i.e. three views already spread ~120 deg apart, which is a sane merge # geometry; only the distance was 628 mm rather than 600. CONFIGS = { "A_original": { # previous layout, brought to 600 mm "RealSense_D435": (90.0, 41.8), "Orbbec_Gemini305": (208.3, 41.8), "Orbbec_Gemini345": (330.0, 41.7), }, "B_side_opposed": { # D435 split to face itself across the belt, low and sideways "RealSense_D435": ("SPLIT", 20.0), "Orbbec_Gemini305": (208.3, 41.8), "Orbbec_Gemini345": (330.0, 41.7), }, "C_low_triad": { # same 120 deg spread, but LOWER - more side/height coverage, "RealSense_D435": (90.0, 25.0), # which is where the old pipeline lost accuracy "Orbbec_Gemini305": (210.0, 25.0), "Orbbec_Gemini345": (330.0, 25.0), }, "D_mixed_elev": { # one overhead for footprint + two low for height/silhouette "RealSense_D435": (90.0, 65.0), "Orbbec_Gemini305": (210.0, 22.0), "Orbbec_Gemini345": (330.0, 22.0), }, } AZ = (90.0, 208.3, 330.0) # rig azimuths in the belt plane, ~120 deg apart # ============================ CANONICAL ARRANGEMENT ============================ # E60: height 700 mm, elevation 60 deg -> working distance 808 mm, azimuths 90 / # 208.3 / 330 deg. Chosen by measurement on 2026-08-01, not by preference: # # config elev distance MAE med D435 G305 G345 # A_orig 42 1051 mm 30.0 29.3 109.5 (1/9) 67.4 (5/9) # E45 45 991 mm 25.5 29.9 50.8 (1/9) 32.8 # E60 60 808 mm 22.3 25.8 27.1 28.0 <-- all 9/9 # E75 75 726 mm 23.6 27.8 26.3 29.4 # EQ15/20 45 per-rig 29.8 42.2 31.3 30.9 # # Two findings are load-bearing: # * Gemini305's 26.5 mm baseline does NOT need its own short distance. At 1051 mm it # produced a cloud once in nine tries because the disparity at the target was only # 17 px; at 808 mm it is 22 px and the rig works. Giving each rig its own # "equal depth precision" distance (EQ15/EQ20) fixed G305 but wrecked D435 # (25.8 -> 42.2) and made the merge worse than any common-distance layout. # * E60 is the first layout where fusing three rigs beats the best single rig # (22.3 vs 25.8). At 1051 mm fusion bought nothing. # # E75 is nearly as accurate but its shared belt region is a third smaller # (5756 vs 7681 cm2), i.e. less room for the item to sit off-centre. DEFAULT = "E60" CONFIGS["E60"] = { "RealSense_D435": (AZ[0], 60.0), "Orbbec_Gemini305": (AZ[1], 60.0), "Orbbec_Gemini345": (AZ[2], 60.0), } # ---- kept for reproducing the sweep above; not used by the pipeline ---- for _el in (45.0, 75.0): CONFIGS[f"E{int(_el)}"] = {rig: (az, _el) for rig, az in zip( ("RealSense_D435", "Orbbec_Gemini305", "Orbbec_Gemini345"), AZ)} _FX = 674.419 _B = {"RealSense_D435": 0.0735, "Orbbec_Gemini305": 0.0265, "Orbbec_Gemini345": 0.1294} for _r_mm in (15.0, 20.0): # per-rig distance for one shared mm-per-disp-px CONFIGS[f"EQ{int(_r_mm)}"] = { rig: (az, 45.0, math.sqrt(_r_mm / 1000.0 * _FX * _B[rig])) for rig, az in zip(("RealSense_D435", "Orbbec_Gemini305", "Orbbec_Gemini345"), AZ)} def _cam(stage, name): for p in stage.Traverse(): if p.IsA(UsdGeom.Camera) and p.GetName() == name: return p raise KeyError(name) def _look_at(pos, target): fwd = target - pos fwd = fwd / (fwd.GetLength() or 1.0) zax = -fwd up = Gf.Vec3d(0, 0, 1) if abs(Gf.Dot(up, zax)) > 0.999: up = Gf.Vec3d(0, 1, 0) xax = Gf.Cross(up, zax); xax = xax / (xax.GetLength() or 1.0) yax = Gf.Cross(zax, xax) M = Gf.Matrix4d(1.0) M.SetRow3(0, xax); M.SetRow3(1, yax); M.SetRow3(2, zax) M.SetTranslateOnly(pos) return M, xax, fwd def _place(stage, name, M): xf = UsdGeom.Xformable(_cam(stage, name)) xf.ClearXformOpOrder() xf.AddTransformOp().Set(M) def apply_config(stage, cfg_name, res=(1280, 720)): """position all six cameras, and square up the apertures for the render resolution. The aperture aspect must match the image aspect or fx != fy and every back-projected point is stretched - a silent scale error in exactly the dimension we are measuring. """ cfg = CONFIGS[cfg_name] W, H = res out = {} for rig, spec in cfg.items(): b = BASELINE[rig] if spec[0] == "SPLIT": th = math.radians(spec[1]) standoff = HEIGHT / max(math.sin(th), 1e-6) for side, sgn in (("Left", +1.0), ("Right", -1.0)): pos = TARGET + Gf.Vec3d(0.0, sgn * standoff * math.cos(th), standoff * math.sin(th)) M, _, _ = _look_at(pos, TARGET) _place(stage, f"{rig}_{side}", M) else: az, el = math.radians(spec[0]), math.radians(spec[1]) d = Gf.Vec3d(math.cos(az) * math.cos(el), math.sin(az) * math.cos(el), math.sin(el)) # a third element pins this rig's own distance: depth cost is Z^2/(fx*B), # so rigs with different baselines need different distances to reach the # same mm-per-disparity-pixel. One shared distance always starves the # narrowest baseline. standoff = (spec[2] if len(spec) > 2 else HEIGHT / max(math.sin(el), 1e-6)) centre = TARGET + d * standoff M, xax, _ = _look_at(centre, TARGET) for side, off in (("Left", -b / 2.0), ("Right", +b / 2.0)): Mi = Gf.Matrix4d(M) Mi.SetTranslateOnly(centre + xax * off) _place(stage, f"{rig}_{side}", Mi) xc = UsdGeom.XformCache() for rig in cfg: for side in ("Left", "Right"): n = f"{rig}_{side}" prim = _cam(stage, n) c = UsdGeom.Camera(prim) ha = c.GetHorizontalApertureAttr().Get() c.CreateVerticalApertureAttr().Set(ha * H / W) # square pixels fl = c.GetFocalLengthAttr().Get() M = xc.GetLocalToWorldTransform(prim) out[n] = dict( fx=fl / ha * W, fy=fl / (ha * H / W) * H, cx=W / 2.0, cy=H / 2.0, width=W, height=H, baseline=BASELINE[rig], rig=rig, side=side, M=[[M[r][col] for col in range(4)] for r in range(4)], pos=[M.ExtractTranslation()[i] for i in range(3)], height_mm=round((M.ExtractTranslation()[2] - TARGET[2]) * 1000, 1), standoff_mm=round((M.ExtractTranslation() - TARGET).GetLength() * 1000, 1)) return out