diff --git a/CHANGELOG.md b/CHANGELOG.md index 518cf41d..583fba36 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -21,6 +21,14 @@ removed no sooner than the next major (see `docs/API_STABILITY.md`). ### Changed +- Extend `register-icp-gpu` with explicit `align_with_gpu_reduction`: retain + original source/target on GPU, transform, match and reduce centered moments + on device, then download a fixed 176-byte result per evaluation. CPU Horn + pose solving and convergence remain; post-update moments are reused for the + next update and final fitness. Ordinary CPU/correspondence paths retain their + math and trimming support. The new route requires untrimmed ICP and documents + f32 matching/reduction/conditioning differences and full-call measurements. + - Add opt-in `filter-mls-gpu` smoothing for constant, linear and quadratic surfaces. Reusable input-bound radius rows feed GPU weighted frames and polynomial fits in three passes; explicit upload/download receipts and diff --git a/README.md b/README.md index a416559a..7ac73edb 100644 --- a/README.md +++ b/README.md @@ -644,6 +644,14 @@ The target KD-tree is uploaded once and reused for update/fitness queries; the result contains the ICP trace and transfer accounting. Pose estimation remains on CPU. See [measurements and limits](notes/2026-10-12_gpu_icp_correspondences.md). +`IcpRegistration::align_with_gpu_reduction(&runtime, &source, &target)` under +the same feature retains both clouds on GPU and also runs coordinate transforms, +centroid/cross-covariance and fitness reductions on device. Each evaluation +downloads only 176 bytes of moments/status; CPU pose solving and convergence +remain. This route requires untrimmed ICP and uses f32 device math, which can +differ near match gates and degenerate geometry. See +[resident ICP measurements](notes/2026-10-13_gpu_icp_reduction.md). + GICP exposes the same explicit correspondence entry point under `register-gicp-gpu`; configured radius covariances run on the supplied runtime and share the transfer receipt. Its Hessian/gradient accumulation and pose diff --git a/crates/spatialrust-gpu/src/kernels/icp_reduction.rs b/crates/spatialrust-gpu/src/kernels/icp_reduction.rs new file mode 100644 index 00000000..7b2276e3 --- /dev/null +++ b/crates/spatialrust-gpu/src/kernels/icp_reduction.rs @@ -0,0 +1,472 @@ +use crate::{scratch::ScratchBuffers, GpuKdTree, GpuQueryBatch, WgpuRuntime}; +use bytemuck::{Pod, Zeroable}; +use spatialrust_core::{SpatialError, SpatialResult, TransferDirection, TransferStats}; +use spatialrust_math::Isometry3; +use spatialrust_search::KdTree; +use std::sync::Arc; +use wgpu::util::DeviceExt; + +const RESULT_BYTES: u64 = 176; + +#[repr(C)] +#[derive(Clone, Copy, Pod, Zeroable)] +struct Params { + rows: [[f32; 4]; 3], + source_anchor: [f32; 4], + target_anchor: [f32; 4], + count: u32, + targets: u32, + groups: u32, + unused: u32, + bound: f32, + max_distance: f32, + scale: f32, + padding: f32, +} + +fn invalid() -> SpatialError { + SpatialError::InvalidArgument("GPU ICP reduction requires matching runtime, finite XYZ/extents, a finite unit-quaternion transform, a positive finite nonzero distance square, nonempty target and supported limits".to_owned()) +} + +fn plan(count: u64, limits: &wgpu::Limits) -> SpatialResult { + let groups = count.div_ceil(64).min(256); + let bound = limits.max_buffer_size.min(u64::from(limits.max_storage_buffer_binding_size)); + if count > u64::from(u32::MAX) + || count.checked_mul(16).map_or(true, |n| n > bound || n > isize::MAX as u64) + || groups * 144 > bound + || bound < RESULT_BYTES + || limits.max_compute_workgroups_per_dimension < 9 + || count.div_ceil(64) > u64::from(limits.max_compute_workgroups_per_dimension) + || limits.max_compute_workgroup_size_x < 64 + || limits.max_compute_workgroup_size_y == 0 + || limits.max_compute_workgroup_size_z == 0 + || limits.max_compute_invocations_per_workgroup < 64 + || limits.max_compute_workgroup_storage_size < 768 + || limits.max_storage_buffers_per_shader_stage < 6 + || limits.max_bindings_per_bind_group < 7 + || limits.max_bind_groups == 0 + || limits.max_uniform_buffers_per_shader_stage == 0 + || limits.max_uniform_buffer_binding_size < 112 + { + return Err(invalid()); + } + Ok(groups as u32) +} + +fn geometry(points: &[[f32; 3]]) -> SpatialResult<([f32; 4], f64)> { + if points.iter().flatten().any(|v| !v.is_finite()) { + return Err(invalid()); + } + if points.is_empty() { + return Ok(([0.0; 4], 0.0)); + } + let mut anchor = [0.0; 4]; + let mut extent_squared = 0.0; + for axis in 0..3 { + let lo = points.iter().map(|p| p[axis]).fold(f32::INFINITY, f32::min); + let hi = points.iter().map(|p| p[axis]).fold(f32::NEG_INFINITY, f32::max); + anchor[axis] = ((f64::from(lo) + f64::from(hi)) * 0.5) as f32; + extent_squared += (f64::from(hi) - f64::from(lo)).powi(2); + } + if !(extent_squared as f32).is_finite() { + return Err(invalid()); + } + Ok((anchor, extent_squared)) +} + +/// Explicitly uploaded source and target for reusable device ICP moments. +/// CPU target-tree construction is external. Original source points remain +/// immutable; every evaluation applies the absolute pose to those originals. +pub struct GpuIcpInput { + source: GpuQueryBatch, + target: GpuQueryBatch, + tree: GpuKdTree, + source_anchor: [f32; 4], + target_anchor: [f32; 4], + scale: f32, + identity: Arc<()>, + transfers: TransferStats, +} + +impl GpuIcpInput { + /// Uploads source XYZ and a nonempty finite target tree. Payload is 16*S + /// source bytes, 32*T target bytes (tree order plus original order), and + /// 32*M tree-node bytes. The combined normalization extent must have a normal + /// finite f32 square; coincident source/target sets use scale 1. + pub fn upload( + runtime: &WgpuRuntime, + source: &[[f32; 3]], + target: &KdTree, + ) -> SpatialResult { + plan(source.len() as u64, &runtime.device().limits())?; + let layout = target.layout(); + let (x, y, z) = layout.positions(); + plan(x.len() as u64, &runtime.device().limits())?; + if x.is_empty() { + return Err(invalid()); + } + let target_positions: Vec<_> = (0..x.len()).map(|i| [x[i], y[i], z[i]]).collect(); + let (source_anchor, source_extent) = geometry(source)?; + let (target_anchor, target_extent) = geometry(&target_positions)?; + let extent = source_extent.max(target_extent); + if extent > 0.0 && (extent as f32) < f32::MIN_POSITIVE { + return Err(invalid()); + } + let scale = if extent == 0.0 { 1.0 } else { extent.sqrt() as f32 }; + let tree = GpuKdTree::upload(runtime, target)?; + let source = GpuQueryBatch::upload(runtime, source)?; + let target = GpuQueryBatch::upload(runtime, &target_positions)?; + let mut transfers = tree.upload_transfers(); + transfers.record( + TransferDirection::HostToDevice, + source.upload_transfers().host_to_device_bytes(), + ); + transfers.record( + TransferDirection::HostToDevice, + target.upload_transfers().host_to_device_bytes(), + ); + Ok(Self { + source, + target, + tree, + source_anchor, + target_anchor, + scale, + identity: runtime.profiling_identity(), + transfers, + }) + } + + /// Payload transferred during explicit input upload. + #[must_use] + pub const fn upload_transfers(&self) -> TransferStats { + self.transfers + } + + /// Original source point count. + #[must_use] + pub fn source_count(&self) -> u32 { + self.source.query_count() + } + + /// Submits source transformation, bounded nearest search, compensated + /// centroid/fitness and centered cross-covariance reductions in six passes. + /// No payload copy/readback or host wait occurs. The fixed-size owned result + /// can outlive inputs; consumers must check status before using its moments. + /// GPU f32 transforms/reductions can differ from CPU f64 accumulation near + /// match gates, degenerate geometry and convergence thresholds. No trimming + /// or pose solve is performed here. Overflow invalidates ALL result payload. + pub fn moments_resident( + &self, + runtime: &WgpuRuntime, + transform: Isometry3, + max_distance: f32, + ) -> SpatialResult { + let q = transform.rotation(); + let t = transform.translation(); + let norm = f64::from(q.x).hypot(f64::from(q.y)).hypot(f64::from(q.z)).hypot(f64::from(q.w)); + let bound = max_distance * max_distance; + if !Arc::ptr_eq(&self.identity, &runtime.profiling_identity()) + || !norm.is_finite() + || (norm - 1.0).abs() > 1e-4 + || !t.x.is_finite() + || !t.y.is_finite() + || !t.z.is_finite() + || !max_distance.is_finite() + || max_distance <= 0.0 + || !bound.is_finite() + || bound == 0.0 + { + return Err(invalid()); + } + let count = self.source_count(); + let groups = plan(u64::from(count), &runtime.device().limits())?; + let output = GpuIcpMoments { + buffer: runtime.device().create_buffer(&wgpu::BufferDescriptor { + label: Some("icp-moments"), + size: RESULT_BYTES, + usage: wgpu::BufferUsages::STORAGE | wgpu::BufferUsages::COPY_SRC, + mapped_at_creation: false, + }), + count, + scale: self.scale, + bound, + identity: Arc::clone(&self.identity), + }; + if count == 0 { + return Ok(output); + } + let r = transform.rotation().to_mat3(); + let params = Params { + rows: [ + [r.m[0][0], r.m[0][1], r.m[0][2], t.x], + [r.m[1][0], r.m[1][1], r.m[1][2], t.y], + [r.m[2][0], r.m[2][1], r.m[2][2], t.z], + ], + source_anchor: self.source_anchor, + target_anchor: self.target_anchor, + count, + targets: self.target.query_count(), + groups, + unused: 0, + bound, + max_distance, + scale: self.scale, + padding: 0.0, + }; + let uniform = runtime.device().create_buffer_init(&wgpu::util::BufferInitDescriptor { + label: Some("icp-reduction-params"), + contents: bytemuck::bytes_of(¶ms), + usage: wgpu::BufferUsages::UNIFORM, + }); + let mut scratch = ScratchBuffers::new(runtime); + let queries = scratch.acquire("icp-transformed", u64::from(count) * 16); + let matches = scratch.acquire("icp-matches", u64::from(count) * 8); + let partials = scratch.acquire("icp-partials", u64::from(groups) * 144); + let cache = runtime + .icp_reduction_pipelines + .get_or_init(|| IcpReductionPipelines::new(runtime.device())); + let buffers = [ + &uniform, + self.source.buffer().unwrap(), + self.target.buffer().unwrap(), + &queries, + &matches, + &partials, + &output.buffer, + ]; + let entries: Vec<_> = buffers + .iter() + .enumerate() + .map(|(binding, buffer)| wgpu::BindGroupEntry { + binding: binding as u32, + resource: buffer.as_entire_binding(), + }) + .collect(); + let group = runtime.device().create_bind_group(&wgpu::BindGroupDescriptor { + label: Some("icp-reduction-bindings"), + layout: &cache.layout, + entries: &entries, + }); + let mut encoder = runtime.device().create_command_encoder(&Default::default()); + dispatch(&mut encoder, &cache.pipelines[0], &group, count.div_ceil(64), 1); + self.tree.encode_nearest(runtime, &mut encoder, &queries, count, bound, &matches)?; + for (index, x, y) in [(1, groups, 7), (2, 1, 1), (3, groups, 9), (4, 1, 1)] { + dispatch(&mut encoder, &cache.pipelines[index], &group, x, y); + } + scratch.submit_and_recycle(encoder); + Ok(output) + } +} + +fn dispatch( + encoder: &mut wgpu::CommandEncoder, + pipeline: &wgpu::ComputePipeline, + group: &wgpu::BindGroup, + x: u32, + y: u32, +) { + let mut pass = encoder.begin_compute_pass(&wgpu::ComputePassDescriptor { + label: Some("icp-reduction"), + timestamp_writes: None, + }); + pass.set_pipeline(pipeline); + pass.set_bind_group(0, group, &[]); + pass.dispatch_workgroups(x, y, 1); +} + +/// Decoded device ICP moments in physical units, with explicit transfer receipt. +#[derive(Clone, Debug, PartialEq)] +pub struct GpuIcpStatistics { + /// Number of gated nearest correspondences; zero yields all-zero moments. + pub correspondences: u32, + /// Arithmetic source centroid after applying the requested transform. + pub source_centroid: [f64; 3], + /// Corresponding target centroid. + pub target_centroid: [f64; 3], + /// Unnormalized centered sum of source[i] * target[j], row-major. + pub cross_covariance: [[f64; 3]; 3], + /// Sum of squared matched residual lengths (not a mean). + pub squared_error_sum: f64, + /// Payload copied by this explicit download, independent of input upload. + pub transfers: TransferStats, +} + +/// Owned fixed-size device ICP moments. No buffer is pooled or input-borrowed. +pub struct GpuIcpMoments { + buffer: wgpu::Buffer, + count: u32, + scale: f32, + bound: f32, + identity: Arc<()>, +} +impl GpuIcpMoments { + /// Physical coordinate scale for the normalized centroid/covariance words. + #[must_use] + pub const fn normalization_scale(&self) -> f32 { + self.scale + } + + /// Physical squared-distance scale for normalized residual sum word 15. + #[must_use] + pub const fn squared_distance_scale(&self) -> f32 { + self.bound + } + + /// 176-byte storage: u32 status/count/two reserved zeros, two vec4 + /// anchors, then 16 vec2 compensated sums. Status bits 1/2/4 denote + /// transform overflow, malformed matches, nonfinite moments. Status must be + /// zero. Terms 0..5 are normalized source/target centroid offsets; 6..14 + /// normalized centered cross-covariance; 15 normalized squared-error sum. + /// Scale is the input bounding-box diagonal; fitness scale is distance^2. + /// Invalid or zero-match results have all-zero payload (status retained). + #[must_use] + pub fn buffer(&self) -> &wgpu::Buffer { + &self.buffer + } + + /// Explicit readback size; zero for an empty source, otherwise 176 bytes. + #[must_use] + pub const fn readback_bytes(&self) -> u64 { + if self.count == 0 { + 0 + } else { + RESULT_BYTES + } + } + + /// Copies the fixed-size moments/status, waits, then validates and decodes + /// compensated f32 sums in f64. No point/query/match payload is downloaded. + pub fn download(&self, runtime: &WgpuRuntime) -> SpatialResult { + if !Arc::ptr_eq(&self.identity, &runtime.profiling_identity()) { + return Err(invalid()); + } + let mut transfers = TransferStats::default(); + let mut words = vec![0u32; RESULT_BYTES as usize / 4]; + if self.count > 0 { + let staging = runtime.device().create_buffer(&wgpu::BufferDescriptor { + label: Some("icp-moments-readback"), + size: RESULT_BYTES, + usage: wgpu::BufferUsages::COPY_DST | wgpu::BufferUsages::MAP_READ, + mapped_at_creation: false, + }); + let mut encoder = runtime.device().create_command_encoder(&Default::default()); + encoder.copy_buffer_to_buffer(&self.buffer, 0, &staging, 0, RESULT_BYTES); + runtime.queue().submit(Some(encoder.finish())); + let slice = staging.slice(..); + let (sender, receiver) = std::sync::mpsc::channel(); + slice.map_async(wgpu::MapMode::Read, move |r| { + let _ = sender.send(r); + }); + runtime.device().poll(wgpu::Maintain::Wait); + receiver + .recv() + .map_err(|e| SpatialError::InvalidArgument(e.to_string()))? + .map_err(|e| SpatialError::InvalidArgument(e.to_string()))?; + let mapped = slice.get_mapped_range(); + words.copy_from_slice(bytemuck::cast_slice(&mapped)); + drop(mapped); + staging.unmap(); + transfers.record(TransferDirection::DeviceToHost, RESULT_BYTES); + } + if words[0] != 0 + || words[1] > self.count + || words[2..4] != [0, 0] + || (words[1] == 0 && words[4..].iter().any(|&v| v != 0)) + { + return Err(SpatialError::InvalidArgument(format!( + "GPU ICP reduction invalid status/count: {:?}", + &words[..4] + ))); + } + let values: Vec<_> = words[4..].iter().map(|&v| f64::from(f32::from_bits(v))).collect(); + if values.iter().any(|v| !v.is_finite()) { + return Err(invalid()); + } + let sum = |term: usize| values[8 + term * 2] + values[9 + term * 2]; + let scale = f64::from(self.scale); + let source_centroid = + std::array::from_fn(|i| if words[1] == 0 { 0.0 } else { values[i] + sum(i) * scale }); + let target_centroid = std::array::from_fn(|i| { + if words[1] == 0 { + 0.0 + } else { + values[4 + i] + sum(3 + i) * scale + } + }); + let cross_covariance = + std::array::from_fn(|i| std::array::from_fn(|j| sum(6 + i * 3 + j) * scale * scale)); + let squared_error_sum = sum(15) * f64::from(self.bound); + if squared_error_sum < 0.0 { + return Err(invalid()); + } + Ok(GpuIcpStatistics { + correspondences: words[1], + source_centroid, + target_centroid, + cross_covariance, + squared_error_sum, + transfers, + }) + } +} + +pub(crate) struct IcpReductionPipelines { + layout: wgpu::BindGroupLayout, + pipelines: [wgpu::ComputePipeline; 5], +} +impl IcpReductionPipelines { + fn new(device: &wgpu::Device) -> Self { + let entries: Vec<_> = (0..7) + .map(|binding| wgpu::BindGroupLayoutEntry { + binding, + visibility: wgpu::ShaderStages::COMPUTE, + ty: wgpu::BindingType::Buffer { + ty: if binding == 0 { + wgpu::BufferBindingType::Uniform + } else { + wgpu::BufferBindingType::Storage { read_only: matches!(binding, 1 | 2 | 4) } + }, + has_dynamic_offset: false, + min_binding_size: None, + }, + count: None, + }) + .collect(); + let layout = device.create_bind_group_layout(&wgpu::BindGroupLayoutDescriptor { + label: Some("icp-reduction-layout"), + entries: &entries, + }); + let pipeline_layout = device.create_pipeline_layout(&wgpu::PipelineLayoutDescriptor { + label: Some("icp-reduction-layout"), + bind_group_layouts: &[&layout], + push_constant_ranges: &[], + }); + let shader = device.create_shader_module(wgpu::ShaderModuleDescriptor { + label: Some("icp-reduction-shader"), + source: wgpu::ShaderSource::Wgsl(include_str!("../shaders/icp_reduction.wgsl").into()), + }); + let pipelines = [ + "transform_source", + "centroid_partials", + "finish_centroids", + "covariance_partials", + "finish_covariance", + ] + .map(|entry| { + device.create_compute_pipeline(&wgpu::ComputePipelineDescriptor { + label: Some("icp-reduction-pipeline"), + layout: Some(&pipeline_layout), + module: &shader, + entry_point: Some(entry), + compilation_options: Default::default(), + cache: None, + }) + }); + Self { layout, pipelines } + } +} + +#[cfg(all(test, not(target_arch = "wasm32")))] +#[path = "icp_reduction_tests.rs"] +mod tests; diff --git a/crates/spatialrust-gpu/src/kernels/icp_reduction_tests.rs b/crates/spatialrust-gpu/src/kernels/icp_reduction_tests.rs new file mode 100644 index 00000000..b29bb245 --- /dev/null +++ b/crates/spatialrust-gpu/src/kernels/icp_reduction_tests.rs @@ -0,0 +1,216 @@ +use super::*; +use spatialrust_math::{Quat, TransformPoint, Vec3}; + +fn tree(points: &[[f32; 3]]) -> KdTree { + let columns: [Vec<_>; 3] = std::array::from_fn(|i| points.iter().map(|p| p[i]).collect()); + KdTree::from_slices(&columns[0], &columns[1], &columns[2]) +} +fn read_words(runtime: &WgpuRuntime, buffer: &wgpu::Buffer) -> Vec { + let staging = runtime.device().create_buffer(&wgpu::BufferDescriptor { + label: None, + size: RESULT_BYTES, + usage: wgpu::BufferUsages::COPY_DST | wgpu::BufferUsages::MAP_READ, + mapped_at_creation: false, + }); + let mut encoder = runtime.device().create_command_encoder(&Default::default()); + encoder.copy_buffer_to_buffer(buffer, 0, &staging, 0, RESULT_BYTES); + runtime.queue().submit(Some(encoder.finish())); + let slice = staging.slice(..); + let (tx, rx) = std::sync::mpsc::channel(); + slice.map_async(wgpu::MapMode::Read, move |r| tx.send(r).unwrap()); + runtime.device().poll(wgpu::Maintain::Wait); + rx.recv().unwrap().unwrap(); + let mapped = slice.get_mapped_range(); + let words = bytemuck::cast_slice(&mapped).to_vec(); + drop(mapped); + staging.unmap(); + words +} + +#[test] +fn reduction_plan_checks_storage_dispatch_uniform_and_shared_memory() { + assert_eq!(std::mem::size_of::(), 112); + let limits = wgpu::Limits::default(); + assert!(plan(u64::MAX, &limits).is_err()); + assert!(plan(u64::from(u32::MAX) + 1, &limits).is_err()); + assert_eq!(plan(65, &limits).unwrap(), 2); + assert_eq!(plan(65537, &limits).unwrap(), 256); + for case in 0..6 { + let mut restricted = limits.clone(); + match case { + 0 => restricted.max_buffer_size = 175, + 1 => restricted.max_uniform_buffer_binding_size = 111, + 2 => restricted.max_storage_buffers_per_shader_stage = 5, + 3 => restricted.max_compute_workgroup_storage_size = 767, + 4 => restricted.max_compute_workgroup_size_x = 63, + _ => restricted.max_compute_workgroups_per_dimension = 8, + } + assert!(plan(1, &restricted).is_err()); + } +} + +#[test] +fn compensated_centroids_covariance_and_fitness_match_independent_f64_math() { + let runtime = WgpuRuntime::new_headless().unwrap(); + for n in [1, 3, 63, 64, 65, 257, 4097, 16385] { + let source: Vec<_> = (0..n) + .map(|i| [(i % 17) as f32 * 0.5, (i / 17 % 17) as f32 * 0.5, (i / 289) as f32 * 0.5]) + .collect(); + let target: Vec<_> = + source.iter().map(|p| [p[0] + 0.03125, p[1] - 0.015625, p[2] + 0.0078125]).collect(); + let target_tree = tree(&target); + let input = GpuIcpInput::upload(&runtime, &source, &target_tree).unwrap(); + let actual = input + .moments_resident(&runtime, Isometry3::identity(), 0.25) + .unwrap() + .download(&runtime) + .unwrap(); + assert_eq!(actual.correspondences as usize, n); + let mean_s: [f64; 3] = + std::array::from_fn(|a| source.iter().map(|p| f64::from(p[a])).sum::() / n as f64); + let mean_t: [f64; 3] = + std::array::from_fn(|a| target.iter().map(|p| f64::from(p[a])).sum::() / n as f64); + for a in 0..3 { + assert!((actual.source_centroid[a] - mean_s[a]).abs() < 1e-5); + assert!((actual.target_centroid[a] - mean_t[a]).abs() < 1e-5); + for b in 0..3 { + let products: Vec<_> = source + .iter() + .zip(&target) + .map(|(s, t)| (f64::from(s[a]) - mean_s[a]) * (f64::from(t[b]) - mean_t[b])) + .collect(); + let expected = products.iter().sum::(); + let absolute_products = products.iter().map(|v| v.abs()).sum::(); + assert!( + (actual.cross_covariance[a][b] - expected).abs() + < 1e-7 + 1e-6 * absolute_products, + "n={n} a={a} b={b} actual={} expected={expected}", + actual.cross_covariance[a][b] + ); + } + } + let error = source + .iter() + .zip(&target) + .map(|(s, t)| (0..3).map(|a| f64::from(s[a] - t[a]).powi(2)).sum::()) + .sum::(); + assert!((actual.squared_error_sum - error).abs() < 1e-7 * (1.0 + error)); + assert_eq!(actual.transfers.device_to_host_bytes(), 176); + assert_eq!( + input.upload_transfers().host_to_device_bytes(), + (n * 48 + target_tree.layout().nodes().len() * 32) as u64 + ); + } +} + +#[test] +fn transformed_origins_ownership_and_concurrent_scratch_keep_owned_results() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let other = WgpuRuntime::new_headless().unwrap(); + for origin in [0.0, 1e6] { + let points: Vec<_> = (0..65) + .map(|i| { + [ + origin + (i % 5) as f32 * 0.5, + -origin + (i / 5 % 5) as f32 * 0.5, + (i / 25) as f32 * 0.5, + ] + }) + .collect(); + let pose = Isometry3::new(Quat::::identity(), Vec3::new(0.125, -0.25, 0.0625)); + let target: Vec<_> = points + .iter() + .map(|p| { + let q = pose.transform_point(Vec3::new(p[0], p[1], p[2])); + [q.x, q.y, q.z] + }) + .collect(); + let input = GpuIcpInput::upload(&runtime, &points, &tree(&target)).unwrap(); + assert!(input.moments_resident(&other, pose, 0.25).is_err()); + let retained = input.moments_resident(&runtime, pose, 0.25).unwrap(); + let expected = retained.download(&runtime).unwrap(); + assert_eq!(expected.correspondences, 65); + assert_eq!(expected.squared_error_sum, 0.0); + std::thread::scope(|scope| { + let callers: Vec<_> = (0..4) + .map(|_| { + scope.spawn(|| { + for _ in 0..2 { + assert_eq!( + input + .moments_resident(&runtime, pose, 0.25) + .unwrap() + .download(&runtime) + .unwrap(), + expected + ); + } + }) + }) + .collect(); + for caller in callers { + caller.join().unwrap(); + } + }); + drop(input); + assert_eq!(retained.download(&runtime).unwrap(), expected); + assert!(retained.download(&other).is_err()); + } +} + +#[test] +fn invalid_config_empty_missing_matches_and_transform_overflow_are_explicit() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let target = tree(&[[0.0; 3]]); + for source in [vec![[f32::NAN, 0.0, 0.0]], vec![[f32::MAX, 0.0, 0.0], [-f32::MAX, 0.0, 0.0]]] { + assert!(GpuIcpInput::upload(&runtime, &source, &target).is_err()); + } + assert!(GpuIcpInput::upload(&runtime, &[[0.0; 3], [1e-22, 0.0, 0.0]], &target).is_err()); + assert!(GpuIcpInput::upload(&runtime, &[[0.0; 3]], &tree(&[])).is_err()); + let empty = GpuIcpInput::upload(&runtime, &[], &target).unwrap(); + assert_eq!( + empty + .moments_resident(&runtime, Isometry3::identity(), 0.25) + .unwrap() + .download(&runtime) + .unwrap() + .transfers + .device_to_host_bytes(), + 0 + ); + let input = GpuIcpInput::upload(&runtime, &[[100.0; 3]], &target).unwrap(); + for distance in [0.0, -1.0, f32::NAN, f32::INFINITY, f32::MAX, f32::MIN_POSITIVE] { + assert!(input.moments_resident(&runtime, Isometry3::identity(), distance).is_err()); + } + assert!(input + .moments_resident( + &runtime, + Isometry3::new(Quat::new(0.0, 0.0, 0.0, 2.0), Vec3::new(0.0, 0.0, 0.0)), + 0.25 + ) + .is_err()); + let none = input.moments_resident(&runtime, Isometry3::identity(), 0.25).unwrap(); + let words = read_words(&runtime, none.buffer()); + assert!(words.iter().all(|&v| v == 0)); + assert_eq!(none.download(&runtime).unwrap().correspondences, 0); + let extreme = GpuIcpInput::upload(&runtime, &[[f32::MAX, 0.0, 0.0]], &target).unwrap(); + let invalid = extreme + .moments_resident( + &runtime, + Isometry3::new(Quat::::identity(), Vec3::new(f32::MAX, 0.0, 0.0)), + 0.25, + ) + .unwrap(); + let words = read_words(&runtime, invalid.buffer()); + assert_ne!(words[0] & 1, 0); + assert!(words[1..].iter().all(|&v| v == 0)); + assert!(invalid.download(&runtime).is_err()); + let edge = GpuIcpInput::upload(&runtime, &[[0.25, 0.0, 0.0]], &target) + .unwrap() + .moments_resident(&runtime, Isometry3::identity(), 0.25) + .unwrap() + .download(&runtime) + .unwrap(); + assert_eq!(edge.correspondences, 1); + assert_eq!(edge.squared_error_sum, 0.0625); +} diff --git a/crates/spatialrust-gpu/src/kernels/mod.rs b/crates/spatialrust-gpu/src/kernels/mod.rs index 00992bca..eddc00f3 100644 --- a/crates/spatialrust-gpu/src/kernels/mod.rs +++ b/crates/spatialrust-gpu/src/kernels/mod.rs @@ -51,8 +51,10 @@ mod voxel_gather; pub(crate) mod cluster_union; pub(crate) mod fpfh; +pub(crate) mod icp_reduction; pub(crate) mod iss; pub(crate) mod mls; +pub use icp_reduction::{GpuIcpInput, GpuIcpMoments, GpuIcpStatistics}; pub use iss::{GpuIssInput, GpuIssKeypoints, GpuIssNeighborhoods}; pub use mls::{ GpuMlsDiagnostics, GpuMlsInput, GpuMlsMode, GpuMlsNeighborhoods, GpuMlsPositions, diff --git a/crates/spatialrust-gpu/src/kernels/nearest.rs b/crates/spatialrust-gpu/src/kernels/nearest.rs index 393bfcb9..37ca5fe4 100644 --- a/crates/spatialrust-gpu/src/kernels/nearest.rs +++ b/crates/spatialrust-gpu/src/kernels/nearest.rs @@ -139,6 +139,58 @@ pub struct GpuKdTree { } impl GpuKdTree { + // Internal device consumer: queries/results remain private to its encoder. + // Reuses the ordinary bounded nearest shader and tie traversal exactly. + pub(crate) fn encode_nearest( + &self, + runtime: &WgpuRuntime, + encoder: &mut wgpu::CommandEncoder, + queries: &wgpu::Buffer, + count: u32, + bound: f32, + results: &wgpu::Buffer, + ) -> SpatialResult<()> { + if !Arc::ptr_eq(&self.identity, &runtime.profiling_identity()) || self.root.is_none() { + return Err(invalid()); + } + check_compute(&runtime.device().limits(), u64::from(count))?; + let params = Params { count, root: self.root.unwrap(), bound, unused: 0 }; + let uniform = runtime.device().create_buffer_init(&wgpu::util::BufferInitDescriptor { + label: Some("resident-nearest-params"), + contents: bytemuck::bytes_of(¶ms), + usage: wgpu::BufferUsages::UNIFORM, + }); + let cache = runtime.nearest_pipeline.get_or_init(|| NearestPipeline::new(runtime.device())); + let buffers = [ + &uniform, + self.nodes.as_ref().unwrap(), + self.points.as_ref().unwrap(), + queries, + results, + ]; + let entries: Vec<_> = buffers + .iter() + .enumerate() + .map(|(binding, buffer)| wgpu::BindGroupEntry { + binding: binding as u32, + resource: buffer.as_entire_binding(), + }) + .collect(); + let group = runtime.device().create_bind_group(&wgpu::BindGroupDescriptor { + label: Some("resident-nearest-bindings"), + layout: &cache.layout, + entries: &entries, + }); + let mut pass = encoder.begin_compute_pass(&wgpu::ComputePassDescriptor { + label: Some("resident-nearest"), + timestamp_writes: None, + }); + pass.set_pipeline(&cache.pipeline); + pass.set_bind_group(0, &group, &[]); + pass.dispatch_workgroups(count.div_ceil(64), 1, 1); + Ok(()) + } + /// Uploads a finite CPU KD-tree, preserving its leaf and tie traversal order. /// The borrowed tree may be dropped afterwards. Upload accounting includes /// 16 bytes per point and 32 per node, excluding uniform control data. diff --git a/crates/spatialrust-gpu/src/lib.rs b/crates/spatialrust-gpu/src/lib.rs index 315b2501..0fe219fc 100644 --- a/crates/spatialrust-gpu/src/lib.rs +++ b/crates/spatialrust-gpu/src/lib.rs @@ -52,10 +52,11 @@ pub use device::{GpuDevice, WgpuDevice}; #[cfg(feature = "gpu-wgpu")] pub use kernels::{ - GpuFpfhDescriptors, GpuFpfhInput, GpuFpfhNeighborhoods, GpuIssInput, GpuIssKeypoints, - GpuIssNeighborhoods, GpuMlsDiagnostics, GpuMlsInput, GpuMlsMode, GpuMlsNeighborhoods, - GpuMlsPositions, GpuMlsReadback, GpuNeighborhoodKind, GpuNeighborhoodReadback, - GpuNeighborhoods, GpuQueryBatch, GPU_FPFH_DESCRIPTOR_LEN, MAX_GPU_K_NEIGHBORS, + GpuFpfhDescriptors, GpuFpfhInput, GpuFpfhNeighborhoods, GpuIcpInput, GpuIcpMoments, + GpuIcpStatistics, GpuIssInput, GpuIssKeypoints, GpuIssNeighborhoods, GpuMlsDiagnostics, + GpuMlsInput, GpuMlsMode, GpuMlsNeighborhoods, GpuMlsPositions, GpuMlsReadback, + GpuNeighborhoodKind, GpuNeighborhoodReadback, GpuNeighborhoods, GpuQueryBatch, + GPU_FPFH_DESCRIPTOR_LEN, MAX_GPU_K_NEIGHBORS, }; #[cfg(feature = "gpu-wgpu")] diff --git a/crates/spatialrust-gpu/src/runtime.rs b/crates/spatialrust-gpu/src/runtime.rs index 887f636e..d33a2545 100644 --- a/crates/spatialrust-gpu/src/runtime.rs +++ b/crates/spatialrust-gpu/src/runtime.rs @@ -28,6 +28,8 @@ pub struct WgpuRuntime { pub(crate) fpfh_pipelines: OnceLock, pub(crate) iss_pipelines: OnceLock, pub(crate) mls_pipelines: OnceLock, + pub(crate) icp_reduction_pipelines: + OnceLock, pub(crate) point_estimator_pipelines: OnceLock, max_gather_channels: u32, @@ -394,6 +396,7 @@ impl WgpuRuntime { fpfh_pipelines: OnceLock::new(), iss_pipelines: OnceLock::new(), mls_pipelines: OnceLock::new(), + icp_reduction_pipelines: OnceLock::new(), point_estimator_pipelines: OnceLock::new(), max_gather_channels, upload_pool: GpuBufferPool::default(), diff --git a/crates/spatialrust-gpu/src/shaders/icp_reduction.wgsl b/crates/spatialrust-gpu/src/shaders/icp_reduction.wgsl new file mode 100644 index 00000000..7a709a80 --- /dev/null +++ b/crates/spatialrust-gpu/src/shaders/icp_reduction.wgsl @@ -0,0 +1,138 @@ +struct Params { + row0: vec4, row1: vec4, row2: vec4, + source_anchor: vec4, target_anchor: vec4, + count: u32, targets: u32, groups: u32, unused: u32, + bound: f32, max_distance: f32, scale: f32, padding: f32, +} +struct Neighbor { index: u32, distance: f32 } +struct Partial { values: array,16>, count: u32, pad0: u32, pad1: u32, pad2: u32 } +struct Statistics { + status: atomic, count: u32, reserved0: u32, reserved1: u32, + source_anchor: vec4, target_anchor: vec4, values: array,16>, +} +@group(0) @binding(0) var params: Params; +@group(0) @binding(1) var source: array>; +@group(0) @binding(2) var target_points: array>; +@group(0) @binding(3) var queries: array>; +@group(0) @binding(4) var matches: array; +@group(0) @binding(5) var partials: array; +@group(0) @binding(6) var result: Statistics; +var sums: array,64>; +var counts: array; + +fn finite(v: f32) -> bool { return abs(v)<=3.4028234663852886e38; } +fn finite3(v: vec3) -> bool { return finite(v.x) && finite(v.y) && finite(v.z); } +fn dot3(a: vec3, b: vec3) -> f32 { return a.x*b.x+a.y*b.y+a.z*b.z; } +fn transform_point(p: vec3) -> vec3 { + return vec3(dot3(params.row0.xyz,p)+params.row0.w, + dot3(params.row1.xyz,p)+params.row1.w,dot3(params.row2.xyz,p)+params.row2.w); +} +fn add(a: vec2, b: f32) -> vec2 { + let s=a.x+b; let v=s-a.x; let e=(a.x-(s-v))+(b-v)+a.y; let h=s+e; + return vec2(h,e-(h-s)); +} +fn merge(a: vec2, b: vec2) -> vec2 { return add(add(a,b.x),b.y); } +fn valid_match(i: u32) -> bool { + let m=matches[i]; + if m.index==0xffffffffu { + if m.distance!=params.bound { atomicOr(&result.status,2u); } + return false; + } + if m.index>=params.targets || !(m.distance>=0.0 && m.distance<=params.bound) { + atomicOr(&result.status,2u); return false; + } + return true; +} + +@compute @workgroup_size(64) +fn transform_source(@builtin(global_invocation_id) id: vec3) { + if id.x>=params.count { return; } + let p=transform_point(source[id.x].xyz); + if !finite3(p) { atomicOr(&result.status,1u); queries[id.x]=vec4(0.0); } + else { queries[id.x]=vec4(p,0.0); } +} + +@compute @workgroup_size(64) +fn centroid_partials(@builtin(workgroup_id) group: vec3, @builtin(local_invocation_index) lane: u32) { + let term=select(group.y,15u,group.y==6u); + let anchor=transform_point(params.source_anchor.xyz); + var total=vec2(0.0); var count=0u; + for (var i=group.x*64u+lane;i0u;stride/=2u) { + if lane) { + if id.x!=0u { return; } + var count=0u; + for (var g=0u;g(transform_point(params.source_anchor.xyz),0.0); + result.target_anchor=params.target_anchor; + if !finite3(result.source_anchor.xyz) { atomicOr(&result.status,1u); } + for (var term=0u;term<16u;term+=1u) { + result.values[term]=vec2(0.0); + if term<6u || term==15u { + var total=vec2(0.0); + for (var g=0u;g0u { + if term<6u { result.values[term]=merge(vec2(total.x/f32(count),0.0),vec2(total.y/f32(count),0.0)); } + else { result.values[term]=total; } + } + } + } +} + +@compute @workgroup_size(64) +fn covariance_partials(@builtin(workgroup_id) group: vec3, @builtin(local_invocation_index) lane: u32) { + let term=group.y; let row=term/3u; let col=term%3u; + let mean_s=result.values[row].x+result.values[row].y; + let mean_t=result.values[col+3u].x+result.values[col+3u].y; + var total=vec2(0.0); + if result.count>0u && atomicLoad(&result.status)==0u { + for (var i=group.x*64u+lane;i0u;stride/=2u) { + if lane) { + if id.x!=0u { return; } + for (var term=6u;term<15u;term+=1u) { + var total=vec2(0.0); + for (var g=0u;g(0.0); result.target_anchor=vec4(0.0); + for (var term=0u;term<16u;term+=1u) { result.values[term]=vec2(0.0); } + } +} diff --git a/crates/spatialrust-registration/src/icp.rs b/crates/spatialrust-registration/src/icp.rs index ae0e8899..ab16ac06 100644 --- a/crates/spatialrust-registration/src/icp.rs +++ b/crates/spatialrust-registration/src/icp.rs @@ -211,13 +211,7 @@ impl IcpRegistration { }) } - fn align_impl( - &self, - source: &PointCloud, - target: &PointCloud, - mut observe: impl FnMut(IcpIteration), - make_search: impl FnOnce(KdTree) -> SpatialResult, - ) -> SpatialResult<(RegistrationResult, IcpStopReason, S)> { + fn validate_inputs(&self, source: &PointCloud, target: &PointCloud) -> SpatialResult<()> { if !self.trim_fraction.is_finite() || self.trim_fraction <= 0.0 || self.trim_fraction > 1.0 { return Err(SpatialError::InvalidArgument( @@ -269,6 +263,21 @@ impl IcpRegistration { } } validate_icp_transform(self.config.initial_guess)?; + Ok(()) + } + + fn align_impl( + &self, + source: &PointCloud, + target: &PointCloud, + mut observe: impl FnMut(IcpIteration), + make_search: impl FnOnce(KdTree) -> SpatialResult, + ) -> SpatialResult<(RegistrationResult, IcpStopReason, S)> { + self.validate_inputs(source, target)?; + let max_distance_squared = + self.config.max_correspondence_distance * self.config.max_correspondence_distance; + let (source_x, source_y, source_z) = source.positions3()?; + let (target_x, target_y, target_z) = target.positions3()?; let mut search = make_search(KdTree::from_slices(target_x, target_y, target_z))?; let mut transform = self.config.initial_guess; @@ -561,6 +570,12 @@ impl IcpCorrespondences for GpuCorrespondences<'_> { } } +#[cfg(feature = "register-icp-gpu")] +#[path = "icp_reduction.rs"] +mod reduction; +#[cfg(feature = "register-icp-gpu")] +pub use reduction::GpuIcpReductionDiagnostics; + #[cfg(test)] mod tests { use super::{ diff --git a/crates/spatialrust-registration/src/icp_reduction.rs b/crates/spatialrust-registration/src/icp_reduction.rs new file mode 100644 index 00000000..208c3162 --- /dev/null +++ b/crates/spatialrust-registration/src/icp_reduction.rs @@ -0,0 +1,137 @@ +use super::{ + transform_delta_magnitudes, validate_icp_transform, IcpDiagnostics, IcpIteration, + IcpRegistration, IcpStopReason, +}; +use crate::{kabsch::estimate_rigid_transform_from_moments, RegistrationResult}; +use spatialrust_core::{ + HasPositions3, PointCloud, SpatialError, SpatialResult, TransferDirection, TransferStats, +}; +use spatialrust_gpu::{GpuIcpInput, GpuIcpStatistics, WgpuRuntime}; +use spatialrust_search::KdTree; + +/// Explicit resident-transform/reduction ICP result and fixed-size transfer receipt. +#[derive(Clone, Debug, PartialEq)] +pub struct GpuIcpReductionDiagnostics { + /// Pose, convergence and per-update measurements from this GPU precision path. + pub diagnostics: IcpDiagnostics, + /// Source/target/tree upload plus fixed-size moments readbacks. + pub transfers: TransferStats, + /// Completed transformation/search/reduction evaluations (updates plus one). + pub evaluation_batches: usize, +} + +impl IcpRegistration { + /// Aligns with explicitly uploaded source/target retained across iterations. + /// GPU transforms original source points, finds bounded correspondences and + /// reduces centered cross-covariance/centroids/fitness. Only 176 moment/status + /// bytes are downloaded per evaluation; pose solving and convergence stay CPU. + /// Reuses post-update moments for the next update and final fitness. No point + /// or correspondence readback, repeated payload upload or CPU fallback occurs. + /// Requires trim_fraction=1; the existing correspondence GPU API supports + /// trimming. f32 transforms/reductions can change matches, pose, fitness and + /// stopping near numerical/geometry degeneracy compared with CPU f64 sums. + #[cfg(feature = "register-icp-gpu")] + pub fn align_with_gpu_reduction( + &self, + runtime: &WgpuRuntime, + source: &PointCloud, + target: &PointCloud, + ) -> SpatialResult { + self.validate_inputs(source, target)?; + if self.trim_fraction != 1.0 { + return Err(SpatialError::InvalidArgument("GPU ICP reduction requires trim_fraction=1; use the explicit correspondence API for trimming".to_owned())); + } + let (sx, sy, sz) = source.positions3()?; + let (tx, ty, tz) = target.positions3()?; + let source_positions: Vec<_> = (0..source.len()).map(|i| [sx[i], sy[i], sz[i]]).collect(); + let tree = KdTree::from_slices(tx, ty, tz); + let input = GpuIcpInput::upload(runtime, &source_positions, &tree)?; + let mut transfers = input.upload_transfers(); + let mut evaluation_batches = 0; + let mut evaluate = |pose| -> SpatialResult { + let statistics = input + .moments_resident(runtime, pose, self.config.max_correspondence_distance)? + .download(runtime)?; + transfers.record( + TransferDirection::DeviceToHost, + statistics.transfers.device_to_host_bytes(), + ); + evaluation_batches += 1; + Ok(statistics) + }; + let mut transform = self.config.initial_guess; + let mut moments = evaluate(transform)?; + let mut history = Vec::new(); + let mut previous_fitness = None; + let mut converged = false; + let mut stop_reason = IcpStopReason::IterationLimit; + for iteration in 1..=self.config.max_iterations { + let count = moments.correspondences as usize; + if count < self.config.min_correspondences { + return Err(SpatialError::InvalidArgument(format!( + "ICP found only {count} correspondences, minimum is {}", + self.config.min_correspondences + ))); + } + let delta = estimate_rigid_transform_from_moments( + count, + moments.source_centroid, + moments.target_centroid, + moments.cross_covariance, + ) + .ok_or_else(|| { + SpatialError::InvalidArgument("ICP failed to estimate a rigid transform".to_owned()) + })?; + validate_icp_transform(delta)?; + transform = delta.compose(transform); + validate_icp_transform(transform)?; + moments = evaluate(transform)?; + let fitness = fitness(&moments); + let (translation_delta, rotation_delta_radians) = transform_delta_magnitudes(delta); + history.push(IcpIteration { + iteration, + correspondences: count, + evaluated_correspondences: moments.correspondences as usize, + fitness, + fitness_change: previous_fitness.map(|previous| previous - fitness), + translation_delta, + rotation_delta_radians, + }); + previous_fitness = Some(fitness); + if fitness < self.criteria.fitness_epsilon { + converged = true; + stop_reason = IcpStopReason::FitnessThreshold; + break; + } + if translation_delta < self.criteria.translation_epsilon + && rotation_delta_radians < self.criteria.rotation_epsilon + { + converged = true; + stop_reason = IcpStopReason::TransformThreshold; + break; + } + } + Ok(GpuIcpReductionDiagnostics { + diagnostics: IcpDiagnostics { + result: RegistrationResult { + transform, + fitness: fitness(&moments), + iterations: history.len(), + converged, + }, + history, + stop_reason, + }, + transfers, + evaluation_batches, + }) + } +} + +fn fitness(statistics: &GpuIcpStatistics) -> f64 { + if statistics.correspondences == 0 { + f64::MAX + } else { + statistics.squared_error_sum / f64::from(statistics.correspondences) + } +} diff --git a/crates/spatialrust-registration/src/kabsch.rs b/crates/spatialrust-registration/src/kabsch.rs index 6d096acc..e99c2ce5 100644 --- a/crates/spatialrust-registration/src/kabsch.rs +++ b/crates/spatialrust-registration/src/kabsch.rs @@ -41,6 +41,20 @@ pub fn estimate_rigid_transform( } } } + estimate_rigid_transform_from_moments(source.len(), mean_source, mean_target, h) +} + +pub(crate) fn estimate_rigid_transform_from_moments( + count: usize, + mean_source: [f64; 3], + mean_target: [f64; 3], + mut h: [[f64; 3]; 3], +) -> Option> { + if count < 3 + || mean_source.iter().chain(&mean_target).chain(h.iter().flatten()).any(|v| !v.is_finite()) + { + return None; + } let scale = h.iter().flatten().fold(0.0_f64, |largest, v| largest.max(v.abs())); let rotation = if scale == 0.0 { Quat::::identity() diff --git a/crates/spatialrust-registration/src/lib.rs b/crates/spatialrust-registration/src/lib.rs index 656a721b..7e5fbda2 100644 --- a/crates/spatialrust-registration/src/lib.rs +++ b/crates/spatialrust-registration/src/lib.rs @@ -46,7 +46,7 @@ pub use fpfh::{ }; #[cfg(feature = "register-icp-gpu")] -pub use icp::GpuIcpDiagnostics; +pub use icp::{GpuIcpDiagnostics, GpuIcpReductionDiagnostics}; #[cfg(feature = "register-gicp-gpu")] pub use gicp::GpuGicpDiagnostics; diff --git a/crates/spatialrust-registration/tests/gpu_icp_reduction.rs b/crates/spatialrust-registration/tests/gpu_icp_reduction.rs new file mode 100644 index 00000000..ff70ea5a --- /dev/null +++ b/crates/spatialrust-registration/tests/gpu_icp_reduction.rs @@ -0,0 +1,213 @@ +#![cfg(all(feature = "register-icp-gpu", not(target_arch = "wasm32")))] +use spatialrust_core::{HasPositions3, PointCloud, PointCloudBuilder}; +use spatialrust_gpu::WgpuRuntime; +use spatialrust_math::{Isometry3, Quat, TransformPoint, Vec3}; +use spatialrust_registration::{ + transform_point_cloud, IcpConfig, IcpConvergenceCriteria, IcpRegistration, +}; + +pub fn fixture(n: usize, kind: usize, scale: f32) -> PointCloud { + let mut builder = PointCloudBuilder::xyz(); + for i in 0..n { + builder + .push_point([ + (i % 13) as f32 * 0.5 * scale, + (i / 13 % 11) as f32 * 0.5 * scale, + if kind == 1 { 0.0 } else { (i / 143) as f32 * 0.5 * scale }, + ]) + .unwrap(); + } + builder.build().unwrap() +} +pub fn registration(scale: f32) -> IcpRegistration { + IcpRegistration::new(IcpConfig { + max_iterations: 4, + max_correspondence_distance: 0.25 * scale, + initial_guess: Isometry3::new( + Quat::::identity(), + Vec3::new(-0.001 * scale, 0.002 * scale, 0.0), + ), + ..Default::default() + }) + .with_convergence_criteria(IcpConvergenceCriteria { + translation_epsilon: 0.0, + rotation_epsilon: 0.0, + fitness_epsilon: 0.0, + }) +} +pub fn source(target: &PointCloud, scale: f32) -> PointCloud { + transform_point_cloud( + target, + Isometry3::new( + Quat::from_axis_angle(Vec3::new(1.0, 2.0, 3.0), 0.003), + Vec3::new(0.015 * scale, -0.01 * scale, 0.005 * scale), + ), + ) + .unwrap() +} +pub fn compare_pose(source: &PointCloud, a: Isometry3, b: Isometry3, scale: f32) -> f32 { + let (x, y, z) = source.positions3().unwrap(); + let mut maximum = 0.0_f32; + for i in 0..source.len() { + let p = Vec3::new(x[i], y[i], z[i]); + let u = a.transform_point(p); + let v = b.transform_point(p); + let error = (u - v).x.abs().max((u - v).y.abs()).max((u - v).z.abs()); + maximum = maximum.max(error); + assert!(error <= 2e-5 * scale.max(1e-4), "point={i} error={error} scale={scale}"); + } + maximum +} + +#[test] +#[ignore = "requires a native wgpu adapter; run explicitly with --include-ignored"] +fn resident_icp_matches_cpu_poses_and_trace_counts_across_sizes_scales_and_unequal_clouds() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let mut maximum = 0.0_f32; + let mut points = 0; + for n in [17, 63, 64, 65, 257, 4097] { + for kind in 0..3 { + for scale in [1e-4, 1.0, 1e4] { + let cloud = fixture(n, kind, scale); + let target = fixture(n + if kind == 2 { 17 } else { 0 }, kind, scale); + let source = source(&cloud, scale); + let saved_source = source.clone(); + let saved_target = target.clone(); + let registration = registration(scale); + let expected = registration.align_with_trace(&source, &target).unwrap(); + let actual = + registration.align_with_gpu_reduction(&runtime, &source, &target).unwrap(); + maximum = maximum.max( + compare_pose( + &source, + actual.diagnostics.result.transform, + expected.result.transform, + scale, + ) / scale, + ); + assert_eq!(actual.diagnostics.result.iterations, 4); + assert!(!actual.diagnostics.result.converged); + assert_eq!(actual.diagnostics.stop_reason, expected.stop_reason); + for (a, b) in actual.diagnostics.history.iter().zip(&expected.history) { + assert_eq!( + (a.correspondences, a.evaluated_correspondences), + (b.correspondences, b.evaluated_correspondences) + ); + assert!(a.fitness.is_finite() && a.fitness >= 0.0); + } + assert_eq!(actual.evaluation_batches, 5); + assert_eq!(actual.transfers.device_to_host_bytes(), 5 * 176); + let (x, y, z) = target.positions3().unwrap(); + let tree = spatialrust_search::KdTree::from_slices(x, y, z); + assert_eq!( + actual.transfers.host_to_device_bytes(), + (n * 16 + target.len() * 32 + tree.layout().nodes().len() * 32) as u64 + ); + let (sx, sy, sz) = source.positions3().unwrap(); + let mut residual = 0.0; + for i in 0..n { + let p = actual + .diagnostics + .result + .transform + .transform_point(Vec3::new(sx[i], sy[i], sz[i])); + let neighbor = + tree.nearest_one_within(p.x, p.y, p.z, (0.25 * scale).powi(2)).unwrap(); + residual += f64::from(p.x - x[neighbor.index]).powi(2) + + f64::from(p.y - y[neighbor.index]).powi(2) + + f64::from(p.z - z[neighbor.index]).powi(2); + } + residual /= n as f64; + assert!( + (actual.diagnostics.result.fitness - residual).abs() + <= 1e-10 * f64::from(scale).powi(2) + residual * 1e-4 + ); + assert_eq!(source, saved_source); + assert_eq!(target, saved_target); + points += n; + } + } + } + eprintln!("GPU_ICP_REDUCTION compared {points} transformed points in 54 conditions; maximum scale-normalized CPU pose-coordinate difference {maximum}"); +} + +#[test] +#[ignore = "requires a native wgpu adapter; run explicitly with --include-ignored"] +fn resident_icp_keeps_validation_and_reports_unsupported_trimming_and_insufficient_matches() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let target = fixture(65, 0, 1.0); + for config in [ + IcpConfig { max_iterations: 0, ..Default::default() }, + IcpConfig { min_correspondences: 66, ..Default::default() }, + IcpConfig { max_correspondence_distance: f32::NAN, ..Default::default() }, + IcpConfig { max_correspondence_distance: f32::MAX, ..Default::default() }, + ] { + let registration = IcpRegistration::new(config); + assert_eq!( + registration.align_with_trace(&target, &target).unwrap_err().to_string(), + registration + .align_with_gpu_reduction(&runtime, &target, &target) + .unwrap_err() + .to_string() + ); + } + for fraction in [0.0, 1.1, f64::NAN] { + let registration = registration(1.0).with_trim_fraction(fraction); + assert_eq!( + registration.align_with_trace(&target, &target).unwrap_err().to_string(), + registration + .align_with_gpu_reduction(&runtime, &target, &target) + .unwrap_err() + .to_string() + ); + } + assert!(registration(1.0) + .with_trim_fraction(0.75) + .align_with_gpu_reduction(&runtime, &target, &target) + .unwrap_err() + .to_string() + .contains("trim_fraction=1")); + let far = transform_point_cloud( + &target, + Isometry3::new(Quat::::identity(), Vec3::new(100.0, 0.0, 0.0)), + ) + .unwrap(); + assert_eq!( + registration(1.0).align_with_trace(&far, &target).unwrap_err().to_string(), + registration(1.0) + .align_with_gpu_reduction(&runtime, &far, &target) + .unwrap_err() + .to_string() + ); + let converged = IcpRegistration::new(IcpConfig::default()) + .align_with_gpu_reduction(&runtime, &target, &target) + .unwrap(); + assert!(converged.diagnostics.result.converged); + assert_eq!(converged.diagnostics.result.iterations, 1); + assert_eq!(converged.evaluation_batches, 2); +} + +#[test] +#[ignore = "requires a native wgpu adapter; run explicitly with --include-ignored"] +fn resident_icp_full_calls_are_deterministic_with_concurrent_shared_runtime() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let target = fixture(257, 0, 1.0); + let source = source(&target, 1.0); + let registration = registration(1.0); + let expected = registration.align_with_gpu_reduction(&runtime, &source, &target).unwrap(); + std::thread::scope(|scope| { + let callers: Vec<_> = (0..4) + .map(|_| { + scope.spawn(|| { + assert_eq!( + registration.align_with_gpu_reduction(&runtime, &source, &target).unwrap(), + expected + ) + }) + }) + .collect(); + for caller in callers { + caller.join().unwrap(); + } + }); +} diff --git a/crates/spatialrust-registration/tests/gpu_icp_reduction_benchmark.rs b/crates/spatialrust-registration/tests/gpu_icp_reduction_benchmark.rs new file mode 100644 index 00000000..72247389 --- /dev/null +++ b/crates/spatialrust-registration/tests/gpu_icp_reduction_benchmark.rs @@ -0,0 +1,127 @@ +#![cfg(all(feature = "register-icp-gpu", not(target_arch = "wasm32")))] +#[path = "gpu_icp_reduction.rs"] +mod reference; +use spatialrust_core::{HasPositions3, PointCloud, PointCloudBuilder}; +use spatialrust_gpu::{GpuIcpInput, WgpuRuntime}; +use spatialrust_math::{Isometry3, Quat, Vec3}; +use spatialrust_registration::transform_point_cloud; +use std::time::Instant; + +fn fixture(n: usize, kind: usize) -> PointCloud { + let mut builder = PointCloudBuilder::xyz(); + for i in 0..n { + let j = if kind == 2 { i / 2 } else { i }; + let p = if kind == 1 { + [(j % 128) as f32 * 0.5, (j / 128) as f32 * 0.5, 0.0] + } else { + [(j % 32) as f32 * 0.5, (j / 32 % 32) as f32 * 0.5, (j / 1024) as f32 * 0.5] + }; + builder.push_point(p).unwrap(); + } + builder.build().unwrap() +} + +#[test] +#[ignore = "completed ICP reduction benchmark; run this exact test alone in release mode"] +fn completed_resident_icp_calls() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let info = runtime.adapter_info(); + println!("GPU_ICP_REDUCTION_ENV backend={} device_type={}", info.backend, info.device_type); + for n in [257, 4097, 16384, 65537] { + for kind in 0..3 { + let cloud = fixture(n, kind); + let target = fixture(n + if kind == 2 { 17 } else { 0 }, kind); + let source = transform_point_cloud( + &cloud, + Isometry3::new(Quat::::identity(), Vec3::new(0.03125, -0.015625, 0.0078125)), + ) + .unwrap(); + let registration = reference::registration(1.0); + let expected_cpu = registration.align_with_trace(&source, &target).unwrap(); + let expected_legacy = + registration.align_with_gpu_correspondences(&runtime, &source, &target).unwrap(); + let expected_gpu = + registration.align_with_gpu_reduction(&runtime, &source, &target).unwrap(); + assert_eq!(expected_cpu, expected_legacy.diagnostics); + let maximum = reference::compare_pose( + &source, + expected_cpu.result.transform, + expected_gpu.diagnostics.result.transform, + 1.0, + ); + assert_eq!(expected_gpu.diagnostics.result.iterations, 4); + assert_eq!(expected_gpu.evaluation_batches, 5); + let (sx, sy, sz) = source.positions3().unwrap(); + let (tx, ty, tz) = target.positions3().unwrap(); + let source_positions: Vec<_> = (0..n).map(|i| [sx[i], sy[i], sz[i]]).collect(); + let tree = spatialrust_search::KdTree::from_slices(tx, ty, tz); + let input = GpuIcpInput::upload(&runtime, &source_positions, &tree).unwrap(); + let resident_pose = expected_gpu.diagnostics.result.transform; + let expected_moments = input + .moments_resident(&runtime, resident_pose, 0.25) + .unwrap() + .download(&runtime) + .unwrap(); + for _ in 0..3 { + assert_eq!(registration.align_with_trace(&source, &target).unwrap(), expected_cpu); + assert_eq!( + registration + .align_with_gpu_correspondences(&runtime, &source, &target) + .unwrap(), + expected_legacy + ); + assert_eq!( + registration.align_with_gpu_reduction(&runtime, &source, &target).unwrap(), + expected_gpu + ); + assert_eq!( + input + .moments_resident(&runtime, resident_pose, 0.25) + .unwrap() + .download(&runtime) + .unwrap(), + expected_moments + ); + } + let (mut cpu_ms, mut legacy_ms, mut gpu_ms, mut resident_ms) = + (vec![], vec![], vec![], vec![]); + for round in 0..20 { + for offset in 0..4 { + let stage = (round + offset) % 4; + let start = Instant::now(); + match stage { + 0 => { + let output = registration.align_with_trace(&source, &target).unwrap(); + cpu_ms.push(start.elapsed().as_secs_f64() * 1000.0); + assert_eq!(output, expected_cpu); + } + 1 => { + let output = registration + .align_with_gpu_correspondences(&runtime, &source, &target) + .unwrap(); + legacy_ms.push(start.elapsed().as_secs_f64() * 1000.0); + assert_eq!(output, expected_legacy); + } + 2 => { + let output = registration + .align_with_gpu_reduction(&runtime, &source, &target) + .unwrap(); + gpu_ms.push(start.elapsed().as_secs_f64() * 1000.0); + assert_eq!(output, expected_gpu); + } + _ => { + let output = + input.moments_resident(&runtime, resident_pose, 0.25).unwrap(); + runtime.wait_idle(); + resident_ms.push(start.elapsed().as_secs_f64() * 1000.0); + assert_eq!(output.download(&runtime).unwrap(), expected_moments); + } + } + } + } + println!("GPU_ICP_REDUCTION_BENCH n={n} kind={kind} iterations=4 max_pose_error={maximum} cpu_fitness={} gpu_fitness={} matches={} legacy_h2d={} legacy_d2h={} gpu_h2d={} gpu_d2h={} cpu_ms={cpu_ms:?} legacy_ms={legacy_ms:?} gpu_ms={gpu_ms:?} resident_ms={resident_ms:?}", + expected_cpu.result.fitness,expected_gpu.diagnostics.result.fitness,expected_moments.correspondences, + expected_legacy.transfers.host_to_device_bytes(),expected_legacy.transfers.device_to_host_bytes(),expected_gpu.transfers.host_to_device_bytes(),expected_gpu.transfers.device_to_host_bytes()); + } + } +} diff --git a/crates/spatialrust/src/lib.rs b/crates/spatialrust/src/lib.rs index 6826e717..5497a76a 100644 --- a/crates/spatialrust/src/lib.rs +++ b/crates/spatialrust/src/lib.rs @@ -238,6 +238,9 @@ pub use spatialrust_registration::{ #[cfg(feature = "register-icp-point-to-plane")] pub use spatialrust_registration::{PointToPlaneIcp, PointToPlaneIcpConfig}; +#[cfg(feature = "register-icp-gpu")] +pub use spatialrust_registration::GpuIcpReductionDiagnostics; + #[cfg(feature = "register-gicp")] pub use spatialrust_registration::{GicpConfig, GicpRegistration}; diff --git a/crates/spatialrust/tests/gpu_icp_reduction_smoke.rs b/crates/spatialrust/tests/gpu_icp_reduction_smoke.rs new file mode 100644 index 00000000..15f70be6 --- /dev/null +++ b/crates/spatialrust/tests/gpu_icp_reduction_smoke.rs @@ -0,0 +1,21 @@ +#![cfg(all(feature = "register-icp-gpu", not(target_arch = "wasm32")))] +use spatialrust::{ + gpu::WgpuRuntime, GpuIcpReductionDiagnostics, IcpConfig, IcpRegistration, PointCloudBuilder, +}; + +#[test] +#[ignore = "requires a native wgpu adapter; run explicitly with --include-ignored"] +fn facade_resident_icp_returns_fixed_size_receipts_and_convergence() { + let mut builder = PointCloudBuilder::xyz(); + for i in 0..65 { + builder.push_point([(i % 5) as f32, (i / 5 % 5) as f32, (i / 25) as f32]).unwrap(); + } + let cloud = builder.build().unwrap(); + let runtime = WgpuRuntime::new_headless().unwrap(); + let result: GpuIcpReductionDiagnostics = IcpRegistration::new(IcpConfig::default()) + .align_with_gpu_reduction(&runtime, &cloud, &cloud) + .unwrap(); + assert!(result.diagnostics.result.converged); + assert_eq!(result.evaluation_batches, 2); + assert_eq!(result.transfers.device_to_host_bytes(), 352); +} diff --git a/docs/ARCHITECTURE.md b/docs/ARCHITECTURE.md index 50cdb204..acc0dafd 100644 --- a/docs/ARCHITECTURE.md +++ b/docs/ARCHITECTURE.md @@ -16,6 +16,20 @@ or dispatch sizes reject before device resource use; ordinary ICP placement is unchanged. This is a native synchronous search boundary, not a claim that the entire registration loop is device resident. +The same feature adds `align_with_gpu_reduction` for untrimmed point-to-point +ICP. `GpuIcpInput` explicitly uploads original source, original-order target +and target tree once. Six ordered passes per evaluation transform originals, +perform the unchanged bounded nearest traversal and reduce centroids/fitness +then centered cross-covariance. Actual transformed anchors, normalized +compensated f32 sums and exact counts form an owned 176-byte result; private +transformed/match/partial scratch is leased through one submission. CPU Horn's +small eigensolve, transform composition and stopping remain. Post-update moments +serve the next update and final fitness, avoiding duplicate search/readback. +There is no point/match readback, repeated payload upload or CPU fallback. +Trimming requires the existing correspondence path; f32 transforms/reductions +can change matching, pose and stopping near gates or degenerate geometry. See +[resident ICP evidence](../notes/2026-10-13_gpu_icp_reduction.md). + GICP reuses this search through its own explicit GPU entry point, retaining unbounded nearest matching before the existing host gate and ordered CPU Hessian/gradient accumulation. Radius covariance, when configured, runs on the diff --git a/docs/FEATURE_MATRIX.md b/docs/FEATURE_MATRIX.md index ec46f653..411729cd 100644 --- a/docs/FEATURE_MATRIX.md +++ b/docs/FEATURE_MATRIX.md @@ -77,6 +77,21 @@ inclusive gates and near-first tie order. Native synchronous execution is verified; browser execution is not claimed. See [correspondence measurements](../notes/2026-10-12_gpu_icp_correspondences.md). +The same `register-icp-gpu` feature adds +`IcpRegistration::align_with_gpu_reduction(runtime, source, target)`. +Original source and original-order target stay resident alongside the tree. +GPU transforms, bounded nearest matching, compensated centroid/fitness and +centered cross-covariance reductions share one six-pass submission per +evaluation; only 176 bytes of owned moments/status are downloaded. CPU Horn +pose solving and convergence remain. Post-update moments feed the next update +and final fitness. Input payload is 16*S + 32*T + 32*M bytes for source/target/ +tree-node counts; no repeated point upload or match readback occurs. The API +requires `trim_fraction=1`, finite extents and a normal positive combined +normalization extent square (coincident inputs use scale 1). Device f32 math +can change matching/pose/stopping near gates and degeneracy. `GpuIcpInput`/ +`GpuIcpMoments` under `gpu-wgpu` expose this reusable boundary directly. See +[resident ICP evidence](../notes/2026-10-13_gpu_icp_reduction.md). + `register-gicp-gpu` also exposes `GicpRegistration::align_with_gpu_correspondences(runtime, source, target)`. The device reuses a target for unbounded nearest matching, followed by GICP's @@ -117,7 +132,7 @@ before allocation; no dependency, public signature or image readback is added. | `mvp` | PCD, PLY, LAS/LAZ, COPC; voxel, normals, plane, Euclidean clustering, ICP, MVP pipeline | CPU-first end-to-end pipeline | | `mvp-http` | `mvp` + HTTP byte-range COPC | Remote COPC input | | `gpu-wgpu` | wgpu runtime and GPU kernels | Explicit GPU operation APIs | -| `register-icp-gpu` | ICP and wgpu correspondence search | Explicit GPU target upload and per-batch query/readback; CPU pose solver | +| `register-icp-gpu` | ICP GPU correspondence search and resident transforms/moment reductions | Explicit correspondence or untrimmed resident reduction API; CPU pose solver | | `register-gicp-gpu` | GICP, GPU radius covariance and explicit GPU correspondences | Caller-owned runtime and full payload receipt on explicit alignment; CPU pose solver | | `register-fpfh-gpu` | GPU radius search, SPFH and weighted FPFH | Explicit descriptor API and payload receipts; CPU matching/RANSAC | | `feature-iss-gpu` | GPU radius graphs, covariance/eigenvalue saliency and local maxima | Explicit detection/receipts; CPU tree, compaction and attribute gathering | diff --git a/docs/ROADMAP.md b/docs/ROADMAP.md index 41a72c31..cb7327f0 100644 --- a/docs/ROADMAP.md +++ b/docs/ROADMAP.md @@ -1132,4 +1132,5 @@ Dependencies and explicit transfers come before broader algorithm coverage. | FPFH descriptors | Complete | Input-bound reusable radius rows, GPU SPFH and weighted FPFH, explicit upload/readback with overflow status; CPU matching/RANSAC retained | CPU/analytic descriptor comparisons, input/ownership/overflow/error guards, native/facade/WASM builds and completed comparisons against current parallel CPU; `notes/2026-10-12_gpu_fpfh_descriptors.md` | | ISS keypoints | Complete | Shared original queries/positions, input-bound radius graphs, GPU covariance/eigenvalue saliency and local maxima; explicit flag/status download, CPU compaction/attributes | Analytic eigen/tie checks, f64 math/error-band and independent NMS references, schema/attribute bits, ownership/overflow/underflow guards and completed parallel-CPU comparisons; `notes/2026-10-13_gpu_iss_keypoints.md` | | MLS compute | Complete | Input-bound reusable radius rows, GPU weighted frames and order-0/1/2 fits, explicit position/mode/status download; CPU tree and reconstruction | Analytic quadratic/noise checks, per-mode CPU references with raw requested-order errors retained, schema/attribute bits, ownership/overflow/underflow guards and completed parallel-CPU comparisons; `notes/2026-10-13_gpu_mls_smoothing.md` | -| Registration updates | Planned | Explicit correspondence reduction/pose-update boundaries after GPU search is established | Pose/convergence/fitness references, numerical conditioning, source/target residency and end-to-end registration timing | +| ICP resident transforms/reductions | Complete | Reusable source/target/tree, GPU absolute transforms and bounded matching, centered centroid/covariance/fitness reductions; fixed moments readback with CPU Horn solve | Independent f64 moments, 54 pose/scale/unequal-cloud conditions, receipt/overflow/ownership/concurrency checks and completed CPU/previous-GPU comparisons; `notes/2026-10-13_gpu_icp_reduction.md` | +| Remaining registration updates | Planned | GPU GICP/point-to-plane Hessian reductions, trimming and small pose-update boundaries | Pose/convergence/fitness references, numerical conditioning, source/target residency and end-to-end registration timing | diff --git a/notes/2026-10-13_gpu_icp_reduction.md b/notes/2026-10-13_gpu_icp_reduction.md new file mode 100644 index 00000000..b5a9d0ed --- /dev/null +++ b/notes/2026-10-13_gpu_icp_reduction.md @@ -0,0 +1,244 @@ +# Resident ICP transforms and moment reduction + +Date: 2026-10-13. Named slice: device source transforms, bounded correspondence +search and centered ICP moments, with an explicit CPU pose-solve boundary. + +## Execution and transfer contract + +`register-icp-gpu` adds +`IcpRegistration::align_with_gpu_reduction(runtime, source, target)` alongside +the existing CPU and GPU-correspondence APIs. Original source and target points +stay on GPU throughout the call. Coordinate transforms, bounded nearest search, +centroid, centered cross-covariance and squared-error reduction run on device. +CPU retains the small Horn quaternion eigensolve, transform composition, trace +construction and convergence checks. CPU target-tree construction remains. +No point/query/match readback, repeated point upload or CPU fallback occurs. + +The new entry point requires `trim_fraction=1`. The existing +`align_with_gpu_correspondences` still supports trimming and retains its +ordered CPU sums. Shared validation preserves existing configuration/input +error order. The ordinary CPU loop and both existing numerical solvers keep +their arithmetic. This slice does not move GICP/point-to-plane Hessians, +trimming or the small pose solve to GPU. + +`GpuIcpInput::upload` under `gpu-wgpu` explicitly retains immutable original +source XYZ, original-order target XYZ and the uploaded target KD-tree. For S +source points, T target points and M tree nodes, H2D payload is +`16*S + 32*T + 32*M` bytes: source 16*S, original target 16*T, tree-ordered +target 16*T and nodes 32*M. Uniform controls are excluded. A nonempty target is +required. Empty source evaluations submit no work/readback, but target upload +still has its ordinary payload. + +`moments_resident(runtime, absolute_pose, max_distance)` applies the absolute +pose to ORIGINAL source points on each evaluation. It validates runtime, +finite unit-quaternion pose, positive finite nonzero f32 distance square, +counts, host/storage/uniform/shared-memory and dispatch limits. Both point sets +need finite XYZ/global extent squares. The maximum of their extent squares +defines normalization: a positive value must be normal in f32, while coincident +sets use scale 1. This is stricter than ordinary CPU ICP. + +Six ordered device passes share ONE submission: + +1. Transform original source into private query scratch; detect overflow. +2. Run the unchanged bounded nearest shader/traversal/tie rules into private + match scratch. The distance gate is inclusive. +3. Accumulate centroid offsets and normalized residual squared lengths in + at most 256 partial groups with 64 threads and compensated scalar sums. +4. Combine partial centroids/fitness and exact u32 counts. +5. Accumulate cross-covariance about those device centroids. +6. Combine covariance and finalize validity, zeroing ALL payload on errors. + +Bounding-box midpoint anchors and a common diagonal scale avoid summing large +absolute coordinates. Actual GPU-transformed source anchors are retained in +the result, so explicit decoding does not substitute a CPU transform's rounding. +Centered covariance is a second pass over the matched points, rather than +subtracting two large uncentered moments. Normalized residuals use the maximum +correspondence distance as their scale. Compensated hi/lo f32 pairs survive +partial combination and are explicitly decoded to f64 physical units on CPU. +Products, division and transforms remain f32; compensation does not supply +f64 arithmetic or guarantee accuracy on all conditioning regimes. + +Private scratch is 16*S transformed bytes, 8*S match bytes and 144 bytes per +partial group (at most 36,864). Consumed scratch words are initialized before +use, remain exclusively leased through submission and then return to the +existing bounded pool. Pipelines are lazy per runtime. Reduction uses six +storage bindings, a 112-byte uniform, 64 threads and 768 shared bytes per +workgroup. The existing nearest pipeline is reused without shader changes. + +`GpuIcpMoments` owns a 176-byte storage buffer independently of the input and +pool. Its first four u32 words are status, correspondence count and two reserved +zeros. Status bits 1/2/4 indicate transformed-coordinate overflow, malformed +match rows and nonfinite moments; any nonzero status invalidates the result. +The following two vec4 records contain source/target anchors, then sixteen +vec2 records contain compensated normalized moments. Terms 0..5 are source/ +target centroid offsets, 6..14 centered cross-covariance in row-major order, +and 15 residual squared-error SUM. `normalization_scale` and +`squared_distance_scale` expose the required physical scales to GPU consumers. +Invalid or zero-match results have all-zero payload with status retained. +Consumers must inspect status and treat exposed storage as immutable. + +`download` copies only 176 D2H bytes for a nonempty source, waits, checks status, +count/reserved words, finite payload and zero-match invariants, then returns +`GpuIcpStatistics` with f64 physical centroids/cross-covariance/error sum and a +transfer receipt. Empty source readback transfers zero bytes. There is no +intermediate transfer or host completion wait in `moments_resident`. + +The registration wrapper evaluates the initial pose once. Every update uses +the current moments for its CPU solve and evaluates the updated absolute pose +once. Post-update moments serve fitness, the next update and final fitness. +I updates therefore need I+1 evaluations and `176*(I+1)` D2H bytes. The existing +correspondence path uses 2*I+1 query/readback batches, including its final fitness +pass. Reuse reduces duplicate matching as well as transfer volume; timing gains +are not attributed solely to the reduction kernel. + +## Precision and correctness evidence + +This is an opt-in f32 device precision path, not a CPU pose/fitness/trace parity +promise. Near match gates, ties, degenerate geometry or stopping thresholds, +transforms and reductions can change matches, updates, fitness and stopping. +Collinear/coincident points do not certify an observable pose. Existing CPU +and GPU-correspondence routes retain their f64 sums and remain available. + +Four primitive tests cover counts 1/3/63/64/65/257/4097/16385. Independent f64 +centroid, all nine centered covariance entries and squared-error sums are +checked. The selected covariance bound is +`1e-7 + 1e-6*sum(abs(centered_source_component*centered_target_component))`. +This handles zero/near-zero cross entries and is not a universal error guarantee. +Other checks cover exact inclusive gate boundaries, zero matches, empty source, +invalid limits/coordinates/extent/radius/quaternion, transform overflow status +and complete payload zeroing. Translation fixtures at origins 0 and 1e6, +four concurrent callers with scratch reuse, retained outputs after input drop +and wrong-runtime rejection pass. + +Three explicitly run adapter integration tests compare 54 combinations of +17/63/64/65/257/4097 points, volume/plane/unequal-cloud geometry and scales +1e-4/1/1e4. Poses are compared by transforming all 41,067 input points, with +maximum scale-normalized CPU coordinate difference 0.0000014305115 under the +selected bound 2e-5*scale. All selected trace correspondence counts and four +updates agree. Fitness is checked separately against CPU nearest/residual math +at the returned pose; receipts verify five fixed-size readbacks and one input +upload. Inputs remain unchanged. Tests also check unsupported trimming, shared +validation errors, insufficient matches, one-update convergence and concurrent +complete calls. + +GPU all-features regression: 211 passed / 23 ignored. Registration library: +35 passed / 1 ignored. Three existing GPU-correspondence adapter tests and one +new facade adapter test pass. All-feature GPU/registration and CPU-only ICP +Clippy, minimal GPU build, WASM feature compilation and formatting pass. +Source comparison against `57afc8958172fa5e1c3059d5cd355a90cc586397` verifies +unchanged CPU centroid/covariance/Horn/eigensolver arithmetic, validation order +and update/fitness/trimming loops after extracting shared helpers. Native +Vulkan execution is verified; WASM evidence is compilation only. Hosted CI +does not replace native adapter evidence. No core/math/search dependency or +unsafe code is added. + +## Completed-call measurements + +Two isolated release surveys on native Vulkan / discrete-device backend use +three warmups and 20 paired rounds, rotating CPU/previous-GPU/new-GPU/resident +stage order. No other local builds, tests or statistical analysis run during +timing. Every sample and condition is retained. Runtime/pipeline setup is warmed +outside timers; repeated outputs are checked against their separate references. + +All complete APIs use FOUR updates with stopping thresholds disabled, the +same initial guess and distance gate 0.25. Full CPU and previous-GPU timings +include their ordinary tree construction, updates, matching, fitness and +trace. New GPU timings include CPU tree/input preparation, one explicit input +upload, five complete device evaluations/readbacks, CPU pose solves and trace. +Verification and returned-output destruction are outside timers; internal +preparation/staging destruction is included. Different schedules (nine versus +five evaluations) and precision paths are part of the complete-call comparison. + +V is a 32x32-column volume grid with spacing 0.5; P is a 128-column plane +without coordinate repetition; D duplicates consecutive V points and has 17 +extra target rows. Source is translated by (0.03125,-0.015625,0.0078125), with +initial guess (-0.001,0.002,0). Before timing, pose coordinates are compared +with CPU; raw coordinate errors, fitness, counts and transfer bytes accompany +measurements. These synthetic fixtures do not establish a universal crossover. + +Resident timings measure one prepared final-pose evaluation, including owned +output allocation, scratch leasing, transform/search/reductions, submission +and `wait_idle`. They exclude upload, tree construction, moment readback, +CPU solving and the iterative registration loop. These are not GPU timestamps +or whole-registration timings. + +Times are medians in milliseconds. Ratios are geometric means of 20 paired +baseline/new-GPU timings. Percentile 95% intervals use 10,000 paired-log +bootstrap resamples, sorted entries 249/9749. Seeds are 20261013 + 2*zero-based +record index + baseline index (CPU=0, previous GPU=1). + +| Survey | Source/target | Fixture | CPU ms | Previous GPU ms | New GPU ms | Resident ms | Max pose-coordinate error | CPU/new ratio (95% interval) | Previous/new ratio (95% interval) | +| --- | ---: | :---: | ---: | ---: | ---: | ---: | ---: | --- | --- | +| 1 | 257/257 | V | 0.365 | 10.134 | 6.105 | 0.255 | 2.38e-07 | 0.06x (0.06-0.06) | 1.68x (1.65-1.71) | +| 1 | 257/257 | P | 0.355 | 11.819 | 6.946 | 0.296 | 9.54e-07 | 0.05x (0.05-0.06) | 1.67x (1.60-1.74) | +| 1 | 257/274 | D | 0.638 | 13.039 | 7.886 | 0.373 | 2.38e-07 | 0.08x (0.07-0.08) | 1.66x (1.61-1.71) | +| 1 | 4,097/4,097 | V | 13.019 | 15.215 | 8.525 | 0.448 | 2.38e-07 | 1.52x (1.39-1.67) | 1.86x (1.79-1.93) | +| 1 | 4,097/4,097 | P | 8.299 | 17.004 | 8.316 | 0.778 | 9.54e-07 | 1.00x (0.90-1.11) | 2.02x (1.93-2.11) | +| 1 | 4,097/4,114 | D | 10.053 | 16.615 | 8.134 | 0.947 | 2.38e-07 | 1.17x (1.07-1.26) | 2.00x (1.88-2.12) | +| 1 | 16,384/16,384 | V | 54.937 | 34.945 | 18.671 | 2.336 | 2.38e-07 | 3.04x (2.82-3.27) | 1.90x (1.85-1.96) | +| 1 | 16,384/16,384 | P | 39.513 | 32.346 | 16.764 | 2.052 | 9.54e-07 | 2.29x (2.08-2.51) | 1.91x (1.84-1.98) | +| 1 | 16,384/16,401 | D | 49.409 | 34.491 | 18.183 | 2.380 | 2.38e-07 | 2.79x (2.57-3.04) | 1.91x (1.87-1.95) | +| 1 | 65,537/65,537 | V | 234.455 | 87.491 | 52.197 | 6.960 | 2.38e-07 | 5.01x (4.37-5.88) | 1.84x (1.71-2.00) | +| 1 | 65,537/65,537 | P | 222.101 | 84.039 | 29.773 | 1.280 | 1.91e-06 | 6.40x (5.65-7.20) | 2.49x (2.27-2.73) | +| 1 | 65,537/65,554 | D | 570.734 | 171.998 | 78.240 | 4.182 | 2.38e-07 | 7.14x (6.55-7.80) | 2.31x (2.12-2.55) | +| 2 | 257/257 | V | 0.709 | 18.968 | 11.550 | 0.429 | 2.38e-07 | 0.06x (0.05-0.06) | 1.61x (1.55-1.68) | +| 2 | 257/257 | P | 0.609 | 13.687 | 8.251 | 0.341 | 9.54e-07 | 0.07x (0.06-0.07) | 1.65x (1.59-1.69) | +| 2 | 257/274 | D | 0.605 | 12.946 | 7.665 | 0.324 | 2.38e-07 | 0.05x (0.03-0.06) | 1.82x (1.67-2.00) | +| 2 | 4,097/4,097 | V | 23.983 | 55.101 | 29.400 | 2.544 | 2.38e-07 | 0.77x (0.69-0.84) | 1.81x (1.63-1.96) | +| 2 | 4,097/4,097 | P | 23.594 | 63.538 | 34.077 | 2.932 | 9.54e-07 | 0.70x (0.65-0.75) | 1.89x (1.73-2.05) | +| 2 | 4,097/4,114 | D | 25.240 | 60.154 | 31.701 | 2.424 | 2.38e-07 | 0.76x (0.67-0.85) | 1.82x (1.68-1.98) | +| 2 | 16,384/16,384 | V | 134.104 | 71.462 | 34.411 | 1.958 | 2.38e-07 | 3.68x (3.36-4.01) | 2.00x (1.81-2.21) | +| 2 | 16,384/16,384 | P | 95.460 | 83.798 | 38.794 | 3.688 | 9.54e-07 | 2.60x (2.40-2.83) | 2.12x (1.99-2.29) | +| 2 | 16,384/16,401 | D | 111.749 | 60.845 | 29.861 | 1.097 | 2.38e-07 | 3.71x (3.28-4.15) | 1.94x (1.71-2.20) | +| 2 | 65,537/65,537 | V | 274.733 | 86.622 | 37.928 | 2.060 | 2.38e-07 | 6.76x (5.87-7.68) | 2.22x (2.08-2.37) | +| 2 | 65,537/65,537 | P | 206.214 | 81.448 | 31.785 | 1.226 | 1.91e-06 | 5.59x (4.52-6.64) | 2.21x (1.87-2.54) | +| 2 | 65,537/65,554 | D | 295.489 | 87.686 | 45.119 | 3.082 | 2.38e-07 | 6.45x (5.09-7.95) | 1.99x (1.64-2.36) | + +The new completed route improves over the previous GPU-correspondence route +in all 24 selected conditions: paired ratios 1.61-2.49x, with all reported +intervals above 1. Against CPU, 257-point calls are slower (0.05-0.08x); +4,097-point conditions vary between 0.70-1.52x across surveys, so no stable +CPU crossover is claimed there. At 16,384 points ratios are 2.29-3.71x; +at 65,537 points 5.01-7.14x. The largest selected benchmark CPU pose-coordinate +difference is 0.0000019073486, all final match counts equal S, and all APIs +perform four updates. Raw CPU/GPU fitness values remain in the local JSON; +small positive residuals can differ despite close poses. Substantial timing +variation is retained, including small-call regressions and the second survey's +4,097-point CPU advantage. This supports an explicit option, without changing +ordinary placement/defaults or promising arbitrary-input accuracy/performance. + +Transfer payloads below are bytes for complete four-update calls, identical +across surveys. Uniform controls are excluded. At 65,537 source points D2H +falls from 4,718,664 to 880 bytes (about 99.98% lower). + +| Source/target | Fixture | Previous H2D | New H2D | Previous D2H | New D2H | +| ---: | :---: | ---: | ---: | ---: | ---: | +| 257/257 | V | 42,176 | 13,392 | 18,504 | 880 | +| 257/257 | P | 42,176 | 13,392 | 18,504 | 880 | +| 257/274 | D | 43,408 | 14,896 | 18,504 | 880 | +| 4,097/4,097 | V | 671,936 | 213,072 | 294,984 | 880 | +| 4,097/4,097 | P | 671,936 | 213,072 | 294,984 | 880 | +| 4,097/4,114 | D | 673,296 | 214,704 | 294,984 | 880 | +| 16,384/16,384 | V | 2,686,944 | 851,936 | 1,179,648 | 880 | +| 16,384/16,384 | P | 2,686,944 | 851,936 | 1,179,648 | 880 | +| 16,384/16,401 | D | 2,688,304 | 853,568 | 1,179,648 | 880 | +| 65,537/65,537 | V | 10,748,096 | 3,407,952 | 4,718,664 | 880 | +| 65,537/65,537 | P | 10,748,096 | 3,407,952 | 4,718,664 | 880 | +| 65,537/65,554 | D | 10,749,456 | 3,409,584 | 4,718,664 | 880 | + + +Reproduction from `C:/Users/rsasa/Workspace/SpatialRust`: + +```powershell +cargo test -p spatialrust-gpu --all-features +cargo test -p spatialrust-registration --all-features --test gpu_icp_reduction -- --include-ignored --nocapture +cargo test -p spatialrust-registration --all-features --test gpu_icp -- --include-ignored +cargo test -p spatialrust --no-default-features --features register-icp-gpu --test gpu_icp_reduction_smoke -- --include-ignored +cargo test -p spatialrust-registration --all-features --release --test gpu_icp_reduction_benchmark completed_resident_icp_calls -- --include-ignored --nocapture --exact --test-threads=1 +``` + +The benchmark exact filter excludes its imported adapter tests. Benchmarks +and integration adapter tests are ignored by default; explicit runs fail +without an adapter rather than silently skipping. Raw surveys, validation and +JSON are at `C:/Users/rsasa/Workspace/SpatialRust/target/automation/gpu-icp-reduction-*.log` +and `C:/Users/rsasa/Workspace/SpatialRust/target/automation/gpu-icp-reduction-measurements.json`.