spatialrust_camera/
distortion.rs

1use spatialrust_math::Vec2;
2
3/// Brown–Conrady radial and tangential lens-distortion coefficients.
4#[derive(Clone, Copy, Debug, Default, PartialEq)]
5pub struct BrownConrady {
6    /// First radial coefficient.
7    pub k1: f64,
8    /// Second radial coefficient.
9    pub k2: f64,
10    /// First tangential coefficient.
11    pub p1: f64,
12    /// Second tangential coefficient.
13    pub p2: f64,
14    /// Third radial coefficient.
15    pub k3: f64,
16}
17
18/// Kannala–Brandt four-coefficient fisheye model.
19#[derive(Clone, Copy, Debug, Default, PartialEq)]
20pub struct KannalaBrandt4 {
21    /// Cubic angle coefficient.
22    pub k1: f64,
23    /// Fifth-order angle coefficient.
24    pub k2: f64,
25    /// Seventh-order angle coefficient.
26    pub k3: f64,
27    /// Ninth-order angle coefficient.
28    pub k4: f64,
29}
30
31impl KannalaBrandt4 {
32    /// Distorts normalized pinhole coordinates using the equidistant angle polynomial.
33    #[must_use]
34    pub fn distort(self, point: Vec2<f64>) -> Vec2<f64> {
35        let radius = point.x.hypot(point.y);
36        if radius <= f64::EPSILON {
37            return point;
38        }
39        let theta = radius.atan();
40        let theta2 = theta * theta;
41        let distorted_theta = theta
42            * (1.0
43                + theta2 * (self.k1 + theta2 * (self.k2 + theta2 * (self.k3 + theta2 * self.k4))));
44        let scale = distorted_theta / radius;
45        Vec2 { x: point.x * scale, y: point.y * scale }
46    }
47
48    /// Iteratively removes fisheye distortion from normalized coordinates.
49    #[must_use]
50    pub fn undistort(self, point: Vec2<f64>) -> Vec2<f64> {
51        let distorted_radius = point.x.hypot(point.y);
52        if distorted_radius <= f64::EPSILON {
53            return point;
54        }
55        let mut theta = distorted_radius.min(std::f64::consts::FRAC_PI_2 - 1e-6);
56        for _ in 0..16 {
57            let theta2 = theta * theta;
58            let theta4 = theta2 * theta2;
59            let theta6 = theta4 * theta2;
60            let theta8 = theta4 * theta4;
61            let value = theta
62                * (1.0 + self.k1 * theta2 + self.k2 * theta4 + self.k3 * theta6 + self.k4 * theta8)
63                - distorted_radius;
64            let derivative = 1.0
65                + 3.0 * self.k1 * theta2
66                + 5.0 * self.k2 * theta4
67                + 7.0 * self.k3 * theta6
68                + 9.0 * self.k4 * theta8;
69            if derivative.abs() < 1e-12 {
70                break;
71            }
72            let step = value / derivative;
73            theta -= step;
74            if step.abs() < 1e-13 {
75                break;
76            }
77        }
78        let radius = theta.tan();
79        let scale = radius / distorted_radius;
80        Vec2 { x: point.x * scale, y: point.y * scale }
81    }
82}
83
84impl BrownConrady {
85    /// Returns whether all coefficients are zero.
86    #[must_use]
87    pub fn is_identity(self) -> bool {
88        self == Self::default()
89    }
90
91    /// Distorts normalized pinhole coordinates.
92    #[must_use]
93    pub fn distort(self, point: Vec2<f64>) -> Vec2<f64> {
94        let x = point.x;
95        let y = point.y;
96        let r2 = x.mul_add(x, y * y);
97        let radial = 1.0 + r2 * (self.k1 + r2 * (self.k2 + r2 * self.k3));
98        Vec2 {
99            x: x * radial + 2.0 * self.p1 * x * y + self.p2 * (r2 + 2.0 * x * x),
100            y: y * radial + self.p1 * (r2 + 2.0 * y * y) + 2.0 * self.p2 * x * y,
101        }
102    }
103
104    /// Iteratively removes distortion from normalized coordinates.
105    ///
106    /// Newton iterations use a numerical 2x2 Jacobian and stop once normalized
107    /// reprojection error reaches machine precision.
108    #[must_use]
109    pub fn undistort(self, distorted: Vec2<f64>) -> Vec2<f64> {
110        if self.is_identity() {
111            return distorted;
112        }
113        let mut estimate = distorted;
114        const STEP: f64 = 1e-7;
115        for _ in 0..12 {
116            let observed = self.distort(estimate);
117            let error = Vec2 { x: observed.x - distorted.x, y: observed.y - distorted.y };
118            if error.x.abs().max(error.y.abs()) < 1e-14 {
119                break;
120            }
121            let dx = self.distort(Vec2 { x: estimate.x + STEP, y: estimate.y });
122            let dy = self.distort(Vec2 { x: estimate.x, y: estimate.y + STEP });
123            let j00 = (dx.x - observed.x) / STEP;
124            let j10 = (dx.y - observed.y) / STEP;
125            let j01 = (dy.x - observed.x) / STEP;
126            let j11 = (dy.y - observed.y) / STEP;
127            let determinant = j00.mul_add(j11, -(j01 * j10));
128            if determinant.abs() < f64::EPSILON {
129                break;
130            }
131            let delta_x = (j11 * error.x - j01 * error.y) / determinant;
132            let delta_y = (-j10 * error.x + j00 * error.y) / determinant;
133            estimate.x -= delta_x;
134            estimate.y -= delta_y;
135        }
136        estimate
137    }
138}
139
140#[cfg(test)]
141mod tests {
142    use super::{BrownConrady, KannalaBrandt4};
143    use spatialrust_math::Vec2;
144
145    #[test]
146    fn distortion_roundtrip() {
147        let model = BrownConrady { k1: -0.2, k2: 0.03, p1: 0.001, p2: -0.002, k3: 0.0 };
148        for point in [Vec2 { x: -0.4, y: 0.3 }, Vec2 { x: 0.2, y: -0.1 }] {
149            let recovered = model.undistort(model.distort(point));
150            assert!((recovered.x - point.x).abs() < 1e-9);
151            assert!((recovered.y - point.y).abs() < 1e-9);
152        }
153    }
154
155    #[test]
156    fn fisheye_roundtrip() {
157        let model = KannalaBrandt4 { k1: 0.02, k2: -0.003, k3: 0.0004, k4: -0.00002 };
158        for point in [Vec2 { x: -0.8, y: 0.5 }, Vec2 { x: 0.2, y: -0.1 }] {
159            let recovered = model.undistort(model.distort(point));
160            assert!((recovered.x - point.x).abs() < 1e-10);
161            assert!((recovered.y - point.y).abs() < 1e-10);
162        }
163    }
164}
spatialrust_camera/distortion.rs

spatialrust_camera/
distortion.rs
