Skip to main content

rust_robotics_mapping/
line_extraction.rs

1//! Line extraction from 2D point clouds using the Split-and-Merge algorithm.
2//!
3//! The Split-and-Merge (Iterative End Point Fit) algorithm recursively splits
4//! a point sequence wherever a point deviates too far from the line connecting
5//! the two endpoints, then merges adjacent collinear segments.
6//!
7//! Reference: PythonRobotics line_extraction by Atsushi Sakai.
8
9/// Configuration parameters for line extraction.
10#[derive(Debug, Clone, PartialEq)]
11pub struct LineExtractionConfig {
12    /// Maximum perpendicular distance \[m\] from a point to its segment line
13    /// before the segment is split.
14    pub distance_threshold: f64,
15    /// Minimum number of points required to form a valid segment.
16    pub min_segment_length: usize,
17    /// Maximum angle difference \[rad\] between two adjacent segments for them
18    /// to be merged into one.
19    pub merge_angle_threshold: f64,
20}
21
22impl Default for LineExtractionConfig {
23    fn default() -> Self {
24        Self {
25            distance_threshold: 0.1,
26            min_segment_length: 3,
27            merge_angle_threshold: 0.1,
28        }
29    }
30}
31
32/// A line segment defined by its two endpoints.
33#[derive(Debug, Clone, PartialEq)]
34pub struct LineSegment {
35    /// X coordinate of the start point.
36    pub start_x: f64,
37    /// Y coordinate of the start point.
38    pub start_y: f64,
39    /// X coordinate of the end point.
40    pub end_x: f64,
41    /// Y coordinate of the end point.
42    pub end_y: f64,
43}
44
45impl LineSegment {
46    /// Euclidean length of the segment.
47    pub fn length(&self) -> f64 {
48        let dx = self.end_x - self.start_x;
49        let dy = self.end_y - self.start_y;
50        dx.hypot(dy)
51    }
52
53    /// Angle of the segment in radians (atan2 of direction vector).
54    fn angle(&self) -> f64 {
55        (self.end_y - self.start_y).atan2(self.end_x - self.start_x)
56    }
57}
58
59/// Result of a line extraction run.
60pub struct LineExtractionResult {
61    /// Extracted line segments.
62    pub segments: Vec<LineSegment>,
63    /// X coordinates of the input points (echoed for convenience).
64    pub x: Vec<f64>,
65    /// Y coordinates of the input points (echoed for convenience).
66    pub y: Vec<f64>,
67}
68
69/// Perpendicular distance from point `(px, py)` to the line through
70/// `(x1, y1)` and `(x2, y2)`.
71///
72/// Returns `0.0` when the two line endpoints coincide.
73pub fn point_to_line_distance(px: f64, py: f64, x1: f64, y1: f64, x2: f64, y2: f64) -> f64 {
74    let dx = x2 - x1;
75    let dy = y2 - y1;
76    let len_sq = dx * dx + dy * dy;
77    if len_sq == 0.0 {
78        return ((px - x1).powi(2) + (py - y1).powi(2)).sqrt();
79    }
80    // |cross product| / |line vector|
81    ((dy * px - dx * py + x2 * y1 - y2 * x1) / len_sq.sqrt()).abs()
82}
83
84/// Extract line segments from a 2D point cloud using Split-and-Merge.
85///
86/// # Panics
87///
88/// Panics if `x` and `y` have different lengths or contain fewer than 2 points.
89pub fn line_extraction(
90    x: &[f64],
91    y: &[f64],
92    config: &LineExtractionConfig,
93) -> LineExtractionResult {
94    assert_eq!(x.len(), y.len(), "x and y must have the same length");
95    assert!(
96        x.len() >= 2,
97        "at least 2 points are required for line extraction"
98    );
99
100    let indices: Vec<usize> = (0..x.len()).collect();
101    let mut segments = split_and_merge(x, y, &indices, config);
102    merge_collinear(&mut segments, config.merge_angle_threshold);
103
104    LineExtractionResult {
105        segments,
106        x: x.to_vec(),
107        y: y.to_vec(),
108    }
109}
110
111// ---------------------------------------------------------------------------
112// Internal helpers
113// ---------------------------------------------------------------------------
114
115/// Recursively split the point subsequence `indices` into line segments.
116fn split_and_merge(
117    x: &[f64],
118    y: &[f64],
119    indices: &[usize],
120    config: &LineExtractionConfig,
121) -> Vec<LineSegment> {
122    if indices.len() < config.min_segment_length {
123        // Not enough points — emit a degenerate segment if at least 2 points.
124        if indices.len() >= 2 {
125            let first = indices[0];
126            let last = *indices.last().unwrap();
127            return vec![LineSegment {
128                start_x: x[first],
129                start_y: y[first],
130                end_x: x[last],
131                end_y: y[last],
132            }];
133        }
134        return vec![];
135    }
136
137    let first = indices[0];
138    let last = *indices.last().unwrap();
139    let (x1, y1) = (x[first], y[first]);
140    let (x2, y2) = (x[last], y[last]);
141
142    // Find the point with the maximum perpendicular distance.
143    let (max_dist, split_pos) = indices[1..indices.len() - 1]
144        .iter()
145        .enumerate()
146        .map(|(i, &idx)| {
147            let d = point_to_line_distance(x[idx], y[idx], x1, y1, x2, y2);
148            (d, i + 1) // offset by 1 because we sliced from index 1
149        })
150        .fold((0.0_f64, 0usize), |(best_d, best_i), (d, i)| {
151            if d > best_d {
152                (d, i)
153            } else {
154                (best_d, best_i)
155            }
156        });
157
158    if max_dist > config.distance_threshold && split_pos > 0 {
159        // Split at the farthest point and recurse on both halves.
160        let left = split_and_merge(x, y, &indices[..=split_pos], config);
161        let right = split_and_merge(x, y, &indices[split_pos..], config);
162        [left, right].concat()
163    } else {
164        vec![LineSegment {
165            start_x: x1,
166            start_y: y1,
167            end_x: x2,
168            end_y: y2,
169        }]
170    }
171}
172
173/// Merge adjacent segments whose angular difference is below `threshold`.
174fn merge_collinear(segments: &mut Vec<LineSegment>, threshold: f64) {
175    let mut i = 0;
176    while i + 1 < segments.len() {
177        let angle_diff = (segments[i].angle() - segments[i + 1].angle())
178            .abs()
179            .rem_euclid(std::f64::consts::PI);
180        let angle_diff = angle_diff.min(std::f64::consts::PI - angle_diff);
181        if angle_diff < threshold {
182            let merged = LineSegment {
183                start_x: segments[i].start_x,
184                start_y: segments[i].start_y,
185                end_x: segments[i + 1].end_x,
186                end_y: segments[i + 1].end_y,
187            };
188            segments[i] = merged;
189            segments.remove(i + 1);
190        } else {
191            i += 1;
192        }
193    }
194}
195
196// ---------------------------------------------------------------------------
197// Tests
198// ---------------------------------------------------------------------------
199
200#[cfg(test)]
201mod tests {
202    use super::*;
203    use std::f64::consts::FRAC_PI_4;
204
205    #[test]
206    fn test_config_defaults() {
207        let cfg = LineExtractionConfig::default();
208        assert_eq!(cfg.distance_threshold, 0.1);
209        assert_eq!(cfg.min_segment_length, 3);
210        assert_eq!(cfg.merge_angle_threshold, 0.1);
211    }
212
213    #[test]
214    fn test_point_to_line_distance_known_values() {
215        // Point (0, 1) to the x-axis should be 1.0.
216        let d = point_to_line_distance(0.0, 1.0, -1.0, 0.0, 1.0, 0.0);
217        assert!((d - 1.0).abs() < 1e-12, "expected 1.0, got {d}");
218
219        // Point on the line should give 0.
220        let d2 = point_to_line_distance(0.5, 0.0, 0.0, 0.0, 1.0, 0.0);
221        assert!(d2.abs() < 1e-12, "expected 0.0, got {d2}");
222
223        // Point (1, 1) to the diagonal y=x (through origin at 45°).
224        // Distance = |1*1 - 1*1| / sqrt(2) = 0
225        let d3 = point_to_line_distance(1.0, 1.0, 0.0, 0.0, 2.0, 2.0);
226        assert!(d3.abs() < 1e-12, "expected 0.0, got {d3}");
227
228        // Point (0, 1) to the diagonal y=x.
229        // Line: y - x = 0  =>  distance = |0 - 1| / sqrt(2) = 1/sqrt(2)
230        let d4 = point_to_line_distance(0.0, 1.0, 0.0, 0.0, 1.0, 1.0);
231        let expected = 1.0 / std::f64::consts::SQRT_2;
232        assert!(
233            (d4 - expected).abs() < 1e-12,
234            "expected {expected}, got {d4}"
235        );
236    }
237
238    #[test]
239    fn test_collinear_points_produce_one_segment() {
240        // Ten points along y = 2x; should yield a single segment.
241        let x: Vec<f64> = (0..10).map(|i| i as f64).collect();
242        let y: Vec<f64> = x.iter().map(|xi| 2.0 * xi).collect();
243
244        let cfg = LineExtractionConfig::default();
245        let result = line_extraction(&x, &y, &cfg);
246
247        assert_eq!(
248            result.segments.len(),
249            1,
250            "collinear points should yield exactly 1 segment, got {:?}",
251            result.segments
252        );
253        let seg = &result.segments[0];
254        assert!((seg.start_x - 0.0).abs() < 1e-10);
255        assert!((seg.start_y - 0.0).abs() < 1e-10);
256        assert!((seg.end_x - 9.0).abs() < 1e-10);
257        assert!((seg.end_y - 18.0).abs() < 1e-10);
258        println!("collinear segment: {seg:?}, length={}", seg.length());
259    }
260
261    #[test]
262    fn test_l_shape_produces_two_segments() {
263        // Horizontal arm: (0,0)..(4,0), then vertical arm: (4,0)..(4,4).
264        let mut x = Vec::new();
265        let mut y = Vec::new();
266        for i in 0..=4 {
267            x.push(i as f64);
268            y.push(0.0);
269        }
270        // Exclude the shared corner to avoid duplicate.
271        for j in 1..=4 {
272            x.push(4.0);
273            y.push(j as f64);
274        }
275
276        let cfg = LineExtractionConfig {
277            distance_threshold: 0.05,
278            min_segment_length: 3,
279            merge_angle_threshold: 0.1,
280        };
281        let result = line_extraction(&x, &y, &cfg);
282
283        assert_eq!(
284            result.segments.len(),
285            2,
286            "L-shape should yield 2 segments, got {:?}",
287            result.segments
288        );
289
290        // The first segment should be nearly horizontal (|angle| ≈ 0).
291        let h_seg = &result.segments[0];
292        assert!(
293            h_seg.angle().abs() < 0.01,
294            "first segment should be horizontal, angle={}",
295            h_seg.angle()
296        );
297
298        // The second segment should be nearly vertical (|angle| ≈ π/2).
299        let v_seg = &result.segments[1];
300        assert!(
301            (v_seg.angle().abs() - FRAC_PI_4 * 2.0).abs() < 0.01,
302            "second segment should be vertical, angle={}",
303            v_seg.angle()
304        );
305
306        println!("L-shape segments: {h_seg:?}, {v_seg:?}");
307    }
308}