vision_calibration_core/models/
sensor.rs

1use nalgebra::{Matrix3, Point2, Point3, RealField};
2use serde::{Deserialize, Serialize};
3
4/// Sensor model mapping between normalized and sensor-plane coordinates.
5pub trait SensorModel<S: RealField + Copy> {
6    /// Map normalized coordinates to the sensor plane.
7    fn normalized_to_sensor(&self, n: &Point2<S>) -> Point2<S>;
8    /// Map sensor-plane coordinates back to normalized coordinates.
9    fn sensor_to_normalized(&self, s: &Point2<S>) -> Point2<S>;
10}
11
12/// Identity sensor model.
13#[derive(Clone, Copy, Debug, Default, Serialize, Deserialize)]
14pub struct IdentitySensor;
15
16impl<S: RealField + Copy> SensorModel<S> for IdentitySensor {
17    fn normalized_to_sensor(&self, n: &Point2<S>) -> Point2<S> {
18        *n
19    }
20
21    fn sensor_to_normalized(&self, s: &Point2<S>) -> Point2<S> {
22        *s
23    }
24}
25
26/// Sensor model represented by a 3x3 homography.
27#[derive(Clone, Debug)]
28pub struct HomographySensor<S: RealField + Copy> {
29    /// Homography matrix (normalized -> sensor).
30    pub h: Matrix3<S>,
31    /// Inverse homography matrix (sensor -> normalized).
32    pub h_inv: Matrix3<S>,
33}
34
35impl<S: RealField + Copy> HomographySensor<S> {
36    /// Build a homography sensor if the matrix is invertible.
37    pub fn new(h: Matrix3<S>) -> Option<Self> {
38        let h_inv = h.try_inverse()?;
39        Some(Self { h, h_inv })
40    }
41}
42
43impl<S: RealField + Copy> SensorModel<S> for HomographySensor<S> {
44    fn normalized_to_sensor(&self, n: &Point2<S>) -> Point2<S> {
45        dehomogenize(&(self.h * homogenize(n)))
46    }
47
48    fn sensor_to_normalized(&self, s: &Point2<S>) -> Point2<S> {
49        dehomogenize(&(self.h_inv * homogenize(s)))
50    }
51}
52
53/// Scheimpflug tilt parameters (OpenCV-compatible).
54#[derive(Clone, Copy, Debug, Serialize, Deserialize)]
55pub struct ScheimpflugParams {
56    /// Tilt around X axis in radians (alias: tau_x).
57    #[serde(alias = "tau_x")]
58    pub tilt_x: f64,
59    /// Tilt around Y axis in radians (alias: tau_y).
60    #[serde(alias = "tau_y")]
61    pub tilt_y: f64,
62}
63
64impl Default for ScheimpflugParams {
65    fn default() -> Self {
66        Self {
67            tilt_x: 0.0,
68            tilt_y: 0.0,
69        }
70    }
71}
72
73impl ScheimpflugParams {
74    /// Build a homography matching OpenCV's tilted sensor model.
75    ///
76    /// Panics if the generated homography is not invertible.
77    pub fn compile(&self) -> HomographySensor<f64> {
78        let h = tilt_projection_matrix(self.tilt_x, self.tilt_y);
79        let h_inv = h
80            .try_inverse()
81            .expect("Scheimpflug homography not invertible");
82        HomographySensor { h, h_inv }
83    }
84}
85
86fn homogenize<S: RealField + Copy>(p: &Point2<S>) -> Point3<S> {
87    Point3::new(p.x, p.y, S::one())
88}
89
90fn dehomogenize<S: RealField + Copy>(p: &Point3<S>) -> Point2<S> {
91    Point2::new(p.x / p.z, p.y / p.z)
92}
93
94fn tilt_projection_matrix(tau_x: f64, tau_y: f64) -> Matrix3<f64> {
95    let (s_tx, c_tx) = tau_x.sin_cos();
96    let (s_ty, c_ty) = tau_y.sin_cos();
97
98    let rot_x = Matrix3::new(1.0, 0.0, 0.0, 0.0, c_tx, s_tx, 0.0, -s_tx, c_tx);
99    let rot_y = Matrix3::new(c_ty, 0.0, -s_ty, 0.0, 1.0, 0.0, s_ty, 0.0, c_ty);
100    let rot_xy = rot_y * rot_x;
101
102    let proj_z = Matrix3::new(
103        rot_xy[(2, 2)],
104        0.0,
105        -rot_xy[(0, 2)],
106        0.0,
107        rot_xy[(2, 2)],
108        -rot_xy[(1, 2)],
109        0.0,
110        0.0,
111        1.0,
112    );
113
114    proj_z * rot_xy
115}
vision_calibration_core/models/sensor.rs

vision_calibration_core/models/
sensor.rs
