cam_geom/
linearize.rs

1//! Linearize camera models by computing the Jacobian matrix.
2
3use crate::{Camera, IntrinsicParametersPerspective, Points, WorldFrame};
4use nalgebra::{storage::Storage, RealField, SMatrix, U1, U3};
5
6/// Required data required for finding Jacobian of perspective camera models.
7///
8/// Create this with the [`new()`](struct.JacobianPerspectiveCache.html#method.new) method.
9pub struct JacobianPerspectiveCache<R: RealField> {
10    m: SMatrix<R, 3, 4>,
11}
12
13impl<R: RealField> JacobianPerspectiveCache<R> {
14    /// Create a new `JacobianPerspectiveCache` from a `Camera` with a perspective model.
15    pub fn new(cam: &Camera<R, IntrinsicParametersPerspective<R>>) -> Self {
16        let m = {
17            let p33 = cam.intrinsics().as_intrinsics_matrix();
18            p33 * cam.extrinsics().matrix()
19        };
20
21        // flip sign if focal length < 0
22        let m = if m[(0, 0)] < nalgebra::zero() { -m } else { m };
23
24        let m = m.clone() / m[(2, 3)].clone(); // normalize
25
26        Self { m }
27    }
28
29    /// Linearize camera model by evaluating around input point `p`.
30    ///
31    /// Returns Jacobian matrix `A` (shape 2x3) such that `Ao = (u,v)` where `o`
32    /// is 3D world coords offset from `p` and `(u,v)` are the shift in pixel
33    /// coords from the projected location of `p`. In other words, for a camera
34    /// model `F(x)`, if `F(p) = (a,b)` and `F(p+o) = (a,b)
35    /// + Ao = (a,b) + (u,v) = (a+u,b+v)`, this function returns `A`.
36    pub fn linearize_at<STORAGE>(&self, p: &Points<WorldFrame, R, U1, STORAGE>) -> SMatrix<R, 2, 3>
37    where
38        STORAGE: Storage<R, U1, U3>,
39    {
40        let pt3d = &p.data;
41
42        // See pinhole_jacobian_demo.py in flydra for the original source of this. It has
43        // been manually factored it a bit futher.
44        // https://github.com/strawlab/flydra/blob/3ab1b5843b095d73f796bf707e6680b923993899/flydra_core/sympy_demo/pinhole_jacobian_demo.py
45        let x = pt3d[(0, 0)].clone();
46        let y = pt3d[(0, 1)].clone();
47        let z = pt3d[(0, 2)].clone();
48
49        let p = &self.m;
50        let denom = p[(2, 0)].clone() * x.clone()
51            + p[(2, 1)].clone() * y.clone()
52            + p[(2, 2)].clone() * z.clone()
53            + p[(2, 3)].clone();
54        let denom_sqrt = denom.clone().powi(-2);
55
56        let factor_u = p[(0, 0)].clone() * x.clone()
57            + p[(0, 1)].clone() * y.clone()
58            + p[(0, 2)].clone() * z.clone()
59            + p[(0, 3)].clone();
60        let ux = -p[(2, 0)].clone() * denom_sqrt.clone() * factor_u.clone()
61            + p[(0, 0)].clone() / denom.clone();
62        let uy = -p[(2, 1)].clone() * denom_sqrt.clone() * factor_u.clone()
63            + p[(0, 1)].clone() / denom.clone();
64        let uz =
65            -p[(2, 2)].clone() * denom_sqrt.clone() * factor_u + p[(0, 2)].clone() / denom.clone();
66
67        let factor_v = p[(1, 0)].clone() * x
68            + p[(1, 1)].clone() * y
69            + p[(1, 2)].clone() * z
70            + p[(1, 3)].clone();
71        let vx = -p[(2, 0)].clone() * denom_sqrt.clone() * factor_v.clone()
72            + p[(1, 0)].clone() / denom.clone();
73        let vy = -p[(2, 1)].clone() * denom_sqrt.clone() * factor_v.clone()
74            + p[(1, 1)].clone() / denom.clone();
75        let vz = -p[(2, 2)].clone() * denom_sqrt * factor_v + p[(1, 2)].clone() / denom;
76
77        SMatrix::<R, 2, 3>::new(ux, uy, uz, vx, vy, vz)
78    }
79}
80
81#[test]
82fn test_jacobian_perspective() {
83    use nalgebra::{OMatrix, RowVector2, RowVector3, Unit, Vector3};
84
85    use super::*;
86    use crate::{Camera, ExtrinsicParameters, IntrinsicParametersPerspective};
87
88    // create a perspective camera
89    let params = PerspectiveParams {
90        fx: 100.0,
91        fy: 102.0,
92        skew: 0.1,
93        cx: 321.0,
94        cy: 239.9,
95    };
96
97    let intrinsics: IntrinsicParametersPerspective<_> = params.into();
98
99    let camcenter = Vector3::new(10.0, 0.0, 10.0);
100    let lookat = Vector3::new(0.0, 0.0, 0.0);
101    let up = Unit::new_normalize(Vector3::new(0.0, 0.0, 1.0));
102    let pose = ExtrinsicParameters::from_view(&camcenter, &lookat, &up);
103
104    let cam = Camera::new(intrinsics, pose);
105
106    // cache the required data to compute a jacobian
107    let cam_jac = JacobianPerspectiveCache::new(&cam);
108
109    // We are going to linearize around this center 3D center point
110    let center = Points::new(RowVector3::new(0.01, 0.02, 0.03));
111
112    // We will test a 3D point at this offset from the center
113    let offset = Vector3::new(0.0, 0.0, 0.01);
114
115    // Get the 2D projection (in pixels) of our center.
116    let center_projected: OMatrix<f64, U1, U2> = cam.world_to_pixel(&center).data;
117
118    // Linearize the camera model around the center 3D point.
119    let linearized_cam = cam_jac.linearize_at(&center);
120
121    // Compute the 3D point which we now want to view.
122    let new_point = Points::new(center.data + offset.transpose());
123
124    // Get the 2D projection (in pixels) using the original, non-linear camera model
125    let nonlin = cam.world_to_pixel(&new_point).data;
126
127    // Get the 2D projection (in pixels) point with the linearized camera model
128    let o = linearized_cam * offset;
129    let linear_prediction = RowVector2::new(center_projected.x + o[0], center_projected.y + o[1]);
130
131    // Check both approaches are equal
132    approx::assert_relative_eq!(linear_prediction, nonlin, epsilon = nalgebra::convert(1e-4));
133}
cam_geom/linearize.rs

cam_geom/
linearize.rs
