apex_camera_models/pinhole.rs
1//! Pinhole Camera Model
2//!
3//! The simplest perspective camera model with no lens distortion.
4//!
5//! # Mathematical Model
6//!
7//! ## Projection (3D → 2D)
8//!
9//! For a 3D point p = (x, y, z) in camera coordinates:
10//!
11//! ```text
12//! u = fx · (x/z) + cx
13//! v = fy · (y/z) + cy
14//! ```
15//!
16//! where:
17//! - (fx, fy) are focal lengths in pixels
18//! - (cx, cy) is the principal point in pixels
19//! - z > 0 (point in front of camera)
20//!
21//! ## Unprojection (2D → 3D)
22//!
23//! For a 2D point (u, v) in image coordinates, the unprojected ray is:
24//!
25//! ```text
26//! mx = (u - cx) / fx
27//! my = (v - cy) / fy
28//! ray = normalize([mx, my, 1])
29//! ```
30//!
31//! # Parameters
32//!
33//! - **Intrinsics**: fx, fy, cx, cy (4 parameters)
34//! - **Distortion**: None
35//!
36//! # Use Cases
37//!
38//! - Standard narrow field-of-view cameras
39//! - Initial calibration estimates
40//! - Testing and validation
41//!
42//! # References
43//!
44//! - Hartley & Zisserman, "Multiple View Geometry in Computer Vision"
45
46use crate::{CameraModel, CameraModelError, DistortionModel, PinholeParams};
47use nalgebra::{DVector, SMatrix, Vector2, Vector3};
48
49/// Pinhole camera model with 4 intrinsic parameters.
50#[derive(Debug, Clone, Copy, PartialEq)]
51pub struct PinholeCamera {
52 pub pinhole: PinholeParams,
53 pub distortion: DistortionModel,
54}
55
56impl PinholeCamera {
57 /// Creates a new Pinhole camera model.
58 ///
59 /// # Arguments
60 ///
61 /// * `pinhole` - Pinhole camera parameters (fx, fy, cx, cy).
62 /// * `distortion` - Distortion model (must be [`DistortionModel::None`]).
63 ///
64 /// # Returns
65 ///
66 /// Returns a new `PinholeCamera` instance if the parameters are valid.
67 ///
68 /// # Errors
69 ///
70 /// Returns [`CameraModelError`] if:
71 /// - The distortion model is not `None`.
72 /// - Parameters are invalid (e.g., negative focal length, infinite principal point).
73 ///
74 /// # Example
75 ///
76 /// ```
77 /// use apex_camera_models::{PinholeCamera, PinholeParams, DistortionModel};
78 ///
79 /// let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
80 /// let distortion = DistortionModel::None;
81 /// let camera = PinholeCamera::new(pinhole, distortion)?;
82 /// # Ok::<(), apex_camera_models::CameraModelError>(())
83 /// ```
84 pub fn new(
85 pinhole: PinholeParams,
86 distortion: DistortionModel,
87 ) -> Result<Self, CameraModelError> {
88 let camera = Self {
89 pinhole,
90 distortion,
91 };
92 camera.validate_params()?;
93 Ok(camera)
94 }
95
96 /// Checks the geometric condition for a valid projection.
97 ///
98 /// # Arguments
99 ///
100 /// * `z` - The z-coordinate of the point in the camera frame.
101 ///
102 /// # Returns
103 ///
104 /// Returns `true` if `z >= 1e-6`, `false` otherwise.
105 fn check_projection_condition(&self, z: f64) -> bool {
106 z >= 1e-6
107 }
108}
109
110/// Convert PinholeCamera to parameter vector.
111///
112/// Returns intrinsic parameters in the order: [fx, fy, cx, cy]
113impl From<&PinholeCamera> for DVector<f64> {
114 fn from(camera: &PinholeCamera) -> Self {
115 DVector::from_vec(vec![
116 camera.pinhole.fx,
117 camera.pinhole.fy,
118 camera.pinhole.cx,
119 camera.pinhole.cy,
120 ])
121 }
122}
123
124/// Convert PinholeCamera to fixed-size parameter array.
125///
126/// Returns intrinsic parameters as [fx, fy, cx, cy]
127impl From<&PinholeCamera> for [f64; 4] {
128 fn from(camera: &PinholeCamera) -> Self {
129 [
130 camera.pinhole.fx,
131 camera.pinhole.fy,
132 camera.pinhole.cx,
133 camera.pinhole.cy,
134 ]
135 }
136}
137
138/// Create PinholeCamera from parameter slice.
139///
140/// Returns an error if the slice has fewer than 4 elements.
141///
142/// # Parameter Order
143///
144/// params = [fx, fy, cx, cy]
145impl TryFrom<&[f64]> for PinholeCamera {
146 type Error = CameraModelError;
147
148 fn try_from(params: &[f64]) -> Result<Self, Self::Error> {
149 if params.len() < 4 {
150 return Err(CameraModelError::InvalidParams(format!(
151 "PinholeCamera requires at least 4 parameters, got {}",
152 params.len()
153 )));
154 }
155 Ok(Self {
156 pinhole: PinholeParams {
157 fx: params[0],
158 fy: params[1],
159 cx: params[2],
160 cy: params[3],
161 },
162 distortion: DistortionModel::None,
163 })
164 }
165}
166
167/// Create PinholeCamera from fixed-size parameter array.
168///
169/// # Parameter Order
170///
171/// params = [fx, fy, cx, cy]
172impl From<[f64; 4]> for PinholeCamera {
173 fn from(params: [f64; 4]) -> Self {
174 Self {
175 pinhole: PinholeParams {
176 fx: params[0],
177 fy: params[1],
178 cx: params[2],
179 cy: params[3],
180 },
181 distortion: DistortionModel::None,
182 }
183 }
184}
185
186/// Creates a `PinholeCamera` from a parameter slice with validation.
187///
188/// Unlike `From<&[f64]>`, this constructor validates all parameters
189/// and returns a `Result` instead of panicking on invalid input.
190///
191/// # Errors
192///
193/// Returns `CameraModelError::InvalidParams` if fewer than 4 parameters are provided.
194/// Returns validation errors if focal lengths are non-positive or parameters are non-finite.
195pub fn try_from_params(params: &[f64]) -> Result<PinholeCamera, CameraModelError> {
196 let camera = PinholeCamera::try_from(params)?;
197 camera.validate_params()?;
198 Ok(camera)
199}
200
201impl CameraModel for PinholeCamera {
202 const INTRINSIC_DIM: usize = 4;
203 type IntrinsicJacobian = SMatrix<f64, 2, 4>;
204 type PointJacobian = SMatrix<f64, 2, 3>;
205
206 /// Projects a 3D point to 2D image coordinates.
207 ///
208 /// # Mathematical Formula
209 ///
210 /// ```text
211 /// u = fx · (x/z) + cx
212 /// v = fy · (y/z) + cy
213 /// ```
214 ///
215 /// # Arguments
216 ///
217 /// * `p_cam` - 3D point in camera coordinate frame (x, y, z).
218 ///
219 /// # Returns
220 ///
221 /// Returns the 2D image coordinates if valid.
222 ///
223 /// # Errors
224 ///
225 /// Returns [`CameraModelError::PointAtCameraCenter`] if the point is behind or at the camera center (`z <= MIN_DEPTH`).
226 fn project(&self, p_cam: &Vector3<f64>) -> Result<Vector2<f64>, CameraModelError> {
227 if !self.check_projection_condition(p_cam.z) {
228 return Err(CameraModelError::PointBehindCamera {
229 z: p_cam.z,
230 min_z: crate::GEOMETRIC_PRECISION,
231 });
232 }
233 let inv_z = 1.0 / p_cam.z;
234 Ok(Vector2::new(
235 self.pinhole.fx * p_cam.x * inv_z + self.pinhole.cx,
236 self.pinhole.fy * p_cam.y * inv_z + self.pinhole.cy,
237 ))
238 }
239
240 /// Unprojects a 2D image point to a 3D ray in camera frame.
241 ///
242 /// # Mathematical Formula
243 ///
244 /// ```text
245 /// mx = (u - cx) / fx
246 /// my = (v - cy) / fy
247 /// ray = normalize([mx, my, 1])
248 /// ```
249 ///
250 /// # Arguments
251 ///
252 /// * `point_2d` - 2D point in image coordinates (u, v).
253 ///
254 /// # Returns
255 ///
256 /// Returns the normalized 3D ray direction.
257 ///
258 /// # Errors
259 ///
260 /// This function never fails for the simple pinhole model, but returns a `Result` for trait consistency.
261 fn unproject(&self, point_2d: &Vector2<f64>) -> Result<Vector3<f64>, CameraModelError> {
262 let mx = (point_2d.x - self.pinhole.cx) / self.pinhole.fx;
263 let my = (point_2d.y - self.pinhole.cy) / self.pinhole.fy;
264
265 let r2 = mx * mx + my * my;
266 let norm = (1.0 + r2).sqrt();
267 let norm_inv = 1.0 / norm;
268
269 Ok(Vector3::new(mx * norm_inv, my * norm_inv, norm_inv))
270 }
271
272 /// Jacobian of projection w.r.t. 3D point coordinates (2×3).
273 ///
274 /// Computes ∂π/∂p where π is the projection function and p = (x, y, z) is the 3D point.
275 ///
276 /// # Mathematical Derivation
277 ///
278 /// Given the pinhole projection model:
279 /// ```text
280 /// u = fx · (x/z) + cx
281 /// v = fy · (y/z) + cy
282 /// ```
283 ///
284 /// Derivatives:
285 /// ```text
286 /// ∂u/∂x = fx/z, ∂u/∂y = 0, ∂u/∂z = -fx·x/z²
287 /// ∂v/∂x = 0, ∂v/∂y = fy/z, ∂v/∂z = -fy·y/z²
288 /// ```
289 ///
290 /// Final Jacobian matrix (2×3):
291 ///
292 /// ```text
293 /// J = [ ∂u/∂x ∂u/∂y ∂u/∂z ] [ fx/z 0 -fx·x/z² ]
294 /// [ ∂v/∂x ∂v/∂y ∂v/∂z ] = [ 0 fy/z -fy·y/z² ]
295 /// ```
296 ///
297 /// # Arguments
298 ///
299 /// * `p_cam` - 3D point in camera coordinate frame.
300 ///
301 /// # Returns
302 ///
303 /// Returns the 2x3 Jacobian matrix.
304 ///
305 /// # Implementation Note
306 ///
307 /// The implementation uses `inv_z = 1/z` and `x_norm = x/z`, `y_norm = y/z`
308 /// to avoid redundant divisions and improve numerical stability.
309 ///
310 /// # References
311 ///
312 /// - Hartley & Zisserman, "Multiple View Geometry", Chapter 6
313 /// - Standard perspective projection derivatives
314 /// - Verified against numerical differentiation in tests
315 fn jacobian_point(&self, p_cam: &Vector3<f64>) -> Self::PointJacobian {
316 let inv_z = 1.0 / p_cam.z;
317 let x_norm = p_cam.x * inv_z;
318 let y_norm = p_cam.y * inv_z;
319
320 // Jacobian ∂(u,v)/∂(x,y,z) where (x,y,z) is point in camera frame
321 SMatrix::<f64, 2, 3>::new(
322 self.pinhole.fx * inv_z,
323 0.0,
324 -self.pinhole.fx * x_norm * inv_z,
325 0.0,
326 self.pinhole.fy * inv_z,
327 -self.pinhole.fy * y_norm * inv_z,
328 )
329 }
330
331 /// Jacobian of projection w.r.t. intrinsic parameters (2×4).
332 ///
333 /// Computes ∂π/∂K where K = [fx, fy, cx, cy] are the intrinsic parameters.
334 ///
335 /// # Mathematical Derivation
336 ///
337 /// The intrinsic parameters for the pinhole model are:
338 /// - **Focal lengths**: fx, fy (scaling factors)
339 /// - **Principal point**: cx, cy (image center offset)
340 ///
341 /// ## Projection Model Recap
342 ///
343 /// ```text
344 /// u = fx · (x/z) + cx
345 /// v = fy · (y/z) + cy
346 /// ```
347 ///
348 /// Derivatives:
349 /// ```text
350 /// ∂u/∂fx = x/z, ∂u/∂fy = 0, ∂u/∂cx = 1, ∂u/∂cy = 0
351 /// ∂v/∂fx = 0, ∂v/∂fy = y/z, ∂v/∂cx = 0, ∂v/∂cy = 1
352 /// ```
353 ///
354 /// Final Jacobian matrix (2×4):
355 ///
356 /// ```text
357 /// J = [ ∂u/∂fx ∂u/∂fy ∂u/∂cx ∂u/∂cy ]
358 /// [ ∂v/∂fx ∂v/∂fy ∂v/∂cx ∂v/∂cy ]
359 ///
360 /// = [ x/z 0 1 0 ]
361 /// [ 0 y/z 0 1 ]
362 /// ```
363 ///
364 /// # Arguments
365 ///
366 /// * `p_cam` - 3D point in camera coordinate frame.
367 ///
368 /// # Returns
369 ///
370 /// Returns the 2x4 Intrinsic Jacobian matrix.
371 ///
372 /// # Implementation Note
373 ///
374 /// The implementation uses precomputed normalized coordinates:
375 /// - `x_norm = x/z`
376 /// - `y_norm = y/z`
377 ///
378 /// This avoids redundant divisions and improves efficiency.
379 ///
380 /// # References
381 ///
382 /// - Standard camera calibration literature
383 /// - Hartley & Zisserman, "Multiple View Geometry", Chapter 6
384 /// - Verified against numerical differentiation in tests
385 fn jacobian_intrinsics(&self, p_cam: &Vector3<f64>) -> Self::IntrinsicJacobian {
386 let inv_z = 1.0 / p_cam.z;
387 let x_norm = p_cam.x * inv_z;
388 let y_norm = p_cam.y * inv_z;
389
390 // Jacobian ∂(u,v)/∂(fx,fy,cx,cy)
391 SMatrix::<f64, 2, 4>::new(x_norm, 0.0, 1.0, 0.0, 0.0, y_norm, 0.0, 1.0)
392 }
393
394 /// Validates camera parameters.
395 ///
396 /// # Validation Rules
397 ///
398 /// - `fx` and `fy` must be positive.
399 /// - `fx` and `fy` must be finite.
400 /// - `cx` and `cy` must be finite.
401 ///
402 /// # Errors
403 ///
404 /// Returns [`CameraModelError`] if any parameter violates validation rules.
405 fn validate_params(&self) -> Result<(), CameraModelError> {
406 self.pinhole.validate()?;
407 self.get_distortion().validate()
408 }
409
410 /// Returns the pinhole parameters of the camera.
411 ///
412 /// # Returns
413 ///
414 /// A [`PinholeParams`] struct containing the focal lengths (fx, fy) and principal point (cx, cy).
415 fn get_pinhole_params(&self) -> PinholeParams {
416 PinholeParams {
417 fx: self.pinhole.fx,
418 fy: self.pinhole.fy,
419 cx: self.pinhole.cx,
420 cy: self.pinhole.cy,
421 }
422 }
423
424 /// Returns the distortion model and parameters of the camera.
425 ///
426 /// # Returns
427 ///
428 /// The [`DistortionModel`] associated with this camera (typically [`DistortionModel::None`] for pinhole).
429 fn get_distortion(&self) -> DistortionModel {
430 self.distortion
431 }
432
433 /// Returns the string identifier for the camera model.
434 ///
435 /// # Returns
436 ///
437 /// The string `"pinhole"`.
438 fn get_model_name(&self) -> &'static str {
439 "pinhole"
440 }
441}
442
443#[cfg(test)]
444mod tests {
445 use super::*;
446
447 type TestResult = Result<(), Box<dyn std::error::Error>>;
448
449 fn assert_approx_eq(a: f64, b: f64, eps: f64) {
450 assert!(
451 (a - b).abs() < eps,
452 "Values {} and {} differ by more than {}",
453 a,
454 b,
455 eps
456 );
457 }
458
459 #[test]
460 fn test_pinhole_camera_creation() -> TestResult {
461 let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
462 let distortion = DistortionModel::None;
463 let camera = PinholeCamera::new(pinhole, distortion)?;
464 assert_eq!(camera.pinhole.fx, 500.0);
465 assert_eq!(camera.pinhole.fy, 500.0);
466 assert_eq!(camera.pinhole.cx, 320.0);
467 assert_eq!(camera.pinhole.cy, 240.0);
468 Ok(())
469 }
470
471 #[test]
472 fn test_pinhole_from_params() -> TestResult {
473 let params = vec![600.0, 600.0, 320.0, 240.0];
474 let camera = PinholeCamera::try_from(params.as_slice())?;
475 assert_eq!(camera.pinhole.fx, 600.0);
476 let params_vec: DVector<f64> = (&camera).into();
477 assert_eq!(params_vec, DVector::from_vec(params));
478 Ok(())
479 }
480
481 #[test]
482 fn test_projection_at_optical_axis() -> TestResult {
483 let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
484 let distortion = DistortionModel::None;
485 let camera = PinholeCamera::new(pinhole, distortion)?;
486 let p_cam = Vector3::new(0.0, 0.0, 1.0);
487
488 let uv = camera.project(&p_cam)?;
489
490 assert_approx_eq(uv.x, 320.0, 1e-10);
491 assert_approx_eq(uv.y, 240.0, 1e-10);
492
493 Ok(())
494 }
495
496 #[test]
497 fn test_projection_off_axis() -> TestResult {
498 let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
499 let distortion = DistortionModel::None;
500 let camera = PinholeCamera::new(pinhole, distortion)?;
501 let p_cam = Vector3::new(0.1, 0.2, 1.0);
502
503 let uv = camera.project(&p_cam)?;
504
505 assert_approx_eq(uv.x, 370.0, 1e-10);
506 assert_approx_eq(uv.y, 340.0, 1e-10);
507
508 Ok(())
509 }
510
511 #[test]
512 fn test_projection_behind_camera() -> TestResult {
513 let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
514 let distortion = DistortionModel::None;
515 let camera = PinholeCamera::new(pinhole, distortion)?;
516 let p_cam = Vector3::new(0.0, 0.0, -1.0);
517
518 let result = camera.project(&p_cam);
519 assert!(result.is_err());
520 Ok(())
521 }
522
523 #[test]
524 fn test_jacobian_point_dimensions() -> TestResult {
525 let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
526 let distortion = DistortionModel::None;
527 let camera = PinholeCamera::new(pinhole, distortion)?;
528 let p_cam = Vector3::new(0.1, 0.2, 1.0);
529
530 let jac = camera.jacobian_point(&p_cam);
531
532 assert_eq!(jac.nrows(), 2);
533 assert_eq!(jac.ncols(), 3);
534
535 Ok(())
536 }
537
538 #[test]
539 fn test_jacobian_intrinsics_dimensions() -> TestResult {
540 let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
541 let distortion = DistortionModel::None;
542 let camera = PinholeCamera::new(pinhole, distortion)?;
543 let p_cam = Vector3::new(0.1, 0.2, 1.0);
544
545 let jac = camera.jacobian_intrinsics(&p_cam);
546
547 assert_eq!(jac.nrows(), 2);
548 assert_eq!(jac.ncols(), 4);
549 Ok(())
550 }
551
552 #[test]
553 fn test_jacobian_point_numerical() -> TestResult {
554 let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
555 let distortion = DistortionModel::None;
556 let camera = PinholeCamera::new(pinhole, distortion)?;
557 let p_cam = Vector3::new(0.1, 0.2, 1.0);
558
559 let jac_analytical = camera.jacobian_point(&p_cam);
560
561 let eps = crate::NUMERICAL_DERIVATIVE_EPS;
562 for i in 0..3 {
563 let mut p_plus = p_cam;
564 let mut p_minus = p_cam;
565 p_plus[i] += eps;
566 p_minus[i] -= eps;
567
568 let uv_plus = camera.project(&p_plus)?;
569 let uv_minus = camera.project(&p_minus)?;
570
571 let numerical_jac = (uv_plus - uv_minus) / (2.0 * eps);
572
573 for r in 0..2 {
574 let analytical = jac_analytical[(r, i)];
575 let numerical = numerical_jac[r];
576 assert!(
577 analytical.is_finite(),
578 "Jacobian point [{r},{i}] is not finite"
579 );
580 let rel_error = (analytical - numerical).abs() / (1.0 + numerical.abs());
581 assert!(
582 rel_error < crate::JACOBIAN_TEST_TOLERANCE,
583 "Jacobian mismatch at ({}, {}): analytical={}, numerical={}, rel_error={}",
584 r,
585 i,
586 analytical,
587 numerical,
588 rel_error
589 );
590 }
591 }
592
593 Ok(())
594 }
595
596 #[test]
597 fn test_jacobian_intrinsics_numerical() -> TestResult {
598 let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
599 let distortion = DistortionModel::None;
600 let camera = PinholeCamera::new(pinhole, distortion)?;
601 let p_cam = Vector3::new(0.1, 0.2, 1.0);
602
603 let jac_analytical = camera.jacobian_intrinsics(&p_cam);
604
605 let eps = crate::NUMERICAL_DERIVATIVE_EPS;
606 let params: DVector<f64> = (&camera).into();
607
608 for i in 0..4 {
609 let mut params_plus = params.clone();
610 let mut params_minus = params.clone();
611 params_plus[i] += eps;
612 params_minus[i] -= eps;
613
614 let cam_plus = PinholeCamera::try_from(params_plus.as_slice())?;
615 let cam_minus = PinholeCamera::try_from(params_minus.as_slice())?;
616
617 let uv_plus = cam_plus.project(&p_cam)?;
618 let uv_minus = cam_minus.project(&p_cam)?;
619
620 let numerical_jac = (uv_plus - uv_minus) / (2.0 * eps);
621
622 for r in 0..2 {
623 let analytical = jac_analytical[(r, i)];
624 let numerical = numerical_jac[r];
625 assert!(
626 analytical.is_finite(),
627 "Jacobian intrinsics [{r},{i}] is not finite"
628 );
629 let rel_error = (analytical - numerical).abs() / (1.0 + numerical.abs());
630 assert!(
631 rel_error < crate::JACOBIAN_TEST_TOLERANCE,
632 "Intrinsics Jacobian mismatch at ({}, {}): analytical={}, numerical={}, rel_error={}",
633 r,
634 i,
635 analytical,
636 numerical,
637 rel_error
638 );
639 }
640 }
641
642 Ok(())
643 }
644
645 #[test]
646 fn test_project_unproject_round_trip() -> TestResult {
647 let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
648 let camera = PinholeCamera::new(pinhole, DistortionModel::None)?;
649
650 let test_points = [
651 Vector3::new(0.1, 0.2, 1.0),
652 Vector3::new(-0.3, 0.1, 2.0),
653 Vector3::new(0.05, -0.1, 0.5),
654 ];
655
656 for p_cam in &test_points {
657 let uv = camera.project(p_cam)?;
658 let ray = camera.unproject(&uv)?;
659 let dot = ray.dot(&p_cam.normalize());
660 assert!(
661 (dot - 1.0).abs() < 1e-6,
662 "Round-trip failed: dot={dot}, expected ~1.0"
663 );
664 }
665
666 Ok(())
667 }
668
669 #[test]
670 fn test_project_returns_error_behind_camera() -> TestResult {
671 let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
672 let camera = PinholeCamera::new(pinhole, DistortionModel::None)?;
673 assert!(camera.project(&Vector3::new(0.0, 0.0, -1.0)).is_err());
674 Ok(())
675 }
676
677 #[test]
678 fn test_project_at_min_depth_boundary() -> TestResult {
679 let pinhole = PinholeParams::new(500.0, 500.0, 320.0, 240.0)?;
680 let camera = PinholeCamera::new(pinhole, DistortionModel::None)?;
681 let p_min = Vector3::new(0.0, 0.0, crate::MIN_DEPTH);
682 if let Ok(uv) = camera.project(&p_min) {
683 assert!(uv.x.is_finite() && uv.y.is_finite());
684 }
685 Ok(())
686 }
687}