eventcv_core/
camera.rs

1//! Pinhole camera intrinsics with Brown–Conrady (radial + tangential) distortion — the model
2//! event-camera calibrations ship (e.g. EV-IMO `calib.txt` = `fx fy cx cy k1 k2 p1 p2`). Used
3//! by [`EventStream::undistort`](crate::EventStream::undistort) to rectify event coordinates.
4
5/// Number of fixed-point iterations for the distortion inverse. The grid is small and the LUT
6/// is built once, so we can afford to iterate well past convergence.
7const UNDISTORT_ITERS: usize = 20;
8
9/// Pinhole intrinsics `(fx, fy, cx, cy)` plus distortion `(k1, k2, p1, p2, k3)`.
10#[derive(Clone, Copy, Debug, PartialEq)]
11pub struct Camera {
12    pub fx: f64,
13    pub fy: f64,
14    pub cx: f64,
15    pub cy: f64,
16    pub k1: f64,
17    pub k2: f64,
18    pub p1: f64,
19    pub p2: f64,
20    pub k3: f64,
21}
22
23impl Camera {
24    /// A distortion-free pinhole camera (all distortion coefficients zero).
25    pub fn new(fx: f64, fy: f64, cx: f64, cy: f64) -> Self {
26        Self {
27            fx,
28            fy,
29            cx,
30            cy,
31            k1: 0.0,
32            k2: 0.0,
33            p1: 0.0,
34            p2: 0.0,
35            k3: 0.0,
36        }
37    }
38
39    /// A camera with the full radial (`k1, k2, k3`) and tangential (`p1, p2`) distortion model.
40    #[allow(clippy::too_many_arguments)]
41    pub fn with_distortion(
42        fx: f64,
43        fy: f64,
44        cx: f64,
45        cy: f64,
46        k1: f64,
47        k2: f64,
48        p1: f64,
49        p2: f64,
50        k3: f64,
51    ) -> Self {
52        Self {
53            fx,
54            fy,
55            cx,
56            cy,
57            k1,
58            k2,
59            p1,
60            p2,
61            k3,
62        }
63    }
64
65    /// Maps a **distorted** pixel `(u, v)` to its **undistorted** pixel location in the same
66    /// camera (same intrinsics), inverting the distortion model iteratively (OpenCV's
67    /// `undistortPoints` scheme).
68    pub fn undistort_point(&self, u: f64, v: f64) -> (f64, f64) {
69        let (x0, y0) = ((u - self.cx) / self.fx, (v - self.cy) / self.fy);
70        let (mut x, mut y) = (x0, y0);
71        for _ in 0..UNDISTORT_ITERS {
72            let r2 = x * x + y * y;
73            let radial = 1.0 + ((self.k3 * r2 + self.k2) * r2 + self.k1) * r2;
74            let dx = 2.0 * self.p1 * x * y + self.p2 * (r2 + 2.0 * x * x);
75            let dy = self.p1 * (r2 + 2.0 * y * y) + 2.0 * self.p2 * x * y;
76            x = (x0 - dx) / radial;
77            y = (y0 - dy) / radial;
78        }
79        (x * self.fx + self.cx, y * self.fy + self.cy)
80    }
81
82    /// Forward model: maps **ideal normalized** coordinates to a **distorted** pixel. The
83    /// inverse of [`Self::undistort_point`]'s normalized stage; used to validate the inverse.
84    pub fn distort_point(&self, x: f64, y: f64) -> (f64, f64) {
85        let r2 = x * x + y * y;
86        let radial = 1.0 + ((self.k3 * r2 + self.k2) * r2 + self.k1) * r2;
87        let dx = 2.0 * self.p1 * x * y + self.p2 * (r2 + 2.0 * x * x);
88        let dy = self.p1 * (r2 + 2.0 * y * y) + 2.0 * self.p2 * x * y;
89        (
90            (x * radial + dx) * self.fx + self.cx,
91            (y * radial + dy) * self.fy + self.cy,
92        )
93    }
94}
95
96#[cfg(test)]
97mod tests {
98    use super::Camera;
99
100    #[test]
101    fn undistort_without_distortion_is_identity() {
102        let camera = Camera::new(256.9, 256.9, 131.6, 188.6);
103        for &(u, v) in &[(0.0, 0.0), (100.0, 50.0), (345.0, 259.0)] {
104            let (nu, nv) = camera.undistort_point(u, v);
105            assert!((nu - u).abs() < 1e-9 && (nv - v).abs() < 1e-9);
106        }
107    }
108
109    #[test]
110    fn undistort_inverts_the_forward_distortion() {
111        // EV-IMO `box/seq_10` calib values.
112        let camera = Camera::with_distortion(
113            256.919, 256.862, 131.651, 188.576, -0.366475, 0.130235, 0.000262, 0.000191, 0.0,
114        );
115        // For several ideal normalized points: distort to a pixel, then undistort back.
116        for &(xn, yn) in &[(0.0, 0.0), (0.2, -0.3), (-0.4, 0.1), (0.5, 0.5)] {
117            let (u, v) = camera.distort_point(xn, yn);
118            let (xu, yu) = camera.undistort_point(u, v);
119            let (ideal_u, ideal_v) = (xn * camera.fx + camera.cx, yn * camera.fy + camera.cy);
120            assert!(
121                (xu - ideal_u).abs() < 1e-3 && (yu - ideal_v).abs() < 1e-3,
122                "round-trip drift at ({xn}, {yn}): got ({xu}, {yu}) vs ({ideal_u}, {ideal_v})"
123            );
124        }
125    }
126}
eventcv_core/camera.rs

eventcv_core/
camera.rs
