1use nalgebra::{Matrix2, Matrix2x3, Matrix3, Rotation2, SVector, Vector2, Vector3};
13use nearest_neighbor::KdTree;
14
15const HUBER_K: f64 = 1.345;
18
19fn huber_rho(e: f64, k: f64) -> f64 {
20 let k_sq = k * k;
21 if e <= k_sq {
22 e
23 } else {
24 2.0 * k * e.sqrt() - k_sq
25 }
26}
27
28fn huber_drho(e: f64, k: f64) -> f64 {
29 let k_sq = k * k;
30 if e <= k_sq {
31 1.0
32 } else {
33 k / e.sqrt()
34 }
35}
36
37#[derive(Copy, Clone, Debug)]
41pub struct Transform2D {
42 pub rot: Rotation2<f64>,
43 pub t: Vector2<f64>,
44}
45
46impl Transform2D {
47 pub fn from_param(param: &Vector3<f64>) -> Self {
49 let theta = param[2];
50 let cos = theta.cos();
51 let sin = theta.sin();
52 let rot = Rotation2::from_matrix_unchecked(Matrix2::new(cos, -sin, sin, cos));
53
54 let t = if theta.abs() < 1e-10 {
55 Vector2::new(param[0], param[1])
56 } else {
57 Vector2::new(
58 (sin * param[0] - (1.0 - cos) * param[1]) / theta,
59 ((1.0 - cos) * param[0] + sin * param[1]) / theta,
60 )
61 };
62 Self { rot, t }
63 }
64
65 pub fn identity() -> Self {
66 Self {
67 rot: Rotation2::identity(),
68 t: Vector2::zeros(),
69 }
70 }
71
72 pub fn transform(&self, p: &Vector2<f64>) -> Vector2<f64> {
73 self.rot * p + self.t
74 }
75
76 pub fn compose(&self, rhs: &Self) -> Self {
78 Self {
79 rot: self.rot * rhs.rot,
80 t: self.rot * rhs.t + self.t,
81 }
82 }
83}
84
85pub struct RobustICPResult {
89 pub transform: Transform2D,
90 pub iterations: usize,
91 pub final_error: f64,
92}
93
94pub struct RobustIcp2D<'a> {
96 kdtree: KdTree<'a, f64, 2>,
97 dst: &'a [SVector<f64, 2>],
98}
99
100impl<'a> RobustIcp2D<'a> {
101 pub fn new(dst: &'a [SVector<f64, 2>]) -> Self {
103 Self {
104 kdtree: KdTree::new(dst, 2),
105 dst,
106 }
107 }
108
109 pub fn estimate(
111 &self,
112 src: &[SVector<f64, 2>],
113 initial: &Transform2D,
114 max_iter: usize,
115 ) -> RobustICPResult {
116 let mut transform = *initial;
117 let mut prev_error = f64::MAX;
118 let mut iterations = 0;
119
120 for iter in 0..max_iter {
121 iterations = iter + 1;
122
123 let src_transformed: Vec<Vector2<f64>> =
125 src.iter().map(|sp| transform.transform(sp)).collect();
126
127 let nearest_dsts: Vec<Vector2<f64>> = src_transformed
129 .iter()
130 .map(|sp| {
131 let query = SVector::<f64, 2>::from([sp[0], sp[1]]);
132 let (index, _dist) = self.kdtree.search(&query);
133 self.dst[index.unwrap()]
134 })
135 .collect();
136
137 let error: f64 = src_transformed
139 .iter()
140 .zip(nearest_dsts.iter())
141 .map(|(s, d)| {
142 let r = s - d;
143 huber_rho(r.dot(&r), HUBER_K)
144 })
145 .sum();
146
147 if error > prev_error {
148 break;
149 }
150 prev_error = error;
151
152 let Some(delta) = weighted_gauss_newton(&src_transformed, &nearest_dsts) else {
154 break;
155 };
156
157 if delta.dot(&delta) < 1e-12 {
158 break;
159 }
160
161 let dtransform = Transform2D::from_param(&delta);
162 transform = dtransform.compose(&transform);
163 }
164
165 RobustICPResult {
166 transform,
167 iterations,
168 final_error: prev_error,
169 }
170 }
171}
172
173fn jacobian(transformed: &Vector2<f64>) -> Matrix2x3<f64> {
176 Matrix2x3::new(1.0, 0.0, -transformed[1], 0.0, 1.0, transformed[0])
177}
178
179fn calc_stddevs(residuals: &[Vector2<f64>]) -> Option<[f64; 2]> {
181 if residuals.is_empty() {
182 return None;
183 }
184 let ppf34 = 1.482602218505602; let mut result = [0.0; 2];
186
187 for j in 0..2 {
188 let mut vals: Vec<f64> = residuals.iter().map(|r| r[j]).collect();
189 let median = median_of(&mut vals)?;
190 let mut abs_devs: Vec<f64> = vals.iter().map(|v| (v - median).abs()).collect();
191 let mad = median_of(&mut abs_devs)?;
192 result[j] = ppf34 * mad;
193 }
194 Some(result)
195}
196
197fn median_of(data: &mut [f64]) -> Option<f64> {
198 let n = data.len();
199 if n == 0 {
200 return None;
201 }
202 let cmp = |a: &f64, b: &f64| a.partial_cmp(b).unwrap();
203 if n % 2 == 1 {
204 data.select_nth_unstable_by(n / 2, cmp);
205 Some(data[n / 2])
206 } else {
207 data.select_nth_unstable_by(n / 2 - 1, cmp);
208 let a = data[n / 2 - 1];
209 data.select_nth_unstable_by(n / 2, cmp);
210 let b = data[n / 2];
211 Some((a + b) / 2.0)
212 }
213}
214
215fn inverse3x3(m: &Matrix3<f64>) -> Option<Matrix3<f64>> {
217 let det = m[(0, 0)] * (m[(2, 2)] * m[(1, 1)] - m[(2, 1)] * m[(1, 2)])
218 - m[(1, 0)] * (m[(2, 2)] * m[(0, 1)] - m[(2, 1)] * m[(0, 2)])
219 + m[(2, 0)] * (m[(1, 2)] * m[(0, 1)] - m[(1, 1)] * m[(0, 2)]);
220 if det.abs() < 1e-30 {
221 return None;
222 }
223 #[rustfmt::skip]
224 let inv = Matrix3::new(
225 m[(2,2)]*m[(1,1)] - m[(2,1)]*m[(1,2)], -(m[(2,2)]*m[(0,1)] - m[(2,1)]*m[(0,2)]), m[(1,2)]*m[(0,1)] - m[(1,1)]*m[(0,2)],
226 -(m[(2,2)]*m[(1,0)] - m[(2,0)]*m[(1,2)]), m[(2,2)]*m[(0,0)] - m[(2,0)]*m[(0,2)], -(m[(1,2)]*m[(0,0)] - m[(1,0)]*m[(0,2)]),
227 m[(2,1)]*m[(1,0)] - m[(2,0)]*m[(1,1)], -(m[(2,1)]*m[(0,0)] - m[(2,0)]*m[(0,1)]), m[(1,1)]*m[(0,0)] - m[(1,0)]*m[(0,1)],
228 );
229 Some(inv / det)
230}
231
232fn weighted_gauss_newton(src: &[Vector2<f64>], dst: &[Vector2<f64>]) -> Option<Vector3<f64>> {
237 if src.len() < 2 {
238 return None;
239 }
240
241 let residuals: Vec<Vector2<f64>> = src.iter().zip(dst.iter()).map(|(s, d)| s - d).collect();
242
243 let stddevs = calc_stddevs(&residuals)?;
244
245 let mut jtr = Vector3::<f64>::zeros();
246 let mut jtj = Matrix3::<f64>::zeros();
247
248 for (s, r) in src.iter().zip(residuals.iter()) {
249 let j = jacobian(s);
250 for (dim, jacobian_row) in j.row_iter().enumerate() {
251 if stddevs[dim] == 0.0 {
252 continue;
253 }
254 let g = 1.0 / stddevs[dim];
255 let r_dim = r[dim];
256 let w = huber_drho(r_dim * r_dim, HUBER_K);
257
258 jtr += w * g * jacobian_row.transpose() * r_dim;
259 jtj += w * g * jacobian_row.transpose() * jacobian_row;
260 }
261 }
262
263 let inv = inverse3x3(&jtj)?;
264 Some(-inv * jtr)
265}
266
267#[cfg(test)]
268mod tests {
269 use super::*;
270
271 fn make_points(coords: &[[f64; 2]]) -> Vec<SVector<f64, 2>> {
272 coords.iter().map(|c| SVector::from(*c)).collect()
273 }
274
275 #[test]
276 fn test_robust_icp_identity() {
277 let dst = make_points(&[
278 [0.0, 0.0],
279 [1.0, 0.0],
280 [0.0, 1.0],
281 [1.0, 1.0],
282 [0.5, 0.5],
283 [0.3, 0.7],
284 [0.7, 0.3],
285 ]);
286 let src = dst.clone();
287
288 let icp = RobustIcp2D::new(&dst);
289 let result = icp.estimate(&src, &Transform2D::identity(), 20);
290
291 assert!(result.final_error < 1e-6);
292 }
293
294 #[test]
295 fn test_robust_icp_translation() {
296 let dst = make_points(&[
298 [-2.0, -2.0],
299 [-2.0, 0.0],
300 [-2.0, 2.0],
301 [0.0, -2.0],
302 [0.0, 0.0],
303 [0.0, 2.0],
304 [2.0, -2.0],
305 [2.0, 0.0],
306 [2.0, 2.0],
307 ]);
308
309 let true_param = Vector3::new(0.1, 0.05, 0.05);
310 let true_transform = Transform2D::from_param(&true_param);
311
312 let src: Vec<SVector<f64, 2>> = dst
313 .iter()
314 .map(|p| {
315 let tp = true_transform.transform(p);
316 SVector::from([tp[0], tp[1]])
317 })
318 .collect();
319
320 let icp = RobustIcp2D::new(&dst);
321 let result = icp.estimate(&src, &Transform2D::identity(), 50);
322
323 for (sp, dp) in src.iter().zip(dst.iter()) {
325 let transformed = result.transform.transform(sp);
326 let error = (transformed - dp).norm();
327 assert!(error < 1e-6, "Point error too large: {}", error);
328 }
329 }
330
331 fn spiral(count: usize) -> Vec<SVector<f64, 2>> {
332 (0..count)
333 .map(|i| {
334 let t = i as f64 * 0.25;
335 SVector::from([(0.5 + 0.1 * t) * t.cos(), (0.5 + 0.1 * t) * t.sin()])
336 })
337 .collect()
338 }
339
340 #[test]
341 fn test_robust_icp_non_identity_seed() {
342 let dst = spiral(60);
343 let truth = Transform2D::from_param(&Vector3::new(0.6, -0.4, 0.35));
344 let inverse_rot = truth.rot.inverse();
346 let src: Vec<SVector<f64, 2>> = dst.iter().map(|p| inverse_rot * (p - truth.t)).collect();
347 let seed = Transform2D::from_param(&Vector3::new(0.03, 0.02, -0.02)).compose(&truth);
348
349 let icp = RobustIcp2D::new(&dst);
350 let result = icp.estimate(&src, &seed, 50);
351
352 assert!((result.transform.t - truth.t).norm() < 1e-6);
353 assert!((result.transform.rot.angle() - truth.rot.angle()).abs() < 1e-6);
354 }
355
356 #[test]
357 fn test_huber_loss() {
358 assert_eq!(huber_rho(0.5, 1.0), 0.5);
360 let rho = huber_rho(4.0, 1.0);
362 assert!((rho - 3.0).abs() < 1e-10); }
364}