rust_robotics_mapping/
point_cloud_sampling.rs1use nalgebra::DVector;
9use rand::Rng;
10use rand::SeedableRng;
11use std::collections::HashMap;
12
13pub fn voxel_point_sampling(points: &[DVector<f64>], voxel_size: f64) -> Vec<DVector<f64>> {
26 assert!(voxel_size > 0.0, "voxel_size must be positive");
27 if points.is_empty() {
28 return Vec::new();
29 }
30
31 let dim = points[0].len();
32 let mut voxel_map: HashMap<Vec<i64>, Vec<&DVector<f64>>> = HashMap::new();
33
34 for p in points {
35 let key: Vec<i64> = (0..dim)
36 .map(|d| (p[d] / voxel_size).floor() as i64)
37 .collect();
38 voxel_map.entry(key).or_default().push(p);
39 }
40
41 voxel_map
42 .values()
43 .map(|group| {
44 let mut mean = DVector::zeros(dim);
45 for &p in group {
46 mean += p;
47 }
48 mean / group.len() as f64
49 })
50 .collect()
51}
52
53pub fn farthest_point_sampling(
69 points: &[DVector<f64>],
70 n_points: usize,
71 seed: u64,
72) -> Vec<DVector<f64>> {
73 assert!(n_points > 0, "n_points must be positive");
74 assert!(
75 n_points <= points.len(),
76 "n_points ({}) exceeds input size ({})",
77 n_points,
78 points.len()
79 );
80
81 let mut rng = rand::rngs::StdRng::seed_from_u64(seed);
82 let n = points.len();
83 let first = rng.random_range(0..n);
84
85 let mut min_distances = vec![f64::INFINITY; n];
86 let mut selected_ids: Vec<usize> = vec![first];
87
88 while selected_ids.len() < n_points {
89 let last = *selected_ids.last().unwrap();
90 let base = &points[last];
91
92 for i in 0..n {
93 let dist = (&points[i] - base).norm();
94 if dist < min_distances[i] {
95 min_distances[i] = dist;
96 }
97 }
98
99 let mut best_idx = 0;
101 let mut best_dist = -1.0f64;
102 for (i, &d) in min_distances.iter().enumerate() {
103 if !selected_ids.contains(&i) && d > best_dist {
104 best_dist = d;
105 best_idx = i;
106 }
107 }
108 selected_ids.push(best_idx);
109 }
110
111 selected_ids.iter().map(|&i| points[i].clone()).collect()
112}
113
114pub fn poisson_disk_sampling(
130 points: &[DVector<f64>],
131 n_points: usize,
132 min_distance: f64,
133 seed: u64,
134 max_iter: usize,
135) -> Vec<DVector<f64>> {
136 if points.is_empty() || n_points == 0 {
137 return Vec::new();
138 }
139
140 let mut rng = rand::rngs::StdRng::seed_from_u64(seed);
141 let n = points.len();
142 let first = rng.random_range(0..n);
143 let mut selected_ids: Vec<usize> = vec![first];
144
145 let mut loop_count = 0;
146 while selected_ids.len() < n_points && loop_count <= max_iter {
147 let candidate = rng.random_range(0..n);
148 let base = &points[candidate];
149
150 let min_dist_to_selected = selected_ids
151 .iter()
152 .map(|&i| (&points[i] - base).norm())
153 .fold(f64::INFINITY, f64::min);
154
155 if min_dist_to_selected >= min_distance {
156 selected_ids.push(candidate);
157 }
158 loop_count += 1;
159 }
160
161 selected_ids.iter().map(|&i| points[i].clone()).collect()
162}
163
164#[cfg(test)]
165mod tests {
166 use super::*;
167
168 fn make_3d_point(x: f64, y: f64, z: f64) -> DVector<f64> {
169 DVector::from_vec(vec![x, y, z])
170 }
171
172 fn make_grid_points() -> Vec<DVector<f64>> {
173 let mut pts = Vec::new();
174 for x in 0..10 {
175 for y in 0..10 {
176 for z in 0..2 {
177 pts.push(make_3d_point(x as f64, y as f64, z as f64));
178 }
179 }
180 }
181 pts
182 }
183
184 #[test]
185 fn test_voxel_sampling_reduces_points() {
186 let pts = make_grid_points();
187 let sampled = voxel_point_sampling(&pts, 3.0);
188 assert!(sampled.len() < pts.len());
189 assert!(!sampled.is_empty());
190 }
191
192 #[test]
193 fn test_voxel_sampling_preserves_single_voxel() {
194 let pts = vec![
195 make_3d_point(0.1, 0.1, 0.1),
196 make_3d_point(0.2, 0.2, 0.2),
197 make_3d_point(0.3, 0.3, 0.3),
198 ];
199 let sampled = voxel_point_sampling(&pts, 10.0);
200 assert_eq!(sampled.len(), 1);
201 assert!((sampled[0][0] - 0.2).abs() < 1e-10);
203 assert!((sampled[0][1] - 0.2).abs() < 1e-10);
204 assert!((sampled[0][2] - 0.2).abs() < 1e-10);
205 }
206
207 #[test]
208 fn test_voxel_sampling_empty() {
209 let pts: Vec<DVector<f64>> = vec![];
210 let sampled = voxel_point_sampling(&pts, 1.0);
211 assert!(sampled.is_empty());
212 }
213
214 #[test]
215 fn test_farthest_point_sampling_count() {
216 let pts = make_grid_points();
217 let sampled = farthest_point_sampling(&pts, 10, 42);
218 assert_eq!(sampled.len(), 10);
219 }
220
221 #[test]
222 fn test_farthest_point_sampling_spread() {
223 let pts = make_grid_points();
224 let sampled = farthest_point_sampling(&pts, 5, 42);
225
226 let mut min_pair_dist = f64::INFINITY;
228 for i in 0..sampled.len() {
229 for j in (i + 1)..sampled.len() {
230 let d = (&sampled[i] - &sampled[j]).norm();
231 if d < min_pair_dist {
232 min_pair_dist = d;
233 }
234 }
235 }
236 assert!(
238 min_pair_dist > 2.0,
239 "Minimum pair distance {} is too small",
240 min_pair_dist
241 );
242 }
243
244 #[test]
245 #[should_panic]
246 fn test_farthest_point_sampling_too_many() {
247 let pts = vec![make_3d_point(0.0, 0.0, 0.0)];
248 farthest_point_sampling(&pts, 5, 42);
249 }
250
251 #[test]
252 fn test_poisson_disk_sampling_min_distance() {
253 let pts = make_grid_points();
254 let min_dist = 2.0;
255 let sampled = poisson_disk_sampling(&pts, 20, min_dist, 42, 5000);
256
257 for i in 0..sampled.len() {
259 for j in (i + 1)..sampled.len() {
260 let d = (&sampled[i] - &sampled[j]).norm();
261 assert!(
262 d >= min_dist - 1e-10,
263 "Points {} and {} are too close: {}",
264 i,
265 j,
266 d
267 );
268 }
269 }
270 }
271
272 #[test]
273 fn test_poisson_disk_sampling_returns_at_least_one() {
274 let pts = make_grid_points();
275 let sampled = poisson_disk_sampling(&pts, 5, 1.0, 42, 1000);
276 assert!(!sampled.is_empty());
277 }
278
279 #[test]
280 fn test_poisson_disk_sampling_empty() {
281 let pts: Vec<DVector<f64>> = vec![];
282 let sampled = poisson_disk_sampling(&pts, 5, 1.0, 42, 100);
283 assert!(sampled.is_empty());
284 }
285}