checkerboard_calibrate/calibrate/
extrinsics.rs

1// Copyright (C) The Strand-Braid Authors
2// SPDX-License-Identifier: MIT OR Apache-2.0
3
4//! Per-view extrinsics (rotation + translation) from a homography and known
5//! intrinsics.
6//!
7//! Decomposes `K^{-1} H = λ [r1 r2 t]` for a planar target: the first two
8//! columns give the first two rotation columns (after scaling so they are
9//! unit-norm), their cross product gives the third, and the result is projected
10//! onto `SO(3)` by SVD. The sign is chosen so the target lies in front of the
11//! camera (`t_z > 0`). This is an initializer for LM refinement.
12
13use nalgebra::{Matrix3, Rotation3, Vector3};
14
15/// Camera pose of the target plane relative to the camera.
16#[derive(Clone, Copy, Debug)]
17pub struct Extrinsics {
18    pub rotation: Rotation3<f64>,
19    pub translation: Vector3<f64>,
20}
21
22/// Recover the pose for one view from intrinsics `k` and homography `h`.
23///
24/// Returns `None` if `k` is singular or the homography is degenerate.
25pub fn init_extrinsics(k: &Matrix3<f64>, h: &Matrix3<f64>) -> Option<Extrinsics> {
26    let kinv = k.try_inverse()?;
27
28    let kh1 = kinv * h.column(0);
29    let kh2 = kinv * h.column(1);
30    let kh3 = kinv * h.column(2);
31
32    let n1 = kh1.norm();
33    if n1 < f64::EPSILON {
34        return None;
35    }
36    // Average the two column norms for a slightly more stable scale, matching
37    // the spirit of OpenCV's decomposition.
38    let lambda = 2.0 / (n1 + kh2.norm());
39
40    let mut r1 = lambda * kh1;
41    let mut r2 = lambda * kh2;
42    let mut t = lambda * kh3;
43
44    // Put the target in front of the camera.
45    if t.z < 0.0 {
46        r1 = -r1;
47        r2 = -r2;
48        t = -t;
49    }
50    let r3 = r1.cross(&r2);
51
52    // Project [r1 r2 r3] onto SO(3).
53    let mut q = Matrix3::zeros();
54    q.set_column(0, &r1);
55    q.set_column(1, &r2);
56    q.set_column(2, &r3);
57
58    let svd = q.svd(true, true);
59    let u = svd.u?;
60    let v_t = svd.v_t?;
61    let mut rot = u * v_t;
62    if rot.determinant() < 0.0 {
63        // Flip the sign of the last U column to keep a right-handed rotation.
64        let mut u2 = u;
65        let last = u2.ncols() - 1;
66        let col = -u2.column(last);
67        u2.set_column(last, &col);
68        rot = u2 * v_t;
69    }
70
71    let rotation = Rotation3::from_matrix_unchecked(rot);
72    Some(Extrinsics {
73        rotation,
74        translation: t,
75    })
76}
77
78#[cfg(test)]
79mod tests {
80    use super::*;
81
82    fn homography_for_view(k: &Matrix3<f64>, r: &Rotation3<f64>, t: &Vector3<f64>) -> Matrix3<f64> {
83        let rm = r.matrix();
84        let mut m = Matrix3::zeros();
85        m.set_column(0, &rm.column(0));
86        m.set_column(1, &rm.column(1));
87        m.set_column(2, t);
88        k * m
89    }
90
91    #[test]
92    fn recovers_pose() {
93        let k = Matrix3::new(520.0, 0.0, 319.5, 0.0, 510.0, 239.5, 0.0, 0.0, 1.0);
94        let r_true = Rotation3::from_euler_angles(0.12, -0.22, 0.07);
95        let t_true = Vector3::new(-1.3, 0.6, 8.0);
96
97        let h = homography_for_view(&k, &r_true, &t_true);
98        let ext = init_extrinsics(&k, &h).expect("extrinsics");
99
100        approx::assert_abs_diff_eq!(ext.translation, t_true, epsilon = 1e-9);
101        approx::assert_abs_diff_eq!(ext.rotation.matrix(), r_true.matrix(), epsilon = 1e-9);
102    }
103}
checkerboard_calibrate/calibrate/extrinsics.rs

checkerboard_calibrate/calibrate/
extrinsics.rs
