vision-calibration-core 0.1.3

Core math types, camera models, and RANSAC primitives for vision-calibration
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115

use nalgebra::{Matrix3, Point2, Point3, RealField};
use serde::{Deserialize, Serialize};

/// Sensor model mapping between normalized and sensor-plane coordinates.
pub trait SensorModel<S: RealField + Copy> {
    /// Map normalized coordinates to the sensor plane.
    fn normalized_to_sensor(&self, n: &Point2<S>) -> Point2<S>;
    /// Map sensor-plane coordinates back to normalized coordinates.
    fn sensor_to_normalized(&self, s: &Point2<S>) -> Point2<S>;
}

/// Identity sensor model.
#[derive(Clone, Copy, Debug, Default, Serialize, Deserialize)]
pub struct IdentitySensor;

impl<S: RealField + Copy> SensorModel<S> for IdentitySensor {
    fn normalized_to_sensor(&self, n: &Point2<S>) -> Point2<S> {
        *n
    }

    fn sensor_to_normalized(&self, s: &Point2<S>) -> Point2<S> {
        *s
    }
}

/// Sensor model represented by a 3x3 homography.
#[derive(Clone, Debug)]
pub struct HomographySensor<S: RealField + Copy> {
    /// Homography matrix (normalized -> sensor).
    pub h: Matrix3<S>,
    /// Inverse homography matrix (sensor -> normalized).
    pub h_inv: Matrix3<S>,
}

impl<S: RealField + Copy> HomographySensor<S> {
    /// Build a homography sensor if the matrix is invertible.
    pub fn new(h: Matrix3<S>) -> Option<Self> {
        let h_inv = h.try_inverse()?;
        Some(Self { h, h_inv })
    }
}

impl<S: RealField + Copy> SensorModel<S> for HomographySensor<S> {
    fn normalized_to_sensor(&self, n: &Point2<S>) -> Point2<S> {
        dehomogenize(&(self.h * homogenize(n)))
    }

    fn sensor_to_normalized(&self, s: &Point2<S>) -> Point2<S> {
        dehomogenize(&(self.h_inv * homogenize(s)))
    }
}

/// Scheimpflug tilt parameters (OpenCV-compatible).
#[derive(Clone, Copy, Debug, Serialize, Deserialize)]
pub struct ScheimpflugParams {
    /// Tilt around X axis in radians (alias: tau_x).
    #[serde(alias = "tau_x")]
    pub tilt_x: f64,
    /// Tilt around Y axis in radians (alias: tau_y).
    #[serde(alias = "tau_y")]
    pub tilt_y: f64,
}

impl Default for ScheimpflugParams {
    fn default() -> Self {
        Self {
            tilt_x: 0.0,
            tilt_y: 0.0,
        }
    }
}

impl ScheimpflugParams {
    /// Build a homography matching OpenCV's tilted sensor model.
    ///
    /// Panics if the generated homography is not invertible.
    pub fn compile(&self) -> HomographySensor<f64> {
        let h = tilt_projection_matrix(self.tilt_x, self.tilt_y);
        let h_inv = h
            .try_inverse()
            .expect("Scheimpflug homography not invertible");
        HomographySensor { h, h_inv }
    }
}

fn homogenize<S: RealField + Copy>(p: &Point2<S>) -> Point3<S> {
    Point3::new(p.x, p.y, S::one())
}

fn dehomogenize<S: RealField + Copy>(p: &Point3<S>) -> Point2<S> {
    Point2::new(p.x / p.z, p.y / p.z)
}

fn tilt_projection_matrix(tau_x: f64, tau_y: f64) -> Matrix3<f64> {
    let (s_tx, c_tx) = tau_x.sin_cos();
    let (s_ty, c_ty) = tau_y.sin_cos();

    let rot_x = Matrix3::new(1.0, 0.0, 0.0, 0.0, c_tx, s_tx, 0.0, -s_tx, c_tx);
    let rot_y = Matrix3::new(c_ty, 0.0, -s_ty, 0.0, 1.0, 0.0, s_ty, 0.0, c_ty);
    let rot_xy = rot_y * rot_x;

    let proj_z = Matrix3::new(
        rot_xy[(2, 2)],
        0.0,
        -rot_xy[(0, 2)],
        0.0,
        rot_xy[(2, 2)],
        -rot_xy[(1, 2)],
        0.0,
        0.0,
        1.0,
    );

    proj_z * rot_xy
}