use rayon::prelude::*;
use crate::spatial::{min_corner, voxel_groups, KdTree};
use crate::{Point, PointCloud};
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
}
}
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)
}
}
pub fn voxel_downsample_indices(points: &[Point], voxel_size: f64) -> Vec<usize> {
let origin = min_corner(points);
let (_, groups) = voxel_groups(points, &origin, voxel_size);
let mut idx: Vec<usize> = groups.iter().map(|g| g[0]).collect();
idx.sort_unstable();
idx
}
pub fn random_subsample(cloud: &PointCloud, n: usize, seed: u64) -> PointCloud {
cloud.take(&random_indices(cloud.len(), n, seed))
}
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()
}
pub fn min_distance_subsample(cloud: &PointCloud, distance: f64) -> PointCloud {
cloud.take(&min_distance_indices(&cloud.xyz, distance))
}
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
}
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()
}
pub fn crop_box(cloud: &PointCloud, min: Point, max: Point) -> PointCloud {
cloud.filter(&crop_box_mask(&cloud.xyz, min, max))
}
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()
}
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))
}
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()
}
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))
}
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)
}
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()
}
pub fn statistical_outlier_removal(cloud: &PointCloud, k: usize, std_ratio: f64) -> PointCloud {
cloud.filter(&statistical_outlier_mask(&cloud.xyz, k, std_ratio))
}
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()
}
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()
}
pub fn radius_outlier_removal(cloud: &PointCloud, radius: f64, min_neighbors: usize) -> PointCloud {
cloud.filter(&radius_outlier_mask(&cloud.xyz, radius, min_neighbors))
}
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| {
let nb = tree.knn(p, k);
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();
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)
})
.unzip()
}
pub fn estimate_normals(points: &[Point], k: usize) -> Vec<Point> {
local_pca(points, k).0
}
pub fn planarity_linearity(points: &[Point], k: usize) -> (Vec<f64>, Vec<f64>) {
let (_, vals) = local_pca(points, k);
vals.iter()
.map(|[l1, l2, l3]| {
if *l3 > 0.0 {
((l2 - l1) / l3, (l3 - l2) / l3)
} else {
(0.0, 0.0)
}
})
.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]);
}
}