nx_essential3d_gate.nx source

1// nx_essential3d_gate.nx -- benchmark the essential-matrix 8-point estimation (cadtwin P1 pose front). Two 2// cameras view a non-coplanar 3D cloud; fit E from 12 correspondences via the SVD-free power-iteration null 3// space; the HELD-OUT epipolar residual (6 points NOT used to fit) proves E is the true two-view geometry, 4// not overfit. Neg-control (wrong correspondences) -> large residual. This recovers relative pose geometry 5// from images with UNKNOWN poses = the front of two-view SfM. expect_exit: 0 license_tier: ORIGINAL 6import "nx_essential3d.nx" 7 8func hg_puts(s: *u8) -> i64 { var n: i64 = 0; while s[n] != (0 as u8) { n = n + 1 } sys_write(1, s, n); return 0 } 9func hg_putn(v: i64) -> i64 { 10 let bb: *u8 = sys_mmap(28) 11 var m: i64 = v 12 if m < 0 { hg_puts("-" as *u8); m = 0 - m } 13 let t: *u8 = sys_mmap(28) 14 var k: i64 = 0 15 if m == 0 { t[0] = 48 as u8; k = 1 } 16 while m > 0 { t[k] = (48 + (m % 10)) as u8; m = m / 10; k = k + 1 } 17 var i: i64 = 0 18 while i < k { bb[i] = t[k - 1 - i]; i = i + 1 } 19 sys_write(1, bb, k) 20 return 0 21} 22func hg_tooth(name: *u8, pass: i64, fails: *i64) -> i64 { 23 hg_puts(" " as *u8); hg_puts(name); hg_puts(" -> " as *u8) 24 if pass == 1 { hg_puts("PASS\n" as *u8); return 0 } 25 hg_puts("FAIL\n" as *u8) 26 fails[0] = fails[0] + 1 27 return 0 28} 29 30func main() -> i64 { 31 hg_puts("=== nx_essential3d_gate -- essential matrix (8-point), held-out epipolar benchmark ===\n" as *u8) 32 let fails: *i64 = sys_mmap(8) as *i64 33 fails[0] = 0 34 35 // non-coplanar 3D cloud (18 points spread in a volume, deterministic scatter) 36 let npt: i64 = 18 37 let X: *i64 = sys_mmap(1024) as *i64 38 var i: i64 = 0 39 while i < npt { 40 X[i * 3] = ((i * 7) % 9 - 4) * 220 41 X[i * 3 + 1] = ((i * 5) % 7 - 3) * 220 42 X[i * 3 + 2] = ((i * 11) % 5 - 2) * 260 43 i = i + 1 44 } 45 46 // two cameras (different positions = baseline), both looking at origin 47 let b0: *i64 = sys_mmap(128) as *i64 48 let b1: *i64 = sys_mmap(128) as *i64 49 r3_lookat(4000, 800, 5000, 0, 0, 0, b0) 50 r3_lookat(0 - 3500, 1200, 5200, 0, 0, 0, b1) 51 52 // correspondences: normalized coords in each camera 53 let c0: *i64 = sys_mmap(1024) as *i64 // (nx,ny) per point in cam0 54 let c1: *i64 = sys_mmap(1024) as *i64 // in cam1 55 let uv: *i64 = sys_mmap(16) as *i64 56 i = 0 57 while i < npt { 58 e3_projn(4000, 800, 5000, b0, X[i * 3], X[i * 3 + 1], X[i * 3 + 2], uv) 59 c0[i * 2] = uv[0]; c0[i * 2 + 1] = uv[1] 60 e3_projn(0 - 3500, 1200, 5200, b1, X[i * 3], X[i * 3 + 1], X[i * 3 + 2], uv) 61 c1[i * 2] = uv[0]; c1[i * 2 + 1] = uv[1] 62 i = i + 1 63 } 64 65 // fit E from the first 12 correspondences (Hartley-normalized 8-point) 66 let nfit: i64 = 12 67 let E: *i64 = sys_mmap(9 * 8) as *i64 68 e3_estimate_norm(c0, c1, nfit, E) 69 hg_puts(" E (Q14, x1000-scaled view): " as *u8) 70 i = 0 71 while i < 9 { hg_putn(E[i]); hg_puts(" " as *u8); i = i + 1 } 72 hg_puts("\n" as *u8) 73 74 // training residual (mean over the 12 fit points) 75 var tr: i64 = 0 76 i = 0 77 while i < nfit { tr = tr + e3_residual(E, c0[i * 2], c0[i * 2 + 1], c1[i * 2], c1[i * 2 + 1]); i = i + 1 } 78 tr = tr / nfit 79 // HELD-OUT residual (points 12..17, NOT used to fit) 80 var ho: i64 = 0 81 var hn: i64 = 0 82 i = nfit 83 while i < npt { ho = ho + e3_residual(E, c0[i * 2], c0[i * 2 + 1], c1[i * 2], c1[i * 2 + 1]); hn = hn + 1; i = i + 1 } 84 ho = ho / hn 85 hg_puts(" TRAIN mean epipolar residual=" as *u8); hg_putn(tr) 86 hg_puts(" HELD-OUT mean residual=" as *u8); hg_putn(ho); hg_puts(" (both expect ~0)\n" as *u8) 87 88 // E non-degenerate (not all ~0) 89 var enorm: i64 = 0 90 i = 0 91 while i < 9 { let a: i64 = E[i]; if a < 0 { enorm = enorm - a } else { enorm = enorm + a } i = i + 1 } 92 var t1: i64 = 0 93 if enorm > 1000 { t1 = 1 } 94 let ig1: i64 = hg_tooth("T1 E non-degenerate (recovered a real matrix)" as *u8, t1, fails) 95 96 var t2: i64 = 0 97 if tr < 60 { t2 = 1 } 98 let ig2: i64 = hg_tooth("T2 TRAINING epipolar residual ~0 (< 60)" as *u8, t2, fails) 99 100 var t3: i64 = 0 101 if ho < 60 { t3 = 1 } 102 let ig3: i64 = hg_tooth("T3 HELD-OUT epipolar residual ~0 (< 60) = TRUE geometry, not overfit" as *u8, t3, fails) 103 104 // neg-control: fit E from WRONG correspondences (shuffle cam1) 105 let Aw: *i64 = sys_mmap(2048) as *i64 106 i = 0 107 while i < nfit { 108 let jj: i64 = (i + 5) % nfit 109 e3_row(c0[i * 2], c0[i * 2 + 1], c1[jj * 2], c1[jj * 2 + 1], row) 110 var j: i64 = 0 111 while j < 9 { Aw[i * 9 + j] = row[j]; j = j + 1 } 112 i = i + 1 113 } 114 let Ew: *i64 = sys_mmap(9 * 8) as *i64 115 e3_estimate(Aw, nfit, Ew) 116 // its held-out residual on the CORRECT correspondences (should be large -- wrong geometry) 117 var nres: i64 = 0 118 i = nfit 119 while i < npt { nres = nres + e3_residual(Ew, c0[i * 2], c0[i * 2 + 1], c1[i * 2], c1[i * 2 + 1]); i = i + 1 } 120 nres = nres / hn 121 hg_puts(" neg-control (wrong-corr E) held-out residual=" as *u8); hg_putn(nres); hg_puts("\n" as *u8) 122 var t4: i64 = 0 123 if nres > ho * 4 { if nres > 100 { t4 = 1 } } 124 let ig4: i64 = hg_tooth("T4 NEG-CONTROL wrong correspondences -> large residual (metric works)" as *u8, t4, fails) 125 126 // determinism 127 let E2: *i64 = sys_mmap(9 * 8) as *i64 128 e3_estimate_norm(c0, c1, nfit, E2) 129 var same: i64 = 1 130 i = 0 131 while i < 9 { if E2[i] != E[i] { same = 0 } i = i + 1 } 132 let ig5: i64 = hg_tooth("T5 deterministic" as *u8, same, fails) 133 134 hg_puts("\nSOTA CONTEXT: the essential matrix = relative pose from images with UNKNOWN poses (Hartley-Zisserman/\n" as *u8) 135 hg_puts("COLMAP front-end). SVD-FREE via power-iteration null space (same tool as nx_register3d). Composes:\n" as *u8) 136 hg_puts("features(nx_features) -> match -> E (this) -> R,t decomposition (next) -> triangulate(nx_recon3d) -> register.\n" as *u8) 137 hg_puts("\nfails=" as *u8); hg_putn(fails[0]); hg_puts("\n" as *u8) 138 if fails[0] == 0 { hg_puts("GREEN -- essential matrix 8-point 5/5, held-out epipolar-benchmarked (pose front of SfM)\n" as *u8); return 0 } 139 hg_puts("RED\n" as *u8) 140 return 1 141}