spatialrust_math/
robust.rs

1/// Robust loss kernel used by registration and estimation algorithms.
2pub trait RobustKernel {
3    /// Computes the robust weight for a residual magnitude.
4    fn weight(&self, residual: f64) -> f64;
5
6    /// Computes the robust rho value for a residual magnitude.
7    fn rho(&self, residual: f64) -> f64;
8}
9
10/// Huber robust kernel.
11#[derive(Clone, Copy, Debug, PartialEq)]
12pub struct HuberKernel {
13    /// Threshold parameter.
14    pub delta: f64,
15}
16
17impl HuberKernel {
18    /// Creates a Huber kernel with the given threshold.
19    #[must_use]
20    pub const fn new(delta: f64) -> Self {
21        Self { delta }
22    }
23}
24
25impl RobustKernel for HuberKernel {
26    fn weight(&self, residual: f64) -> f64 {
27        let r = residual.abs();
28        if r <= self.delta {
29            1.0
30        } else {
31            self.delta / r
32        }
33    }
34
35    fn rho(&self, residual: f64) -> f64 {
36        let r = residual.abs();
37        if r <= self.delta {
38            0.5 * r * r
39        } else {
40            self.delta * (r - 0.5 * self.delta)
41        }
42    }
43}
44
45/// Cauchy robust kernel.
46#[derive(Clone, Copy, Debug, PartialEq)]
47pub struct CauchyKernel {
48    /// Scale parameter.
49    pub c: f64,
50}
51
52impl CauchyKernel {
53    /// Creates a Cauchy kernel with the given scale.
54    #[must_use]
55    pub const fn new(c: f64) -> Self {
56        Self { c }
57    }
58}
59
60impl RobustKernel for CauchyKernel {
61    fn weight(&self, residual: f64) -> f64 {
62        let r2 = residual * residual;
63        let c2 = self.c * self.c;
64        1.0 / (1.0 + r2 / c2)
65    }
66
67    fn rho(&self, residual: f64) -> f64 {
68        let r2 = residual * residual;
69        let c2 = self.c * self.c;
70        0.5 * c2 * (1.0 + r2 / c2).ln()
71    }
72}
73
74/// Tukey biweight robust kernel.
75#[derive(Clone, Copy, Debug, PartialEq)]
76pub struct TukeyKernel {
77    /// Threshold parameter.
78    pub c: f64,
79}
80
81impl TukeyKernel {
82    /// Creates a Tukey kernel with the given threshold.
83    #[must_use]
84    pub const fn new(c: f64) -> Self {
85        Self { c }
86    }
87}
88
89impl RobustKernel for TukeyKernel {
90    fn weight(&self, residual: f64) -> f64 {
91        let r = residual.abs();
92        if r >= self.c {
93            return 0.0;
94        }
95        let t = 1.0 - (r / self.c).powi(2);
96        t * t
97    }
98
99    fn rho(&self, residual: f64) -> f64 {
100        let r = residual.abs();
101        if r >= self.c {
102            return self.c * self.c / 6.0;
103        }
104        let t = 1.0 - (r / self.c).powi(2);
105        (self.c * self.c / 6.0) * (1.0 - t.powi(3))
106    }
107}
108
109#[cfg(test)]
110mod tests {
111    use super::{CauchyKernel, HuberKernel, RobustKernel, TukeyKernel};
112
113    #[test]
114    fn huber_weight_matches_reference() {
115        let kernel = HuberKernel::new(1.0);
116        assert_eq!(kernel.weight(0.5), 1.0);
117        assert!((kernel.weight(2.0) - 0.5).abs() < 1e-12);
118        assert!((kernel.rho(2.0) - 1.5).abs() < 1e-12);
119    }
120
121    #[test]
122    fn cauchy_and_tukey_are_bounded() {
123        let cauchy = CauchyKernel::new(1.0);
124        assert!(cauchy.weight(10.0) < 0.1);
125
126        let tukey = TukeyKernel::new(1.0);
127        assert_eq!(tukey.weight(2.0), 0.0);
128    }
129}
spatialrust_math/robust.rs

spatialrust_math/
robust.rs
