Skip to main content

rust_robotics_mapping/
dbscan_clustering.rs

1//! DBSCAN (Density-Based Spatial Clustering of Applications with Noise) for 2D point clouds.
2//!
3//! Based on the work by Atsushi Sakai (@Atsushi_twi).
4
5/// Configuration for the DBSCAN algorithm.
6#[derive(Debug, Clone)]
7pub struct DBSCANConfig {
8    /// Neighborhood radius \[m\]. Points within this distance are considered neighbors.
9    pub eps: f64,
10    /// Minimum number of neighbors (including the point itself) required to form a core point.
11    pub min_points: usize,
12}
13
14impl Default for DBSCANConfig {
15    fn default() -> Self {
16        Self {
17            eps: 1.0,
18            min_points: 5,
19        }
20    }
21}
22
23/// Result of DBSCAN clustering.
24#[derive(Debug, Clone)]
25pub struct DBSCANResult {
26    /// X-coordinates of the input points.
27    pub x: Vec<f64>,
28    /// Y-coordinates of the input points.
29    pub y: Vec<f64>,
30    /// Cluster label for each point. `None` means the point is noise.
31    pub labels: Vec<Option<usize>>,
32    /// Number of clusters found (excluding noise).
33    pub n_clusters: usize,
34}
35
36impl DBSCANResult {
37    /// Return the (x, y) coordinates of points assigned to the given cluster label.
38    pub fn get_cluster_points(&self, label: usize) -> (Vec<f64>, Vec<f64>) {
39        let mut cx = Vec::new();
40        let mut cy = Vec::new();
41        for (i, &l) in self.labels.iter().enumerate() {
42            if l == Some(label) {
43                cx.push(self.x[i]);
44                cy.push(self.y[i]);
45            }
46        }
47        (cx, cy)
48    }
49
50    /// Return the (x, y) coordinates of noise points (label `None`).
51    pub fn get_noise_points(&self) -> (Vec<f64>, Vec<f64>) {
52        let mut nx = Vec::new();
53        let mut ny = Vec::new();
54        for (i, &l) in self.labels.iter().enumerate() {
55            if l.is_none() {
56                nx.push(self.x[i]);
57                ny.push(self.y[i]);
58            }
59        }
60        (nx, ny)
61    }
62}
63
64/// Run DBSCAN clustering on 2D points.
65///
66/// # Arguments
67/// * `x` - X-coordinates of data points.
68/// * `y` - Y-coordinates of data points.
69/// * `config` - Algorithm configuration (`eps`, `min_points`).
70///
71/// # Panics
72/// Panics if `x` and `y` have different lengths.
73pub fn dbscan_clustering(x: &[f64], y: &[f64], config: &DBSCANConfig) -> DBSCANResult {
74    assert_eq!(x.len(), y.len(), "x and y must have the same length");
75
76    let n = x.len();
77    let eps2 = config.eps * config.eps;
78
79    // None = unvisited; will be set to Some(cluster_id) or remain None (noise).
80    let mut labels: Vec<Option<usize>> = vec![None; n];
81    // Track whether a point has been visited.
82    let mut visited = vec![false; n];
83    let mut cluster_id = 0usize;
84
85    for i in 0..n {
86        if visited[i] {
87            continue;
88        }
89        visited[i] = true;
90
91        let neighbors = region_query(x, y, i, eps2);
92
93        if neighbors.len() < config.min_points {
94            // Mark as noise for now; may be absorbed into a cluster later.
95            // labels[i] stays None.
96        } else {
97            expand_cluster(
98                x,
99                y,
100                &mut labels,
101                &mut visited,
102                i,
103                &neighbors,
104                cluster_id,
105                config.min_points,
106                eps2,
107            );
108            cluster_id += 1;
109        }
110    }
111
112    DBSCANResult {
113        x: x.to_vec(),
114        y: y.to_vec(),
115        labels,
116        n_clusters: cluster_id,
117    }
118}
119
120/// Return indices of all points within `eps^2` squared distance of point `idx`.
121fn region_query(x: &[f64], y: &[f64], idx: usize, eps2: f64) -> Vec<usize> {
122    let mut neighbors = Vec::new();
123    let px = x[idx];
124    let py = y[idx];
125    for j in 0..x.len() {
126        let dx = x[j] - px;
127        let dy = y[j] - py;
128        if dx * dx + dy * dy <= eps2 {
129            neighbors.push(j);
130        }
131    }
132    neighbors
133}
134
135/// Grow a cluster starting from `core_idx` using the provided initial neighbor set.
136#[allow(clippy::too_many_arguments)]
137fn expand_cluster(
138    x: &[f64],
139    y: &[f64],
140    labels: &mut [Option<usize>],
141    visited: &mut [bool],
142    core_idx: usize,
143    initial_neighbors: &[usize],
144    cluster_id: usize,
145    min_points: usize,
146    eps2: f64,
147) {
148    labels[core_idx] = Some(cluster_id);
149
150    let mut queue: Vec<usize> = initial_neighbors.to_vec();
151    let mut head = 0;
152
153    while head < queue.len() {
154        let q = queue[head];
155        head += 1;
156
157        if !visited[q] {
158            visited[q] = true;
159            let q_neighbors = region_query(x, y, q, eps2);
160            if q_neighbors.len() >= min_points {
161                for &nb in &q_neighbors {
162                    if !visited[nb] {
163                        queue.push(nb);
164                    }
165                }
166            }
167        }
168
169        if labels[q].is_none() {
170            labels[q] = Some(cluster_id);
171        }
172    }
173}
174
175#[cfg(test)]
176mod tests {
177    use super::*;
178
179    fn make_cluster(cx: f64, cy: f64, n: usize, spread: f64) -> (Vec<f64>, Vec<f64>) {
180        // Deterministic points arranged in a grid-like pattern around (cx, cy).
181        let side = (n as f64).sqrt().ceil() as usize;
182        let step = spread / side.max(1) as f64;
183        let mut xs = Vec::with_capacity(n);
184        let mut ys = Vec::with_capacity(n);
185        'outer: for row in 0..side {
186            for col in 0..side {
187                xs.push(cx + col as f64 * step);
188                ys.push(cy + row as f64 * step);
189                if xs.len() == n {
190                    break 'outer;
191                }
192            }
193        }
194        (xs, ys)
195    }
196
197    #[test]
198    fn test_two_separated_clusters() {
199        let (mut x, mut y) = make_cluster(0.0, 0.0, 10, 0.5);
200        let (x2, y2) = make_cluster(100.0, 100.0, 10, 0.5);
201        x.extend(x2);
202        y.extend(y2);
203
204        let config = DBSCANConfig {
205            eps: 2.0,
206            min_points: 3,
207        };
208        let result = dbscan_clustering(&x, &y, &config);
209
210        assert_eq!(result.n_clusters, 2);
211        assert_eq!(result.labels.len(), 20);
212
213        // All points should be assigned to one of the two clusters.
214        for &l in &result.labels {
215            assert!(l.is_some(), "expected no noise in well-separated clusters");
216        }
217
218        // First 10 points belong to one cluster, last 10 to another.
219        let first_label = result.labels[0];
220        for &l in &result.labels[..10] {
221            assert_eq!(l, first_label);
222        }
223        let second_label = result.labels[10];
224        assert_ne!(first_label, second_label);
225        for &l in &result.labels[10..] {
226            assert_eq!(l, second_label);
227        }
228    }
229
230    #[test]
231    fn test_noise_detection() {
232        // Dense cluster + isolated outliers.
233        let (mut x, mut y) = make_cluster(0.0, 0.0, 10, 0.4);
234        // Three isolated outliers far away.
235        x.extend([50.0, 51.0, 200.0]);
236        y.extend([50.0, 51.0, 200.0]);
237
238        let config = DBSCANConfig {
239            eps: 1.0,
240            min_points: 5,
241        };
242        let result = dbscan_clustering(&x, &y, &config);
243
244        let (noise_x, _) = result.get_noise_points();
245        // At least the lone outlier at (200, 200) should be noise.
246        assert!(!noise_x.is_empty(), "expected some noise points");
247    }
248
249    #[test]
250    fn test_single_cluster() {
251        let (x, y) = make_cluster(5.0, 5.0, 15, 0.5);
252
253        let config = DBSCANConfig {
254            eps: 2.0,
255            min_points: 3,
256        };
257        let result = dbscan_clustering(&x, &y, &config);
258
259        assert_eq!(result.n_clusters, 1);
260        for &l in &result.labels {
261            assert_eq!(l, Some(0), "all points should be in cluster 0");
262        }
263    }
264
265    #[test]
266    fn test_all_noise_high_min_points() {
267        // Spread points far apart so no point has enough neighbors.
268        let x: Vec<f64> = (0..10).map(|i| i as f64 * 100.0).collect();
269        let y: Vec<f64> = vec![0.0; 10];
270
271        let config = DBSCANConfig {
272            eps: 1.0,
273            min_points: 5,
274        };
275        let result = dbscan_clustering(&x, &y, &config);
276
277        assert_eq!(result.n_clusters, 0);
278        for &l in &result.labels {
279            assert!(l.is_none(), "all points should be noise");
280        }
281    }
282
283    #[test]
284    fn test_get_cluster_points_correctness() {
285        // Two tiny well-separated clusters.
286        let x = vec![0.0, 0.1, 0.2, 10.0, 10.1, 10.2];
287        let y = vec![0.0, 0.1, 0.2, 10.0, 10.1, 10.2];
288
289        let config = DBSCANConfig {
290            eps: 0.5,
291            min_points: 2,
292        };
293        let result = dbscan_clustering(&x, &y, &config);
294
295        assert_eq!(result.n_clusters, 2);
296
297        let (c0x, c0y) = result.get_cluster_points(0);
298        let (c1x, c1y) = result.get_cluster_points(1);
299
300        assert_eq!(c0x.len(), c0y.len());
301        assert_eq!(c1x.len(), c1y.len());
302        assert_eq!(c0x.len() + c1x.len(), 6, "all points must be in a cluster");
303
304        let (nx, _) = result.get_noise_points();
305        assert!(nx.is_empty(), "no noise expected");
306    }
307}