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