nx_essential3d.nx source
↩ module page · 217 lines · 8150 B
1// nx_essential3d.nx -- ESSENTIAL MATRIX estimation (8-point algorithm), the piece that recovers relative camera
2// pose from 2D image correspondences with UNKNOWN poses = the front of two-view SfM (cadtwin P1). Reuses the
3// SVD-FREE power-iteration null-space tool proven in nx_register3d: the 8-point solves A f = 0 (f = smallest
4// eigenvector of A^T A), found by power-iterating a Gershgorin-shifted (cI - A^T A). Calibrated cameras ->
5// ESSENTIAL matrix E s.t. x'^T E x = 0 for corresponding normalized image points x,x'. Benchmarked by the
6// HELD-OUT epipolar residual (E must satisfy the constraint on points it was NOT fit to = not overfit).
7// Composes with nx_recon3d (E -> pose -> triangulate). All Q14 fixed-point, deterministic. license_tier: ORIGINAL
8import "nx_recon3d.nx"
9import "nx_vecmath.nx"
10const E3_MAGIC_23170: i64 = 23170
11const E3_MAGIC_1024: i64 = 1024
12const E3_MAGIC_2048: i64 = 2048
13
14const E3_Q: i64 = 16384
15
16// normalized (calibrated) image coords of world point X in a camera -> out2 = (Q*xc/zc, Q*yc/zc); ret zc
17func e3_projn(cx: i64, cy: i64, cz: i64, basis: *i64, x: i64, y: i64, z: i64, out2: *i64) -> i64 {
18 let vx: i64 = x - cx
19 let vy: i64 = y - cy
20 let vz: i64 = z - cz
21 let xc: i64 = (basis[0] * vx + basis[1] * vy + basis[2] * vz) / E3_Q
22 let yc: i64 = (basis[3] * vx + basis[4] * vy + basis[5] * vz) / E3_Q
23 let zc: i64 = (basis[6] * vx + basis[7] * vy + basis[8] * vz) / E3_Q
24 if zc == 0 { out2[0] = 0; out2[1] = 0; return 0 }
25 out2[0] = (E3_Q * xc) / zc
26 out2[1] = (E3_Q * yc) / zc
27 return zc
28}
29
30// build one row (9 entries) of A from a correspondence (nx,ny)<->(nx',ny') (Q14 normalized). homogeneous 3rd = Q.
31// row = [x'x, x'y, x'w, y'x, y'y, y'w, wx, wy, ww] in Q14 (products /Q).
32func e3_row(nx: i64, ny: i64, nxp: i64, nyp: i64, out9: *i64) -> i64 {
33 let w: i64 = E3_Q
34 out9[0] = (nxp * nx) / E3_Q
35 out9[1] = (nxp * ny) / E3_Q
36 out9[2] = (nxp * w) / E3_Q
37 out9[3] = (nyp * nx) / E3_Q
38 out9[4] = (nyp * ny) / E3_Q
39 out9[5] = (nyp * w) / E3_Q
40 out9[6] = (w * nx) / E3_Q
41 out9[7] = (w * ny) / E3_Q
42 out9[8] = (w * w) / E3_Q
43 return 0
44}
45
46func e3_abs(a: i64) -> i64 { if a < 0 { return 0 - a } return a }
47func e3_isqrt(n: i64) -> i64 { return vm_isqrt(n) }
48
49// estimate E (9-vector, then 3x3) from A (nrows x 9). out_e9 = the null-space vector (Q14 unit).
50func e3_estimate(a: *i64, nrows: i64, out_e9: *i64) -> i64 {
51 // ata = A^T A (9x9 symmetric)
52 let ata: *i64 = sys_mmap(81 * 8) as *i64
53 var i: i64 = 0
54 while i < 9 {
55 var j: i64 = 0
56 while j < 9 {
57 var s: i64 = 0
58 var k: i64 = 0
59 while k < nrows {
60 s = s + (a[k * 9 + i] * a[k * 9 + j]) / E3_Q
61 k = k + 1
62 }
63 ata[i * 9 + j] = s
64 j = j + 1
65 }
66 i = i + 1
67 }
68 // Gershgorin shift: M = cI - ata (smallest eigvec of ata = largest of M)
69 var c: i64 = 0
70 i = 0
71 while i < 9 {
72 var rs: i64 = 0
73 var j2: i64 = 0
74 while j2 < 9 { rs = rs + e3_abs(ata[i * 9 + j2]); j2 = j2 + 1 }
75 if rs > c { c = rs }
76 i = i + 1
77 }
78 let m: *i64 = sys_mmap(81 * 8) as *i64
79 i = 0
80 while i < 81 { m[i] = 0 - ata[i]; i = i + 1 }
81 i = 0
82 while i < 9 { m[i * 9 + i] = m[i * 9 + i] + c; i = i + 1 }
83 // scale m to ~Q14
84 var maxa: i64 = 1
85 i = 0
86 while i < 81 { let av: i64 = e3_abs(m[i]); if av > maxa { maxa = av } i = i + 1 }
87 let scl: i64 = (maxa / E3_Q) + 1
88 i = 0
89 while i < 81 { m[i] = m[i] / scl; i = i + 1 }
90 // power iteration (9-vec)
91 let v: *i64 = sys_mmap(9 * 8) as *i64
92 let w: *i64 = sys_mmap(9 * 8) as *i64
93 i = 0
94 while i < 9 { v[i] = E3_Q; i = i + 1 } // init all-ones
95 // normalize init
96 var l0: i64 = e3_isqrt(9 * E3_Q * E3_Q)
97 i = 0
98 while i < 9 { v[i] = (v[i] * E3_Q) / l0; i = i + 1 }
99 var it: i64 = 0
100 while it < 120 {
101 i = 0
102 while i < 9 {
103 var s: i64 = 0
104 var j3: i64 = 0
105 while j3 < 9 { s = s + (m[i * 9 + j3] * v[j3]) / E3_Q; j3 = j3 + 1 }
106 w[i] = s
107 i = i + 1
108 }
109 var sq: i64 = 0
110 i = 0
111 while i < 9 { sq = sq + (w[i] * w[i]) / E3_Q; i = i + 1 }
112 let len: i64 = e3_isqrt(sq * E3_Q)
113 if len == 0 { i = 0; while i < 9 { v[i] = 0; i = i + 1 } v[0] = E3_Q } else {
114 i = 0
115 while i < 9 { v[i] = (w[i] * E3_Q) / len; i = i + 1 }
116 }
117 it = it + 1
118 }
119 i = 0
120 while i < 9 { out_e9[i] = v[i]; i = i + 1 }
121 return 0
122}
123
124// ---- Hartley normalization: translate to centroid, isotropic-scale to mean-distance sqrt(2). REQUIRED for
125// the 8-point to be well-conditioned. cn = normalized coords, T[9] = the Q14 transform (so x_norm = T x). ----
126func e3_normset(c: *i64, n: i64, cn: *i64, t: *i64) -> i64 {
127 var mx: i64 = 0
128 var my: i64 = 0
129 var i: i64 = 0
130 while i < n { mx = mx + c[i * 2]; my = my + c[i * 2 + 1]; i = i + 1 }
131 mx = mx / n; my = my / n
132 var d: i64 = 0
133 i = 0
134 while i < n {
135 let dx: i64 = c[i * 2] - mx
136 let dy: i64 = c[i * 2 + 1] - my
137 d = d + e3_isqrt(dx * dx + dy * dy)
138 i = i + 1
139 }
140 d = d / n
141 if d == 0 { d = 1 }
142 let sqrt2q: i64 = E3_MAGIC_23170 // sqrt(2) in Q14
143 let s: i64 = (sqrt2q * E3_Q) / d // Q14 scale
144 i = 0
145 while i < n {
146 cn[i * 2] = (s * (c[i * 2] - mx)) / E3_Q
147 cn[i * 2 + 1] = (s * (c[i * 2 + 1] - my)) / E3_Q
148 i = i + 1
149 }
150 t[0] = s; t[1] = 0; t[2] = 0 - (s * mx) / E3_Q
151 t[3] = 0; t[4] = s; t[5] = 0 - (s * my) / E3_Q
152 t[6] = 0; t[7] = 0; t[8] = E3_Q
153 return 0
154}
155func e3_mat3T(a: *i64, o: *i64) -> i64 {
156 o[0] = a[0]; o[1] = a[3]; o[2] = a[6]
157 o[3] = a[1]; o[4] = a[4]; o[5] = a[7]
158 o[6] = a[2]; o[7] = a[5]; o[8] = a[8]
159 return 0
160}
161func e3_mat3mul(a: *i64, b: *i64, o: *i64) -> i64 {
162 var r: i64 = 0
163 while r < 3 {
164 var cc: i64 = 0
165 while cc < 3 {
166 o[r * 3 + cc] = (a[r * 3] * b[cc] + a[r * 3 + 1] * b[3 + cc] + a[r * 3 + 2] * b[6 + cc]) / E3_Q
167 cc = cc + 1
168 }
169 r = r + 1
170 }
171 return 0
172}
173// FULL normalized 8-point: raw correspondences c0,c1 (n) -> denormalized E (9-vec row-major, Q14).
174func e3_estimate_norm(c0: *i64, c1: *i64, n: i64, out_e: *i64) -> i64 {
175 let c0n: *i64 = sys_mmap(E3_MAGIC_1024) as *i64
176 let c1n: *i64 = sys_mmap(E3_MAGIC_1024) as *i64
177 let t0: *i64 = sys_mmap(9 * 8) as *i64
178 let t1: *i64 = sys_mmap(9 * 8) as *i64
179 e3_normset(c0, n, c0n, t0)
180 e3_normset(c1, n, c1n, t1)
181 let a: *i64 = sys_mmap(E3_MAGIC_2048) as *i64
182 let row: *i64 = sys_mmap(9 * 8) as *i64
183 var i: i64 = 0
184 while i < n {
185 e3_row(c0n[i * 2], c0n[i * 2 + 1], c1n[i * 2], c1n[i * 2 + 1], row)
186 var j: i64 = 0
187 while j < 9 { a[i * 9 + j] = row[j]; j = j + 1 }
188 i = i + 1
189 }
190 let eh: *i64 = sys_mmap(9 * 8) as *i64
191 e3_estimate(a, n, eh) // Ê (normalized frame), 3x3 row-major
192 let t1t: *i64 = sys_mmap(9 * 8) as *i64
193 e3_mat3T(t1, t1t)
194 let tmp: *i64 = sys_mmap(9 * 8) as *i64
195 e3_mat3mul(t1t, eh, tmp)
196 e3_mat3mul(tmp, t0, out_e) // E = T1^T Ê T0
197 // normalize E to max-entry Q14 (epipolar constraint is scale-invariant; makes residuals comparable)
198 var mxa: i64 = 0
199 var ii: i64 = 0
200 while ii < 9 { let av: i64 = e3_abs(out_e[ii]); if av > mxa { mxa = av } ii = ii + 1 }
201 if mxa > 0 {
202 ii = 0
203 while ii < 9 { out_e[ii] = (out_e[ii] * E3_Q) / mxa; ii = ii + 1 }
204 }
205 return 0
206}
207
208// epipolar residual |x'^T E x| for a correspondence (E as 9-vec row-major, Q14). x=(nx,ny,Q), x'=(nxp,nyp,Q).
209func e3_residual(e9: *i64, nx: i64, ny: i64, nxp: i64, nyp: i64) -> i64 {
210 let w: i64 = E3_Q
211 // Ex = E * x (3-vec)
212 let ex0: i64 = (e9[0] * nx + e9[1] * ny + e9[2] * w) / E3_Q
213 let ex1: i64 = (e9[3] * nx + e9[4] * ny + e9[5] * w) / E3_Q
214 let ex2: i64 = (e9[6] * nx + e9[7] * ny + e9[8] * w) / E3_Q
215 let r: i64 = (nxp * ex0 + nyp * ex1 + w * ex2) / E3_Q
216 return e3_abs(r)
217}