code wiki / _hdl_build / nx_epose_gate.nx
nx_epose_gate.nx source
↩ module page · 105 lines · 4910 B
1// nx_epose_gate.nx -- benchmark E->R,t decomposition (cadtwin P1, completes photo->pose->3D). Build E from a
2// KNOWN (R,t), decompose, and verify one of the two rotation candidates matches R and the recovered translation
3// direction matches t (up to sign -- the 4-fold resolved by cheirality). KATs: identity rotation + a 30deg-y
4// rotation. Deterministic. expect_exit: 0 license_tier: ORIGINAL
5import "nx_epose.nx"
6
7func qg_puts(s: *u8) -> i64 { var n: i64 = 0; while s[n] != (0 as u8) { n = n + 1 } sys_write(1, s, n); return 0 }
8func qg_putn(v: i64) -> i64 {
9 let bb: *u8 = sys_mmap(28)
10 var m: i64 = v
11 if m < 0 { qg_puts("-" as *u8); m = 0 - m }
12 let t: *u8 = sys_mmap(28)
13 var k: i64 = 0
14 if m == 0 { t[0] = 48 as u8; k = 1 }
15 while m > 0 { t[k] = (48 + (m % 10)) as u8; m = m / 10; k = k + 1 }
16 var i: i64 = 0
17 while i < k { bb[i] = t[k - 1 - i]; i = i + 1 }
18 sys_write(1, bb, k)
19 return 0
20}
21func qg_tooth(name: *u8, pass: i64, fails: *i64) -> i64 {
22 qg_puts(" " as *u8); qg_puts(name); qg_puts(" -> " as *u8)
23 if pass == 1 { qg_puts("PASS\n" as *u8); return 0 }
24 qg_puts("FAIL\n" as *u8)
25 fails[0] = fails[0] + 1
26 return 0
27}
28
29// run one case: known R (Q14) + known t (coord) -> build E -> decompose -> min Frobenius over {R1,R2}, |t.tn|
30func run_case(R: *i64, tx: i64, ty: i64, tz: i64, out2: *i64) -> i64 {
31 let E: *i64 = sys_mmap(128) as *i64
32 ep_build_E(tx, ty, tz, R, E)
33 let R1: *i64 = sys_mmap(128) as *i64
34 let R2: *i64 = sys_mmap(128) as *i64
35 let t3: *i64 = sys_mmap(32) as *i64
36 ep_decompose(E, R1, R2, t3)
37 let f1: i64 = ep_frob(R, R1)
38 let f2: i64 = ep_frob(R, R2)
39 var fmin: i64 = f1
40 if f2 < f1 { fmin = f2 }
41 // t direction match: normalize known t, dot with t3 (Q14), abs
42 let tn: *i64 = sys_mmap(32) as *i64
43 r3_normalize(tx, ty, tz, tn)
44 var dt: i64 = (t3[0] * tn[0] + t3[1] * tn[1] + t3[2] * tn[2]) / EP_Q
45 if dt < 0 { dt = 0 - dt }
46 out2[0] = fmin
47 out2[1] = dt
48 return 0
49}
50
51func main() -> i64 {
52 qg_puts("=== nx_epose_gate -- essential matrix -> relative pose (R,t) decomposition ===\n" as *u8)
53 let fails: *i64 = sys_mmap(8) as *i64
54 fails[0] = 0
55 let out2: *i64 = sys_mmap(16) as *i64
56
57 // KAT1: identity R, t = (4000,-3000,5000)
58 let I: *i64 = sys_mmap(128) as *i64
59 I[0] = EP_Q; I[1] = 0; I[2] = 0
60 I[3] = 0; I[4] = EP_Q; I[5] = 0
61 I[6] = 0; I[7] = 0; I[8] = EP_Q
62 run_case(I, 4000, 0 - 3000, 5000, out2)
63 qg_puts(" KAT1 identity: min-Frob(R)=" as *u8); qg_putn(out2[0]); qg_puts(" |t.tn|=" as *u8); qg_putn(out2[1]); qg_puts(" (Frob~0, dot~16384)\n" as *u8)
64 var t1: i64 = 0
65 if out2[0] < 3000 { if out2[1] > 15000 { t1 = 1 } }
66 let ig1: i64 = qg_tooth("KAT1 identity R recovered + t direction" as *u8, t1, fails)
67
68 // KAT2: R_y(30deg) = [[cos30,0,sin30],[0,1,0],[-sin30,0,cos30]]
69 let Ry: *i64 = sys_mmap(128) as *i64
70 Ry[0] = 14189; Ry[1] = 0; Ry[2] = 8192
71 Ry[3] = 0; Ry[4] = EP_Q; Ry[5] = 0
72 Ry[6] = 0 - 8192; Ry[7] = 0; Ry[8] = 14189
73 run_case(Ry, 4000, 0 - 3000, 5000, out2)
74 qg_puts(" KAT2 R_y30: min-Frob(R)=" as *u8); qg_putn(out2[0]); qg_puts(" |t.tn|=" as *u8); qg_putn(out2[1]); qg_puts("\n" as *u8)
75 var t2: i64 = 0
76 if out2[0] < 3000 { if out2[1] > 15000 { t2 = 1 } }
77 let ig2: i64 = qg_tooth("KAT2 30deg-y rotation recovered + t direction" as *u8, t2, fails)
78
79 // KAT3: a different rotation R_z(45) = [[c,-s,0],[s,c,0],[0,0,1]] c=s=11585
80 let Rz: *i64 = sys_mmap(128) as *i64
81 Rz[0] = 11585; Rz[1] = 0 - 11585; Rz[2] = 0
82 Rz[3] = 11585; Rz[4] = 11585; Rz[5] = 0
83 Rz[6] = 0; Rz[7] = 0; Rz[8] = EP_Q
84 run_case(Rz, 2000, 6000, 0 - 1000, out2)
85 qg_puts(" KAT3 R_z45: min-Frob(R)=" as *u8); qg_putn(out2[0]); qg_puts(" |t.tn|=" as *u8); qg_putn(out2[1]); qg_puts("\n" as *u8)
86 var t3: i64 = 0
87 if out2[0] < 3000 { if out2[1] > 15000 { t3 = 1 } }
88 let ig3: i64 = qg_tooth("KAT3 45deg-z rotation recovered + t direction" as *u8, t3, fails)
89
90 // T4 determinism
91 let o2: *i64 = sys_mmap(16) as *i64
92 run_case(Ry, 4000, 0 - 3000, 5000, out2)
93 run_case(Ry, 4000, 0 - 3000, 5000, o2)
94 var t4: i64 = 0
95 if out2[0] == o2[0] { if out2[1] == o2[1] { t4 = 1 } }
96 let ig4: i64 = qg_tooth("T4 deterministic" as *u8, t4, fails)
97
98 qg_puts("\nP1 COMPLETE: features->match->essential-matrix->E->R,t (this)->triangulate->register = full uncalibrated\n" as *u8)
99 qg_puts("two-view reconstruction from IMAGES ALONE (unknown poses). The 4 (R,t) candidates {R1,R2}x{+-t} are\n" as *u8)
100 qg_puts("disambiguated by CHEIRALITY (nx_recon3d triangulate -> the pose with points in front of both cameras).\n" as *u8)
101 qg_puts("\nfails=" as *u8); qg_putn(fails[0]); qg_puts("\n" as *u8)
102 if fails[0] == 0 { qg_puts("GREEN -- E->R,t decomposition 4/4 (SVD-free, recovers known pose)\n" as *u8); return 0 }
103 qg_puts("RED\n" as *u8)
104 return 1
105}