Skip to main content

rust_robotics_mapping/
kmeans_clustering.rs

1//! K-means clustering for 2D point clouds.
2//!
3//! Based on the work by Atsushi Sakai (@Atsushi_twi).
4
5use rand::Rng;
6use rand::SeedableRng;
7
8/// Default maximum number of iterations.
9pub const DEFAULT_MAX_ITERATIONS: usize = 10;
10/// Default cost-change threshold for convergence.
11pub const DEFAULT_COST_THRESHOLD: f64 = 0.1;
12
13/// Configuration for the k-means algorithm.
14#[derive(Debug, Clone)]
15pub struct KMeansConfig {
16    /// Number of clusters.
17    pub k: usize,
18    /// Maximum number of iterations.
19    pub max_iterations: usize,
20    /// Convergence threshold on cost change.
21    pub cost_threshold: f64,
22}
23
24impl KMeansConfig {
25    /// Create a new configuration with the given number of clusters and default parameters.
26    pub fn new(k: usize) -> Self {
27        Self {
28            k,
29            max_iterations: DEFAULT_MAX_ITERATIONS,
30            cost_threshold: DEFAULT_COST_THRESHOLD,
31        }
32    }
33
34    /// Set maximum iterations.
35    #[must_use]
36    pub fn with_max_iterations(mut self, max_iterations: usize) -> Self {
37        self.max_iterations = max_iterations;
38        self
39    }
40
41    /// Set convergence cost threshold.
42    #[must_use]
43    pub fn with_cost_threshold(mut self, cost_threshold: f64) -> Self {
44        self.cost_threshold = cost_threshold;
45        self
46    }
47}
48
49/// Result of k-means clustering.
50#[derive(Debug, Clone)]
51pub struct KMeansResult {
52    /// X-coordinates of the input points.
53    pub x: Vec<f64>,
54    /// Y-coordinates of the input points.
55    pub y: Vec<f64>,
56    /// Cluster label for each point (0-indexed).
57    pub labels: Vec<usize>,
58    /// X-coordinates of cluster centroids.
59    pub center_x: Vec<f64>,
60    /// Y-coordinates of cluster centroids.
61    pub center_y: Vec<f64>,
62    /// Number of clusters.
63    pub k: usize,
64}
65
66impl KMeansResult {
67    /// Return the (x, y) coordinates of points assigned to the given label.
68    pub fn get_cluster_points(&self, label: usize) -> (Vec<f64>, Vec<f64>) {
69        let mut cx = Vec::new();
70        let mut cy = Vec::new();
71        for (i, &l) in self.labels.iter().enumerate() {
72            if l == label {
73                cx.push(self.x[i]);
74                cy.push(self.y[i]);
75            }
76        }
77        (cx, cy)
78    }
79}
80
81/// Run k-means clustering on 2D points.
82///
83/// # Arguments
84/// * `x` - X-coordinates of data points.
85/// * `y` - Y-coordinates of data points.
86/// * `config` - Algorithm configuration (k, max iterations, threshold).
87///
88/// # Panics
89/// Panics if `x` and `y` have different lengths, if either is empty,
90/// or if `k` is zero or greater than the number of points.
91pub fn kmeans_clustering(x: &[f64], y: &[f64], config: &KMeansConfig) -> KMeansResult {
92    assert_eq!(x.len(), y.len(), "x and y must have the same length");
93    assert!(!x.is_empty(), "input must not be empty");
94    assert!(config.k > 0, "k must be positive");
95    assert!(
96        config.k <= x.len(),
97        "k must not exceed the number of points"
98    );
99
100    let n = x.len();
101    let k = config.k;
102
103    // Use seeded RNG for deterministic results. Seed from data to ensure
104    // reproducibility while varying with different inputs.
105    let seed = n as u64 ^ k as u64 ^ x[0].to_bits() ^ y[0].to_bits();
106    let mut rng = rand::rngs::StdRng::seed_from_u64(seed);
107    let mut labels = vec![0; n];
108    let mut center_x = vec![0.0; k];
109    let mut center_y = vec![0.0; k];
110
111    init_centroids(x, y, k, &mut rng, &mut center_x, &mut center_y);
112
113    let mut pre_cost = f64::INFINITY;
114    for _ in 0..config.max_iterations {
115        let cost = update_labels(x, y, &center_x, &center_y, &mut labels);
116        calc_centroids(x, y, &labels, k, &mut center_x, &mut center_y);
117
118        let d_cost = (cost - pre_cost).abs();
119        if d_cost < config.cost_threshold {
120            break;
121        }
122        pre_cost = cost;
123    }
124
125    KMeansResult {
126        x: x.to_vec(),
127        y: y.to_vec(),
128        labels,
129        center_x,
130        center_y,
131        k,
132    }
133}
134
135/// Initialize centroids with a deterministic farthest-point spread.
136fn init_centroids(
137    x: &[f64],
138    y: &[f64],
139    k: usize,
140    rng: &mut rand::rngs::StdRng,
141    center_x: &mut [f64],
142    center_y: &mut [f64],
143) {
144    let n = x.len();
145    let first = rng.random_range(0..n);
146    center_x[0] = x[first];
147    center_y[0] = y[first];
148
149    for label in 1..k {
150        let mut farthest_index = 0;
151        let mut farthest_dist = f64::NEG_INFINITY;
152        for i in 0..n {
153            let nearest_dist = (0..label)
154                .map(|existing| {
155                    (x[i] - center_x[existing]).powi(2) + (y[i] - center_y[existing]).powi(2)
156                })
157                .fold(f64::INFINITY, f64::min);
158            if nearest_dist > farthest_dist {
159                farthest_dist = nearest_dist;
160                farthest_index = i;
161            }
162        }
163        center_x[label] = x[farthest_index];
164        center_y[label] = y[farthest_index];
165    }
166}
167
168/// Compute centroids from current label assignments.
169fn calc_centroids(
170    x: &[f64],
171    y: &[f64],
172    labels: &[usize],
173    k: usize,
174    center_x: &mut [f64],
175    center_y: &mut [f64],
176) {
177    for label in 0..k {
178        let mut sx = 0.0;
179        let mut sy = 0.0;
180        let mut count = 0usize;
181        for (i, &l) in labels.iter().enumerate() {
182            if l == label {
183                sx += x[i];
184                sy += y[i];
185                count += 1;
186            }
187        }
188        if count > 0 {
189            center_x[label] = sx / count as f64;
190            center_y[label] = sy / count as f64;
191        }
192    }
193}
194
195/// Assign each point to the nearest centroid and return total cost.
196fn update_labels(
197    x: &[f64],
198    y: &[f64],
199    center_x: &[f64],
200    center_y: &[f64],
201    labels: &mut [usize],
202) -> f64 {
203    let mut cost = 0.0;
204    for i in 0..x.len() {
205        let mut min_dist = f64::INFINITY;
206        let mut min_label = 0;
207        for (j, (&cx, &cy)) in center_x.iter().zip(center_y.iter()).enumerate() {
208            let dist = ((x[i] - cx).powi(2) + (y[i] - cy).powi(2)).sqrt();
209            if dist < min_dist {
210                min_dist = dist;
211                min_label = j;
212            }
213        }
214        labels[i] = min_label;
215        cost += min_dist;
216    }
217    cost
218}
219
220#[cfg(test)]
221mod tests {
222    use super::*;
223
224    /// Generate a simple point cloud around given centers.
225    fn generate_test_data(
226        centers: &[(f64, f64)],
227        points_per_cluster: usize,
228        spread: f64,
229    ) -> (Vec<f64>, Vec<f64>) {
230        let mut rng = rand::rngs::StdRng::seed_from_u64(42);
231        let mut x = Vec::new();
232        let mut y = Vec::new();
233        for &(cx, cy) in centers {
234            for _ in 0..points_per_cluster {
235                x.push(cx + spread * (rng.random::<f64>() - 0.5));
236                y.push(cy + spread * (rng.random::<f64>() - 0.5));
237            }
238        }
239        (x, y)
240    }
241
242    #[test]
243    fn test_two_clusters_well_separated() {
244        let centers = [(0.0, 0.0), (100.0, 100.0)];
245        let (x, y) = generate_test_data(&centers, 20, 2.0);
246
247        let config = KMeansConfig::new(2).with_max_iterations(50);
248        let result = kmeans_clustering(&x, &y, &config);
249
250        assert_eq!(result.labels.len(), 40);
251        assert_eq!(result.k, 2);
252
253        // All points from the first group should share one label,
254        // all points from the second group should share another.
255        let first_label = result.labels[0];
256        for &l in &result.labels[..20] {
257            assert_eq!(l, first_label);
258        }
259        let second_label = result.labels[20];
260        assert_ne!(first_label, second_label);
261        for &l in &result.labels[20..] {
262            assert_eq!(l, second_label);
263        }
264    }
265
266    #[test]
267    fn test_three_clusters() {
268        let centers = [(0.0, 0.0), (50.0, 0.0), (25.0, 50.0)];
269        let (x, y) = generate_test_data(&centers, 30, 3.0);
270
271        let config = KMeansConfig::new(3).with_max_iterations(50);
272        let result = kmeans_clustering(&x, &y, &config);
273
274        assert_eq!(result.labels.len(), 90);
275        assert_eq!(result.center_x.len(), 3);
276        assert_eq!(result.center_y.len(), 3);
277
278        // Each cluster should have roughly 30 points.
279        for label in 0..3 {
280            let (cx, cy) = result.get_cluster_points(label);
281            assert!(!cx.is_empty(), "cluster {label} should not be empty");
282            assert_eq!(cx.len(), cy.len());
283        }
284    }
285
286    #[test]
287    fn test_single_cluster() {
288        let (x, y) = generate_test_data(&[(5.0, 5.0)], 10, 1.0);
289        let config = KMeansConfig::new(1);
290        let result = kmeans_clustering(&x, &y, &config);
291
292        // All points should be in cluster 0.
293        for &l in &result.labels {
294            assert_eq!(l, 0);
295        }
296
297        // Centroid should be near (5, 5).
298        assert!((result.center_x[0] - 5.0).abs() < 1.0);
299        assert!((result.center_y[0] - 5.0).abs() < 1.0);
300    }
301
302    #[test]
303    fn test_get_cluster_points() {
304        let x = vec![0.0, 1.0, 10.0, 11.0];
305        let y = vec![0.0, 1.0, 10.0, 11.0];
306        let config = KMeansConfig::new(2).with_max_iterations(50);
307        let result = kmeans_clustering(&x, &y, &config);
308
309        let mut total = 0;
310        for label in 0..2 {
311            let (cx, _cy) = result.get_cluster_points(label);
312            total += cx.len();
313        }
314        assert_eq!(total, 4);
315    }
316
317    #[test]
318    fn test_convergence_with_cost_threshold() {
319        let centers = [(0.0, 0.0), (20.0, 20.0)];
320        let (x, y) = generate_test_data(&centers, 15, 1.0);
321
322        let config = KMeansConfig::new(2)
323            .with_max_iterations(100)
324            .with_cost_threshold(0.01);
325        let result = kmeans_clustering(&x, &y, &config);
326
327        // Should still converge correctly.
328        assert_eq!(result.labels.len(), 30);
329    }
330
331    #[test]
332    #[should_panic(expected = "x and y must have the same length")]
333    fn test_mismatched_lengths() {
334        let config = KMeansConfig::new(2);
335        kmeans_clustering(&[1.0, 2.0], &[1.0], &config);
336    }
337
338    #[test]
339    #[should_panic(expected = "k must be positive")]
340    fn test_zero_k() {
341        let config = KMeansConfig::new(0);
342        kmeans_clustering(&[1.0], &[1.0], &config);
343    }
344}