sophus_sensor 0.15.0

Camera models for computer vision
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

use core::borrow::Borrow;

use sophus_autodiff::{
    linalg::{
        IsMatrix,
        IsScalar,
        IsVector,
    },
    prelude::IsSingleScalar,
};

use crate::traits::IsProjection;

/// Perspective camera projection - using z=1 plane
///
/// Projects a 3D point in the camera frame to a 2D point in the z=1 plane
#[derive(Debug, Clone, Copy)]
pub struct PerspectiveProjectionImpl<
    S: IsScalar<BATCH, DM, DN>,
    const BATCH: usize,
    const DM: usize,
    const DN: usize,
> {
    phantom: core::marker::PhantomData<S>,
}

impl<S: IsScalar<BATCH, DM, DN>, const BATCH: usize, const DM: usize, const DN: usize>
    IsProjection<S, BATCH, DM, DN> for PerspectiveProjectionImpl<S, BATCH, DM, DN>
{
    fn proj<P>(point_in_camera: P) -> S::Vector<2>
    where
        P: Borrow<S::Vector<3>>,
    {
        let point_in_camera = point_in_camera.borrow();
        S::Vector::<2>::from_array([
            point_in_camera.elem(0) / point_in_camera.elem(2),
            point_in_camera.elem(1) / point_in_camera.elem(2),
        ])
    }

    fn unproj<P>(point_in_camera: P, extension: S) -> S::Vector<3>
    where
        P: Borrow<S::Vector<2>>,
    {
        let point_in_camera = point_in_camera.borrow();

        S::Vector::<3>::from_array([
            point_in_camera.elem(0) * extension,
            point_in_camera.elem(1) * extension,
            extension,
        ])
    }

    fn dx_proj_x<P>(point_in_camera: P) -> S::Matrix<2, 3>
    where
        P: Borrow<S::Vector<3>>,
    {
        let point_in_camera = point_in_camera.borrow();

        S::Matrix::<2, 3>::from_array2([
            [
                S::ones() / point_in_camera.elem(2),
                S::zeros(),
                -point_in_camera.elem(0) / (point_in_camera.elem(2) * point_in_camera.elem(2)),
            ],
            [
                S::zeros(),
                S::ones() / point_in_camera.elem(2),
                -point_in_camera.elem(1) / (point_in_camera.elem(2) * point_in_camera.elem(2)),
            ],
        ])
    }
}

/// Perspective projection for single scalar
pub fn perspect_proj<
    S: IsSingleScalar<DM, DN> + 'static + Send + Sync,
    const DM: usize,
    const DN: usize,
>(
    point_in_camera: &S::Vector<3>,
) -> S::Vector<2> {
    PerspectiveProjectionImpl::<S, 1, DM, DN>::proj(point_in_camera)
}