sylva-rs 2.0.0

Terrestrial and airborne laser scanning processing for forest ecology and remote sensing research.
Documentation
// Sylva: LiDAR processing for forest ecology and remote sensing research.
// Copyright (C) 2026 Tim Devereux, The University of Queensland.
// Free software under the GNU General Public License v3.0 or later;
// see the LICENSE file. There is no warranty, to the extent permitted by law.
//! Subsampling, cropping and noise filtering.

use rayon::prelude::*;

use crate::util::spatial::{first_in_voxel, min_corner, voxel_groups, KdTree};
use crate::{Point, PointCloud};

/// Simple xorshift RNG so results are reproducible without extra deps.
pub(crate) struct Rng(u64);

impl Rng {
    pub fn new(seed: u64) -> Self {
        Rng(seed.wrapping_mul(0x9E3779B97F4A7C15).max(1))
    }

    pub fn next_u64(&mut self) -> u64 {
        let mut x = self.0;
        x ^= x << 13;
        x ^= x >> 7;
        x ^= x << 17;
        self.0 = x;
        x
    }

    pub fn below(&mut self, n: usize) -> usize {
        (self.next_u64() % n as u64) as usize
    }

}

/// Keep one point per voxel.
///
/// `centroid = false` keeps the first original point of each voxel (attributes
/// preserved); `true` returns voxel centroids with attributes dropped.
pub fn voxel_downsample(cloud: &PointCloud, voxel_size: f64, centroid: bool) -> PointCloud {
    if cloud.is_empty() {
        return cloud.clone();
    }
    let origin = min_corner(&cloud.xyz);
    let (_, groups) = voxel_groups(&cloud.xyz, &origin, voxel_size);
    if centroid {
        let xyz: Vec<Point> = groups
            .iter()
            .map(|g| {
                let n = g.len() as f64;
                let mut c = [0.0; 3];
                for &i in g {
                    for k in 0..3 {
                        c[k] += cloud.xyz[i][k];
                    }
                }
                [c[0] / n, c[1] / n, c[2] / n]
            })
            .collect();
        PointCloud::new(xyz)
    } else {
        let mut idx: Vec<usize> = groups.iter().map(|g| g[0]).collect();
        idx.sort_unstable();
        cloud.take(&idx)
    }
}

/// Indices kept by [`voxel_downsample`] with `centroid = false`.
pub fn voxel_downsample_indices(points: &[Point], voxel_size: f64) -> Vec<usize> {
    voxel_downsample_indices_at(points, &min_corner(points), voxel_size)
}

/// The first point (in order) of each voxel of a grid with a corner at
/// `origin`, as sorted indices. [`voxel_downsample_indices`] anchors the grid
/// at the cloud's minimum corner; a fixed origin puts every cloud on one grid.
pub fn voxel_downsample_indices_at(points: &[Point], origin: &Point, voxel_size: f64) -> Vec<usize> {
    first_in_voxel(points, 0..points.len(), origin, voxel_size)
}

/// Random subsample of `n` points (without replacement), in original order.
pub fn random_subsample(cloud: &PointCloud, n: usize, seed: u64) -> PointCloud {
    cloud.take(&random_indices(cloud.len(), n, seed))
}

/// Indices of an approximate Poisson-disk subsample: no two kept points closer than `distance`.
pub fn min_distance_indices(points: &[Point], distance: f64) -> Vec<usize> {
    let coarse_idx = voxel_downsample_indices(points, distance / 3f64.sqrt());
    let coarse: Vec<Point> = coarse_idx.iter().map(|&i| points[i]).collect();
    let tree = KdTree::new(&coarse);
    let mut keep = vec![true; coarse.len()];
    for i in 0..coarse.len() {
        if !keep[i] {
            continue;
        }
        for (j, _) in tree.within(&coarse[i], distance) {
            if j > i {
                keep[j] = false;
            }
        }
    }
    coarse_idx.into_iter().zip(keep).filter(|(_, k)| *k).map(|(i, _)| i).collect()
}

/// Approximate Poisson-disk subsampling: no two kept points closer than `distance`.
pub fn min_distance_subsample(cloud: &PointCloud, distance: f64) -> PointCloud {
    cloud.take(&min_distance_indices(&cloud.xyz, distance))
}

/// Indices of a random subsample of `n` points (sorted).
pub fn random_indices(total: usize, n: usize, seed: u64) -> Vec<usize> {
    let n = n.min(total);
    let mut idx: Vec<usize> = (0..total).collect();
    let mut rng = Rng::new(seed);
    for i in 0..n {
        let j = i + rng.below(total - i);
        idx.swap(i, j);
    }
    idx.truncate(n);
    idx.sort_unstable();
    idx
}

/// Mask of points inside an axis-aligned box, bounds inclusive. A NaN bound
/// leaves that side open (as ±infinity); a NaN coordinate is never inside.
pub fn crop_box_mask(points: &[Point], min: Point, max: Point) -> Vec<bool> {
    let lo = min.map(|v| if v.is_nan() { f64::NEG_INFINITY } else { v });
    let hi = max.map(|v| if v.is_nan() { f64::INFINITY } else { v });
    points.iter().map(|p| (0..3).all(|k| p[k] >= lo[k] && p[k] <= hi[k])).collect()
}

/// Keep points inside an axis-aligned box; use ±infinity (or NaN) for open bounds.
pub fn crop_box(cloud: &PointCloud, min: Point, max: Point) -> PointCloud {
    cloud.filter(&crop_box_mask(&cloud.xyz, min, max))
}

/// Mask of points inside a vertical cylinder: horizontal distance from
/// `(cx, cy)` at most `radius`, and `zmin <= z <= zmax`.
pub fn crop_cylinder_mask(points: &[Point], cx: f64, cy: f64, radius: f64, zmin: f64, zmax: f64) -> Vec<bool> {
    let r2 = radius * radius;
    points
        .iter()
        .map(|p| {
            let dx = p[0] - cx;
            let dy = p[1] - cy;
            dx * dx + dy * dy <= r2 && p[2] >= zmin && p[2] <= zmax
        })
        .collect()
}

/// Keep points inside a vertical cylinder (a circular plot).
pub fn crop_cylinder(cloud: &PointCloud, cx: f64, cy: f64, radius: f64, zmin: f64, zmax: f64) -> PointCloud {
    cloud.filter(&crop_cylinder_mask(&cloud.xyz, cx, cy, radius, zmin, zmax))
}

/// Mask of points whose 3-D distance from `origin` is in `[min_range, max_range]`.
pub fn range_mask(points: &[Point], origin: Point, min_range: f64, max_range: f64) -> Vec<bool> {
    points
        .iter()
        .map(|p| {
            let r = crate::transform::norm(&crate::transform::sub(p, &origin));
            r >= min_range && r <= max_range
        })
        .collect()
}

/// Keep points within `[min_range, max_range]` of `origin`.
pub fn range_filter(cloud: &PointCloud, origin: Point, min_range: f64, max_range: f64) -> PointCloud {
    cloud.filter(&range_mask(&cloud.xyz, origin, min_range, max_range))
}

/// The `k` nearest of `points` to each query, nearest first, as row-major
/// `queries.len() x k` distances and indices; a query with fewer than `k`
/// points to find gets infinity and -1 in the missing places.
pub fn knn(points: &[Point], queries: &[Point], k: usize) -> (Vec<f64>, Vec<i64>) {
    let tree = KdTree::new(points);
    let rows: Vec<Vec<(usize, f64)>> = queries.par_iter().map(|x| tree.knn(x, k)).collect();
    let mut d = Vec::with_capacity(queries.len() * k);
    let mut i = Vec::with_capacity(queries.len() * k);
    for r in rows {
        for j in 0..k {
            match r.get(j) {
                Some(&(idx, dist)) => {
                    d.push(dist);
                    i.push(idx as i64);
                }
                None => {
                    d.push(f64::INFINITY);
                    i.push(-1);
                }
            }
        }
    }
    (d, i)
}

/// Statistical outlier removal mask (Rusu et al. 2008): `true` = keep.
///
/// A point is dropped when its mean distance to `k` neighbours exceeds
/// `mean + std_ratio * std` of that statistic over the cloud.
pub fn statistical_outlier_mask(points: &[Point], k: usize, std_ratio: f64) -> Vec<bool> {
    if points.len() <= k {
        return vec![true; points.len()];
    }
    let tree = KdTree::new(points);
    let mean_d: Vec<f64> = points
        .par_iter()
        .map(|p| {
            let nb = tree.knn(p, k + 1);
            let s: f64 = nb.iter().skip(1).map(|(_, d)| d).sum();
            s / (nb.len().saturating_sub(1).max(1)) as f64
        })
        .collect();
    let n = mean_d.len() as f64;
    let mu = mean_d.iter().sum::<f64>() / n;
    let var = mean_d.iter().map(|d| (d - mu) * (d - mu)).sum::<f64>() / n;
    let thr = mu + std_ratio * var.sqrt();
    mean_d.iter().map(|&d| d <= thr).collect()
}

/// The cloud without its statistical outliers (Rusu et al. 2008):
/// [`statistical_outlier_mask`] of the coordinates applied to every column.
pub fn statistical_outlier_removal(cloud: &PointCloud, k: usize, std_ratio: f64) -> PointCloud {
    cloud.filter(&statistical_outlier_mask(&cloud.xyz, k, std_ratio))
}

/// Number of points within `radius` of every point, itself included.
pub fn count_within(points: &[Point], radius: f64) -> Vec<usize> {
    let tree = KdTree::new(points);
    points.par_iter().map(|p| tree.count_within(p, radius)).collect()
}

/// Radius outlier removal mask: `true` for a point with at least `min_neighbors`
/// other points within `radius` (m).
pub fn radius_outlier_mask(points: &[Point], radius: f64, min_neighbors: usize) -> Vec<bool> {
    let tree = KdTree::new(points);
    points.par_iter().map(|p| tree.count_within(p, radius).saturating_sub(1) >= min_neighbors).collect()
}

/// The cloud without its radius outliers: [`radius_outlier_mask`] applied to
/// every column.
pub fn radius_outlier_removal(cloud: &PointCloud, radius: f64, min_neighbors: usize) -> PointCloud {
    cloud.filter(&radius_outlier_mask(&cloud.xyz, radius, min_neighbors))
}

/// Per-point local PCA descriptors from `k` neighbours.
///
/// Returns `(normals, eigenvalues ascending)`; normals are unoriented.
pub fn local_pca(points: &[Point], k: usize) -> (Vec<Point>, Vec<[f64; 3]>) {
    let tree = KdTree::new(points);
    let k = k.max(3);
    points.par_iter().map(|p| pca_of_neighbours(points, &tree.knn(p, k))).unzip()
}

/// [`local_pca`] with neighbours at equal distances taken by index
/// ([`KdTree::knn_by_index`]), so that a point's descriptors are the same in
/// any cloud holding its neighbourhood in the same relative order.
pub fn local_pca_by_index(points: &[Point], k: usize) -> (Vec<Point>, Vec<[f64; 3]>) {
    let tree = KdTree::new(points);
    let k = k.max(3);
    points.par_iter().map(|p| pca_of_neighbours(points, &tree.knn_by_index(p, k))).unzip()
}

/// The PCA descriptors of one neighbourhood (`(index, distance)` pairs into
/// `points`, as [`KdTree::knn`] gives them): the unoriented normal and the
/// eigenvalues ascending, as [`local_pca`] computes them for each point.
pub fn pca_of_neighbours(points: &[Point], nb: &[(usize, f64)]) -> (Point, [f64; 3]) {
    let n = nb.len() as f64;
    let mut c = [0.0; 3];
    for (i, _) in nb {
        for a in 0..3 {
            c[a] += points[*i][a];
        }
    }
    for a in 0..3 {
        c[a] /= n;
    }
    let mut cov = nalgebra::Matrix3::<f64>::zeros();
    for (i, _) in nb {
        let d = crate::transform::sub(&points[*i], &c);
        for a in 0..3 {
            for b in 0..3 {
                cov[(a, b)] += d[a] * d[b];
            }
        }
    }
    let eig = cov.symmetric_eigen();
    // Sort eigenpairs ascending.
    let mut order = [0usize, 1, 2];
    order.sort_by(|&i, &j| eig.eigenvalues[i].partial_cmp(&eig.eigenvalues[j]).unwrap());
    let v = eig.eigenvectors.column(order[0]);
    let normal = [v[0], v[1], v[2]];
    let vals = [eig.eigenvalues[order[0]], eig.eigenvalues[order[1]], eig.eigenvalues[order[2]]];
    (normal, vals)
}

/// Planarity `(l2 - l1) / l3` and linearity `(l3 - l2) / l3` from eigenvalues
/// ascending, 0 for a degenerate neighbourhood.
pub fn planarity_linearity_of(vals: &[f64; 3]) -> (f64, f64) {
    let [l1, l2, l3] = *vals;
    if l3 > 0.0 {
        ((l2 - l1) / l3, (l3 - l2) / l3)
    } else {
        (0.0, 0.0)
    }
}

/// Unoriented per-point normals from local PCA.
pub fn estimate_normals(points: &[Point], k: usize) -> Vec<Point> {
    local_pca(points, k).0
}

/// Planarity `(l2 - l1) / l3` and linearity `(l3 - l2) / l3` per point (eigenvalues ascending),
/// as in Weinmann et al. (2015).
pub fn planarity_linearity(points: &[Point], k: usize) -> (Vec<f64>, Vec<f64>) {
    let (_, vals) = local_pca(points, k);
    vals.iter().map(planarity_linearity_of).unzip()
}


#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn a_box_with_nan_bounds_is_open_on_those_sides() {
        let p = vec![[0.0, 0.0, 0.0], [1.0, 5.0, -3.0], [2.0, 1.0, 1.0], [f64::NAN, 0.0, 0.0]];
        let m = crop_box_mask(&p, [f64::NAN, 0.0, f64::NEG_INFINITY], [1.0, f64::NAN, 0.5]);
        assert_eq!(m, vec![true, true, false, false]);
    }

    #[test]
    fn knn_pads_what_it_cannot_find() {
        let p = vec![[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [3.0, 0.0, 0.0]];
        let (d, i) = knn(&p, &[[0.9, 0.0, 0.0]], 4);
        assert_eq!(i, vec![1, 0, 2, -1]);
        assert!((d[0] - 0.1).abs() < 1e-12 && (d[2] - 2.1).abs() < 1e-12 && d[3].is_infinite());
    }

    #[test]
    fn cylinder_and_range_bounds_are_inclusive() {
        let p = vec![[3.0, 0.0, 1.0], [3.0, 0.1, 1.0], [0.0, 0.0, 2.0], [0.0, 0.0, 2.5]];
        assert_eq!(crop_cylinder_mask(&p, 0.0, 0.0, 3.0, 0.0, 2.0), vec![true, false, true, false]);
        assert_eq!(range_mask(&p, [0.0, 0.0, 1.0], 1.0, 1.5), vec![false, false, true, true]);
        assert_eq!(range_mask(&p, [0.0, 0.0, 0.0], 2.0, 2.5), vec![false, false, true, true]);
    }
}

#[cfg(test)]
mod first_in_voxel_tests {
    use super::*;

    #[test]
    fn first_index_per_voxel_matches_the_grouped_thinning() {
        let mut rng = Rng::new(3);
        let mut unit = || (rng.next_u64() >> 11) as f64 / (1u64 << 53) as f64;
        let mut pts: Vec<Point> = (0..5000).map(|_| [3.0 * unit() - 1.0, 3.0 * unit(), 2.0 * unit() - 0.5]).collect();
        for i in 0..500 {
            pts[4000 + i] = pts[i]; // coincident points
        }
        for origin in [None, Some([0.0, 0.0, 0.0]), Some([-7.0, 2.5, 0.1])] {
            let o = origin.unwrap_or_else(|| min_corner(&pts));
            let (_, groups) = voxel_groups(&pts, &o, 0.1);
            let mut expect: Vec<usize> = groups.iter().map(|g| g[0]).collect();
            expect.sort_unstable();
            assert_eq!(voxel_downsample_indices_at(&pts, &o, 0.1), expect);
        }
        assert_eq!(voxel_downsample_indices(&pts, 0.25), {
            let (_, groups) = voxel_groups(&pts, &min_corner(&pts), 0.25);
            let mut e: Vec<usize> = groups.iter().map(|g| g[0]).collect();
            e.sort_unstable();
            e
        });
        assert!(voxel_downsample_indices(&[], 0.1).is_empty());
    }
}