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(&transform, &src_transformed, &nearest_dsts)
154            else {
155                break;
156            };
157
158            if delta.dot(&delta) < 1e-12 {
159                break;
160            }
161
162            let dtransform = Transform2D::from_param(&delta);
163            transform = dtransform.compose(&transform);
164        }
165
166        RobustICPResult {
167            transform,
168            iterations,
169            final_error: prev_error,
170        }
171    }
172}
173
174/// Jacobian of transform w.r.t. SE(2) parameters
175fn jacobian(rot: &Rotation2<f64>, landmark: &Vector2<f64>) -> Matrix2x3<f64> {
176    let a = Vector2::new(-landmark[1], landmark[0]);
177    let r = rot.matrix();
178    let b = rot * a;
179    Matrix2x3::new(r[(0, 0)], r[(0, 1)], b[0], r[(1, 0)], r[(1, 1)], b[1])
180}
181
182/// Compute MAD-based robust standard deviation
183fn calc_stddevs(residuals: &[Vector2<f64>]) -> Option<[f64; 2]> {
184    if residuals.is_empty() {
185        return None;
186    }
187    let ppf34 = 1.482602218505602; // 1 / PPF(0.75) for normal distribution
188    let mut result = [0.0; 2];
189
190    for j in 0..2 {
191        let mut vals: Vec<f64> = residuals.iter().map(|r| r[j]).collect();
192        let median = median_of(&mut vals)?;
193        let mut abs_devs: Vec<f64> = vals.iter().map(|v| (v - median).abs()).collect();
194        let mad = median_of(&mut abs_devs)?;
195        result[j] = ppf34 * mad;
196    }
197    Some(result)
198}
199
200fn median_of(data: &mut [f64]) -> Option<f64> {
201    let n = data.len();
202    if n == 0 {
203        return None;
204    }
205    let cmp = |a: &f64, b: &f64| a.partial_cmp(b).unwrap();
206    if n % 2 == 1 {
207        data.select_nth_unstable_by(n / 2, cmp);
208        Some(data[n / 2])
209    } else {
210        data.select_nth_unstable_by(n / 2 - 1, cmp);
211        let a = data[n / 2 - 1];
212        data.select_nth_unstable_by(n / 2, cmp);
213        let b = data[n / 2];
214        Some((a + b) / 2.0)
215    }
216}
217
218/// Inverse of 3x3 matrix (explicit formula)
219fn inverse3x3(m: &Matrix3<f64>) -> Option<Matrix3<f64>> {
220    let det = m[(0, 0)] * (m[(2, 2)] * m[(1, 1)] - m[(2, 1)] * m[(1, 2)])
221        - m[(1, 0)] * (m[(2, 2)] * m[(0, 1)] - m[(2, 1)] * m[(0, 2)])
222        + m[(2, 0)] * (m[(1, 2)] * m[(0, 1)] - m[(1, 1)] * m[(0, 2)]);
223    if det.abs() < 1e-30 {
224        return None;
225    }
226    #[rustfmt::skip]
227    let inv = Matrix3::new(
228        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)],
229       -(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)]),
230        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)],
231    );
232    Some(inv / det)
233}
234
235/// Weighted Gauss-Newton update with Huber loss
236fn weighted_gauss_newton(
237    transform: &Transform2D,
238    src: &[Vector2<f64>],
239    dst: &[Vector2<f64>],
240) -> Option<Vector3<f64>> {
241    if src.len() < 2 {
242        return None;
243    }
244
245    let residuals: Vec<Vector2<f64>> = src
246        .iter()
247        .zip(dst.iter())
248        .map(|(s, d)| transform.transform(s) - d)
249        .collect();
250
251    let stddevs = calc_stddevs(&residuals)?;
252
253    let mut jtr = Vector3::<f64>::zeros();
254    let mut jtj = Matrix3::<f64>::zeros();
255
256    for (s, r) in src.iter().zip(residuals.iter()) {
257        let j = jacobian(&transform.rot, s);
258        for (dim, jacobian_row) in j.row_iter().enumerate() {
259            if stddevs[dim] == 0.0 {
260                continue;
261            }
262            let g = 1.0 / stddevs[dim];
263            let r_dim = r[dim];
264            let w = huber_drho(r_dim * r_dim, HUBER_K);
265
266            jtr += w * g * jacobian_row.transpose() * r_dim;
267            jtj += w * g * jacobian_row.transpose() * jacobian_row;
268        }
269    }
270
271    let inv = inverse3x3(&jtj)?;
272    Some(-inv * jtr)
273}
274
275#[cfg(test)]
276mod tests {
277    use super::*;
278
279    fn make_points(coords: &[[f64; 2]]) -> Vec<SVector<f64, 2>> {
280        coords.iter().map(|c| SVector::from(*c)).collect()
281    }
282
283    #[test]
284    fn test_robust_icp_identity() {
285        let dst = make_points(&[
286            [0.0, 0.0],
287            [1.0, 0.0],
288            [0.0, 1.0],
289            [1.0, 1.0],
290            [0.5, 0.5],
291            [0.3, 0.7],
292            [0.7, 0.3],
293        ]);
294        let src = dst.clone();
295
296        let icp = RobustIcp2D::new(&dst);
297        let result = icp.estimate(&src, &Transform2D::identity(), 20);
298
299        assert!(result.final_error < 1e-6);
300    }
301
302    #[test]
303    fn test_robust_icp_translation() {
304        // Use more spread-out points for better conditioning
305        let dst = make_points(&[
306            [-2.0, -2.0],
307            [-2.0, 0.0],
308            [-2.0, 2.0],
309            [0.0, -2.0],
310            [0.0, 0.0],
311            [0.0, 2.0],
312            [2.0, -2.0],
313            [2.0, 0.0],
314            [2.0, 2.0],
315        ]);
316
317        let true_param = Vector3::new(0.1, 0.05, 0.05);
318        let true_transform = Transform2D::from_param(&true_param);
319
320        let src: Vec<SVector<f64, 2>> = dst
321            .iter()
322            .map(|p| {
323                let tp = true_transform.transform(p);
324                SVector::from([tp[0], tp[1]])
325            })
326            .collect();
327
328        let icp = RobustIcp2D::new(&dst);
329        let result = icp.estimate(&src, &Transform2D::identity(), 50);
330
331        // Verify each point is close after transform
332        for (sp, dp) in src.iter().zip(dst.iter()) {
333            let transformed = result.transform.transform(sp);
334            let error = (transformed - dp).norm();
335            assert!(error < 0.3, "Point error too large: {}", error);
336        }
337    }
338
339    #[test]
340    fn test_huber_loss() {
341        // Within threshold: identity
342        assert_eq!(huber_rho(0.5, 1.0), 0.5);
343        // Beyond threshold: linear growth
344        let rho = huber_rho(4.0, 1.0);
345        assert!((rho - 3.0).abs() < 1e-10); // 2*1*2 - 1 = 3
346    }
347}
rust_robotics_slam/robust_icp.rs

rust_robotics_slam/
robust_icp.rs
