Skip to main content

rust_robotics_mapping/
normal_vector_estimation.rs

1//! Normal vector estimation for 3D point clouds.
2//!
3//! Provides triangle-based normal vector calculation and RANSAC-based
4//! plane fitting with normal vector estimation.
5
6use nalgebra::Vector3;
7use rand::prelude::IndexedRandom;
8use rand::SeedableRng;
9use rand_distr::{Distribution, Normal};
10
11/// Calculate the unit normal vector of a triangle defined by three 3D points.
12///
13/// The normal direction follows the right-hand rule with respect to
14/// the ordering (p1 -> p2, p1 -> p3).
15///
16/// # Panics
17/// Panics if the three points are collinear (cross product is zero).
18pub fn calc_normal_vector(p1: &Vector3<f64>, p2: &Vector3<f64>, p3: &Vector3<f64>) -> Vector3<f64> {
19    let v1 = p2 - p1;
20    let v2 = p3 - p1;
21    let normal = v1.cross(&v2);
22    let norm = normal.norm();
23    assert!(norm > 1e-12, "Points are collinear, cannot compute normal");
24    normal / norm
25}
26
27/// Compute the distance from a point to a plane defined by a normal and an origin point.
28pub fn distance_to_plane(
29    point: &Vector3<f64>,
30    normal: &Vector3<f64>,
31    origin: &Vector3<f64>,
32) -> f64 {
33    let dot = normal.dot(point) - normal.dot(origin);
34    if dot.abs() < 1e-15 {
35        0.0
36    } else {
37        dot.abs() / normal.norm()
38    }
39}
40
41/// Result of RANSAC normal vector estimation.
42#[derive(Debug, Clone)]
43pub struct RansacResult {
44    /// Center (mean) of the point cloud.
45    pub center: Vector3<f64>,
46    /// Estimated unit normal vector of the best-fit plane.
47    pub normal: Vector3<f64>,
48}
49
50/// Estimate the normal vector of a plane from a 3D point cloud using RANSAC.
51///
52/// # Arguments
53/// * `points` - Slice of 3D points.
54/// * `inlier_ratio_threshold` - Stop early if the inlier ratio exceeds this value.
55/// * `inlier_dist` - Maximum distance from the plane for a point to be considered an inlier.
56/// * `confidence` - Probability that at least one sample set is outlier-free (e.g. 0.99).
57/// * `seed` - Random seed for reproducibility.
58///
59/// # Returns
60/// `Some(RansacResult)` if a valid plane is found, `None` otherwise.
61pub fn ransac_normal_vector_estimation(
62    points: &[Vector3<f64>],
63    inlier_ratio_threshold: f64,
64    inlier_dist: f64,
65    confidence: f64,
66    seed: u64,
67) -> Option<RansacResult> {
68    if points.len() < 3 {
69        return None;
70    }
71
72    let center: Vector3<f64> = points.iter().copied().sum::<Vector3<f64>>() / points.len() as f64;
73
74    let max_iter =
75        ((1.0 - confidence).ln() / (1.0 - (1.0 - inlier_ratio_threshold).powi(3)).ln()) as usize;
76
77    let mut rng = rand::rngs::StdRng::seed_from_u64(seed);
78    let indices: Vec<usize> = (0..points.len()).collect();
79
80    for _iter in 0..max_iter {
81        // Sample 3 distinct points
82        let sampled: Vec<&usize> = indices.choose_multiple(&mut rng, 3).collect();
83        let p1 = &points[*sampled[0]];
84        let p2 = &points[*sampled[1]];
85        let p3 = &points[*sampled[2]];
86
87        // Skip degenerate triangles
88        let v1 = p2 - p1;
89        let v2 = p3 - p1;
90        if v1.cross(&v2).norm() < 1e-12 {
91            continue;
92        }
93
94        let normal = calc_normal_vector(p1, p2, p3);
95
96        let n_inliers = points
97            .iter()
98            .filter(|p| distance_to_plane(p, &normal, &center) <= inlier_dist)
99            .count();
100        let inlier_ratio = n_inliers as f64 / points.len() as f64;
101
102        if inlier_ratio > inlier_ratio_threshold {
103            return Some(RansacResult { center, normal });
104        }
105    }
106
107    None
108}
109
110/// Generate 3D points sampled from a plane with optional Gaussian noise.
111///
112/// Useful for testing. Points are generated by projecting random 2D points
113/// onto a plane with the given normal vector.
114pub fn sample_points_on_plane(
115    num_samples: usize,
116    normal: &Vector3<f64>,
117    noise_scale: f64,
118    seed: u64,
119) -> Vec<Vector3<f64>> {
120    let mut rng = rand::rngs::StdRng::seed_from_u64(seed);
121    let dist = Normal::new(0.0, 1.0).unwrap();
122    let noise_dist = Normal::new(0.0, noise_scale).unwrap();
123    let unit_normal = normal.normalize();
124
125    // Generate 2D points and project onto the plane
126    let mut points_2d: Vec<Vector3<f64>> = Vec::with_capacity(num_samples);
127    for _ in 0..num_samples {
128        let x = dist.sample(&mut rng);
129        let y = dist.sample(&mut rng);
130        points_2d.push(Vector3::new(x, y, 0.0));
131    }
132
133    // Compute mean offset d
134    let d: f64 = points_2d.iter().map(|p| unit_normal.dot(p)).sum::<f64>() / num_samples as f64;
135
136    // Project points onto the plane
137    let mut points_3d: Vec<Vector3<f64>> = Vec::with_capacity(num_samples);
138    for p2d in &points_2d {
139        let proj_len = (d - unit_normal.dot(p2d)) / unit_normal.norm_squared();
140        let mut p3d = p2d + proj_len * unit_normal;
141        // Add noise
142        p3d.x += noise_dist.sample(&mut rng);
143        p3d.y += noise_dist.sample(&mut rng);
144        p3d.z += noise_dist.sample(&mut rng);
145        points_3d.push(p3d);
146    }
147
148    points_3d
149}
150
151#[cfg(test)]
152mod tests {
153    use super::*;
154
155    #[test]
156    fn test_calc_normal_vector_xy_plane() {
157        let p1 = Vector3::new(0.0, 0.0, 0.0);
158        let p2 = Vector3::new(1.0, 0.0, 0.0);
159        let p3 = Vector3::new(0.0, 1.0, 0.0);
160        let n = calc_normal_vector(&p1, &p2, &p3);
161        // Normal should be along z-axis
162        assert!((n.x.abs()) < 1e-10);
163        assert!((n.y.abs()) < 1e-10);
164        assert!((n.z.abs() - 1.0) < 1e-10);
165    }
166
167    #[test]
168    fn test_calc_normal_vector_unit_length() {
169        let p1 = Vector3::new(0.0, 0.0, 1.0);
170        let p2 = Vector3::new(1.0, 1.0, 0.0);
171        let p3 = Vector3::new(0.0, 1.0, 0.0);
172        let n = calc_normal_vector(&p1, &p2, &p3);
173        assert!((n.norm() - 1.0).abs() < 1e-10);
174    }
175
176    #[test]
177    #[should_panic]
178    fn test_calc_normal_vector_collinear_panics() {
179        let p1 = Vector3::new(0.0, 0.0, 0.0);
180        let p2 = Vector3::new(1.0, 1.0, 1.0);
181        let p3 = Vector3::new(2.0, 2.0, 2.0);
182        calc_normal_vector(&p1, &p2, &p3);
183    }
184
185    #[test]
186    fn test_distance_to_plane() {
187        let origin = Vector3::new(0.0, 0.0, 0.0);
188        let normal = Vector3::new(0.0, 0.0, 1.0);
189        let point = Vector3::new(3.0, 4.0, 5.0);
190        let d = distance_to_plane(&point, &normal, &origin);
191        assert!((d - 5.0).abs() < 1e-10);
192    }
193
194    #[test]
195    fn test_distance_to_plane_on_plane() {
196        let origin = Vector3::new(0.0, 0.0, 0.0);
197        let normal = Vector3::new(0.0, 0.0, 1.0);
198        let point = Vector3::new(3.0, 4.0, 0.0);
199        let d = distance_to_plane(&point, &normal, &origin);
200        assert!(d.abs() < 1e-10);
201    }
202
203    #[test]
204    fn test_ransac_with_perfect_plane() {
205        // Points exactly on the z=0 plane
206        let points: Vec<Vector3<f64>> = vec![
207            Vector3::new(0.0, 0.0, 0.0),
208            Vector3::new(1.0, 0.0, 0.0),
209            Vector3::new(0.0, 1.0, 0.0),
210            Vector3::new(1.0, 1.0, 0.0),
211            Vector3::new(0.5, 0.5, 0.0),
212            Vector3::new(-1.0, 0.5, 0.0),
213            Vector3::new(0.3, -0.7, 0.0),
214            Vector3::new(-0.5, -0.5, 0.0),
215            Vector3::new(2.0, 0.0, 0.0),
216            Vector3::new(0.0, 2.0, 0.0),
217        ];
218
219        let result = ransac_normal_vector_estimation(&points, 0.7, 0.01, 0.99, 42);
220        assert!(result.is_some());
221        let res = result.unwrap();
222        // Normal should be along z-axis (up to sign)
223        assert!(res.normal.z.abs() > 0.99);
224    }
225
226    #[test]
227    fn test_ransac_with_noisy_plane() {
228        let true_normal = Vector3::new(0.0, 1.0, 1.0).normalize();
229        let noise_scale = 0.05;
230        let points = sample_points_on_plane(100, &true_normal, noise_scale, 12345);
231
232        let result = ransac_normal_vector_estimation(&points, 0.6, noise_scale * 3.0, 0.99, 42);
233        assert!(result.is_some());
234        let res = result.unwrap();
235        // Estimated normal should be close to true normal (up to sign)
236        let dot = res.normal.dot(&true_normal).abs();
237        assert!(
238            dot > 0.9,
239            "Estimated normal too far from true normal: dot={}",
240            dot
241        );
242    }
243
244    #[test]
245    fn test_ransac_too_few_points() {
246        let points = vec![Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 0.0, 0.0)];
247        let result = ransac_normal_vector_estimation(&points, 0.7, 0.1, 0.99, 42);
248        assert!(result.is_none());
249    }
250
251    #[test]
252    fn test_sample_points_on_plane_count() {
253        let normal = Vector3::new(0.0, 0.0, 1.0);
254        let pts = sample_points_on_plane(50, &normal, 0.0, 0);
255        assert_eq!(pts.len(), 50);
256    }
257}