use thiserror::Error;
#[derive(Debug, Error)]
pub enum LensDistortionError {
#[error("Invalid config: {0}")]
InvalidConfig(String),
#[error("Invalid image dimensions: width={w}, height={h}")]
InvalidDimensions { w: usize, h: usize },
#[error("Iteration did not converge after {iterations} steps")]
ConvergenceError { iterations: usize },
#[error(
"Distorted radius {r_d} exceeds the model's maximum representable \
distorted radius {r_d_max}: no undistorted point maps here"
)]
OutsideDistortionDomain {
r_d: f32,
r_d_max: f32,
},
#[error("Empty image")]
EmptyImage,
#[error("Image buffer length {actual} does not match {width}×{height}×3 = {expected}")]
BufferSizeMismatch {
actual: usize,
expected: usize,
width: usize,
height: usize,
},
}
#[derive(Debug, Clone, PartialEq)]
pub enum DistortionModel {
BrownConrady {
k1: f32,
k2: f32,
k3: f32,
p1: f32,
p2: f32,
},
FisheyeEquidistant {
k1: f32,
k2: f32,
},
FisheyeEquisolid {
k1: f32,
k2: f32,
},
FisheyeStereographic {
k1: f32,
k2: f32,
},
SimpleRadial {
k1: f32,
k2: f32,
},
Division {
lambda: f32,
},
}
impl DistortionModel {
pub fn validate(&self) -> Result<(), LensDistortionError> {
let ok = match self {
Self::BrownConrady { k1, k2, k3, p1, p2 } => {
k1.is_finite()
&& k2.is_finite()
&& k3.is_finite()
&& p1.is_finite()
&& p2.is_finite()
}
Self::FisheyeEquidistant { k1, k2 }
| Self::FisheyeEquisolid { k1, k2 }
| Self::FisheyeStereographic { k1, k2 }
| Self::SimpleRadial { k1, k2 } => k1.is_finite() && k2.is_finite(),
Self::Division { lambda } => lambda.is_finite(),
};
if ok {
Ok(())
} else {
Err(LensDistortionError::InvalidConfig(
"distortion coefficients contain NaN or Inf".into(),
))
}
}
pub fn name(&self) -> &'static str {
match self {
Self::BrownConrady { .. } => "BrownConrady",
Self::FisheyeEquidistant { .. } => "FisheyeEquidistant",
Self::FisheyeEquisolid { .. } => "FisheyeEquisolid",
Self::FisheyeStereographic { .. } => "FisheyeStereographic",
Self::SimpleRadial { .. } => "SimpleRadial",
Self::Division { .. } => "Division",
}
}
pub fn is_fisheye(&self) -> bool {
matches!(
self,
Self::FisheyeEquidistant { .. }
| Self::FisheyeEquisolid { .. }
| Self::FisheyeStereographic { .. }
)
}
}
#[derive(Debug, Clone)]
pub struct CameraIntrinsics {
pub fx: f32,
pub fy: f32,
pub cx: f32,
pub cy: f32,
pub width: usize,
pub height: usize,
}
impl CameraIntrinsics {
pub fn new(fx: f32, fy: f32, cx: f32, cy: f32, width: usize, height: usize) -> Self {
Self {
fx,
fy,
cx,
cy,
width,
height,
}
}
pub fn pinhole(focal: f32, width: usize, height: usize) -> Self {
Self {
fx: focal,
fy: focal,
cx: width as f32 / 2.0,
cy: height as f32 / 2.0,
width,
height,
}
}
pub fn validate(&self) -> Result<(), LensDistortionError> {
if self.fx <= 0.0 || !self.fx.is_finite() {
return Err(LensDistortionError::InvalidConfig(format!(
"focal length fx must be positive finite, got {}",
self.fx
)));
}
if self.fy <= 0.0 || !self.fy.is_finite() {
return Err(LensDistortionError::InvalidConfig(format!(
"focal length fy must be positive finite, got {}",
self.fy
)));
}
if self.width == 0 || self.height == 0 {
return Err(LensDistortionError::InvalidDimensions {
w: self.width,
h: self.height,
});
}
Ok(())
}
#[inline]
pub fn to_normalized(&self, px: f32, py: f32) -> (f32, f32) {
((px - self.cx) / self.fx, (py - self.cy) / self.fy)
}
#[inline]
pub fn to_pixel(&self, nx: f32, ny: f32) -> (f32, f32) {
(nx * self.fx + self.cx, ny * self.fy + self.cy)
}
}
#[inline]
fn bilinear_sample(pixels: &[u8], width: usize, height: usize, x: f32, y: f32) -> [u8; 3] {
let x = x.clamp(0.0, (width as f32) - 1.0);
let y = y.clamp(0.0, (height as f32) - 1.0);
let x0 = x.floor() as usize;
let y0 = y.floor() as usize;
let x1 = (x0 + 1).min(width - 1);
let y1 = (y0 + 1).min(height - 1);
let tx = x - x0 as f32;
let ty = y - y0 as f32;
let idx00 = (y0 * width + x0) * 3;
let idx10 = (y0 * width + x1) * 3;
let idx01 = (y1 * width + x0) * 3;
let idx11 = (y1 * width + x1) * 3;
let mut out = [0u8; 3];
for c in 0..3 {
let v00 = pixels[idx00 + c] as f32;
let v10 = pixels[idx10 + c] as f32;
let v01 = pixels[idx01 + c] as f32;
let v11 = pixels[idx11 + c] as f32;
let v = v00 * (1.0 - tx) * (1.0 - ty)
+ v10 * tx * (1.0 - ty)
+ v01 * (1.0 - tx) * ty
+ v11 * tx * ty;
out[c] = v.round().clamp(0.0, 255.0) as u8;
}
out
}
pub fn distort_point(xu: f32, yu: f32, model: &DistortionModel) -> (f32, f32) {
match model {
DistortionModel::BrownConrady { k1, k2, k3, p1, p2 } => {
let r2 = xu * xu + yu * yu;
let r4 = r2 * r2;
let r6 = r4 * r2;
let radial = 1.0 + k1 * r2 + k2 * r4 + k3 * r6;
let xd = xu * radial + 2.0 * p1 * xu * yu + p2 * (r2 + 2.0 * xu * xu);
let yd = yu * radial + p1 * (r2 + 2.0 * yu * yu) + 2.0 * p2 * xu * yu;
(xd, yd)
}
DistortionModel::SimpleRadial { k1, k2 } => {
let r2 = xu * xu + yu * yu;
let r4 = r2 * r2;
let radial = 1.0 + k1 * r2 + k2 * r4;
(xu * radial, yu * radial)
}
DistortionModel::FisheyeEquidistant { k1, k2 } => {
fisheye_distort(xu, yu, *k1, *k2, FisheyeProjection::Equidistant)
}
DistortionModel::FisheyeEquisolid { k1, k2 } => {
fisheye_distort(xu, yu, *k1, *k2, FisheyeProjection::Equisolid)
}
DistortionModel::FisheyeStereographic { k1, k2 } => {
fisheye_distort(xu, yu, *k1, *k2, FisheyeProjection::Stereographic)
}
DistortionModel::Division { lambda } => {
let r2 = xu * xu + yu * yu;
let denom = 1.0 + lambda * r2;
if denom.abs() < 1e-12 {
return (xu, yu);
}
(xu / denom, yu / denom)
}
}
}
mod fisheye;
use fisheye::{fisheye_distort, undistort_fisheye, FisheyeProjection};
mod radial_branch;
use radial_branch::RadialBranch;
pub fn undistort_point_iterative(
xd: f32,
yd: f32,
model: &DistortionModel,
max_iters: usize,
tol: f32,
) -> Result<(f32, f32), LensDistortionError> {
undistort_point_cached(
xd,
yd,
model,
model_radial_branch(model).as_ref(),
max_iters,
tol,
)
}
fn model_radial_branch(model: &DistortionModel) -> Option<RadialBranch> {
match model {
DistortionModel::SimpleRadial { k1, k2 } => Some(RadialBranch::new(*k1, *k2, 0.0)),
DistortionModel::BrownConrady { k1, k2, k3, .. } => Some(RadialBranch::new(*k1, *k2, *k3)),
_ => None,
}
}
fn undistort_point_cached(
xd: f32,
yd: f32,
model: &DistortionModel,
branch: Option<&RadialBranch>,
max_iters: usize,
tol: f32,
) -> Result<(f32, f32), LensDistortionError> {
match model {
DistortionModel::Division { lambda } => {
let r_d = (xd * xd + yd * yd).sqrt();
if r_d < 1e-12 || lambda.abs() < 1e-12 {
return Ok((xd, yd));
}
let discriminant = 1.0 - 4.0 * lambda * r_d * r_d;
if discriminant < 0.0 {
return Err(LensDistortionError::OutsideDistortionDomain {
r_d,
r_d_max: if *lambda > 0.0 {
0.5 / lambda.sqrt()
} else {
f32::INFINITY
},
});
}
let two_lambda_rd = 2.0 * lambda * r_d;
let r_u = (1.0 - discriminant.sqrt()) / two_lambda_rd;
let scale = r_u / r_d;
Ok((xd * scale, yd * scale))
}
DistortionModel::SimpleRadial { k1, k2 } => {
let owned;
let branch = match branch {
Some(b) => b,
None => {
owned = RadialBranch::new(*k1, *k2, 0.0);
&owned
}
};
undistort_radial(xd, yd, branch, 0.0, 0.0, max_iters, tol)
}
DistortionModel::BrownConrady { k1, k2, k3, p1, p2 } => {
let owned;
let branch = match branch {
Some(b) => b,
None => {
owned = RadialBranch::new(*k1, *k2, *k3);
&owned
}
};
undistort_radial(xd, yd, branch, *p1, *p2, max_iters, tol)
}
DistortionModel::FisheyeEquidistant { k1, k2 } => undistort_fisheye(
xd,
yd,
*k1,
*k2,
FisheyeProjection::Equidistant,
max_iters,
tol,
),
DistortionModel::FisheyeEquisolid { k1, k2 } => undistort_fisheye(
xd,
yd,
*k1,
*k2,
FisheyeProjection::Equisolid,
max_iters,
tol,
),
DistortionModel::FisheyeStereographic { k1, k2 } => undistort_fisheye(
xd,
yd,
*k1,
*k2,
FisheyeProjection::Stereographic,
max_iters,
tol,
),
}
}
fn undistort_radial(
xd: f32,
yd: f32,
branch: &RadialBranch,
p1: f32,
p2: f32,
max_iters: usize,
tol: f32,
) -> Result<(f32, f32), LensDistortionError> {
let r_d = (xd * xd + yd * yd).sqrt();
if r_d <= 0.0 {
return Ok((xd, yd));
}
let r_u = branch.invert(r_d)?;
let scale = r_u / r_d;
let mut xu = xd * scale;
let mut yu = yd * scale;
if p1 == 0.0 && p2 == 0.0 {
return Ok((xu, yu));
}
let tangential = |x: f32, y: f32| {
let r2 = x * x + y * y;
(
2.0 * p1 * x * y + p2 * (r2 + 2.0 * x * x),
p1 * (r2 + 2.0 * y * y) + 2.0 * p2 * x * y,
)
};
let radial_factor = |x: f32, y: f32| {
let r2 = x * x + y * y;
1.0 + branch.k1 * r2 + branch.k2 * r2 * r2 + branch.k3 * r2 * r2 * r2
};
for iter in 0..max_iters {
let radial = radial_factor(xu, yu);
if !radial.is_finite() || radial.abs() < 1e-8 {
return Err(LensDistortionError::ConvergenceError { iterations: iter });
}
let (dx_tan, dy_tan) = tangential(xu, yu);
let xu_next = (xd - dx_tan) / radial;
let yu_next = (yd - dy_tan) / radial;
let ex = xu_next - xu;
let ey = yu_next - yu;
xu = xu_next;
yu = yu_next;
if ex.abs().max(ey.abs()) < tol {
return Ok((xu, yu));
}
if iter + 1 == max_iters {
return Err(LensDistortionError::ConvergenceError {
iterations: max_iters,
});
}
}
let radial = radial_factor(xu, yu);
if radial.is_finite() && radial.abs() >= 1e-8 {
let (dx_tan, dy_tan) = tangential(xu, yu);
let xu_next = (xd - dx_tan) / radial;
let yu_next = (yd - dy_tan) / radial;
if (xu_next - xu).abs().max((yu_next - yu).abs()) < tol {
return Ok((xu_next, yu_next));
}
}
Err(LensDistortionError::ConvergenceError { iterations: 0 })
}
fn validate_image(pixels: &[u8], width: usize, height: usize) -> Result<(), LensDistortionError> {
if pixels.is_empty() {
return Err(LensDistortionError::EmptyImage);
}
if width == 0 || height == 0 {
return Err(LensDistortionError::InvalidDimensions {
w: width,
h: height,
});
}
let expected = width
.checked_mul(height)
.and_then(|n| n.checked_mul(3))
.ok_or(LensDistortionError::InvalidDimensions {
w: width,
h: height,
})?;
if pixels.len() != expected {
return Err(LensDistortionError::BufferSizeMismatch {
actual: pixels.len(),
expected,
width,
height,
});
}
Ok(())
}
const UNMAPPED_PIXEL: [u8; 3] = [0, 0, 0];
pub fn distort_image(
pixels: &[u8],
width: usize,
height: usize,
intrinsics: &CameraIntrinsics,
model: &DistortionModel,
) -> Result<Vec<u8>, LensDistortionError> {
validate_image(pixels, width, height)?;
intrinsics.validate()?;
model.validate()?;
let mut out = vec![0u8; width * height * 3];
let branch = model_radial_branch(model);
let mut unmapped = 0usize;
for row in 0..height {
for col in 0..width {
let (xd_n, yd_n) = intrinsics.to_normalized(col as f32, row as f32);
let idx = (row * width + col) * 3;
let sample = match undistort_point_cached(xd_n, yd_n, model, branch.as_ref(), 20, 1e-6)
{
Ok((xu_n, yu_n)) => {
let (src_x, src_y) = intrinsics.to_pixel(xu_n, yu_n);
bilinear_sample(pixels, width, height, src_x, src_y)
}
Err(LensDistortionError::OutsideDistortionDomain { .. }) => {
unmapped += 1;
UNMAPPED_PIXEL
}
Err(e) => return Err(e),
};
out[idx] = sample[0];
out[idx + 1] = sample[1];
out[idx + 2] = sample[2];
}
}
if unmapped > 0 {
tracing::debug!(
unmapped,
total = width * height,
"Destination pixels outside the distortion model's image were \
filled with the unmapped-pixel colour"
);
}
Ok(out)
}
pub fn undistort_image(
pixels: &[u8],
width: usize,
height: usize,
intrinsics: &CameraIntrinsics,
model: &DistortionModel,
) -> Result<Vec<u8>, LensDistortionError> {
validate_image(pixels, width, height)?;
intrinsics.validate()?;
model.validate()?;
let mut out = vec![0u8; width * height * 3];
for row in 0..height {
for col in 0..width {
let (xu_n, yu_n) = intrinsics.to_normalized(col as f32, row as f32);
let (xd_n, yd_n) = distort_point(xu_n, yu_n, model);
let (src_x, src_y) = intrinsics.to_pixel(xd_n, yd_n);
let sample = bilinear_sample(pixels, width, height, src_x, src_y);
let idx = (row * width + col) * 3;
out[idx] = sample[0];
out[idx + 1] = sample[1];
out[idx + 2] = sample[2];
}
}
Ok(out)
}
pub fn compute_distortion_map(
width: usize,
height: usize,
intrinsics: &CameraIntrinsics,
model: &DistortionModel,
) -> Result<(Vec<f32>, Vec<f32>), LensDistortionError> {
if width == 0 || height == 0 {
return Err(LensDistortionError::InvalidDimensions {
w: width,
h: height,
});
}
intrinsics.validate()?;
model.validate()?;
let n = width * height;
let mut dx_map = vec![0.0f32; n];
let mut dy_map = vec![0.0f32; n];
for row in 0..height {
for col in 0..width {
let (xu_n, yu_n) = intrinsics.to_normalized(col as f32, row as f32);
let (xd_n, yd_n) = distort_point(xu_n, yu_n, model);
let (src_x, src_y) = intrinsics.to_pixel(xd_n, yd_n);
let i = row * width + col;
dx_map[i] = src_x - col as f32;
dy_map[i] = src_y - row as f32;
}
}
Ok((dx_map, dy_map))
}
pub fn remap_image(
pixels: &[u8],
width: usize,
height: usize,
dx_map: &[f32],
dy_map: &[f32],
) -> Result<Vec<u8>, LensDistortionError> {
validate_image(pixels, width, height)?;
let n = width * height;
if dx_map.len() != n || dy_map.len() != n {
return Err(LensDistortionError::InvalidConfig(format!(
"map size mismatch: expected {n}, got dx={}, dy={}",
dx_map.len(),
dy_map.len(),
)));
}
let mut out = vec![0u8; n * 3];
for row in 0..height {
for col in 0..width {
let i = row * width + col;
let src_x = col as f32 + dx_map[i];
let src_y = row as f32 + dy_map[i];
let sample = bilinear_sample(pixels, width, height, src_x, src_y);
out[i * 3] = sample[0];
out[i * 3 + 1] = sample[1];
out[i * 3 + 2] = sample[2];
}
}
Ok(out)
}
pub fn barrel_distort_image(
pixels: &[u8],
width: usize,
height: usize,
k1: f32,
) -> Result<Vec<u8>, LensDistortionError> {
validate_image(pixels, width, height)?;
if !k1.is_finite() {
return Err(LensDistortionError::InvalidConfig(
"k1 must be finite".into(),
));
}
let intrinsics = CameraIntrinsics::pinhole(((width.min(height)) as f32) / 2.0, width, height);
let model = DistortionModel::SimpleRadial { k1, k2: 0.0 };
distort_image(pixels, width, height, &intrinsics, &model)
}
pub fn estimate_radial_coefficients(
measurements: &[(f32, f32)],
) -> Result<(f32, f32), LensDistortionError> {
if measurements.is_empty() {
return Err(LensDistortionError::InvalidConfig(
"no measurements provided".into(),
));
}
let mut a = 0.0f64;
let mut b = 0.0f64;
let mut c = 0.0f64;
let mut e = 0.0f64;
let mut f = 0.0f64;
let mut count = 0usize;
for &(ru, rd) in measurements {
if ru < 1e-6 {
continue;
}
let ru = ru as f64;
let rd = rd as f64;
let x1 = ru * ru;
let x2 = x1 * x1;
let y = rd / ru - 1.0;
a += x1 * x1;
b += x1 * x2;
c += x2 * x2;
e += x1 * y;
f += x2 * y;
count += 1;
}
if count == 0 {
return Err(LensDistortionError::InvalidConfig(
"all measurements had r_u < 1e-6".into(),
));
}
let det = a * c - b * b;
if det.abs() < 1e-30 {
if a.abs() < 1e-30 {
return Ok((0.0, 0.0));
}
let k1 = (e / a) as f32;
return Ok((k1, 0.0));
}
let k1 = ((e * c - f * b) / det) as f32;
let k2 = ((a * f - b * e) / det) as f32;
Ok((k1, k2))
}
#[derive(Debug, Clone)]
pub struct DistortionStats {
pub max_shift_px: f32,
pub mean_shift_px: f32,
pub rms_shift_px: f32,
pub barrel_pincushion: f32,
}
pub fn compute_distortion_stats(
width: usize,
height: usize,
intrinsics: &CameraIntrinsics,
model: &DistortionModel,
) -> Result<DistortionStats, LensDistortionError> {
if width == 0 || height == 0 {
return Err(LensDistortionError::InvalidDimensions {
w: width,
h: height,
});
}
intrinsics.validate()?;
model.validate()?;
let n = (width * height) as f64;
let mut max_shift = 0.0f32;
let mut sum_shift = 0.0f64;
let mut sum_sq = 0.0f64;
for row in 0..height {
for col in 0..width {
let (xu_n, yu_n) = intrinsics.to_normalized(col as f32, row as f32);
let (xd_n, yd_n) = distort_point(xu_n, yu_n, model);
let (dst_x, dst_y) = intrinsics.to_pixel(xd_n, yd_n);
let dx = dst_x - col as f32;
let dy = dst_y - row as f32;
let shift = (dx * dx + dy * dy).sqrt();
if shift > max_shift {
max_shift = shift;
}
let shift_f64 = shift as f64;
sum_shift += shift_f64;
sum_sq += shift_f64 * shift_f64;
}
}
let mean_shift = (sum_shift / n) as f32;
let rms_shift = (sum_sq / n).sqrt() as f32;
let corners = [
(0.0f32, 0.0f32),
((width - 1) as f32, 0.0f32),
(0.0f32, (height - 1) as f32),
((width - 1) as f32, (height - 1) as f32),
];
let cx = intrinsics.cx;
let cy = intrinsics.cy;
let mut corner_sum = 0.0f32;
for (px, py) in corners {
let (xu_n, yu_n) = intrinsics.to_normalized(px, py);
let (xd_n, yd_n) = distort_point(xu_n, yu_n, model);
let (dst_x, dst_y) = intrinsics.to_pixel(xd_n, yd_n);
let dir_x = px - cx;
let dir_y = py - cy;
let len = (dir_x * dir_x + dir_y * dir_y).sqrt().max(1e-9);
let shift_x = dst_x - px;
let shift_y = dst_y - py;
corner_sum += (shift_x * dir_x + shift_y * dir_y) / len;
}
let barrel_pincushion = corner_sum / 4.0;
Ok(DistortionStats {
max_shift_px: max_shift,
mean_shift_px: mean_shift,
rms_shift_px: rms_shift,
barrel_pincushion,
})
}
#[cfg(test)]
mod tests {
use super::*;
fn solid_image(w: usize, h: usize, r: u8, g: u8, b: u8) -> Vec<u8> {
let mut v = Vec::with_capacity(w * h * 3);
for _ in 0..w * h {
v.push(r);
v.push(g);
v.push(b);
}
v
}
#[test]
fn test_intrinsics_new() {
let k = CameraIntrinsics::new(500.0, 500.0, 320.0, 240.0, 640, 480);
assert_eq!(k.fx, 500.0);
assert_eq!(k.cx, 320.0);
assert_eq!(k.width, 640);
}
#[test]
fn test_intrinsics_pinhole() {
let k = CameraIntrinsics::pinhole(400.0, 800, 600);
assert_eq!(k.fx, 400.0);
assert_eq!(k.fy, 400.0);
assert_eq!(k.cx, 400.0);
assert_eq!(k.cy, 300.0);
}
#[test]
fn test_intrinsics_validate_ok() {
let k = CameraIntrinsics::pinhole(400.0, 640, 480);
assert!(k.validate().is_ok());
}
#[test]
fn test_intrinsics_validate_bad_focal() {
let k = CameraIntrinsics::new(-1.0, 400.0, 320.0, 240.0, 640, 480);
assert!(k.validate().is_err());
}
#[test]
fn test_intrinsics_validate_zero_dims() {
let k = CameraIntrinsics::new(400.0, 400.0, 0.0, 0.0, 0, 480);
assert!(k.validate().is_err());
}
#[test]
fn test_intrinsics_to_normalized_roundtrip() {
let k = CameraIntrinsics::new(500.0, 500.0, 320.0, 240.0, 640, 480);
let (nx, ny) = k.to_normalized(320.0, 240.0);
assert!((nx).abs() < 1e-6);
assert!((ny).abs() < 1e-6);
let (px, py) = k.to_pixel(nx, ny);
assert!((px - 320.0).abs() < 1e-4);
assert!((py - 240.0).abs() < 1e-4);
}
#[test]
fn test_intrinsics_to_pixel_roundtrip() {
let k = CameraIntrinsics::pinhole(600.0, 800, 600);
for (col, row) in [(100.0f32, 200.0f32), (400.0, 300.0), (750.0, 50.0)] {
let (nx, ny) = k.to_normalized(col, row);
let (px, py) = k.to_pixel(nx, ny);
assert!((px - col).abs() < 1e-4, "col roundtrip failed");
assert!((py - row).abs() < 1e-4, "row roundtrip failed");
}
}
#[test]
fn test_model_validate_ok() {
assert!(DistortionModel::SimpleRadial { k1: 0.1, k2: 0.01 }
.validate()
.is_ok());
assert!(DistortionModel::Division { lambda: -0.5 }
.validate()
.is_ok());
}
#[test]
fn test_model_validate_nan() {
assert!(DistortionModel::SimpleRadial {
k1: f32::NAN,
k2: 0.0
}
.validate()
.is_err());
}
#[test]
fn test_model_validate_inf() {
assert!(DistortionModel::Division {
lambda: f32::INFINITY
}
.validate()
.is_err());
}
#[test]
fn test_model_name() {
assert_eq!(
DistortionModel::BrownConrady {
k1: 0.0,
k2: 0.0,
k3: 0.0,
p1: 0.0,
p2: 0.0
}
.name(),
"BrownConrady"
);
assert_eq!(
DistortionModel::FisheyeEquidistant { k1: 0.0, k2: 0.0 }.name(),
"FisheyeEquidistant"
);
assert_eq!(DistortionModel::Division { lambda: 0.0 }.name(), "Division");
}
#[test]
fn test_model_is_fisheye() {
assert!(DistortionModel::FisheyeEquidistant { k1: 0.0, k2: 0.0 }.is_fisheye());
assert!(DistortionModel::FisheyeEquisolid { k1: 0.0, k2: 0.0 }.is_fisheye());
assert!(DistortionModel::FisheyeStereographic { k1: 0.0, k2: 0.0 }.is_fisheye());
assert!(!DistortionModel::SimpleRadial { k1: 0.0, k2: 0.0 }.is_fisheye());
assert!(!DistortionModel::Division { lambda: 0.0 }.is_fisheye());
}
#[test]
fn test_distort_point_identity_simple_radial() {
let model = DistortionModel::SimpleRadial { k1: 0.0, k2: 0.0 };
let (xd, yd) = distort_point(0.3, -0.2, &model);
assert!((xd - 0.3).abs() < 1e-6);
assert!((yd + 0.2).abs() < 1e-6);
}
#[test]
fn test_distort_point_barrel_k1_negative() {
let model = DistortionModel::SimpleRadial { k1: -0.1, k2: 0.0 };
let (xd, yd) = distort_point(0.5, 0.0, &model);
assert!(xd < 0.5, "expected inward shift, got xd={xd}");
assert!(yd.abs() < 1e-6);
}
#[test]
fn test_distort_point_brown_conrady_zero() {
let model = DistortionModel::BrownConrady {
k1: 0.0,
k2: 0.0,
k3: 0.0,
p1: 0.0,
p2: 0.0,
};
let (xd, yd) = distort_point(0.4, 0.2, &model);
assert!((xd - 0.4).abs() < 1e-6);
assert!((yd - 0.2).abs() < 1e-6);
}
#[test]
fn test_distort_point_fisheye_equidistant_identity() {
let model = DistortionModel::FisheyeEquidistant { k1: 0.0, k2: 0.0 };
let (xd, yd) = distort_point(0.0, 0.0, &model);
assert!(xd.abs() < 1e-6);
assert!(yd.abs() < 1e-6);
}
#[test]
fn test_undistort_roundtrip_simple_radial() {
let model = DistortionModel::SimpleRadial { k1: -0.1, k2: 0.02 };
let xu0 = 0.3f32;
let yu0 = -0.15f32;
let (xd, yd) = distort_point(xu0, yu0, &model);
let (xu1, yu1) =
undistort_point_iterative(xd, yd, &model, 20, 1e-6).expect("should converge");
assert!((xu1 - xu0).abs() < 1e-4, "xu roundtrip: {xu1} vs {xu0}");
assert!((yu1 - yu0).abs() < 1e-4, "yu roundtrip: {yu1} vs {yu0}");
}
#[test]
fn test_undistort_roundtrip_brown_conrady() {
let model = DistortionModel::BrownConrady {
k1: -0.15,
k2: 0.02,
k3: 0.001,
p1: 0.001,
p2: -0.001,
};
let (xu0, yu0) = (0.2f32, 0.1f32);
let (xd, yd) = distort_point(xu0, yu0, &model);
let (xu1, yu1) =
undistort_point_iterative(xd, yd, &model, 30, 1e-6).expect("should converge");
assert!((xu1 - xu0).abs() < 1e-4);
assert!((yu1 - yu0).abs() < 1e-4);
}
#[test]
fn test_undistort_roundtrip_division() {
let model = DistortionModel::Division { lambda: -0.3 };
let (xu0, yu0) = (0.25f32, -0.1f32);
let (xd, yd) = distort_point(xu0, yu0, &model);
let (xu1, yu1) =
undistort_point_iterative(xd, yd, &model, 20, 1e-6).expect("should converge");
assert!((xu1 - xu0).abs() < 1e-4, "xu: {xu1} vs {xu0}");
assert!((yu1 - yu0).abs() < 1e-4, "yu: {yu1} vs {yu0}");
}
#[test]
fn test_undistort_convergence_error() {
let model = DistortionModel::SimpleRadial { k1: -0.5, k2: 0.0 };
let result = undistort_point_iterative(0.3, 0.2, &model, 0, 1e-12);
let _ = result; }
#[test]
fn test_distort_image_1x1() {
let pixels = vec![128u8, 64, 32];
let k = CameraIntrinsics::pinhole(100.0, 1, 1);
let model = DistortionModel::SimpleRadial { k1: 0.0, k2: 0.0 };
let out = distort_image(&pixels, 1, 1, &k, &model).expect("1x1 distort");
assert_eq!(out, pixels);
}
#[test]
fn test_undistort_image_1x1() {
let pixels = vec![200u8, 100, 50];
let k = CameraIntrinsics::pinhole(100.0, 1, 1);
let model = DistortionModel::SimpleRadial { k1: 0.0, k2: 0.0 };
let out = undistort_image(&pixels, 1, 1, &k, &model).expect("1x1 undistort");
assert_eq!(out, pixels);
}
#[test]
fn test_distort_image_4x4_no_distortion() {
let w = 4usize;
let h = 4usize;
let pixels = solid_image(w, h, 100, 150, 200);
let k = CameraIntrinsics::pinhole(10.0, w, h);
let model = DistortionModel::SimpleRadial { k1: 0.0, k2: 0.0 };
let out = distort_image(&pixels, w, h, &k, &model).expect("4x4 distort");
assert_eq!(out.len(), w * h * 3);
for chunk in out.chunks(3) {
assert_eq!(chunk[0], 100);
assert_eq!(chunk[1], 150);
assert_eq!(chunk[2], 200);
}
}
#[test]
fn test_undistort_image_4x4() {
let w = 4usize;
let h = 4usize;
let pixels = solid_image(w, h, 80, 160, 240);
let k = CameraIntrinsics::pinhole(10.0, w, h);
let model = DistortionModel::SimpleRadial { k1: -0.05, k2: 0.0 };
let out = undistort_image(&pixels, w, h, &k, &model).expect("4x4 undistort");
assert_eq!(out.len(), w * h * 3);
for chunk in out.chunks(3) {
assert_eq!(chunk[0], 80);
assert_eq!(chunk[1], 160);
assert_eq!(chunk[2], 240);
}
}
#[test]
fn test_distort_image_empty_error() {
let pixels: Vec<u8> = Vec::new();
let k = CameraIntrinsics::pinhole(100.0, 4, 4);
let model = DistortionModel::SimpleRadial { k1: 0.0, k2: 0.0 };
assert!(distort_image(&pixels, 4, 4, &k, &model).is_err());
}
#[test]
fn test_undistort_image_zero_dims_error() {
let pixels = solid_image(1, 1, 0, 0, 0);
let k = CameraIntrinsics::pinhole(100.0, 0, 4);
let model = DistortionModel::SimpleRadial { k1: 0.0, k2: 0.0 };
assert!(undistort_image(&pixels, 0, 4, &k, &model).is_err());
}
#[test]
fn test_barrel_distort_valid() {
let w = 8usize;
let h = 8usize;
let pixels = solid_image(w, h, 128, 128, 128);
let out = barrel_distort_image(&pixels, w, h, -0.1).expect("barrel distort");
assert_eq!(out.len(), w * h * 3);
}
#[test]
fn test_barrel_distort_zero_size_error() {
let pixels: Vec<u8> = Vec::new();
assert!(barrel_distort_image(&pixels, 0, 0, -0.1).is_err());
}
#[test]
fn test_barrel_distort_nan_k1_error() {
let pixels = solid_image(4, 4, 0, 0, 0);
assert!(barrel_distort_image(&pixels, 4, 4, f32::NAN).is_err());
}
#[test]
fn test_compute_distortion_map_correct_size() {
let w = 6usize;
let h = 4usize;
let k = CameraIntrinsics::pinhole(50.0, w, h);
let model = DistortionModel::SimpleRadial { k1: -0.1, k2: 0.0 };
let (dx, dy) = compute_distortion_map(w, h, &k, &model).expect("distortion map");
assert_eq!(dx.len(), w * h);
assert_eq!(dy.len(), w * h);
}
#[test]
fn test_compute_distortion_map_zero_dims_error() {
let k = CameraIntrinsics::pinhole(50.0, 4, 4);
let model = DistortionModel::SimpleRadial { k1: 0.0, k2: 0.0 };
assert!(compute_distortion_map(0, 4, &k, &model).is_err());
}
#[test]
fn test_compute_distortion_map_identity_zero_offset() {
let w = 4usize;
let h = 4usize;
let k = CameraIntrinsics::pinhole(100.0, w, h);
let model = DistortionModel::SimpleRadial { k1: 0.0, k2: 0.0 };
let (dx, dy) = compute_distortion_map(w, h, &k, &model).expect("identity map");
for v in dx.iter().chain(dy.iter()) {
assert!(v.abs() < 1e-4, "non-zero offset {v} in identity map");
}
}
#[test]
fn test_remap_image_identity_map() {
let w = 4usize;
let h = 4usize;
let pixels = solid_image(w, h, 77, 88, 99);
let dx = vec![0.0f32; w * h];
let dy = vec![0.0f32; w * h];
let out = remap_image(&pixels, w, h, &dx, &dy).expect("identity remap");
assert_eq!(out, pixels);
}
#[test]
fn test_remap_image_map_size_mismatch_error() {
let w = 4usize;
let h = 4usize;
let pixels = solid_image(w, h, 10, 20, 30);
let dx = vec![0.0f32; w * h - 1];
let dy = vec![0.0f32; w * h];
assert!(remap_image(&pixels, w, h, &dx, &dy).is_err());
}
#[test]
fn test_remap_image_empty_error() {
let pixels: Vec<u8> = Vec::new();
let dx = Vec::new();
let dy = Vec::new();
assert!(remap_image(&pixels, 0, 0, &dx, &dy).is_err());
}
#[test]
fn test_estimate_radial_empty_error() {
assert!(estimate_radial_coefficients(&[]).is_err());
}
#[test]
fn test_estimate_radial_linear_case() {
let k1_true = -0.1f32;
let k2_true = 0.02f32;
let measurements: Vec<(f32, f32)> = (1..=10)
.map(|i| {
let ru = i as f32 * 0.05;
let rd = ru * (1.0 + k1_true * ru * ru + k2_true * ru * ru * ru * ru);
(ru, rd)
})
.collect();
let (k1_est, k2_est) = estimate_radial_coefficients(&measurements).expect("estimate");
assert!((k1_est - k1_true).abs() < 0.02, "k1 off: {k1_est}");
assert!((k2_est - k2_true).abs() < 0.01, "k2 off: {k2_est}");
}
#[test]
fn test_estimate_radial_skip_near_zero() {
let measurements = vec![(0.0f32, 0.0f32), (0.1f32, 0.09f32)];
let result = estimate_radial_coefficients(&measurements);
assert!(result.is_ok());
}
#[test]
fn test_compute_distortion_stats_non_empty() {
let w = 8usize;
let h = 8usize;
let k = CameraIntrinsics::pinhole(50.0, w, h);
let model = DistortionModel::SimpleRadial { k1: -0.1, k2: 0.0 };
let stats = compute_distortion_stats(w, h, &k, &model).expect("stats");
assert!(stats.max_shift_px >= 0.0);
assert!(stats.mean_shift_px >= 0.0);
assert!(stats.rms_shift_px >= 0.0);
}
#[test]
fn test_compute_distortion_stats_barrel_sign() {
let w = 8usize;
let h = 8usize;
let k = CameraIntrinsics::pinhole(50.0, w, h);
let model = DistortionModel::SimpleRadial { k1: -0.2, k2: 0.0 };
let stats = compute_distortion_stats(w, h, &k, &model).expect("barrel stats");
assert!(
stats.barrel_pincushion <= 0.0,
"expected barrel (<=0), got {}",
stats.barrel_pincushion
);
}
#[test]
fn test_compute_distortion_stats_zero_dims_error() {
let k = CameraIntrinsics::pinhole(50.0, 4, 4);
let model = DistortionModel::SimpleRadial { k1: 0.0, k2: 0.0 };
assert!(compute_distortion_stats(0, 4, &k, &model).is_err());
}
#[test]
fn test_compute_distortion_stats_identity_zero_shift() {
let w = 6usize;
let h = 6usize;
let k = CameraIntrinsics::pinhole(100.0, w, h);
let model = DistortionModel::SimpleRadial { k1: 0.0, k2: 0.0 };
let stats = compute_distortion_stats(w, h, &k, &model).expect("identity stats");
assert!(stats.max_shift_px < 1e-4);
assert!(stats.mean_shift_px < 1e-4);
assert!(stats.rms_shift_px < 1e-4);
}
#[test]
fn test_distort_undistort_fisheye_roundtrip() {
let model = DistortionModel::FisheyeEquidistant {
k1: -0.05,
k2: 0.005,
};
let (xu0, yu0) = (0.1f32, 0.08f32);
let (xd, yd) = distort_point(xu0, yu0, &model);
let (xu1, yu1) =
undistort_point_iterative(xd, yd, &model, 40, 1e-6).expect("fisheye convergence");
assert!((xu1 - xu0).abs() < 1e-4, "fisheye xu: {xu1} vs {xu0}");
assert!((yu1 - yu0).abs() < 1e-4, "fisheye yu: {yu1} vs {yu0}");
}
#[test]
fn test_distort_point_origin_unchanged() {
let models = [
DistortionModel::SimpleRadial { k1: -0.3, k2: 0.1 },
DistortionModel::Division { lambda: -0.5 },
DistortionModel::BrownConrady {
k1: -0.1,
k2: 0.05,
k3: 0.0,
p1: 0.01,
p2: -0.01,
},
DistortionModel::FisheyeEquidistant { k1: -0.1, k2: 0.01 },
];
for model in &models {
let (xd, yd) = distort_point(0.0, 0.0, model);
assert!(
xd.abs() < 1e-6 && yd.abs() < 1e-6,
"{}: origin not preserved: ({xd},{yd})",
model.name()
);
}
}
#[test]
fn test_remap_with_computed_map() {
let w = 6usize;
let h = 6usize;
let pixels = solid_image(w, h, 50, 100, 200);
let k = CameraIntrinsics::pinhole(30.0, w, h);
let model = DistortionModel::SimpleRadial { k1: -0.05, k2: 0.0 };
let expected = undistort_image(&pixels, w, h, &k, &model).expect("undistort");
let (dx, dy) = compute_distortion_map(w, h, &k, &model).expect("map");
let got = remap_image(&pixels, w, h, &dx, &dy).expect("remap");
assert_eq!(expected, got);
}
#[test]
fn test_distort_image_buffer_too_short_error() {
let pixels = vec![0u8; 3]; let k = CameraIntrinsics::pinhole(100.0, 1000, 1000);
let model = DistortionModel::SimpleRadial { k1: 0.0, k2: 0.0 };
let result = distort_image(&pixels, 1000, 1000, &k, &model);
assert!(
matches!(result, Err(LensDistortionError::BufferSizeMismatch { .. })),
"expected BufferSizeMismatch, got {result:?}"
);
}
#[test]
fn test_undistort_image_buffer_too_short_error() {
let pixels = vec![0u8; 3];
let k = CameraIntrinsics::pinhole(100.0, 1000, 1000);
let model = DistortionModel::SimpleRadial { k1: 0.0, k2: 0.0 };
let result = undistort_image(&pixels, 1000, 1000, &k, &model);
assert!(matches!(
result,
Err(LensDistortionError::BufferSizeMismatch { .. })
));
}
#[test]
fn test_remap_image_buffer_too_short_error() {
let pixels = vec![0u8; 3];
let w = 1000;
let h = 1000;
let dx = vec![0.0f32; w * h];
let dy = vec![0.0f32; w * h];
let result = remap_image(&pixels, w, h, &dx, &dy);
assert!(matches!(
result,
Err(LensDistortionError::BufferSizeMismatch { .. })
));
}
#[test]
fn test_barrel_distort_image_buffer_too_short_error() {
let pixels = vec![0u8; 3];
let result = barrel_distort_image(&pixels, 1000, 1000, -0.1);
assert!(matches!(
result,
Err(LensDistortionError::BufferSizeMismatch { .. })
));
}
#[test]
fn test_undistort_converges_across_wide_k1_range() {
let mut k1 = -0.5_f32;
while k1 <= 0.5 {
if k1.abs() > 1e-6 {
let model = DistortionModel::SimpleRadial { k1, k2: 0.0 };
for &(xu0, yu0) in &[(0.3_f32, 0.2_f32), (0.5, -0.4), (0.1, 0.1)] {
let (xd, yd) = distort_point(xu0, yu0, &model);
let result = undistort_point_iterative(xd, yd, &model, 50, 1e-6);
let (xu1, yu1) = result.unwrap_or_else(|e| {
panic!("k1={k1} did not converge from ({xu0},{yu0}): {e}")
});
assert!(
(xu1 - xu0).abs() < 1e-3 && (yu1 - yu0).abs() < 1e-3,
"k1={k1}: roundtrip ({xu1},{yu1}) vs ({xu0},{yu0})"
);
}
}
k1 += 0.05;
}
}
#[test]
fn test_undistort_point_iterative_reports_points_with_no_preimage() {
let model = DistortionModel::SimpleRadial { k1: -4.0, k2: 0.0 };
let result = undistort_point_iterative(0.5, 0.0, &model, 10, 1e-6);
match result {
Err(LensDistortionError::OutsideDistortionDomain { r_d, r_d_max }) => {
assert!((r_d - 0.5).abs() < 1e-6, "r_d: {r_d}");
assert!(
(r_d_max - 0.192_450_09).abs() < 1e-5,
"r_d_max should be the analytic peak 0.19245009, got {r_d_max}"
);
}
other => panic!("expected OutsideDistortionDomain, got {other:?}"),
}
let r_u_probe = 0.2_f32;
let (xd, yd) = distort_point(r_u_probe, 0.0, &model);
let (xu, yu) =
undistort_point_iterative(xd, yd, &model, 10, 1e-6).expect("inside the domain");
assert!((xu - r_u_probe).abs() < 1e-4, "xu: {xu}");
assert!(yu.abs() < 1e-6, "yu: {yu}");
}
#[test]
fn test_division_model_reports_points_with_no_preimage() {
let model = DistortionModel::Division { lambda: 1.0 };
match undistort_point_iterative(0.75, 0.0, &model, 20, 1e-6) {
Err(LensDistortionError::OutsideDistortionDomain { r_d, r_d_max }) => {
assert!((r_d - 0.75).abs() < 1e-6, "r_d: {r_d}");
assert!((r_d_max - 0.5).abs() < 1e-6, "r_d_max: {r_d_max}");
}
other => panic!("expected OutsideDistortionDomain, got {other:?}"),
}
let (xd, yd) = distort_point(0.4, 0.0, &model);
let (xu, _) = undistort_point_iterative(xd, yd, &model, 20, 1e-6).expect("inside");
assert!((xu - 0.4).abs() < 1e-4, "xu: {xu}");
let pixels = vec![10u8, 20u8, 30u8];
let k = CameraIntrinsics::new(1.0, 1.0, -0.75, 0.0, 1, 1);
let out = distort_image(&pixels, 1, 1, &k, &model).expect("unmapped pixels are geometry");
assert_eq!(out, UNMAPPED_PIXEL.to_vec());
}
#[test]
fn test_distort_image_marks_unmapped_pixels_instead_of_inventing_content() {
let pixels = vec![10u8, 20u8, 30u8];
let k = CameraIntrinsics::new(1.0, 1.0, -0.5, 0.0, 1, 1);
let model = DistortionModel::SimpleRadial { k1: -4.0, k2: 0.0 };
let out = distort_image(&pixels, 1, 1, &k, &model)
.expect("an unmapped pixel is geometry, not an error");
assert_eq!(
out,
UNMAPPED_PIXEL.to_vec(),
"a destination pixel with no pre-image must be marked unmapped, \
not filled with the source colour"
);
}
#[test]
fn test_barrel_distort_fills_folded_corners_and_keeps_centre() {
let w = 8usize;
let h = 8usize;
let pixels = solid_image(w, h, 128, 128, 128);
let out = barrel_distort_image(&pixels, w, h, -0.1).expect("barrel distort");
assert_eq!(out.len(), w * h * 3);
let px = |col: usize, row: usize| {
let i = (row * w + col) * 3;
[out[i], out[i + 1], out[i + 2]]
};
for &(col, row) in &[(0, 0), (0, 1), (1, 0), (0, 7), (7, 0)] {
assert_eq!(
px(col, row),
UNMAPPED_PIXEL,
"pixel (col {col}, row {row}) is past the fold and has no pre-image"
);
}
assert_eq!(px(4, 4), [128, 128, 128], "the centre must be sampled");
assert_eq!(
px(w - 1, h - 1),
[128, 128, 128],
"(0.75, 0.75) has radius 1.0607, inside the domain"
);
let unmapped = out
.chunks(3)
.filter(|chunk| {
assert!(
*chunk == [128, 128, 128] || *chunk == UNMAPPED_PIXEL,
"unexpected pixel {chunk:?}"
);
*chunk == UNMAPPED_PIXEL
})
.count();
assert_eq!(unmapped, 5, "expected exactly 5 unmapped pixels");
let mild = barrel_distort_image(&pixels, w, h, -0.02).expect("mild barrel distort");
assert!(
mild.chunks(3).all(|c| c == [128, 128, 128]),
"k1 = -0.02 folds at r_d = 2.72, well outside the frame, so no \
pixel should be unmapped"
);
}
#[test]
fn test_compute_distortion_stats_f64_accumulation_precision() {
let w = 1000usize;
let h = 1000usize;
let k = CameraIntrinsics::pinhole(50.0, w, h);
let model = DistortionModel::SimpleRadial { k1: -0.4, k2: 0.0 };
let mut ref_sum = 0.0f64;
for row in 0..h {
for col in 0..w {
let (xu_n, yu_n) = k.to_normalized(col as f32, row as f32);
let (xd_n, yd_n) = distort_point(xu_n, yu_n, &model);
let (dst_x, dst_y) = k.to_pixel(xd_n, yd_n);
let dx = dst_x - col as f32;
let dy = dst_y - row as f32;
ref_sum += ((dx * dx + dy * dy).sqrt()) as f64;
}
}
assert!(
ref_sum > 1.677e7,
"test setup should exceed f32's exact-integer accumulation limit, got {ref_sum:e}"
);
let ref_mean = (ref_sum / (w * h) as f64) as f32;
let stats = compute_distortion_stats(w, h, &k, &model).expect("stats");
assert!(
(stats.mean_shift_px - ref_mean).abs() < 1.0,
"mean_shift_px={} should match the f64 reference {ref_mean}",
stats.mean_shift_px
);
}
}