Skip to main content

rust_robotics_mapping/
circle_fitting.rs

1//! Circle fitting using a least-squares approach.
2//!
3//! Fits a circle (center + radius) to a set of 2D points by solving
4//! a 3x3 linear system derived from minimising the algebraic distance.
5//!
6//! Reference: PythonRobotics circle_fitting by Atsushi Sakai.
7
8use nalgebra::{Matrix3, Vector3};
9
10/// Result of a circle fit.
11#[derive(Debug, Clone, PartialEq)]
12pub struct CircleFitResult {
13    /// X coordinate of the fitted circle centre.
14    pub cx: f64,
15    /// Y coordinate of the fitted circle centre.
16    pub cy: f64,
17    /// Radius of the fitted circle.
18    pub radius: f64,
19    /// Sum of signed residuals (distance to circle boundary).
20    pub error: f64,
21}
22
23/// Fit a circle to a set of 2D points using the least-squares method.
24///
25/// The algorithm builds the normal equations from the point sums and solves
26/// for the circle parameters `(cx, cy, r)` that minimise the algebraic
27/// distance.
28///
29/// # Panics
30///
31/// Panics if `x` and `y` have different lengths or contain fewer than 3
32/// points (a circle cannot be uniquely determined with fewer than 3 points).
33pub fn circle_fitting(x: &[f64], y: &[f64]) -> CircleFitResult {
34    assert_eq!(x.len(), y.len(), "x and y must have the same length");
35    assert!(
36        x.len() >= 3,
37        "at least 3 points are required for circle fitting"
38    );
39
40    let n = x.len() as f64;
41
42    let sum_x: f64 = x.iter().sum();
43    let sum_y: f64 = y.iter().sum();
44    let sum_x2: f64 = x.iter().map(|v| v * v).sum();
45    let sum_y2: f64 = y.iter().map(|v| v * v).sum();
46    let sum_xy: f64 = x.iter().zip(y.iter()).map(|(xi, yi)| xi * yi).sum();
47
48    // F * T = G  =>  T = F^{-1} * G
49    let f = Matrix3::new(
50        sum_x2, sum_xy, sum_x, sum_xy, sum_y2, sum_y, sum_x, sum_y, n,
51    );
52
53    let g = Vector3::new(
54        -x.iter()
55            .zip(y.iter())
56            .map(|(xi, yi)| xi.powi(3) + xi * yi.powi(2))
57            .sum::<f64>(),
58        -x.iter()
59            .zip(y.iter())
60            .map(|(xi, yi)| xi.powi(2) * yi + yi.powi(3))
61            .sum::<f64>(),
62        -x.iter()
63            .zip(y.iter())
64            .map(|(xi, yi)| xi.powi(2) + yi.powi(2))
65            .sum::<f64>(),
66    );
67
68    let f_inv = f
69        .try_inverse()
70        .expect("singular matrix – points may be collinear");
71    let t = f_inv * g;
72
73    let cx = t[0] / -2.0;
74    let cy = t[1] / -2.0;
75    let radius = (cx * cx + cy * cy - t[2]).sqrt();
76
77    let error: f64 = x
78        .iter()
79        .zip(y.iter())
80        .map(|(xi, yi)| ((cx - xi).powi(2) + (cy - yi).powi(2)).sqrt() - radius)
81        .sum();
82
83    CircleFitResult {
84        cx,
85        cy,
86        radius,
87        error,
88    }
89}
90
91#[cfg(test)]
92mod tests {
93    use super::*;
94    use std::f64::consts::PI;
95
96    /// Generate points on a circle with optional noise.
97    fn sample_circle(cx: f64, cy: f64, r: f64, n: usize) -> (Vec<f64>, Vec<f64>) {
98        let mut xs = Vec::with_capacity(n);
99        let mut ys = Vec::with_capacity(n);
100        for i in 0..n {
101            let theta = 2.0 * PI * i as f64 / n as f64;
102            xs.push(cx + r * theta.cos());
103            ys.push(cy + r * theta.sin());
104        }
105        (xs, ys)
106    }
107
108    #[test]
109    fn test_exact_circle() {
110        let (x, y) = sample_circle(3.0, -2.0, 5.0, 36);
111        let res = circle_fitting(&x, &y);
112        assert!((res.cx - 3.0).abs() < 1e-10, "cx mismatch: {}", res.cx);
113        assert!((res.cy - (-2.0)).abs() < 1e-10, "cy mismatch: {}", res.cy);
114        assert!(
115            (res.radius - 5.0).abs() < 1e-10,
116            "radius mismatch: {}",
117            res.radius
118        );
119        assert!(res.error.abs() < 1e-10, "error should be ~0: {}", res.error);
120    }
121
122    #[test]
123    fn test_unit_circle_at_origin() {
124        let (x, y) = sample_circle(0.0, 0.0, 1.0, 100);
125        let res = circle_fitting(&x, &y);
126        assert!((res.cx).abs() < 1e-10);
127        assert!((res.cy).abs() < 1e-10);
128        assert!((res.radius - 1.0).abs() < 1e-10);
129    }
130
131    #[test]
132    fn test_small_circle() {
133        let (x, y) = sample_circle(-1.0, 2.0, 0.5, 20);
134        let res = circle_fitting(&x, &y);
135        assert!((res.cx - (-1.0)).abs() < 1e-9);
136        assert!((res.cy - 2.0).abs() < 1e-9);
137        assert!((res.radius - 0.5).abs() < 1e-9);
138    }
139
140    #[test]
141    #[should_panic(expected = "at least 3 points")]
142    fn test_too_few_points() {
143        circle_fitting(&[1.0, 2.0], &[3.0, 4.0]);
144    }
145
146    #[test]
147    #[should_panic(expected = "same length")]
148    fn test_mismatched_lengths() {
149        circle_fitting(&[1.0, 2.0, 3.0], &[4.0, 5.0]);
150    }
151}