rigidity_core/icp/kernel.rs
1//! Robust loss functions.
2//!
3//! A squared loss assumes Gaussian errors. Real clouds do not supply them:
4//! wrong correspondences near boundaries, points from another surface,
5//! reflections. One such point at a ten-sigma residual contributes as much
6//! to the squared sum as a hundred well-behaved ones, and drags the
7//! solution with it.
8//!
9//! A robust kernel bounds that contribution. What gets minimised is
10//! `Σ ρ(e)` rather than `Σ e²`, solved by IRLS — iteratively reweighted
11//! least squares with weight `w(e) = ψ(e)/e`, where `ψ = dρ/de`.
12
13/// A loss function.
14///
15/// Each kernel's parameter is a scale in units of the residual, that is,
16/// in metres. It sets the boundary of a "normal" error; three standard
17/// deviations of the sensor noise is a reasonable starting point.
18#[derive(Debug, Clone, Copy, PartialEq)]
19pub enum Kernel {
20 /// Squared: `ρ(e) = e²/2`. Not robust; kept for comparison.
21 Squared,
22 /// Huber: quadratic near zero, linear outside.
23 ///
24 /// An outlier's contribution still grows, but only linearly. Convex,
25 /// so it creates no local minima.
26 Huber(f64),
27 /// Cauchy: the contribution grows logarithmically.
28 ///
29 /// Suppresses harder than Huber, at the cost of being non-convex.
30 Cauchy(f64),
31 /// Tukey: an outlier's contribution is exactly zero past the
32 /// threshold.
33 ///
34 /// The harshest of these: a point beyond the threshold has no
35 /// influence at all. It demands a decent initial guess.
36 Tukey(f64),
37 /// Geman–McClure: smooth saturation without a hard threshold.
38 GemanMcClure(f64),
39}
40
41impl Kernel {
42 /// The IRLS weight `w(e) = ψ(e)/e`.
43 ///
44 /// At zero every kernel gives one: near the origin all of them agree
45 /// with the squared loss.
46 pub fn weight(self, residual: f64) -> f64 {
47 let e = residual.abs();
48 match self {
49 Self::Squared => 1.0,
50 Self::Huber(delta) => {
51 if e <= delta {
52 1.0
53 } else {
54 delta / e
55 }
56 }
57 Self::Cauchy(c) => {
58 let t = e / c;
59 1.0 / (1.0 + t * t)
60 }
61 Self::Tukey(c) => {
62 if e <= c {
63 let t = e / c;
64 let s = 1.0 - t * t;
65 s * s
66 } else {
67 0.0
68 }
69 }
70 Self::GemanMcClure(c) => {
71 let denominator = c * c + e * e;
72 (c * c * c * c) / (denominator * denominator)
73 }
74 }
75 }
76
77 /// The value of `ρ(e)`.
78 ///
79 /// Needed by the LM step-acceptance test: what must be compared is the
80 /// robust cost rather than the sum of squares, or a step that is right
81 /// by the kernel's own measure gets rejected.
82 pub fn loss(self, residual: f64) -> f64 {
83 let e = residual.abs();
84 match self {
85 Self::Squared => 0.5 * e * e,
86 Self::Huber(delta) => {
87 if e <= delta {
88 0.5 * e * e
89 } else {
90 delta * (e - 0.5 * delta)
91 }
92 }
93 Self::Cauchy(c) => {
94 let t = e / c;
95 // ln_1p rather than ln(1 + x): at a small residual
96 // `1 + t²` loses significant digits. At e = 10⁻⁶ and c = 1
97 // the direct form has relative error 9·10⁻⁵ — four digits
98 // out of sixteen.
99 0.5 * c * c * (t * t).ln_1p()
100 }
101 Self::Tukey(c) => {
102 let limit = c * c / 6.0;
103 if e <= c {
104 let u = (e / c) * (e / c);
105 // `1 − (1 − u)³` expanded into `u·(3 − 3u + u²)`. The
106 // direct form subtracts nearly equal numbers and loses
107 // as many digits as the naive Cauchy does.
108 limit * u * (3.0 - 3.0 * u + u * u)
109 } else {
110 limit
111 }
112 }
113 Self::GemanMcClure(c) => {
114 let e2 = e * e;
115 0.5 * c * c * e2 / (c * c + e2)
116 }
117 }
118 }
119
120 /// The scale parameter, where one exists.
121 pub fn scale(self) -> Option<f64> {
122 match self {
123 Self::Squared => None,
124 Self::Huber(c) | Self::Cauchy(c) | Self::Tukey(c) | Self::GemanMcClure(c) => Some(c),
125 }
126 }
127}
128
129#[cfg(test)]
130mod tests {
131 use super::*;
132
133 const KERNELS: [Kernel; 5] = [
134 Kernel::Squared,
135 Kernel::Huber(1.0),
136 Kernel::Cauchy(1.0),
137 Kernel::Tukey(1.0),
138 Kernel::GemanMcClure(1.0),
139 ];
140
141 /// Near zero every kernel agrees with the squared loss: `w → 1`,
142 /// `ρ → e²/2`.
143 #[test]
144 fn all_kernels_agree_near_zero() {
145 for kernel in KERNELS {
146 assert!((kernel.weight(0.0) - 1.0).abs() < 1e-12, "{kernel:?}");
147 let e = 1e-6;
148 assert!((kernel.weight(e) - 1.0).abs() < 1e-10, "{kernel:?}");
149 assert!(
150 (kernel.loss(e) - 0.5 * e * e).abs() < 1e-20,
151 "{kernel:?}: ρ = {}",
152 kernel.loss(e)
153 );
154 }
155 }
156
157 /// The weight must equal `ψ(e)/e`, with `ψ` the numerical derivative
158 /// of `ρ`.
159 ///
160 /// A weight that disagrees with its loss is a classic bug: IRLS still
161 /// converges, just not to the minimum of the `ρ` you declared.
162 #[test]
163 fn weight_is_the_derivative_of_loss_over_residual() {
164 const H: f64 = 1e-6;
165 for kernel in KERNELS {
166 for e in [0.1, 0.5, 0.9, 1.5, 3.0, 10.0] {
167 let psi = (kernel.loss(e + H) - kernel.loss(e - H)) / (2.0 * H);
168 let expected = psi / e;
169 let actual = kernel.weight(e);
170 assert!(
171 (actual - expected).abs() < 1e-6,
172 "{kernel:?} at e = {e}: weight {actual}, but ψ/e = {expected}"
173 );
174 }
175 }
176 }
177
178 /// Robust kernels bound an outlier's influence; the squared loss does
179 /// not.
180 #[test]
181 fn robust_kernels_bound_outlier_influence() {
182 let far = 1e4;
183 assert!(Kernel::Squared.loss(far) > 1e7);
184 assert!(Kernel::Huber(1.0).loss(far) < 1e5);
185 assert!(Kernel::Cauchy(1.0).loss(far) < 20.0);
186 assert_eq!(Kernel::Tukey(1.0).loss(far), 1.0 / 6.0);
187 assert!(Kernel::GemanMcClure(1.0).loss(far) < 0.51);
188
189 assert_eq!(Kernel::Tukey(1.0).weight(1.5), 0.0);
190 assert!(Kernel::Huber(1.0).weight(far) < 1e-3);
191 }
192
193 /// The weight is non-increasing: farther is never more important.
194 #[test]
195 fn weight_is_non_increasing() {
196 for kernel in KERNELS {
197 let mut previous = f64::INFINITY;
198 for step in 0..200 {
199 let weight = kernel.weight(step as f64 * 0.05);
200 assert!(weight <= previous + 1e-12, "{kernel:?} at step {step}");
201 previous = weight;
202 }
203 }
204 }
205}