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

//! Linear triangulation of 3D points from multiple views.
//!
//! Uses a DLT formulation on the camera projection matrices and image points.

use crate::Error;
use nalgebra::DMatrix;
use vision_calibration_core::{Pt2, Pt3, Real};

use crate::camera_matrix::Mat34;

/// Linear triangulation from multiple views using DLT.
///
/// `cameras` are projection matrices `P_i`, and `points` are their corresponding
/// pixel coordinates. The returned 3D point is in the same world frame as the
/// camera matrices.
///
/// # Errors
///
/// Returns [`Error::InsufficientData`] if fewer than 2 views are given,
/// [`Error::InvalidInput`] if camera and point counts differ,
/// [`Error::Singular`] if SVD fails, or [`Error::Numerical`] if the
/// triangulated homogeneous weight is zero.
pub fn triangulate_point_linear(cameras: &[Mat34], points: &[Pt2]) -> Result<Pt3, Error> {
    if cameras.len() < 2 {
        return Err(Error::InsufficientData {
            need: 2,
            got: cameras.len(),
        });
    }
    if cameras.len() != points.len() {
        return Err(Error::invalid_input(format!(
            "mismatched number of cameras ({}) and points ({})",
            cameras.len(),
            points.len()
        )));
    }

    let mut a = DMatrix::<Real>::zeros(2 * cameras.len(), 4);
    for (i, (p, cam)) in points.iter().zip(cameras.iter()).enumerate() {
        let u = p.x;
        let v = p.y;

        let r0 = 2 * i;
        let r1 = 2 * i + 1;

        let row0 = cam.row(0);
        let row1 = cam.row(1);
        let row2 = cam.row(2);

        a.row_mut(r0).copy_from(&(u * row2 - row0));
        a.row_mut(r1).copy_from(&(v * row2 - row1));
    }

    let svd = a.svd(true, true);
    let v_t = svd.v_t.ok_or(Error::Singular)?;
    let x_h = v_t.row(v_t.nrows() - 1);

    let w = x_h[3];
    if w.abs() <= Real::EPSILON {
        return Err(Error::numerical("triangulation produced an invalid point"));
    }

    let x = x_h[0] / w;
    let y = x_h[1] / w;
    let z = x_h[2] / w;

    Ok(Pt3::new(x, y, z))
}

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

    fn project(cam: &Mat34, p: &Pt3) -> Pt2 {
        let x = cam * Vector4::new(p.x, p.y, p.z, 1.0);
        Pt2::new(x.x / x.z, x.y / x.z)
    }

    #[test]
    fn triangulation_two_views_recovers_point() {
        let cam1 = Mat34::new(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
        let cam2 = Mat34::new(1.0, 0.0, 0.0, -0.2, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);

        let pw = Pt3::new(0.1, -0.05, 2.0);
        let p1 = project(&cam1, &pw);
        let p2 = project(&cam2, &pw);

        let est = triangulate_point_linear(&[cam1, cam2], &[p1, p2]).unwrap();

        let err = (est - pw).norm();
        assert!(err < 1e-6, "triangulation error too large: {}", err);
    }
}