apex_camera_models/
pinhole.rs

1//! Pinhole camera model.
2//!
3//! The simplest perspective camera: a 3D point is divided by its depth, then
4//! scaled and shifted to pixel coordinates. There is no lens distortion.
5//! Suitable for narrow FOV lenses and as a baseline for calibration.
6//! See the [cookbook](../doc/cookbook/src/pinhole.html) for the formulation.
7
8use crate::{CameraModel, CameraModelError, DistortionModel, PinholeParams};
9use nalgebra::{DVector, SMatrix, Vector2, Vector3};
10
11/// Pinhole camera model with 4 intrinsic parameters.
12#[derive(Debug, Clone, Copy, PartialEq)]
13pub struct PinholeCamera {
14    pub pinhole: PinholeParams,
15    pub distortion: DistortionModel,
16}
17
18impl PinholeCamera {
19    /// Creates a new Pinhole camera model.
20    ///
21    /// # Arguments
22    ///
23    /// * `pinhole` - Pinhole camera parameters (fx, fy, cx, cy).
24    /// * `distortion` - Distortion model (must be [`DistortionModel::None`]).
25    ///
26    /// # Returns
27    ///
28    /// Returns a new `PinholeCamera` instance if the parameters are valid.
29    ///
30    /// # Errors
31    ///
32    /// Returns [`CameraModelError`] if:
33    /// - The distortion model is not `None`.
34    /// - Parameters are invalid (e.g., negative focal length, infinite principal point).
35    ///
36    /// # Example
37    ///
38    /// ```
39    /// use apex_camera_models::{PinholeCamera, PinholeParams, DistortionModel};
40    ///
41    /// let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
42    /// let distortion = DistortionModel::None;
43    /// let camera = PinholeCamera::new(pinhole, distortion)?;
44    /// # Ok::<(), apex_camera_models::CameraModelError>(())
45    /// ```
46    pub fn new(
47        pinhole: PinholeParams,
48        distortion: DistortionModel,
49    ) -> Result<Self, CameraModelError> {
50        let camera = Self {
51            pinhole,
52            distortion,
53        };
54        camera.validate_params()?;
55        Ok(camera)
56    }
57
58    /// True if `z` is far enough from the optical centre for a numerically
59    /// stable projection.
60    fn check_projection_condition(&self, z: f64) -> bool {
61        z >= crate::GEOMETRIC_PRECISION
62    }
63}
64
65/// Parameter order: `[fx, fy, cx, cy]`.
66impl From<&PinholeCamera> for DVector<f64> {
67    fn from(camera: &PinholeCamera) -> Self {
68        DVector::from_vec(vec![
69            camera.pinhole.fx,
70            camera.pinhole.fy,
71            camera.pinhole.cx,
72            camera.pinhole.cy,
73        ])
74    }
75}
76
77/// Parameter order: `[fx, fy, cx, cy]`.
78impl From<&PinholeCamera> for [f64; 4] {
79    fn from(camera: &PinholeCamera) -> Self {
80        [
81            camera.pinhole.fx,
82            camera.pinhole.fy,
83            camera.pinhole.cx,
84            camera.pinhole.cy,
85        ]
86    }
87}
88
89/// Parameter order: `[fx, fy, cx, cy]`. Returns an error if the slice has
90/// fewer than 4 elements.
91impl TryFrom<&[f64]> for PinholeCamera {
92    type Error = CameraModelError;
93
94    fn try_from(params: &[f64]) -> Result<Self, Self::Error> {
95        if params.len() < 4 {
96            return Err(CameraModelError::InvalidParams(format!(
97                "PinholeCamera requires at least 4 parameters, got {}",
98                params.len()
99            )));
100        }
101        Ok(Self {
102            pinhole: PinholeParams {
103                fx: params[0],
104                fy: params[1],
105                cx: params[2],
106                cy: params[3],
107            },
108            distortion: DistortionModel::None,
109        })
110    }
111}
112
113/// Parameter order: `[fx, fy, cx, cy]`.
114impl From<[f64; 4]> for PinholeCamera {
115    fn from(params: [f64; 4]) -> Self {
116        Self {
117            pinhole: PinholeParams {
118                fx: params[0],
119                fy: params[1],
120                cx: params[2],
121                cy: params[3],
122            },
123            distortion: DistortionModel::None,
124        }
125    }
126}
127
128/// Like [`<PinholeCamera as TryFrom<&[f64]>>::try_from`] but also validates
129/// the resulting parameters. Returns
130/// [`CameraModelError::InvalidParams`] on a short slice and a validation
131/// error otherwise.
132pub fn try_from_params(params: &[f64]) -> Result<PinholeCamera, CameraModelError> {
133    let camera = PinholeCamera::try_from(params)?;
134    camera.validate_params()?;
135    Ok(camera)
136}
137
138impl CameraModel for PinholeCamera {
139    const INTRINSIC_DIM: usize = 4;
140    type IntrinsicJacobian = SMatrix<f64, 2, 4>;
141    type PointJacobian = SMatrix<f64, 2, 3>;
142
143    /// Projects a 3D point to 2D pixel coordinates. See the
144    /// [cookbook](../doc/cookbook/src/pinhole.html#projection) for the
145    /// formula.
146    ///
147    /// # Errors
148    ///
149    /// Returns [`CameraModelError::PointBehindCamera`] when `z < GEOMETRIC_PRECISION`.
150    fn project(&self, p_cam: &Vector3<f64>) -> Result<Vector2<f64>, CameraModelError> {
151        if !self.check_projection_condition(p_cam.z) {
152            return Err(CameraModelError::PointBehindCamera {
153                z: p_cam.z,
154                min_z: crate::GEOMETRIC_PRECISION,
155            });
156        }
157        let inv_z = 1.0 / p_cam.z;
158        Ok(Vector2::new(
159            self.pinhole.fx * p_cam.x * inv_z + self.pinhole.cx,
160            self.pinhole.fy * p_cam.y * inv_z + self.pinhole.cy,
161        ))
162    }
163
164    /// Unprojects a pixel to a unit ray. Algebraic. See the
165    /// [cookbook](../doc/cookbook/src/pinhole.html#unprojection).
166    ///
167    /// # Errors
168    ///
169    /// Never fails; the `Result` is for trait uniformity.
170    fn unproject(&self, point_2d: &Vector2<f64>) -> Result<Vector3<f64>, CameraModelError> {
171        let mx = (point_2d.x - self.pinhole.cx) / self.pinhole.fx;
172        let my = (point_2d.y - self.pinhole.cy) / self.pinhole.fy;
173
174        let r2 = mx * mx + my * my;
175        let norm = (1.0 + r2).sqrt();
176        let norm_inv = 1.0 / norm;
177
178        Ok(Vector3::new(mx * norm_inv, my * norm_inv, norm_inv))
179    }
180
181    /// ∂(u,v)/∂(x,y,z) — 2×3 projection Jacobian.
182    /// See the [cookbook](../doc/cookbook/src/pinhole.html#point-jacobian).
183    fn jacobian_point(&self, p_cam: &Vector3<f64>) -> Self::PointJacobian {
184        let inv_z = 1.0 / p_cam.z;
185        let x_norm = p_cam.x * inv_z;
186        let y_norm = p_cam.y * inv_z;
187
188        SMatrix::<f64, 2, 3>::new(
189            self.pinhole.fx * inv_z,
190            0.0,
191            -self.pinhole.fx * x_norm * inv_z,
192            0.0,
193            self.pinhole.fy * inv_z,
194            -self.pinhole.fy * y_norm * inv_z,
195        )
196    }
197
198    /// ∂(u,v)/∂(fx, fy, cx, cy) — 2×4 intrinsic Jacobian. Parameter
199    /// order: `[fx, fy, cx, cy]`. See the
200    /// [cookbook](../doc/cookbook/src/pinhole.html#intrinsic-jacobian).
201    fn jacobian_intrinsics(&self, p_cam: &Vector3<f64>) -> Self::IntrinsicJacobian {
202        let inv_z = 1.0 / p_cam.z;
203        let x_norm = p_cam.x * inv_z;
204        let y_norm = p_cam.y * inv_z;
205
206        SMatrix::<f64, 2, 4>::new(x_norm, 0.0, 1.0, 0.0, 0.0, y_norm, 0.0, 1.0)
207    }
208
209    /// Validates pinhole intrinsics. Rules (mirrored in the
210    /// [cookbook](../doc/cookbook/src/pinhole.html#validation-rules)):
211    ///
212    /// - `fx > 0`, `fy > 0` and finite.
213    /// - `cx`, `cy` finite.
214    fn validate_params(&self) -> Result<(), CameraModelError> {
215        self.pinhole.validate()?;
216        self.get_distortion().validate()
217    }
218
219    /// Returns the linear intrinsics `(fx, fy, cx, cy)`.
220    fn get_pinhole_params(&self) -> PinholeParams {
221        PinholeParams {
222            fx: self.pinhole.fx,
223            fy: self.pinhole.fy,
224            cx: self.pinhole.cx,
225            cy: self.pinhole.cy,
226        }
227    }
228
229    /// Returns the distortion model (always [`DistortionModel::None`] for this camera).
230    fn get_distortion(&self) -> DistortionModel {
231        self.distortion
232    }
233
234    /// Returns the model identifier `"pinhole"`.
235    fn get_model_name(&self) -> &'static str {
236        "pinhole"
237    }
238}
239
240#[cfg(test)]
241mod tests {
242    use super::*;
243
244    type TestResult = Result<(), Box<dyn std::error::Error>>;
245
246    fn assert_approx_eq(a: f64, b: f64, eps: f64) {
247        assert!(
248            (a - b).abs() < eps,
249            "Values {} and {} differ by more than {}",
250            a,
251            b,
252            eps
253        );
254    }
255
256    #[test]
257    fn test_pinhole_camera_creation() -> TestResult {
258        let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
259        let distortion = DistortionModel::None;
260        let camera = PinholeCamera::new(pinhole, distortion)?;
261        assert_eq!(camera.pinhole.fx, 500.0);
262        assert_eq!(camera.pinhole.fy, 500.0);
263        assert_eq!(camera.pinhole.cx, 320.0);
264        assert_eq!(camera.pinhole.cy, 240.0);
265        Ok(())
266    }
267
268    #[test]
269    fn test_pinhole_from_params() -> TestResult {
270        let params = vec![600.0, 600.0, 320.0, 240.0];
271        let camera = PinholeCamera::try_from(params.as_slice())?;
272        assert_eq!(camera.pinhole.fx, 600.0);
273        let params_vec: DVector<f64> = (&camera).into();
274        assert_eq!(params_vec, DVector::from_vec(params));
275        Ok(())
276    }
277
278    #[test]
279    fn test_projection_at_optical_axis() -> TestResult {
280        let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
281        let distortion = DistortionModel::None;
282        let camera = PinholeCamera::new(pinhole, distortion)?;
283        let p_cam = Vector3::new(0.0, 0.0, 1.0);
284
285        let uv = camera.project(&p_cam)?;
286
287        assert_approx_eq(uv.x, 320.0, 1e-10);
288        assert_approx_eq(uv.y, 240.0, 1e-10);
289
290        Ok(())
291    }
292
293    #[test]
294    fn test_projection_off_axis() -> TestResult {
295        let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
296        let distortion = DistortionModel::None;
297        let camera = PinholeCamera::new(pinhole, distortion)?;
298        let p_cam = Vector3::new(0.1, 0.2, 1.0);
299
300        let uv = camera.project(&p_cam)?;
301
302        assert_approx_eq(uv.x, 370.0, 1e-10);
303        assert_approx_eq(uv.y, 340.0, 1e-10);
304
305        Ok(())
306    }
307
308    #[test]
309    fn test_projection_behind_camera() -> TestResult {
310        let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
311        let distortion = DistortionModel::None;
312        let camera = PinholeCamera::new(pinhole, distortion)?;
313        let p_cam = Vector3::new(0.0, 0.0, -1.0);
314
315        let result = camera.project(&p_cam);
316        assert!(result.is_err());
317        Ok(())
318    }
319
320    #[test]
321    fn test_jacobian_point_dimensions() -> TestResult {
322        let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
323        let distortion = DistortionModel::None;
324        let camera = PinholeCamera::new(pinhole, distortion)?;
325        let p_cam = Vector3::new(0.1, 0.2, 1.0);
326
327        let jac = camera.jacobian_point(&p_cam);
328
329        assert_eq!(jac.nrows(), 2);
330        assert_eq!(jac.ncols(), 3);
331
332        Ok(())
333    }
334
335    #[test]
336    fn test_jacobian_intrinsics_dimensions() -> TestResult {
337        let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
338        let distortion = DistortionModel::None;
339        let camera = PinholeCamera::new(pinhole, distortion)?;
340        let p_cam = Vector3::new(0.1, 0.2, 1.0);
341
342        let jac = camera.jacobian_intrinsics(&p_cam);
343
344        assert_eq!(jac.nrows(), 2);
345        assert_eq!(jac.ncols(), 4);
346        Ok(())
347    }
348
349    #[test]
350    fn test_jacobian_point_numerical() -> TestResult {
351        let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
352        let distortion = DistortionModel::None;
353        let camera = PinholeCamera::new(pinhole, distortion)?;
354        let p_cam = Vector3::new(0.1, 0.2, 1.0);
355
356        let jac_analytical = camera.jacobian_point(&p_cam);
357
358        let eps = crate::NUMERICAL_DERIVATIVE_EPS;
359        for i in 0..3 {
360            let mut p_plus = p_cam;
361            let mut p_minus = p_cam;
362            p_plus[i] += eps;
363            p_minus[i] -= eps;
364
365            let uv_plus = camera.project(&p_plus)?;
366            let uv_minus = camera.project(&p_minus)?;
367
368            let numerical_jac = (uv_plus - uv_minus) / (2.0 * eps);
369
370            for r in 0..2 {
371                let analytical = jac_analytical[(r, i)];
372                let numerical = numerical_jac[r];
373                assert!(
374                    analytical.is_finite(),
375                    "Jacobian point [{r},{i}] is not finite"
376                );
377                let rel_error = (analytical - numerical).abs() / (1.0 + numerical.abs());
378                assert!(
379                    rel_error < crate::JACOBIAN_TEST_TOLERANCE,
380                    "Jacobian mismatch at ({}, {}): analytical={}, numerical={}, rel_error={}",
381                    r,
382                    i,
383                    analytical,
384                    numerical,
385                    rel_error
386                );
387            }
388        }
389
390        Ok(())
391    }
392
393    #[test]
394    fn test_jacobian_intrinsics_numerical() -> TestResult {
395        let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
396        let distortion = DistortionModel::None;
397        let camera = PinholeCamera::new(pinhole, distortion)?;
398        let p_cam = Vector3::new(0.1, 0.2, 1.0);
399
400        let jac_analytical = camera.jacobian_intrinsics(&p_cam);
401
402        let eps = crate::NUMERICAL_DERIVATIVE_EPS;
403        let params: DVector<f64> = (&camera).into();
404
405        for i in 0..4 {
406            let mut params_plus = params.clone();
407            let mut params_minus = params.clone();
408            params_plus[i] += eps;
409            params_minus[i] -= eps;
410
411            let cam_plus = PinholeCamera::try_from(params_plus.as_slice())?;
412            let cam_minus = PinholeCamera::try_from(params_minus.as_slice())?;
413
414            let uv_plus = cam_plus.project(&p_cam)?;
415            let uv_minus = cam_minus.project(&p_cam)?;
416
417            let numerical_jac = (uv_plus - uv_minus) / (2.0 * eps);
418
419            for r in 0..2 {
420                let analytical = jac_analytical[(r, i)];
421                let numerical = numerical_jac[r];
422                assert!(
423                    analytical.is_finite(),
424                    "Jacobian intrinsics [{r},{i}] is not finite"
425                );
426                let rel_error = (analytical - numerical).abs() / (1.0 + numerical.abs());
427                assert!(
428                    rel_error < crate::JACOBIAN_TEST_TOLERANCE,
429                    "Intrinsics Jacobian mismatch at ({}, {}): analytical={}, numerical={}, rel_error={}",
430                    r,
431                    i,
432                    analytical,
433                    numerical,
434                    rel_error
435                );
436            }
437        }
438
439        Ok(())
440    }
441
442    #[test]
443    fn test_project_unproject_round_trip() -> TestResult {
444        let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
445        let camera = PinholeCamera::new(pinhole, DistortionModel::None)?;
446
447        let test_points = [
448            Vector3::new(0.1, 0.2, 1.0),
449            Vector3::new(-0.3, 0.1, 2.0),
450            Vector3::new(0.05, -0.1, 0.5),
451        ];
452
453        for p_cam in &test_points {
454            let uv = camera.project(p_cam)?;
455            let ray = camera.unproject(&uv)?;
456            let dot = ray.dot(&p_cam.normalize());
457            assert!(
458                (dot - 1.0).abs() < 1e-6,
459                "Round-trip failed: dot={dot}, expected ~1.0"
460            );
461        }
462
463        Ok(())
464    }
465
466    #[test]
467    fn test_project_returns_error_behind_camera() -> TestResult {
468        let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
469        let camera = PinholeCamera::new(pinhole, DistortionModel::None)?;
470        assert!(camera.project(&Vector3::new(0.0, 0.0, -1.0)).is_err());
471        Ok(())
472    }
473
474    #[test]
475    fn test_project_at_min_depth_boundary() -> TestResult {
476        let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
477        let camera = PinholeCamera::new(pinhole, DistortionModel::None)?;
478        let p_min = Vector3::new(0.0, 0.0, crate::MIN_DEPTH);
479        if let Ok(uv) = camera.project(&p_min) {
480            assert!(uv.x.is_finite() && uv.y.is_finite());
481        }
482        Ok(())
483    }
484}
apex_camera_models/pinhole.rs

apex_camera_models/
pinhole.rs
