projective_grid/
homography.rs

1use nalgebra::{DMatrix, Matrix3, Point2, SMatrix, SVector, Vector3};
2
3/// A 3×3 projective homography matrix.
4///
5/// Maps 2D points between two projective planes: `p_dst ~ H * p_src`.
6#[derive(Clone, Copy, Debug, PartialEq)]
7pub struct Homography {
8    pub h: Matrix3<f64>,
9}
10
11impl Homography {
12    pub fn new(h: Matrix3<f64>) -> Self {
13        Self { h }
14    }
15
16    pub fn from_array(rows: [[f64; 3]; 3]) -> Self {
17        Self::new(Matrix3::from_row_slice(&[
18            rows[0][0], rows[0][1], rows[0][2], rows[1][0], rows[1][1], rows[1][2], rows[2][0],
19            rows[2][1], rows[2][2],
20        ]))
21    }
22
23    pub fn to_array(&self) -> [[f64; 3]; 3] {
24        [
25            [self.h[(0, 0)], self.h[(0, 1)], self.h[(0, 2)]],
26            [self.h[(1, 0)], self.h[(1, 1)], self.h[(1, 2)]],
27            [self.h[(2, 0)], self.h[(2, 1)], self.h[(2, 2)]],
28        ]
29    }
30
31    pub fn zero() -> Self {
32        Self {
33            h: Matrix3::zeros(),
34        }
35    }
36
37    /// Apply the homography to a 2D point.
38    #[inline]
39    pub fn apply(&self, p: Point2<f32>) -> Point2<f32> {
40        let v = self.h * Vector3::new(p.x as f64, p.y as f64, 1.0);
41        let w = v[2];
42        Point2::new((v[0] / w) as f32, (v[1] / w) as f32)
43    }
44
45    /// Compute the inverse homography, if the matrix is invertible.
46    pub fn inverse(&self) -> Option<Self> {
47        self.h.try_inverse().map(Self::new)
48    }
49}
50
51// ---- Hartley normalization ----
52
53fn hartley_normalization(cx: f64, cy: f64, mean_dist: f64) -> Matrix3<f64> {
54    let s = if mean_dist > 1e-12 {
55        (2.0_f64).sqrt() / mean_dist
56    } else {
57        1.0
58    };
59
60    Matrix3::<f64>::new(s, 0.0, -s * cx, 0.0, s, -s * cy, 0.0, 0.0, 1.0)
61}
62
63fn normalize_points(pts: &[Point2<f32>]) -> (Vec<Point2<f64>>, Matrix3<f64>) {
64    let n = pts.len() as f64;
65    let mut cx = 0.0;
66    let mut cy = 0.0;
67    for p in pts {
68        cx += p.x as f64;
69        cy += p.y as f64;
70    }
71    cx /= n;
72    cy /= n;
73
74    let mut mean_dist = 0.0;
75    for p in pts {
76        let dx = p.x as f64 - cx;
77        let dy = p.y as f64 - cy;
78        mean_dist += (dx * dx + dy * dy).sqrt();
79    }
80    mean_dist /= n;
81
82    let t = hartley_normalization(cx, cy, mean_dist);
83
84    let mut out = Vec::with_capacity(pts.len());
85    for p in pts {
86        let v = t * Vector3::new(p.x as f64, p.y as f64, 1.0);
87        out.push(Point2::new(v[0], v[1]));
88    }
89    (out, t)
90}
91
92fn normalize_points4(pts: &[Point2<f32>; 4]) -> ([Point2<f64>; 4], Matrix3<f64>) {
93    let n = 4.0_f64;
94    let mut cx = 0.0_f64;
95    let mut cy = 0.0_f64;
96    for p in pts {
97        cx += p.x as f64;
98        cy += p.y as f64;
99    }
100    cx /= n;
101    cy /= n;
102
103    let mut mean_dist = 0.0_f64;
104    for p in pts {
105        let dx = p.x as f64 - cx;
106        let dy = p.y as f64 - cy;
107        mean_dist += (dx * dx + dy * dy).sqrt();
108    }
109    mean_dist /= n;
110
111    let t = hartley_normalization(cx, cy, mean_dist);
112
113    let mut out = [Point2::new(0.0_f64, 0.0_f64); 4];
114    for (i, p) in pts.iter().enumerate() {
115        let v = t * Vector3::new(p.x as f64, p.y as f64, 1.0);
116        out[i] = Point2::new(v[0], v[1]);
117    }
118
119    (out, t)
120}
121
122fn normalize_homography(h: Matrix3<f64>) -> Option<Matrix3<f64>> {
123    let s = h[(2, 2)];
124    if s.abs() < 1e-12 {
125        return None;
126    }
127    Some(h / s)
128}
129
130fn denormalize_homography(
131    hn: Matrix3<f64>,
132    t_src: Matrix3<f64>,
133    t_dst: Matrix3<f64>,
134) -> Option<Matrix3<f64>> {
135    let t_dst_inv = t_dst.try_inverse()?;
136    Some(t_dst_inv * hn * t_src)
137}
138
139/// Estimate H such that `p_dst ~ H * p_src` from N >= 4 point correspondences.
140///
141/// Uses Hartley normalization + DLT for N > 4 and a direct 4-point solver for N == 4.
142pub fn estimate_homography(src_pts: &[Point2<f32>], dst_pts: &[Point2<f32>]) -> Option<Homography> {
143    if src_pts.len() != dst_pts.len() || src_pts.len() < 4 {
144        return None;
145    }
146
147    if src_pts.len() == 4 {
148        let src: &[Point2<f32>; 4] = src_pts.try_into().ok()?;
149        let dst: &[Point2<f32>; 4] = dst_pts.try_into().ok()?;
150        return homography_from_4pt(src, dst);
151    }
152
153    let (r, tr) = normalize_points(src_pts);
154    let (im, ti) = normalize_points(dst_pts);
155
156    let n = src_pts.len();
157    let rows = 2 * n;
158    let mut a = DMatrix::<f64>::zeros(rows, 9);
159
160    for k in 0..n {
161        let x = r[k].x;
162        let y = r[k].y;
163        let u = im[k].x;
164        let v = im[k].y;
165
166        a[(2 * k, 0)] = -x;
167        a[(2 * k, 1)] = -y;
168        a[(2 * k, 2)] = -1.0;
169        a[(2 * k, 6)] = u * x;
170        a[(2 * k, 7)] = u * y;
171        a[(2 * k, 8)] = u;
172
173        a[(2 * k + 1, 3)] = -x;
174        a[(2 * k + 1, 4)] = -y;
175        a[(2 * k + 1, 5)] = -1.0;
176        a[(2 * k + 1, 6)] = v * x;
177        a[(2 * k + 1, 7)] = v * y;
178        a[(2 * k + 1, 8)] = v;
179    }
180
181    let svd = a.svd(true, true);
182    let vt = svd.v_t?;
183    let last = vt.nrows().checked_sub(1)?;
184    let h = vt.row(last);
185
186    let hn =
187        Matrix3::<f64>::from_row_slice(&[h[0], h[1], h[2], h[3], h[4], h[5], h[6], h[7], h[8]]);
188
189    let h_den = denormalize_homography(hn, tr, ti)?;
190    let h_den = normalize_homography(h_den)?;
191
192    Some(Homography::new(h_den))
193}
194
195/// Compute H from exactly 4 point correspondences: `dst ~ H * src`.
196///
197/// Uses Hartley normalization for numerical stability.
198pub fn homography_from_4pt(src: &[Point2<f32>; 4], dst: &[Point2<f32>; 4]) -> Option<Homography> {
199    let (src_n, t_src) = normalize_points4(src);
200    let (dst_n, t_dst) = normalize_points4(dst);
201
202    let mut a = SMatrix::<f64, 8, 8>::zeros();
203    let mut b = SVector::<f64, 8>::zeros();
204
205    for k in 0..4 {
206        let x = src_n[k].x;
207        let y = src_n[k].y;
208        let u = dst_n[k].x;
209        let v = dst_n[k].y;
210
211        let r0 = 2 * k;
212        a[(r0, 0)] = x;
213        a[(r0, 1)] = y;
214        a[(r0, 2)] = 1.0;
215        a[(r0, 6)] = -u * x;
216        a[(r0, 7)] = -u * y;
217        b[r0] = u;
218
219        let r1 = 2 * k + 1;
220        a[(r1, 3)] = x;
221        a[(r1, 4)] = y;
222        a[(r1, 5)] = 1.0;
223        a[(r1, 6)] = -v * x;
224        a[(r1, 7)] = -v * y;
225        b[r1] = v;
226    }
227
228    let x = a.lu().solve(&b)?;
229
230    let hn = Matrix3::<f64>::new(
231        x[0], x[1], x[2], //
232        x[3], x[4], x[5], //
233        x[6], x[7], 1.0,
234    );
235
236    let h_den = denormalize_homography(hn, t_src, t_dst)?;
237    let h_den = normalize_homography(h_den)?;
238
239    Some(Homography::new(h_den))
240}
241
242#[cfg(test)]
243mod tests {
244    use super::*;
245
246    fn assert_close(a: Point2<f32>, b: Point2<f32>, tol: f32) {
247        let dx = (a.x - b.x).abs();
248        let dy = (a.y - b.y).abs();
249        assert!(
250            dx < tol && dy < tol,
251            "expected ({:.6},{:.6}) ~ ({:.6},{:.6}) within {}",
252            a.x,
253            a.y,
254            b.x,
255            b.y,
256            tol
257        );
258    }
259
260    #[test]
261    fn inverse_round_trips_points() {
262        let h = Homography::new(Matrix3::new(
263            1.2, 0.1, 5.0, //
264            -0.05, 0.9, 3.0, //
265            0.001, 0.0005, 1.0,
266        ));
267        let inv = h.inverse().expect("invertible");
268
269        for p in [
270            Point2::new(0.0_f32, 0.0),
271            Point2::new(50.0_f32, -20.0),
272            Point2::new(320.0_f32, 200.0),
273        ] {
274            let q = h.apply(p);
275            let back = inv.apply(q);
276            assert_close(back, p, 1e-3);
277        }
278    }
279
280    #[test]
281    fn four_point_specialization_recovers_h() {
282        let ground_truth = Homography::new(Matrix3::new(
283            0.8, 0.05, 120.0, //
284            -0.02, 1.1, 80.0, //
285            0.0009, -0.0004, 1.0,
286        ));
287
288        let rect = [
289            Point2::new(0.0_f32, 0.0),
290            Point2::new(180.0_f32, 0.0),
291            Point2::new(180.0_f32, 130.0),
292            Point2::new(0.0_f32, 130.0),
293        ];
294        let dst = rect.map(|p| ground_truth.apply(p));
295
296        let recovered = homography_from_4pt(&rect, &dst).expect("recoverable");
297
298        for p in [
299            Point2::new(0.0_f32, 0.0),
300            Point2::new(60.0, 40.0),
301            Point2::new(150.0, 120.0),
302        ] {
303            assert_close(recovered.apply(p), ground_truth.apply(p), 1e-3);
304        }
305    }
306
307    #[test]
308    fn dlt_handles_overdetermined_case() {
309        let ground_truth = Homography::new(Matrix3::new(
310            1.0, 0.2, 12.0, //
311            -0.1, 0.9, 6.0, //
312            0.0006, 0.0004, 1.0,
313        ));
314
315        let rect: Vec<Point2<f32>> = (0..3)
316            .flat_map(|y| (0..3).map(move |x| Point2::new(x as f32 * 40.0, y as f32 * 50.0)))
317            .collect();
318        let img: Vec<Point2<f32>> = rect.iter().map(|&p| ground_truth.apply(p)).collect();
319
320        let estimated = estimate_homography(&rect, &img).expect("estimate");
321        for p in [
322            Point2::new(0.0_f32, 0.0),
323            Point2::new(60.0, 40.0),
324            Point2::new(80.0, 90.0),
325            Point2::new(80.0, 100.0),
326        ] {
327            assert_close(estimated.apply(p), ground_truth.apply(p), 1e-3);
328        }
329    }
330
331    #[test]
332    fn mismatched_input_lengths_fail() {
333        let rect = [Point2::new(0.0_f32, 0.0); 4];
334        let img = [Point2::new(1.0_f32, 1.0); 3];
335        assert!(estimate_homography(&rect, &img).is_none());
336    }
337}
projective_grid/homography.rs

projective_grid/
homography.rs
