nx_recon3d.nx source

1// nx_recon3d.nx -- MULTI-VIEW TRIANGULATION, the classical MVS core of photo->3D reconstruction (cadtwin P1, 2// the "photograph -> 3D" capture half). This is the CLASSICAL / no-trained-model path (Hartley-Zisserman 3// multi-view geometry / COLMAP triangulation) -- the part a SOVEREIGN no-float stack CAN build; the 2025-26 4// feed-forward SOTA (DUSt3R/MASt3R/VGGT) is neural = trained-model-bound (our named gap). Benchmarked by 5// CHAMFER DISTANCE (the DTU metric the field scores reconstruction on): synthesize N camera views of a KNOWN 6// point cloud -> triangulate each point back -> integer Chamfer vs ground truth. Deterministic, no dataset 7// dependency, no float. Composes toward the full pipeline (feature match nx_features + pose est = later rungs). 8// license_tier: ORIGINAL 9import "nx_syscalls.nx" 10import "nx_vecmath.nx" 11 12const R3_Q: i64 = 16384 // Q14 one 13const R3_WORLDUP_Y: i64 = 16384 // world up = (0,1,0) 14 15func r3_isqrt(n: i64) -> i64 { return vm_isqrt(n) } 16// dot of two Q14 vectors -> Q14 (sum(a_i*b_i)/Q) 17func r3_dotq(ax: i64, ay: i64, az: i64, bx: i64, by: i64, bz: i64) -> i64 { return (ax * bx + ay * by + az * bz) / R3_Q } 18// dot of a Q14 vector with a COORD vector -> coord 19func r3_dotc(ax: i64, ay: i64, az: i64, bx: i64, by: i64, bz: i64) -> i64 { return (ax * bx + ay * by + az * bz) / R3_Q } 20// cross of two Q14 vectors -> Q14 into out3 21func r3_cross(ax: i64, ay: i64, az: i64, bx: i64, by: i64, bz: i64, out3: *i64) -> i64 { 22 out3[0] = (ay * bz - az * by) / R3_Q 23 out3[1] = (az * bx - ax * bz) / R3_Q 24 out3[2] = (ax * by - ay * bx) / R3_Q 25 return 0 26} 27// normalize a raw vector (any scale) to a Q14 unit vector into out3; returns length (raw) 28func r3_normalize(x: i64, y: i64, z: i64, out3: *i64) -> i64 { 29 let len: i64 = r3_isqrt(x * x + y * y + z * z) 30 if len == 0 { out3[0] = 0; out3[1] = 0; out3[2] = 0; return 0 } 31 out3[0] = (x * R3_Q) / len 32 out3[1] = (y * R3_Q) / len 33 out3[2] = (z * R3_Q) / len 34 return len 35} 36 37// ---- CAMERA: center C (coord) + orthonormal basis right/up/forward (Q14) + focal f (px). basis[9]. ---- 38// look-at: forward = norm(target - C); right = norm(worldup x forward); up = forward x right. 39func r3_lookat(cx: i64, cy: i64, cz: i64, tx: i64, ty: i64, tz: i64, basis: *i64) -> i64 { 40 let f3: *i64 = sys_mmap(32) as *i64 41 r3_normalize(tx - cx, ty - cy, tz - cz, f3) 42 let r3v: *i64 = sys_mmap(32) as *i64 43 // worldup (0,Q,0) x forward 44 r3_cross(0, R3_WORLDUP_Y, 0, f3[0], f3[1], f3[2], r3v) 45 let rn: *i64 = sys_mmap(32) as *i64 46 r3_normalize(r3v[0], r3v[1], r3v[2], rn) 47 let u3: *i64 = sys_mmap(32) as *i64 48 r3_cross(f3[0], f3[1], f3[2], rn[0], rn[1], rn[2], u3) 49 basis[0] = rn[0]; basis[1] = rn[1]; basis[2] = rn[2] // right 50 basis[3] = u3[0]; basis[4] = u3[1]; basis[5] = u3[2] // up 51 basis[6] = f3[0]; basis[7] = f3[1]; basis[8] = f3[2] // forward 52 return 0 53} 54// project world point X (coord) through camera -> pixel (u,v) into out2; returns zc (depth, coord); <=0 = behind 55func r3_project(cx: i64, cy: i64, cz: i64, basis: *i64, f: i64, x: i64, y: i64, z: i64, out2: *i64) -> i64 { 56 let vx: i64 = x - cx 57 let vy: i64 = y - cy 58 let vz: i64 = z - cz 59 let xc: i64 = r3_dotc(basis[0], basis[1], basis[2], vx, vy, vz) 60 let yc: i64 = r3_dotc(basis[3], basis[4], basis[5], vx, vy, vz) 61 let zc: i64 = r3_dotc(basis[6], basis[7], basis[8], vx, vy, vz) 62 if zc <= 0 { out2[0] = 0; out2[1] = 0; return zc } 63 out2[0] = (f * xc) / zc 64 out2[1] = (f * yc) / zc 65 return zc 66} 67// back-project pixel (u,v) -> world ray UNIT direction (Q14) into out3 68func r3_ray(basis: *i64, f: i64, u: i64, v: i64, out3: *i64) -> i64 { 69 // dir_world = u*right + v*up + f*forward (raw), then normalize 70 let dx: i64 = u * basis[0] + v * basis[3] + f * basis[6] 71 let dy: i64 = u * basis[1] + v * basis[4] + f * basis[7] 72 let dz: i64 = u * basis[2] + v * basis[5] + f * basis[8] 73 r3_normalize(dx, dy, dz, out3) 74 return 0 75} 76 77// ---- TRIANGULATION: closest point of two rays (midpoint method). rays (C1,d1),(C2,d2), d Q14 unit. ---- 78// out3 = midpoint (coord). returns 1 ok, 0 near-parallel. 79func r3_midpoint(c1x: i64, c1y: i64, c1z: i64, d1x: i64, d1y: i64, d1z: i64, 80 c2x: i64, c2y: i64, c2z: i64, d2x: i64, d2y: i64, d2z: i64, out3: *i64) -> i64 { 81 let wx: i64 = c1x - c2x 82 let wy: i64 = c1y - c2y 83 let wz: i64 = c1z - c2z 84 let b: i64 = r3_dotq(d1x, d1y, d1z, d2x, d2y, d2z) // Q14 85 let d: i64 = r3_dotc(d1x, d1y, d1z, wx, wy, wz) // coord 86 let e: i64 = r3_dotc(d2x, d2y, d2z, wx, wy, wz) // coord 87 let denom: i64 = R3_Q - (b * b) / R3_Q // Q14 (1 - cos^2) 88 if denom < 4 { return 0 } // near parallel 89 let s: i64 = (((b * e) / R3_Q) - d) * R3_Q / denom // coord (param along d1) 90 let t: i64 = (e - (b * d) / R3_Q) * R3_Q / denom // coord (param along d2) 91 let p1x: i64 = c1x + (s * d1x) / R3_Q 92 let p1y: i64 = c1y + (s * d1y) / R3_Q 93 let p1z: i64 = c1z + (s * d1z) / R3_Q 94 let p2x: i64 = c2x + (t * d2x) / R3_Q 95 let p2y: i64 = c2y + (t * d2y) / R3_Q 96 let p2z: i64 = c2z + (t * d2z) / R3_Q 97 out3[0] = (p1x + p2x) / 2 98 out3[1] = (p1y + p2y) / 2 99 out3[2] = (p1z + p2z) / 2 100 return 1 101} 102 103// N-view triangulation = average of all pairwise-ray midpoints (robust, no 3x3 solve). cams: ncam*C(3)+basis(9) 104// packed? -- simpler: caller passes parallel arrays: cx[],cy[],cz[] (centers) and dx[],dy[],dz[] (Q14 ray dirs 105// already back-projected for THIS point), ncam. out3 = triangulated point. 106func r3_triangulate_n(cx: *i64, cy: *i64, cz: *i64, dx: *i64, dy: *i64, dz: *i64, ncam: i64, out3: *i64) -> i64 { 107 let m: *i64 = sys_mmap(32) as *i64 108 var sx: i64 = 0 109 var sy: i64 = 0 110 var sz: i64 = 0 111 var cnt: i64 = 0 112 var i: i64 = 0 113 while i < ncam { 114 var j: i64 = i + 1 115 while j < ncam { 116 let ok: i64 = r3_midpoint(cx[i], cy[i], cz[i], dx[i], dy[i], dz[i], cx[j], cy[j], cz[j], dx[j], dy[j], dz[j], m) 117 if ok == 1 { sx = sx + m[0]; sy = sy + m[1]; sz = sz + m[2]; cnt = cnt + 1 } 118 j = j + 1 119 } 120 i = i + 1 121 } 122 if cnt == 0 { out3[0] = 0; out3[1] = 0; out3[2] = 0; return 0 } 123 out3[0] = sx / cnt 124 out3[1] = sy / cnt 125 out3[2] = sz / cnt 126 return cnt 127} 128 129// euclidean distance (coord) between two points 130func r3_dist(ax: i64, ay: i64, az: i64, bx: i64, by: i64, bz: i64) -> i64 { 131 let dx: i64 = ax - bx 132 let dy: i64 = ay - by 133 let dz: i64 = az - bz 134 return r3_isqrt(dx * dx + dy * dy + dz * dz) 135} 136// CHAMFER distance (coord) between point sets A (na) and B (nb), each stored as [x,y,z] stride 3. 137// = (mean_a min_b d(a,b) + mean_b min_a d(b,a)) / 2 -- the DTU reconstruction metric. 138func r3_chamfer(a: *i64, na: i64, b: *i64, nb: i64) -> i64 { 139 var suma: i64 = 0 140 var i: i64 = 0 141 while i < na { 142 var best: i64 = 0 - 1 143 var j: i64 = 0 144 while j < nb { 145 let dd: i64 = r3_dist(a[i * 3], a[i * 3 + 1], a[i * 3 + 2], b[j * 3], b[j * 3 + 1], b[j * 3 + 2]) 146 if best < 0 { best = dd } else { if dd < best { best = dd } } 147 j = j + 1 148 } 149 suma = suma + best 150 i = i + 1 151 } 152 var sumb: i64 = 0 153 i = 0 154 while i < nb { 155 var best2: i64 = 0 - 1 156 var j2: i64 = 0 157 while j2 < na { 158 let dd2: i64 = r3_dist(b[i * 3], b[i * 3 + 1], b[i * 3 + 2], a[j2 * 3], a[j2 * 3 + 1], a[j2 * 3 + 2]) 159 if best2 < 0 { best2 = dd2 } else { if dd2 < best2 { best2 = dd2 } } 160 j2 = j2 + 1 161 } 162 sumb = sumb + best2 163 i = i + 1 164 } 165 let ma: i64 = suma / na 166 let mb: i64 = sumb / nb 167 return (ma + mb) / 2 168}