verify_r1_moge_sim.py

"""R1-5 — MoGe point-map plane fit certification (CI-safe: synthetic point
map, no torch, no model).

A ground plane is constructed analytically: rays through a known camera K
intersect a tilted floor plane, giving a perfect metric point map. The only
thing under test is the REAL plane_homography_from_moge from app.py: it must
recover a metre-unit homography (1 m in the world measures 1.0 in plane
space), report geometrySource=moge-plane, honour the model's own intrinsics
(passed normalized, the MoGe convention), and abstain on wall-dominant fits,
noise, and missing geometry.
"""

import numpy as np
import cv2  # noqa: F401  (the extracted code uses np only, cv2 kept for parity)

IMG_W, IMG_H = 640, 480
F_PX = 612.0  # deliberately NOT ~width: the old pinhole f~w guess would be wrong

# --- real implementation from app.py ------------------------------------------
src = open("app.py").read()
ns = {"np": np, "cv2": cv2}
for fn in ("def pixel_intrinsics_from_model", "def plane_homography_from_moge"):
    start = src.index(fn)
    end = src.index("\n\ndef ", start + 10)
    exec(compile(src[start:end], "app.py", "exec"), ns)
plane_homography_from_moge = ns["plane_homography_from_moge"]


def synthetic_geometry(pitch_deg=35.0, cam_h=1.5, noise=0.0, wall=False):
    """Perfect metric point map of a plane seen by a pitched camera.
    Camera frame: y down, z forward; camera pitched down by pitch_deg.
    Floor: world plane 1.5 m below the camera. Wall: vertical plane 3 m
    ahead (normal mostly horizontal — must trip the floor-sanity gate)."""
    pitch = np.deg2rad(pitch_deg)
    cp, sp = np.cos(pitch), np.sin(pitch)
    R = np.array([[1, 0, 0], [0, cp, -sp], [0, sp, cp]])  # world -> camera
    if wall:
        n = R @ np.array([0.0, 0.0, -1.0])
        c0 = R @ np.array([0.0, 0.0, 3.0])
    else:
        n = R @ np.array([0.0, -1.0, 0.0])
        c0 = R @ np.array([0.0, cam_h, 0.0])
    d = float(n @ c0)

    K = np.array([[F_PX, 0, IMG_W / 2.0], [0, F_PX, IMG_H / 2.0], [0, 0, 1.0]])
    Kinv = np.linalg.inv(K)
    uu, vv = np.meshgrid(np.arange(IMG_W, dtype=np.float64), np.arange(IMG_H, dtype=np.float64))
    rays = np.stack([uu, vv, np.ones_like(uu)], axis=-1) @ Kinv.T
    denom = rays @ n
    with np.errstate(divide="ignore", invalid="ignore"):
        t = np.where(np.abs(denom) > 1e-9, d / denom, np.nan)
    points = rays * t[..., None]
    ok = np.isfinite(t) & (t > 0.5) & (t < 30.0) & (points[..., 2] > 0.05)

    rng = np.random.default_rng(3)
    if noise > 0:
        points = points + rng.normal(0, noise, points.shape)

    mask = ok & (vv > IMG_H * 0.45)  # floor occupies the lower image
    pts = points.astype(np.float32)
    pts[~ok] = np.nan
    K_norm = K.copy()
    K_norm[0, 0] /= IMG_W
    K_norm[0, 2] /= IMG_W
    K_norm[1, 1] /= IMG_H
    K_norm[1, 2] /= IMG_H
    return {
        "provider": "moge-2",
        "points": pts,
        "validMask": ok,
        "intrinsics": K_norm.astype(np.float32),
    }, mask.astype(np.uint8), points, ok


def main():
    ok_all = True

    # 1 — recovers a metre-unit plane chart from a clean point map
    geometry, mask, points, ok = synthetic_geometry()
    fitted = plane_homography_from_moge(geometry, mask, IMG_W, IMG_H)
    good = fitted is not None
    if good:
        H = np.asarray(fitted[0], np.float64).reshape(3, 3)
        plane = fitted[1]
        # measure: random floor pixel pairs, 3D distance vs plane distance
        ys, xs = np.nonzero((mask > 0) & ok)
        rng = np.random.default_rng(5)
        sel = rng.choice(len(xs), 3000, replace=False)
        i, j = sel[: len(sel) // 2], sel[len(sel) // 2 :]
        P = points[ys, xs]
        d3 = np.linalg.norm(P[i] - P[j], axis=1)
        den = H[2, 0] * xs + H[2, 1] * ys + H[2, 2]
        pa = (H[0, 0] * xs + H[0, 1] * ys + H[0, 2]) / den
        pb = (H[1, 0] * xs + H[1, 1] * ys + H[1, 2]) / den
        dp = np.hypot(pa[i] - pa[j], pb[i] - pb[j])
        far = dp > 0.3
        ratio = float(np.median(d3[far] / dp[far]))
        good = abs(ratio - 1.0) <= 0.02 and plane.get("geometrySource") == "moge-plane"
        print(f"  [{'PASS' if good else 'FAIL'}] clean point map -> metre chart "
              f"(3D/plane ratio {ratio:.4f}, source {plane.get('geometrySource')})")
    else:
        print("  [FAIL] clean point map: abstained")
    ok_all &= good

    # 2 — model intrinsics honoured: with f=612 vs the f~w=640 guess, a wrong
    # K would skew the ratio by ~5%; the gate above (2%) already proves K is
    # consumed, so here check the fit survives mild sensor noise
    geometry_n, mask_n, _, _ = synthetic_geometry(noise=0.01)
    fitted_n = plane_homography_from_moge(geometry_n, mask_n, IMG_W, IMG_H)
    print(f"  [{'PASS' if fitted_n is not None else 'FAIL'}] 1 cm point noise -> still fits")
    ok_all &= fitted_n is not None

    # 3 — wall-dominant fit must abstain (floor-normal sanity gate)
    geometry_w, mask_w, _, _ = synthetic_geometry(wall=True)
    fitted_w = plane_homography_from_moge(geometry_w, mask_w, IMG_W, IMG_H)
    print(f"  [{'PASS' if fitted_w is None else 'FAIL'}] wall-like plane -> abstains")
    ok_all &= fitted_w is None

    # 4 — garbage points must abstain (scale self-check / inlier gates)
    rng = np.random.default_rng(9)
    geometry_g, mask_g, _, _ = synthetic_geometry()
    geometry_g["points"] = rng.uniform(0.1, 10.0, geometry_g["points"].shape).astype(np.float32)
    fitted_g = plane_homography_from_moge(geometry_g, mask_g, IMG_W, IMG_H)
    print(f"  [{'PASS' if fitted_g is None else 'FAIL'}] random point cloud -> abstains")
    ok_all &= fitted_g is None

    # 5 — missing geometry -> None (caller falls through to the depth path)
    none_ok = (
        plane_homography_from_moge(None, mask, IMG_W, IMG_H) is None
        and plane_homography_from_moge({"points": None}, mask, IMG_W, IMG_H) is None
    )
    print(f"  [{'PASS' if none_ok else 'FAIL'}] no geometry -> None")
    ok_all &= none_ok

    # --- R1-6 (Pull 2) — per-region plane charts -----------------------------
    start = src.index("def region_plane_chart")
    end = src.index("\n# ---", start)
    ns["plane_homography_from_depth"] = lambda *a, **k: None
    exec(compile(src[start:end], "app.py", "exec"), ns)
    region_plane_chart = ns["region_plane_chart"]

    # Two-level floor: left half is the main floor (1.5 m below the camera),
    # right half a raised platform (1.0 m). One point map, two region masks.
    geom_a, mask_a, points_a, ok_a = synthetic_geometry(cam_h=1.5)
    geom_b, mask_b, points_b, ok_b = synthetic_geometry(cam_h=1.0)
    half = IMG_W // 2
    points = points_a.copy()
    points[:, half:] = points_b[:, half:]
    okm = ok_a.copy()
    okm[:, half:] = ok_b[:, half:]
    geometry2 = dict(geom_a)
    geometry2["points"] = points.astype(np.float32)
    geometry2["validMask"] = okm
    region_a = (mask_a > 0) & (np.indices((IMG_H, IMG_W))[1] < half)
    region_b = (mask_b > 0) & (np.indices((IMG_H, IMG_W))[1] >= half)

    # 6 — each region gets its OWN metre chart for its OWN plane
    fit_b = region_plane_chart(region_b.astype(np.uint8), geometry2, None, IMG_W, IMG_H)
    good = fit_b is not None
    if good:
        Hb = np.asarray(fit_b[0], np.float64).reshape(3, 3)
        ys, xs = np.nonzero(region_b & okm)
        rngb = np.random.default_rng(13)
        sel = rngb.choice(len(xs), 3000, replace=False)
        i, j = sel[:1500], sel[1500:]
        P = points[ys, xs]
        d3 = np.linalg.norm(P[i] - P[j], axis=1)
        den = Hb[2, 0] * xs + Hb[2, 1] * ys + Hb[2, 2]
        pa = (Hb[0, 0] * xs + Hb[0, 1] * ys + Hb[0, 2]) / den
        pb = (Hb[1, 0] * xs + Hb[1, 1] * ys + Hb[1, 2]) / den
        dp = np.hypot(pa[i] - pa[j], pb[i] - pb[j])
        far = dp > 0.3
        ratio_b = float(np.median(d3[far] / dp[far]))
        good = abs(ratio_b - 1.0) <= 0.02
        print(f"  [{'PASS' if good else 'FAIL'}] region chart: raised region gets its own "
              f"metre chart (ratio {ratio_b:.4f})")
    else:
        print("  [FAIL] region chart: abstained on a clean raised region")
    ok_all &= good

    # 7 — tiny/garbage regions abstain so the caller keeps the shared chart
    tiny = np.zeros((IMG_H, IMG_W), np.uint8)
    tiny[200:230, 200:230] = 1
    abstain_ok = region_plane_chart(tiny, geometry2, None, IMG_W, IMG_H) is None
    print(f"  [{'PASS' if abstain_ok else 'FAIL'}] region chart: tiny region abstains "
          f"(shared chart kept)")
    ok_all &= abstain_ok

    print("\n" + ("ALL R1-5 SIM CHECKS PASSED" if ok_all else "R1-5 SIM CHECKS FAILED"))
    return 0 if ok_all else 1


if __name__ == "__main__":
    raise SystemExit(main())