isaac/control_test/cam_configs.py

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