`nx_pnprefine.nx` source

↩ module page · 302 lines · 12301 B
1// nx_pnprefine.nx -- Refines camera pose by minimizing reprojection error with a Gauss-Newton linear solve.
2﻿// nx_pnprefine.nx -- GAUSS-NEWTON pose refinement (PnP): given 3D points and their observed 2D projections,
3// refine the camera pose by minimising REPROJECTION error with a real 6x6 linear solve.
4//
5// WHY THIS EXISTS: nx_bundleadjust refines pose by COORDINATE DESCENT over an epipolar cost and is honestly
6// documented as PLATEAUING above machine precision (cost 1270 vs a floor of 67; rotation only ~29% recovered)
7// -- "full metric convergence needs coupled Gauss-Newton/LM (Jacobians + linear solve)" was named as the next
8// rung there. This is that rung. Two structural differences from the coordinate-descent attempt:
9//   (1) the cost is REPROJECTION against points held FIXED, not re-triangulated ones. Re-triangulating inside
10//       the cost lets the points chase any pose, which is degenerate -- every pose scores well.
11//   (2) all six degrees of freedom move TOGETHER from one normal-equation solve, so rotation/translation
12//       trade-offs that a per-axis search cannot see are resolved in a single step.
13// Jacobians are NUMERIC (forward differences at a fixed probe step) -- the projection already exists and is
14// exact, so a derived analytic Jacobian would add a second thing to get wrong for no accuracy gain.
15// All integer/fixed-point, so the refinement is bit-reproducible. license_tier: ORIGINAL
16import "nx_recon3d.nx"
17
18const PNP_DT: i64 = 32             // translation probe step, coord units
19const PNP_DW: i64 = 96             // rotation probe step, Q14 radians (~0.0059 rad)
20const PNP_SQ: i64 = 1024           // fixed-point scale for the linear solve
21const PNP_MAXSTEP: i64 = 8         // clamp a single update to 8 probe steps (a runaway solve cannot bolt)
22// ★image displacement a translation probe should produce, in pixels. This is a TWO-SIDED tradeoff and it was
23// MEASURED, not derived: too small and the forward difference drowns in the integer quantisation of the
24// residuals; too large and the linearisation the Jacobian assumes stops holding. On the real as1 bracket with
25// a wide lens, 8 px gave k1 710 permille out while 4 px gave 6 permille -- so bigger is NOT safer.
26const PNP_TARGET_PX: i64 = 4
27
28// âš âš SCENE-SCALE PROBE. PNP_DT is an ABSOLUTE step in world units, chosen when every scene in this lane sat
29// ~1000-3000 units from the camera. Aim the same solver at a real part in real millimetres 48723 units away
30// and a 32-unit camera nudge moves the projection by f*32/48723 -- UNDER ONE PIXEL. The forward-difference
31// column then quantises to 0s and 1s, the normal equations go ill-conditioned, and the solve STOPS CONVERGING
32// WHILE STILL RETURNING A PLAUSIBLE-LOOKING ANSWER. Measured on the real as1 bracket: SSE 3919 -> 2901 and
33// k1 451 -> 152 permille from this one change (nx_realcal_gate).
34// â˜…THE FLOOR IS WHAT MAKES THIS SAFE TO APPLY EVERYWHERE: at 1000-3000 units dist/100 is 10-30, below
35// PNP_DT, so every existing synthetic scene keeps EXACTLY the old probe of 32 and every existing gate result
36// is bit-identical. Only genuinely large scenes change, and there the old behaviour was broken.
37// â˜…â˜…THE RULE IS EXPRESSED IN PIXELS, NOT IN WORLD UNITS, because pixels are what the integer residuals are
38// quantised in -- that is the quantity a numeric Jacobian actually needs. A probe moves the image by
39// f*dt/dist, so solving for a target displacement gives dt = dist*TARGET/f.
40// âš FIRST ATTEMPT AT THIS FIX USED dist/100 AND WAS WRONG -- not because it failed at real scale, but because
41// it ALSO engaged at ~3200 units where the old probe was producing a perfectly healthy 12 px of motion. It
42// silently changed four already-correct results (selfcalib far 1536/SSE 63 -> 1555/SSE 77, and three multical
43// probes). A FIX MUST BE SCOPED TO THE REGIME THAT IS ACTUALLY BROKEN; widening it further is not "more
44// safety", it is an unrequested change to working behaviour. With the pixel rule, dist 1000-3000 at f=1200
45// yields 6-20, below PNP_DT, so every existing scene keeps EXACTLY the old probe of 32.
46func pnp_probe_px(dist: i64, f: i64) -> i64 {
  if f <= 0 { return PNP_DT }
  var dt: i64 = (dist * PNP_TARGET_PX) / f
  if dt < PNP_DT { dt = PNP_DT }
  return dt
51}
52// working distance = camera centre to the centroid of the points it is looking at
53func pnp_dist_to(cam: *i64, p3: *i64, n: i64) -> i64 {
  if n <= 0 { return 0 }
  var sx: i64 = 0
  var sy: i64 = 0
  var sz: i64 = 0
  var i: i64 = 0
  while i < n {
      sx = sx + p3[i * 3 + 0]
      sy = sy + p3[i * 3 + 1]
      sz = sz + p3[i * 3 + 2]
      i = i + 1
  }
  let dx: i64 = sx / n - cam[0]
  let dy: i64 = sy / n - cam[1]
  let dz: i64 = sz / n - cam[2]
  return r3_isqrt(dx * dx + dy * dy + dz * dz)
69}
70// derive the probe from the data itself, so a caller cannot forget to size it
71func pnp_probe_auto(cam: *i64, p3: *i64, n: i64, f: i64) -> i64 {
  return pnp_probe_px(pnp_dist_to(cam, p3, n), f)
73}
74// residuals r[2k],r[2k+1] = projected - observed, for all n points. Returns the sum of squares.
75func pnp_resid(cx: i64, cy: i64, cz: i64, basis: *i64, f: i64, p3: *i64, obs: *i64, n: i64, r: *i64) -> i64 {
  let o2: *i64 = sys_mmap(32) as *i64
  var k: i64 = 0
  var sse: i64 = 0
  while k < n {
      let zc: i64 = r3_project(cx, cy, cz, basis, 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 {
          du = o2[0] - obs[k * 2 + 0]
          dv = o2[1] - obs[k * 2 + 1]
      }
      r[k * 2 + 0] = du
      r[k * 2 + 1] = dv
      sse = sse + du * du + dv * dv
      k = k + 1
  }
  return sse
93}
94// apply a small world-space rotation w (Q14) to the camera basis rows, then RE-ORTHONORMALISE. Without the
95// re-orthonormalisation the small-angle update slowly shears the basis and it stops being a rotation.
96func pnp_rot(basis: *i64, wx: i64, wy: i64, wz: i64) -> i64 {
  var i: i64 = 0
  while i < 3 {
      let rx: i64 = basis[i * 3 + 0]
      let ry: i64 = basis[i * 3 + 1]
      let rz: i64 = basis[i * 3 + 2]
      basis[i * 3 + 0] = rx + (wy * rz - wz * ry) / R3_Q
      basis[i * 3 + 1] = ry + (wz * rx - wx * rz) / R3_Q
      basis[i * 3 + 2] = rz + (wx * ry - wy * rx) / R3_Q
      i = i + 1
  }
  let t3: *i64 = sys_mmap(32) as *i64
  // forward := normalize(forward); right := normalize(up x forward); up := forward x right
  r3_normalize(basis[6], basis[7], basis[8], t3)
  basis[6] = t3[0]
  basis[7] = t3[1]
  basis[8] = t3[2]
  let c3: *i64 = sys_mmap(32) as *i64
  r3_cross(basis[3], basis[4], basis[5], basis[6], basis[7], basis[8], c3)
  r3_normalize(c3[0], c3[1], c3[2], t3)
  basis[0] = t3[0]
  basis[1] = t3[1]
  basis[2] = t3[2]
  r3_cross(basis[6], basis[7], basis[8], basis[0], basis[1], basis[2], c3)
  r3_normalize(c3[0], c3[1], c3[2], t3)
  basis[3] = t3[0]
  basis[4] = t3[1]
  basis[5] = t3[2]
  return 0
125}
126// 6x6 Gaussian elimination with partial pivoting; x returned in PNP_SQ fixed point. 0 = singular.
127func pnp_solve6(A: *i64, b: *i64, x: *i64) -> i64 {
  var col: i64 = 0
  while col < 6 {
      var piv: i64 = col
      var best: i64 = 0
      var rr: i64 = col
      while rr < 6 {
          var a: i64 = A[rr * 6 + col]
          if a < 0 { a = 0 - a }
          if a > best {
              best = a
              piv = rr
          }
          rr = rr + 1
      }
      if best == 0 { return 0 }
      if piv != col {
          var c1: i64 = 0
          while c1 < 6 {
              let tmp: i64 = A[col * 6 + c1]
              A[col * 6 + c1] = A[piv * 6 + c1]
              A[piv * 6 + c1] = tmp
              c1 = c1 + 1
          }
          let tb: i64 = b[col]
          b[col] = b[piv]
          b[piv] = tb
      }
      var r2: i64 = col + 1
      while r2 < 6 {
          let m: i64 = (A[r2 * 6 + col] * PNP_SQ) / A[col * 6 + col]
          var c2: i64 = col
          while c2 < 6 {
              A[r2 * 6 + c2] = A[r2 * 6 + c2] - (m * A[col * 6 + c2]) / PNP_SQ
              c2 = c2 + 1
          }
          b[r2] = b[r2] - (m * b[col]) / PNP_SQ
          r2 = r2 + 1
      }
      col = col + 1
  }
  var i: i64 = 5
  while i >= 0 {
      var s: i64 = b[i] * PNP_SQ
      var j: i64 = i + 1
      while j < 6 {
          s = s - A[i * 6 + j] * x[j]
          j = j + 1
      }
      if A[i * 6 + i] == 0 { return 0 }
      x[i] = s / A[i * 6 + i]
      i = i - 1
  }
  return 1
181}
182// ONE Gauss-Newton step. cam[0..2] = centre, basis = 3x3. Returns the sum-of-squares BEFORE the step.
183func pnp_step(cam: *i64, basis: *i64, f: i64, p3: *i64, obs: *i64, n: i64, w: *i64) -> i64 {
  let nr: i64 = n * 2
  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 dt: i64 = pnp_probe_auto(cam, p3, n, f)
  let sse: i64 = pnp_resid(cam[0], cam[1], cam[2], basis, f, p3, obs, n, r0)
  var j: i64 = 0
  while j < 6 {
      var i2: i64 = 0
      while i2 < 9 {
          bb[i2] = basis[i2]
          i2 = i2 + 1
      }
      var ax: i64 = cam[0]
      var ay: i64 = cam[1]
      var az: i64 = cam[2]
      if j == 0 { ax = ax + dt }
      if j == 1 { ay = ay + dt }
      if j == 2 { az = az + dt }
      if j == 3 { pnp_rot(bb, PNP_DW, 0, 0) }
      if j == 4 { pnp_rot(bb, 0, PNP_DW, 0) }
      if j == 5 { pnp_rot(bb, 0, 0, PNP_DW) }
      let ig: i64 = pnp_resid(ax, ay, az, bb, f, p3, obs, n, rp)
      var k: i64 = 0
      while k < nr {
          J[j * nr + k] = rp[k] - r0[k]
          k = k + 1
      }
      j = j + 1
  }
  // normal equations: A = J^T J, b = -J^T r0
  var a1: i64 = 0
  while a1 < 6 {
      var a2: i64 = 0
      while a2 < 6 {
          var s: i64 = 0
          var k2: i64 = 0
          while k2 < nr {
              s = s + J[a1 * nr + k2] * J[a2 * nr + k2]
              k2 = k2 + 1
          }
          A[a1 * 6 + a2] = s
          a2 = a2 + 1
      }
      var sb: i64 = 0
      var k3: i64 = 0
      while k3 < nr {
          sb = sb + J[a1 * nr + k3] * r0[k3]
          k3 = k3 + 1
      }
      b[a1] = 0 - sb
      a1 = a1 + 1
  }
  if pnp_solve6(A, b, x) == 0 { return sse }
  // clamp, then apply: translation directly, rotation through the small-angle update
  var q: i64 = 0
  while q < 6 {
      let lim: i64 = PNP_MAXSTEP * PNP_SQ
      if x[q] > lim { x[q] = lim }
      if x[q] < 0 - lim { x[q] = 0 - lim }
      q = q + 1
  }
  cam[0] = cam[0] + (x[0] * dt) / PNP_SQ
  cam[1] = cam[1] + (x[1] * dt) / PNP_SQ
  cam[2] = cam[2] + (x[2] * dt) / PNP_SQ
  pnp_rot(basis, (x[3] * PNP_DW) / PNP_SQ, (x[4] * PNP_DW) / PNP_SQ, (x[5] * PNP_DW) / PNP_SQ)
  return sse
255}
256// iterate; keeps the BEST pose seen so the refinement can never end worse than it started
257func pnp_refine(cam: *i64, basis: *i64, f: i64, p3: *i64, obs: *i64, n: i64, iters: i64, w: *i64) -> i64 {
  let rtmp: *i64 = (w[0]) as *i64
  var best: i64 = pnp_resid(cam[0], cam[1], cam[2], basis, f, p3, obs, n, rtmp)
  let bcam: *i64 = (w[7]) as *i64
  let bbas: *i64 = (w[8]) as *i64
  var i: i64 = 0
  while i < 3 {
      bcam[i] = cam[i]
      i = i + 1
  }
  i = 0
  while i < 9 {
      bbas[i] = basis[i]
      i = i + 1
  }
  var it: i64 = 0
  while it < iters {
      let ig: i64 = pnp_step(cam, basis, f, p3, obs, n, w)
      let cur: i64 = pnp_resid(cam[0], cam[1], cam[2], basis, f, p3, obs, n, rtmp)
      if cur < best {
          best = cur
          i = 0
          while i < 3 {
              bcam[i] = cam[i]
              i = i + 1
          }
          i = 0
          while i < 9 {
              bbas[i] = basis[i]
              i = i + 1
          }
      }
      it = it + 1
  }
  i = 0
  while i < 3 {
      cam[i] = bcam[i]
      i = i + 1
  }
  i = 0
  while i < 9 {
      basis[i] = bbas[i]
      i = i + 1
  }
  return best
302}
nx_pnprefine.nx source

`nx_pnprefine.nx` source
