`nx_distcal.nx` source

↩ module page · 233 lines · 9209 B
1// nx_distcal.nx -- Solves distortion by jointly optimizing pose, focal length, and radial lens distortion coefficient using Gauss-Newton.
2﻿// nx_distcal.nx -- SOLVE THE DISTORTION, do not assume it: joint pose + focal + k1 Gauss-Newton.
3//
4// WHY: nx_lensdist models radial distortion, inverts it and corrects for it -- but every one of those calls
5// takes k1 as a KNOWN input. A photograph does not come with its distortion coefficient any more than it
6// comes with its focal length, so the whole correction chain currently presumes a calibration nobody has
7// performed. This adds k1 as one more column of the same joint Jacobian that already carries pose and focal.
8//
9// Parameter vector: 6*nv poses + focal + k1 = 6*nv + 2.
10// âš the k1 probe step must be LARGE ENOUGH TO MOVE WHOLE PIXELS. The residuals are integer; a probe that
11// displaces the image by a fraction of a pixel produces an all-but-zero Jacobian column, and the solve then
12// reports that k1 has no effect -- an instrument artefact that looks exactly like a genuine degeneracy.
13// license_tier: ORIGINAL
14import "nx_multical.nx"
15import "nx_lensdist.nx"
16
17const DC_DK: i64 = 512             // k1 probe step in Q14 (~0.031) -> ~3 px at the frame corner
18const DC_KMIN: i64 = 0 - 8192      // |k1| > 0.5 is not a lens, it is a diverged solve
19const DC_KMAX: i64 = 8192
20const DC_SQ: i64 = 1024
21const DC_MAXSTEP: i64 = 8
22
23// residuals for all views: project through the pinhole, THEN push through the lens, then compare to what the
24// camera actually recorded. This is the forward model of a real camera, in the order a real camera applies it.
25func dc_resid(cams: *i64, bases: *i64, f: i64, k1: i64, rref: i64, p3: *i64, obs: *i64, np: i64, nv: i64, r: *i64) -> i64 {
  let o2: *i64 = sys_mmap(32) as *i64
  let d2: *i64 = sys_mmap(32) as *i64
  var sse: i64 = 0
  var v: i64 = 0
  while v < nv {
      let bs: *i64 = ((bases as i64) + v * 72) as *i64
      var k: i64 = 0
      while k < np {
          let zc: i64 = r3_project(cams[v * 3 + 0], cams[v * 3 + 1], cams[v * 3 + 2], bs, f, p3[k * 3 + 0], p3[k * 3 + 1], p3[k * 3 + 2], o2)
          var du: i64 = 0
          var dv: i64 = 0
          if zc > 0 {
              let ig: i64 = ld_distort(o2[0], o2[1], k1, 0, rref, d2)
              du = d2[0] - obs[(v * np + k) * 2 + 0]
              dv = d2[1] - obs[(v * np + k) * 2 + 1]
          }
          r[(v * np + k) * 2 + 0] = du
          r[(v * np + k) * 2 + 1] = dv
          sse = sse + du * du + dv * dv
          k = k + 1
      }
      v = v + 1
  }
  return sse
50}
51// ONE joint step over 6*nv + 2 parameters. fp[0] = focal in/out, kp[0] = k1 in/out.
52// âš âš THE TRANSLATION PROBE MUST BE SCALED TO THE SCENE. PNP_DT is an absolute constant (32 units) chosen for a
53// scene ~1000 units away. Point a camera at a real part in real millimetres, 48000 units back, and a 32-unit
54// nudge moves the projection by UNDER ONE PIXEL -- so the forward-difference column is integer-quantised to
55// 0s and 1s, the normal equations go ill-conditioned, and the solve silently stops converging while still
56// reporting a plausible-looking answer. dc_step now sizes the probe AUTOMATICALLY from the data (see
57// pnp_probe_px); dc_step_s still takes it explicitly for a caller that wants to control it.
58func dc_step(cams: *i64, bases: *i64, fp: *i64, kp: *i64, rref: i64, p3: *i64, obs: *i64, np: i64, nv: i64, w: *i64) -> i64 {
  return dc_step_s(cams, bases, fp, kp, rref, p3, obs, np, nv, w, pnp_probe_auto(cams, p3, np, fp[0]))
60}
61// kept as the named entry point for callers that know their working distance; the sizing rule itself lives
62// ONCE, in nx_pnprefine, so the three solvers cannot drift apart on it.
63func dc_probe_for(dist: i64, f: i64) -> i64 {
  return pnp_probe_px(dist, f)
65}
66func dc_step_s(cams: *i64, bases: *i64, fp: *i64, kp: *i64, rref: i64, p3: *i64, obs: *i64, np: i64, nv: i64, w: *i64, dt: i64) -> i64 {
  let npar: i64 = 6 * nv + 2
  let nres: i64 = 2 * np * nv
  let r0: *i64 = (w[0]) as *i64
  let rp: *i64 = (w[1]) as *i64
  let J: *i64 = (w[2]) as *i64
  let A: *i64 = (w[3]) as *i64
  let b: *i64 = (w[4]) as *i64
  let x: *i64 = (w[5]) as *i64
  let bb: *i64 = (w[6]) as *i64
  let sse: i64 = dc_resid(cams, bases, fp[0], kp[0], rref, p3, obs, np, nv, r0)
77
  var j: i64 = 0
  while j < npar {
      var af: i64 = fp[0]
      var ak: i64 = kp[0]
      var vv: i64 = 0
      var sub: i64 = 0
      var kind: i64 = 0                       // 0 = pose, 1 = focal, 2 = k1
      if j == npar - 2 { kind = 1 }
      if j == npar - 1 { kind = 2 }
      if kind == 1 { af = af + MC_DF }
      if kind == 2 { ak = ak + DC_DK }
      if kind == 0 {
          vv = j / 6
          sub = j % 6
      }
      var saved: i64 = 0
      if kind == 0 {
          if sub < 3 {
              saved = cams[vv * 3 + sub]
              cams[vv * 3 + sub] = saved + dt
          }
          if sub >= 3 {
              var i2: i64 = 0
              while i2 < 9 {
                  bb[i2] = bases[vv * 9 + i2]
                  i2 = i2 + 1
              }
              let bs: *i64 = ((bases as i64) + vv * 72) as *i64
              if sub == 3 { pnp_rot(bs, PNP_DW, 0, 0) }
              if sub == 4 { pnp_rot(bs, 0, PNP_DW, 0) }
              if sub == 5 { pnp_rot(bs, 0, 0, PNP_DW) }
          }
      }
      let ig: i64 = dc_resid(cams, bases, af, ak, rref, p3, obs, np, nv, rp)
      if kind == 0 {
          if sub < 3 { cams[vv * 3 + sub] = saved }
          if sub >= 3 {
              var i3: i64 = 0
              while i3 < 9 {
                  bases[vv * 9 + i3] = bb[i3]
                  i3 = i3 + 1
              }
          }
      }
      var kk: i64 = 0
      while kk < nres {
          J[j * nres + kk] = rp[kk] - r0[kk]
          kk = kk + 1
      }
      j = j + 1
  }
  var a1: i64 = 0
  while a1 < npar {
      var a2: i64 = 0
      while a2 < npar {
          var s: i64 = 0
          var k2: i64 = 0
          while k2 < nres {
              s = s + J[a1 * nres + k2] * J[a2 * nres + k2]
              k2 = k2 + 1
          }
          A[a1 * npar + a2] = s
          a2 = a2 + 1
      }
      var sb: i64 = 0
      var k3: i64 = 0
      while k3 < nres {
          sb = sb + J[a1 * nres + k3] * r0[k3]
          k3 = k3 + 1
      }
      b[a1] = 0 - sb
      a1 = a1 + 1
  }
  if sc_solve(A, b, x, npar) == 0 { return sse }
  var q: i64 = 0
  while q < npar {
      let lim: i64 = DC_MAXSTEP * DC_SQ
      if x[q] > lim { x[q] = lim }
      if x[q] < 0 - lim { x[q] = 0 - lim }
      q = q + 1
  }
  var v2: i64 = 0
  while v2 < nv {
      cams[v2 * 3 + 0] = cams[v2 * 3 + 0] + (x[v2 * 6 + 0] * dt) / DC_SQ
      cams[v2 * 3 + 1] = cams[v2 * 3 + 1] + (x[v2 * 6 + 1] * dt) / DC_SQ
      cams[v2 * 3 + 2] = cams[v2 * 3 + 2] + (x[v2 * 6 + 2] * dt) / DC_SQ
      let bs2: *i64 = ((bases as i64) + v2 * 72) as *i64
      pnp_rot(bs2, (x[v2 * 6 + 3] * PNP_DW) / DC_SQ, (x[v2 * 6 + 4] * PNP_DW) / DC_SQ, (x[v2 * 6 + 5] * PNP_DW) / DC_SQ)
      v2 = v2 + 1
  }
  var nf: i64 = fp[0] + (x[npar - 2] * MC_DF) / DC_SQ
  if nf < MC_FMIN { nf = MC_FMIN }
  if nf > MC_FMAX { nf = MC_FMAX }
  fp[0] = nf
  var nk: i64 = kp[0] + (x[npar - 1] * DC_DK) / DC_SQ
  if nk < DC_KMIN { nk = DC_KMIN }
  if nk > DC_KMAX { nk = DC_KMAX }
  kp[0] = nk
  return sse
177}
178// iterate, keeping the BEST (poses, focal, k1) seen
179func dc_refine(cams: *i64, bases: *i64, fp: *i64, kp: *i64, rref: i64, p3: *i64, obs: *i64, np: i64, nv: i64, iters: i64, w: *i64) -> i64 {
  return dc_refine_s(cams, bases, fp, kp, rref, p3, obs, np, nv, iters, w, pnp_probe_auto(cams, p3, np, fp[0]))
181}
182func dc_refine_s(cams: *i64, bases: *i64, fp: *i64, kp: *i64, rref: i64, p3: *i64, obs: *i64, np: i64, nv: i64, iters: i64, w: *i64, dt: i64) -> i64 {
  let rtmp: *i64 = (w[0]) as *i64
  let bcam: *i64 = (w[7]) as *i64
  let bbas: *i64 = (w[8]) as *i64
  var best: i64 = dc_resid(cams, bases, fp[0], kp[0], rref, p3, obs, np, nv, rtmp)
  var bf: i64 = fp[0]
  var bk: i64 = kp[0]
  var i: i64 = 0
  while i < nv * 3 {
      bcam[i] = cams[i]
      i = i + 1
  }
  i = 0
  while i < nv * 9 {
      bbas[i] = bases[i]
      i = i + 1
  }
  var it: i64 = 0
  while it < iters {
      let ig: i64 = dc_step_s(cams, bases, fp, kp, rref, p3, obs, np, nv, w, dt)
      let cur: i64 = dc_resid(cams, bases, fp[0], kp[0], rref, p3, obs, np, nv, rtmp)
      if cur < best {
          best = cur
          bf = fp[0]
          bk = kp[0]
          i = 0
          while i < nv * 3 {
              bcam[i] = cams[i]
              i = i + 1
          }
          i = 0
          while i < nv * 9 {
              bbas[i] = bases[i]
              i = i + 1
          }
      }
      it = it + 1
  }
  i = 0
  while i < nv * 3 {
      cams[i] = bcam[i]
      i = i + 1
  }
  i = 0
  while i < nv * 9 {
      bases[i] = bbas[i]
      i = i + 1
  }
  fp[0] = bf
  kp[0] = bk
  return best
233}
nx_distcal.nx source

`nx_distcal.nx` source
