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
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133

//! Linearize camera models by computing the Jacobian matrix.

use crate::{Camera, IntrinsicParametersPerspective, Points, WorldFrame};
use nalgebra::{storage::Storage, RealField, SMatrix, U1, U3};

/// Required data required for finding Jacobian of perspective camera models.
///
/// Create this with the [`new()`](struct.JacobianPerspectiveCache.html#method.new) method.
pub struct JacobianPerspectiveCache<R: RealField> {
    m: SMatrix<R, 3, 4>,
}

impl<R: RealField> JacobianPerspectiveCache<R> {
    /// Create a new `JacobianPerspectiveCache` from a `Camera` with a perspective model.
    pub fn new(cam: &Camera<R, IntrinsicParametersPerspective<R>>) -> Self {
        let m = {
            let p33 = cam.intrinsics().as_intrinsics_matrix();
            p33 * cam.extrinsics().matrix()
        };

        // flip sign if focal length < 0
        let m = if m[(0, 0)] < nalgebra::zero() { -m } else { m };

        let m = m.clone() / m[(2, 3)].clone(); // normalize

        Self { m }
    }

    /// Linearize camera model by evaluating around input point `p`.
    ///
    /// Returns Jacobian matrix `A` (shape 2x3) such that `Ao = (u,v)` where `o`
    /// is 3D world coords offset from `p` and `(u,v)` are the shift in pixel
    /// coords from the projected location of `p`. In other words, for a camera
    /// model `F(x)`, if `F(p) = (a,b)` and `F(p+o) = (a,b)
    /// + Ao = (a,b) + (u,v) = (a+u,b+v)`, this function returns `A`.
    pub fn linearize_at<STORAGE>(&self, p: &Points<WorldFrame, R, U1, STORAGE>) -> SMatrix<R, 2, 3>
    where
        STORAGE: Storage<R, U1, U3>,
    {
        let pt3d = &p.data;

        // See pinhole_jacobian_demo.py in flydra for the original source of this. It has
        // been manually factored it a bit futher.
        // https://github.com/strawlab/flydra/blob/3ab1b5843b095d73f796bf707e6680b923993899/flydra_core/sympy_demo/pinhole_jacobian_demo.py
        let x = pt3d[(0, 0)].clone();
        let y = pt3d[(0, 1)].clone();
        let z = pt3d[(0, 2)].clone();

        let p = &self.m;
        let denom = p[(2, 0)].clone() * x.clone()
            + p[(2, 1)].clone() * y.clone()
            + p[(2, 2)].clone() * z.clone()
            + p[(2, 3)].clone();
        let denom_sqrt = denom.clone().powi(-2);

        let factor_u = p[(0, 0)].clone() * x.clone()
            + p[(0, 1)].clone() * y.clone()
            + p[(0, 2)].clone() * z.clone()
            + p[(0, 3)].clone();
        let ux = -p[(2, 0)].clone() * denom_sqrt.clone() * factor_u.clone()
            + p[(0, 0)].clone() / denom.clone();
        let uy = -p[(2, 1)].clone() * denom_sqrt.clone() * factor_u.clone()
            + p[(0, 1)].clone() / denom.clone();
        let uz =
            -p[(2, 2)].clone() * denom_sqrt.clone() * factor_u + p[(0, 2)].clone() / denom.clone();

        let factor_v = p[(1, 0)].clone() * x
            + p[(1, 1)].clone() * y
            + p[(1, 2)].clone() * z
            + p[(1, 3)].clone();
        let vx = -p[(2, 0)].clone() * denom_sqrt.clone() * factor_v.clone()
            + p[(1, 0)].clone() / denom.clone();
        let vy = -p[(2, 1)].clone() * denom_sqrt.clone() * factor_v.clone()
            + p[(1, 1)].clone() / denom.clone();
        let vz = -p[(2, 2)].clone() * denom_sqrt * factor_v + p[(1, 2)].clone() / denom;

        SMatrix::<R, 2, 3>::new(ux, uy, uz, vx, vy, vz)
    }
}

#[test]
fn test_jacobian_perspective() {
    use nalgebra::{OMatrix, RowVector2, RowVector3, Unit, Vector3};

    use super::*;
    use crate::{Camera, ExtrinsicParameters, IntrinsicParametersPerspective};

    // create a perspective camera
    let params = PerspectiveParams {
        fx: 100.0,
        fy: 102.0,
        skew: 0.1,
        cx: 321.0,
        cy: 239.9,
    };

    let intrinsics: IntrinsicParametersPerspective<_> = params.into();

    let camcenter = Vector3::new(10.0, 0.0, 10.0);
    let lookat = Vector3::new(0.0, 0.0, 0.0);
    let up = Unit::new_normalize(Vector3::new(0.0, 0.0, 1.0));
    let pose = ExtrinsicParameters::from_view(&camcenter, &lookat, &up);

    let cam = Camera::new(intrinsics, pose);

    // cache the required data to compute a jacobian
    let cam_jac = JacobianPerspectiveCache::new(&cam);

    // We are going to linearize around this center 3D center point
    let center = Points::new(RowVector3::new(0.01, 0.02, 0.03));

    // We will test a 3D point at this offset from the center
    let offset = Vector3::new(0.0, 0.0, 0.01);

    // Get the 2D projection (in pixels) of our center.
    let center_projected: OMatrix<f64, U1, U2> = cam.world_to_pixel(&center).data;

    // Linearize the camera model around the center 3D point.
    let linearized_cam = cam_jac.linearize_at(&center);

    // Compute the 3D point which we now want to view.
    let new_point = Points::new(center.data + offset.transpose());

    // Get the 2D projection (in pixels) using the original, non-linear camera model
    let nonlin = cam.world_to_pixel(&new_point).data;

    // Get the 2D projection (in pixels) point with the linearized camera model
    let o = linearized_cam * offset;
    let linear_prediction = RowVector2::new(center_projected.x + o[0], center_projected.y + o[1]);

    // Check both approaches are equal
    approx::assert_relative_eq!(linear_prediction, nonlin, epsilon = nalgebra::convert(1e-4));
}