verify_r1_plane_sim.py

"""R1-2 — depth-based plane-fit fallback certification (CI-safe: analytic
depth, no torch).

Reuses the exact pinhole ground-plane scene from verify_r1_scale_sim (camera
1.5 m up, pitch 25 deg, f = image width) and runs the REAL
plane_homography_from_depth from app.py.

Checks:
  1. engages: returns a homography + plane on a clean metric ground plane
  2. metric by construction: the certified R1-3 estimator measures
     metersPerUnit ~ 1 on the produced homography (within 2%)
  3. shear-free and true-to-size: a known 1 m ground square maps to a
     1 x 1 plane square with right angles (sides within 2%, angle within 2 deg)
  4. orientation: plane-y grows toward the camera (near field), plane-x
     image-right — the bundle convention the frontend assumes
  5. rejection: non-planar depth (a dome) returns None
  6. rejection: relative (normalised) depth returns None
"""

import cv2
import numpy as np

from verify_r1_scale_sim import CAM_H, F, H as IMG_H, PITCH, W as IMG_W, scene

# --- real implementations from app.py ----------------------------------------
src = open("app.py").read()
ns = {"np": np, "cv2": cv2, "depth_model_is_metric": lambda name=None: True}
for fn in ["estimate_meters_per_unit", "plane_homography_from_depth"]:
    start = src.index(f"def {fn}")
    end = src.index("\ndef ", start + 10)
    exec(compile(src[start:end], "app.py", "exec"), ns)
plane_homography_from_depth = ns["plane_homography_from_depth"]
estimate_meters_per_unit = ns["estimate_meters_per_unit"]


def to_plane(H, px, py):
    den = H[2, 0] * px + H[2, 1] * py + H[2, 2]
    return (
        (H[0, 0] * px + H[0, 1] * py + H[0, 2]) / den,
        (H[1, 0] * px + H[1, 1] * py + H[1, 2]) / den,
    )


def project_ground(x_w, fwd_w):
    """Image pixel of a world ground point — inverse of the scene mapping."""
    # world -> camera: y_c, z_c from CAM_H/pitch; then u,v via pinhole
    y_w = -CAM_H
    z_c = np.cos(PITCH) * fwd_w - np.sin(PITCH) * y_w
    y_c = -np.sin(PITCH) * fwd_w - np.cos(PITCH) * y_w
    u = x_w / z_c * F + IMG_W / 2.0
    v = y_c / z_c * F + IMG_H / 2.0
    return u, v


def main():
    ok = True
    mask, z, x_w, fwd_w = scene()

    fitted = plane_homography_from_depth(z, mask, IMG_W, IMG_H)
    if fitted is None:
        print("  [FAIL] fallback did not engage on a clean metric ground plane")
        print("\nR1-2 SIM CHECKS FAILED")
        return 1
    hom, plane = fitted
    H = np.asarray(hom, np.float64).reshape(3, 3)
    print(f"  [PASS] engages: plane {plane['width']:.2f} x {plane['height']:.2f} m, "
          f"source={plane.get('geometrySource')}")

    mpu = estimate_meters_per_unit(z, mask, hom, IMG_W, IMG_H)
    good = mpu is not None and abs(mpu - 1.0) <= 0.02
    print(f"  [{'PASS' if good else 'FAIL'}] metric: metersPerUnit = {mpu}")
    ok &= good

    # known 1m ground square in the near field, centred
    cx_w = 0.0
    f0 = CAM_H / np.tan(PITCH) * 0.9   # comfortably inside the visible floor
    corners_w = [(cx_w - 0.5, f0), (cx_w + 0.5, f0), (cx_w + 0.5, f0 + 1.0), (cx_w - 0.5, f0 + 1.0)]
    corners_p = []
    for xw, fw in corners_w:
        u, v = project_ground(xw, fw)
        corners_p.append(to_plane(H, u, v))
    corners_p = np.asarray(corners_p)
    s1 = np.linalg.norm(corners_p[1] - corners_p[0])
    s2 = np.linalg.norm(corners_p[2] - corners_p[1])
    d1 = corners_p[1] - corners_p[0]
    d2 = corners_p[2] - corners_p[1]
    angle = np.degrees(np.arccos(abs(d1 @ d2) / (s1 * s2 + 1e-12)))
    square_ok = abs(s1 - 1) <= 0.02 and abs(s2 - 1) <= 0.02 and angle >= 88.0
    print(f"  [{'PASS' if square_ok else 'FAIL'}] 1m square -> sides {s1:.3f} x {s2:.3f} m, "
          f"corner angle {angle:.1f} deg")
    ok &= square_ok

    # orientation: nearer ground (smaller fwd) must have LARGER plane-y;
    # world +x (image right) must have larger plane-x
    u_near, v_near = project_ground(0.0, f0)
    u_far, v_far = project_ground(0.0, f0 + 2.0)
    _, b_near = to_plane(H, u_near, v_near)
    _, b_far = to_plane(H, u_far, v_far)
    u_r, v_r = project_ground(1.0, f0)
    a_l, _ = to_plane(H, u_near, v_near)
    a_r, _ = to_plane(H, u_r, v_r)
    orient_ok = b_near > b_far and a_r > a_l
    print(f"  [{'PASS' if orient_ok else 'FAIL'}] orientation: near-y {b_near:.2f} > far-y {b_far:.2f}, "
          f"right-x {a_r:.2f} > left-x {a_l:.2f}")
    ok &= orient_ok

    # rejection: dome instead of plane
    yy, xx = np.mgrid[0:IMG_H, 0:IMG_W].astype(np.float64)
    dome = (2.5 - 1.2 * np.exp(-(((xx - IMG_W / 2) / 300) ** 2 + ((yy - IMG_H / 2) / 220) ** 2))).astype(np.float32)
    r_dome = plane_homography_from_depth(dome, mask, IMG_W, IMG_H)
    print(f"  [{'PASS' if r_dome is None else 'FAIL'}] rejection: dome depth -> {None if r_dome is None else 'accepted'}")
    ok &= r_dome is None

    # rejection: relative depth
    ns["depth_model_is_metric"] = lambda name=None: False
    r_rel = plane_homography_from_depth(z, mask, IMG_W, IMG_H)
    ns["depth_model_is_metric"] = lambda name=None: True
    print(f"  [{'PASS' if r_rel is None else 'FAIL'}] rejection: relative depth -> {None if r_rel is None else 'accepted'}")
    ok &= r_rel is None

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


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