Skip to main content

rust_robotics_slam/
robust_icp.rs

1/*!
2 * Robust ICP with Gauss-Newton optimization and Huber loss
3 *
4 * Based on tier4/icp_rust architecture:
5 * - KdTree-accelerated nearest neighbor search
6 * - Huber loss for outlier robustness
7 * - Weighted Gauss-Newton optimization on SE(2)
8 *
9 * Reference: <https://github.com/tier4/icp_rust>
10 */
11
12use nalgebra::{Matrix2, Matrix2x3, Matrix3, Rotation2, SVector, Vector2, Vector3};
13use nearest_neighbor::KdTree;
14
15// ── Huber loss ───────────────────────────────────────────────────
16
17const 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// ── SE(2) Transform ──────────────────────────────────────────────
38
39/// 2D rigid body transform (rotation + translation)
40#[derive(Copy, Clone, Debug)]
41pub struct Transform2D {
42    pub rot: Rotation2<f64>,
43    pub t: Vector2<f64>,
44}
45
46impl Transform2D {
47    /// Create from parameter vector [tx, ty, theta]
48    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    /// Compose: self * rhs
77    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
85// ── Robust ICP ───────────────────────────────────────────────────
86
87/// Result of robust ICP matching
88pub struct RobustICPResult {
89    pub transform: Transform2D,
90    pub iterations: usize,
91    pub final_error: f64,
92}
93
94/// Robust 2D ICP using KdTree + Huber-weighted Gauss-Newton
95pub struct RobustIcp2D<'a> {
96    kdtree: KdTree<'a, f64, 2>,
97    dst: &'a [SVector<f64, 2>],
98}
99
100impl<'a> RobustIcp2D<'a> {
101    /// Build KdTree from reference (destination) point cloud
102    pub fn new(dst: &'a [SVector<f64, 2>]) -> Self {
103        Self {
104            kdtree: KdTree::new(dst, 2),
105            dst,
106        }
107    }
108
109    /// Estimate transform that aligns src to dst
110    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            // Transform source points
124            let src_transformed: Vec<Vector2<f64>> =
125                src.iter().map(|sp| transform.transform(sp)).collect();
126
127            // Find nearest neighbors in dst
128            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            // Compute error
138            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            // Weighted Gauss-Newton update
153            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
173/// Jacobian of `exp(δ) ∘ T` applied to a point, w.r.t. the left SE(2)
174/// perturbation `δ = [tx, ty, θ]`, evaluated at the already transformed point.
175fn jacobian(transformed: &Vector2<f64>) -> Matrix2x3<f64> {
176    Matrix2x3::new(1.0, 0.0, -transformed[1], 0.0, 1.0, transformed[0])
177}
178
179/// Compute MAD-based robust standard deviation
180fn calc_stddevs(residuals: &[Vector2<f64>]) -> Option<[f64; 2]> {
181    if residuals.is_empty() {
182        return None;
183    }
184    let ppf34 = 1.482602218505602; // 1 / PPF(0.75) for normal distribution
185    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
215/// Inverse of 3x3 matrix (explicit formula)
216fn 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
232/// Weighted Gauss-Newton update with Huber loss.
233///
234/// `src` must already be transformed by the current estimate; the returned
235/// step is a left perturbation, applied as `from_param(step).compose(&T)`.
236fn 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        // Use more spread-out points for better conditioning
297        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        // Verify each point is close after transform
324        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        // src = truth⁻¹(dst), so the transform aligning src to dst is `truth`.
345        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        // Within threshold: identity
359        assert_eq!(huber_rho(0.5, 1.0), 0.5);
360        // Beyond threshold: linear growth
361        let rho = huber_rho(4.0, 1.0);
362        assert!((rho - 3.0).abs() < 1e-10); // 2*1*2 - 1 = 3
363    }
364}