1use nalgebra::Vector3;
7use rand::prelude::IndexedRandom;
8use rand::SeedableRng;
9use rand_distr::{Distribution, Normal};
10
11pub 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
27pub 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#[derive(Debug, Clone)]
43pub struct RansacResult {
44 pub center: Vector3<f64>,
46 pub normal: Vector3<f64>,
48}
49
50pub 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 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 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, ¢er) <= 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
110pub 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 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 let d: f64 = points_2d.iter().map(|p| unit_normal.dot(p)).sum::<f64>() / num_samples as f64;
135
136 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 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 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 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 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 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}