verify_r1_rotation_sim.py

"""R1-4 — wall-aligned rotation v2 certification (CI-safe: analytic scene,
no torch).

Reuses the pinhole ground-plane camera from verify_r1_scale_sim and builds a
ROTATED room: a floor rectangle at a known world angle phi with wall bands
along its far edges. The analytic metre-plane homography is constructed in
closed form (and self-checked against the projection), so the only thing
under test is the REAL estimate_wall_rotation from app.py.

Checks:
  1-4. recovers phi = 0 / 18 / -30 / 70 deg within 2 deg (70 folds to -20:
       the tile grid is 90-deg symmetric; perpendicular walls reinforce)
  5.   abstains on scattered wall blobs (no straight contact line)
  6.   abstains when two walls disagree (0 vs 45 deg — conflicting cues)
"""

import cv2
import numpy as np

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

# --- real implementation from app.py ------------------------------------------
# R1-4 v3: the span covers the shared voting helper plus both estimators
# (wall-contact and floor-lattice), ending at the next section banner.
src = open("app.py").read()
ns = {"np": np, "cv2": cv2}
start = src.index("def _fold_segments_to_rotation")
end = src.index("\n# ---", start)
exec(compile(src[start:end], "app.py", "exec"), ns)
estimate_wall_rotation = ns["estimate_wall_rotation"]
estimate_floor_lattice_rotation = ns["estimate_floor_lattice_rotation"]


def plane_homography():
    """Closed-form image -> (x_w, fwd) metre-plane homography for the scene
    camera (height CAM_H, pitch PITCH, f = F, principal point at centre)."""
    c, s = np.cos(PITCH), np.sin(PITCH)
    M = np.array(
        [
            [F, (IMG_W / 2.0) * c, (IMG_W / 2.0) * s * CAM_H],
            [0.0, -F * s + (IMG_H / 2.0) * c, F * c * CAM_H + (IMG_H / 2.0) * s * CAM_H],
            [0.0, c, s * CAM_H],
        ]
    )
    return np.linalg.inv(M)


def world_grid(Hp):
    """Per-pixel (x_w, fwd) via the plane homography; fwd<=0 marked invalid."""
    yy, xx = np.mgrid[0:IMG_H, 0:IMG_W].astype(np.float64)
    p = np.stack([xx, yy, np.ones_like(xx)], axis=-1) @ Hp.T
    z = p[..., 2]
    valid = np.abs(z) > 1e-9
    x_w = np.where(valid, p[..., 0] / np.where(valid, z, 1), 0)
    fwd = np.where(valid, p[..., 1] / np.where(valid, z, 1), -1)
    return x_w, fwd, valid & (fwd > 0.2)


def rotated_room(phi_deg, Hp, walls=("back", "right")):
    """Floor rect at angle phi with 0.35 m wall bands on the given edges."""
    x_w, fwd, ok = world_grid(Hp)
    phi = np.radians(phi_deg)
    f_c = CAM_H / np.tan(PITCH) * 0.9 + 2.0
    fx = fwd - f_c
    xr = np.cos(phi) * x_w + np.sin(phi) * fx
    fr = -np.sin(phi) * x_w + np.cos(phi) * fx
    HX, HF = 4.0, 2.0
    floor = ok & (np.abs(xr) <= HX) & (np.abs(fr) <= HF)
    wall = np.zeros_like(floor)
    if "back" in walls:
        wall |= ok & (np.abs(xr) <= HX) & (fr > HF) & (fr <= HF + 0.35)
    if "right" in walls:
        wall |= ok & (np.abs(fr) <= HF) & (xr > HX) & (xr <= HX + 0.35)
    return wall.astype(np.uint8), floor.astype(np.uint8)


def lattice_room(phi_deg, Hp, tile=0.5):
    """R1-4 v3 — synthetic photo of an EXISTING tiled floor: dark grout lines
    every `tile` metres in a world frame rotated by phi (perspective-correct
    through the scene camera), on a light tile field."""
    x_w, fwd, ok = world_grid(Hp)
    phi = np.radians(phi_deg)
    f_c = CAM_H / np.tan(PITCH) * 0.9 + 2.0
    fx = fwd - f_c
    xr = np.cos(phi) * x_w + np.sin(phi) * fx
    fr = -np.sin(phi) * x_w + np.cos(phi) * fx
    HX, HF = 4.0, 2.0
    floor = ok & (np.abs(xr) <= HX) & (np.abs(fr) <= HF)
    img = np.full((IMG_H, IMG_W, 3), 200, np.uint8)
    gx = np.abs(xr / tile - np.round(xr / tile)) * tile < 0.015
    gf = np.abs(fr / tile - np.round(fr / tile)) * tile < 0.015
    img[floor & (gx | gf)] = 90
    img[~floor] = 230
    return img, floor.astype(np.uint8)


def fold(a):
    a = a % 90.0
    return a - 90.0 if a >= 45.0 else a


def main():
    ok = True
    Hp = plane_homography()

    # self-check: the closed-form homography inverts the scene projection
    f0 = CAM_H / np.tan(PITCH) * 0.9
    z_c = np.cos(PITCH) * f0 + np.sin(PITCH) * CAM_H
    y_c = -np.sin(PITCH) * f0 + np.cos(PITCH) * CAM_H
    u, v = 1.0 / z_c * F + IMG_W / 2.0, y_c / z_c * F + IMG_H / 2.0
    p = Hp @ np.array([u, v, 1.0])
    assert abs(p[0] / p[2] - 1.0) < 1e-6 and abs(p[1] / p[2] - f0) < 1e-6, \
        "scene homography inconsistent with projection"

    for phi in (0.0, 18.0, -30.0, 70.0):
        wall, floor = rotated_room(phi, Hp)
        got = estimate_wall_rotation(wall, floor, Hp)
        want = fold(phi)
        good = got is not None and abs(fold(got - want)) <= 2.0
        print(f"  [{'PASS' if good else 'FAIL'}] phi={phi:+.0f} deg -> "
              f"{'None' if got is None else f'{got:+.2f}'} (want {want:+.1f})")
        ok &= good

    # 5 — scattered blobs over a wide band: no dominant direction to read
    rng = np.random.default_rng(7)
    _, floor = rotated_room(0.0, Hp)
    blobs = np.zeros((IMG_H, IMG_W), np.uint8)
    edge_rows = np.where(floor.any(axis=1))[0]
    top = int(edge_rows[0])
    for _ in range(40):
        cx = int(rng.integers(40, IMG_W - 40))
        cy = int(np.clip(top + rng.integers(-60, 60), 4, IMG_H - 4))
        blobs[cy - 3 : cy + 3, max(0, cx - 5) : cx + 5] = 1
    got = estimate_wall_rotation(blobs, floor, Hp)
    print(f"  [{'PASS' if got is None else 'FAIL'}] scattered blobs -> {got}")
    ok &= got is None

    # 6 — conflicting walls: 0-deg back band + 45-deg diagonal band
    wall0, floor0 = rotated_room(0.0, Hp, walls=("back",))
    x_w, fwd, valid = world_grid(Hp)
    f_c = CAM_H / np.tan(PITCH) * 0.9 + 2.0
    diag = valid & (np.abs((fwd - f_c) - x_w) <= 0.25) & (np.abs(x_w) <= 3.0)
    conflicted = np.maximum(wall0, diag.astype(np.uint8))
    got = estimate_wall_rotation(conflicted, floor0, Hp)
    print(f"  [{'PASS' if got is None else 'FAIL'}] conflicting 0/45 walls -> {got}")
    ok &= got is None

    # 7-8 — R1-4 v3: the floor-lattice estimator recovers the EXISTING
    # floor's grout direction from the photo (the cue that outranks walls)
    for phi in (18.0, -30.0):
        img, floor = lattice_room(phi, Hp)
        got = estimate_floor_lattice_rotation(img, floor, Hp)
        want = fold(phi)
        good = got is not None and abs(fold(got - want)) <= 2.0
        print(f"  [{'PASS' if good else 'FAIL'}] lattice phi={phi:+.0f} deg -> "
              f"{'None' if got is None else f'{got:+.2f}'} (want {want:+.1f})")
        ok &= good

    # 9 — carpet / lineless floor: speckle texture, no lattice — must abstain
    # so the pipeline falls through to the wall-contact estimate
    rng = np.random.default_rng(11)
    _, floor = rotated_room(0.0, Hp)
    carpet = np.clip(
        180 + rng.normal(0, 6, (IMG_H, IMG_W, 3)), 0, 255
    ).astype(np.uint8)
    got = estimate_floor_lattice_rotation(carpet, floor, Hp)
    print(f"  [{'PASS' if got is None else 'FAIL'}] carpet (speckle) -> {got}")
    ok &= got is None

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


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