vision-calibration-linear 0.3.0

Closed-form and linear initialization solvers for calibration-rs
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

//! Essential matrix decomposition into rotation and translation.
//!
//! Recovers candidate camera poses from an essential matrix using SVD
//! decomposition. Returns four possible (R, t) pairs that must be
//! disambiguated via cheirality checks.

use crate::Error;
use nalgebra::SMatrix;
use vision_calibration_core::{Mat3, Real, Vec3};

/// Enforce essential matrix constraints via SVD projection.
///
/// Projects a 3x3 matrix onto the essential matrix manifold by forcing
/// the singular values to be (σ, σ, 0) where σ is the mean of the first
/// two singular values.
pub(super) fn enforce_essential_constraints(e: &Mat3) -> Result<Mat3, Error> {
    let svd = e.svd(true, true);
    let u = svd.u.ok_or(Error::Singular)?;
    let v_t = svd.v_t.ok_or(Error::Singular)?;

    let s1 = svd.singular_values[0];
    let s2 = svd.singular_values[1];
    let s = 0.5 * (s1 + s2);

    let s_mat = SMatrix::<Real, 3, 3>::from_diagonal(&nalgebra::Vector3::new(s, s, 0.0));
    Ok(u * s_mat * v_t)
}

/// Decompose an essential matrix into candidate rotation and translation pairs.
///
/// Returns four possible `(R, t)` pairs; the correct one can be selected by
/// cheirality checks on triangulated points. The translation is unit-length
/// (direction only).
///
/// # Errors
///
/// Returns [`Error::Singular`] if the essential matrix SVD fails or the
/// essential structure constraint cannot be enforced numerically.
pub fn decompose_essential(e: &Mat3) -> Result<Vec<(Mat3, Vec3)>, Error> {
    let e = enforce_essential_constraints(e)?;
    let svd = e.svd(true, true);
    let mut u = svd.u.ok_or(Error::Singular)?;
    let mut v_t = svd.v_t.ok_or(Error::Singular)?;

    if u.determinant() < 0.0 {
        u.column_mut(2).neg_mut();
    }
    if v_t.determinant() < 0.0 {
        v_t.row_mut(2).neg_mut();
    }

    let w = Mat3::new(0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0);

    let r1 = u * w * v_t;
    let r2 = u * w.transpose() * v_t;

    let t = u.column(2).normalize();

    let mut solutions = vec![
        (r1, t.into_owned()),
        (r1, (-t).into_owned()),
        (r2, t.into_owned()),
        (r2, (-t).into_owned()),
    ];

    for (r, t) in solutions.iter_mut() {
        if r.determinant() < 0.0 {
            *r = -*r;
            *t = -*t;
        }
    }

    Ok(solutions)
}

#[cfg(test)]
mod tests {
    use super::*;
    use nalgebra::Rotation3;
    use vision_calibration_core::Vec3;

    fn skew(v: &Vec3) -> Mat3 {
        Mat3::new(0.0, -v.z, v.y, v.z, 0.0, -v.x, -v.y, v.x, 0.0)
    }

    #[test]
    fn essential_decomposition_recovers_pose() {
        let rot = Rotation3::from_euler_angles(0.1, -0.05, 0.2);
        let t = Vec3::new(0.1, 0.02, -0.03);

        let e = skew(&t) * rot.matrix();
        let solutions = decompose_essential(&e).unwrap();

        let mut found = false;
        for (r_est, t_est) in solutions {
            let r_diff = r_est.transpose() * rot.matrix();
            let trace = r_diff.trace();
            let cos_theta = ((trace - 1.0) * 0.5).clamp(-1.0, 1.0);
            let ang = cos_theta.acos();

            let t_dir = t_est.normalize();
            let cos_t = t_dir.dot(&t.normalize()).abs();

            if ang < 1e-6 && (1.0 - cos_t) < 1e-6 {
                found = true;
                break;
            }
        }

        assert!(found, "essential decomposition did not recover pose");
    }
}