spatialrust_camera/
model.rs

1use crate::BrownConrady;
2use spatialrust_math::{Vec2, Vec3};
3
4/// Camera model errors.
5#[derive(Clone, Debug, PartialEq, thiserror::Error)]
6pub enum CameraError {
7    /// A focal length was zero, negative, or non-finite.
8    #[error("camera focal lengths must be finite and positive")]
9    InvalidFocalLength,
10    /// The principal point was non-finite.
11    #[error("camera principal point must be finite")]
12    InvalidPrincipalPoint,
13    /// Projection was requested for a point outside the positive camera half-space.
14    #[error("point depth must be finite and positive, found {0}")]
15    InvalidDepth(f64),
16    /// Pixel coordinates were non-finite.
17    #[error("pixel coordinates must be finite")]
18    InvalidPixel,
19}
20
21/// Pinhole camera intrinsic parameters and image dimensions.
22#[derive(Clone, Copy, Debug, PartialEq)]
23pub struct CameraIntrinsics {
24    /// Horizontal focal length in pixels.
25    pub fx: f64,
26    /// Vertical focal length in pixels.
27    pub fy: f64,
28    /// Principal point x coordinate in pixels.
29    pub cx: f64,
30    /// Principal point y coordinate in pixels.
31    pub cy: f64,
32    /// Calibrated image width.
33    pub width: usize,
34    /// Calibrated image height.
35    pub height: usize,
36}
37
38impl CameraIntrinsics {
39    /// Creates and validates camera intrinsics.
40    pub fn try_new(
41        fx: f64,
42        fy: f64,
43        cx: f64,
44        cy: f64,
45        width: usize,
46        height: usize,
47    ) -> Result<Self, CameraError> {
48        if !fx.is_finite() || !fy.is_finite() || fx <= 0.0 || fy <= 0.0 {
49            return Err(CameraError::InvalidFocalLength);
50        }
51        if !cx.is_finite() || !cy.is_finite() {
52            return Err(CameraError::InvalidPrincipalPoint);
53        }
54        Ok(Self { fx, fy, cx, cy, width, height })
55    }
56}
57
58/// A pinhole camera with optional Brown–Conrady lens distortion.
59#[derive(Clone, Copy, Debug, PartialEq)]
60pub struct PinholeCamera {
61    /// Intrinsic calibration.
62    pub intrinsics: CameraIntrinsics,
63    /// Lens distortion coefficients.
64    pub distortion: BrownConrady,
65}
66
67impl PinholeCamera {
68    /// Creates a camera with no lens distortion.
69    #[must_use]
70    pub const fn new(intrinsics: CameraIntrinsics) -> Self {
71        Self {
72            intrinsics,
73            distortion: BrownConrady { k1: 0.0, k2: 0.0, p1: 0.0, p2: 0.0, k3: 0.0 },
74        }
75    }
76
77    /// Attaches a Brown–Conrady distortion model.
78    #[must_use]
79    pub const fn with_distortion(mut self, distortion: BrownConrady) -> Self {
80        self.distortion = distortion;
81        self
82    }
83
84    /// Projects a camera-space point into distorted pixel coordinates.
85    pub fn project(&self, point: Vec3<f64>) -> Result<Vec2<f64>, CameraError> {
86        if !point.z.is_finite() || point.z <= 0.0 {
87            return Err(CameraError::InvalidDepth(point.z));
88        }
89        let normalized = Vec2 { x: point.x / point.z, y: point.y / point.z };
90        let distorted = self.distortion.distort(normalized);
91        Ok(Vec2 {
92            x: self.intrinsics.fx.mul_add(distorted.x, self.intrinsics.cx),
93            y: self.intrinsics.fy.mul_add(distorted.y, self.intrinsics.cy),
94        })
95    }
96
97    /// Unprojects a distorted pixel and metric depth into camera space.
98    pub fn unproject(&self, pixel: Vec2<f64>, depth: f64) -> Result<Vec3<f64>, CameraError> {
99        if !depth.is_finite() || depth <= 0.0 {
100            return Err(CameraError::InvalidDepth(depth));
101        }
102        if !pixel.x.is_finite() || !pixel.y.is_finite() {
103            return Err(CameraError::InvalidPixel);
104        }
105        let distorted = Vec2 {
106            x: (pixel.x - self.intrinsics.cx) / self.intrinsics.fx,
107            y: (pixel.y - self.intrinsics.cy) / self.intrinsics.fy,
108        };
109        let normalized = self.distortion.undistort(distorted);
110        Ok(Vec3::new(normalized.x * depth, normalized.y * depth, depth))
111    }
112}
113
114#[cfg(test)]
115mod tests {
116    use super::{CameraIntrinsics, PinholeCamera};
117    use crate::BrownConrady;
118    use spatialrust_math::Vec3;
119
120    #[test]
121    fn project_unproject_roundtrip_with_distortion() {
122        let intrinsics = CameraIntrinsics::try_new(525.0, 520.0, 319.5, 239.5, 640, 480).unwrap();
123        let camera = PinholeCamera::new(intrinsics).with_distortion(BrownConrady {
124            k1: -0.15,
125            k2: 0.02,
126            p1: 0.001,
127            p2: -0.001,
128            k3: 0.0,
129        });
130        let point = Vec3::new(0.4, -0.2, 2.5);
131        let pixel = camera.project(point).unwrap();
132        let recovered = camera.unproject(pixel, point.z).unwrap();
133        assert!((recovered.x - point.x).abs() < 1e-8);
134        assert!((recovered.y - point.y).abs() < 1e-8);
135        assert_eq!(recovered.z, point.z);
136    }
137}
spatialrust_camera/model.rs

spatialrust_camera/
model.rs
