rust_robotics_mapping/
circle_fitting.rs1use nalgebra::{Matrix3, Vector3};
9
10#[derive(Debug, Clone, PartialEq)]
12pub struct CircleFitResult {
13 pub cx: f64,
15 pub cy: f64,
17 pub radius: f64,
19 pub error: f64,
21}
22
23pub 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 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 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}