use super::LensDistortionError;
use std::f32::consts::FRAC_PI_2;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub(super) enum FisheyeProjection {
Equidistant,
Equisolid,
Stereographic,
}
const FISHEYE_THETA_MARGIN: f32 = 1e-3;
const FISHEYE_MAX_THETA: f32 = FRAC_PI_2 - FISHEYE_THETA_MARGIN;
pub(super) fn fisheye_distort(
xu: f32,
yu: f32,
k1: f32,
k2: f32,
proj: FisheyeProjection,
) -> (f32, f32) {
let r = (xu * xu + yu * yu).sqrt();
if r < 1e-9 {
return (xu, yu);
}
let theta = r.atan();
let theta2 = theta * theta;
let theta4 = theta2 * theta2;
let theta_d = theta * (1.0 + k1 * theta2 + k2 * theta4);
let r_d = match proj {
FisheyeProjection::Equidistant => theta_d,
FisheyeProjection::Equisolid => 2.0 * (theta_d / 2.0).sin(),
FisheyeProjection::Stereographic => 2.0 * (theta_d / 2.0).tan(),
};
let scale = r_d / r;
(xu * scale, yu * scale)
}
fn fisheye_domain_max_radius(k1: f32, k2: f32, proj: FisheyeProjection) -> f32 {
let theta = FISHEYE_MAX_THETA;
let theta2 = theta * theta;
let theta_d = theta * (1.0 + k1 * theta2 + k2 * theta2 * theta2);
let r_d_max = match proj {
FisheyeProjection::Equidistant => theta_d,
FisheyeProjection::Equisolid => 2.0 * (theta_d / 2.0).sin(),
FisheyeProjection::Stereographic => 2.0 * (theta_d / 2.0).tan(),
};
if r_d_max.is_finite() && r_d_max > 0.0 {
r_d_max
} else {
f32::INFINITY
}
}
fn pinhole_from_theta(
theta: f32,
xd: f32,
yd: f32,
r_d: f32,
k1: f32,
k2: f32,
proj: FisheyeProjection,
) -> Result<(f32, f32), LensDistortionError> {
if !theta.is_finite() || theta.abs() >= FISHEYE_MAX_THETA {
return Err(LensDistortionError::OutsideDistortionDomain {
r_d,
r_d_max: fisheye_domain_max_radius(k1, k2, proj),
});
}
let r_u = theta.tan();
let scale = r_u / r_d;
Ok((xd * scale, yd * scale))
}
pub(super) fn undistort_fisheye(
xd: f32,
yd: f32,
k1: f32,
k2: f32,
proj: FisheyeProjection,
max_iters: usize,
tol: f32,
) -> Result<(f32, f32), LensDistortionError> {
let r_d = (xd * xd + yd * yd).sqrt();
if r_d < 1e-9 {
return Ok((xd, yd));
}
let theta_d = match proj {
FisheyeProjection::Equidistant => r_d,
FisheyeProjection::Equisolid => 2.0 * (r_d * 0.5).clamp(-1.0, 1.0).asin(),
FisheyeProjection::Stereographic => 2.0 * (r_d * 0.5).atan(),
};
let mut theta = theta_d;
for iter in 0..max_iters {
let t2 = theta * theta;
let t4 = t2 * t2;
let f = theta * (1.0 + k1 * t2 + k2 * t4) - theta_d;
let fp = 1.0 + 3.0 * k1 * t2 + 5.0 * k2 * t4;
if !fp.is_finite() || fp.abs() < 1e-8 {
return Err(LensDistortionError::ConvergenceError { iterations: iter });
}
let step = f / fp;
theta -= step;
if !theta.is_finite() {
return Err(LensDistortionError::ConvergenceError {
iterations: iter + 1,
});
}
if step.abs() < tol {
return pinhole_from_theta(theta, xd, yd, r_d, k1, k2, proj);
}
if iter + 1 == max_iters {
return Err(LensDistortionError::ConvergenceError {
iterations: max_iters,
});
}
}
let t2 = theta * theta;
let t4 = t2 * t2;
let f = theta * (1.0 + k1 * t2 + k2 * t4) - theta_d;
if f.abs() < tol {
return pinhole_from_theta(theta, xd, yd, r_d, k1, k2, proj);
}
Err(LensDistortionError::ConvergenceError { iterations: 0 })
}
#[cfg(test)]
mod tests {
use super::*;
fn undistort(
xd: f32,
yd: f32,
k1: f32,
k2: f32,
proj: FisheyeProjection,
) -> Result<(f32, f32), LensDistortionError> {
undistort_fisheye(xd, yd, k1, k2, proj, 40, 1e-6)
}
#[test]
fn test_undistort_fisheye_rejects_rays_past_the_90_degree_fold() {
assert!(
(2.0_f32).tan() < 0.0,
"the premise of this regression: tan is negative past π/2"
);
match undistort(2.0, 0.0, 0.0, 0.0, FisheyeProjection::Equidistant) {
Err(LensDistortionError::OutsideDistortionDomain { r_d, r_d_max }) => {
assert!((r_d - 2.0).abs() < 1e-6, "r_d: {r_d}");
assert!(
(r_d_max - FISHEYE_MAX_THETA).abs() < 1e-5,
"an equidistant model's boundary radius is the boundary \
angle itself, got {r_d_max}"
);
}
other => panic!("expected OutsideDistortionDomain, got {other:?}"),
}
for (proj, r_d) in [
(FisheyeProjection::Equisolid, 2.0_f32),
(FisheyeProjection::Stereographic, 10.0_f32),
] {
match undistort(r_d, 0.0, 0.0, 0.0, proj) {
Err(LensDistortionError::OutsideDistortionDomain { r_d_max, .. }) => {
assert!(
r_d > r_d_max,
"{proj:?}: r_d {r_d} should exceed the reported \
boundary {r_d_max}"
);
}
other => panic!("{proj:?}: expected OutsideDistortionDomain, got {other:?}"),
}
}
}
#[test]
fn test_undistort_fisheye_boundary_is_exactly_the_fold() {
let just_inside = FISHEYE_MAX_THETA - 1e-3;
let (xu, yu) = undistort(just_inside, 0.0, 0.0, 0.0, FisheyeProjection::Equidistant)
.expect("just below the fold still has a pre-image");
assert!(
xu.is_finite() && xu > 0.0,
"a radius just inside the domain must stay finite and unmirrored, got {xu}"
);
assert!(
(xu - just_inside.tan()).abs() < 1e-2 * xu.abs(),
"xu should be tan(theta) = {}, got {xu}",
just_inside.tan()
);
assert!(yu.abs() < 1e-6, "yu: {yu}");
for r_d in [FISHEYE_MAX_THETA, FISHEYE_MAX_THETA + 1e-3, FRAC_PI_2] {
assert!(
matches!(
undistort(r_d, 0.0, 0.0, 0.0, FisheyeProjection::Equidistant),
Err(LensDistortionError::OutsideDistortionDomain { .. })
),
"r_d = {r_d} is at or past the fold"
);
}
}
#[test]
fn test_undistort_fisheye_still_round_trips_wide_but_valid_rays() {
for proj in [
FisheyeProjection::Equidistant,
FisheyeProjection::Equisolid,
FisheyeProjection::Stereographic,
] {
let (xu0, yu0) = (3.0_f32, 0.5_f32);
let (xd, yd) = fisheye_distort(xu0, yu0, -0.05, 0.005, proj);
let (xu1, yu1) = undistort(xd, yd, -0.05, 0.005, proj)
.unwrap_or_else(|e| panic!("{proj:?}: a 71.6° ray is inside the domain: {e}"));
assert!((xu1 - xu0).abs() < 1e-3, "{proj:?}: xu {xu1} vs {xu0}");
assert!((yu1 - yu0).abs() < 1e-3, "{proj:?}: yu {yu1} vs {yu0}");
}
}
#[test]
fn test_fisheye_domain_max_radius_bounds_the_forward_map() {
for proj in [
FisheyeProjection::Equidistant,
FisheyeProjection::Equisolid,
FisheyeProjection::Stereographic,
] {
let r_d_max = fisheye_domain_max_radius(0.0, 0.0, proj);
assert!(r_d_max.is_finite() && r_d_max > 0.0, "{proj:?}: {r_d_max}");
let near_edge = (0.99 * FISHEYE_MAX_THETA).tan();
let (xd, _) = fisheye_distort(near_edge, 0.0, 0.0, 0.0, proj);
assert!(
xd < r_d_max,
"{proj:?}: forward radius {xd} at 0.99 of the boundary angle \
must stay under the reported maximum {r_d_max}"
);
}
}
}