`nx_multical.nx` source

↩ module page · 217 lines · 8263 B
1// nx_multical.nx -- Jointly calibrates multiple camera views with a shared focal length and per-view poses using Gauss-Newton optimization.
2﻿// nx_multical.nx -- JOINT multi-view calibration: ONE SHARED focal length plus EVERY camera pose, solved
3// together in a single Gauss-Newton system.
4//
5// WHY: nx_selfcalib measured that a 7-DOF single-view solve reaches the reprojection floor while leaving the
6// focal 28% wrong, and named the remedy in its own closing text -- "a known-geometry target, or
7// AUTO-CALIBRATION ACROSS SEVERAL CAMERA MOTIONS". This is that remedy. The structural difference is not more
8// iterations, it is a SHARED PARAMETER: the same focal must explain every view at once, so the focal/distance
9// trade-off that one view cannot separate has to hold simultaneously against several different camera
10// distances -- and it cannot, unless the focal is right.
11//
12// The parameter vector is 6*nv + 1: six pose degrees of freedom per view, plus the single shared focal as the
13// LAST column of the Jacobian. Sharing is the whole point; a per-view focal would re-create the single-view
14// degeneracy nv times over and fit just as well while meaning nothing.
15// license_tier: ORIGINAL
16import "nx_selfcalib.nx"
17
18const MC_DF: i64 = 4               // focal probe step (pixels)
19const MC_SQ: i64 = 1024
20const MC_MAXSTEP: i64 = 8
21const MC_FMIN: i64 = 20            // outside this band it is not a camera, it is a diverged solve
22const MC_FMAX: i64 = 4000
23const MC_MAXV: i64 = 6             // views; the work buffers below are sized for this
24
25// work buffers. Allocated here rather than demanded from the caller: mc_step needs seven separate arrays
26// whose sizes all depend on nv, and a caller-supplied layout is exactly the kind of silent mismatch nx_cc
27// cannot catch.
28func mc_work() -> *i64 {
  let w: *i64 = sys_mmap(256) as *i64
  w[0] = sys_mmap(65536) as i64      // r0   residuals, 2*np*nv
  w[1] = sys_mmap(65536) as i64      // rp   perturbed residuals
  w[2] = sys_mmap(1048576) as i64    // J    (6*nv+1) x (2*np*nv)
  w[3] = sys_mmap(65536) as i64      // A    (6*nv+1)^2
  w[4] = sys_mmap(4096) as i64       // b
  w[5] = sys_mmap(4096) as i64       // x
  w[6] = sys_mmap(256) as i64        // bb   one saved 3x3 basis
  w[7] = sys_mmap(1024) as i64       // best cams
  w[8] = sys_mmap(2048) as i64       // best bases
  return w
40}
41// residuals for ALL views against ONE shared focal. cams = 3 per view, bases = 9 per view,
42// obs/r = 2 per (view,point) laid out view-major. Returns the summed sum-of-squares.
43func mc_resid(cams: *i64, bases: *i64, f: i64, p3: *i64, obs: *i64, np: i64, nv: i64, r: *i64) -> i64 {
  var sse: i64 = 0
  var v: i64 = 0
  while v < nv {
      let ob: *i64 = ((obs as i64) + v * np * 16) as *i64
      let rr: *i64 = ((r as i64) + v * np * 16) as *i64
      let bs: *i64 = ((bases as i64) + v * 72) as *i64
      sse = sse + pnp_resid(cams[v * 3 + 0], cams[v * 3 + 1], cams[v * 3 + 2], bs, f, p3, ob, np, rr)
      v = v + 1
  }
  return sse
54}
55// ONE joint Gauss-Newton step over 6*nv + 1 parameters. fp[0] carries the shared focal in/out.
56func mc_step(cams: *i64, bases: *i64, fp: *i64, p3: *i64, obs: *i64, np: i64, nv: i64, w: *i64) -> i64 {
  let npar: i64 = 6 * nv + 1
  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
  // âš scene-scale probe -- see pnp_probe_for. `cams` already points at view 0, the representative camera.
  let dt: i64 = pnp_probe_auto(cams, p3, np, fp[0])
  let sse: i64 = mc_resid(cams, bases, fp[0], p3, obs, np, nv, r0)
69
  var j: i64 = 0
  while j < npar {
      var af: i64 = fp[0]
      var vv: i64 = 0
      var sub: i64 = 0
      var isf: i64 = 0
      if j == npar - 1 {
          isf = 1
          af = af + MC_DF
      }
      if isf == 0 {
          vv = j / 6
          sub = j % 6
      }
      // perturb IN PLACE, evaluate, then restore. Translation restores exactly by subtraction;
      // âš rotation does NOT -- pnp_rot re-orthonormalises, so the basis must be restored from a saved copy.
      var saved: i64 = 0
      if isf == 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 = mc_resid(cams, bases, af, p3, obs, np, nv, rp)
      if isf == 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 k: i64 = 0
      while k < nres {
          J[j * nres + k] = rp[k] - r0[k]
          k = k + 1
      }
      j = j + 1
  }
  // normal equations over the joint system
  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 = MC_MAXSTEP * MC_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) / MC_SQ
      cams[v2 * 3 + 1] = cams[v2 * 3 + 1] + (x[v2 * 6 + 1] * dt) / MC_SQ
      cams[v2 * 3 + 2] = cams[v2 * 3 + 2] + (x[v2 * 6 + 2] * dt) / MC_SQ
      let bs2: *i64 = ((bases as i64) + v2 * 72) as *i64
      pnp_rot(bs2, (x[v2 * 6 + 3] * PNP_DW) / MC_SQ, (x[v2 * 6 + 4] * PNP_DW) / MC_SQ, (x[v2 * 6 + 5] * PNP_DW) / MC_SQ)
      v2 = v2 + 1
  }
  var nf: i64 = fp[0] + (x[npar - 1] * MC_DF) / MC_SQ
  if nf < MC_FMIN { nf = MC_FMIN }
  if nf > MC_FMAX { nf = MC_FMAX }
  fp[0] = nf
  return sse
167}
168// iterate, keeping the BEST (all poses, shared focal) seen, so a refinement can never end worse than it began
169func mc_refine(cams: *i64, bases: *i64, fp: *i64, p3: *i64, obs: *i64, np: i64, nv: i64, iters: i64, w: *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 = mc_resid(cams, bases, fp[0], p3, obs, np, nv, rtmp)
  var bf: i64 = fp[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 = mc_step(cams, bases, fp, p3, obs, np, nv, w)
      let cur: i64 = mc_resid(cams, bases, fp[0], p3, obs, np, nv, rtmp)
      if cur < best {
          best = cur
          bf = fp[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
  return best
217}
nx_multical.nx source

`nx_multical.nx` source
