diff --git a/CHANGELOG.md b/CHANGELOG.md index f92ece19..830a68b8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -21,6 +21,14 @@ removed no sooner than the next major (see `docs/API_STABILITY.md`). ### Changed +- Add explicit resident GPU ICP trimming under `register-icp-gpu`. Stable + distance/source-index selection retains floor(trim_fraction*gated_count) + pairs for moments while all gated pairs still define fitness/stopping. + Integer device arithmetic preserves binary64 fraction rounding and floor; + no point/match readback, repeated payload upload or CPU sorting is required. + Each evaluation downloads 176 bytes, and fraction one uses canonical + untrimmed kernels. CPU target-tree/Horn solving and convergence remain. + - Add explicit bounded parallel KD-tree construction under search `parallel` and facade `search-parallel`. Disjoint preorder node/point ranges retain exact canonical partition order and search ties, without unsafe, per-worker trees diff --git a/crates/spatialrust-gpu/src/kernels/icp_reduction.rs b/crates/spatialrust-gpu/src/kernels/icp_reduction.rs index a3265744..fb6cfb59 100644 --- a/crates/spatialrust-gpu/src/kernels/icp_reduction.rs +++ b/crates/spatialrust-gpu/src/kernels/icp_reduction.rs @@ -8,6 +8,9 @@ use wgpu::util::DeviceExt; const RESULT_BYTES: u64 = 176; +#[path = "icp_trim.rs"] +pub(crate) mod trim; + #[repr(C)] #[derive(Clone, Copy, Pod, Zeroable)] struct Params { @@ -161,6 +164,40 @@ impl GpuIcpInput { runtime: &WgpuRuntime, transform: Isometry3, max_distance: f32, + ) -> SpatialResult { + self.moments_impl(runtime, transform, max_distance, None) + } + + /// Submits stable GPU selection of the lowest-distance floor(fraction*N) + /// gated pairs, breaking distance ties by original source index. Moments + /// use retained pairs; fitness uses ALL gated pairs. Binary64 fraction + /// multiplication/floor is emulated with integer limbs on the device. + /// No match/point payload readback or host wait occurs. A fraction of one + /// delegates to the canonical untrimmed kernels; no CPU fallback occurs. + pub fn trimmed_moments_resident( + &self, + runtime: &WgpuRuntime, + transform: Isometry3, + max_distance: f32, + fraction: f64, + ) -> SpatialResult { + if !fraction.is_finite() || fraction <= 0.0 || fraction > 1.0 { + return Err(SpatialError::InvalidArgument( + "trim_fraction must be finite and in (0, 1]".to_owned(), + )); + } + let trimmed = fraction < 1.0; + let inner = + self.moments_impl(runtime, transform, max_distance, trimmed.then_some(fraction))?; + Ok(GpuIcpTrimmedMoments { inner, trimmed }) + } + + fn moments_impl( + &self, + runtime: &WgpuRuntime, + transform: Isometry3, + max_distance: f32, + fraction: Option, ) -> SpatialResult { let q = transform.rotation(); let t = transform.translation(); @@ -180,7 +217,11 @@ impl GpuIcpInput { return Err(invalid()); } let count = self.source_count(); - let groups = plan(u64::from(count), &runtime.device().limits())?; + let groups = if fraction.is_some() { + trim::plan_trim(u64::from(count), &runtime.device().limits())? + } else { + plan(u64::from(count), &runtime.device().limits())? + }; let output = GpuIcpMoments { buffer: runtime.device().create_buffer(&wgpu::BufferDescriptor { label: Some("icp-moments"), @@ -223,10 +264,18 @@ impl GpuIcpInput { 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 = [ + let selection = fraction.map(|_| scratch.acquire("icp-trim-selection", 32)); + let cache = if fraction.is_some() { + &runtime + .icp_trim_pipelines + .get_or_init(|| trim::TrimPipelines::new(runtime.device())) + .reduction + } else { + runtime + .icp_reduction_pipelines + .get_or_init(|| IcpReductionPipelines::new(runtime.device())) + }; + let mut buffers = vec![ &uniform, self.source.buffer().unwrap(), self.target.buffer().unwrap(), @@ -235,6 +284,9 @@ impl GpuIcpInput { &partials, &output.buffer, ]; + if let Some(selection) = &selection { + buffers.push(selection); + } let entries: Vec<_> = buffers .iter() .enumerate() @@ -251,6 +303,17 @@ impl GpuIcpInput { 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)?; + if let Some(fraction) = fraction { + trim::encode_selection( + runtime, + &mut encoder, + &mut scratch, + ¶ms, + fraction, + &matches, + selection.as_ref().unwrap(), + ); + } 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); } @@ -278,7 +341,8 @@ fn dispatch( /// 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. + /// Pairs contributing to centroids/covariance; ordinary moments use every + /// gated match. Zero yields zero centroids/covariance. pub correspondences: u32, /// Arithmetic source centroid after applying the requested transform. pub source_centroid: [f64; 3], @@ -286,7 +350,8 @@ pub struct GpuIcpStatistics { 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). + /// Sum of squared matched residual lengths (not a mean). In the containing + /// trimmed statistics, use its all-gated count as the fitness denominator. pub squared_error_sum: f64, /// Payload copied by this explicit download, independent of input upload. pub transfers: TransferStats, @@ -338,6 +403,14 @@ impl GpuIcpMoments { /// 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 { + self.download_impl(runtime, false).map(|(statistics, _)| statistics) + } + + fn download_impl( + &self, + runtime: &WgpuRuntime, + trimmed: bool, + ) -> SpatialResult<(GpuIcpStatistics, u32)> { if !Arc::ptr_eq(&self.identity, &runtime.profiling_identity()) { return Err(invalid()); } @@ -369,10 +442,14 @@ impl GpuIcpMoments { staging.unmap(); transfers.record(TransferDirection::DeviceToHost, RESULT_BYTES); } + let evaluated = if trimmed { words[2] } else { words[1] }; if words[0] != 0 - || words[1] > self.count - || words[2..4] != [0, 0] - || (words[1] == 0 && words[4..].iter().any(|&v| v != 0)) + || words[1] > evaluated + || evaluated > self.count + || words[3] != 0 + || (!trimmed && words[2] != 0) + || (evaluated == 0 && words[4..].iter().any(|&v| v != 0)) + || (words[1] == 0 && words[4..42].iter().any(|&v| v != 0)) { return Err(SpatialError::InvalidArgument(format!( "GPU ICP reduction invalid status/count: {:?}", @@ -400,14 +477,69 @@ impl GpuIcpMoments { if squared_error_sum < 0.0 { return Err(invalid()); } - Ok(GpuIcpStatistics { - correspondences: words[1], - source_centroid, - target_centroid, - cross_covariance, - squared_error_sum, - transfers, - }) + Ok(( + GpuIcpStatistics { + correspondences: words[1], + source_centroid, + target_centroid, + cross_covariance, + squared_error_sum, + transfers, + }, + evaluated, + )) + } +} + +/// Fixed-size selected moments plus the all-gated fitness denominator. +#[derive(Clone, Debug, PartialEq)] +pub struct GpuIcpTrimmedStatistics { + /// Retained-pair centroids/covariance, ALL-gated residual sum, and readback receipt. + pub moments: GpuIcpStatistics, + /// Number of all gated matches used for fitness (before trimming). + pub evaluated_correspondences: u32, +} + +/// Owned 176-byte trimmed moments/status; no point or match readback is implicit. +pub struct GpuIcpTrimmedMoments { + inner: GpuIcpMoments, + trimmed: bool, +} + +impl GpuIcpTrimmedMoments { + /// Device storage layout matches ordinary moments, except word 2 is the + /// all-gated count. Word 1 is retained count. Terms 0..14 use retained pairs; + /// term 15 uses every gated pair. Zero retained count zeros terms 0..14 and + /// anchors but preserves all-gated fitness. Any invalid status zeros payload. + /// Fraction one retains ordinary moments' two reserved zeros. + #[must_use] + pub fn buffer(&self) -> &wgpu::Buffer { + self.inner.buffer() + } + + /// Explicit fixed readback size, zero for empty source and 176 otherwise. + #[must_use] + pub const fn readback_bytes(&self) -> u64 { + self.inner.readback_bytes() + } + + /// Physical coordinate scale for normalized centroid/covariance words. + #[must_use] + pub const fn normalization_scale(&self) -> f32 { + self.inner.normalization_scale() + } + + /// Physical squared-distance scale for normalized all-gated residual sum. + #[must_use] + pub const fn squared_distance_scale(&self) -> f32 { + self.inner.squared_distance_scale() + } + + /// Explicitly copies/waits/validates only the fixed-size moments and counts. + pub fn download(&self, runtime: &WgpuRuntime) -> SpatialResult { + let (moments, evaluated_correspondences) = + self.inner.download_impl(runtime, self.trimmed)?; + Ok(GpuIcpTrimmedStatistics { moments, evaluated_correspondences }) } } @@ -417,7 +549,11 @@ pub(crate) struct IcpReductionPipelines { } impl IcpReductionPipelines { fn new(device: &wgpu::Device) -> Self { - let entries: Vec<_> = (0..7) + Self::with_shader(device, include_str!("../shaders/icp_reduction.wgsl"), false) + } + + fn with_shader(device: &wgpu::Device, shader_source: &str, trimmed: bool) -> Self { + let entries: Vec<_> = (0..if trimmed { 8 } else { 7 }) .map(|binding| wgpu::BindGroupLayoutEntry { binding, visibility: wgpu::ShaderStages::COMPUTE, @@ -425,7 +561,9 @@ impl IcpReductionPipelines { ty: if binding == 0 { wgpu::BufferBindingType::Uniform } else { - wgpu::BufferBindingType::Storage { read_only: matches!(binding, 1 | 2 | 4) } + wgpu::BufferBindingType::Storage { + read_only: matches!(binding, 1 | 2 | 4 | 7), + } }, has_dynamic_offset: false, min_binding_size: None, @@ -444,7 +582,7 @@ impl IcpReductionPipelines { }); let shader = device.create_shader_module(wgpu::ShaderModuleDescriptor { label: Some("icp-reduction-shader"), - source: wgpu::ShaderSource::Wgsl(include_str!("../shaders/icp_reduction.wgsl").into()), + source: wgpu::ShaderSource::Wgsl(shader_source.into()), }); let pipelines = [ "transform_source", diff --git a/crates/spatialrust-gpu/src/kernels/icp_trim.rs b/crates/spatialrust-gpu/src/kernels/icp_trim.rs new file mode 100644 index 00000000..de1bae62 --- /dev/null +++ b/crates/spatialrust-gpu/src/kernels/icp_trim.rs @@ -0,0 +1,197 @@ +//! Private resident stable distance/index selection for ICP updates. +use super::*; + +pub(super) const SHADER: &str = include_str!("../shaders/icp_trim.wgsl"); + +#[repr(C)] +#[derive(Clone, Copy, Pod, Zeroable)] +pub(super) struct TrimParams { + count: u32, + groups: u32, + step: u32, + mantissa_lo: u32, + mantissa_hi: u32, + shift: u32, + bound: f32, + targets: u32, +} + +pub(super) fn plan_trim(count: u64, limits: &wgpu::Limits) -> SpatialResult { + let groups = plan(count, limits)?; + let bound = limits.max_buffer_size.min(u64::from(limits.max_storage_buffer_binding_size)); + if u64::from(groups) * 1024 > bound + || limits.max_storage_buffers_per_shader_stage < 7 + || limits.max_bindings_per_bind_group < 8 + || limits.max_compute_workgroup_storage_size < 2048 + { + return Err(invalid()); + } + Ok(groups) +} + +impl TrimParams { + fn new(count: u32, groups: u32, fraction: f64, bound: f32, targets: u32) -> Self { + let bits = fraction.to_bits(); + let exponent = ((bits >> 52) & 2047) as u32; + let mantissa = (bits & ((1u64 << 52) - 1)) | if exponent == 0 { 0 } else { 1u64 << 52 }; + Self { + count, + groups, + step: 0, + mantissa_lo: mantissa as u32, + mantissa_hi: (mantissa >> 32) as u32, + shift: if exponent == 0 { 1074 } else { 1075 - exponent }, + bound, + targets, + } + } +} + +pub(crate) struct TrimPipelines { + pub(super) reduction: IcpReductionPipelines, + layout: wgpu::BindGroupLayout, + histogram: wgpu::ComputePipeline, + select: wgpu::ComputePipeline, +} + +impl TrimPipelines { + pub(super) fn new(device: &wgpu::Device) -> Self { + let entries: Vec<_> = (0..4) + .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: binding == 1 } + }, + has_dynamic_offset: false, + min_binding_size: None, + }, + count: None, + }) + .collect(); + let layout = device.create_bind_group_layout(&wgpu::BindGroupLayoutDescriptor { + label: Some("icp-trim-layout"), + entries: &entries, + }); + let pipeline_layout = device.create_pipeline_layout(&wgpu::PipelineLayoutDescriptor { + label: Some("icp-trim-layout"), + bind_group_layouts: &[&layout], + push_constant_ranges: &[], + }); + let shader = device.create_shader_module(wgpu::ShaderModuleDescriptor { + label: Some("icp-trim-shader"), + source: wgpu::ShaderSource::Wgsl(SHADER.into()), + }); + let [histogram, select] = ["histogram_prefix", "select_digit"].map(|entry| { + device.create_compute_pipeline(&wgpu::ComputePipelineDescriptor { + label: Some("icp-trim-pipeline"), + layout: Some(&pipeline_layout), + module: &shader, + entry_point: Some(entry), + compilation_options: Default::default(), + cache: None, + }) + }); + Self { + reduction: IcpReductionPipelines::with_shader(device, &trimmed_shader(), true), + layout, + histogram, + select, + } + } + + pub(super) fn encode( + &self, + runtime: &WgpuRuntime, + encoder: &mut wgpu::CommandEncoder, + scratch: &mut ScratchBuffers<'_>, + params: TrimParams, + matches: &wgpu::Buffer, + selection: &wgpu::Buffer, + ) { + let histograms = scratch.acquire("icp-trim-histograms", u64::from(params.groups) * 1024); + encoder.clear_buffer(selection, 0, None); + // Source indices have known leading zero bytes; do not dispatch those + // digits. Distance still uses all four bytes, without a quantized cutoff. + let first_index_step = 4 + params.count.saturating_sub(1).leading_zeros() / 8; + for step in (0..4).chain(first_index_step..8) { + let uniform = runtime.device().create_buffer_init(&wgpu::util::BufferInitDescriptor { + label: Some("icp-trim-params"), + contents: bytemuck::bytes_of(&TrimParams { step, ..params }), + usage: wgpu::BufferUsages::UNIFORM, + }); + let entries: Vec<_> = [&uniform, matches, &histograms, selection] + .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-trim-bindings"), + layout: &self.layout, + entries: &entries, + }); + dispatch(encoder, &self.histogram, &group, params.groups, 1); + dispatch(encoder, &self.select, &group, 1, 1); + } + } +} + +pub(super) fn encode_selection( + runtime: &WgpuRuntime, + encoder: &mut wgpu::CommandEncoder, + scratch: &mut ScratchBuffers<'_>, + params: &Params, + fraction: f64, + matches: &wgpu::Buffer, + selection: &wgpu::Buffer, +) { + runtime.icp_trim_pipelines.get().unwrap().encode( + runtime, + encoder, + scratch, + TrimParams::new(params.count, params.groups, fraction, params.bound, params.targets), + matches, + selection, + ); +} + +fn replace_once(shader: &mut String, from: &str, to: &str) { + assert_eq!(shader.matches(from).count(), 1, "ICP trim shader anchor changed: {from}"); + *shader = shader.replacen(from, to, 1); +} + +// Keep the untrimmed numerical kernels canonical; change only selection/count +// semantics. Assertions make later edits to the shared shader fail explicitly. +fn trimmed_shader() -> String { + let mut shader = include_str!("../shaders/icp_reduction.wgsl").replace("\r\n", "\n"); + shader.push_str("\nstruct Selection { gated:u32, retained:u32, rank:u32, distance:u32, source_index:u32, unused0:u32, unused1:u32, unused2:u32 }\n@group(0) @binding(7) var selection: Selection;\nfn retained_match(i:u32)->bool { let distance=bitcast(matches[i].distance)&0x7fffffffu; return selection.retained>0u && (distance0u {\n if term<6u", + "if count>0u || term==15u {\n if term<6u", + ); + replace_once(&mut shader, "if atomicLoad(&result.status)!=0u || result.count==0u {", "if result.count==0u { result.source_anchor=vec4(0.0); result.target_anchor=vec4(0.0); }\n if atomicLoad(&result.status)!=0u || result.reserved0==0u {"); + replace_once( + &mut shader, + "result.count=0u; result.source_anchor=", + "result.count=0u; result.reserved0=0u; result.source_anchor=", + ); + shader +} + +#[cfg(all(test, not(target_arch = "wasm32")))] +#[path = "icp_trim_tests.rs"] +mod tests; diff --git a/crates/spatialrust-gpu/src/kernels/icp_trim_tests.rs b/crates/spatialrust-gpu/src/kernels/icp_trim_tests.rs new file mode 100644 index 00000000..95e1c6aa --- /dev/null +++ b/crates/spatialrust-gpu/src/kernels/icp_trim_tests.rs @@ -0,0 +1,517 @@ +use super::*; +use spatialrust_math::{Quat, Vec3}; + +fn read(runtime: &WgpuRuntime, buffer: &wgpu::Buffer, bytes: u64) -> Vec { + let staging = runtime.device().create_buffer(&wgpu::BufferDescriptor { + label: Some("trim-test-readback"), + size: 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, 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 +} + +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]) +} + +#[test] +fn trim_plan_and_canonical_shader_anchors_are_checked() { + assert_eq!(std::mem::size_of::(), 32); + let limits = wgpu::Limits::default(); + assert_eq!(plan_trim(65, &limits).unwrap(), 2); + for case in 0..4 { + let mut limits = limits.clone(); + match case { + 0 => limits.max_storage_buffers_per_shader_stage = 6, + 1 => limits.max_bindings_per_bind_group = 7, + 2 => limits.max_compute_workgroup_storage_size = 2047, + _ => limits.max_storage_buffer_binding_size = 2047, + } + assert!(plan_trim(65, &limits).is_err()); + } + assert!(trimmed_shader().contains("result.reserved0=selection.gated")); +} + +#[test] +fn integer_fraction_floor_matches_binary64_for_rounding_boundaries_and_large_counts() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let cache = TrimPipelines::new(runtime.device()); + let mut counts: Vec = (0..4097).collect(); + for bit in 1..32 { + for delta in -3i64..=3 { + counts.push(((1i64 << bit) + delta).clamp(0, i64::from(u32::MAX)) as u32); + } + } + counts.extend([u32::MAX, u32::MAX - 1, u32::MAX - 2]); + let mut seed = 20261013u32; + for _ in 0..10000 { + seed = seed.wrapping_mul(1664525).wrapping_add(1013904223); + counts.push(seed); + } + let matches: Vec<[u32; 2]> = counts.iter().map(|&n| [n, 0]).collect(); + let input = runtime.device().create_buffer_init(&wgpu::util::BufferInitDescriptor { + label: None, + contents: bytemuck::cast_slice(&matches), + usage: wgpu::BufferUsages::STORAGE, + }); + let output = runtime.device().create_buffer(&wgpu::BufferDescriptor { + label: None, + size: counts.len() as u64 * 4, + usage: wgpu::BufferUsages::STORAGE | wgpu::BufferUsages::COPY_SRC, + mapped_at_creation: false, + }); + let selection = runtime.device().create_buffer(&wgpu::BufferDescriptor { + label: None, + size: 32, + usage: wgpu::BufferUsages::STORAGE, + mapped_at_creation: false, + }); + let shader = runtime.device().create_shader_module(wgpu::ShaderModuleDescriptor { + label: None, + source: wgpu::ShaderSource::Wgsl(format!("{SHADER}\n@compute @workgroup_size(64) fn verify_fraction(@builtin(global_invocation_id) id:vec3) {{ if id.x = [&uniform, &input, &output, &selection] + .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: None, + layout: &cache.layout, + entries: &entries, + }); + let mut encoder = runtime.device().create_command_encoder(&Default::default()); + dispatch(&mut encoder, &pipeline, &group, (counts.len() as u32).div_ceil(64), 1); + runtime.queue().submit(Some(encoder.finish())); + let actual = read(&runtime, &output, output.size()); + for (&n, &k) in counts.iter().zip(&actual) { + assert_eq!(k, (f64::from(n) * fraction).floor() as u32, "n={n} fraction={fraction:?}"); + } + } +} + +#[test] +fn stable_radix_selection_matches_independent_sort_with_ties_missing_and_boundaries() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let cache = TrimPipelines::new(runtime.device()); + for n in [1, 3, 63, 64, 65, 257, 4097, 16385, 65537] { + for kind in 0..3 { + let matches: Vec<[u32; 2]> = (0..n) + .map(|i| { + let distance = match kind { + 0 => ((i * 7919 % 1024) as f32) / 1024.0, + 1 => 0.25, + _ => [ + 0.0, + -0.0, + f32::from_bits(1), + f32::from_bits(0x007fffff), + 0.5, + 1.0, + f32::from_bits(0x3f7fffff), + ][i % 7], + }; + if kind != 1 && i % 7 == 0 { + [u32::MAX, 1.0f32.to_bits()] + } else { + [(i % 31) as u32, distance.to_bits()] + } + }) + .collect(); + let mut order: Vec<_> = matches + .iter() + .enumerate() + .filter(|(_, m)| m[0] != u32::MAX) + .map(|(i, m)| (if f32::from_bits(m[1]) == 0.0 { 0 } else { m[1] }, i as u32)) + .collect(); + order.sort_unstable(); + let input = runtime.device().create_buffer_init(&wgpu::util::BufferInitDescriptor { + label: None, + contents: bytemuck::cast_slice(&matches), + usage: wgpu::BufferUsages::STORAGE, + }); + for fraction in [0.1, 0.3, 0.5, 0.9, f64::from_bits(1.0f64.to_bits() - 1)] { + let mut scratch = ScratchBuffers::new(&runtime); + let selection = scratch.acquire("trim-test-selection", 32); + let mut encoder = runtime.device().create_command_encoder(&Default::default()); + cache.encode( + &runtime, + &mut encoder, + &mut scratch, + TrimParams::new( + n as u32, + plan_trim(n as u64, &runtime.device().limits()).unwrap(), + fraction, + 1.0, + 31, + ), + &input, + &selection, + ); + // Copy before scratch recycling; do not retain a pooled output. + let owned = runtime.device().create_buffer(&wgpu::BufferDescriptor { + label: None, + size: 32, + usage: wgpu::BufferUsages::COPY_DST | wgpu::BufferUsages::COPY_SRC, + mapped_at_creation: false, + }); + encoder.copy_buffer_to_buffer(&selection, 0, &owned, 0, 32); + scratch.submit_and_recycle(encoder); + let actual = read(&runtime, &owned, 32); + let k = (order.len() as f64 * fraction).floor() as usize; + assert_eq!((actual[0] as usize, actual[1] as usize), (order.len(), k)); + if k > 0 { + assert_eq!( + (actual[3], actual[4]), + order[k - 1], + "n={n} kind={kind} fraction={fraction}" + ); + let chosen: Vec<_> = order + .iter() + .copied() + .filter(|&pair| pair <= (actual[3], actual[4])) + .collect(); + assert_eq!(chosen, order[..k]); + } + } + } + } +} + +#[test] +fn trimmed_moments_preserve_all_gated_fitness_and_stable_input_order() { + let runtime = WgpuRuntime::new_headless().unwrap(); + for n in [3, 63, 64, 65, 257, 4097] { + let source: Vec<_> = (0..n).map(|i| [0.125, [0.0, 0.0625, -0.0625][i % 3], 0.0]).collect(); + let input = GpuIcpInput::upload(&runtime, &source, &tree(&[[0.0; 3]])).unwrap(); + let all = input + .moments_resident(&runtime, Isometry3::identity(), 0.25) + .unwrap() + .download(&runtime) + .unwrap(); + for fraction in [0.1, 0.3, 0.5, 0.8, 1.0] { + let actual = input + .trimmed_moments_resident(&runtime, Isometry3::identity(), 0.25, fraction) + .unwrap() + .download(&runtime) + .unwrap(); + let mut order: Vec<_> = (0..n).collect(); + order.sort_unstable_by_key(|&i| { + ((source[i][0] * source[i][0] + source[i][1] * source[i][1]).to_bits(), i) + }); + let k = (n as f64 * fraction).floor() as usize; + assert_eq!(actual.moments.correspondences as usize, k); + assert_eq!(actual.evaluated_correspondences as usize, n); + assert_eq!(actual.moments.squared_error_sum, all.squared_error_sum); + assert_eq!(actual.moments.transfers.device_to_host_bytes(), 176); + for (a, ¢roid) in actual.moments.source_centroid.iter().enumerate() { + let expected = if k == 0 { + 0.0 + } else { + order[..k].iter().map(|&i| f64::from(source[i][a])).sum::() / k as f64 + }; + assert!((centroid - expected).abs() < 1e-8); + } + assert_eq!(actual.moments.target_centroid, [0.0; 3]); + assert_eq!(actual.moments.cross_covariance, [[0.0; 3]; 3]); + if fraction == 1.0 { + assert_eq!(actual.moments, all); + } + } + } +} + +#[test] +fn selected_centroids_and_cross_covariance_match_independent_f64_math() { + let runtime = WgpuRuntime::new_headless().unwrap(); + for n in [17, 63, 64, 65, 257, 4097] { + for scale in [1e-4_f32, 1.0, 1e4] { + let target: Vec<_> = (0..n + 17) + .map(|i| { + [ + (i % 13) as f32 * 0.5 * scale, + (i / 13 % 11) as f32 * 0.5 * scale, + (i / 143) as f32 * 0.5 * scale, + ] + }) + .collect(); + let source: Vec<_> = target[..n] + .iter() + .enumerate() + .map(|(i, p)| { + [ + p[0] + [0.015625, 0.03125, 0.125][i % 3] * scale, + p[1] - 0.015625 * scale, + p[2] + 0.0078125 * scale, + ] + }) + .collect(); + let target_tree = tree(&target); + let input = GpuIcpInput::upload(&runtime, &source, &target_tree).unwrap(); + // GPU distance words define ordering on this precision path. Read + // them only in this oracle, verify known scalar membership, then + // independently comparison-sort and accumulate in f64. + let (neighbors, _) = input + .tree + .nearest_one_within_download(&runtime, &source, (0.25 * scale).powi(2)) + .unwrap(); + let mut pairs: Vec<_> = neighbors + .iter() + .enumerate() + .filter_map(|(i, m)| { + m.map(|m| { + assert_eq!(m.index, i); + (m.distance_squared, i, m.index) + }) + }) + .collect(); + pairs.sort_unstable_by(|a, b| a.0.total_cmp(&b.0).then(a.1.cmp(&b.1))); + for fraction in [0.3, 0.5, 0.8] { + let actual = input + .trimmed_moments_resident( + &runtime, + Isometry3::identity(), + 0.25 * scale, + fraction, + ) + .unwrap() + .download(&runtime) + .unwrap(); + let k = (pairs.len() as f64 * fraction).floor() as usize; + assert_eq!(actual.moments.correspondences as usize, k); + assert_eq!(actual.evaluated_correspondences as usize, pairs.len()); + let mean_s: [f64; 3] = std::array::from_fn(|a| { + pairs[..k].iter().map(|p| f64::from(source[p.1][a])).sum::() / k as f64 + }); + let mean_t: [f64; 3] = std::array::from_fn(|a| { + pairs[..k].iter().map(|p| f64::from(target[p.2][a])).sum::() / k as f64 + }); + for a in 0..3 { + assert!( + (actual.moments.source_centroid[a] - mean_s[a]).abs() + < 1e-5 * f64::from(scale) + ); + assert!( + (actual.moments.target_centroid[a] - mean_t[a]).abs() + < 1e-5 * f64::from(scale) + ); + for b in 0..3 { + let products: Vec<_> = pairs[..k] + .iter() + .map(|p| { + (f64::from(source[p.1][a]) - mean_s[a]) + * (f64::from(target[p.2][b]) - mean_t[b]) + }) + .collect(); + let expected = products.iter().sum::(); + let absolute = products.iter().map(|v| v.abs()).sum::(); + assert!( + (actual.moments.cross_covariance[a][b] - expected).abs() + < 1e-7 * f64::from(scale).powi(2) + 1e-6 * absolute, + "n={n} scale={scale} fraction={fraction} a={a} b={b}" + ); + } + } + let all_error = pairs + .iter() + .map(|p| { + (0..3) + .map(|a| f64::from(source[p.1][a] - target[p.2][a]).powi(2)) + .sum::() + }) + .sum::(); + assert!( + (actual.moments.squared_error_sum - all_error).abs() + < 1e-7 * f64::from(scale).powi(2) + 1e-6 * all_error + ); + } + } + } +} + +#[test] +fn trimmed_ownership_concurrency_empty_gate_and_invalid_inputs_are_explicit() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let other = WgpuRuntime::new_headless().unwrap(); + let points: Vec<_> = (0..65) + .map(|i| [(i % 5) as f32 * 0.5, (i / 5 % 5) as f32 * 0.5, (i / 25) as f32 * 0.5]) + .collect(); + let input = GpuIcpInput::upload(&runtime, &points, &tree(&points)).unwrap(); + for fraction in [0.0, -1.0, 1.1, f64::NAN, f64::INFINITY] { + assert!(input + .trimmed_moments_resident(&runtime, Isometry3::identity(), 0.25, fraction) + .is_err()); + } + assert!(input.trimmed_moments_resident(&other, Isometry3::identity(), 0.25, 0.5).is_err()); + let retained = + input.trimmed_moments_resident(&runtime, Isometry3::identity(), 0.25, 0.5).unwrap(); + let expected = retained.download(&runtime).unwrap(); + std::thread::scope(|scope| { + let callers: Vec<_> = (0..4) + .map(|_| { + scope.spawn(|| { + for _ in 0..2 { + assert_eq!( + input + .trimmed_moments_resident( + &runtime, + Isometry3::identity(), + 0.25, + 0.5 + ) + .unwrap() + .download(&runtime) + .unwrap(), + expected + ); + } + }) + }) + .collect(); + for caller in callers { + caller.join().unwrap(); + } + }); + let far = input + .trimmed_moments_resident( + &runtime, + Isometry3::new(Quat::::identity(), Vec3::new(100.0, 0.0, 0.0)), + 0.25, + 0.5, + ) + .unwrap() + .download(&runtime) + .unwrap(); + assert_eq!(far.evaluated_correspondences, 0); + assert_eq!(far.moments.source_centroid, [0.0; 3]); + assert_eq!(far.moments.squared_error_sum, 0.0); + let empty = GpuIcpInput::upload(&runtime, &[], &tree(&points)) + .unwrap() + .trimmed_moments_resident(&runtime, Isometry3::identity(), 0.25, 0.5) + .unwrap() + .download(&runtime) + .unwrap(); + assert_eq!(empty.evaluated_correspondences, 0); + assert_eq!(empty.moments.transfers.device_to_host_bytes(), 0); + drop(input); + assert_eq!(retained.download(&runtime).unwrap(), expected); + assert!(retained.download(&other).is_err()); +} + +#[test] +fn trimmed_gate_overflow_and_malformed_fixed_readbacks_reject_all_payload() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let input = + GpuIcpInput::upload(&runtime, &[[0.25, 0.0, 0.0], [0.125, 0.0, 0.0]], &tree(&[[0.0; 3]])) + .unwrap(); + let edge = input + .trimmed_moments_resident(&runtime, Isometry3::identity(), 0.25, 0.5) + .unwrap() + .download(&runtime) + .unwrap(); + assert_eq!(edge.evaluated_correspondences, 2); + assert_eq!(edge.moments.correspondences, 1); + assert_eq!(edge.moments.source_centroid, [0.125, 0.0, 0.0]); + assert_eq!(edge.moments.squared_error_sum, 0.078125); + for distance in [0.0, -1.0, f32::NAN, f32::INFINITY, f32::MAX, f32::MIN_POSITIVE] { + assert!(input + .trimmed_moments_resident(&runtime, Isometry3::identity(), distance, 0.5) + .is_err()); + } + let extreme = + GpuIcpInput::upload(&runtime, &[[f32::MAX, 0.0, 0.0]], &tree(&[[0.0; 3]])).unwrap(); + let invalid = extreme + .trimmed_moments_resident( + &runtime, + Isometry3::new(Quat::::identity(), Vec3::new(f32::MAX, 0.0, 0.0)), + 0.25, + 0.5, + ) + .unwrap(); + let words = read(&runtime, invalid.buffer(), 176); + assert_ne!(words[0] & 1, 0); + assert!(words[1..].iter().all(|&v| v == 0)); + assert!(invalid.download(&runtime).is_err()); + for case in 0..7 { + let mut words = [0u32; 44]; + match case { + 0 => words[0] = 2, + 1 => { + words[1] = 3; + words[2] = 2; + } + 2 => words[2] = 4, + 3 => words[3] = 1, + 4 => words[42] = 1.0f32.to_bits(), + 5 => { + words[2] = 2; + words[4] = 1.0f32.to_bits(); + } + _ => { + words[2] = 2; + words[42] = f32::NAN.to_bits(); + } + } + let buffer = runtime.device().create_buffer_init(&wgpu::util::BufferInitDescriptor { + label: None, + contents: bytemuck::cast_slice(&words), + usage: wgpu::BufferUsages::STORAGE | wgpu::BufferUsages::COPY_SRC, + }); + let result = GpuIcpTrimmedMoments { + inner: GpuIcpMoments { + buffer, + count: 3, + scale: 1.0, + bound: 1.0, + identity: runtime.profiling_identity(), + }, + trimmed: true, + }; + assert!(result.download(&runtime).is_err(), "malformed case {case}"); + } +} diff --git a/crates/spatialrust-gpu/src/kernels/mod.rs b/crates/spatialrust-gpu/src/kernels/mod.rs index 3991a18b..e7052ca9 100644 --- a/crates/spatialrust-gpu/src/kernels/mod.rs +++ b/crates/spatialrust-gpu/src/kernels/mod.rs @@ -60,7 +60,9 @@ pub(crate) mod icp_reduction; pub use gicp_reduction::{GpuGicpEquations, GpuGicpInput, GpuGicpNormalEquations}; pub(crate) mod iss; pub(crate) mod mls; -pub use icp_reduction::{GpuIcpInput, GpuIcpMoments, GpuIcpStatistics}; +pub use icp_reduction::{ + GpuIcpInput, GpuIcpMoments, GpuIcpStatistics, GpuIcpTrimmedMoments, GpuIcpTrimmedStatistics, +}; pub use iss::{GpuIssInput, GpuIssKeypoints, GpuIssNeighborhoods}; pub use mls::{ GpuMlsDiagnostics, GpuMlsInput, GpuMlsMode, GpuMlsNeighborhoods, GpuMlsPositions, diff --git a/crates/spatialrust-gpu/src/lib.rs b/crates/spatialrust-gpu/src/lib.rs index 5a7481a3..cca83cad 100644 --- a/crates/spatialrust-gpu/src/lib.rs +++ b/crates/spatialrust-gpu/src/lib.rs @@ -53,11 +53,12 @@ pub use device::{GpuDevice, WgpuDevice}; #[cfg(feature = "gpu-wgpu")] pub use kernels::{ GpuFpfhDescriptors, GpuFpfhInput, GpuFpfhNeighborhoods, GpuGicpEquations, GpuGicpInput, - GpuGicpNormalEquations, GpuIcpInput, GpuIcpMoments, GpuIcpStatistics, GpuIssInput, - GpuIssKeypoints, GpuIssNeighborhoods, GpuMlsDiagnostics, GpuMlsInput, GpuMlsMode, - GpuMlsNeighborhoods, GpuMlsPositions, GpuMlsReadback, GpuNeighborhoodKind, - GpuNeighborhoodReadback, GpuNeighborhoods, GpuPointToPlaneEquations, GpuPointToPlaneInput, - GpuPointToPlaneNormalEquations, GpuQueryBatch, GPU_FPFH_DESCRIPTOR_LEN, MAX_GPU_K_NEIGHBORS, + GpuGicpNormalEquations, GpuIcpInput, GpuIcpMoments, GpuIcpStatistics, GpuIcpTrimmedMoments, + GpuIcpTrimmedStatistics, GpuIssInput, GpuIssKeypoints, GpuIssNeighborhoods, GpuMlsDiagnostics, + GpuMlsInput, GpuMlsMode, GpuMlsNeighborhoods, GpuMlsPositions, GpuMlsReadback, + GpuNeighborhoodKind, GpuNeighborhoodReadback, GpuNeighborhoods, GpuPointToPlaneEquations, + GpuPointToPlaneInput, GpuPointToPlaneNormalEquations, 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 1cf09eca..2ab8b8ec 100644 --- a/crates/spatialrust-gpu/src/runtime.rs +++ b/crates/spatialrust-gpu/src/runtime.rs @@ -30,6 +30,7 @@ pub struct WgpuRuntime { pub(crate) mls_pipelines: OnceLock, pub(crate) icp_reduction_pipelines: OnceLock, + pub(crate) icp_trim_pipelines: OnceLock, pub(crate) point_to_plane_reduction_pipelines: OnceLock, pub(crate) gicp_covariance_pipelines: @@ -405,6 +406,7 @@ impl WgpuRuntime { iss_pipelines: OnceLock::new(), mls_pipelines: OnceLock::new(), icp_reduction_pipelines: OnceLock::new(), + icp_trim_pipelines: OnceLock::new(), gicp_reduction_pipelines: OnceLock::new(), gicp_covariance_pipelines: OnceLock::new(), gicp_grid_covariance_pipelines: OnceLock::new(), diff --git a/crates/spatialrust-gpu/src/shaders/icp_trim.wgsl b/crates/spatialrust-gpu/src/shaders/icp_trim.wgsl new file mode 100644 index 00000000..270be573 --- /dev/null +++ b/crates/spatialrust-gpu/src/shaders/icp_trim.wgsl @@ -0,0 +1,120 @@ +// Stable MSD selection of (nonnegative distance, original source index). +// Only the selected prefix is counted; matches never leave the device. +struct Params { + count: u32, groups: u32, step: u32, mantissa_lo: u32, + mantissa_hi: u32, shift: u32, bound: f32, targets: u32, +} +struct Neighbor { index: u32, distance: f32 } +struct Selection { + gated: u32, retained: u32, rank: u32, distance: u32, + source_index: u32, unused0: u32, unused1: u32, unused2: u32, +} +@group(0) @binding(0) var params: Params; +@group(0) @binding(1) var matches: array; +@group(0) @binding(2) var histograms: array; +@group(0) @binding(3) var selection: Selection; +var histogram: array,256>; +var totals: array; + +fn multiply_wide(a: u32, b: u32) -> vec2 { + let a0=a&65535u; let a1=a>>16u; let b0=b&65535u; let b1=b>>16u; + let w0=a0*b0; let t=a1*b0+(w0>>16u); + let w1=a0*b1+(t&65535u); + return vec2((w1<<16u)|(w0&65535u),a1*b1+(t>>16u)+(w1>>16u)); +} + +// Emulate floor(f64(count) * fraction), including the binary64 product's +// round-to-nearest-even before floor. Casting the fraction to f32 changes K. +fn retained_count(count: u32) -> u32 { + let lo=multiply_wide(count,params.mantissa_lo); + let hi=multiply_wide(count,params.mantissa_hi); + let middle=lo.y+hi.x; + var p=vec3(lo.x,middle,hi.y+select(0u,1u,middle53u { + let discarded_bits=bits-53u; // At most 32 bits for a u32 count. + var mask=0xffffffffu; + var unit=0u; + var odd=(p.y&1u)!=0u; + if discarded_bits<32u { unit=1u<half || (remainder==half && odd); + p.x=p.x&~mask; + if up { + let before=p.x; p.x+=unit; + if discarded_bits==32u || p.x=52. Subnormal fractions retain zero. + if params.shift>=96u { return 0u; } + if params.shift>=64u { return p.z>>(params.shift-64u); } + return (p.y>>(params.shift-32u))|(p.z<<(64u-params.shift)); +} + +fn distance_key(distance: f32) -> u32 { + return bitcast(distance)&0x7fffffffu; +} + +@compute @workgroup_size(64) +fn histogram_prefix(@builtin(workgroup_id) group: vec3, @builtin(local_invocation_index) lane: u32) { + for (var digit=lane;digit<256u;digit+=64u) { atomicStore(&histogram[digit],0u); } + workgroupBarrier(); + let shift=24u-(params.step%4u)*8u; + for (var i=group.x*64u+lane;i=params.targets || !(m.distance>=0.0 && m.distance<=params.bound) { continue; } + let distance=distance_key(m.distance); + var key=distance; + var prefix=selection.distance; + if params.step>=4u { + if distance!=selection.distance { continue; } + key=i; prefix=selection.source_index; + } + if params.step!=0u { + if selection.retained==0u { continue; } + // Avoid the undefined/implementation-dependent shift by 32. + if shift<24u && (key>>(shift+8u))!=(prefix>>(shift+8u)) { continue; } + } + atomicAdd(&histogram[(key>>shift)&255u],1u); + } + workgroupBarrier(); + for (var digit=lane;digit<256u;digit+=64u) { + histograms[group.x*256u+digit]=atomicLoad(&histogram[digit]); + } +} + +@compute @workgroup_size(64) +fn select_digit(@builtin(local_invocation_index) lane: u32) { + for (var digit=lane;digit<256u;digit+=64u) { + var total=0u; + for (var g=0u;g0u); + } + if selection.retained==0u { return; } + var preceding=0u; + for (var digit=0u;digit<256u;digit+=1u) { + if selection.rank-preceding SpatialResult { + self.align_reduction_impl(runtime, source, target, false) + } + + /// Aligns using GPU stable distance/source-index trimming and resident + /// moments. floor(trim_fraction * gated_count) pairs contribute to each + /// pose solve. Post-update fitness/stopping use ALL gated pairs. Only 176 + /// bytes cross to CPU per evaluation; source/target/search/selection remain + /// resident. CPU target-tree construction, Horn solve and convergence remain. + /// A fraction of one uses the canonical untrimmed GPU evaluation. f32 device + /// matching/moments can differ from CPU near gates, ties or degeneracy. + #[cfg(feature = "register-icp-gpu")] + pub fn align_with_gpu_trimmed_reduction( + &self, + runtime: &WgpuRuntime, + source: &PointCloud, + target: &PointCloud, + ) -> SpatialResult { + self.align_reduction_impl(runtime, source, target, true) + } + + fn align_reduction_impl( + &self, + runtime: &WgpuRuntime, + source: &PointCloud, + target: &PointCloud, + trimmed: bool, ) -> SpatialResult { self.validate_inputs(source, target)?; - if self.trim_fraction != 1.0 { + if !trimmed && 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()?; @@ -48,13 +75,18 @@ impl IcpRegistration { 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 mut evaluate = |pose| -> SpatialResult { let statistics = input - .moments_resident(runtime, pose, self.config.max_correspondence_distance)? + .trimmed_moments_resident( + runtime, + pose, + self.config.max_correspondence_distance, + self.trim_fraction, + )? .download(runtime)?; transfers.record( TransferDirection::DeviceToHost, - statistics.transfers.device_to_host_bytes(), + statistics.moments.transfers.device_to_host_bytes(), ); evaluation_batches += 1; Ok(statistics) @@ -66,18 +98,25 @@ impl IcpRegistration { let mut converged = false; let mut stop_reason = IcpStopReason::IterationLimit; for iteration in 1..=self.config.max_iterations { - let count = moments.correspondences as usize; + let evaluated = moments.evaluated_correspondences as usize; + let count = moments.moments.correspondences as usize; + if evaluated < self.config.min_correspondences { + return Err(SpatialError::InvalidArgument(format!( + "ICP found only {evaluated} correspondences, minimum is {}", + self.config.min_correspondences + ))); + } if count < self.config.min_correspondences { return Err(SpatialError::InvalidArgument(format!( - "ICP found only {count} correspondences, minimum is {}", + "trim_fraction retains only {count} of {evaluated} correspondences, minimum is {}", self.config.min_correspondences ))); } let delta = estimate_rigid_transform_from_moments( count, - moments.source_centroid, - moments.target_centroid, - moments.cross_covariance, + moments.moments.source_centroid, + moments.moments.target_centroid, + moments.moments.cross_covariance, ) .ok_or_else(|| { SpatialError::InvalidArgument("ICP failed to estimate a rigid transform".to_owned()) @@ -91,7 +130,7 @@ impl IcpRegistration { history.push(IcpIteration { iteration, correspondences: count, - evaluated_correspondences: moments.correspondences as usize, + evaluated_correspondences: moments.evaluated_correspondences as usize, fitness, fitness_change: previous_fitness.map(|previous| previous - fitness), translation_delta, @@ -128,10 +167,10 @@ impl IcpRegistration { } } -fn fitness(statistics: &GpuIcpStatistics) -> f64 { - if statistics.correspondences == 0 { +fn fitness(statistics: &GpuIcpTrimmedStatistics) -> f64 { + if statistics.evaluated_correspondences == 0 { f64::MAX } else { - statistics.squared_error_sum / f64::from(statistics.correspondences) + statistics.moments.squared_error_sum / f64::from(statistics.evaluated_correspondences) } } diff --git a/crates/spatialrust-registration/tests/gpu_icp_trimmed_reduction.rs b/crates/spatialrust-registration/tests/gpu_icp_trimmed_reduction.rs new file mode 100644 index 00000000..3b2cf455 --- /dev/null +++ b/crates/spatialrust-registration/tests/gpu_icp_trimmed_reduction.rs @@ -0,0 +1,277 @@ +#![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::{IcpConfig, IcpConvergenceCriteria, IcpRegistration, IcpStopReason}; +use spatialrust_search::KdTree; + +pub fn fixture(n: usize, profile: usize, scale: f32, source: bool) -> PointCloud { + let mut builder = PointCloudBuilder::xyz(); + for i in 0..n { + let j = if profile == 2 { i / 2 } else { i }; + let mut p = [(j % 32) as f32 * 0.5, (j / 32 % 32) as f32 * 0.5, (j / 1024) as f32 * 0.5]; + if source { + // Distinct inlier/outlier residuals; well separated nearest targets. + let residual = if i % 7 == 0 { 0.1875 } else { 0.03125 }; + p[0] += residual; + p[1] -= 0.015625; + p[2] += 0.0078125; + if profile == 1 && i % 11 == 0 { + p[2] += 0.375; + } + } + builder.push_point(p.map(|v| v * scale)).unwrap(); + } + builder.build().unwrap() +} + +pub fn registration(scale: f32, fraction: f64, stop: usize) -> IcpRegistration { + IcpRegistration::new(IcpConfig { + max_iterations: 3, + max_correspondence_distance: 0.25 * scale, + ..Default::default() + }) + .with_trim_fraction(fraction) + .with_convergence_criteria(IcpConvergenceCriteria { + translation_epsilon: if stop == 2 { 1e10 } else { 0.0 }, + rotation_epsilon: if stop == 2 { 1e10 } else { 0.0 }, + fitness_epsilon: if stop == 1 { 1e30 } else { 0.0 }, + }) +} + +pub fn pose_error(source: &PointCloud, a: Isometry3, b: Isometry3) -> f32 { + let (x, y, z) = source.positions3().unwrap(); + (0..source.len()) + .map(|i| { + let p = Vec3::new(x[i], y[i], z[i]); + let d = a.transform_point(p) - b.transform_point(p); + d.x.abs().max(d.y.abs()).max(d.z.abs()) + }) + .fold(0.0, f32::max) +} + +pub fn independent_fitness( + source: &PointCloud, + target: &PointCloud, + pose: Isometry3, + distance: f32, +) -> (usize, f64) { + let (sx, sy, sz) = source.positions3().unwrap(); + let (tx, ty, tz) = target.positions3().unwrap(); + let tree = KdTree::from_slices(tx, ty, tz); + let (mut count, mut sum) = (0, 0.0); + for i in 0..source.len() { + let p = pose.transform_point(Vec3::new(sx[i], sy[i], sz[i])); + if let Some(neighbor) = tree.nearest_one_within(p.x, p.y, p.z, distance * distance) { + count += 1; + sum += f64::from(p.x - tx[neighbor.index]).powi(2) + + f64::from(p.y - ty[neighbor.index]).powi(2) + + f64::from(p.z - tz[neighbor.index]).powi(2); + } + } + (count, if count == 0 { f64::MAX } else { sum / count as f64 }) +} + +#[test] +#[ignore = "requires a native wgpu adapter; run explicitly with --include-ignored"] +fn resident_trimmed_icp_matches_pose_counts_fitness_and_stopping_across_profiles_scales() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let (mut maximum, mut conditions, mut points) = (0.0_f32, 0, 0); + for n in [17, 63, 64, 65, 257, 4097] { + for profile in 0..3 { + for scale in [1e-4, 1.0, 1e4] { + let source = fixture(n, profile, scale, true); + let target = fixture(n + 17, profile, scale, false); + let saved_source = source.clone(); + let saved_target = target.clone(); + for fraction in [0.5, 0.8, 1.0] { + for stop in 0..3 { + let registration = registration(scale, fraction, stop); + let cpu = registration.align_with_trace(&source, &target).unwrap(); + let gpu = registration + .align_with_gpu_trimmed_reduction(&runtime, &source, &target) + .unwrap(); + let error = pose_error( + &source, + cpu.result.transform, + gpu.diagnostics.result.transform, + ) / scale; + maximum = maximum.max(error); + assert!(error<3e-5,"n={n} profile={profile} scale={scale} fraction={fraction} stop={stop} error={error}"); + assert_eq!( + gpu.diagnostics.stop_reason, + [ + IcpStopReason::IterationLimit, + IcpStopReason::FitnessThreshold, + IcpStopReason::TransformThreshold + ][stop] + ); + assert_eq!(gpu.diagnostics.stop_reason, cpu.stop_reason); + assert_eq!(gpu.diagnostics.result.iterations, cpu.result.iterations); + assert_eq!(gpu.diagnostics.result.converged, cpu.result.converged); + for (a, b) in gpu.diagnostics.history.iter().zip(&cpu.history) { + assert_eq!( + (a.correspondences, a.evaluated_correspondences), + (b.correspondences, b.evaluated_correspondences) + ); + } + let (count, fitness) = independent_fitness( + &source, + &target, + gpu.diagnostics.result.transform, + 0.25 * scale, + ); + assert_eq!( + count, + gpu.diagnostics.history.last().unwrap().evaluated_correspondences + ); + assert!( + (fitness - gpu.diagnostics.result.fitness).abs() + <= 1e-10 * f64::from(scale).powi(2) + fitness * 1e-4 + ); + assert_eq!(gpu.evaluation_batches, gpu.diagnostics.result.iterations + 1); + assert_eq!( + gpu.transfers.device_to_host_bytes(), + 176 * gpu.evaluation_batches as u64 + ); + let (x, y, z) = target.positions3().unwrap(); + let tree = KdTree::from_slices(x, y, z); + assert_eq!( + gpu.transfers.host_to_device_bytes(), + (16 * n + 32 * target.len() + 32 * tree.layout().nodes().len()) as u64 + ); + if fraction == 1.0 { + assert_eq!( + gpu, + registration + .align_with_gpu_reduction(&runtime, &source, &target) + .unwrap() + ); + } + conditions += 1; + points += n; + } + } + assert_eq!(source, saved_source); + assert_eq!(target, saved_target); + } + } + } + eprintln!("GPU_ICP_TRIMMED compared {conditions} conditions / {points} source points; maximum scale-normalized CPU pose-coordinate difference {maximum}"); +} + +#[test] +#[ignore = "requires a native wgpu adapter; run explicitly with --include-ignored"] +fn resident_trimmed_validation_insufficient_retained_and_shared_runtime_are_preserved() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let target = fixture(65, 0, 1.0, false); + for fraction in [0.0, 1.1, f64::NAN, f64::INFINITY, f64::from_bits(1), 0.001] { + let registration = registration(1.0, fraction, 0); + assert_eq!( + registration.align_with_trace(&target, &target).unwrap_err().to_string(), + registration + .align_with_gpu_trimmed_reduction(&runtime, &target, &target) + .unwrap_err() + .to_string() + ); + } + 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() }, + IcpConfig { + initial_guess: Isometry3::new(Quat::new(0.0, 0.0, 0.0, 2.0), Vec3::new(0.0, 0.0, 0.0)), + ..Default::default() + }, + ] { + let registration = IcpRegistration::new(config).with_trim_fraction(0.8); + assert_eq!( + registration.align_with_trace(&target, &target).unwrap_err().to_string(), + registration + .align_with_gpu_trimmed_reduction(&runtime, &target, &target) + .unwrap_err() + .to_string() + ); + } + let far = spatialrust_registration::transform_point_cloud( + &target, + Isometry3::new(Quat::::identity(), Vec3::new(100.0, 0.0, 0.0)), + ) + .unwrap(); + let registration = registration(1.0, 0.8, 0); + assert_eq!( + registration.align_with_trace(&far, &target).unwrap_err().to_string(), + registration + .align_with_gpu_trimmed_reduction(&runtime, &far, &target) + .unwrap_err() + .to_string() + ); + let source = fixture(257, 0, 1.0, true); + let target = fixture(274, 0, 1.0, false); + let expected = + registration.align_with_gpu_trimmed_reduction(&runtime, &source, &target).unwrap(); + std::thread::scope(|scope| { + let callers: Vec<_> = (0..4) + .map(|_| { + scope.spawn(|| { + assert_eq!( + registration + .align_with_gpu_trimmed_reduction(&runtime, &source, &target) + .unwrap(), + expected + ) + }) + }) + .collect(); + for caller in callers { + caller.join().unwrap(); + } + }); +} + +#[test] +#[ignore = "requires a native wgpu adapter; run explicitly with --include-ignored"] +fn resident_trimmed_rotated_initial_guesses_use_absolute_original_positions() { + let runtime = WgpuRuntime::new_headless().unwrap(); + for n in [65, 257, 4097] { + let target = fixture(n + 17, 0, 1.0, false); + let source = spatialrust_registration::transform_point_cloud( + &fixture(n, 0, 1.0, true), + Isometry3::new( + Quat::from_axis_angle(Vec3::new(1.0, 2.0, 3.0), 0.003), + Vec3::new(0.005, -0.002, 0.001), + ), + ) + .unwrap(); + let registration = IcpRegistration::new(IcpConfig { + max_iterations: 3, + max_correspondence_distance: 0.25, + initial_guess: Isometry3::new( + Quat::from_axis_angle(Vec3::new(0.0, 0.0, 1.0), -0.001), + Vec3::new(-0.003, 0.002, 0.0), + ), + ..Default::default() + }) + .with_trim_fraction(0.8) + .with_convergence_criteria(IcpConvergenceCriteria { + translation_epsilon: 0.0, + rotation_epsilon: 0.0, + fitness_epsilon: 0.0, + }); + let expected = registration.align_with_trace(&source, &target).unwrap(); + let actual = + registration.align_with_gpu_trimmed_reduction(&runtime, &source, &target).unwrap(); + assert!( + pose_error(&source, expected.result.transform, actual.diagnostics.result.transform) + < 3e-5 + ); + 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) + ); + } + } +} diff --git a/crates/spatialrust-registration/tests/gpu_icp_trimmed_reduction_benchmark.rs b/crates/spatialrust-registration/tests/gpu_icp_trimmed_reduction_benchmark.rs new file mode 100644 index 00000000..f5cfe959 --- /dev/null +++ b/crates/spatialrust-registration/tests/gpu_icp_trimmed_reduction_benchmark.rs @@ -0,0 +1,150 @@ +#![cfg(all(feature = "register-icp-gpu", not(target_arch = "wasm32")))] +#[path = "gpu_icp_trimmed_reduction.rs"] +mod reference; +use spatialrust_core::HasPositions3; +use spatialrust_gpu::{GpuIcpInput, WgpuRuntime}; +use spatialrust_search::KdTree; +use std::time::Instant; + +#[test] +#[ignore = "completed trimmed ICP benchmark; run this exact test alone in release mode"] +fn completed_resident_trimmed_icp_calls() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let info = runtime.adapter_info(); + println!("GPU_ICP_TRIMMED_ENV backend={} device_type={}", info.backend, info.device_type); + for n in [257, 4097, 65537, 262145] { + for profile in 0..3 { + let fraction = [0.5, 0.8, 0.9][profile]; + let source = reference::fixture(n, profile, 1.0, true); + let target = reference::fixture(n + 17, profile, 1.0, false); + let registration = reference::registration(1.0, fraction, 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_trimmed_reduction(&runtime, &source, &target).unwrap(); + let legacy_maximum = reference::pose_error( + &source, + expected_cpu.result.transform, + expected_legacy.diagnostics.result.transform, + ); + assert!( + legacy_maximum < 3e-5, + "n={n} profile={profile} legacy_pose_error={legacy_maximum}" + ); + // Each precision path keeps its own exact warm/timed control. + // GPU distances can alter trimmed subsets on near ties. + for diagnostics in [&expected_legacy.diagnostics, &expected_gpu.diagnostics] { + assert_eq!(diagnostics.stop_reason, expected_cpu.stop_reason); + assert_eq!(diagnostics.result.iterations, expected_cpu.result.iterations); + assert_eq!(diagnostics.result.converged, expected_cpu.result.converged); + for (a, b) in diagnostics.history.iter().zip(&expected_cpu.history) { + assert_eq!( + (a.correspondences, a.evaluated_correspondences), + (b.correspondences, b.evaluated_correspondences) + ); + } + let (count, fitness) = reference::independent_fitness( + &source, + &target, + diagnostics.result.transform, + 0.25, + ); + assert_eq!(count, diagnostics.history.last().unwrap().evaluated_correspondences); + assert!((fitness - diagnostics.result.fitness).abs() <= 1e-10 + fitness * 1e-4); + } + let maximum = reference::pose_error( + &source, + expected_cpu.result.transform, + expected_gpu.diagnostics.result.transform, + ); + assert!(maximum.is_finite()); + let within_pose_tolerance = maximum < 3e-5; + // Preserve adverse large gated cases in the measurements rather + // than silently increasing the small-fixture agreement tolerance. + if n != 262145 || profile != 1 { + assert!(within_pose_tolerance, "n={n} profile={profile} max_pose_error={maximum}"); + } + assert_eq!(expected_gpu.diagnostics.result.iterations, 3); + assert_eq!(expected_gpu.evaluation_batches, 4); + for (a, b) in expected_cpu.history.iter().zip(&expected_gpu.diagnostics.history) { + assert_eq!( + (a.correspondences, a.evaluated_correspondences), + (b.correspondences, b.evaluated_correspondences) + ); + } + 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 = KdTree::from_slices(tx, ty, tz); + let input = GpuIcpInput::upload(&runtime, &source_positions, &tree).unwrap(); + let pose = expected_gpu.diagnostics.result.transform; + let expected_moments = input + .trimmed_moments_resident(&runtime, pose, 0.25, fraction) + .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_trimmed_reduction(&runtime, &source, &target) + .unwrap(), + expected_gpu + ); + assert_eq!( + input + .trimmed_moments_resident(&runtime, pose, 0.25, fraction) + .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_trimmed_reduction(&runtime, &source, &target) + .unwrap(); + gpu_ms.push(start.elapsed().as_secs_f64() * 1000.0); + assert_eq!(output, expected_gpu); + } + _ => { + let output = input + .trimmed_moments_resident(&runtime, pose, 0.25, fraction) + .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_TRIMMED_BENCH n={n} profile={profile} fraction={fraction} iterations=3 max_pose_error={maximum} within_pose_tolerance={within_pose_tolerance} legacy_pose_error={legacy_maximum} legacy_fitness={} cpu_fitness={} gpu_fitness={} retained={} gated={} 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_legacy.diagnostics.result.fitness,expected_cpu.result.fitness,expected_gpu.diagnostics.result.fitness,expected_moments.moments.correspondences,expected_moments.evaluated_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/tests/gpu_icp_reduction_smoke.rs b/crates/spatialrust/tests/gpu_icp_reduction_smoke.rs index 15f70be6..4ae9239b 100644 --- a/crates/spatialrust/tests/gpu_icp_reduction_smoke.rs +++ b/crates/spatialrust/tests/gpu_icp_reduction_smoke.rs @@ -19,3 +19,23 @@ fn facade_resident_icp_returns_fixed_size_receipts_and_convergence() { assert_eq!(result.evaluation_batches, 2); assert_eq!(result.transfers.device_to_host_bytes(), 352); } + +#[test] +#[ignore = "requires a native wgpu adapter; run explicitly with --include-ignored"] +fn facade_resident_trimmed_icp_exposes_retained_and_all_gated_counts() { + 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()) + .with_trim_fraction(0.8) + .align_with_gpu_trimmed_reduction(&runtime, &cloud, &cloud) + .unwrap(); + assert!(result.diagnostics.result.converged); + assert_eq!(result.diagnostics.history[0].correspondences, 52); + assert_eq!(result.diagnostics.history[0].evaluated_correspondences, 65); + 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 b7ba4a49..769e642d 100644 --- a/docs/ARCHITECTURE.md +++ b/docs/ARCHITECTURE.md @@ -26,10 +26,27 @@ 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 +f32 transforms/reductions can change matching, pose and stopping near gates or +numerically degenerate geometry. See [resident ICP evidence](../notes/2026-10-13_gpu_icp_reduction.md). +`align_with_gpu_trimmed_reduction` and `GpuIcpInput::trimmed_moments_resident` +extend the resident path with stable MSD selection by device distance and original +source index. Four distance bytes and the necessary source-index bytes determine +an exact retained threshold without sorting or copying matches to CPU. Device +integer limbs emulate binary64 multiplication/round-to-nearest-even/floor for +`trim_fraction*gated_count`; casting the fraction to f32 is avoided. Retained +pairs define centroids/covariance, while ALL gated pairs define fitness and its +stopping denominator. A 176-byte result adds the all-gated count in reserved word +2; fraction one delegates to ordinary kernels with both reserved words zero. +Private histogram/selection scratch stays leased through the same submission. +CPU target-tree construction/Horn solving/composition/convergence remain; there +is no payload readback or fallback. GPU distance rounding can change selection +near ties compared with CPU, even when retained counts agree. A measured large +partially gated case exceeds CPU pose-agreement tolerance; the evidence retains +its error/fitness and does not classify its speed as accuracy-qualified. See +[resident trimming evidence](../notes/2026-10-13_gpu_icp_resident_trimming.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/ROADMAP.md b/docs/ROADMAP.md index a1e9ac8f..dccad123 100644 --- a/docs/ROADMAP.md +++ b/docs/ROADMAP.md @@ -1139,4 +1139,5 @@ Dependencies and explicit transfers come before broader algorithm coverage. | GICP resident k-NN covariances | Complete | Reused query/sample positions and target tree, bounded GPU k-NN/sample covariance/plane eigenvectors with status carried into equations; no covariance readback/re-upload | Independent f64/spectral/Rayleigh checks, 162 pose/scale/k/stopping cases, malformed/status/ownership/receipt guards and completed CPU/previous-GPU comparisons; `notes/2026-10-13_gpu_gicp_resident_covariances.md` | | GICP resident radius covariances | Complete | Complete inclusive GPU radius neighborhoods with explicit capacity, compensated central sums/eigenvectors retained for equations; sparse epsilon*I and overflow rejection | Independent brute-force/f64/spectral references, 162 pose/scale/radius/stopping cases, sparse extent beyond grid capacity, receipts/ownership/status checks and completed API comparisons; `notes/2026-10-13_gpu_gicp_resident_radius_covariances.md` | | GICP resident grid covariances | Complete | Reused original positions and canonical GPU grid gather/covariance sweeps; no source KD-tree/neighborhood rows, private grid scratch and complete-support rejection | Exact legacy covariance-word parity, 162 registration conditions, boundary/error/ownership/concurrency and transfer checks with completed-call comparisons; `notes/2026-10-13_gpu_gicp_resident_grid_covariances.md` | -| Remaining registration updates | Planned | Trimming and small pose-update boundaries | Pose/convergence/fitness references, numerical conditioning, source/target residency and end-to-end registration timing | +| ICP resident trimming | Complete | Stable GPU distance/source-index selection and exact binary64 retained-count floor; selected moments plus all-gated fitness in one submission, fixed 176-byte readback | Independent comparison-sort/f64 moment references, rounding/boundary/ownership/status checks, 486 registration conditions, completed comparisons and an explicit adverse large-gate precision case; `notes/2026-10-13_gpu_icp_resident_trimming.md` | +| Remaining registration updates | Planned | Small pose-update boundaries and adverse trimming precision | Pose/convergence/fitness references, numerical conditioning, source/target residency and end-to-end registration timing | diff --git a/notes/2026-10-13_gpu_icp_resident_trimming.md b/notes/2026-10-13_gpu_icp_resident_trimming.md new file mode 100644 index 00000000..93317112 --- /dev/null +++ b/notes/2026-10-13_gpu_icp_resident_trimming.md @@ -0,0 +1,263 @@ +# Resident GPU ICP trimming + +Date: 2026-10-13. Named slice: stable device distance/index selection for +point-to-point ICP, retaining the explicit host pose-solve boundary. + +## API and numerical contract + +`IcpRegistration::align_with_gpu_trimmed_reduction(runtime, source, target)` +under `register-icp-gpu` honors the existing `with_trim_fraction(f64)` setting. +`GpuIcpInput::trimmed_moments_resident(runtime, absolute_pose, max_distance, +fraction)` under `gpu-wgpu` provides the low-level asynchronous owned result. +Both APIs explicitly retain original source/target/tree buffers. GPU computes +absolute transforms, bounded nearest matching, stable trimming, centroids, +centered cross-covariance and all-gated squared error. CPU builds the target +tree, solves the existing Horn quaternion eigensystem, composes the pose and +applies convergence criteria. There is no CPU selection/fallback or intermediate +point/match readback. Ordinary CPU and correspondence APIs retain their math. + +For N gated matches, K is `floor(f64(N)*trim_fraction)` with the binary64 +product rounded to nearest-even before floor, exactly as the CPU configuration +contract specifies. The fraction must be finite and in (0,1]. GPU integer limbs +multiply the u32 count by the exact 53-bit significand, round the resulting +96-bit representation to 53 significant bits and shift/floor it. Subnormal +fractions retain zero points. Casting the fraction to f32 or using a quantized +histogram cutoff would change K and is avoided. + +Selection order is ascending GPU squared-distance word, then original source +index; signed zero distances share key zero. Retaining the first K matches +preserves source input order on exact device-distance ties. A most-significant +digit histogram narrows the Kth pair's prefix, without permuting matches or +performing CPU sorting. Four distance bytes and only the significant index +bytes are processed. Each digit has a histogram pass and a prefix-selection +pass. Up to 256 groups of 64 threads use private 256-bin histograms; the final +64-thread group combines them. The exact threshold selects the retained set +for the canonical compensated moment arithmetic. + +Retained matches contribute to centroids/covariance and the next pose solve. +ALL gated matches contribute to post-update fitness and its denominator; +fitness stopping does not become easier by dropping residuals. The trace's +`correspondences` reports retained update support, while +`evaluated_correspondences` reports all post-update gated matches. Insufficient +gated or retained support reports the corresponding existing error. A fraction +of one delegates to ordinary resident kernels and preserves their complete +diagnostics and transfer receipts. `align_with_gpu_reduction` retains its +existing fraction-one requirement and error contract. + +Selection and reduction share ONE queue submission. Input upload remains +`16*S+32*T+32*M` bytes for source/target counts S/T and M target tree nodes; +uniform controls are excluded. Each nonempty evaluation explicitly downloads +176 bytes, with I+1 evaluations for I updates. There are no intermediate host +waits or repeated payload uploads. Empty source low-level calls transfer zero +readback bytes. The wrapper validates nonempty clouds as before. + +`GpuIcpTrimmedMoments` owns its output outside the scratch pool and exposes +`buffer`, `normalization_scale`, `squared_distance_scale`, `readback_bytes` and +explicit `download`. Its layout is the canonical 176 bytes: status, retained +count, ALL-gated count, reserved zero; two vec4 anchors and sixteen compensated +vec2 terms. Terms 0..14 use retained support, and term 15 uses all gated support. +Fraction one uses canonical reserved zeros, with the decoder supplying the +ordinary count as the all-gated denominator. Zero retained support zeros +anchors/centroids/covariance while preserving all-gated residual error. Invalid +status or zero gated support zeros all payload. Status bits 1/2/4 retain their +transform-overflow/malformed-match/nonfinite-moment meanings. + +`GpuIcpTrimmedStatistics` contains the retained-pair `moments` and separate +`evaluated_correspondences`. Its nested squared-error sum must be divided by +the separate all-gated count. Download validates runtime ownership, status, +retained<=gated<=source count, reserved words, zero-support invariants and finite +payload before decoding physical f64 values. Result buffers outlive inputs; +callers must inspect status before using device storage and keep it immutable. + +The trim route requires seven storage bindings, eight total bindings, and +2048 shared bytes, in addition to canonical dispatch/uniform/geometry limits. +Histogram scratch is `1024*groups` bytes (maximum 262,144), plus 32-byte +selection state; transformed/match/partial scratch remains canonical. +Selection state is explicitly cleared and every consumed histogram word is +overwritten. Scratch stays exclusively leased through submission before +recycling, including concurrent shared-runtime calls. Unsupported limits or +nonfinite arithmetic return errors without choosing another backend. + +f32 device transforms, products and distances still have their existing +precision limits. CPU/GPU distance rounding can alter ordering near ties; +different retained subsets can alter centroids, pose and stopping. Matching K +does not promise the same subset or bit-exact CPU moments. Independent f64 +moment oracles use verified GPU distance words for ordering on nonbinary +coordinate scales; full registration also compares against ordinary CPU ICP. + +## Validation + +Seven new native primitive tests cover: + +- 443,827 exact retained-count comparisons across 31 fraction encodings and + 14,317 counts, including binary64 neighbors of common fractions, subnormal + fractions, integer/power-of-two boundaries and u32 maximum values. +- 135 independent comparison-sort selection cases: sizes 1/3/63/64/65/257/ + 4097/16385/65537, five fractions, and dispersed/all-tied/signed-zero/boundary/ + subnormal distance keys with missing matches. Every selected threshold/set + and gated/retained count matches the reference. +- Independent f64 selected means/cross-covariance and all-gated error at six + sizes, three scales and three fractions (54 cases). Known scalar target + membership is verified before comparison sorting device distance words. +- Exact all-gated error retention while K varies, including zero retained + support and fraction-one equality; inclusive distance boundaries; plan + limits; malformed readbacks; transform overflow zeroing; empty gates/inputs; + result ownership after input drop; runtime mismatches; four concurrent callers. + +The native full-call suite checks 486 conditions (369,603 source points): +six sizes 17/63/64/65/257/4097, three unique/partially gated/duplicate profiles, +three scales 1e-4/1/1e4, three fractions 0.5/0.8/1, and three stopping modes. +CPU/GPU update/evaluation counts, stop reason and convergence agree. Independent +all-gated final residual fitness agrees within the stated f32 tolerance. +Maximum scale-normalized CPU pose-coordinate difference was 0.0000019073486, +below the 3e-5 test limit. Three additional rotated initial-guess cases exercise +absolute original-position reuse. Inputs, complete receipts and fraction-one +diagnostics are preserved. Invalid configuration, insufficient gated/retained +support and concurrent full calls are checked separately. + +Existing untrimmed native tests also passed: 54 pose/scale/unequal-cloud cases, +exact concurrent diagnostics, validation/error receipts and convergence. +The public facade checks retained/all-gated counts and fixed readback size. +All-feature registration library tests passed (35 passed, three intentional +ignored tests); Clippy all targets/features, CPU-only ICP, WASM optional ICP +and formatting checks passed. No dependency, unsafe boundary or core model is +added. The canonical untrimmed WGSL file, nearest WGSL file, ordinary ICP loop +and Horn solver remain unchanged. + +Source references: + +- `C:/Users/rsasa/Workspace/SpatialRust/crates/spatialrust-gpu/src/kernels/icp_reduction.rs` +- `C:/Users/rsasa/Workspace/SpatialRust/crates/spatialrust-gpu/src/kernels/icp_trim.rs` +- `C:/Users/rsasa/Workspace/SpatialRust/crates/spatialrust-gpu/src/shaders/icp_trim.wgsl` +- `C:/Users/rsasa/Workspace/SpatialRust/crates/spatialrust-gpu/src/kernels/icp_trim_tests.rs` +- `C:/Users/rsasa/Workspace/SpatialRust/crates/spatialrust-registration/src/icp_reduction.rs` +- `C:/Users/rsasa/Workspace/SpatialRust/crates/spatialrust-registration/tests/gpu_icp_trimmed_reduction.rs` +- `C:/Users/rsasa/Workspace/SpatialRust/crates/spatialrust-registration/tests/gpu_icp_trimmed_reduction_benchmark.rs` + +## Completed-call measurements and precision limit + +Two sequential isolated release surveys completed on the same native Vulkan / +DiscreteGpu runtime. No local build, test, statistical analysis or other local +compute work overlapped a survey. Compilation was completed separately. Each +condition has three warmups per stage and 20 measured rounds. The stage order +rotates CPU ICP, previous GPU correspondences with CPU trimming/moments, new +resident trimmed ICP, and a completed prepared resident evaluation. Every warm +and timed result equals its own precision path's complete control result. +Source sizes are 257/4097/65537/262145, with N+17 target points and three updates +with stopping thresholds disabled. U is unique geometry with trim 0.5; G adds +partially gated/next-layer matches and trim 0.8; D includes duplicates and trim +0.9. All profiles include repeated inlier/outlier residual levels. + +Full calls include XYZ extraction, source packing, serial target-tree +construction, upload, all evaluations, explicit waits/readback, CPU Horn +solving, composition, history and final fitness. The prepared evaluation +includes host command/buffer preparation, transformation/search/selection/ +reduction submission and GPU completion wait; it EXCLUDES input packing/tree +construction/upload, fixed readback, pose solve and registration looping. +It is a completed prepared-operation wall time, not a GPU timestamp or a +full-registration speedup. Uniform control bytes remain excluded from receipts. + +The table reports stage median milliseconds. Ratios are geometric means of +paired baseline/new times, with 10,000 paired-log bootstrap draws, sorted +entries 249/9749, seed `20261013+2*record_index+baseline_index` (CPU 0, previous +GPU 1). Intervals describe paired rounds within one run, not other devices, +drivers or environmental drift; the repeated surveys retain their separate +ranges. The tables do not discard any measured condition. + +| Survey | Source/target | Fixture | CPU ms | Previous GPU ms | New GPU ms | Prepared evaluation ms | Max new / legacy pose-coordinate error | Pose within 3e-5 | CPU/new ratio (95% interval) | Previous/new ratio (95% interval) | +| --- | ---: | :---: | ---: | ---: | ---: | ---: | ---: | :---: | --- | --- | +| 1 | 257/274 | U/0.5 | 0.270 | 6.491 | 4.459 | 0.459 | 5.96e-08/0 | yes | 0.06x (0.06-0.06) | 1.47x (1.43-1.50) | +| 1 | 257/274 | G/0.8 | 0.270 | 6.547 | 4.439 | 0.499 | 2.38e-07/0 | yes | 0.06x (0.06-0.06) | 1.49x (1.46-1.52) | +| 1 | 257/274 | D/0.9 | 0.326 | 6.532 | 4.421 | 0.494 | 1.19e-07/0 | yes | 0.07x (0.07-0.07) | 1.47x (1.41-1.53) | +| 1 | 4,097/4,114 | U/0.5 | 5.647 | 9.037 | 5.348 | 0.682 | 0/0 | yes | 1.05x (1.02-1.08) | 1.69x (1.65-1.73) | +| 1 | 4,097/4,114 | G/0.8 | 6.716 | 9.192 | 5.359 | 0.757 | 9.54e-07/0 | yes | 1.27x (1.22-1.32) | 1.69x (1.64-1.75) | +| 1 | 4,097/4,114 | D/0.9 | 6.960 | 11.034 | 6.530 | 1.298 | 9.54e-07/0 | yes | 1.06x (1.02-1.10) | 1.64x (1.59-1.69) | +| 1 | 65,537/65,554 | U/0.5 | 130.350 | 72.595 | 50.208 | 8.578 | 1.98e-06/0 | yes | 2.61x (2.54-2.69) | 1.42x (1.40-1.44) | +| 1 | 65,537/65,554 | G/0.8 | 147.968 | 75.154 | 57.030 | 10.529 | 5.25e-06/0 | yes | 2.63x (2.57-2.70) | 1.36x (1.32-1.42) | +| 1 | 65,537/65,554 | D/0.9 | 163.664 | 73.972 | 55.959 | 9.804 | 9.54e-07/0 | yes | 2.99x (2.93-3.06) | 1.34x (1.32-1.35) | +| 1 | 262,145/262,162 | U/0.5 | 570.988 | 236.647 | 143.786 | 19.344 | 7.15e-07/0 | yes | 4.19x (3.87-4.56) | 1.67x (1.60-1.75) | +| 1 | 262,145/262,162 | G/0.8 | 623.784 | 211.499 | 121.726 | 14.520 | 0.000815/7.63e-06 | NO | 5.01x (4.52-5.58) | 1.77x (1.63-1.93) | +| 1 | 262,145/262,162 | D/0.9 | 677.595 | 243.593 | 150.065 | 14.175 | 5.72e-06/0 | yes | 4.92x (4.50-5.38) | 1.73x (1.62-1.86) | +| 2 | 257/274 | U/0.5 | 0.273 | 6.613 | 4.472 | 0.470 | 5.96e-08/0 | yes | 0.06x (0.06-0.06) | 1.47x (1.44-1.50) | +| 2 | 257/274 | G/0.8 | 0.278 | 6.702 | 4.589 | 0.504 | 2.38e-07/0 | yes | 0.06x (0.06-0.06) | 1.45x (1.43-1.48) | +| 2 | 257/274 | D/0.9 | 0.335 | 6.666 | 4.598 | 0.499 | 1.19e-07/0 | yes | 0.07x (0.07-0.07) | 1.43x (1.37-1.47) | +| 2 | 4,097/4,114 | U/0.5 | 5.759 | 9.106 | 5.377 | 0.679 | 0/0 | yes | 1.06x (1.01-1.10) | 1.69x (1.64-1.73) | +| 2 | 4,097/4,114 | G/0.8 | 6.826 | 8.571 | 5.106 | 0.709 | 9.54e-07/0 | yes | 1.30x (1.25-1.35) | 1.66x (1.60-1.72) | +| 2 | 4,097/4,114 | D/0.9 | 7.030 | 8.921 | 5.413 | 0.728 | 9.54e-07/0 | yes | 1.30x (1.25-1.34) | 1.64x (1.60-1.69) | +| 2 | 65,537/65,554 | U/0.5 | 132.530 | 68.646 | 47.349 | 7.986 | 1.98e-06/0 | yes | 2.99x (2.82-3.19) | 1.53x (1.45-1.62) | +| 2 | 65,537/65,554 | G/0.8 | 166.110 | 72.527 | 52.598 | 9.054 | 5.25e-06/0 | yes | 3.32x (3.10-3.61) | 1.44x (1.38-1.50) | +| 2 | 65,537/65,554 | D/0.9 | 180.553 | 78.617 | 57.076 | 10.136 | 9.54e-07/0 | yes | 3.12x (2.97-3.26) | 1.34x (1.30-1.38) | +| 2 | 262,145/262,162 | U/0.5 | 618.907 | 250.262 | 151.087 | 19.347 | 7.15e-07/0 | yes | 4.50x (4.17-4.86) | 1.77x (1.67-1.88) | +| 2 | 262,145/262,162 | G/0.8 | 650.866 | 235.857 | 134.637 | 14.365 | 0.000815/7.63e-06 | NO | 5.06x (4.62-5.56) | 1.77x (1.65-1.90) | +| 2 | 262,145/262,162 | D/0.9 | 804.375 | 249.540 | 162.823 | 22.086 | 5.72e-06/0 | yes | 5.36x (4.99-5.75) | 1.70x (1.60-1.81) | + +65,537-point full calls improve 2.61-3.32x over CPU and 1.34-1.53x over previous +GPU, with all within-run intervals above one. The pose-qualified unique/ +duplicate 262,145-point calls improve 4.19-5.36x over CPU and 1.67-1.77x over +previous GPU; their maximum CPU pose-coordinate difference is 0.000005722046. +4,097-point CPU/new ratios are only 1.05-1.30x (lower intervals near one), while +257-point CPU/new ratios are 0.06-0.07x: CPU is substantially faster there. +The route remains explicitly selected and does not infer placement from size. + +The large G/0.8 case does NOT meet the 3e-5 CPU pose-agreement tolerance in +either survey. Its maximum coordinate difference is 0.0008154977 versus CPU; +previous GPU/CPU difference is 0.0000076293945. CPU / previous GPU / new GPU +final fitness is 0.004841253771790894 / 0.004841253990255459 / +0.0048409760054502335. Both runs retain 209,642 of 262,053 gated pairs and agree +on counts, iteration limit and nonconvergence; independent all-gated residual +fitness checks pass. GPU f32 transforms/distances/moments can change subsets +near the repeated residual cutoff and propagate into pose. These measurements +do not isolate which stage accounts for the difference. The case is preserved +with `within_pose_tolerance=false` and excluded from pose-qualified speedup +ranges above; its timing is NOT evidence of CPU-equivalent accuracy. Further +selected-set and precision investigation is needed for this geometry/regime. +The tolerance is not increased to make that condition pass. + +An initial probe stopped on CPU/previous-GPU complete-diagnostics equality; +a second stopped on the large G case's pose tolerance. Both diagnostic logs +are retained separately and are not inputs to the successful-survey bootstrap. +The final benchmark retains the original tolerance and records the explicit +adverse-case flag. Production kernels did not change between those probes +and the final surveys. + +Payload receipts for three updates (four resident evaluations vs seven +previous correspondence batches): + +| Source/target | Fixture | Previous H2D | New H2D | Previous D2H | New D2H | +| ---: | :---: | ---: | ---: | ---: | ---: | +| 257/274 | U/0.5 | 35,184 | 14,896 | 14,392 | 704 | +| 257/274 | G/0.8 | 35,184 | 14,896 | 14,392 | 704 | +| 257/274 | D/0.9 | 35,184 | 14,896 | 14,392 | 704 | +| 4,097/4,114 | U/0.5 | 542,192 | 214,704 | 229,432 | 704 | +| 4,097/4,114 | G/0.8 | 542,192 | 214,704 | 229,432 | 704 | +| 4,097/4,114 | D/0.9 | 542,192 | 214,704 | 229,432 | 704 | +| 65,537/65,554 | U/0.5 | 8,652,272 | 3,409,584 | 3,670,072 | 704 | +| 65,537/65,554 | G/0.8 | 8,652,272 | 3,409,584 | 3,670,072 | 704 | +| 65,537/65,554 | D/0.9 | 8,652,272 | 3,409,584 | 3,670,072 | 704 | +| 262,145/262,162 | U/0.5 | 34,604,528 | 13,633,200 | 14,680,120 | 704 | +| 262,145/262,162 | G/0.8 | 34,604,528 | 13,633,200 | 14,680,120 | 704 | +| 262,145/262,162 | D/0.9 | 34,604,528 | 13,633,200 | 14,680,120 | 704 | + +At 262,145 points the D2H payload falls from 14,680,120 to 704 bytes; H2D falls +from 34,604,528 to 13,633,200 bytes. These savings include the whole call, not +only a selected pass. Host target-tree construction, pose solving and stopping +remain, and the adverse precision case remains visible. + +Reproduction (PowerShell, isolated exact benchmark; compile separately first): + +```powershell +Set-Location C:/Users/rsasa/Workspace/SpatialRust +$env:PATH='C:/Users/rsasa/.cargo/bin;'+$env:PATH +cargo test -p spatialrust-registration --no-default-features --features register-icp-gpu --release --test gpu_icp_trimmed_reduction_benchmark --no-run +cargo test -p spatialrust-registration --no-default-features --features register-icp-gpu --release --test gpu_icp_trimmed_reduction_benchmark completed_resident_trimmed_icp_calls -- --include-ignored --exact --nocapture --test-threads=1 +``` + +Raw successful surveys: +`C:/Users/rsasa/Workspace/SpatialRust/target/automation/gpu-icp-resident-trimming-benchmark-1.log` +and `C:/Users/rsasa/Workspace/SpatialRust/target/automation/gpu-icp-resident-trimming-benchmark-2.log`. +Parsed samples/medians/intervals: +`C:/Users/rsasa/Workspace/SpatialRust/target/automation/gpu-icp-resident-trimming-measurements.json`. +Analysis: +`C:/Users/rsasa/Workspace/SpatialRust/target/automation/analyze-gpu-icp-resident-trimming.py`. +These local artifacts contain backend/device class and measurement conditions, +without an adapter model name.