rigidity_core/
normals.rs

1//! Normal estimation by principal component analysis.
2
3use nalgebra::{Matrix3, Vector3};
4use rayon::prelude::*;
5
6use crate::cloud::PointCloud;
7use crate::neighbors::NeighborSearch;
8
9/// Estimates the normal at every point from its `k` nearest neighbours.
10///
11/// The covariance of the neighbourhood is decomposed into eigenvectors;
12/// the normal is the eigenvector of the smallest eigenvalue, that is, the
13/// direction of least spread.
14///
15/// # Sign
16///
17/// Normals are not consistently oriented across points: consistent
18/// orientation requires propagation over the neighbourhood graph and is
19/// beside the point here. For a point-to-plane constraint the sign is
20/// irrelevant — the residual enters squared, and flipping `n` in both the
21/// Jacobian row and the residual leaves `JᵀWJ` and `JᵀWe` unchanged.
22///
23/// The sign is nevertheless fixed deterministically, by the component of
24/// largest magnitude, so that results stay reproducible.
25///
26/// # Determinism
27///
28/// Points are independent, output order is preserved, and each
29/// neighbourhood's covariance accumulates in order of increasing distance
30/// as guaranteed by the [`NeighborSearch`] contract.
31pub fn estimate_normals<S>(cloud: &PointCloud, search: &S, k: usize) -> Vec<Vector3<f64>>
32where
33    S: NeighborSearch + Sync,
34{
35    estimate_normals_observed(cloud, search, k, |_, _| {})
36}
37
38/// How many points are estimated between two progress reports.
39///
40/// The chunk exists only for reporting. Points are independent, so
41/// splitting the range changes neither the values nor their order — the
42/// only cost is one rayon barrier per chunk, which at this size is lost in
43/// the noise of a single neighbourhood query.
44const PROGRESS_CHUNK: usize = 16_384;
45
46/// The same, reporting progress in points.
47///
48/// `progress` receives `(completed, total)` after each chunk, always
49/// increasing, always from the calling thread. A cloud shorter than one
50/// chunk reports once, an empty cloud not at all.
51pub fn estimate_normals_observed<S, F>(
52    cloud: &PointCloud,
53    search: &S,
54    k: usize,
55    mut progress: F,
56) -> Vec<Vector3<f64>>
57where
58    S: NeighborSearch + Sync,
59    F: FnMut(usize, usize),
60{
61    assert!(k >= 3, "normal estimation needs at least three neighbours");
62    let count = cloud.len();
63    let mut normals: Vec<Vector3<f64>> = Vec::with_capacity(count);
64    let mut done = 0;
65    while done < count {
66        let end = (done + PROGRESS_CHUNK).min(count);
67        normals.par_extend((done..end).into_par_iter().map(|index| {
68            let query = cloud.point(index);
69            let neighbours = search.knn(&query, k);
70            if neighbours.len() < 3 {
71                return Vector3::z();
72            }
73
74            let mut centroid = Vector3::zeros();
75            for neighbour in &neighbours {
76                centroid += cloud.point(neighbour.index as usize);
77            }
78            centroid /= neighbours.len() as f64;
79
80            let mut covariance = Matrix3::zeros();
81            for neighbour in &neighbours {
82                let delta = cloud.point(neighbour.index as usize) - centroid;
83                covariance += delta * delta.transpose();
84            }
85
86            let eigen = nalgebra::SymmetricEigen::new(covariance);
87            let mut smallest = 0;
88            for axis in 1..3 {
89                if eigen.eigenvalues[axis] < eigen.eigenvalues[smallest] {
90                    smallest = axis;
91                }
92            }
93            let normal: Vector3<f64> = eigen.eigenvectors.column(smallest).into();
94
95            // Deterministic sign.
96            let mut dominant = 0;
97            for axis in 1..3 {
98                if normal[axis].abs() > normal[dominant].abs() {
99                    dominant = axis;
100                }
101            }
102            if normal[dominant] < 0.0 {
103                -normal
104            } else {
105                normal
106            }
107        }));
108        done = end;
109        progress(done, count);
110    }
111    normals
112}
rigidity_core/normals.rs

rigidity_core/
normals.rs
