nx_lensdist.nx source

1// nx_lensdist.nx -- Models and corrects radial lens distortion in images using fixed-point arithmetic. 2// nx_lensdist.nx -- RADIAL LENS DISTORTION: the model, the inverse, and the image correction. 3// 4// WHY: the cadtwin census listed four real-camera defects as untested -- sensor noise, lighting, unknown 5// intrinsics, and LENS DISTORTION. The first three have since been priced and fixed (nx_photoreal_gate, 6// nx_selfcalib/nx_multical). Distortion is the last one on that list, and it is the one EVERY real lens has: 7// a straight edge photographs as a curve, and a pipeline that assumes a pinhole camera silently measures the 8// curve. Nothing downstream can recover from it, exactly like a wrong focal. 9// 10// Model: Brown-Conrady radial terms evaluated at a NORMALISED radius rn = r / rref, so the coefficients are 11// dimensionless and the same k1/k2 describe the lens at any image size. 12// factor = 1 + k1*rn^2 + k2*rn^4 (all in Q14 fixed point) 13// distorted = ideal * factor 14// âš rref MUST be at least the largest radius in the image (the half-diagonal). The polynomial is only sane for 15// rn <= 1; feed it rn = 5 and the factor goes NEGATIVE and the "lens" folds the image through itself. 16// license_tier: ORIGINAL 17import "nx_planesweep.nx" 18import "nx_vecmath.nx" 19 20const LD_Q: i64 = 16384 // Q14, matching R3_Q so the two mix without rescaling 21const LD_ITERS: i64 = 12 // fixed-point iterations for the inverse 22const LD_SUB: i64 = 256 // sub-pixel resolution carried THROUGH the inverse iteration 23 24// the radial scale factor at normalised radius rn (Q14), returned in Q14 25func ld_factor(rn: i64, k1: i64, k2: i64) -> i64 { 26 let rn2: i64 = (rn * rn) / LD_Q 27 let rn4: i64 = (rn2 * rn2) / LD_Q 28 return LD_Q + (k1 * rn2) / LD_Q + (k2 * rn4) / LD_Q 29} 30// integer square root, for the radius 31func ld_isqrt(v: i64) -> i64 { return vm_isqrt(v) } 32// IDEAL pinhole image coords -> what the lens actually records 33func ld_distort(u: i64, v: i64, k1: i64, k2: i64, rref: i64, out2: *i64) -> i64 { 34 if rref <= 0 { return 0 } 35 let r: i64 = ld_isqrt(u * u + v * v) 36 let rn: i64 = (r * LD_Q) / rref 37 let fac: i64 = ld_factor(rn, k1, k2) 38 out2[0] = (u * fac) / LD_Q 39 out2[1] = (v * fac) / LD_Q 40 return fac 41} 42// what the lens recorded -> IDEAL pinhole coords. No closed form exists for the radial polynomial, so this 43// is the standard fixed-point iteration u <- ud / factor(|u|), started at the distorted point. 44// ★It converges because barrel distortion is a CONTRACTION near the centre; it is NOT valid for arbitrarily 45// large k1, so the round-trip is asserted in the gate rather than assumed. 46// âš THE ITERATION MUST CARRY SUB-PIXEL STATE. Iterating in whole pixels re-quantises the estimate on EVERY 47// pass, and because each pass divides by a factor < 1 the truncation is amplified rather than averaged out -- 48// measured at 4 px of round-trip error against a 1 px quantisation floor. Carrying u,v in LD_SUB-ths of a 49// pixel and rounding only at the very end costs nothing and removes it. 50func ld_undistort(ud: i64, vd: i64, k1: i64, k2: i64, rref: i64, out2: *i64) -> i64 { 51 if rref <= 0 { return 0 } 52 let udf: i64 = ud * LD_SUB 53 let vdf: i64 = vd * LD_SUB 54 var u: i64 = udf 55 var v: i64 = vdf 56 var it: i64 = 0 57 while it < LD_ITERS { 58 let r: i64 = ld_isqrt(u * u + v * v) 59 let rn: i64 = (r * LD_Q) / (rref * LD_SUB) 60 let fac: i64 = ld_factor(rn, k1, k2) 61 if fac == 0 { return 0 } 62 u = (udf * LD_Q) / fac 63 v = (vdf * LD_Q) / fac 64 it = it + 1 65 } 66 out2[0] = ld_round(u) 67 out2[1] = ld_round(v) 68 return 1 69} 70// round a LD_SUB fixed-point value to the nearest whole pixel, away from zero on a tie 71func ld_round(fx: i64) -> i64 { 72 if fx >= 0 { return (fx + LD_SUB / 2) / LD_SUB } 73 return (fx - LD_SUB / 2) / LD_SUB 74} 75// render the textured plane AS A DISTORTED CAMERA RECORDS IT. For each recorded pixel, recover the ideal 76// image coords it corresponds to, and cast the ray through those -- which is exactly what a real lens does. 77func ld_render(cx: i64, cy: i64, cz: i64, basis: *i64, f: i64, zp: i64, w: i64, h: i64, k1: i64, k2: i64, rref: i64, img: *i64) -> i64 { 78 let d3: *i64 = sys_mmap(32) as *i64 79 let id2: *i64 = sys_mmap(32) as *i64 80 var py: i64 = 0 81 while py < h { 82 var px: i64 = 0 83 while px < w { 84 let ud: i64 = px - w / 2 85 let vd: i64 = py - h / 2 86 let ok: i64 = ld_undistort(ud, vd, k1, k2, rref, id2) 87 var val: i64 = 0 88 if ok == 1 { 89 r3_ray(basis, f, id2[0], id2[1], d3) 90 if d3[2] != 0 { 91 let s: i64 = ((zp - cz) * R3_Q) / d3[2] 92 if s > 0 { 93 let wx: i64 = cx + (d3[0] * s) / R3_Q 94 let wy: i64 = cy + (d3[1] * s) / R3_Q 95 val = ps_tex(wx, wy) 96 } 97 } 98 } 99 img[py * w + px] = val 100 px = px + 1 101 } 102 py = py + 1 103 } 104 return 0 105} 106// forward distortion at SUB-PIXEL resolution: same model as ld_distort, but the answer is returned in 107// LD_SUB-ths of a pixel so a resampler can interpolate instead of snapping to the nearest source pixel. 108func ld_distort_fx(u: i64, v: i64, k1: i64, k2: i64, rref: i64, out2: *i64) -> i64 { 109 if rref <= 0 { return 0 } 110 let r: i64 = ld_isqrt(u * u + v * v) 111 let rn: i64 = (r * LD_Q) / rref 112 let fac: i64 = ld_factor(rn, k1, k2) 113 out2[0] = (u * fac * LD_SUB) / LD_Q 114 out2[1] = (v * fac * LD_SUB) / LD_Q 115 return fac 116} 117// floor division -- âš integer division TRUNCATES TOWARD ZERO, so a source coordinate left of centre would 118// round the wrong way and the interpolation weights would come out mirrored on one half of the image. 119func ld_floordiv(a: i64, b: i64) -> i64 { 120 var q: i64 = a / b 121 if a % b != 0 { 122 if a < 0 { q = q - 1 } 123 } 124 return q 125} 126// bilinear sample at LD_SUB fixed-point coordinates. -1 if any of the four taps falls outside. 127func ld_bilinear(src: *i64, w: i64, h: i64, xfx: i64, yfx: i64) -> i64 { 128 let x0: i64 = ld_floordiv(xfx, LD_SUB) 129 let y0: i64 = ld_floordiv(yfx, LD_SUB) 130 let fx: i64 = xfx - x0 * LD_SUB 131 let fy: i64 = yfx - y0 * LD_SUB 132 let v00: i64 = ps_at(src, w, h, x0, y0) 133 let v10: i64 = ps_at(src, w, h, x0 + 1, y0) 134 let v01: i64 = ps_at(src, w, h, x0, y0 + 1) 135 let v11: i64 = ps_at(src, w, h, x0 + 1, y0 + 1) 136 if v00 < 0 { return 0 - 1 } 137 if v10 < 0 { return 0 - 1 } 138 if v01 < 0 { return 0 - 1 } 139 if v11 < 0 { return 0 - 1 } 140 let top: i64 = v00 * (LD_SUB - fx) + v10 * fx 141 let bot: i64 = v01 * (LD_SUB - fx) + v11 * fx 142 return (top * (LD_SUB - fy) + bot * fy) / (LD_SUB * LD_SUB) 143} 144// THE CORRECTION, BILINEAR. Identical geometry to ld_correct -- only the sampling differs -- so any change in 145// downstream error is attributable to RESAMPLING ALONE and to nothing else. 146func ld_correct_bl(src: *i64, dst: *i64, w: i64, h: i64, k1: i64, k2: i64, rref: i64) -> i64 { 147 let o2: *i64 = sys_mmap(32) as *i64 148 var miss: i64 = 0 149 var py: i64 = 0 150 while py < h { 151 var px: i64 = 0 152 while px < w { 153 let u: i64 = px - w / 2 154 let v: i64 = py - h / 2 155 let ig: i64 = ld_distort_fx(u, v, k1, k2, rref, o2) 156 let sx: i64 = o2[0] + (w / 2) * LD_SUB 157 let sy: i64 = o2[1] + (h / 2) * LD_SUB 158 let val: i64 = ld_bilinear(src, w, h, sx, sy) 159 if val < 0 { 160 dst[py * w + px] = 0 161 miss = miss + 1 162 } 163 if val >= 0 { dst[py * w + px] = val } 164 px = px + 1 165 } 166 py = py + 1 167 } 168 return miss 169} 170// THE CORRECTION. Resample a recorded image into an ideal pinhole one: for every output pixel, ask where the 171// lens PUT that ray (forward distort) and read the source there. Backward mapping, so the output has no holes. 172// Returns the count of pixels that fell outside the source and were written as 0. 173func ld_correct(src: *i64, dst: *i64, w: i64, h: i64, k1: i64, k2: i64, rref: i64) -> i64 { 174 let o2: *i64 = sys_mmap(32) as *i64 175 var miss: i64 = 0 176 var py: i64 = 0 177 while py < h { 178 var px: i64 = 0 179 while px < w { 180 let u: i64 = px - w / 2 181 let v: i64 = py - h / 2 182 let ig: i64 = ld_distort(u, v, k1, k2, rref, o2) 183 let sx: i64 = o2[0] + w / 2 184 let sy: i64 = o2[1] + h / 2 185 let val: i64 = ps_at(src, w, h, sx, sy) 186 if val < 0 { 187 dst[py * w + px] = 0 188 miss = miss + 1 189 } 190 if val >= 0 { dst[py * w + px] = val } 191 px = px + 1 192 } 193 py = py + 1 194 } 195 return miss 196}