nabled_sensor/
camera.rs

1//! Pinhole camera model.
2
3use nabled_core::scalar::NabledReal;
4use ndarray::{Array1, Array2, ArrayView1};
5
6use crate::SensorError;
7
8/// Pinhole intrinsics `{ fx, fy, cx, cy }`.
9#[derive(Debug, Clone, Copy, PartialEq)]
10pub struct PinholeIntrinsics<T> {
11    pub fx: T,
12    pub fy: T,
13    pub cx: T,
14    pub cy: T,
15}
16
17impl<T: NabledReal> PinholeIntrinsics<T> {
18    /// Build a 3×3 intrinsic matrix `K`.
19    #[must_use]
20    pub fn matrix(&self) -> Array2<T> {
21        let mut k = Array2::<T>::zeros((3, 3));
22        k[[0, 0]] = self.fx;
23        k[[1, 1]] = self.fy;
24        k[[0, 2]] = self.cx;
25        k[[1, 2]] = self.cy;
26        k[[2, 2]] = T::one();
27        k
28    }
29}
30
31pub fn pinhole_project<T: NabledReal>(
32    point: &ArrayView1<'_, T>,
33    intrinsics: &PinholeIntrinsics<T>,
34) -> Result<Array1<T>, SensorError> {
35    if point.len() != 3 {
36        return Err(SensorError::DimensionMismatch);
37    }
38    if point[2] <= T::from_f64(1e-12).unwrap_or(T::zero()) {
39        return Err(SensorError::InvalidInput("point behind camera".to_string()));
40    }
41    let u = intrinsics.fx * point[0] / point[2] + intrinsics.cx;
42    let v = intrinsics.fy * point[1] / point[2] + intrinsics.cy;
43    Ok(ndarray::arr1(&[u, v]))
44}
45
46pub fn pinhole_project_k<T: NabledReal>(
47    point: &ArrayView1<'_, T>,
48    intrinsics: &Array2<T>,
49) -> Result<Array1<T>, SensorError> {
50    if intrinsics.dim() != (3, 3) {
51        return Err(SensorError::DimensionMismatch);
52    }
53    let k = PinholeIntrinsics {
54        fx: intrinsics[[0, 0]],
55        fy: intrinsics[[1, 1]],
56        cx: intrinsics[[0, 2]],
57        cy: intrinsics[[1, 2]],
58    };
59    pinhole_project(point, &k)
60}
61
62pub fn pinhole_jacobian<T: NabledReal>(
63    point: &ArrayView1<'_, T>,
64    intrinsics: &PinholeIntrinsics<T>,
65) -> Result<Array2<T>, SensorError> {
66    if point.len() != 3 {
67        return Err(SensorError::DimensionMismatch);
68    }
69    let (x, y, z) = (point[0], point[1], point[2]);
70    if z <= T::from_f64(1e-12).unwrap_or(T::zero()) {
71        return Err(SensorError::InvalidInput("point behind camera".to_string()));
72    }
73    let z2 = z * z;
74    let mut j = Array2::<T>::zeros((2, 3));
75    j[[0, 0]] = intrinsics.fx / z;
76    j[[0, 2]] = -intrinsics.fx * x / z2;
77    j[[1, 1]] = intrinsics.fy / z;
78    j[[1, 2]] = -intrinsics.fy * y / z2;
79    Ok(j)
80}
81
82pub fn pinhole_jacobian_k<T: NabledReal>(
83    point: &ArrayView1<'_, T>,
84    intrinsics: &Array2<T>,
85) -> Result<Array2<T>, SensorError> {
86    if intrinsics.dim() != (3, 3) {
87        return Err(SensorError::DimensionMismatch);
88    }
89    let k = PinholeIntrinsics {
90        fx: intrinsics[[0, 0]],
91        fy: intrinsics[[1, 1]],
92        cx: intrinsics[[0, 2]],
93        cy: intrinsics[[1, 2]],
94    };
95    pinhole_jacobian(point, &k)
96}
97
98#[cfg(test)]
99mod tests {
100    use approx::assert_relative_eq;
101
102    use super::*;
103
104    #[test]
105    fn pinhole_project_centered() {
106        let k = PinholeIntrinsics { fx: 500.0, fy: 500.0, cx: 320.0, cy: 240.0 };
107        let p = ndarray::arr1(&[0.1_f64, 0.2, 1.0]);
108        let uv = pinhole_project(&p.view(), &k).unwrap();
109        assert_relative_eq!(uv[0], 370.0, epsilon = 1e-10);
110        assert_relative_eq!(uv[1], 340.0, epsilon = 1e-10);
111    }
112
113    #[test]
114    fn pinhole_jacobian_finite_diff() {
115        let k = PinholeIntrinsics { fx: 400.0, fy: 400.0, cx: 0.0, cy: 0.0 };
116        let p = ndarray::arr1(&[0.2_f64, -0.1, 2.0]);
117        let j = pinhole_jacobian(&p.view(), &k).unwrap();
118        let h = 1e-6;
119        for col in 0..3 {
120            let mut plus = p.clone();
121            plus[col] += h;
122            let mut minus = p.clone();
123            minus[col] -= h;
124            let u_plus = pinhole_project(&plus.view(), &k).unwrap();
125            let u_minus = pinhole_project(&minus.view(), &k).unwrap();
126            let deriv_u = (u_plus[0] - u_minus[0]) / (2.0 * h);
127            let deriv_v = (u_plus[1] - u_minus[1]) / (2.0 * h);
128            assert_relative_eq!(j[[0, col]], deriv_u, epsilon = 1e-5);
129            assert_relative_eq!(j[[1, col]], deriv_v, epsilon = 1e-5);
130        }
131    }
132
133    #[test]
134    fn pinhole_rejects_degenerate_intrinsics_matrix() {
135        let point = ndarray::arr1(&[0.1_f64, 0.2, 1.0]);
136        let bad_k = ndarray::arr2(&[[500.0, 0.0], [0.0, 500.0]]);
137        assert_eq!(pinhole_project_k(&point.view(), &bad_k), Err(SensorError::DimensionMismatch));
138        assert_eq!(pinhole_jacobian_k(&point.view(), &bad_k), Err(SensorError::DimensionMismatch));
139    }
140
141    #[test]
142    fn pinhole_rejects_point_behind_camera() {
143        let k = PinholeIntrinsics { fx: 500.0, fy: 500.0, cx: 320.0, cy: 240.0 };
144        let on_plane = ndarray::arr1(&[0.1_f64, 0.2, 0.0]);
145        let near_plane = ndarray::arr1(&[0.1_f64, 0.2, 1e-15]);
146        let err = SensorError::InvalidInput("point behind camera".to_string());
147        assert_eq!(pinhole_project(&on_plane.view(), &k), Err(err.clone()));
148        assert_eq!(pinhole_jacobian(&on_plane.view(), &k), Err(err.clone()));
149        assert_eq!(pinhole_project(&near_plane.view(), &k), Err(err));
150    }
151
152    #[test]
153    fn pinhole_rejects_wrong_point_dimension() {
154        let k = PinholeIntrinsics { fx: 500.0, fy: 500.0, cx: 320.0, cy: 240.0 };
155        let point = ndarray::arr1(&[1.0_f64, 2.0]);
156        assert_eq!(pinhole_project(&point.view(), &k), Err(SensorError::DimensionMismatch));
157        assert_eq!(pinhole_jacobian(&point.view(), &k), Err(SensorError::DimensionMismatch));
158    }
159}
nabled_sensor/camera.rs

nabled_sensor/
camera.rs
