spatialrust_filtering/
outlier.rs

1//! Neighborhood-based outlier removal filters.
2//!
3//! Both filters use a KD-tree over the input positions and drop points whose
4//! local neighborhood looks sparse, which removes scanner speckle and stray
5//! returns before downstream estimation (normals, registration, segmentation).
6
7use spatialrust_core::{
8    DType, FieldSemantic, HasPositions3, PointBuffer, PointBufferSet, PointCloud, PointField,
9    SpatialError, SpatialResult,
10};
11use spatialrust_search::{parallel_worker_count, KdTree};
12
13use crate::filter::PointCloudFilter;
14
15/// Configuration for [`StatisticalOutlierRemoval`].
16#[derive(Clone, Copy, Debug, PartialEq)]
17pub struct StatisticalOutlierConfig {
18    /// Number of nearest neighbors averaged per point.
19    pub k_neighbors: usize,
20    /// Standard-deviation multiplier; points whose mean neighbor distance
21    /// exceeds `global_mean + std_mul * global_std` are removed.
22    pub std_mul: f32,
23}
24
25impl Default for StatisticalOutlierConfig {
26    fn default() -> Self {
27        Self { k_neighbors: 16, std_mul: 1.0 }
28    }
29}
30
31impl StatisticalOutlierConfig {
32    /// Creates a config from the neighbor count and std multiplier.
33    #[must_use]
34    pub const fn new(k_neighbors: usize, std_mul: f32) -> Self {
35        Self { k_neighbors, std_mul }
36    }
37}
38
39/// Statistical Outlier Removal (SOR).
40///
41/// For each point the mean distance to its `k` nearest neighbors is computed.
42/// Assuming those means are roughly Gaussian, points whose mean distance is
43/// more than `std_mul` standard deviations above the global mean are dropped.
44#[derive(Clone, Copy, Debug, PartialEq)]
45pub struct StatisticalOutlierRemoval {
46    config: StatisticalOutlierConfig,
47}
48
49impl StatisticalOutlierRemoval {
50    /// Creates a filter from config.
51    #[must_use]
52    pub const fn new(config: StatisticalOutlierConfig) -> Self {
53        Self { config }
54    }
55
56    /// Returns the filter config.
57    #[must_use]
58    pub const fn config(&self) -> StatisticalOutlierConfig {
59        self.config
60    }
61
62    /// Computes the keep mask without materializing the filtered cloud.
63    pub fn keep_mask(&self, input: &PointCloud) -> SpatialResult<Vec<bool>> {
64        if self.config.k_neighbors == 0 {
65            return Err(SpatialError::InvalidArgument(
66                "k_neighbors must be greater than zero".to_owned(),
67            ));
68        }
69        let len = input.len();
70        if len == 0 {
71            return Ok(Vec::new());
72        }
73
74        let (x, y, z) = input.positions3()?;
75        let tree = KdTree::from_slices(x, y, z);
76
77        // Mean distance to the k nearest neighbors (excluding the point itself).
78        let mut mean_dist = vec![0.0_f32; len];
79        fill_mean_neighbor_distances(self.config.k_neighbors, &tree, x, y, z, &mut mean_dist);
80
81        let n = len as f64;
82        let mean: f64 = mean_dist.iter().map(|&d| d as f64).sum::<f64>() / n;
83        let variance: f64 = mean_dist.iter().map(|&d| (d as f64 - mean).powi(2)).sum::<f64>() / n;
84        let std = variance.sqrt();
85        let threshold = mean + self.config.std_mul as f64 * std;
86
87        Ok(mean_dist.iter().map(|&d| d as f64 <= threshold).collect())
88    }
89}
90
91impl PointCloudFilter for StatisticalOutlierRemoval {
92    fn name(&self) -> &'static str {
93        "StatisticalOutlierRemoval"
94    }
95
96    fn filter(&self, input: &PointCloud) -> SpatialResult<PointCloud> {
97        let mask = self.keep_mask(input)?;
98        gather_mask(input, &mask)
99    }
100}
101
102fn fill_mean_neighbor_distances(
103    k_neighbors: usize,
104    tree: &KdTree,
105    x: &[f32],
106    y: &[f32],
107    z: &[f32],
108    mean_dist: &mut [f32],
109) {
110    let worker_count = parallel_worker_count(mean_dist.len());
111    if worker_count == 1 {
112        fill_mean_neighbor_distances_chunk(k_neighbors, tree, x, y, z, 0, mean_dist);
113        return;
114    }
115
116    let chunk_size = mean_dist.len().div_ceil(worker_count);
117    std::thread::scope(|scope| {
118        for (chunk_index, chunk) in mean_dist.chunks_mut(chunk_size).enumerate() {
119            let start = chunk_index * chunk_size;
120            scope.spawn(move || {
121                fill_mean_neighbor_distances_chunk(k_neighbors, tree, x, y, z, start, chunk);
122            });
123        }
124    });
125}
126
127fn fill_mean_neighbor_distances_chunk(
128    k_neighbors: usize,
129    tree: &KdTree,
130    x: &[f32],
131    y: &[f32],
132    z: &[f32],
133    start: usize,
134    mean_dist: &mut [f32],
135) {
136    let mut neighbors = Vec::with_capacity(k_neighbors.saturating_add(1));
137    for (offset, mean) in mean_dist.iter_mut().enumerate() {
138        let i = start + offset;
139        tree.nearest_k_unsorted_into(
140            x[i],
141            y[i],
142            z[i],
143            k_neighbors.saturating_add(1),
144            &mut neighbors,
145        );
146        let mut sum = 0.0_f32;
147        let mut count = 0_u32;
148        for neighbor in &neighbors {
149            if neighbor.index == i {
150                continue;
151            }
152            sum += neighbor.distance_squared.sqrt();
153            count += 1;
154        }
155        *mean = if count == 0 { 0.0 } else { sum / count as f32 };
156    }
157}
158
159/// Configuration for [`RadiusOutlierRemoval`].
160#[derive(Clone, Copy, Debug, PartialEq)]
161pub struct RadiusOutlierConfig {
162    /// Search radius (not squared) defining a point's neighborhood.
163    pub radius: f32,
164    /// Minimum neighbors (excluding the point itself) required to keep a point.
165    pub min_neighbors: usize,
166}
167
168impl Default for RadiusOutlierConfig {
169    fn default() -> Self {
170        Self { radius: 0.5, min_neighbors: 4 }
171    }
172}
173
174impl RadiusOutlierConfig {
175    /// Creates a config from the radius and minimum neighbor count.
176    #[must_use]
177    pub const fn new(radius: f32, min_neighbors: usize) -> Self {
178        Self { radius, min_neighbors }
179    }
180}
181
182/// Radius Outlier Removal (ROR).
183///
184/// Drops every point that has fewer than `min_neighbors` other points within
185/// `radius`. Unlike SOR this uses an absolute density threshold, so it is robust
186/// when outliers are clustered rather than isolated.
187#[derive(Clone, Copy, Debug, PartialEq)]
188pub struct RadiusOutlierRemoval {
189    config: RadiusOutlierConfig,
190}
191
192impl RadiusOutlierRemoval {
193    /// Creates a filter from config.
194    #[must_use]
195    pub const fn new(config: RadiusOutlierConfig) -> Self {
196        Self { config }
197    }
198
199    /// Returns the filter config.
200    #[must_use]
201    pub const fn config(&self) -> RadiusOutlierConfig {
202        self.config
203    }
204
205    /// Computes the keep mask without materializing the filtered cloud.
206    pub fn keep_mask(&self, input: &PointCloud) -> SpatialResult<Vec<bool>> {
207        if self.config.radius <= 0.0 || self.config.radius.is_nan() {
208            return Err(SpatialError::InvalidArgument("radius must be positive".to_owned()));
209        }
210        let len = input.len();
211        if len == 0 {
212            return Ok(Vec::new());
213        }
214
215        let (x, y, z) = input.positions3()?;
216        let tree = KdTree::from_slices(x, y, z);
217
218        // The query point itself is in the tree, so requiring `min_neighbors`
219        // *other* points within radius means reaching `min_neighbors + 1` total.
220        // `radius_reaches` early-exits at that threshold without allocating.
221        let target = self.config.min_neighbors + 1;
222        let mut keep = vec![false; len];
223        fill_radius_reaches_mask(&tree, x, y, z, self.config.radius, target, &mut keep);
224        Ok(keep)
225    }
226}
227
228impl PointCloudFilter for RadiusOutlierRemoval {
229    fn name(&self) -> &'static str {
230        "RadiusOutlierRemoval"
231    }
232
233    fn filter(&self, input: &PointCloud) -> SpatialResult<PointCloud> {
234        let mask = self.keep_mask(input)?;
235        gather_mask(input, &mask)
236    }
237}
238
239fn fill_radius_reaches_mask(
240    tree: &KdTree,
241    x: &[f32],
242    y: &[f32],
243    z: &[f32],
244    radius: f32,
245    target: usize,
246    keep: &mut [bool],
247) {
248    let worker_count = parallel_worker_count(keep.len());
249    if worker_count == 1 {
250        fill_radius_reaches_mask_chunk(tree, x, y, z, radius, target, 0, keep);
251        return;
252    }
253
254    let chunk_size = keep.len().div_ceil(worker_count);
255    std::thread::scope(|scope| {
256        for (chunk_index, chunk) in keep.chunks_mut(chunk_size).enumerate() {
257            let start = chunk_index * chunk_size;
258            scope.spawn(move || {
259                fill_radius_reaches_mask_chunk(tree, x, y, z, radius, target, start, chunk);
260            });
261        }
262    });
263}
264
265fn fill_radius_reaches_mask_chunk(
266    tree: &KdTree,
267    x: &[f32],
268    y: &[f32],
269    z: &[f32],
270    radius: f32,
271    target: usize,
272    start: usize,
273    keep: &mut [bool],
274) {
275    for (offset, keep_point) in keep.iter_mut().enumerate() {
276        let i = start + offset;
277        *keep_point = tree.radius_reaches(x[i], y[i], z[i], radius, target);
278    }
279}
280
281/// Builds a new cloud from the points where `mask` is true, preserving schema.
282fn gather_mask(input: &PointCloud, mask: &[bool]) -> SpatialResult<PointCloud> {
283    if let Some(output) = gather_xyz_mask(input, mask)? {
284        return Ok(output);
285    }
286
287    let indices: Vec<usize> =
288        mask.iter().enumerate().filter_map(|(i, &keep)| keep.then_some(i)).collect();
289
290    let mut buffers = PointBufferSet::new();
291    for field in input.schema().fields() {
292        let source = input.field(&field.name)?;
293        buffers.insert(field.name.clone(), gather_buffer(source, &indices));
294    }
295    PointCloud::try_from_parts(input.schema().clone(), buffers, input.metadata().clone())
296}
297
298fn gather_xyz_mask(input: &PointCloud, mask: &[bool]) -> SpatialResult<Option<PointCloud>> {
299    let schema = input.schema();
300    if schema.len() != 3 {
301        return Ok(None);
302    }
303
304    let Some(x_field) = xyz_f32_field(input, FieldSemantic::PositionX) else {
305        return Ok(None);
306    };
307    let Some(y_field) = xyz_f32_field(input, FieldSemantic::PositionY) else {
308        return Ok(None);
309    };
310    let Some(z_field) = xyz_f32_field(input, FieldSemantic::PositionZ) else {
311        return Ok(None);
312    };
313
314    let (x, y, z) = input.positions3()?;
315    let output_len = mask.iter().filter(|&&keep| keep).count();
316    let mut out_x = Vec::with_capacity(output_len);
317    let mut out_y = Vec::with_capacity(output_len);
318    let mut out_z = Vec::with_capacity(output_len);
319
320    for (index, &keep) in mask.iter().enumerate() {
321        if keep {
322            out_x.push(x[index]);
323            out_y.push(y[index]);
324            out_z.push(z[index]);
325        }
326    }
327
328    let mut buffers = PointBufferSet::new();
329    buffers.insert(x_field.name.clone(), PointBuffer::from_f32(out_x));
330    buffers.insert(y_field.name.clone(), PointBuffer::from_f32(out_y));
331    buffers.insert(z_field.name.clone(), PointBuffer::from_f32(out_z));
332    PointCloud::try_from_parts(schema.clone(), buffers, input.metadata().clone()).map(Some)
333}
334
335fn xyz_f32_field(input: &PointCloud, semantic: FieldSemantic) -> Option<&PointField> {
336    let field = input.schema().find_semantic(semantic)?;
337    (field.dtype == DType::F32 && field.components == 1).then_some(field)
338}
339
340fn gather_buffer(source: &PointBuffer, indices: &[usize]) -> PointBuffer {
341    match source {
342        PointBuffer::F32(v) => PointBuffer::from_f32(indices.iter().map(|&i| v[i]).collect()),
343        PointBuffer::F64(v) => PointBuffer::F64(indices.iter().map(|&i| v[i]).collect()),
344        PointBuffer::U8(v) => PointBuffer::U8(indices.iter().map(|&i| v[i]).collect()),
345        PointBuffer::U16(v) => PointBuffer::U16(indices.iter().map(|&i| v[i]).collect()),
346        PointBuffer::U32(v) => PointBuffer::U32(indices.iter().map(|&i| v[i]).collect()),
347        PointBuffer::I32(v) => PointBuffer::I32(indices.iter().map(|&i| v[i]).collect()),
348    }
349}
350
351#[cfg(test)]
352mod tests {
353    use super::*;
354    use spatialrust_core::{DType, FieldSemantic, PointField, PointSchema};
355
356    fn cloud_from_xyz(points: &[[f32; 3]]) -> PointCloud {
357        let schema = PointSchema::new()
358            .with_field(PointField::scalar("x", FieldSemantic::PositionX, DType::F32))
359            .with_field(PointField::scalar("y", FieldSemantic::PositionY, DType::F32))
360            .with_field(PointField::scalar("z", FieldSemantic::PositionZ, DType::F32));
361        let mut buffers = PointBufferSet::new();
362        buffers
363            .insert("x".to_owned(), PointBuffer::from_f32(points.iter().map(|p| p[0]).collect()));
364        buffers
365            .insert("y".to_owned(), PointBuffer::from_f32(points.iter().map(|p| p[1]).collect()));
366        buffers
367            .insert("z".to_owned(), PointBuffer::from_f32(points.iter().map(|p| p[2]).collect()));
368        PointCloud::try_from_parts(schema, buffers, Default::default()).unwrap()
369    }
370
371    /// A dense unit-spaced grid plus one far-away speckle point.
372    fn grid_with_outlier() -> (PointCloud, usize) {
373        let mut points = Vec::new();
374        for ix in 0..6 {
375            for iy in 0..6 {
376                points.push([ix as f32, iy as f32, 0.0]);
377            }
378        }
379        let outlier_index = points.len();
380        points.push([100.0, 100.0, 100.0]);
381        (cloud_from_xyz(&points), outlier_index)
382    }
383
384    #[test]
385    fn sor_removes_isolated_speckle() {
386        let (cloud, outlier) = grid_with_outlier();
387        let filter = StatisticalOutlierRemoval::new(StatisticalOutlierConfig::new(8, 1.0));
388        let mask = filter.keep_mask(&cloud).unwrap();
389        assert!(!mask[outlier], "the far speckle must be dropped");
390        // Every dense grid point should survive.
391        assert!(mask[..outlier].iter().all(|&k| k));
392        let out = filter.filter(&cloud).unwrap();
393        assert_eq!(out.len(), cloud.len() - 1);
394    }
395
396    #[test]
397    fn ror_removes_isolated_speckle() {
398        let (cloud, outlier) = grid_with_outlier();
399        let filter = RadiusOutlierRemoval::new(RadiusOutlierConfig::new(1.5, 2));
400        let mask = filter.keep_mask(&cloud).unwrap();
401        assert!(!mask[outlier], "the far speckle has no neighbors in radius");
402        assert!(mask[..outlier].iter().all(|&k| k));
403    }
404
405    #[test]
406    fn empty_cloud_is_passthrough() {
407        let cloud = cloud_from_xyz(&[]);
408        let sor = StatisticalOutlierRemoval::new(StatisticalOutlierConfig::default());
409        assert_eq!(sor.filter(&cloud).unwrap().len(), 0);
410        let ror = RadiusOutlierRemoval::new(RadiusOutlierConfig::default());
411        assert_eq!(ror.filter(&cloud).unwrap().len(), 0);
412    }
413
414    #[test]
415    fn invalid_params_error() {
416        let cloud = cloud_from_xyz(&[[0.0, 0.0, 0.0]]);
417        assert!(StatisticalOutlierRemoval::new(StatisticalOutlierConfig::new(0, 1.0))
418            .keep_mask(&cloud)
419            .is_err());
420        assert!(RadiusOutlierRemoval::new(RadiusOutlierConfig::new(0.0, 1))
421            .keep_mask(&cloud)
422            .is_err());
423    }
424}
spatialrust_filtering/outlier.rs

spatialrust_filtering/
outlier.rs
