From 713ba2023c2da7cd714c5ac0cc96e6c3997e828f Mon Sep 17 00:00:00 2001 From: rsasaki0109 Date: Tue, 13 Oct 2026 09:52:18 +0900 Subject: [PATCH] Add GPU resident grid GICP covariance preparation --- CHANGELOG.md | 8 + README.md | 9 + .../src/kernels/gicp_grid_covariance_tests.rs | 400 ++++++++++++++++++ .../src/kernels/gicp_grid_covariances.rs | 296 +++++++++++++ .../src/kernels/gicp_reduction.rs | 3 + crates/spatialrust-gpu/src/runtime.rs | 3 + .../src/shaders/gicp_grid_covariances.wgsl | 32 ++ .../src/gicp_reduction.rs | 45 +- .../gpu_gicp_resident_grid_covariances.rs | 214 ++++++++++ ...icp_resident_grid_covariances_benchmark.rs | 214 ++++++++++ ...pu_gicp_resident_grid_covariances_smoke.rs | 25 ++ docs/ARCHITECTURE.md | 13 + docs/FEATURE_MATRIX.md | 12 + docs/ROADMAP.md | 1 + ...0-13_gpu_gicp_resident_grid_covariances.md | 237 +++++++++++ 15 files changed, 1509 insertions(+), 3 deletions(-) create mode 100644 crates/spatialrust-gpu/src/kernels/gicp_grid_covariance_tests.rs create mode 100644 crates/spatialrust-gpu/src/kernels/gicp_grid_covariances.rs create mode 100644 crates/spatialrust-gpu/src/shaders/gicp_grid_covariances.wgsl create mode 100644 crates/spatialrust-registration/tests/gpu_gicp_resident_grid_covariances.rs create mode 100644 crates/spatialrust-registration/tests/gpu_gicp_resident_grid_covariances_benchmark.rs create mode 100644 crates/spatialrust/tests/gpu_gicp_resident_grid_covariances_smoke.rs create mode 100644 notes/2026-10-13_gpu_gicp_resident_grid_covariances.md diff --git a/CHANGELOG.md b/CHANGELOG.md index fa07e78d..e94216ba 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 grid GICP covariance preparation under + `register-gicp-gpu`. Reuse original GPU positions and canonical radius-grid + arithmetic, eliminating source KD-tree preparation, per-point neighborhood + buffers and covariance readback/re-upload. CPU counting-sort grids and target + tree remain; support capacity includes self/duplicates and overflow rejects + all equations. Grid indices use private leased scratch with full receipts. + Existing sparse/resident/legacy routes retain their explicit contracts. + - Add explicit resident radius covariance preparation for GICP under `register-gicp-gpu`. Inclusive bounded GPU neighborhoods, centered/scaled compensated central sums and plane eigenvectors feed resident equations diff --git a/README.md b/README.md index fcb7bd84..bf840467 100644 --- a/README.md +++ b/README.md @@ -681,6 +681,15 @@ Sparse neighborhoods retain the grid route's epsilon*I fallback. CPU tree construction and 6x6 solves remain; only 256-byte equations are downloaded. See [resident radius evidence](notes/2026-10-13_gpu_gicp_resident_radius_covariances.md). +`align_with_gpu_resident_grid_covariances(&runtime, &source, &target, +neighbor_capacity)` uses the existing radius-grid covariance math directly on +resident original positions. CPU builds counting-sort grids and the target +tree; no source KD-tree or per-point neighborhood buffer is needed. Capacity +includes self/duplicates and overflow rejects all output. Dense grids must fit +the existing cell/device limits; choose the bounded radius method explicitly +for sparse extents. Only 256-byte equations are downloaded per evaluation. +See [resident grid evidence](notes/2026-10-13_gpu_gicp_resident_grid_covariances.md). + Point-to-plane ICP also exposes `PointToPlaneIcp::align_with_gpu_reduction(&runtime, &source, &target)` under `register-icp-point-to-plane-gpu`. Transforms, nearest matching and plane diff --git a/crates/spatialrust-gpu/src/kernels/gicp_grid_covariance_tests.rs b/crates/spatialrust-gpu/src/kernels/gicp_grid_covariance_tests.rs new file mode 100644 index 00000000..2be921ae --- /dev/null +++ b/crates/spatialrust-gpu/src/kernels/gicp_grid_covariance_tests.rs @@ -0,0 +1,400 @@ +use super::*; +use spatialrust_math::{Isometry3, Quat, Vec3}; + +fn axes(points: &[[f32; 3]]) -> [Vec; 3] { + std::array::from_fn(|a| points.iter().map(|p| p[a]).collect()) +} +fn tree(points: &[[f32; 3]]) -> KdTree { + let c = axes(points); + KdTree::from_slices(&c[0], &c[1], &c[2]) +} +fn words(runtime: &WgpuRuntime, buffer: &wgpu::Buffer) -> Vec { + let staging = runtime.device().create_buffer(&wgpu::BufferDescriptor { + label: None, + size: buffer.size(), + usage: wgpu::BufferUsages::MAP_READ | wgpu::BufferUsages::COPY_DST, + mapped_at_creation: false, + }); + let mut encoder = runtime.device().create_command_encoder(&Default::default()); + encoder.copy_buffer_to_buffer(buffer, 0, &staging, 0, buffer.size()); + 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.wait_idle(); + rx.recv().unwrap().unwrap(); + let map = slice.get_mapped_range(); + let data = bytemuck::cast_slice(&map).to_vec(); + drop(map); + staging.unmap(); + data +} +fn upload( + runtime: &WgpuRuntime, + source: &[[f32; 3]], + target: &KdTree, + radius: f32, + capacity: u32, +) -> GpuGicpInput { + let c = axes(source); + GpuGicpInput::upload_with_gpu_grid_covariances( + runtime, + (&c[0], &c[1], &c[2]), + target, + radius, + capacity, + 0.001, + ) + .unwrap() +} + +#[test] +fn grid_plans_and_shader_adapter_bound_counts_uniforms_dispatch_and_capacity() { + assert_eq!(std::mem::size_of::(), 64); + assert_eq!( + grid_plan(65537, 65554, 0.25, 128, 0.001, &wgpu::Limits::default()).unwrap(), + (65537 + 65554 + 1) * 32 + ); + for (s, t) in + [(0, 1), (1, 0), (u64::MAX, 1), (1, u64::MAX), ((1 << 24) + 1, 1), (1, (1 << 24) + 1)] + { + assert!(grid_plan(s, t, 0.25, 128, 0.001, &wgpu::Limits::default()).is_err()); + } + for r in [0.0, -1.0, f32::NAN, f32::INFINITY, 1e20, 1e-22] { + assert!(grid_plan(1, 1, r, 128, 0.001, &wgpu::Limits::default()).is_err()); + } + for e in [0.0, 1e-5, 1.1, f32::NAN] { + assert!(grid_plan(1, 1, 0.25, 128, e, &wgpu::Limits::default()).is_err()); + } + assert!(grid_plan(1, 1, 0.25, 0, 0.001, &wgpu::Limits::default()).is_err()); + for case in 0..5 { + let mut l = wgpu::Limits::default(); + match case { + 0 => l.max_compute_invocations_per_workgroup = 255, + 1 => l.max_compute_workgroup_size_x = 255, + 2 => l.max_compute_workgroups_per_dimension = 27, + 3 => l.max_storage_buffer_binding_size = 255, + _ => l.max_uniform_buffer_binding_size = 95, + }; + assert!(grid_plan(1, 1, 0.25, 128, 0.001, &l).is_err()); + } + let shader = shader_source(); + let old = crate::kernels::covariances_grid::COV_GRID_WGSL; + let begin = old.find("fn rotate(").unwrap(); + let end = old.find("fn cell_coord(").unwrap(); + assert!(shader.contains(&old[begin..end])); + assert!(!shader.contains("out_cov")); + assert!(!shader.contains("xs[")); +} + +#[test] +fn resident_grid_covariances_preserve_all_legacy_f32_words_and_receipts() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let mut checked = 0; + for n in [1, 2, 3, 63, 64, 65, 257, 1025] { + for scale in [1e-4_f32, 1.0, 1e4] { + for radius in [0.075, 0.25, 0.5] { + let points: Vec<_> = (0..n) + .map(|i| { + let j = i / 2; + [ + (j % 17) as f32 * 0.125 * scale, + (j / 17) as f32 * 0.125 * scale, + (0.3 * (j % 17) as f32 * 0.125 - 0.2 * (j / 17) as f32 * 0.125 + 2.0) + * scale, + ] + }) + .collect(); + let target: Vec<_> = points.iter().skip(n / 3).copied().collect(); + let t = tree(&target); + let input = upload(&runtime, &points, &t, radius * scale, n as u32); + let actual = words(&runtime, &input.covariances); + assert!(actual[..8].iter().all(|v| *v == 0)); + let mut cells = 0; + let mut expected = vec![]; + for cloud in [&points, &target] { + let c = axes(cloud); + let (old, _) = crate::estimate_plane_covariances_grid_gpu_with_receipt( + &runtime, + &c[0], + &c[1], + &c[2], + radius * scale, + 0.001, + ) + .unwrap(); + let dims = spatialrust_search::grid_bounds(&c[0], &c[1], &c[2], radius * scale) + .unwrap() + .1; + cells += dims.into_iter().map(u64::from).product::(); + expected.extend(old); + } + assert_eq!( + input.upload_transfers().host_to_device_bytes(), + 20 * points.len() as u64 + + 36 * target.len() as u64 + + 32 * t.layout().nodes().len() as u64 + + 4 * (cells + 2) + ); + assert_eq!(input.upload_transfers().device_to_host_bytes(), 0); + for (i, c) in expected.iter().enumerate() { + let row = &actual[8 + 8 * i..16 + 8 * i]; + assert_eq!( + &row[..6], + &c.map(f32::to_bits), + "n={n} scale={scale} radius={radius} i={i}" + ); + assert_eq!(&row[6..], &[0, 0]); + checked += 1; + } + } + } + } + eprintln!( + "GPU_RESIDENT_GRID_COVARIANCE matched {checked} legacy rows ({} covariance words) exactly", + checked * 6 + ); +} + +#[test] +fn grid_capacity_and_numeric_errors_zero_both_sets_and_all_equations() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let duplicate = vec![[0.0; 3]; 65]; + let single = vec![[0.0; 3]]; + for second in [false, true] { + let (s, t) = if second { (&single, &duplicate) } else { (&duplicate, &single) }; + let t = tree(t); + let bad = upload(&runtime, s, &t, 0.5, 64); + let w = words(&runtime, &bad.covariances); + assert_eq!(w[0], 8); + assert!(w[1..].iter().all(|v| *v == 0)); + let out = bad.equations_resident(&runtime, Isometry3::identity(), 0.25).unwrap(); + let w = words(&runtime, out.buffer()); + assert_eq!(w[0], 16); + assert!(w[1..].iter().all(|v| *v == 0)); + assert!(out.download(&runtime).is_err()); + let good = upload(&runtime, s, &t, 0.5, 65); + assert!(words(&runtime, &good.covariances)[..8].iter().all(|v| *v == 0)); + } + let huge = vec![[2e38_f32; 3]; 3]; + let bad = upload(&runtime, &huge, &tree(&huge), 0.5, 3); + let w = words(&runtime, &bad.covariances); + assert_eq!(w[0], 4); + assert!(w[1..].iter().all(|v| *v == 0)); + let t = tree(&single); + assert!(GpuGicpInput::upload_with_gpu_grid_covariances( + &runtime, + (&[0.0], &[], &[0.0]), + &t, + 0.5, + 1, + 0.001 + ) + .is_err()); + assert!(GpuGicpInput::upload_with_gpu_grid_covariances( + &runtime, + (&[], &[], &[]), + &t, + 0.5, + 1, + 0.001 + ) + .is_err()); + assert!(GpuGicpInput::upload_with_gpu_grid_covariances( + &runtime, + (&[f32::NAN], &[0.0], &[0.0]), + &t, + 0.5, + 1, + 0.001 + ) + .is_err()); + let sparse = [[0.0; 3], [10000.0; 3]]; + let c = axes(&sparse); + assert!(GpuGicpInput::upload_with_gpu_grid_covariances( + &runtime, + (&c[0], &c[1], &c[2]), + &t, + 0.01, + 2, + 0.001 + ) + .is_err()); +} + +#[test] +fn grid_inclusive_boundary_sparse_epsilon_and_malformed_indices_are_checked() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let points = [ + [0.0; 3], + [0.5, 0.0, 0.0], + [0.0, 0.5, 0.0], + [f32::from_bits(0.5_f32.to_bits() + 1), 0.0, 0.0], + ]; + let t = tree(&points); + let good = upload(&runtime, &points, &t, 0.5, 3); + let w = words(&runtime, &good.covariances); + assert!(w[..8].iter().all(|v| *v == 0)); + // Legacy sweep/rebuild retains f32 orthogonality rounding even on an axial plane. + for (word, expected) in w[8..14].iter().zip([1.0_f32, 1.0, 0.001, 0.0, 0.0, 0.0]) { + assert!((f32::from_bits(*word) - expected).abs() < 1e-6); + } + let bad = upload(&runtime, &points, &t, 0.5, 2); + assert_eq!(words(&runtime, &bad.covariances)[0], 8); + let t = tree(&[[0.0; 3]]); + for epsilon in [1e-4, 0.1, 1.0] { + let good = GpuGicpInput::upload_with_gpu_grid_covariances( + &runtime, + (&[0.0], &[0.0], &[0.0]), + &t, + 0.5, + u32::MAX, + epsilon, + ) + .unwrap(); + let w = words(&runtime, &good.covariances); + assert_eq!(&w[8..14], &[epsilon, epsilon, epsilon, 0.0, 0.0, 0.0].map(f32::to_bits)); + } + let points = [[0.0; 3], [0.25, 0.0, 0.0], [0.5, 0.0, 0.0]]; + let t = tree(&points); + for case in 0..2 { + let positions = GpuIcpInput::upload(&runtime, &points, &t).unwrap(); + let params = Params { + origin: [0.0; 4], + dims: [1, 1, 1, 3], + inv_cell: 1.0, + radius_sq: 1.0, + epsilon: 0.001, + capacity: 3, + offset: 0, + total: 6, + padding: [0; 2], + }; + let uniform = runtime.device().create_buffer_init(&wgpu::util::BufferInitDescriptor { + label: None, + contents: bytemuck::bytes_of(¶ms), + usage: wgpu::BufferUsages::UNIFORM, + }); + let sorted = if case == 0 { [u32::MAX, 1, 2] } else { [0, 1, 2] }; + let starts = if case == 0 { [0u32, 3] } else { [0u32, 4] }; + let aux = [bytemuck::cast_slice(&sorted), bytemuck::cast_slice(&starts)].map(|data| { + runtime.device().create_buffer_init(&wgpu::util::BufferInitDescriptor { + label: None, + contents: data, + usage: wgpu::BufferUsages::STORAGE, + }) + }); + let covariances = runtime.device().create_buffer(&wgpu::BufferDescriptor { + label: None, + size: 7 * 32, + usage: wgpu::BufferUsages::STORAGE | wgpu::BufferUsages::COPY_SRC, + mapped_at_creation: false, + }); + let cache = runtime + .gicp_grid_covariance_pipelines + .get_or_init(|| GridPipelines::new(runtime.device())); + let entries: Vec<_> = + [&uniform, positions.source.buffer().unwrap(), &aux[0], &aux[1], &covariances] + .iter() + .enumerate() + .map(|(i, b)| wgpu::BindGroupEntry { + binding: i as u32, + resource: b.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()); + for pipeline in [&cache.build, &cache.finalize] { + let mut pass = encoder.begin_compute_pass(&wgpu::ComputePassDescriptor { + label: None, + timestamp_writes: None, + }); + pass.set_pipeline(pipeline); + pass.set_bind_group(0, &group, &[]); + pass.dispatch_workgroups(1, 1, 1); + } + runtime.queue().submit(Some(encoder.finish())); + let w = words(&runtime, &covariances); + assert_eq!(w[0], 1); + assert!(w[1..].iter().all(|v| *v == 0)); + let transfers = positions.upload_transfers(); + let input = GpuGicpInput { + positions, + covariances, + weight_scale: 6000.0, + covariance_offset: 1, + transfers, + }; + let output = input.equations_resident(&runtime, Isometry3::identity(), 1.0).unwrap(); + let w = words(&runtime, output.buffer()); + assert_eq!(w[0], 16); + assert!(w[1..].iter().all(|v| *v == 0)); + assert!(output.download(&runtime).is_err()); + } +} + +#[test] +fn grid_equations_preserve_physical_weights_owned_outputs_and_concurrent_leases() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let other = WgpuRuntime::new_headless().unwrap(); + let points: Vec<_> = (0..65) + .map(|i| [(i % 5) as f32 * 0.125, (i / 5 % 5) as f32 * 0.125, (i / 25) as f32 * 0.125]) + .collect(); + let t = tree(&points); + let input = upload(&runtime, &points, &t, 0.25, 65); + let c = words(&runtime, &input.covariances); + let c: Vec<[f32; 6]> = + c[8..].chunks_exact(8).map(|r| std::array::from_fn(|i| f32::from_bits(r[i]))).collect(); + let old = GpuGicpInput::upload(&runtime, &points, &c[..65], &t, &c[65..]).unwrap(); + let pose = Isometry3::new(Quat::::identity(), Vec3::new(0.01, -0.005, 0.002)); + let result = input.equations_resident(&runtime, pose, 0.25).unwrap(); + let actual = result.download(&runtime).unwrap(); + let expected = + old.equations_resident(&runtime, pose, 0.25).unwrap().download(&runtime).unwrap(); + assert_eq!(actual.squared_error_sum, expected.squared_error_sum); + for a in 0..6 { + assert!( + (actual.gradient[a] * actual.weight_scale + - expected.gradient[a] * expected.weight_scale) + .abs() + < 2e-4 * (1.0 + (expected.gradient[a] * expected.weight_scale).abs()) + ); + for b in 0..6 { + assert!( + (actual.hessian[a][b] * actual.weight_scale + - expected.hessian[a][b] * expected.weight_scale) + .abs() + < 2e-4 * (1.0 + (expected.hessian[a][b] * expected.weight_scale).abs()) + ); + } + } + std::thread::scope(|scope| { + let jobs: Vec<_> = (0..4) + .map(|_| { + scope.spawn(|| { + for _ in 0..3 { + let next = upload(&runtime, &points, &t, 0.25, 65); + assert_eq!( + next.equations_resident(&runtime, pose, 0.25) + .unwrap() + .download(&runtime) + .unwrap(), + actual + ); + } + }) + }) + .collect(); + for job in jobs { + job.join().unwrap(); + } + }); + assert!(input.equations_resident(&other, pose, 0.25).is_err()); + drop(input); + assert_eq!(result.download(&runtime).unwrap(), actual); + assert!(result.download(&other).is_err()); +} diff --git a/crates/spatialrust-gpu/src/kernels/gicp_grid_covariances.rs b/crates/spatialrust-gpu/src/kernels/gicp_grid_covariances.rs new file mode 100644 index 00000000..954c43e2 --- /dev/null +++ b/crates/spatialrust-gpu/src/kernels/gicp_grid_covariances.rs @@ -0,0 +1,296 @@ +//! Radius grid covariance preparation without a source KD-tree or neighbor rows. +use super::{invalid, plan as equation_plan, GpuGicpInput}; +use crate::{ + kernels::point_estimators::{prepare_grid, Kind}, + scratch::ScratchBuffers, + GpuIcpInput, WgpuRuntime, +}; +use bytemuck::{Pod, Zeroable}; +use spatialrust_core::{SpatialError, SpatialResult, TransferDirection}; +use spatialrust_search::KdTree; +use wgpu::util::DeviceExt; + +#[repr(C)] +#[derive(Clone, Copy, Pod, Zeroable)] +struct Params { + origin: [f32; 4], + dims: [u32; 4], + inv_cell: f32, + radius_sq: f32, + epsilon: f32, + capacity: u32, + offset: u32, + total: u32, + padding: [u32; 2], +} + +fn grid_plan( + source: u64, + target: u64, + radius: f32, + capacity: u32, + epsilon: f32, + limits: &wgpu::Limits, +) -> SpatialResult { + let total = source.checked_add(target).ok_or_else(invalid)?; + if source == 0 + || target == 0 + || source > 1 << 24 + || target > 1 << 24 + || capacity == 0 + || !radius.is_finite() + || radius <= 0.0 + || !(radius * radius).is_normal() + || !(1e-4..=1.0).contains(&epsilon) + || limits.max_compute_workgroup_size_x < 256 + || limits.max_compute_invocations_per_workgroup < 256 + || total.div_ceil(256) > u64::from(limits.max_compute_workgroups_per_dimension) + || total + .checked_add(1) + .and_then(|n| n.checked_mul(8)) + .map_or(true, |n| n > u64::from(u32::MAX)) + { + return Err(invalid()); + } + equation_plan(source, total + 1, limits)?; + Ok((total + 1) * 32) +} + +impl GpuGicpInput { + /// Uploads original positions/target tree and prepares radius covariances + /// directly on GPU using CPU-built counting-sort grids. No source KD-tree, + /// neighborhood rows or covariance readback/re-upload is needed. + /// Capacity includes self/duplicates; overflow invalidates both sets/all + /// equations. Dense grids must fit the existing cell/device limits; there + /// is no automatic switch to the bounded-neighborhood or CPU paths. + /// The existing grid's raw f32 means, central sums and 16 three-pivot sweeps + /// are retained, including sparse epsilon*I. Numerical failure rejects. + /// Requires nonempty finite equal-length source axes/target tree, positive + /// capacity/radius with normal finite f32 square, epsilon=1e-4..=1, and + /// supported reduction limits. Each dataset is bounded to 2^24 points for + /// exact f32 support counts. CPU grid/tree preparation remains explicit. + /// H2D is 20*S+36*T+32*target_nodes+4*(source_cells+target_cells+2); + /// there is no preparation D2H. Prepared inputs own covariance storage. + pub fn upload_with_gpu_grid_covariances( + runtime: &WgpuRuntime, + source: (&[f32], &[f32], &[f32]), + target: &KdTree, + radius: f32, + capacity: u32, + epsilon: f32, + ) -> SpatialResult { + let (x, y, z) = source; + for axis in [y, z] { + if axis.len() != x.len() { + return Err(SpatialError::BufferLengthMismatch { + expected: x.len(), + found: axis.len(), + }); + } + } + let target_axes = target.layout().positions(); + let targets = target_axes.0.len(); + let bytes = grid_plan( + x.len() as u64, + targets as u64, + radius, + capacity, + epsilon, + &runtime.device().limits(), + )?; + let grids = [ + prepare_grid(runtime, Kind::PlaneCovariances, x, y, z, radius)?, + prepare_grid( + runtime, + Kind::PlaneCovariances, + target_axes.0, + target_axes.1, + target_axes.2, + radius, + )?, + ]; + let points: Vec<_> = (0..x.len()).map(|i| [x[i], y[i], z[i]]).collect(); + let positions = GpuIcpInput::upload(runtime, &points, target)?; + let covariances = runtime.device().create_buffer(&wgpu::BufferDescriptor { + label: Some("gicp-grid-covariances"), + size: bytes, + usage: wgpu::BufferUsages::STORAGE | wgpu::BufferUsages::COPY_SRC, + mapped_at_creation: false, + }); + let total = (x.len() + targets) as u32; + let counts = [x.len() as u32, targets as u32]; + let offsets = [0, x.len() as u32]; + let cache = runtime + .gicp_grid_covariance_pipelines + .get_or_init(|| GridPipelines::new(runtime.device())); + let mut scratch = ScratchBuffers::new(runtime); + let mut transfers = positions.upload_transfers(); + let groups: Vec<_> = grids + .iter() + .enumerate() + .map(|(i, (origin, dims, sorted, starts))| { + let params = Params { + origin: [origin[0], origin[1], origin[2], 0.0], + dims: [dims[0], dims[1], dims[2], counts[i]], + inv_cell: 1.0 / radius, + radius_sq: radius * radius, + epsilon, + capacity, + offset: offsets[i], + total, + padding: [0; 2], + }; + let uniform = + runtime.device().create_buffer_init(&wgpu::util::BufferInitDescriptor { + label: Some("gicp-grid-params"), + contents: bytemuck::bytes_of(¶ms), + usage: wgpu::BufferUsages::UNIFORM, + }); + let aux = [sorted, starts].map(|words| { + let data = bytemuck::cast_slice(words); + let buffer = scratch.acquire("gicp-grid-index", data.len() as u64); + runtime.queue().write_buffer(&buffer, 0, data); + transfers.record(TransferDirection::HostToDevice, data.len() as u64); + buffer + }); + let queries = if i == 0 { &positions.source } else { &positions.target }; + let buffers = [&uniform, queries.buffer().unwrap(), &aux[0], &aux[1], &covariances]; + let entries: Vec<_> = buffers + .iter() + .enumerate() + .map(|(binding, buffer)| wgpu::BindGroupEntry { + binding: binding as u32, + resource: buffer.as_entire_binding(), + }) + .collect(); + runtime.device().create_bind_group(&wgpu::BindGroupDescriptor { + label: Some("gicp-grid-bindings"), + layout: &cache.layout, + entries: &entries, + }) + }) + .collect(); + let mut encoder = runtime.device().create_command_encoder(&Default::default()); + { + let mut pass = encoder.begin_compute_pass(&wgpu::ComputePassDescriptor { + label: Some("gicp-grid-build"), + timestamp_writes: None, + }); + pass.set_pipeline(&cache.build); + for (i, group) in groups.iter().enumerate() { + pass.set_bind_group(0, group, &[]); + pass.dispatch_workgroups(counts[i].div_ceil(256), 1, 1); + } + } + { + let mut pass = encoder.begin_compute_pass(&wgpu::ComputePassDescriptor { + label: Some("gicp-grid-finalize"), + timestamp_writes: None, + }); + pass.set_pipeline(&cache.finalize); + pass.set_bind_group(0, &groups[0], &[]); + pass.dispatch_workgroups(total.div_ceil(256), 1, 1); + } + scratch.submit_and_recycle(encoder); + Ok(Self { + positions, + covariances, + weight_scale: 6.0 / epsilon, + covariance_offset: 1, + transfers, + }) + } +} + +// Adapt only storage access/output and add bounded-support checks to the one +// canonical legacy grid kernel. Its arithmetic/order is not duplicated. Exact +// anchors deliberately fail tests if the canonical interface changes. +fn shader_source() -> String { + let old = crate::kernels::covariances_grid::COV_GRID_WGSL; + let mut body = old[old.find("fn rotate(").expect("canonical grid rotate")..].to_owned(); + for (axis, component) in [("xs", "x"), ("ys", "y"), ("zs", "z")] { + for index in ["i", "j"] { + let from = format!("{axis}[{index}]"); + assert!(body.contains(&from)); + body = body.replace(&from, &format!("points[{index}].{component}")); + } + } + let edits=[ + ("count = count + 1.0;", "count = count + 1.0;\n if u32(count) > params.capacity { atomicOr(&result.status,8u); return; }"), + ("out_cov[i] = vec4(params.epsilon, params.epsilon, params.epsilon, 0.0);", "store_covariance(i,vec4(params.epsilon,params.epsilon,params.epsilon,0.0),vec4(0.0));"), + ("out_cov[params.dims.w + i] = vec4(0.0);", ""), + ("out_cov[i] = vec4(reg[0], reg[1], reg[2], reg[3]);", "store_covariance(i,vec4(reg[0],reg[1],reg[2],reg[3]),vec4(reg[4],reg[5],0.0,0.0));"), + ("out_cov[params.dims.w + i] = vec4(reg[4], reg[5], 0.0, 0.0);", ""), + ]; + for (from, to) in edits { + assert_eq!(body.matches(from).count(), 1); + body = body.replace(from, to); + } + for from in ["let end = cell_start[row + after_x];", "let j = sorted[s];"] { + assert_eq!(body.matches(from).count(), 2); + let check = if from.starts_with("let end") { + "if begin>end || end>params.dims.w {atomicOr(&result.status,1u);return;}" + } else { + "if j>=params.dims.w {atomicOr(&result.status,1u);return;}" + }; + body = body.replace(from, &format!("{from}\n {check}")); + } + // The legacy recycled-output comment is irrelevant to owned packed rows. + body = + body.replace("// Recycled output storage is uninitialized; overwrite the second row.", ""); + format!("{}\n{body}", include_str!("../shaders/gicp_grid_covariances.wgsl")) +} + +pub(crate) struct GridPipelines { + layout: wgpu::BindGroupLayout, + build: wgpu::ComputePipeline, + finalize: wgpu::ComputePipeline, +} +impl GridPipelines { + fn new(device: &wgpu::Device) -> Self { + let entries: Vec<_> = (0..5) + .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 != 4 } + }, + has_dynamic_offset: false, + min_binding_size: None, + }, + count: None, + }) + .collect(); + let layout = device.create_bind_group_layout(&wgpu::BindGroupLayoutDescriptor { + label: Some("gicp-grid-layout"), + entries: &entries, + }); + let pipeline_layout = device.create_pipeline_layout(&wgpu::PipelineLayoutDescriptor { + label: Some("gicp-grid-pipeline-layout"), + bind_group_layouts: &[&layout], + push_constant_ranges: &[], + }); + let shader = device.create_shader_module(wgpu::ShaderModuleDescriptor { + label: Some("gicp-grid-shader"), + source: wgpu::ShaderSource::Wgsl(shader_source().into()), + }); + let pipeline = |entry| { + device.create_compute_pipeline(&wgpu::ComputePipelineDescriptor { + label: Some("gicp-grid-pipeline"), + layout: Some(&pipeline_layout), + module: &shader, + entry_point: Some(entry), + compilation_options: Default::default(), + cache: None, + }) + }; + Self { build: pipeline("main"), finalize: pipeline("finalize"), layout } + } +} + +#[cfg(all(test, not(target_arch = "wasm32")))] +#[path = "gicp_grid_covariance_tests.rs"] +mod tests; diff --git a/crates/spatialrust-gpu/src/kernels/gicp_reduction.rs b/crates/spatialrust-gpu/src/kernels/gicp_reduction.rs index 1293b7f6..401aa1da 100644 --- a/crates/spatialrust-gpu/src/kernels/gicp_reduction.rs +++ b/crates/spatialrust-gpu/src/kernels/gicp_reduction.rs @@ -526,6 +526,9 @@ impl GicpReductionPipelines { #[path = "gicp_covariances.rs"] pub(crate) mod resident_covariances; +#[path = "gicp_grid_covariances.rs"] +pub(crate) mod resident_grid_covariances; + #[cfg(all(test, not(target_arch = "wasm32")))] #[path = "gicp_reduction_tests.rs"] mod tests; diff --git a/crates/spatialrust-gpu/src/runtime.rs b/crates/spatialrust-gpu/src/runtime.rs index 69a2d018..1cf09eca 100644 --- a/crates/spatialrust-gpu/src/runtime.rs +++ b/crates/spatialrust-gpu/src/runtime.rs @@ -34,6 +34,8 @@ pub struct WgpuRuntime { OnceLock, pub(crate) gicp_covariance_pipelines: OnceLock, + pub(crate) gicp_grid_covariance_pipelines: + OnceLock, pub(crate) gicp_reduction_pipelines: OnceLock, pub(crate) point_estimator_pipelines: @@ -405,6 +407,7 @@ impl WgpuRuntime { icp_reduction_pipelines: OnceLock::new(), gicp_reduction_pipelines: OnceLock::new(), gicp_covariance_pipelines: OnceLock::new(), + gicp_grid_covariance_pipelines: OnceLock::new(), point_to_plane_reduction_pipelines: OnceLock::new(), point_estimator_pipelines: OnceLock::new(), max_gather_channels, diff --git a/crates/spatialrust-gpu/src/shaders/gicp_grid_covariances.wgsl b/crates/spatialrust-gpu/src/shaders/gicp_grid_covariances.wgsl new file mode 100644 index 00000000..1ef9ed32 --- /dev/null +++ b/crates/spatialrust-gpu/src/shaders/gicp_grid_covariances.wgsl @@ -0,0 +1,32 @@ +// Storage adapter/validation for the canonical radius-grid covariance kernel. +struct Params { + origin:vec4, dims:vec4, inv_cell:f32, radius_sq:f32, + epsilon:f32, capacity:u32, offset:u32, total:u32, padding:vec2, +} +struct Covariance { diagonal:vec4, off:vec4 } +struct Covariances { status:atomic, reserved:array, rows:array } +@group(0) @binding(0) var params:Params; +@group(0) @binding(1) var points:array>; +@group(0) @binding(2) var sorted:array; +@group(0) @binding(3) var cell_start:array; +@group(0) @binding(4) var result:Covariances; +fn finite(x:f32)->bool {return abs(x)<=3.4028234663852886e38;} +fn store_covariance(i:u32,diagonal:vec4,off:vec4) { + // Packed Cholesky and inverse trace validate exactly the stored values. + let xx=diagonal.x;let yy=diagonal.y;let zz=diagonal.z; + let xy=diagonal.w;let xz=off.x;let yz=off.y; + if !finite(xx)||!finite(yy)||!finite(zz)||!finite(xy)||!finite(xz)||!finite(yz)||xx<=0.0 {atomicOr(&result.status,4u);return;} + let a=sqrt(xx);let b=xy/a;let d=xz/a;let pivot1=yy-b*b; + if pivot1<=0.0 {atomicOr(&result.status,4u);return;} + let e=sqrt(pivot1);let f=(yz-d*b)/e;let pivot2=zz-d*d-f*f; + if pivot2<=0.0 {atomicOr(&result.status,4u);return;} + let g=sqrt(pivot2); + let i00=1.0/a;let i10=-b/(a*e);let i11=1.0/e;let i20=(b*f/e-d)/(a*g);let i21=-f/(e*g);let i22=1.0/g; + let trace=i00*i00+i10*i10+i11*i11+i20*i20+i21*i21+i22*i22; + if !finite(trace)||trace>6.0/params.epsilon {atomicOr(&result.status,4u);return;} + result.rows[params.offset+i]=Covariance(diagonal,off); +} +@compute @workgroup_size(256) +fn finalize(@builtin(global_invocation_id) id:vec3) { + if id.x(0.0),vec4(0.0));} +} diff --git a/crates/spatialrust-registration/src/gicp_reduction.rs b/crates/spatialrust-registration/src/gicp_reduction.rs index 78001ea9..604e3e29 100644 --- a/crates/spatialrust-registration/src/gicp_reduction.rs +++ b/crates/spatialrust-registration/src/gicp_reduction.rs @@ -67,6 +67,7 @@ enum CovariancePreparation { Legacy, ResidentKnn, ResidentRadius(u32), + ResidentGrid(u32), } impl GicpRegistration { @@ -132,6 +133,30 @@ impl GicpRegistration { ) } + /// Prepares radius covariances with CPU counting-sort grids and GPU gather, + /// retaining the canonical grid's raw f32 means/central sums/Jacobi sweeps. + /// No source KD-tree, per-point neighborhood buffer or covariance readback + /// is needed. Positive capacity includes self/duplicates; overflow rejects + /// all equations. Configured radius/epsilon and normal-equation constraints + /// match the resident-radius API. Dense grids must fit existing cell/device + /// limits; no automatic sparse-path/CPU fallback occurs. CPU prepares grids + /// and the target tree and solves 6x6 equations. Downloads remain 256 bytes + /// per evaluation; all grid-index uploads are included in the receipt. + pub fn align_with_gpu_resident_grid_covariances( + &self, + runtime: &WgpuRuntime, + source: &PointCloud, + target: &PointCloud, + neighbor_capacity: u32, + ) -> SpatialResult { + self.align_reduction_impl( + runtime, + source, + target, + CovariancePreparation::ResidentGrid(neighbor_capacity), + ) + } + fn align_reduction_impl( &self, runtime: &WgpuRuntime, @@ -150,7 +175,9 @@ impl GicpRegistration { { return Err(SpatialError::InvalidArgument("resident GPU GICP covariances require covariance_radius=None, k_neighbors=3..=64 and epsilon (as f32) in 1e-4..=1".to_owned())); } - if let CovariancePreparation::ResidentRadius(capacity) = preparation { + if let CovariancePreparation::ResidentRadius(capacity) + | CovariancePreparation::ResidentGrid(capacity) = preparation + { if capacity == 0 || !(1e-4..=1.0).contains(&epsilon) || c.covariance_radius @@ -184,7 +211,17 @@ impl GicpRegistration { let tree = KdTree::from_slices(tx, ty, tz); let mut transfers = TransferStats::default(); let mut covariance_batches = 0; - let input = if !matches!(preparation, CovariancePreparation::Legacy) { + let input = if let CovariancePreparation::ResidentGrid(capacity) = preparation { + covariance_batches = 2; + GpuGicpInput::upload_with_gpu_grid_covariances( + runtime, + (sx, sy, sz), + &tree, + c.covariance_radius.unwrap(), + capacity, + epsilon, + )? + } else if !matches!(preparation, CovariancePreparation::Legacy) { let source_tree = KdTree::from_slices(sx, sy, sz); covariance_batches = 2; match preparation { @@ -208,7 +245,9 @@ impl GicpRegistration { epsilon, )? } - CovariancePreparation::Legacy => unreachable!(), + CovariancePreparation::Legacy | CovariancePreparation::ResidentGrid(_) => { + unreachable!() + } } } else { let mut prepare = |x: &[f32], diff --git a/crates/spatialrust-registration/tests/gpu_gicp_resident_grid_covariances.rs b/crates/spatialrust-registration/tests/gpu_gicp_resident_grid_covariances.rs new file mode 100644 index 00000000..b5d584f2 --- /dev/null +++ b/crates/spatialrust-registration/tests/gpu_gicp_resident_grid_covariances.rs @@ -0,0 +1,214 @@ +#![cfg(all(feature = "register-gicp-gpu", not(target_arch = "wasm32")))] +#[path = "gpu_gicp_reduction.rs"] +mod reference; +use spatialrust_core::{HasPositions3, PointCloudBuilder}; +use spatialrust_gpu::WgpuRuntime; +use spatialrust_math::{Isometry3, Quat, TransformPoint, Vec3}; +use spatialrust_registration::{transform_point_cloud, GicpConfig, GicpRegistration}; +use spatialrust_search::{KdTree, NearestNeighborIndex}; + +#[test] +#[ignore = "requires a native wgpu adapter; run explicitly with --include-ignored"] +fn resident_grid_gicp_matches_cpu_poses_stopping_fitness_and_transfer_receipts() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let mut maximum = 0.0_f32; + let mut compared = 0; + for n in [17, 63, 64, 65, 257, 1366] { + for scale in [1e-4, 1.0, 1e4] { + for radius in [0.075, 0.25, 0.5] { + for stop in 0..3 { + let target = reference::corner(n, scale); + let source = reference::shifted(&target, scale); + let saved_source = source.clone(); + let saved_target = target.clone(); + let mut config = + reference::registration(scale, Some(radius * scale), stop).config(); + config.k_neighbors = 6; + let registration = GicpRegistration::new(config); + let cpu = registration.align_with_diagnostics(&source, &target).unwrap(); + let previous = + registration.align_with_gpu_reduction(&runtime, &source, &target).unwrap(); + let actual = registration + .align_with_gpu_resident_grid_covariances(&runtime, &source, &target, 128) + .unwrap_or_else(|e| { + panic!("n={n} scale={scale} radius={radius} stop={stop}: {e}") + }); + maximum = maximum.max( + reference::compare_pose( + &source, + cpu.transform, + actual.result.transform, + scale, + ) / scale, + ); + reference::compare_pose( + &source, + previous.result.transform, + actual.result.transform, + scale, + ); + compared += source.len(); + assert_eq!(actual.result.iterations, cpu.iterations); + assert_eq!(actual.result.converged, cpu.converged); + assert_eq!(actual.covariance_batches, 2); + assert_eq!(actual.evaluation_batches, actual.result.iterations + 1); + assert_eq!(actual.history.len(), actual.result.iterations); + let (sx, sy, sz) = source.positions3().unwrap(); + let (tx, ty, tz) = target.positions3().unwrap(); + let tt = KdTree::from_slices(tx, ty, tz); + let cells = [&source, &target] + .iter() + .map(|c| { + let (x, y, z) = c.positions3().unwrap(); + spatialrust_search::grid_bounds(x, y, z, radius * scale) + .unwrap() + .1 + .into_iter() + .map(u64::from) + .product::() + }) + .sum::(); + assert_eq!( + actual.transfers.host_to_device_bytes(), + 20 * source.len() as u64 + + 36 * target.len() as u64 + + 32 * tt.layout().nodes().len() as u64 + + 4 * (cells + 2) + ); + assert_eq!( + actual.transfers.device_to_host_bytes(), + actual.evaluation_batches as u64 * 256 + ); + let mut error = 0.0; + for i in 0..source.len() { + let p = + actual.result.transform.transform_point(Vec3::new(sx[i], sy[i], sz[i])); + let j = tt.nearest_one(p.x, p.y, p.z).unwrap().index; + error += f64::from(p.x - tx[j]).powi(2) + + f64::from(p.y - ty[j]).powi(2) + + f64::from(p.z - tz[j]).powi(2); + } + error /= source.len() as f64; + assert!( + (actual.result.fitness - error).abs() + <= 1e-9 * f64::from(scale).powi(2) + 1e-4 * error + ); + assert!(actual.history.iter().all(|row| row.correspondences >= 6 + && row.evaluated_correspondences >= 6 + && row.fitness.is_finite() + && row.update_magnitude.is_finite())); + assert_eq!(source, saved_source); + assert_eq!(target, saved_target); + } + } + } + } + eprintln!("GPU_RESIDENT_GRID_GICP compared {compared} points in 162 conditions; maximum scale-normalized CPU pose-coordinate error {maximum}"); +} + +#[test] +#[ignore = "requires a native wgpu adapter; run explicitly with --include-ignored"] +fn resident_grid_gicp_rejects_unsupported_settings_and_preserves_concurrent_calls() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let target = reference::corner(85, 1.0); + let (x, y, z) = target.positions3().unwrap(); + let mut b = PointCloudBuilder::xyz(); + for i in (0..target.len()).step_by(2) { + b.push_point([x[i] + 0.015625, y[i], z[i] - 0.0078125]).unwrap(); + } + for i in 0..7 { + b.push_point([20.0 + i as f32, 20.0, 20.0]).unwrap(); + } + let source = b.build().unwrap(); + let registration = reference::registration(1.0, Some(0.25), 0); + let actual = registration + .align_with_gpu_resident_grid_covariances(&runtime, &source, &target, 128) + .unwrap(); + let cpu = registration.align_with_diagnostics(&source, &target).unwrap(); + reference::compare_pose(&source, cpu.transform, actual.result.transform, 1.0); + assert!(actual.history.iter().all(|row| row.correspondences as usize == source.len() - 7)); + std::thread::scope(|scope| { + let callers: Vec<_> = (0..4) + .map(|_| { + scope.spawn(|| { + assert_eq!( + registration + .align_with_gpu_resident_grid_covariances( + &runtime, &source, &target, 128 + ) + .unwrap(), + actual + ); + }) + }) + .collect(); + for c in callers { + c.join().unwrap(); + } + }); + for config in [ + GicpConfig { k_neighbors: 2, ..Default::default() }, + GicpConfig { epsilon: 1e-5, ..Default::default() }, + GicpConfig { epsilon: 1.1, ..Default::default() }, + GicpConfig { epsilon: f64::NAN, ..Default::default() }, + GicpConfig { max_iterations: 0, ..Default::default() }, + GicpConfig { min_correspondences: 5, ..Default::default() }, + GicpConfig { transformation_epsilon: -1.0, ..Default::default() }, + ] { + let config = GicpConfig { covariance_radius: Some(0.25), ..config }; + assert!(GicpRegistration::new(config) + .align_with_gpu_resident_grid_covariances(&runtime, &source, &target, 128) + .is_err()); + } + assert!(GicpRegistration::new(GicpConfig::default()) + .align_with_gpu_resident_grid_covariances(&runtime, &source, &target, 128) + .is_err()); + for radius in [0.0, -1.0, f32::NAN, f32::INFINITY, 1e20, 1e-22] { + assert!(GicpRegistration::new(GicpConfig { + covariance_radius: Some(radius), + ..Default::default() + }) + .align_with_gpu_resident_grid_covariances(&runtime, &source, &target, 128) + .is_err()); + } + for capacity in [0, 1] { + assert!(registration + .align_with_gpu_resident_grid_covariances(&runtime, &source, &target, capacity) + .is_err()); + } + let far = transform_point_cloud( + &target, + Isometry3::new(Quat::::identity(), Vec3::new(100.0, 0.0, 0.0)), + ) + .unwrap(); + assert!(registration + .align_with_gpu_resident_grid_covariances(&runtime, &far, &target, 128) + .unwrap_err() + .to_string() + .contains("only 0 correspondences")); + let mut b = PointCloudBuilder::xyz(); + b.push_point([f32::INFINITY, 0.0, 0.0]).unwrap(); + let bad = b.build().unwrap(); + assert!(registration + .align_with_gpu_resident_grid_covariances(&runtime, &bad, &target, 128) + .is_err()); +} + +#[test] +#[ignore = "requires a native wgpu adapter; run explicitly with --include-ignored"] +fn resident_grid_gicp_requires_dense_grid_capacity_without_automatic_fallback() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let cloud = reference::corner(1366, 1.0); + let registration = + GicpRegistration::new(GicpConfig { covariance_radius: Some(0.01), ..Default::default() }); + assert!(registration + .align_with_gpu_resident_grid_covariances(&runtime, &cloud, &cloud, 2) + .unwrap_err() + .to_string() + .contains("cells")); + let actual = registration + .align_with_gpu_resident_radius_covariances(&runtime, &cloud, &cloud, 2) + .unwrap(); + assert!(actual.result.converged); + assert_eq!(actual.result.fitness, 0.0); +} diff --git a/crates/spatialrust-registration/tests/gpu_gicp_resident_grid_covariances_benchmark.rs b/crates/spatialrust-registration/tests/gpu_gicp_resident_grid_covariances_benchmark.rs new file mode 100644 index 00000000..024e5fca --- /dev/null +++ b/crates/spatialrust-registration/tests/gpu_gicp_resident_grid_covariances_benchmark.rs @@ -0,0 +1,214 @@ +#![cfg(all(feature = "register-gicp-gpu", not(target_arch = "wasm32")))] +#[path = "gpu_gicp_reduction.rs"] +mod reference; +use spatialrust_core::{HasPositions3, PointCloud, PointCloudBuilder}; +use spatialrust_gpu::{GpuGicpInput, WgpuRuntime}; +use spatialrust_math::{Isometry3, Quat, Vec3}; +use spatialrust_registration::{transform_point_cloud, GicpConfig, GicpRegistration}; +use std::time::Instant; + +fn fixture(n: usize, profile: usize) -> PointCloud { + let mut builder = PointCloudBuilder::xyz(); + for i in 0..n { + let j = if profile == 2 { i / 2 } else { i }; + builder + .push_point([ + (j % 32) as f32 * 0.5, + (j / 32 % 32) as f32 * 0.5, + (j / 1024) as f32 * 0.5, + ]) + .unwrap(); + } + builder.build().unwrap() +} + +#[test] +#[ignore = "completed resident grid covariance GICP benchmark; run this exact test alone in release mode"] +fn completed_resident_grid_covariance_gicp_calls() { + let runtime = WgpuRuntime::new_headless().unwrap(); + let info = runtime.adapter_info(); + println!( + "GPU_GICP_RESIDENT_GRID_COVARIANCES_ENV backend={} device_type={}", + info.backend, info.device_type + ); + let limits = runtime.device().limits(); + println!("GPU_GICP_RESIDENT_GRID_COVARIANCES_LIMITS storage_bytes={} workgroups_per_dimension={} storage_bindings={}",limits.max_storage_buffer_binding_size,limits.max_compute_workgroups_per_dimension,limits.max_storage_buffers_per_shader_stage); + // `host` retains CPU matching/equations/solve; its radius covariances use + // the legacy shared GPU runtime. It is not a pure CPU radius benchmark. + for n in [257, 4097, 16384, 65537] { + for profile in 0..3 { + let cloud = fixture(n, profile); + let target = fixture(n + if profile == 2 { 17 } else { 0 }, profile); + let source = transform_point_cloud( + &cloud, + Isometry3::new(Quat::::identity(), Vec3::new(0.03125, -0.015625, 0.0078125)), + ) + .unwrap(); + let registration = GicpRegistration::new(GicpConfig { + max_iterations: 3, + max_correspondence_distance: 0.25, + k_neighbors: 20, + transformation_epsilon: 0.0, + fitness_epsilon: 0.0, + covariance_radius: Some([0.51, 0.8, 1.05][profile]), + ..Default::default() + }); + let host = registration.align_with_diagnostics(&source, &target).unwrap(); + let legacy = registration.align_with_gpu_reduction(&runtime, &source, &target).unwrap(); + let bounded = registration + .align_with_gpu_resident_radius_covariances( + &runtime, + &source, + &target, + [32, 64, 128][profile], + ) + .unwrap(); + let gpu = registration + .align_with_gpu_resident_grid_covariances( + &runtime, + &source, + &target, + [32, 64, 128][profile], + ) + .unwrap(); + assert_eq!(gpu.result.iterations, 3); + assert_eq!(gpu.evaluation_batches, 4); + assert_eq!(gpu.covariance_batches, 2); + let maximum = + reference::compare_pose(&source, host.transform, gpu.result.transform, 1.0); + reference::compare_pose(&source, legacy.result.transform, gpu.result.transform, 1.0); + let (sx, sy, sz) = source.positions3().unwrap(); + let (tx, ty, tz) = target.positions3().unwrap(); + let tree = spatialrust_search::KdTree::from_slices(tx, ty, tz); + let input = GpuGicpInput::upload_with_gpu_grid_covariances( + &runtime, + (sx, sy, sz), + &tree, + [0.51, 0.8, 1.05][profile], + [32, 64, 128][profile], + 0.001, + ) + .unwrap(); + let cells = [&source, &target] + .iter() + .map(|c| { + let (x, y, z) = c.positions3().unwrap(); + spatialrust_search::grid_bounds(x, y, z, [0.51, 0.8, 1.05][profile]) + .unwrap() + .1 + .into_iter() + .map(u64::from) + .product::() + }) + .sum::(); + assert_eq!( + gpu.transfers.host_to_device_bytes(), + 20 * source.len() as u64 + + 36 * target.len() as u64 + + 32 * tree.layout().nodes().len() as u64 + + 4 * (cells + 2) + ); + assert_eq!(gpu.transfers.device_to_host_bytes(), 1024); + let expected = input + .equations_resident(&runtime, gpu.result.transform, 0.25) + .unwrap() + .download(&runtime) + .unwrap(); + assert_eq!(expected.correspondences as usize, source.len()); + reference::compare_pose(&source, bounded.result.transform, gpu.result.transform, 1.0); + for _ in 0..3 { + assert_eq!( + registration + .align_with_gpu_resident_radius_covariances( + &runtime, + &source, + &target, + [32, 64, 128][profile] + ) + .unwrap(), + bounded + ); + assert_eq!(registration.align_with_diagnostics(&source, &target).unwrap(), host); + assert_eq!( + registration.align_with_gpu_reduction(&runtime, &source, &target).unwrap(), + legacy + ); + assert_eq!( + registration + .align_with_gpu_resident_grid_covariances( + &runtime, + &source, + &target, + [32, 64, 128][profile] + ) + .unwrap(), + gpu + ); + assert_eq!( + input + .equations_resident(&runtime, gpu.result.transform, 0.25) + .unwrap() + .download(&runtime) + .unwrap(), + expected + ); + } + let (mut host_ms, mut legacy_ms, mut bounded_ms, mut gpu_ms, mut resident_ms) = + (vec![], vec![], vec![], vec![], vec![]); + for round in 0..20 { + for offset in 0..5 { + let stage = (round + offset) % 5; + let start = Instant::now(); + match stage { + 0 => { + let output = + registration.align_with_diagnostics(&source, &target).unwrap(); + host_ms.push(start.elapsed().as_secs_f64() * 1000.0); + assert_eq!(output, host); + } + 1 => { + let output = registration + .align_with_gpu_reduction(&runtime, &source, &target) + .unwrap(); + legacy_ms.push(start.elapsed().as_secs_f64() * 1000.0); + assert_eq!(output, legacy); + } + 2 => { + let output = registration + .align_with_gpu_resident_radius_covariances( + &runtime, + &source, + &target, + [32, 64, 128][profile], + ) + .unwrap(); + bounded_ms.push(start.elapsed().as_secs_f64() * 1000.0); + assert_eq!(output, bounded); + } + 3 => { + let output = registration + .align_with_gpu_resident_grid_covariances( + &runtime, + &source, + &target, + [32, 64, 128][profile], + ) + .unwrap(); + gpu_ms.push(start.elapsed().as_secs_f64() * 1000.0); + assert_eq!(output, gpu); + } + _ => { + let output = input + .equations_resident(&runtime, gpu.result.transform, 0.25) + .unwrap(); + runtime.wait_idle(); + resident_ms.push(start.elapsed().as_secs_f64() * 1000.0); + assert_eq!(output.download(&runtime).unwrap(), expected); + } + } + } + } + println!("GPU_GICP_RESIDENT_GRID_COVARIANCES_BENCH n={n} profile={profile} iterations=3 max_pose_error={maximum} radius={} capacity={} host_fitness={} gpu_fitness={} matches={} legacy_h2d={} legacy_d2h={} bounded_h2d={} bounded_d2h={} gpu_h2d={} gpu_d2h={} host_ms={host_ms:?} legacy_ms={legacy_ms:?} bounded_ms={bounded_ms:?} gpu_ms={gpu_ms:?} resident_ms={resident_ms:?}",[0.51,0.8,1.05][profile],[32,64,128][profile],host.fitness,gpu.result.fitness,expected.correspondences,legacy.transfers.host_to_device_bytes(),legacy.transfers.device_to_host_bytes(),bounded.transfers.host_to_device_bytes(),bounded.transfers.device_to_host_bytes(),gpu.transfers.host_to_device_bytes(),gpu.transfers.device_to_host_bytes()); + } + } +} diff --git a/crates/spatialrust/tests/gpu_gicp_resident_grid_covariances_smoke.rs b/crates/spatialrust/tests/gpu_gicp_resident_grid_covariances_smoke.rs new file mode 100644 index 00000000..c96fae88 --- /dev/null +++ b/crates/spatialrust/tests/gpu_gicp_resident_grid_covariances_smoke.rs @@ -0,0 +1,25 @@ +#![cfg(all(feature = "register-gicp-gpu", not(target_arch = "wasm32")))] +#[test] +#[ignore = "requires a native wgpu adapter; run explicitly with --include-ignored"] +fn facade_exposes_resident_gicp_grid_covariance_preparation() { + use spatialrust::{ + gpu::WgpuRuntime, GicpConfig, GicpRegistration, GpuGicpReductionDiagnostics, + PointCloudBuilder, + }; + let mut b = PointCloudBuilder::xyz(); + for i in 0..65 { + b.push_point([(i % 5) as f32 * 0.25, (i / 5 % 5) as f32 * 0.25, (i / 25) as f32 * 0.25]) + .unwrap(); + } + let cloud = b.build().unwrap(); + let runtime = WgpuRuntime::new_headless().unwrap(); + let actual: GpuGicpReductionDiagnostics = + GicpRegistration::new(GicpConfig { covariance_radius: Some(0.5), ..Default::default() }) + .align_with_gpu_resident_grid_covariances(&runtime, &cloud, &cloud, 65) + .unwrap(); + assert!(actual.result.converged); + assert_eq!(actual.result.fitness, 0.0); + assert_eq!(actual.evaluation_batches, 2); + assert_eq!(actual.covariance_batches, 2); + assert_eq!(actual.transfers.device_to_host_bytes(), 512); +} diff --git a/docs/ARCHITECTURE.md b/docs/ARCHITECTURE.md index d582a3cf..4f04e1ce 100644 --- a/docs/ARCHITECTURE.md +++ b/docs/ARCHITECTURE.md @@ -80,6 +80,19 @@ epsilon=1e-4..=1 are required. Upload/download ownership and receipts match resident k-NN. CPU trees/6x6 updates remain; legacy preparation is unchanged. See [resident radius evidence](../notes/2026-10-13_gpu_gicp_resident_radius_covariances.md). +`align_with_gpu_resident_grid_covariances` / `upload_with_gpu_grid_covariances` +offer explicit dense-grid preparation. CPU counting-sort indices/offsets are +uploaded into private scratch leased through one submission. Two grid gathers +share the existing original query/sample buffers and write directly into owned +packed covariances; a final pass zeros both sets on invalid data/support/SPD. +The canonical grid arithmetic is reused, preserving raw f32 means, central sums +and 16 three-pivot sweeps. Support capacity includes self/duplicates but requires +no neighborhood allocation; overflow rejects all equations. Source KD-tree +construction/upload is removed. Upload is 20*S+36*T+32*target_nodes plus four +bytes per grid offset entry. CPU grid construction/target tree/6x6 solve remain; +dense-grid cell limits reject without automatically selecting another route. +See [resident grid evidence](../notes/2026-10-13_gpu_gicp_resident_grid_covariances.md). + `PointToPlaneIcp::align_with_gpu_reduction` under the optional `register-icp-point-to-plane-gpu` feature retains original positions/tree and raw target normals. Five passes transform originals, perform the existing diff --git a/docs/FEATURE_MATRIX.md b/docs/FEATURE_MATRIX.md index b0ee988a..34982664 100644 --- a/docs/FEATURE_MATRIX.md +++ b/docs/FEATURE_MATRIX.md @@ -147,6 +147,18 @@ differ from grid raw means/sweeps. Upload is 32*(S+T+source_nodes+target_nodes), download is only 256 bytes per evaluation; CPU trees and 6x6 solve remain. See [resident radius evidence](../notes/2026-10-13_gpu_gicp_resident_radius_covariances.md). +`GicpRegistration::align_with_gpu_resident_grid_covariances` and +`GpuGicpInput::upload_with_gpu_grid_covariances` prepare radius covariances +directly from resident original positions and CPU-built counting-sort grids. +The canonical grid f32 math is retained, with complete support bounded by the +caller's capacity including self/duplicates. No source KD-tree, neighborhood +row storage or covariance readback/re-upload is needed. Dense grids must fit +existing cell/device limits; sparse selection remains explicit. Grid indices +are leased scratch, covariance/equation outputs owned. Invalid support/data/SPD +zero all covariances and reject equations. CPU grids/target tree/6x6 solves +remain; download is 256 bytes per evaluation with complete upload receipts. +See [resident grid evidence](../notes/2026-10-13_gpu_gicp_resident_grid_covariances.md). + `register-icp-point-to-plane-gpu` adds `PointToPlaneIcp::align_with_gpu_reduction` and diagnostic iteration history. `GpuPointToPlaneInput` under `gpu-wgpu` explicitly retains originals/tree and diff --git a/docs/ROADMAP.md b/docs/ROADMAP.md index 41afae30..97eb3a12 100644 --- a/docs/ROADMAP.md +++ b/docs/ROADMAP.md @@ -1137,4 +1137,5 @@ Dependencies and explicit transfers come before broader algorithm coverage. | Point-to-plane resident normal equations | Complete | Original positions/tree/raw normals, GPU transforms/matching/plane Hessian/gradient/fitness; 256-byte equations with CPU normalized solve/world conversion | Independent raw-normal f64 equations, 108 pose/scale/weight/stopping cases, gating/ownership/schema/receipt guards and completed CPU comparisons; `notes/2026-10-13_gpu_point_to_plane_reduction.md` | | 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 | diff --git a/notes/2026-10-13_gpu_gicp_resident_grid_covariances.md b/notes/2026-10-13_gpu_gicp_resident_grid_covariances.md new file mode 100644 index 00000000..4d28078e --- /dev/null +++ b/notes/2026-10-13_gpu_gicp_resident_grid_covariances.md @@ -0,0 +1,237 @@ +# Resident grid GICP covariance preparation — 2026-10-13 + +The bounded resident-radius preparation removed covariance transfers but added +source KD-tree construction/staging and N*(8+8*capacity) neighborhood storage. +This slice offers explicit `align_with_gpu_resident_grid_covariances` under +`register-gicp-gpu` for point sets whose existing dense radius grids fit. +It reuses the canonical grid covariance kernel on resident original positions, +with no source tree, neighborhood rows or covariance readback/re-upload. +CPU counting-sort grids, target tree and the existing 6x6 pose solve remain. +No default placement or automatic sparse/CPU fallback is added. + +## Data, ownership and transfer boundary + +`GpuGicpInput::upload_with_gpu_grid_covariances(runtime, (x,y,z), target_tree, +radius, capacity, epsilon)` validates source axes and builds both CPU grids +with the existing checked counting-sort preparation. Original source/target +query buffers supply GPU covariance samples. Sorted indices and CSR offsets +are the only new grid uploads. These private buffers are exclusively leased +through one encoder submission, then safely recycled in queue order. Two +source/target grid dispatches share one compute pass; a separate final pass +zeros BOTH covariance sets on any invalid status. No host wait is introduced. + +Covariance storage is owned, 32*(S+T+1) bytes: a 32-byte status prefix followed +by original-order source and target `[xx,yy,zz,xy,xz,yz,0,0]` rows. Status bits +1/4/8 represent invalid grid rows, nonfinite/non-SPD/unbounded covariance or +support overflow. Any failure sets equation status 16 and zeros ALL equation +payload before explicit download rejects it. Outputs survive input drop and +scratch reuse. No extra production diagnostic readback is required. + +H2D is **20*S+36*T+32*target_nodes+4*(source_cells+target_cells+2)** bytes. +This includes original positions/tree, sorted indices and grid sentinel +entries. Preparation D2H is zero. Registration downloads only **256*(I+1)** +bytes for I updates; `covariance_batches=2`. Device writes/control uniforms are +excluded consistently with existing receipts. Grid auxiliary storage is +4*(S+T+source_cells+target_cells+2) bytes, replacing both bounded neighborhood +buffers and the additional source-tree staging. Dense grids can still cost +more device memory for sparse extents; the caller selects a route explicitly. + +## Math and checked limits + +The one canonical radius-grid shader remains unchanged. The new shader adapter +changes only position loads, packed output stores and validation. Raw absolute +f32 means, central sums, inclusive radius predicate, counting-sort traversal +order, 16 sweeps of three Jacobi pivots and eigenvector covariance rebuild are +retained. Fewer than three samples use epsilon*I; coincident larger support +retains the legacy smallest-axis convention. f32 orthogonality rounding of +legacy sweeps is preserved, rather than replacing the rebuild with an ideal +rank-one formula. Static adaptation anchors and exact legacy-word tests guard +future canonical interface changes. + +Support capacity includes self/duplicates. The first pass rejects at capacity+1, +without truncating a covariance or allocating capacity-sized rows. Grid starts, +ends and original indices are checked before sample access. Packed f32 Cholesky +pivots and analytic inverse trace validate both covariance sets; global weight +normalization is 6/epsilon. The common equation/world-update loop is unchanged. + +Requires nonempty finite clouds, matching source axes, positive capacity, +epsilon as f32 in 1e-4..=1, and positive finite radius with normal f32 square. +Existing grid cell cap, reciprocal/dimension/storage/dispatch validation apply. +Each dataset is restricted to 2^24 points to keep legacy f32 support counts +exact, subject to tighter equation/device limits. 256-thread workgroups and +combined covariance/dispatch offsets are checked before CPU grid allocation. +Dense grid rejection is explicit; callers can use the existing bounded-radius +method for sparse extents. Numerical failure, including overflowing raw means +at huge absolute origins, propagates instead of silently changing arithmetic. +CPU/grid and centered bounded-radius math can still choose different normals, +poses or stopping near gates, repeated spectra and numerical limits. + +## Correctness evidence + +- Native source/unequal-target comparisons cover 1/2/3/63/64/65/257/1025 points, + three scales (1e-4/1/1e4) and three radii (0.075/0.25/0.5 times scale), with + duplicates, tilted planes and sparse support. **22,230 covariance rows / + 133,380 f32 values** match the previous grid API bit for bit; all new padding + and prefix reserved words are zero. Exact upload receipts match independent + cell/node counts; there is no covariance/neighborhood readback in production. +- Exact-radius and next-representable positions, sparse epsilon boundaries, + source/target support overflow, malformed sorted indices/grid offsets and + raw-mean overflow exercise prefix/global zeroing and equation rejection. +- Covariance-derived equations preserve Euclidean error and physically rescaled + Hessian/gradient tolerances. Owned equations survive input drops; mismatched + runtimes reject. Four concurrent input preparations repeatedly exercise + private grid scratch leases without changing outputs. +- **162** registration size/scale/radius/stopping conditions compare host updates + and previous GPU reduction, immutable inputs, independent Euclidean fitness + and exact complete receipts. Maximum scale-normalized host-update coordinate + difference is **9.536743e-7** across **148,392** points. The host reference + uses legacy shared-runtime GPU radius covariance, not pure CPU preparation. +- Unequal clouds/outliers and concurrent complete calls retain correct gated + counts. Dense cell overflow rejects this explicit method; the existing + bounded-radius method handles the tested sparse extent. Minimal facade smoke + and invalid configuration/limits are checked. + +## Completed-call measurements + +The exact native release benchmark is +`completed_resident_grid_covariance_gicp_calls` in +`C:\Users\rsasa\Workspace\SpatialRust\crates\spatialrust-registration\tests\gpu_gicp_resident_grid_covariances_benchmark.rs`. +Runtime creation is excluded. Three warmups precede 20 cyclic samples for +five stages: ordinary host updates, legacy GPU reduction, bounded resident +radius preparation, new resident grid preparation and prepared equations. +Complete calls include CPU grids/trees, uploads, preparation, all GPU waits, +readbacks, CPU solves/composition and final fitness. Prepared equations time +one completed evaluation on precomputed/uploaded input, excluding preparation, +upload, readback and CPU solve; this is a separate stage, not full-call latency. + +Four sizes (257/4097/16384/65537) use fixed 0.5 spacing, a 32x32 coordinate +grid, radii 0.51/0.8/1.05 and capacities 32/64/128. The last profile duplicates +each grid point and adds 17 targets. Sources start at binary translation +`(0.03125,-0.015625,0.0078125)`, distance gate 0.25, epsilon 0.001, three +updates and zero stopping thresholds. All warmed/timed results match their +warmed references; every final evaluation retains N correspondences. + +Ordinary **host updates** still use shared-runtime GPU radius covariances; +they are not a pure CPU radius baseline. The **legacy GPU** route reads back +and re-uploads grid covariances. The **bounded radius** route retains the +centered/scaled KD-tree-neighborhood precision path. All three completed-call +baselines are measured, preserving differences in arithmetic and preparation. +Surveys run alone without simultaneous local builds/tests/statistical work. +Backend/type/limits are recorded without the local adapter model. + +### Survey 1 + +| N | Radius / capacity | Host ms | Legacy GPU ms | Bounded radius ms | New grid ms | Prepared ms | Host/new [95%] | Legacy/new [95%] | Bounded/new [95%] | +| ---: | --- | ---: | ---: | ---: | ---: | ---: | --- | --- | --- | +| 257 | 0.51 / 32 | 2.711 | 7.175 | 5.210 | 5.005 | 0.257 | 0.54 [0.53–0.56] | 1.43 [1.40–1.47] | 1.02 [1.00–1.04] | +| 257 | 0.8 / 64 | 2.937 | 7.933 | 5.752 | 5.284 | 0.271 | 0.55 [0.53–0.57] | 1.50 [1.45–1.55] | 1.06 [1.02–1.10] | +| 257 | 1.05 / 128 | 2.758 | 7.665 | 5.375 | 5.235 | 0.249 | 0.51 [0.49–0.53] | 1.39 [1.30–1.47] | 0.98 [0.92–1.05] | +| 4,097 | 0.51 / 32 | 10.539 | 9.144 | 6.595 | 5.648 | 0.723 | 1.86 [1.77–1.94] | 1.60 [1.51–1.69] | 1.20 [1.15–1.24] | +| 4,097 | 0.8 / 64 | 10.797 | 10.538 | 9.297 | 6.138 | 0.887 | 1.73 [1.61–1.83] | 1.62 [1.50–1.72] | 1.52 [1.46–1.59] | +| 4,097 | 1.05 / 128 | 9.519 | 10.713 | 13.002 | 6.404 | 0.849 | 1.46 [1.42–1.50] | 1.62 [1.54–1.70] | 1.92 [1.81–2.04] | +| 16,384 | 0.51 / 32 | 41.818 | 20.641 | 21.481 | 15.122 | 2.196 | 2.64 [2.47–2.79] | 1.34 [1.24–1.44] | 1.45 [1.36–1.53] | +| 16,384 | 0.8 / 64 | 49.736 | 18.893 | 23.217 | 13.831 | 1.844 | 3.47 [3.34–3.61] | 1.36 [1.32–1.39] | 1.67 [1.64–1.71] | +| 16,384 | 1.05 / 128 | 44.878 | 16.087 | 20.483 | 11.324 | 1.051 | 3.81 [3.50–4.14] | 1.36 [1.25–1.47] | 1.88 [1.71–2.11] | +| 65,537 | 0.51 / 32 | 202.410 | 52.115 | 50.756 | 29.753 | 2.169 | 5.80 [5.14–6.48] | 1.47 [1.33–1.63] | 1.60 [1.46–1.78] | +| 65,537 | 0.8 / 64 | 210.300 | 46.296 | 48.961 | 34.220 | 2.261 | 5.62 [4.94–6.37] | 1.31 [1.22–1.43] | 1.58 [1.47–1.71] | +| 65,537 | 1.05 / 128 | 173.850 | 43.560 | 66.350 | 35.464 | 2.445 | 4.42 [3.98–4.86] | 1.18 [1.06–1.31] | 1.84 [1.66–2.07] | + +### Survey 2 + +| N | Radius / capacity | Host ms | Legacy GPU ms | Bounded radius ms | New grid ms | Prepared ms | Host/new [95%] | Legacy/new [95%] | Bounded/new [95%] | +| ---: | --- | ---: | ---: | ---: | ---: | ---: | --- | --- | --- | +| 257 | 0.51 / 32 | 4.768 | 13.285 | 9.253 | 9.208 | 0.411 | 0.54 [0.51–0.57] | 1.49 [1.41–1.59] | 1.08 [1.00–1.18] | +| 257 | 0.8 / 64 | 4.728 | 12.959 | 8.489 | 8.475 | 0.389 | 0.54 [0.52–0.57] | 1.49 [1.40–1.59] | 1.03 [0.98–1.08] | +| 257 | 1.05 / 128 | 3.266 | 8.322 | 6.215 | 6.181 | 0.306 | 0.52 [0.48–0.55] | 1.36 [1.29–1.42] | 1.01 [0.95–1.05] | +| 4,097 | 0.51 / 32 | 15.123 | 13.122 | 9.640 | 8.775 | 0.986 | 1.73 [1.56–1.93] | 1.54 [1.43–1.65] | 1.16 [1.09–1.23] | +| 4,097 | 0.8 / 64 | 21.513 | 17.068 | 14.783 | 11.950 | 1.227 | 1.74 [1.67–1.82] | 1.41 [1.35–1.49] | 1.23 [1.18–1.29] | +| 4,097 | 1.05 / 128 | 18.519 | 17.157 | 17.956 | 10.842 | 1.143 | 1.58 [1.45–1.70] | 1.48 [1.39–1.55] | 1.67 [1.59–1.74] | +| 16,384 | 0.51 / 32 | 67.336 | 28.589 | 30.716 | 21.468 | 3.076 | 3.38 [3.14–3.66] | 1.33 [1.23–1.42] | 1.42 [1.32–1.50] | +| 16,384 | 0.8 / 64 | 60.468 | 22.640 | 27.925 | 18.429 | 2.138 | 3.44 [3.16–3.76] | 1.29 [1.23–1.35] | 1.68 [1.59–1.77] | +| 16,384 | 1.05 / 128 | 56.347 | 19.219 | 24.583 | 13.971 | 1.168 | 3.98 [3.61–4.39] | 1.45 [1.37–1.54] | 2.03 [1.78–2.31] | +| 65,537 | 0.51 / 32 | 250.854 | 63.364 | 74.648 | 49.058 | 6.938 | 5.59 [4.85–6.45] | 1.40 [1.31–1.51] | 1.65 [1.56–1.77] | +| 65,537 | 0.8 / 64 | 234.957 | 59.528 | 70.864 | 47.342 | 2.336 | 5.17 [4.54–5.89] | 1.25 [1.16–1.35] | 1.56 [1.48–1.66] | +| 65,537 | 1.05 / 128 | 181.037 | 43.432 | 63.620 | 34.838 | 2.027 | 5.40 [5.06–5.79] | 1.36 [1.24–1.51] | 1.91 [1.67–2.24] | + +Times are medians in milliseconds. Ratios are geometric means of paired +baseline/new latency; >1 favors the new grid method. The 95% intervals use +10,000 paired log-ratio bootstrap draws, seed +20261013+36*(survey-1)+3*condition_index+baseline_index (host=0, legacy=1, +bounded=2), sorted endpoints 249/9749. Intervals describe each survey, not +portable backend/device gains, cold-runtime latency or a universal crossover. +Every sample, including adverse cases, is retained; large within/between-survey +variance is visible even in the separate prepared-equation stage. + +For every tested 4,097-or-more condition, new grid improves on legacy GPU by +1.18–1.62x and on bounded radius by 1.16–2.03x; within-survey intervals stay +above one. At 65,537 points, legacy/new is **1.18–1.47x**, bounded/new is +**1.56–1.91x**, and host-update/new is **4.42–5.80x**. This completes an +end-to-end latency improvement over the previous implementations in these +conditions, including CPU preparation and transfers. The measurements do not +separately attribute time to tree removal, neighborhood storage removal, +submission count or the retained grid precision path. + +At 257 points, host/new is 0.51–0.55: host updates remain faster. Legacy GPU/new +is 1.36–1.50, while bounded/new is about 0.98–1.08 with several intervals +crossing one. Small-cloud improvement over bounded radius is not established. +No automatic dispatch or default placement change is introduced. + +All timed outputs match their warmed references. Every final evaluation +retains N correspondences. Maximum measured host/new pose-coordinate error is +**5.9604645e-8**. Host fitness is zero; new fitness is at most +**1.7646659e-16**. Relative fitness error near zero is not useful. The covariance +values preserve the canonical grid; common weight normalization differs from +the legacy uploaded-covariance route and can change low-order pose rounding. +Both surveys pass (41.68/50.12 seconds excluding compilation), using Vulkan / +DiscreteGpu with storage binding limit 2,147,483,647 bytes and maximum dispatch +dimension 65,535. Eight storage bindings and 256-thread workgroups are required +by the full route. No local adapter model is included. + +| N | Radius | Legacy H2D | Bounded H2D | New grid H2D | Legacy D2H | Bounded / new D2H | +| ---: | ---: | ---: | ---: | ---: | ---: | ---: | +| 257 | 0.51 | 40,056 | 18,560 | 17,440 | 17,472 | 1,024 | +| 257 | 0.8 | 39,032 | 18,560 | 16,416 | 17,472 | 1,024 | +| 257 | 1.05 | 40,632 | 20,064 | 17,268 | 18,016 | 1,024 | +| 4,097 | 0.51 | 637,144 | 295,040 | 276,608 | 263,232 | 1,024 | +| 4,097 | 0.8 | 615,992 | 295,040 | 255,456 | 263,232 | 1,024 | +| 4,097 | 1.05 | 610,640 | 296,672 | 249,356 | 263,776 | 1,024 | +| 16,384 | 0.51 | 2,540,128 | 1,179,584 | 1,098,336 | 1,049,600 | 1,024 | +| 16,384 | 0.8 | 2,456,808 | 1,179,584 | 1,015,016 | 1,049,600 | 1,024 | +| 16,384 | 1.05 | 2,434,456 | 1,181,216 | 991,916 | 1,050,144 | 1,024 | +| 65,537 | 0.51 | 10,183,856 | 4,718,720 | 4,416,600 | 4,195,392 | 1,024 | +| 65,537 | 0.8 | 9,830,712 | 4,718,720 | 4,063,456 | 4,195,392 | 1,024 | +| 65,537 | 1.05 | 9,730,760 | 4,720,352 | 3,962,756 | 4,195,936 | 1,024 | + +Legacy-to-new H2D removes exactly **44*(S+T)** bytes: radius XYZ re-upload and +packed covariance upload. For 65,537 points, new H2D is +3,962,756–4,416,600 bytes versus legacy 9,730,760–10,183,856 (56.63–59.28% less), +and versus bounded 4,718,720–4,720,352 (6.40–16.05% less). D2H is still only +**1,024 bytes** for three updates, versus legacy 4,195,392–4,195,936 bytes +(99.9756% less), matching bounded radius. The duplicated 65,537/65,554 capacity +128 condition no longer allocates its 135,285,912-byte neighborhood payload. +These are logical payload/storage counts, not measured process/device peak +memory; pooling and driver resource lifetime can affect actual allocation. + +## Final verification and delivery + +- GPU all-feature library: **205 passed, 20 ignored**; integration targets: + **27 passed, 3 ignored**, **232 passing tests** in total. New grid tests and + prior resident covariance, independent f64 equations and neighborhood tests + pass together. +- Registration all-feature library: **35 passed, 1 ignored**. Three new native + grid tests, three bounded-radius tests and two k-NN tests pass, covering 162 + conditions per preparation family. Minimal grid facade smoke passes. +- GPU/registration all-feature/all-target Clippy, CPU-only GICP Clippy, optional + WASM registration compile, minimal facade check and workspace formatting pass. + WASM is compile checked, not browser executed. +- CPU GICP, canonical grid shader/API and the common post-preparation update/ + fitness loop are unchanged against parent 5688fc6. No core/math/search code, + dependency, unsafe or automatic placement changes are added. +- Measurements and bootstrap results: + `C:\Users\rsasa\Workspace\SpatialRust\target\automation\gpu-gicp-resident-grid-covariances-measurements.json`. + Survey logs: + `C:\Users\rsasa\Workspace\SpatialRust\target\automation\gpu-gicp-resident-grid-covariances-bench-1.log` + and + `C:\Users\rsasa\Workspace\SpatialRust\target\automation\gpu-gicp-resident-grid-covariances-bench-2.log`. + Published files and PR text omit the local adapter model; scan matches are zero. + The existing 124 named CI checks and pinned parent slice remain merge gates.