nx_epose.nx source
↩ module page · 246 lines · 10459 B
1// nx_epose.nx -- ESSENTIAL MATRIX -> RELATIVE POSE (R, t) decomposition (cadtwin P1, completes photo->pose->3D).
2// The last link of two-view SfM: given E (from nx_essential3d), recover the camera rotation R + translation
3// direction t. Classical SVD decomposition E = U diag(s,s,0) V^T -> R = U W V^T (or U W^T V^T), t = u3; the 4
4// (R,t) candidates are disambiguated by CHEIRALITY (points in front of both cameras). SVD done SVD-FREE:
5// V = eigenvectors of E^T E (power-iteration dominant + null; v2 = v3 x v1), and U DERIVED from E V (consistent,
6// avoids the repeated-singular-value ambiguity). All Q14 fixed-point. Composes nx_recon3d (vec math + triangulate
7// for cheirality). license_tier: ORIGINAL
8import "nx_recon3d.nx"
9const EP_MAGIC_9459: i64 = 9459
10const EP_MAGIC_5039: i64 = 5039
11const EP_MAGIC_12000: i64 = 12000
12
13const EP_Q: i64 = 16384
14
15func ep_abs(a: i64) -> i64 { if a < 0 { return 0 - a } return a }
16// 3x3 (row-major Q14) times 3-vec -> out3 (Q14)
17func ep_mv(m: *i64, x: i64, y: i64, z: i64, out3: *i64) -> i64 {
18 out3[0] = (m[0] * x + m[1] * y + m[2] * z) / EP_Q
19 out3[1] = (m[3] * x + m[4] * y + m[5] * z) / EP_Q
20 out3[2] = (m[6] * x + m[7] * y + m[8] * z) / EP_Q
21 return 0
22}
23func ep_matmul(a: *i64, b: *i64, o: *i64) -> i64 {
24 var r: i64 = 0
25 while r < 3 {
26 var c: i64 = 0
27 while c < 3 {
28 o[r * 3 + c] = (a[r * 3] * b[c] + a[r * 3 + 1] * b[3 + c] + a[r * 3 + 2] * b[6 + c]) / EP_Q
29 c = c + 1
30 }
31 r = r + 1
32 }
33 return 0
34}
35func ep_transpose(a: *i64, o: *i64) -> i64 {
36 o[0] = a[0]; o[1] = a[3]; o[2] = a[6]
37 o[3] = a[1]; o[4] = a[4]; o[5] = a[7]
38 o[6] = a[2]; o[7] = a[5]; o[8] = a[8]
39 return 0
40}
41func ep_cross(ax: i64, ay: i64, az: i64, bx: i64, by: i64, bz: i64, o: *i64) -> i64 {
42 o[0] = (ay * bz - az * by) / EP_Q
43 o[1] = (az * bx - ax * bz) / EP_Q
44 o[2] = (ax * by - ay * bx) / EP_Q
45 return 0
46}
47// handedness sign of columns of a 3x3 (row-major): det = c0 . (c1 x c2); returns +1/-1
48func ep_dethand(m: *i64) -> i64 {
49 let cx: *i64 = sys_mmap(32) as *i64
50 // c1 x c2 (columns 1,2): c1=(m1,m4,m7), c2=(m2,m5,m8)
51 ep_cross(m[1], m[4], m[7], m[2], m[5], m[8], cx)
52 // c0 . that: c0=(m0,m3,m6)
53 let d: i64 = (m[0] * cx[0] + m[3] * cx[1] + m[6] * cx[2]) / EP_Q
54 if d < 0 { return 0 - 1 }
55 return 1
56}
57
58// dominant eigenvector (unit Q14) of a 3x3 SYMMETRIC matrix m via power iteration -> out3
59func ep_eig_dom(m: *i64, out3: *i64) -> i64 {
60 let v: *i64 = sys_mmap(32) as *i64
61 let w: *i64 = sys_mmap(32) as *i64
62 // scale m to ~Q14 (direction-only)
63 let sm: *i64 = sys_mmap(128) as *i64
64 var maxa: i64 = 1
65 var i: i64 = 0
66 while i < 9 { let a: i64 = ep_abs(m[i]); if a > maxa { maxa = a } i = i + 1 }
67 let scl: i64 = (maxa / EP_Q) + 1
68 i = 0
69 while i < 9 { sm[i] = m[i] / scl; i = i + 1 }
70 v[0] = EP_MAGIC_9459; v[1] = EP_MAGIC_5039; v[2] = EP_MAGIC_12000 // arbitrary non-degenerate init
71 r3_normalize(v[0], v[1], v[2], v)
72 var it: i64 = 0
73 while it < 80 {
74 ep_mv(sm, v[0], v[1], v[2], w)
75 r3_normalize(w[0], w[1], w[2], v)
76 it = it + 1
77 }
78 out3[0] = v[0]; out3[1] = v[1]; out3[2] = v[2]
79 return 0
80}
81// null (smallest-eigenvalue) eigenvector of symmetric m: power-iter on (cI - m), c = Gershgorin -> out3
82func ep_eig_null(m: *i64, out3: *i64) -> i64 {
83 let sh: *i64 = sys_mmap(128) as *i64
84 var c: i64 = 0
85 var i: i64 = 0
86 while i < 3 {
87 var rs: i64 = 0
88 var j: i64 = 0
89 while j < 3 { rs = rs + ep_abs(m[i * 3 + j]); j = j + 1 }
90 if rs > c { c = rs }
91 i = i + 1
92 }
93 i = 0
94 while i < 9 { sh[i] = 0 - m[i]; i = i + 1 }
95 sh[0] = sh[0] + c; sh[4] = sh[4] + c; sh[8] = sh[8] + c
96 return ep_eig_dom(sh, out3)
97}
98
99// build E = [t]_x R (row-major Q14). t coord, R Q14. E entries then normalized to max Q14.
100func ep_build_E(tx: i64, ty: i64, tz: i64, R: *i64, E: *i64) -> i64 {
101 let tx3: *i64 = sys_mmap(128) as *i64
102 tx3[0] = 0; tx3[1] = 0 - tz; tx3[2] = ty
103 tx3[3] = tz; tx3[4] = 0; tx3[5] = 0 - tx
104 tx3[6] = 0 - ty; tx3[7] = tx; tx3[8] = 0
105 ep_matmul(tx3, R, E)
106 var maxa: i64 = 1
107 var i: i64 = 0
108 while i < 9 { let a: i64 = ep_abs(E[i]); if a > maxa { maxa = a } i = i + 1 }
109 i = 0
110 while i < 9 { E[i] = (E[i] * EP_Q) / maxa; i = i + 1 }
111 return 0
112}
113
114// DECOMPOSE E -> R1, R2 (two rotation candidates, Q14 row-major), t3 (unit translation dir). 4 poses =
115// {R1,R2} x {t,-t}; caller picks by cheirality.
116func ep_decompose(E: *i64, R1: *i64, R2: *i64, t3: *i64) -> i64 {
117 let Et: *i64 = sys_mmap(128) as *i64
118 ep_transpose(E, Et)
119 let M: *i64 = sys_mmap(128) as *i64
120 ep_matmul(Et, E, M) // M = E^T E (symmetric)
121 let v1: *i64 = sys_mmap(32) as *i64
122 let v2: *i64 = sys_mmap(32) as *i64
123 let v3: *i64 = sys_mmap(32) as *i64
124 ep_eig_dom(M, v1)
125 ep_eig_null(M, v3)
126 ep_cross(v3[0], v3[1], v3[2], v1[0], v1[1], v1[2], v2)
127 r3_normalize(v2[0], v2[1], v2[2], v2)
128 // V columns = v1 v2 v3
129 let V: *i64 = sys_mmap(128) as *i64
130 V[0] = v1[0]; V[1] = v2[0]; V[2] = v3[0]
131 V[3] = v1[1]; V[4] = v2[1]; V[5] = v3[1]
132 V[6] = v1[2]; V[7] = v2[2]; V[8] = v3[2]
133 if ep_dethand(V) < 0 {
134 V[2] = 0 - V[2]; V[5] = 0 - V[5]; V[8] = 0 - V[8] // flip v3 -> det +1
135 v3[0] = 0 - v3[0]; v3[1] = 0 - v3[1]; v3[2] = 0 - v3[2]
136 }
137 // U columns: u1 = norm(E v1), u3 = norm(u1 x E v2), u2 = norm(u3 x u1)
138 let u1: *i64 = sys_mmap(32) as *i64
139 let u2: *i64 = sys_mmap(32) as *i64
140 let u3: *i64 = sys_mmap(32) as *i64
141 let tmp: *i64 = sys_mmap(32) as *i64
142 ep_mv(E, v1[0], v1[1], v1[2], tmp)
143 r3_normalize(tmp[0], tmp[1], tmp[2], u1)
144 ep_mv(E, v2[0], v2[1], v2[2], tmp) // E v2
145 let ev2: *i64 = sys_mmap(32) as *i64
146 r3_normalize(tmp[0], tmp[1], tmp[2], ev2)
147 ep_cross(u1[0], u1[1], u1[2], ev2[0], ev2[1], ev2[2], tmp)
148 r3_normalize(tmp[0], tmp[1], tmp[2], u3)
149 ep_cross(u3[0], u3[1], u3[2], u1[0], u1[1], u1[2], tmp)
150 r3_normalize(tmp[0], tmp[1], tmp[2], u2)
151 let U: *i64 = sys_mmap(128) as *i64
152 U[0] = u1[0]; U[1] = u2[0]; U[2] = u3[0]
153 U[3] = u1[1]; U[4] = u2[1]; U[5] = u3[1]
154 U[6] = u1[2]; U[7] = u2[2]; U[8] = u3[2]
155 if ep_dethand(U) < 0 {
156 U[2] = 0 - U[2]; U[5] = 0 - U[5]; U[8] = 0 - U[8] // flip u3 -> det +1 (t sign resolved by cheirality)
157 }
158 t3[0] = U[2]; t3[1] = U[5]; t3[2] = U[8] // third column of U = translation dir
159 // W and W^T (Q14)
160 let W: *i64 = sys_mmap(128) as *i64
161 W[0] = 0; W[1] = 0 - EP_Q; W[2] = 0
162 W[3] = EP_Q; W[4] = 0; W[5] = 0
163 W[6] = 0; W[7] = 0; W[8] = EP_Q
164 let Wt: *i64 = sys_mmap(128) as *i64
165 ep_transpose(W, Wt)
166 // R1 = U W V^T ; R2 = U W^T V^T
167 let Vt: *i64 = sys_mmap(128) as *i64
168 ep_transpose(V, Vt)
169 let WV: *i64 = sys_mmap(128) as *i64
170 ep_matmul(W, Vt, WV)
171 ep_matmul(U, WV, R1)
172 ep_matmul(Wt, Vt, WV)
173 ep_matmul(U, WV, R2)
174 return 0
175}
176
177// Frobenius distance^2 (sum sq entry diff) between two 3x3 Q14 matrices
178func ep_frob(a: *i64, b: *i64) -> i64 {
179 var s: i64 = 0
180 var i: i64 = 0
181 while i < 9 { let d: i64 = (a[i] - b[i]) / 64; s = s + d * d; i = i + 1 } // /64 to avoid overflow
182 return s
183}
184
185// R^T * vec (Q14) -> out3
186func ep_mvT(R: *i64, x: i64, y: i64, z: i64, out3: *i64) -> i64 {
187 out3[0] = (R[0] * x + R[3] * y + R[6] * z) / EP_Q
188 out3[1] = (R[1] * x + R[4] * y + R[7] * z) / EP_Q
189 out3[2] = (R[2] * x + R[5] * y + R[8] * z) / EP_Q
190 return 0
191}
192
193// two-view triangulate a normalized correspondence (x1,y1)<->(x2,y2) with cam1=[I|0], cam2=[R|t_unit] (world =
194// cam1 frame). outX = 3D point (world, unit-baseline scale). Uses recon3d midpoint of the two world-frame rays.
195func ep_tv_tri(R: *i64, t: *i64, x1: i64, y1: i64, x2: i64, y2: i64, outX: *i64) -> i64 {
196 let d1: *i64 = sys_mmap(32) as *i64
197 r3_normalize(x1, y1, EP_Q, d1) // cam1 ray dir (world)
198 let rt: *i64 = sys_mmap(32) as *i64
199 ep_mvT(R, t[0], t[1], t[2], rt) // R^T t
200 let c2x: i64 = 0 - rt[0]
201 let c2y: i64 = 0 - rt[1]
202 let c2z: i64 = 0 - rt[2] // cam2 center = -R^T t
203 let d2r: *i64 = sys_mmap(32) as *i64
204 ep_mvT(R, x2, y2, EP_Q, d2r) // R^T (x2,y2,1) = cam2 ray in world
205 let d2: *i64 = sys_mmap(32) as *i64
206 r3_normalize(d2r[0], d2r[1], d2r[2], d2)
207 r3_midpoint(0, 0, 0, d1[0], d1[1], d1[2], c2x, c2y, c2z, d2[0], d2[1], d2[2], outX)
208 return 0
209}
210// depth of world point X in cam2 = (R X + t).z
211func ep_depth2(R: *i64, t: *i64, X: *i64) -> i64 {
212 return (R[6] * X[0] + R[7] * X[1] + R[8] * X[2]) / EP_Q + t[2]
213}
214// cheirality count: # correspondences (corr stride4 [x1,y1,x2,y2]) whose triangulated point is in FRONT of BOTH
215// cameras (positive depth). The physically-correct (R,t) maximizes this.
216func ep_cheir(R: *i64, t: *i64, corr: *i64, n: i64) -> i64 {
217 let X: *i64 = sys_mmap(32) as *i64
218 var cnt: i64 = 0
219 var i: i64 = 0
220 while i < n {
221 ep_tv_tri(R, t, corr[i * 4], corr[i * 4 + 1], corr[i * 4 + 2], corr[i * 4 + 3], X)
222 if X[2] > 0 { if ep_depth2(R, t, X) > 0 { cnt = cnt + 1 } }
223 i = i + 1
224 }
225 return cnt
226}
227// pick the physically-correct pose from the 4 candidates {R1,R2}x{+t,-t} by max cheirality. out_R,out_t filled.
228func ep_select(R1: *i64, R2: *i64, tin: *i64, corr: *i64, n: i64, out_R: *i64, out_t: *i64) -> i64 {
229 let tneg: *i64 = sys_mmap(32) as *i64
230 tneg[0] = 0 - tin[0]; tneg[1] = 0 - tin[1]; tneg[2] = 0 - tin[2]
231 let c1: i64 = ep_cheir(R1, tin, corr, n)
232 let c2: i64 = ep_cheir(R1, tneg, corr, n)
233 let c3: i64 = ep_cheir(R2, tin, corr, n)
234 let c4: i64 = ep_cheir(R2, tneg, corr, n)
235 var best: i64 = c1
236 var bi: i64 = 0
237 if c2 > best { best = c2; bi = 1 }
238 if c3 > best { best = c3; bi = 2 }
239 if c4 > best { best = c4; bi = 3 }
240 var i: i64 = 0
241 if bi == 0 { while i < 9 { out_R[i] = R1[i]; i = i + 1 } out_t[0] = tin[0]; out_t[1] = tin[1]; out_t[2] = tin[2] }
242 if bi == 1 { while i < 9 { out_R[i] = R1[i]; i = i + 1 } out_t[0] = tneg[0]; out_t[1] = tneg[1]; out_t[2] = tneg[2] }
243 if bi == 2 { while i < 9 { out_R[i] = R2[i]; i = i + 1 } out_t[0] = tin[0]; out_t[1] = tin[1]; out_t[2] = tin[2] }
244 if bi == 3 { while i < 9 { out_R[i] = R2[i]; i = i + 1 } out_t[0] = tneg[0]; out_t[1] = tneg[1]; out_t[2] = tneg[2] }
245 return best
246}