nx_bundleadjust.nx source
↩ module page · 140 lines · 6680 B
1// nx_bundleadjust.nx -- POSE REFINEMENT / bundle-adjustment (cadtwin P1, the named fix for the fixed-point
2// 8-point estimation-precision limit). Real SfM: the 8-point gives an initial (R,t); nonlinear minimization of
3// REPROJECTION error refines it to metric accuracy. Here: two-view pose-only refinement by COORDINATE DESCENT
4// (greedy hill-climbing) -- no Jacobians/linear-solves (fixed-point-friendly): try +-delta on each of 3 rotation
5// axes + 3 translation components; keep any move that lowers total reprojection error; shrink delta when stuck.
6// Points are implicitly re-triangulated (nx_recon3d midpoint) each cost eval. Composes nx_epose. Deterministic.
7// license_tier: ORIGINAL
8import "nx_epose.nx"
9const BA_MAGIC_1143: i64 = 1143
10const BA_MAGIC_16344: i64 = 16344
11const BA_MAGIC_16374: i64 = 16374
12const BA_MAGIC_16381: i64 = 16381
13const BA_MAGIC_16383: i64 = 16383
14const BA_MAGIC_16384: i64 = 16384
15
16const BA_Q: i64 = 16384
17
18func ba_abs(a: i64) -> i64 { if a < 0 { return 0 - a } return a }
19
20// EPIPOLAR cost: sum |x2^T E x1| over corr, with E=[t]_x R built from the pose. This is the CORRECT pose-only
21// cost -- it constrains (R,t) directly with NO free points (re-triangulating points lets them adapt to ANY pose,
22// giving a degenerate near-zero reprojection min at the WRONG pose -- the bug this replaces). x1=(x1,y1,Q),
23// x2=(x2,y2,Q). E normalized (max-entry Q) inside ep_build_E so the cost is pose-scale-invariant.
24func ba_cost(R: *i64, t: *i64, corr: *i64, n: i64) -> i64 {
25 let E: *i64 = sys_mmap(128) as *i64
26 ep_build_E(t[0], t[1], t[2], R, E)
27 var s: i64 = 0
28 var i: i64 = 0
29 while i < n {
30 let x1: i64 = corr[i * 4]
31 let y1: i64 = corr[i * 4 + 1]
32 let x2: i64 = corr[i * 4 + 2]
33 let y2: i64 = corr[i * 4 + 3]
34 let ex0: i64 = (E[0] * x1 + E[1] * y1 + E[2] * BA_Q) / BA_Q
35 let ex1: i64 = (E[3] * x1 + E[4] * y1 + E[5] * BA_Q) / BA_Q
36 let ex2: i64 = (E[6] * x1 + E[7] * y1 + E[8] * BA_Q) / BA_Q
37 let res: i64 = (x2 * ex0 + y2 * ex1 + BA_Q * ex2) / BA_Q
38 s = s + ba_abs(res)
39 i = i + 1
40 }
41 return s
42}
43// small rotation matrix about axis (0=x 1=y 2=z), sin s, cos c (Q14) -> out9
44func ba_rotmat(axis: i64, s: i64, c: i64, out: *i64) -> i64 {
45 var i: i64 = 0
46 while i < 9 { out[i] = 0; i = i + 1 }
47 if axis == 0 { out[0] = BA_Q; out[4] = c; out[5] = 0 - s; out[7] = s; out[8] = c; return 0 }
48 if axis == 1 { out[0] = c; out[2] = s; out[4] = BA_Q; out[6] = 0 - s; out[8] = c; return 0 }
49 out[0] = c; out[1] = 0 - s; out[3] = s; out[4] = c; out[8] = BA_Q
50 return 0
51}
52// apply a small rotation about axis to R (left-multiply): Rout = Rot(axis,s,c) * R
53func ba_rot_apply(R: *i64, axis: i64, s: i64, c: i64, Rout: *i64) -> i64 {
54 let rm: *i64 = sys_mmap(128) as *i64
55 ba_rotmat(axis, s, c, rm)
56 ep_matmul(rm, R, Rout)
57 return 0
58}
59
60// CHEIRALITY-CONSTRAINED coordinate descent. Greedy: each iteration evaluate all 12 moves (3 rot axes +-, 3
61// trans comps +-), pick the single move that most lowers the epipolar cost AMONG moves that keep ALL points in
62// front of both cameras (cheirality == n). The cheirality gate pins the descent to the TRUE basin -- the
63// twisted-pair rotation (which the epipolar cost alone cannot distinguish) puts points BEHIND, so it is rejected.
64func ba_refine(R0: *i64, t0: *i64, corr: *i64, n: i64, R: *i64, t: *i64) -> i64 {
65 var i: i64 = 0
66 while i < 9 { R[i] = R0[i]; i = i + 1 }
67 t[0] = t0[0]; t[1] = t0[1]; t[2] = t0[2]
68 let sins: *i64 = sys_mmap(64) as *i64
69 let coss: *i64 = sys_mmap(64) as *i64
70 let tdel: *i64 = sys_mmap(64) as *i64
71 sins[0] = BA_MAGIC_1143; coss[0] = BA_MAGIC_16344; tdel[0] = 400
72 sins[1] = 572; coss[1] = BA_MAGIC_16374; tdel[1] = 200
73 sins[2] = 286; coss[2] = BA_MAGIC_16381; tdel[2] = 100
74 sins[3] = 143; coss[3] = BA_MAGIC_16383; tdel[3] = 40
75 sins[4] = 71; coss[4] = BA_MAGIC_16384; tdel[4] = 16
76 sins[5] = 36; coss[5] = BA_MAGIC_16384; tdel[5] = 8
77 sins[6] = 18; coss[6] = BA_MAGIC_16384; tdel[6] = 4
78 let Rtry: *i64 = sys_mmap(128) as *i64
79 let ttry: *i64 = sys_mmap(32) as *i64
80 let Rbest: *i64 = sys_mmap(128) as *i64
81 let tbest: *i64 = sys_mmap(32) as *i64
82 var lvl: i64 = 0
83 while lvl < 7 {
84 let s: i64 = sins[lvl]
85 let c: i64 = coss[lvl]
86 let td: i64 = tdel[lvl]
87 var iters: i64 = 0
88 var going: i64 = 1
89 while going == 1 {
90 if iters >= 80 { going = 0 } else {
91 var bestcost: i64 = ba_cost(R, t, corr, n)
92 var found: i64 = 0
93 // rotation moves (both signs, 3 axes)
94 var mi: i64 = 0
95 while mi < 6 {
96 let ax: i64 = mi / 2
97 var sg: i64 = s
98 if (mi % 2) == 1 { sg = 0 - s }
99 ba_rot_apply(R, ax, sg, c, Rtry)
100 let cc: i64 = ba_cost(Rtry, t, corr, n)
101 if cc < bestcost {
102 if ep_cheir(Rtry, t, corr, n) == n {
103 bestcost = cc; found = 1
104 var k: i64 = 0
105 while k < 9 { Rbest[k] = Rtry[k]; k = k + 1 }
106 tbest[0] = t[0]; tbest[1] = t[1]; tbest[2] = t[2]
107 }
108 }
109 mi = mi + 1
110 }
111 // translation moves (both signs, 3 comps)
112 var ti: i64 = 0
113 while ti < 6 {
114 let tcp: i64 = ti / 2
115 ttry[0] = t[0]; ttry[1] = t[1]; ttry[2] = t[2]
116 if (ti % 2) == 0 { ttry[tcp] = t[tcp] + td } else { ttry[tcp] = t[tcp] - td }
117 r3_normalize(ttry[0], ttry[1], ttry[2], ttry)
118 let cc2: i64 = ba_cost(R, ttry, corr, n)
119 if cc2 < bestcost {
120 if ep_cheir(R, ttry, corr, n) == n {
121 bestcost = cc2; found = 1
122 var k2: i64 = 0
123 while k2 < 9 { Rbest[k2] = R[k2]; k2 = k2 + 1 }
124 tbest[0] = ttry[0]; tbest[1] = ttry[1]; tbest[2] = ttry[2]
125 }
126 }
127 ti = ti + 1
128 }
129 if found == 1 {
130 var kk: i64 = 0
131 while kk < 9 { R[kk] = Rbest[kk]; kk = kk + 1 }
132 t[0] = tbest[0]; t[1] = tbest[1]; t[2] = tbest[2]
133 iters = iters + 1
134 } else { going = 0 }
135 }
136 }
137 lvl = lvl + 1
138 }
139 return ba_cost(R, t, corr, n)
140}