projective_grid/
homography.rs

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

projective_grid/
homography.rs
