projective_grid/geometry/
mod.rs

1//! Geometry helpers shared by grid tasks.
2//!
3//! Owns the crate's homography estimators: the public [`estimate_projective`]
4//! (plain DLT returning `nalgebra::Projective2`, used for the final lattice
5//! fit and consistency check) and the [`homography`] submodule's Hartley-
6//! normalised DLT + quality (returning the [`Homography`] wrapper, used for
7//! local per-cell fits in seed, validate, and extension).
8
9pub mod homography;
10pub use homography::{
11    estimate_homography, estimate_homography_with_quality, homography_from_4pt,
12    homography_from_4pt_with_quality, Homography, HomographyQuality,
13};
14
15use nalgebra::{DMatrix, DVector, Matrix3, Point2, Projective2, Vector3};
16
17use crate::error::{GridError, Result};
18use crate::float::{lit, Float};
19
20/// Estimate a projective transform from model-plane points to image points.
21///
22/// Returns [`GridError::InsufficientEvidence`] for fewer than four
23/// correspondences and [`GridError::DegenerateGeometry`] when either point set
24/// has no two-dimensional spread or the direct linear transform cannot produce
25/// a finite homography.
26pub fn estimate_projective<F: Float>(
27    model_points: &[Point2<F>],
28    image_points: &[Point2<F>],
29) -> Result<Projective2<F>> {
30    if model_points.len() != image_points.len() {
31        return Err(GridError::InconsistentInput(format!(
32            "model/image correspondence count mismatch: model={}, image={}",
33            model_points.len(),
34            image_points.len()
35        )));
36    }
37    if model_points.len() < 4 {
38        return Err(GridError::InsufficientEvidence);
39    }
40    if !has_two_dimensional_spread(model_points) || !has_two_dimensional_spread(image_points) {
41        return Err(GridError::DegenerateGeometry);
42    }
43
44    let rows = model_points.len() * 2;
45    let mut a = DMatrix::<F>::zeros(rows, 8);
46    let mut b = DVector::<F>::zeros(rows);
47    for (idx, (src, dst)) in model_points.iter().zip(image_points).enumerate() {
48        let x = src.x;
49        let y = src.y;
50        let u = dst.x;
51        let v = dst.y;
52        let r0 = 2 * idx;
53        let r1 = r0 + 1;
54
55        a[(r0, 0)] = x;
56        a[(r0, 1)] = y;
57        a[(r0, 2)] = F::one();
58        a[(r0, 6)] = -u * x;
59        a[(r0, 7)] = -u * y;
60        b[r0] = u;
61
62        a[(r1, 3)] = x;
63        a[(r1, 4)] = y;
64        a[(r1, 5)] = F::one();
65        a[(r1, 6)] = -v * x;
66        a[(r1, 7)] = -v * y;
67        b[r1] = v;
68    }
69
70    let svd = a.svd(true, true);
71    let eps = lit::<F>(1e-12);
72    let h = svd
73        .solve(&b, eps)
74        .map_err(|_| GridError::DegenerateGeometry)?;
75    let matrix = Matrix3::new(h[0], h[1], h[2], h[3], h[4], h[5], h[6], h[7], F::one());
76    if matrix.iter().any(|x| !x.is_finite()) {
77        return Err(GridError::DegenerateGeometry);
78    }
79
80    Ok(Projective2::from_matrix_unchecked(matrix))
81}
82
83/// Apply a projective transform to a point and return `None` when the
84/// homogeneous denominator is zero or non-finite.
85pub fn apply_projective<F: Float>(
86    transform: &Projective2<F>,
87    point: Point2<F>,
88) -> Option<Point2<F>> {
89    let h = transform.matrix();
90    let p = h * Vector3::new(point.x, point.y, F::one());
91    let eps = lit::<F>(1e-12);
92    if !p.z.is_finite() || p.z.abs() <= eps {
93        return None;
94    }
95    Some(Point2::new(p.x / p.z, p.y / p.z))
96}
97
98fn has_two_dimensional_spread<F: Float>(points: &[Point2<F>]) -> bool {
99    let eps = lit::<F>(1e-8);
100    for a in 0..points.len() {
101        for b in (a + 1)..points.len() {
102            for c in (b + 1)..points.len() {
103                let ab = points[b] - points[a];
104                let ac = points[c] - points[a];
105                let cross = ab.x * ac.y - ab.y * ac.x;
106                if cross.abs() > eps {
107                    return true;
108                }
109            }
110        }
111    }
112    false
113}
114
115#[cfg(test)]
116mod tests {
117    use super::*;
118
119    #[test]
120    fn estimate_projective_recovers_translation_scale() {
121        let src = [
122            Point2::new(0.0_f64, 0.0),
123            Point2::new(1.0, 0.0),
124            Point2::new(0.0, 1.0),
125            Point2::new(1.0, 1.0),
126        ];
127        let dst = src.map(|p| Point2::new(10.0 + 2.0 * p.x, -3.0 + 3.0 * p.y));
128        let h = estimate_projective(&src, &dst).unwrap();
129        let q = apply_projective(&h, Point2::new(0.25, 0.5)).unwrap();
130        assert!((q.x - 10.5).abs() < 1e-9);
131        assert!((q.y + 1.5).abs() < 1e-9);
132    }
133
134    #[test]
135    fn estimate_projective_rejects_collinear_model_points() {
136        let src = [
137            Point2::new(0.0_f32, 0.0),
138            Point2::new(1.0, 0.0),
139            Point2::new(2.0, 0.0),
140            Point2::new(3.0, 0.0),
141        ];
142        let dst = [
143            Point2::new(0.0_f32, 0.0),
144            Point2::new(1.0, 0.0),
145            Point2::new(2.0, 0.0),
146            Point2::new(3.0, 0.0),
147        ];
148        assert_eq!(
149            estimate_projective(&src, &dst).unwrap_err(),
150            GridError::DegenerateGeometry
151        );
152    }
153}
projective_grid/geometry/mod.rs

projective_grid/geometry/
mod.rs
