1import json
  2import math
  3from pathlib import Path
  4import numpy as np
  5from scipy.optimize import least_squares
  6
  7
  8def rot(a):
  9    c, s = np.cos(a), np.sin(a)
 10    return np.array([[c, -s], [s, c]])
 11
 12
 13def wrap(a):
 14    return (a + np.pi) % (2 * np.pi) - np.pi
 15
 16
 17def two_view_init(q, ell_v, ell_t, eps=1e-9):
 18    """Constructive gauge initialization; pair is the largest global displacement."""
 19    q, ell_v, ell_t = map(np.asarray, (q, ell_v, ell_t))
 20    best = None
 21    for a in range(len(q)):
 22        for b in range(a + 1, len(q)):
 23            n = np.linalg.norm(q[b] - q[a])
 24            if best is None or n > best[0]:
 25                best = (n, a, b)
 26    _, a, b = best
 27    dq = q[b] - q[a]
 28    dl = ell_v[b] - ell_v[a]
 29    if np.linalg.norm(dl) < eps:
 30        raise ValueError("unexcited relay displacement")
 31    psi = math.atan2(dq[1], dq[0]) - math.atan2(dl[1], dl[0])
 32    psi = wrap(psi)
 33    R = rot(psi)
 34    x = q[a] - R @ ell_v[a]
 35    # Equal robust weights are the noiseless/low-noise special case. Huber weights
 36    # could be added here without changing the gauge construction.
 37    p_views = x[None, :] + (R @ ell_t.T).T
 38    p = np.mean(p_views, axis=0)
 39    return np.array([x[0], x[1], p[0], p[1], psi]), (a, b)
 40
 41
 42def make_data(d, psi=1.1, x=np.array([0.8, -0.4]), p=np.array([2.0, 1.3]),
 43              sigma=0.0, rng=None):
 44    rng = np.random.default_rng() if rng is None else rng
 45    # Two views with known global motion d along x. The second view is selected.
 46    q = np.array([[0., 0.], [d, 0.]])
 47    Rm = rot(-psi)
 48    ell_v = np.array([Rm @ (qk - x) for qk in q])
 49    ell_t = np.array([Rm @ (p - x) for _ in q])
 50    if sigma:
 51        q = q + rng.normal(0, sigma, q.shape)
 52        ell_v = ell_v + rng.normal(0, sigma, ell_v.shape)
 53        ell_t = ell_t + rng.normal(0, sigma, ell_t.shape)
 54    return q, ell_v, ell_t
 55
 56
 57def residual(z, q, ev, et):
 58    x, p, psi = z[:2], z[2:4], z[4]
 59    R = rot(psi)
 60    rv = q - x[None, :] - (R @ ev.T).T
 61    rt = p[None, :] - x[None, :] - (R @ et.T).T
 62    return np.concatenate([rv.ravel(), rt.ravel()])
 63
 64
 65def refine(z0, q, ev, et):
 66    out = least_squares(residual, z0, args=(q, ev, et), max_nfev=200,
 67                        xtol=1e-12, ftol=1e-12, gtol=1e-12)
 68    return out.x, np.linalg.norm(residual(out.x, q, ev, et)), out.nfev
 69
 70
 71def main():
 72    rng = np.random.default_rng(2048)
 73    # Prediction 1: noiseless nonzero displacement gives exact gauge recovery.
 74    exact = []
 75    for d in [0.01, 0.1, 1.0, 3.0]:
 76        q, ev, et = make_data(d, sigma=0, rng=rng)
 77        z, pair = two_view_init(q, ev, et)
 78        truth = np.array([.8, -.4, 2., 1.3, 1.1])
 79        exact.append([d, float(np.linalg.norm(z[:4] - truth[:4])), abs(wrap(z[4]-truth[4])), pair])
 80
 81    # Prediction 2: with isotropic measurement noise, yaw RMSE scales as 1/d.
 82    # For two independent noisy vectors, first-order theory predicts sqrt(2)*sigma/d.
 83    sigma = 0.002
 84    ds = np.array([0.05, 0.1, 0.2, 0.4, 0.8, 1.6, 3.2])
 85    trials = 800
 86    sweep = []
 87    for d in ds:
 88        errs = []
 89        for _ in range(trials):
 90            q, ev, et = make_data(d, sigma=sigma, rng=rng)
 91            try:
 92                z, _ = two_view_init(q, ev, et)
 93                errs.append(abs(wrap(z[4] - 1.1)))
 94            except ValueError:
 95                errs.append(np.pi)
 96        rmse = float(np.sqrt(np.mean(np.square(errs))))
 97        predicted = math.sqrt(2) * sigma / d
 98        sweep.append([float(d), rmse, predicted, rmse / predicted])
 99
100    # Prediction 3: at zero relay displacement the angle is not identifiable;
101    # numerically, tiny displacement has rapidly increasing error.
102    small = []
103    for d in [0.0, 1e-5, 1e-4, 1e-3, 1e-2]:
104        errs = []
105        rejected = 0
106        for _ in range(300):
107            q, ev, et = make_data(d, sigma=sigma, rng=rng)
108            try:
109                z, _ = two_view_init(q, ev, et, eps=1e-10)
110                errs.append(abs(wrap(z[4] - 1.1)))
111            except ValueError:
112                rejected += 1
113        small.append([d, float(np.sqrt(np.mean(np.square(errs)))) if errs else None, rejected])
114
115    # Secondary mini comparison: nonlinear refinement from random gauge versus initializer.
116    q, ev, et = make_data(1.5, sigma=0.01, rng=rng)
117    zi, _ = two_view_init(q, ev, et)
118    truth = np.array([.8, -.4, 2., 1.3, 1.1])
119    init_runs, random_runs = [], []
120    for _ in range(40):
121        zr = np.array([rng.uniform(-2, 2), rng.uniform(-2, 2), rng.uniform(-2, 4),
122                       rng.uniform(-2, 4), rng.uniform(-np.pi, np.pi)])
123        zo, lr, nr = refine(zr, q, ev, et)
124        _, li, ni = refine(zi, q, ev, et)
125        random_runs.append([lr, nr, float(np.linalg.norm(zo[:4]-truth[:4]))])
126        init_runs.append([li, ni, float(np.linalg.norm(zi[:4]-truth[:4]))])
127    result = {
128        "exact_recovery": exact,
129        "noise_inverse_displacement": sweep,
130        "near_singular": small,
131        "refinement": {
132            "random_median_residual": float(np.median(np.array(random_runs)[:,0])),
133            "initializer_median_residual": float(np.median(np.array(init_runs)[:,0])),
134            "random_median_nfev": float(np.median(np.array(random_runs)[:,1])),
135            "initializer_median_nfev": float(np.median(np.array(init_runs)[:,1])),
136            "random_target_error_median": float(np.median(np.array(random_runs)[:,2])),
137            "initializer_target_error": float(np.median(np.array(init_runs)[:,2]))
138        }
139    }
140    Path("results.json").write_text(json.dumps(result, indent=2))
141    print(json.dumps(result, indent=2))
142
143if __name__ == "__main__":
144    main()