nx_pc.nx source

1// nx_pc.nx -- 2D point cloud + ICP alignment + nearest neighbor. 2// 3// What this unlocks: 4// + register two views of a planar object (paper / floor / map) 5// + align Hough-line endpoints across two photos 6// + match feature points from V8 salience peaks across frames 7// + planar SLAM scaffolding 8// + 2D landmark localization 9// 10// All pure i64 with Q10 trig. Brute-force NN is O(N*M); for larger 11// clouds users can pre-bucket; we keep the substrate primitive simple. 12// 13// genealogy_id: horn_1987_closed_form_rotation + besl_mckay_1992_icp 14// lineage_id: point_cloud_registration + iterative_closest_point 15 16// nx_safety_envelope: 17// intended_use: AUTO_APPLIED -- primitive-specific tuning queued 18// sil_target: SIL1 19// evidence: [bulk_applied_2026-05-16, see-file-comment-for-detail] 20// verdict: NOT_YET_EVALUATED 21 22import "syscalls.nx" 23import "nx_math.nx" 24 25const NX_PC_Q: i64 = 1024 // Q10 26 27struct PointCloud2D { 28 n: i64, 29 xs: *i64, 30 ys: *i64, 31} 32 33// ===== Allocation + access ============================================= 34 35func nx_pc_alloc(n: i64) -> *PointCloud2D { 36 let pc: *PointCloud2D = (sys_mmap(24)) as *PointCloud2D 37 pc.n = n 38 pc.xs = (sys_mmap(n * 8)) as *i64 39 pc.ys = (sys_mmap(n * 8)) as *i64 40 return pc 41} 42 43func nx_pc_set(pc: *PointCloud2D, i: i64, x: i64, y: i64) -> i64 { 44 pc.xs[i] = x 45 pc.ys[i] = y 46 return 0 47} 48 49func nx_pc_get_x(pc: *PointCloud2D, i: i64) -> i64 { 50 return pc.xs[i] 51} 52 53func nx_pc_get_y(pc: *PointCloud2D, i: i64) -> i64 { 54 return pc.ys[i] 55} 56 57// Integer sqrt is canonical in nx_math.nx. 58 59// ===== Centroid ========================================================= 60 61func nx_pc_centroid(pc: *PointCloud2D, out_cx: *i64, out_cy: *i64) -> i64 { 62 if pc.n == 0 { 63 out_cx[0] = 0 64 out_cy[0] = 0 65 return 0 66 } 67 var sx: i64 = 0 68 var sy: i64 = 0 69 var i: i64 = 0 70 while i < pc.n { 71 sx = sx + pc.xs[i] 72 sy = sy + pc.ys[i] 73 i = i + 1 74 } 75 out_cx[0] = sx / pc.n 76 out_cy[0] = sy / pc.n 77 return 0 78} 79 80// ===== Bounding box ===================================================== 81 82func nx_pc_bbox(pc: *PointCloud2D, out_xmin: *i64, out_ymin: *i64, 83 out_xmax: *i64, out_ymax: *i64) -> i64 { 84 if pc.n == 0 { 85 out_xmin[0] = 0; out_ymin[0] = 0 86 out_xmax[0] = 0; out_ymax[0] = 0 87 return 0 88 } 89 var xmn: i64 = pc.xs[0] 90 var xmx: i64 = pc.xs[0] 91 var ymn: i64 = pc.ys[0] 92 var ymx: i64 = pc.ys[0] 93 var i: i64 = 1 94 while i < pc.n { 95 let x: i64 = pc.xs[i] 96 let y: i64 = pc.ys[i] 97 if x < xmn { xmn = x } 98 if x > xmx { xmx = x } 99 if y < ymn { ymn = y } 100 if y > ymx { ymx = y } 101 i = i + 1 102 } 103 out_xmin[0] = xmn; out_ymin[0] = ymn 104 out_xmax[0] = xmx; out_ymax[0] = ymx 105 return 0 106} 107 108// ===== Brute-force nearest neighbor ==================================== 109// 110// Returns index of point in `cloud` nearest (in squared distance) to (qx, qy). 111 112func nx_pc_nearest(cloud: *PointCloud2D, qx: i64, qy: i64) -> i64 { 113 if cloud.n == 0 { return -1 } 114 var best_i: i64 = 0 115 let dx0: i64 = cloud.xs[0] - qx 116 let dy0: i64 = cloud.ys[0] - qy 117 var best_d: i64 = dx0 * dx0 + dy0 * dy0 118 var i: i64 = 1 119 while i < cloud.n { 120 let dx: i64 = cloud.xs[i] - qx 121 let dy: i64 = cloud.ys[i] - qy 122 let d: i64 = dx * dx + dy * dy 123 if d < best_d { 124 best_d = d 125 best_i = i 126 } 127 i = i + 1 128 } 129 return best_i 130} 131 132// ===== Apply 2D rigid transform (rotation + translation) =============== 133// 134// (x', y') = R * (x, y) + t where R is from (cos_q10, sin_q10). 135 136func nx_pc_apply_transform(pc: *PointCloud2D, cos_q10: i64, sin_q10: i64, 137 tx: i64, ty: i64) -> i64 { 138 var i: i64 = 0 139 while i < pc.n { 140 let x: i64 = pc.xs[i] 141 let y: i64 = pc.ys[i] 142 pc.xs[i] = (cos_q10 * x - sin_q10 * y) / NX_PC_Q + tx 143 pc.ys[i] = (sin_q10 * x + cos_q10 * y) / NX_PC_Q + ty 144 i = i + 1 145 } 146 return 0 147} 148 149// ===== One ICP iteration =============================================== 150// 151// Given source cloud and target cloud (must have same size for our 152// simplified version OR using nearest-neighbor pairing for arbitrary 153// sizes), compute and apply the optimal rigid transform that aligns 154// source to target. 155// 156// Horn 1987 closed-form for 2D: 157// 1. Compute centroids of paired points. 158// 2. Center both. 159// 3. Form correlation matrix M = sum (src_centered)(tgt_centered)^T. 160// 4. Optimal rotation: 161// c = (Sxx + Syy) / norm 162// s = (Syx - Sxy) / norm 163// where norm = sqrt((Sxx+Syy)^2 + (Syx-Sxy)^2) 164// 5. t = tgt_centroid - R * src_centroid. 165// 166// Apply transform to source in place. Returns 0 on success, -1 if 167// degenerate (norm == 0). 168 169func nx_pc_icp_step(src: *PointCloud2D, tgt: *PointCloud2D) -> i64 { 170 if src.n == 0 { return -1 } 171 if tgt.n == 0 { return -1 } 172 // Pair via nearest neighbor: for each src point, find nearest tgt. 173 // Compute centroids of paired points. 174 var src_cx: i64 = 0 175 var src_cy: i64 = 0 176 var tgt_cx: i64 = 0 177 var tgt_cy: i64 = 0 178 var i: i64 = 0 179 let pairs_x: *i64 = (sys_mmap(src.n * 8)) as *i64 180 let pairs_y: *i64 = (sys_mmap(src.n * 8)) as *i64 181 while i < src.n { 182 let sx: i64 = src.xs[i] 183 let sy: i64 = src.ys[i] 184 let tj: i64 = nx_pc_nearest(tgt, sx, sy) 185 pairs_x[i] = tgt.xs[tj] 186 pairs_y[i] = tgt.ys[tj] 187 src_cx = src_cx + sx 188 src_cy = src_cy + sy 189 tgt_cx = tgt_cx + pairs_x[i] 190 tgt_cy = tgt_cy + pairs_y[i] 191 i = i + 1 192 } 193 src_cx = src_cx / src.n 194 src_cy = src_cy / src.n 195 tgt_cx = tgt_cx / src.n 196 tgt_cy = tgt_cy / src.n 197 // Form 2x2 correlation matrix on centered points. 198 var Sxx: i64 = 0 199 var Sxy: i64 = 0 200 var Syx: i64 = 0 201 var Syy: i64 = 0 202 var j: i64 = 0 203 while j < src.n { 204 let cx_s: i64 = src.xs[j] - src_cx 205 let cy_s: i64 = src.ys[j] - src_cy 206 let cx_t: i64 = pairs_x[j] - tgt_cx 207 let cy_t: i64 = pairs_y[j] - tgt_cy 208 Sxx = Sxx + cx_s * cx_t 209 Sxy = Sxy + cx_s * cy_t 210 Syx = Syx + cy_s * cx_t 211 Syy = Syy + cy_s * cy_t 212 j = j + 1 213 } 214 let a: i64 = Sxx + Syy 215 let b: i64 = Syx - Sxy 216 let norm2: i64 = a * a + b * b 217 if norm2 == 0 { return -1 } 218 let norm: i64 = nx_math_isqrt(norm2) 219 if norm == 0 { return -1 } 220 let cos_q10: i64 = (a * NX_PC_Q) / norm 221 let sin_q10: i64 = (b * NX_PC_Q) / norm 222 // Translation: t = tgt_centroid - R * src_centroid 223 let tx: i64 = tgt_cx - (cos_q10 * src_cx - sin_q10 * src_cy) / NX_PC_Q 224 let ty: i64 = tgt_cy - (sin_q10 * src_cx + cos_q10 * src_cy) / NX_PC_Q 225 nx_pc_apply_transform(src, cos_q10, sin_q10, tx, ty) 226 return 0 227} 228 229// Run ICP for up to max_iter iterations. Returns number of iterations 230// performed. 231 232func nx_pc_icp(src: *PointCloud2D, tgt: *PointCloud2D, 233 max_iter: i64) -> i64 { 234 var i: i64 = 0 235 while i < max_iter { 236 if nx_pc_icp_step(src, tgt) != 0 { return i } 237 i = i + 1 238 } 239 return max_iter 240} 241 242// ===== Mean squared error between paired clouds ======================== 243// 244// For each src point, find nearest in tgt, accumulate squared distance. 245// Returns MSE / src.n. 246 247func nx_pc_mse(src: *PointCloud2D, tgt: *PointCloud2D) -> i64 { 248 if src.n == 0 { return 0 } 249 var acc: i64 = 0 250 var i: i64 = 0 251 while i < src.n { 252 let tj: i64 = nx_pc_nearest(tgt, src.xs[i], src.ys[i]) 253 let dx: i64 = src.xs[i] - tgt.xs[tj] 254 let dy: i64 = src.ys[i] - tgt.ys[tj] 255 acc = acc + dx * dx + dy * dy 256 i = i + 1 257 } 258 return acc / src.n 259}