nx_selfcalib.nx source
↩ module page · 209 lines · 7061 B
1// nx_selfcalib.nx -- Recovers focal length and 7-DOF pose using Gauss-Newton optimization from correspondence data.
2// nx_selfcalib.nx -- recover the FOCAL LENGTH along with the pose: 7-DOF Gauss-Newton.
3//
4// WHY: nx_photoreal_gate priced the remaining real-camera blockers and found that after brightness
5// invariance the next one is WRONG INTRINSICS -- a focal length 10% off costs 172 units of depth error
6// against a 108 tolerance, and NOTHING in the chain recovers from it. A photograph does not come with its
7// focal length; EXIF is often absent, wrong, or in the wrong units. So the calibration must be SOLVED, not
8// assumed, and it is solved from the same correspondences the pose already uses.
9//
10// This extends nx_pnprefine from 6 DOF to 7 by adding focal length as another column of the Jacobian.
11// âš a NEW function rather than a 14th parameter on the old one: nx_cc does not check call arity, so
12// widening an existing signature makes every old call site read a garbage register.
13// license_tier: ORIGINAL
14import "nx_pnprefine.nx"
15
16const SC_DF: i64 = 4 // focal probe step (pixels)
17const SC_SQ: i64 = 1024
18const SC_MAXSTEP: i64 = 8
19const SC_FMIN: i64 = 20 // a focal outside this band is not a camera, it is a diverged solve
20const SC_FMAX: i64 = 4000
21
22// generic n x n Gaussian elimination with partial pivoting; x in SC_SQ fixed point. 0 = singular.
23func sc_solve(A: *i64, b: *i64, x: *i64, n: i64) -> i64 {
24 var col: i64 = 0
25 while col < n {
26 var piv: i64 = col
27 var best: i64 = 0
28 var rr: i64 = col
29 while rr < n {
30 var a: i64 = A[rr * n + col]
31 if a < 0 { a = 0 - a }
32 if a > best {
33 best = a
34 piv = rr
35 }
36 rr = rr + 1
37 }
38 if best == 0 { return 0 }
39 if piv != col {
40 var c1: i64 = 0
41 while c1 < n {
42 let tmp: i64 = A[col * n + c1]
43 A[col * n + c1] = A[piv * n + c1]
44 A[piv * n + c1] = tmp
45 c1 = c1 + 1
46 }
47 let tb: i64 = b[col]
48 b[col] = b[piv]
49 b[piv] = tb
50 }
51 var r2: i64 = col + 1
52 while r2 < n {
53 let m: i64 = (A[r2 * n + col] * SC_SQ) / A[col * n + col]
54 var c2: i64 = col
55 while c2 < n {
56 A[r2 * n + c2] = A[r2 * n + c2] - (m * A[col * n + c2]) / SC_SQ
57 c2 = c2 + 1
58 }
59 b[r2] = b[r2] - (m * b[col]) / SC_SQ
60 r2 = r2 + 1
61 }
62 col = col + 1
63 }
64 var i: i64 = n - 1
65 while i >= 0 {
66 var s: i64 = b[i] * SC_SQ
67 var j: i64 = i + 1
68 while j < n {
69 s = s - A[i * n + j] * x[j]
70 j = j + 1
71 }
72 if A[i * n + i] == 0 { return 0 }
73 x[i] = s / A[i * n + i]
74 i = i - 1
75 }
76 return 1
77}
78// ONE 7-DOF step: 3 translation + 3 rotation + FOCAL. fp[0] carries the focal in/out.
79func sc_step7(cam: *i64, basis: *i64, fp: *i64, p3: *i64, obs: *i64, n: i64, w: *i64) -> i64 {
80 let nr: i64 = n * 2
81 let r0: *i64 = (w[0]) as *i64
82 let rp: *i64 = (w[1]) as *i64
83 let J: *i64 = (w[2]) as *i64
84 let A: *i64 = (w[3]) as *i64
85 let b: *i64 = (w[4]) as *i64
86 let x: *i64 = (w[5]) as *i64
87 let bb: *i64 = (w[6]) as *i64
88 // âš scene-scale probe -- see pnp_probe_for. Floored at PNP_DT, so synthetic scenes are bit-unchanged.
89 let dt: i64 = pnp_probe_auto(cam, p3, n, fp[0])
90 let sse: i64 = pnp_resid(cam[0], cam[1], cam[2], basis, fp[0], p3, obs, n, r0)
91 var j: i64 = 0
92 while j < 7 {
93 var i2: i64 = 0
94 while i2 < 9 {
95 bb[i2] = basis[i2]
96 i2 = i2 + 1
97 }
98 var ax: i64 = cam[0]
99 var ay: i64 = cam[1]
100 var az: i64 = cam[2]
101 var af: i64 = fp[0]
102 if j == 0 { ax = ax + dt }
103 if j == 1 { ay = ay + dt }
104 if j == 2 { az = az + dt }
105 if j == 3 { pnp_rot(bb, PNP_DW, 0, 0) }
106 if j == 4 { pnp_rot(bb, 0, PNP_DW, 0) }
107 if j == 5 { pnp_rot(bb, 0, 0, PNP_DW) }
108 if j == 6 { af = af + SC_DF }
109 let ig: i64 = pnp_resid(ax, ay, az, bb, af, p3, obs, n, rp)
110 var k: i64 = 0
111 while k < nr {
112 J[j * nr + k] = rp[k] - r0[k]
113 k = k + 1
114 }
115 j = j + 1
116 }
117 var a1: i64 = 0
118 while a1 < 7 {
119 var a2: i64 = 0
120 while a2 < 7 {
121 var s: i64 = 0
122 var k2: i64 = 0
123 while k2 < nr {
124 s = s + J[a1 * nr + k2] * J[a2 * nr + k2]
125 k2 = k2 + 1
126 }
127 A[a1 * 7 + a2] = s
128 a2 = a2 + 1
129 }
130 var sb: i64 = 0
131 var k3: i64 = 0
132 while k3 < nr {
133 sb = sb + J[a1 * nr + k3] * r0[k3]
134 k3 = k3 + 1
135 }
136 b[a1] = 0 - sb
137 a1 = a1 + 1
138 }
139 if sc_solve(A, b, x, 7) == 0 { return sse }
140 var q: i64 = 0
141 while q < 7 {
142 let lim: i64 = SC_MAXSTEP * SC_SQ
143 if x[q] > lim { x[q] = lim }
144 if x[q] < 0 - lim { x[q] = 0 - lim }
145 q = q + 1
146 }
147 cam[0] = cam[0] + (x[0] * dt) / SC_SQ
148 cam[1] = cam[1] + (x[1] * dt) / SC_SQ
149 cam[2] = cam[2] + (x[2] * dt) / SC_SQ
150 pnp_rot(basis, (x[3] * PNP_DW) / SC_SQ, (x[4] * PNP_DW) / SC_SQ, (x[5] * PNP_DW) / SC_SQ)
151 var nf: i64 = fp[0] + (x[6] * SC_DF) / SC_SQ
152 // ★CLAMP TO A PHYSICALLY POSSIBLE CAMERA. An unconstrained focal can walk to zero or negative, which
153 // makes every projection meaningless while the residual still "improves" -- a diverged solve that
154 // reports success.
155 if nf < SC_FMIN { nf = SC_FMIN }
156 if nf > SC_FMAX { nf = SC_FMAX }
157 fp[0] = nf
158 return sse
159}
160// iterate, keeping the BEST (pose, focal) seen so refinement can never end worse than it started
161func sc_refine7(cam: *i64, basis: *i64, fp: *i64, p3: *i64, obs: *i64, n: i64, iters: i64, w: *i64) -> i64 {
162 let rtmp: *i64 = (w[0]) as *i64
163 var best: i64 = pnp_resid(cam[0], cam[1], cam[2], basis, fp[0], p3, obs, n, rtmp)
164 let bcam: *i64 = (w[7]) as *i64
165 let bbas: *i64 = (w[8]) as *i64
166 var bf: i64 = fp[0]
167 var i: i64 = 0
168 while i < 3 {
169 bcam[i] = cam[i]
170 i = i + 1
171 }
172 i = 0
173 while i < 9 {
174 bbas[i] = basis[i]
175 i = i + 1
176 }
177 var it: i64 = 0
178 while it < iters {
179 let ig: i64 = sc_step7(cam, basis, fp, p3, obs, n, w)
180 let cur: i64 = pnp_resid(cam[0], cam[1], cam[2], basis, fp[0], p3, obs, n, rtmp)
181 if cur < best {
182 best = cur
183 bf = fp[0]
184 i = 0
185 while i < 3 {
186 bcam[i] = cam[i]
187 i = i + 1
188 }
189 i = 0
190 while i < 9 {
191 bbas[i] = basis[i]
192 i = i + 1
193 }
194 }
195 it = it + 1
196 }
197 i = 0
198 while i < 3 {
199 cam[i] = bcam[i]
200 i = i + 1
201 }
202 i = 0
203 while i < 9 {
204 basis[i] = bbas[i]
205 i = i + 1
206 }
207 fp[0] = bf
208 return best
209}