`nx_register3d.nx` source

↩ module page · 252 lines · 11025 B
1// nx_register3d.nx -- 3D RIGID REGISTRATION (Horn's absolute-orientation, quaternion method) -- the "align two
2// 3D scans into one frame" capability (cadtwin: fuse multi-view/multi-scan captures of a car into ONE twin;
3// extends nx_pc's 2D ICP to 3D). Given two corresponding 3D point sets P,Q, recovers the rotation R + translation
4// t s.t. R*P + t ~= Q. SVD-FREE: builds Horn's 4x4 symmetric matrix N from the cross-covariance and finds its
5// dominant eigenvector (the optimal rotation quaternion) by POWER ITERATION on a Gershgorin-shifted N (guarantees
6// convergence to the most-positive eigenvalue). All fixed-point Q14, deterministic. Benchmarked by residual RMS.
7// license_tier: ORIGINAL
8import "nx_syscalls.nx"
9import "nx_vecmath.nx"
10
11const RG_Q: i64 = 16384
12
13func rg_isqrt(n: i64) -> i64 { return vm_isqrt(n) }
14func rg_abs(a: i64) -> i64 { if a < 0 { return 0 - a } return a }
15
16// unit quaternion (Q14) -> 3x3 rotation matrix R (Q14, row-major 9). q = [w,x,y,z].
17func rg_quat_to_r(q: *i64, r: *i64) -> i64 {
  let w: i64 = q[0]
  let x: i64 = q[1]
  let y: i64 = q[2]
  let z: i64 = q[3]
  let xx: i64 = (x * x) / RG_Q
  let yy: i64 = (y * y) / RG_Q
  let zz: i64 = (z * z) / RG_Q
  let xy: i64 = (x * y) / RG_Q
  let xz: i64 = (x * z) / RG_Q
  let yz: i64 = (y * z) / RG_Q
  let wx: i64 = (w * x) / RG_Q
  let wy: i64 = (w * y) / RG_Q
  let wz: i64 = (w * z) / RG_Q
  r[0] = RG_Q - 2 * (yy + zz)
  r[1] = 2 * (xy - wz)
  r[2] = 2 * (xz + wy)
  r[3] = 2 * (xy + wz)
  r[4] = RG_Q - 2 * (xx + zz)
  r[5] = 2 * (yz - wx)
  r[6] = 2 * (xz - wy)
  r[7] = 2 * (yz + wx)
  r[8] = RG_Q - 2 * (xx + yy)
  return 0
41}
42// apply R (Q14) + t (coord) to a coord point -> out3
43func rg_apply(r: *i64, tx: i64, ty: i64, tz: i64, px: i64, py: i64, pz: i64, out3: *i64) -> i64 {
  out3[0] = (r[0] * px + r[1] * py + r[2] * pz) / RG_Q + tx
  out3[1] = (r[3] * px + r[4] * py + r[5] * pz) / RG_Q + ty
  out3[2] = (r[6] * px + r[7] * py + r[8] * pz) / RG_Q + tz
  return 0
48}
49
50// HORN registration. P,Q = coord point arrays stride 3, n points. out_r[9] (Q14), out_t[3] (coord).
51func rg_register(p: *i64, q: *i64, n: i64, out_r: *i64, out_t: *i64) -> i64 {
  // centroids
  var pcx: i64 = 0; var pcy: i64 = 0; var pcz: i64 = 0
  var qcx: i64 = 0; var qcy: i64 = 0; var qcz: i64 = 0
  var i: i64 = 0
  while i < n {
      pcx = pcx + p[i * 3]; pcy = pcy + p[i * 3 + 1]; pcz = pcz + p[i * 3 + 2]
      qcx = qcx + q[i * 3]; qcy = qcy + q[i * 3 + 1]; qcz = qcz + q[i * 3 + 2]
      i = i + 1
  }
  pcx = pcx / n; pcy = pcy / n; pcz = pcz / n
  qcx = qcx / n; qcy = qcy / n; qcz = qcz / n
  // cross-covariance S[a][b] = sum P_a Q_b (centered)
  var sxx: i64 = 0; var sxy: i64 = 0; var sxz: i64 = 0
  var syx: i64 = 0; var syy: i64 = 0; var syz: i64 = 0
  var szx: i64 = 0; var szy: i64 = 0; var szz: i64 = 0
  i = 0
  while i < n {
      let ax: i64 = p[i * 3] - pcx
      let ay: i64 = p[i * 3 + 1] - pcy
      let az: i64 = p[i * 3 + 2] - pcz
      let bx: i64 = q[i * 3] - qcx
      let by: i64 = q[i * 3 + 1] - qcy
      let bz: i64 = q[i * 3 + 2] - qcz
      sxx = sxx + ax * bx; sxy = sxy + ax * by; sxz = sxz + ax * bz
      syx = syx + ay * bx; syy = syy + ay * by; syz = syz + ay * bz
      szx = szx + az * bx; szy = szy + az * by; szz = szz + az * bz
      i = i + 1
  }
  // Horn N (4x4 symmetric, raw scale)
  let nm: *i64 = sys_mmap(16 * 8) as *i64
  nm[0] = sxx + syy + szz
  nm[1] = syz - szy;  nm[4] = nm[1]
  nm[2] = szx - sxz;  nm[8] = nm[2]
  nm[3] = sxy - syx;  nm[12] = nm[3]
  nm[5] = sxx - syy - szz
  nm[6] = sxy + syx;  nm[9] = nm[6]
  nm[7] = szx + sxz;  nm[13] = nm[7]
  nm[10] = 0 - sxx + syy - szz
  nm[11] = syz + szy; nm[14] = nm[11]
  nm[15] = 0 - sxx - syy + szz
  // Gershgorin shift c = max row abs-sum -> N' = N + cI is PSD (dominant eigval = optimal)
  var c: i64 = 0
  var rr: i64 = 0
  while rr < 4 {
      var s: i64 = 0
      var cc: i64 = 0
      while cc < 4 { s = s + rg_abs(nm[rr * 4 + cc]); cc = cc + 1 }
      if s > c { c = s }
      rr = rr + 1
  }
  nm[0] = nm[0] + c; nm[5] = nm[5] + c; nm[10] = nm[10] + c; nm[15] = nm[15] + c
  // scale N to ~Q14 (power iteration cares only about direction; keeps products in range)
  var maxa: i64 = 1
  i = 0
  while i < 16 { let a: i64 = rg_abs(nm[i]); if a > maxa { maxa = a } i = i + 1 }
  let scl: i64 = (maxa / RG_Q) + 1
  i = 0
  while i < 16 { nm[i] = nm[i] / scl; i = i + 1 }
  // power iteration
  let v: *i64 = sys_mmap(32) as *i64
  let w: *i64 = sys_mmap(32) as *i64
  v[0] = RG_Q; v[1] = 0; v[2] = 0; v[3] = 0     // init toward identity quat
  var it: i64 = 0
  while it < 80 {
      var a: i64 = 0
      while a < 4 {
          w[a] = (nm[a * 4] * v[0] + nm[a * 4 + 1] * v[1] + nm[a * 4 + 2] * v[2] + nm[a * 4 + 3] * v[3]) / RG_Q
          a = a + 1
      }
      let len: i64 = rg_isqrt(w[0] * w[0] + w[1] * w[1] + w[2] * w[2] + w[3] * w[3])
      if len == 0 { v[0] = RG_Q; v[1] = 0; v[2] = 0; v[3] = 0 } else {
          v[0] = (w[0] * RG_Q) / len; v[1] = (w[1] * RG_Q) / len; v[2] = (w[2] * RG_Q) / len; v[3] = (w[3] * RG_Q) / len
      }
      it = it + 1
  }
  // convention: keep w>=0 (quaternion double-cover)
  if v[0] < 0 { v[0] = 0 - v[0]; v[1] = 0 - v[1]; v[2] = 0 - v[2]; v[3] = 0 - v[3] }
  rg_quat_to_r(v, out_r)
  // t = qcentroid - R * pcentroid
  let rp: *i64 = sys_mmap(32) as *i64
  rg_apply(out_r, 0, 0, 0, pcx, pcy, pcz, rp)
  out_t[0] = qcx - rp[0]
  out_t[1] = qcy - rp[1]
  out_t[2] = qcz - rp[2]
  return 0
137}
138
139// ============================================================================================
140// ★★★SIMILARITY REGISTRATION -- rigid PLUS SCALE (7 DOF), added 2026-07-27 for the twin loop.
141// rg_register above solves R and t only, which is correct for fusing two scans OF THE SAME OBJECT at
142// the same size -- the cadtwin case it was written for. It is NOT enough to fit a PERSON:
143//   * two bodies with identical proportions at different statures differ by a SCALE, and a rigid fit
144//     cannot express that, so it reports a large residual for a subject who is merely a different size;
145//   * uncalibrated photographs carry NO METRIC SCALE AT ALL, so scale is the one quantity you never
146//     know going in.
147// Horn's own paper includes the scale factor; this implementation had dropped it. ★The symmetric form
148// s = sqrt(Sq/Sp) over the CENTERED clouds is used because it is INDEPENDENT OF THE ROTATION -- so
149// scale can be solved without trusting R, and a bad R cannot quietly corrupt s.
150// ⚠SCALE IS RECOVERED, NOT MEASURED. It tells you the ratio between two point sets. If one of them
151// came from an uncalibrated image, the resulting metric size is only as good as whatever real-world
152// length was used to anchor it -- that anchor is an ASSUMPTION and must be reported as one.
153// ⚠BOUNDS: Sp,Sq are sums of squared centered coords. At body scale (~2e3 mm, n<=1e4) Sq <= ~1.2e11,
154// and the largest intermediate below is Sq*RG_Q ~= 2e15 -- three orders clear of the i64 ceiling.
155func rg_centroid(p: *i64, n: i64, c3: *i64) -> i64 {
  var cx: i64 = 0
  var cy: i64 = 0
  var cz: i64 = 0
  var i: i64 = 0
  while i < n { cx = cx + p[i*3]; cy = cy + p[i*3+1]; cz = cz + p[i*3+2]; i = i + 1 }
  c3[0] = cx/n; c3[1] = cy/n; c3[2] = cz/n
  return 0
163}
164// sum of squared distances from the centroid -- the cloud's "size", rotation-invariant by construction
165func rg_spread(p: *i64, n: i64) -> i64 {
  let c: *i64 = sys_mmap(32) as *i64
  rg_centroid(p, n, c)
  var s: i64 = 0
  var i: i64 = 0
  while i < n {
      let dx: i64 = p[i*3] - c[0]
      let dy: i64 = p[i*3+1] - c[1]
      let dz: i64 = p[i*3+2] - c[2]
      s = s + dx*dx + dy*dy + dz*dz
      i = i + 1
  }
  return s
178}
179// scale in Q14 such that Q ~= s*R*P + t. Degenerate (a cloud with no spread) returns unity rather
180// than dividing by zero -- a single point has no scale, and inventing one would be a lie.
181func rg_scale(p: *i64, q: *i64, n: i64) -> i64 {
  let sp: i64 = rg_spread(p, n)
  let sq: i64 = rg_spread(q, n)
  if sp <= 0 { return RG_Q }
  if sq <= 0 { return RG_Q }
  let ratio: i64 = (sq * RG_Q) / sp
  return rg_isqrt(ratio * RG_Q)
188}
189// apply s,R,t to a point
190func rg_apply_s(r: *i64, s: i64, tx: i64, ty: i64, tz: i64, px: i64, py: i64, pz: i64, out3: *i64) -> i64 {
  let rx: i64 = (r[0]*px + r[1]*py + r[2]*pz) / RG_Q
  let ry: i64 = (r[3]*px + r[4]*py + r[5]*pz) / RG_Q
  let rz: i64 = (r[6]*px + r[7]*py + r[8]*pz) / RG_Q
  out3[0] = (s*rx)/RG_Q + tx
  out3[1] = (s*ry)/RG_Q + ty
  out3[2] = (s*rz)/RG_Q + tz
  return 0
198}
199// full similarity fit. out_s is Q14. Rotation reuses the PROVEN rg_register -- compose, do not
200// reimplement -- and only the scale and translation are new.
201// ★ARGUMENT ORDER IS (r, s, t), matching how the transform is WRITTEN: q = s*R*p + t. The first
202// version declared (r, t, s) and the gate called it (r, s, t) -- the mismatch returned the
203// TRANSLATION-X where the scale was expected, and because translation-x was 500 the wrong answer came
204// back as "502" instead of as garbage. ★LAW: three same-typed out-pointers in a row is a footgun; make
205// the order match the notation the caller already has in their head, rather than fixing the call site.
206func rg_register_sim(p: *i64, q: *i64, n: i64, out_r: *i64, out_s: *i64, out_t: *i64) -> i64 {
  let rt: *i64 = sys_mmap(32) as *i64
  rg_register(p, q, n, out_r, rt)
  let s: i64 = rg_scale(p, q, n)
  out_s[0] = s
  let pc: *i64 = sys_mmap(32) as *i64
  let qc: *i64 = sys_mmap(32) as *i64
  rg_centroid(p, n, pc)
  rg_centroid(q, n, qc)
  let rp: *i64 = sys_mmap(32) as *i64
  rg_apply_s(out_r, s, 0, 0, 0, pc[0], pc[1], pc[2], rp)
  out_t[0] = qc[0] - rp[0]
  out_t[1] = qc[1] - rp[1]
  out_t[2] = qc[2] - rp[2]
  return 0
221}
222// residual RMS under the similarity transform
223func rg_residual_s(p: *i64, q: *i64, n: i64, r: *i64, s: i64, t: *i64) -> i64 {
  let o: *i64 = sys_mmap(32) as *i64
  var sum: i64 = 0
  var i: i64 = 0
  while i < n {
      rg_apply_s(r, s, t[0], t[1], t[2], p[i*3], p[i*3+1], p[i*3+2], o)
      let dx: i64 = o[0] - q[i*3]
      let dy: i64 = o[1] - q[i*3+1]
      let dz: i64 = o[2] - q[i*3+2]
      sum = sum + rg_isqrt(dx*dx + dy*dy + dz*dz)
      i = i + 1
  }
  return sum / n
236}
237
238// residual RMS (coord): mean distance from (R*P+t) to Q over n corresponding points
239func rg_residual(p: *i64, q: *i64, n: i64, r: *i64, t: *i64) -> i64 {
  let o: *i64 = sys_mmap(32) as *i64
  var sum: i64 = 0
  var i: i64 = 0
  while i < n {
      rg_apply(r, t[0], t[1], t[2], p[i * 3], p[i * 3 + 1], p[i * 3 + 2], o)
      let dx: i64 = o[0] - q[i * 3]
      let dy: i64 = o[1] - q[i * 3 + 1]
      let dz: i64 = o[2] - q[i * 3 + 2]
      sum = sum + rg_isqrt(dx * dx + dy * dy + dz * dz)
      i = i + 1
  }
  return sum / n
252}
nx_register3d.nx source

`nx_register3d.nx` source
