From 612aaed4b4ffd882426dbb6b307f51421fb20c45 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Wed, 27 May 2026 19:40:20 -0700 Subject: [PATCH 1/3] Add test --- dimos/mapping/ray_tracing/rust/src/main.rs | 64 ++++++++++++++++++++++ 1 file changed, 64 insertions(+) diff --git a/dimos/mapping/ray_tracing/rust/src/main.rs b/dimos/mapping/ray_tracing/rust/src/main.rs index 3950338bf8..83e4fb89a1 100644 --- a/dimos/mapping/ray_tracing/rust/src/main.rs +++ b/dimos/mapping/ray_tracing/rust/src/main.rs @@ -330,8 +330,19 @@ fn find_misses_along_ray( let shadow_sq = shadow_depth.powi(2); let grace_sq = grace_depth.powi(2); + let ray_len = (dx * dx + dy * dy + dz * dz).sqrt(); + let t_max = 1.0 + shadow_depth / ray_len.max(f32::EPSILON); + let mut past_endpoint = false; loop { + let t_enter = tx.min(ty).min(tz); + if t_enter > t_max { + return; + } + if t_enter >= 1.0 { + past_endpoint = true; + } + if tx < ty { if tx < tz { x += step_x; @@ -850,6 +861,59 @@ mod tests { assert!(cloud_points(&local).contains(&voxel_center(10, 10, 10))); } + /// Test how bad the planar ray clipping is. + /// For example, points on floors can be counted as misses because they are close to the same ray as the hit. + #[test] + fn ground_clipping_single_ray() { + let voxel_size = 0.1_f32; + let lidar_height = 1.0_f32; + let cfg = Config { + voxel_size, + max_range: 50.0, + ray_subsample: 1, + shadow_depth: 0.2, + grace_depth: 0.2, + min_health: 0, + max_health: 1, + }; + let inv = 1.0 / voxel_size; + + // Cover the full range we will probe, plus a little for shadow. + let max_x = 25.0_f32; + let n_ground = (max_x / voxel_size).ceil() as i32; + + let ranges: Vec = (1..=20).map(|i| i as f32).collect(); + eprintln!( + "voxel_size={voxel_size} lidar_height={lidar_height} grace={} shadow={}", + cfg.grace_depth, cfg.shadow_depth + ); + eprintln!("range_m ground_voxels_in_row clipped clipped_pct"); + for &range in &ranges { + let mut map = VoxelMap::default(); + for i in 0..n_ground { + let x = (i as f32) * voxel_size + voxel_size * 0.5; + let key = world_to_voxel(x, 0.0, 0.0, inv); + map.voxels.insert(key, cfg.max_health); + } + let n_before = map.voxels.len(); + + let origin = (0.0_f32, 0.0_f32, lidar_height); + let hits = vec![(range, 0.0_f32, 0.0_f32)]; + update_map(&mut map, origin, &hits, &cfg); + + let n_after_ground: usize = (0..n_ground) + .filter(|i| { + let x = (*i as f32) * voxel_size + voxel_size * 0.5; + let key = world_to_voxel(x, 0.0, 0.0, inv); + map.voxels.contains_key(&key) + }) + .count(); + let clipped = n_before - n_after_ground; + let pct = 100.0 * clipped as f32 / n_before as f32; + eprintln!("{range:>6.1} {n_before:>20} {clipped:>7} {pct:>10.1}"); + } + } + #[test] fn two_misses_needed_when_max_health_is_two() { let cfg = Config { From 5e59e129c29ec782afd48a411b256ecbcf18dae5 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Wed, 27 May 2026 19:51:55 -0700 Subject: [PATCH 2/3] Planar grace region --- dimos/mapping/ray_tracing/rust/src/main.rs | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/dimos/mapping/ray_tracing/rust/src/main.rs b/dimos/mapping/ray_tracing/rust/src/main.rs index 83e4fb89a1..2bd5f2b2be 100644 --- a/dimos/mapping/ray_tracing/rust/src/main.rs +++ b/dimos/mapping/ray_tracing/rust/src/main.rs @@ -364,6 +364,12 @@ fn find_misses_along_ray( continue; } + // don't remove points in the same xy plane as the hit, unless the plane only walks that plane + // we do this to preserve floors, which is more important than some missed points + if origin_voxel.2 != endpoint.2 && z == endpoint.2 { + continue; + } + let cx = x as f32 * voxel_size + half; let cy = y as f32 * voxel_size + half; let cz = z as f32 * voxel_size + half; @@ -636,7 +642,7 @@ mod tests { let origin_voxel = world_to_voxel(origin.0, origin.1, origin.2, inv); let endpoint = world_to_voxel(end.0, end.1, end.2, inv); - let expected: AHashSet = [ + let walked: AHashSet = [ (1, 0, 0), (1, 1, 0), (1, 1, 1), @@ -649,7 +655,7 @@ mod tests { .into_iter() .collect(); let mut map_voxels: AHashMap = AHashMap::new(); - for v in &expected { + for v in &walked { map_voxels.insert(*v, 1); } @@ -666,6 +672,13 @@ mod tests { endpoint, ); + // z-slab protection skips voxels in the endpoint's z-slab (z=1) when the + // ray crosses z-slabs. + let expected: AHashSet = walked + .iter() + .filter(|v| v.2 != endpoint.2) + .copied() + .collect(); assert_eq!(misses, expected); } From d938196757e6e2c5d3cbfc17e6574ffb0c38329c Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 28 May 2026 09:38:19 -0700 Subject: [PATCH 3/3] Enforce planar condition in test --- dimos/mapping/ray_tracing/rust/src/main.rs | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/dimos/mapping/ray_tracing/rust/src/main.rs b/dimos/mapping/ray_tracing/rust/src/main.rs index 2bd5f2b2be..e5a010b8d8 100644 --- a/dimos/mapping/ray_tracing/rust/src/main.rs +++ b/dimos/mapping/ray_tracing/rust/src/main.rs @@ -896,11 +896,12 @@ mod tests { let n_ground = (max_x / voxel_size).ceil() as i32; let ranges: Vec = (1..=20).map(|i| i as f32).collect(); - eprintln!( - "voxel_size={voxel_size} lidar_height={lidar_height} grace={} shadow={}", + let mut table = format!( + "voxel_size={voxel_size} lidar_height={lidar_height} grace={} shadow={}\n\ + range_m ground_voxels_in_row clipped clipped_pct\n", cfg.grace_depth, cfg.shadow_depth ); - eprintln!("range_m ground_voxels_in_row clipped clipped_pct"); + let mut total_clipped = 0usize; for &range in &ranges { let mut map = VoxelMap::default(); for i in 0..n_ground { @@ -923,8 +924,16 @@ mod tests { .count(); let clipped = n_before - n_after_ground; let pct = 100.0 * clipped as f32 / n_before as f32; - eprintln!("{range:>6.1} {n_before:>20} {clipped:>7} {pct:>10.1}"); - } + table.push_str(&format!( + "{range:>6.1} {n_before:>20} {clipped:>7} {pct:>10.1}\n" + )); + total_clipped += clipped; + } + eprint!("{table}"); + assert!( + total_clipped == 0, + "planar grace regressed, ground voxels clipped:\n{table}" + ); } #[test]