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