Skip to main content

rust_robotics_mapping/
rectangle_fitting.rs

1//! Rectangle fitting via L-shape search.
2//!
3//! Fits a minimum bounding rectangle to a set of 2D points by searching
4//! over orientations and selecting the one that best matches according to
5//! a chosen criterion (area, closeness, or variance).
6//!
7//! Reference: PythonRobotics rectangle_fitting by Atsushi Sakai.
8//! Based on "Efficient L-Shape Fitting for Vehicle Detection Using Laser
9//! Scanners" (CMU Robotics Institute).
10
11use nalgebra::{Matrix2, Vector2};
12use std::f64::consts::FRAC_PI_2;
13
14// ---------------------------------------------------------------------------
15// Rectangle data
16// ---------------------------------------------------------------------------
17
18/// Four-line representation of a fitted rectangle.
19///
20/// Each side `i` is represented by the line `a[i]*x + b[i]*y = c[i]`.
21#[derive(Debug, Clone, PartialEq)]
22pub struct RectangleData {
23    /// Coefficients a for the four sides.
24    pub a: [f64; 4],
25    /// Coefficients b for the four sides.
26    pub b: [f64; 4],
27    /// Coefficients c for the four sides.
28    pub c: [f64; 4],
29}
30
31impl RectangleData {
32    fn new() -> Self {
33        Self {
34            a: [0.0; 4],
35            b: [0.0; 4],
36            c: [0.0; 4],
37        }
38    }
39
40    /// Compute the four corner vertices of the rectangle.
41    ///
42    /// Returns `[(x0,y0), (x1,y1), (x2,y2), (x3,y3)]`.
43    pub fn corners(&self) -> [(f64, f64); 4] {
44        let cross = |i: usize, j: usize| -> (f64, f64) {
45            let det = self.a[i] * self.b[j] - self.a[j] * self.b[i];
46            let x = (self.b[i] * -self.c[j] - self.b[j] * -self.c[i]) / det;
47            let y = (self.a[j] * -self.c[i] - self.a[i] * -self.c[j]) / det;
48            (x, y)
49        };
50        [cross(0, 1), cross(1, 2), cross(2, 3), cross(3, 0)]
51    }
52}
53
54// ---------------------------------------------------------------------------
55// Fitting criteria
56// ---------------------------------------------------------------------------
57
58/// Criterion used to score rectangle orientations.
59#[derive(Debug, Clone, Copy, PartialEq, Eq)]
60pub enum Criteria {
61    /// Minimise bounding area (negative area is maximised).
62    Area,
63    /// Maximise closeness of points to the nearest edge.
64    Closeness,
65    /// Minimise variance of point-to-edge distances.
66    Variance,
67}
68
69// ---------------------------------------------------------------------------
70// L-shape fitting
71// ---------------------------------------------------------------------------
72
73/// Configuration and entry point for the L-shape rectangle fitting algorithm.
74#[derive(Debug, Clone)]
75pub struct LShapeFitting {
76    /// Which criterion to optimise.
77    pub criteria: Criteria,
78    /// Minimum distance clamp for the closeness criterion \[m\].
79    pub min_dist_of_closeness_criteria: f64,
80    /// Angular search step \[rad\].
81    pub d_theta: f64,
82    /// Base range for adaptive segmentation \[m\].
83    pub r0: f64,
84    /// Range-dependent factor for adaptive segmentation \[m\].
85    pub rd: f64,
86}
87
88impl Default for LShapeFitting {
89    fn default() -> Self {
90        Self {
91            criteria: Criteria::Variance,
92            min_dist_of_closeness_criteria: 0.01,
93            d_theta: 1.0_f64.to_radians(),
94            r0: 3.0,
95            rd: 0.001,
96        }
97    }
98}
99
100impl LShapeFitting {
101    /// Create a new `LShapeFitting` with default parameters.
102    pub fn new() -> Self {
103        Self::default()
104    }
105
106    /// Run the full fitting pipeline: segment the points, then fit a
107    /// rectangle to each cluster.
108    ///
109    /// Returns a vector of `(RectangleData, Vec<usize>)` pairs where
110    /// the second element contains the point indices belonging to that
111    /// cluster.
112    pub fn fitting(&self, ox: &[f64], oy: &[f64]) -> Vec<(RectangleData, Vec<usize>)> {
113        let segments = self.adaptive_range_segmentation(ox, oy);
114        segments
115            .into_iter()
116            .map(|ids| {
117                let cx: Vec<f64> = ids.iter().map(|&i| ox[i]).collect();
118                let cy: Vec<f64> = ids.iter().map(|&i| oy[i]).collect();
119                let rect = self.rectangle_search(&cx, &cy);
120                (rect, ids)
121            })
122            .collect()
123    }
124
125    /// Fit a single rectangle to the given points (without segmentation).
126    pub fn rectangle_search(&self, x: &[f64], y: &[f64]) -> RectangleData {
127        assert_eq!(x.len(), y.len());
128        assert!(!x.is_empty(), "need at least one point");
129
130        let n = x.len();
131
132        let mut best_cost = f64::NEG_INFINITY;
133        let mut best_theta: f64 = 0.0;
134
135        let mut theta = 0.0_f64;
136        while theta < FRAC_PI_2 - self.d_theta {
137            let rot = rot_mat_2d(theta);
138
139            let mut c1 = Vec::with_capacity(n);
140            let mut c2 = Vec::with_capacity(n);
141            for i in 0..n {
142                let p = rot * Vector2::new(x[i], y[i]);
143                c1.push(p[0]);
144                c2.push(p[1]);
145            }
146
147            let cost = match self.criteria {
148                Criteria::Area => calc_area_criterion(&c1, &c2),
149                Criteria::Closeness => self.calc_closeness_criterion(&c1, &c2),
150                Criteria::Variance => calc_variance_criterion(&c1, &c2),
151            };
152
153            if cost > best_cost {
154                best_cost = cost;
155                best_theta = theta;
156            }
157
158            theta += self.d_theta;
159        }
160
161        let sin_s = best_theta.sin();
162        let cos_s = best_theta.cos();
163
164        let c1_s: Vec<f64> = x
165            .iter()
166            .zip(y.iter())
167            .map(|(xi, yi)| cos_s * xi + sin_s * yi)
168            .collect();
169        let c2_s: Vec<f64> = x
170            .iter()
171            .zip(y.iter())
172            .map(|(xi, yi)| -sin_s * xi + cos_s * yi)
173            .collect();
174
175        let c1_min = c1_s.iter().cloned().fold(f64::INFINITY, f64::min);
176        let c1_max = c1_s.iter().cloned().fold(f64::NEG_INFINITY, f64::max);
177        let c2_min = c2_s.iter().cloned().fold(f64::INFINITY, f64::min);
178        let c2_max = c2_s.iter().cloned().fold(f64::NEG_INFINITY, f64::max);
179
180        let mut rect = RectangleData::new();
181        rect.a = [cos_s, -sin_s, cos_s, -sin_s];
182        rect.b = [sin_s, cos_s, sin_s, cos_s];
183        rect.c = [c1_min, c2_min, c1_max, c2_max];
184
185        rect
186    }
187
188    /// Adaptive range segmentation – groups nearby points into clusters.
189    fn adaptive_range_segmentation(&self, ox: &[f64], oy: &[f64]) -> Vec<Vec<usize>> {
190        let n = ox.len();
191        let mut segments: Vec<Vec<usize>> = Vec::new();
192
193        // Build initial per-point neighbour sets.
194        let mut sets: Vec<Vec<usize>> = Vec::with_capacity(n);
195        for i in 0..n {
196            let r = self.r0 + self.rd * (ox[i].powi(2) + oy[i].powi(2)).sqrt();
197            let mut cluster = Vec::new();
198            for j in 0..n {
199                let d = ((ox[i] - ox[j]).powi(2) + (oy[i] - oy[j]).powi(2)).sqrt();
200                if d <= r {
201                    cluster.push(j);
202                }
203            }
204            sets.push(cluster);
205        }
206
207        // Merge overlapping clusters.
208        let mut merged: Vec<Vec<bool>> = sets
209            .iter()
210            .map(|s| {
211                let mut bv = vec![false; n];
212                for &idx in s {
213                    bv[idx] = true;
214                }
215                bv
216            })
217            .collect();
218
219        loop {
220            let mut changed = false;
221            let len = merged.len();
222            let mut i = 0;
223            while i < len {
224                let mut j = i + 1;
225                while j < merged.len() {
226                    let overlap = merged[i]
227                        .iter()
228                        .zip(merged[j].iter())
229                        .any(|(a, b)| *a && *b);
230                    if overlap {
231                        let other = merged.remove(j);
232                        for (k, value) in other.iter().enumerate().take(n) {
233                            merged[i][k] = merged[i][k] || *value;
234                        }
235                        changed = true;
236                    } else {
237                        j += 1;
238                    }
239                }
240                i += 1;
241            }
242            if !changed {
243                break;
244            }
245        }
246
247        for bv in merged {
248            let ids: Vec<usize> = bv
249                .iter()
250                .enumerate()
251                .filter_map(|(i, &v)| if v { Some(i) } else { None })
252                .collect();
253            if !ids.is_empty() {
254                segments.push(ids);
255            }
256        }
257
258        segments
259    }
260
261    fn calc_closeness_criterion(&self, c1: &[f64], c2: &[f64]) -> f64 {
262        let (c1_max, c1_min, c2_max, c2_min) = find_min_max(c1, c2);
263        c1.iter()
264            .zip(c2.iter())
265            .map(|(v1, v2)| {
266                let d1 = (c1_max - v1).min(v1 - c1_min);
267                let d2 = (c2_max - v2).min(v2 - c2_min);
268                1.0 / d1.min(d2).max(self.min_dist_of_closeness_criteria)
269            })
270            .sum()
271    }
272}
273
274// ---------------------------------------------------------------------------
275// Helper functions
276// ---------------------------------------------------------------------------
277
278fn rot_mat_2d(angle: f64) -> Matrix2<f64> {
279    let (s, c) = angle.sin_cos();
280    Matrix2::new(c, s, -s, c)
281}
282
283fn find_min_max(c1: &[f64], c2: &[f64]) -> (f64, f64, f64, f64) {
284    let c1_max = c1.iter().cloned().fold(f64::NEG_INFINITY, f64::max);
285    let c1_min = c1.iter().cloned().fold(f64::INFINITY, f64::min);
286    let c2_max = c2.iter().cloned().fold(f64::NEG_INFINITY, f64::max);
287    let c2_min = c2.iter().cloned().fold(f64::INFINITY, f64::min);
288    (c1_max, c1_min, c2_max, c2_min)
289}
290
291fn calc_area_criterion(c1: &[f64], c2: &[f64]) -> f64 {
292    let (c1_max, c1_min, c2_max, c2_min) = find_min_max(c1, c2);
293    -((c1_max - c1_min) * (c2_max - c2_min))
294}
295
296fn calc_variance_criterion(c1: &[f64], c2: &[f64]) -> f64 {
297    let (c1_max, c1_min, c2_max, c2_min) = find_min_max(c1, c2);
298
299    let mut e1: Vec<f64> = Vec::new();
300    let mut e2: Vec<f64> = Vec::new();
301
302    for (v1, v2) in c1.iter().zip(c2.iter()) {
303        let d1 = (c1_max - v1).min(v1 - c1_min);
304        let d2 = (c2_max - v2).min(v2 - c2_min);
305        if d1 < d2 {
306            e1.push(d1);
307        } else {
308            e2.push(d2);
309        }
310    }
311
312    let var = |vals: &[f64]| -> f64 {
313        if vals.is_empty() {
314            return 0.0;
315        }
316        let mean = vals.iter().sum::<f64>() / vals.len() as f64;
317        vals.iter().map(|v| (v - mean).powi(2)).sum::<f64>() / vals.len() as f64
318    };
319
320    let v1 = if e1.is_empty() { 0.0 } else { -var(&e1) };
321    let v2 = if e2.is_empty() { 0.0 } else { -var(&e2) };
322    v1 + v2
323}
324
325#[cfg(test)]
326mod tests {
327    use super::*;
328    use std::f64::consts::PI;
329
330    /// Generate points on the perimeter of an axis-aligned rectangle.
331    fn rect_points(cx: f64, cy: f64, w: f64, h: f64, n_per_side: usize) -> (Vec<f64>, Vec<f64>) {
332        let mut xs = Vec::new();
333        let mut ys = Vec::new();
334        let half_w = w / 2.0;
335        let half_h = h / 2.0;
336        for i in 0..n_per_side {
337            let t = i as f64 / n_per_side as f64;
338            // bottom
339            xs.push(cx - half_w + w * t);
340            ys.push(cy - half_h);
341            // top
342            xs.push(cx - half_w + w * t);
343            ys.push(cy + half_h);
344            // left
345            xs.push(cx - half_w);
346            ys.push(cy - half_h + h * t);
347            // right
348            xs.push(cx + half_w);
349            ys.push(cy - half_h + h * t);
350        }
351        (xs, ys)
352    }
353
354    #[test]
355    fn test_rectangle_search_axis_aligned() {
356        let (x, y) = rect_points(0.0, 0.0, 4.0, 2.0, 20);
357        let fitter = LShapeFitting::new();
358        let rect = fitter.rectangle_search(&x, &y);
359
360        let corners = rect.corners();
361        let mut xs: Vec<f64> = corners.iter().map(|c| c.0).collect();
362        let mut ys: Vec<f64> = corners.iter().map(|c| c.1).collect();
363        xs.sort_by(|a, b| a.partial_cmp(b).unwrap());
364        ys.sort_by(|a, b| a.partial_cmp(b).unwrap());
365
366        let width = xs[3] - xs[0];
367        let height = ys[3] - ys[0];
368        let (w, h) = if width > height {
369            (width, height)
370        } else {
371            (height, width)
372        };
373
374        assert!((w - 4.0).abs() < 0.2, "width mismatch: {}", w);
375        assert!((h - 2.0).abs() < 0.2, "height mismatch: {}", h);
376    }
377
378    #[test]
379    fn test_rectangle_search_rotated() {
380        let angle = PI / 6.0; // 30 degrees
381        let (s, c) = angle.sin_cos();
382        let (base_x, base_y) = rect_points(0.0, 0.0, 6.0, 3.0, 20);
383        let x: Vec<f64> = base_x
384            .iter()
385            .zip(base_y.iter())
386            .map(|(bx, by)| c * bx - s * by)
387            .collect();
388        let y: Vec<f64> = base_x
389            .iter()
390            .zip(base_y.iter())
391            .map(|(bx, by)| s * bx + c * by)
392            .collect();
393
394        let fitter = LShapeFitting::new();
395        let rect = fitter.rectangle_search(&x, &y);
396
397        let corners = rect.corners();
398        // Check that all 4 edges have roughly the right lengths.
399        let dist =
400            |a: (f64, f64), b: (f64, f64)| ((a.0 - b.0).powi(2) + (a.1 - b.1).powi(2)).sqrt();
401        let mut sides = [
402            dist(corners[0], corners[1]),
403            dist(corners[1], corners[2]),
404            dist(corners[2], corners[3]),
405            dist(corners[3], corners[0]),
406        ];
407        sides.sort_by(|a, b| a.partial_cmp(b).unwrap());
408
409        assert!((sides[0] - 3.0).abs() < 0.3, "short side: {}", sides[0]);
410        assert!((sides[1] - 3.0).abs() < 0.3, "short side: {}", sides[1]);
411        assert!((sides[2] - 6.0).abs() < 0.3, "long side: {}", sides[2]);
412        assert!((sides[3] - 6.0).abs() < 0.3, "long side: {}", sides[3]);
413    }
414
415    #[test]
416    fn test_fitting_with_segmentation() {
417        // Two separated clusters
418        let (mut x, mut y) = rect_points(0.0, 0.0, 2.0, 1.0, 10);
419        let (x2, y2) = rect_points(20.0, 20.0, 3.0, 1.5, 10);
420        x.extend(x2);
421        y.extend(y2);
422
423        let fitter = LShapeFitting {
424            r0: 3.0,
425            rd: 0.001,
426            ..LShapeFitting::default()
427        };
428        let results = fitter.fitting(&x, &y);
429        assert_eq!(results.len(), 2, "should detect two clusters");
430    }
431
432    #[test]
433    fn test_criteria_area() {
434        let (x, y) = rect_points(0.0, 0.0, 4.0, 2.0, 15);
435        let fitter = LShapeFitting {
436            criteria: Criteria::Area,
437            ..LShapeFitting::default()
438        };
439        let rect = fitter.rectangle_search(&x, &y);
440        let corners = rect.corners();
441        // Just verify we get 4 finite corners.
442        for (cx, cy) in &corners {
443            assert!(cx.is_finite() && cy.is_finite());
444        }
445    }
446
447    #[test]
448    fn test_criteria_closeness() {
449        let (x, y) = rect_points(0.0, 0.0, 4.0, 2.0, 15);
450        let fitter = LShapeFitting {
451            criteria: Criteria::Closeness,
452            ..LShapeFitting::default()
453        };
454        let rect = fitter.rectangle_search(&x, &y);
455        let corners = rect.corners();
456        for (cx, cy) in &corners {
457            assert!(cx.is_finite() && cy.is_finite());
458        }
459    }
460}