Skip to main content

rust_robotics_mapping/
point_cloud_sampling.rs

1//! Point cloud sampling algorithms.
2//!
3//! Provides three sampling strategies for N-dimensional point clouds:
4//! - Voxel point sampling
5//! - Farthest point sampling
6//! - Poisson disk sampling
7
8use nalgebra::DVector;
9use rand::Rng;
10use rand::SeedableRng;
11use std::collections::HashMap;
12
13/// Voxel point sampling.
14///
15/// Points that fall within the same voxel grid cell are merged by computing
16/// their mean. This produces a downsampled point cloud with at most one
17/// point per voxel.
18///
19/// # Arguments
20/// * `points` - Original N-dimensional points.
21/// * `voxel_size` - Side length of each voxel cell.
22///
23/// # Returns
24/// A vector of sampled points (one per occupied voxel).
25pub 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
53/// Farthest point sampling.
54///
55/// Iteratively selects the point that is farthest from all previously
56/// selected points, producing a well-distributed subset.
57///
58/// # Arguments
59/// * `points` - Original N-dimensional points.
60/// * `n_points` - Number of points to sample.
61/// * `seed` - Random seed for selecting the initial point.
62///
63/// # Returns
64/// A vector of `n_points` sampled points.
65///
66/// # Panics
67/// Panics if `n_points` is 0 or exceeds the number of input points.
68pub 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        // Select the point with maximum minimum distance
100        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
114/// Poisson disk sampling.
115///
116/// Randomly selects points while enforcing a minimum distance between
117/// all selected points. If the desired number of points cannot be reached
118/// within `max_iter` attempts, fewer points are returned.
119///
120/// # Arguments
121/// * `points` - Original N-dimensional points.
122/// * `n_points` - Desired number of points to sample.
123/// * `min_distance` - Minimum distance between any pair of selected points.
124/// * `seed` - Random seed.
125/// * `max_iter` - Maximum number of random selection attempts.
126///
127/// # Returns
128/// A vector of sampled points (at most `n_points`).
129pub 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        // Mean should be (0.2, 0.2, 0.2)
202        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        // All pairs should have reasonable distance (not all clustered)
227        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        // With 200 points in a 10x10x2 grid, 5 farthest points should be at least 3 apart
237        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        // Verify minimum distance constraint
258        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}