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