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