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
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
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151

use core::{
    borrow::Borrow,
    fmt::Debug,
};

use sophus_autodiff::params::IsParamsImpl;
use sophus_image::ImageSize;

use crate::{
    BrownConradyCamera,
    KannalaBrandtCamera,
    PinholeCamera,
    camera_enum::EnhancedUnifiedCamera,
    prelude::*,
};

/// Camera distortion implementation trait
pub trait IsCameraDistortionImpl<
    S: IsScalar<BATCH, DM, DN>,
    const DISTORT: usize,
    const PARAMS: usize,
    const BATCH: usize,
    const DM: usize,
    const DN: usize,
>: IsParamsImpl<S, PARAMS, BATCH, DM, DN> + Debug + Clone + Send + Sync + 'static
{
    /// identity parameters
    fn identity_params() -> S::Vector<PARAMS> {
        let mut params = S::Vector::<PARAMS>::zeros();
        *params.elem_mut(0) = S::ones();
        *params.elem_mut(1) = S::ones();
        params
    }

    /// Distortion - maps a point in the camera z=1 plane to a distorted point
    fn distort<PA, PO>(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Vector<2>
    where
        PA: Borrow<S::Vector<PARAMS>>,
        PO: Borrow<S::Vector<2>>;

    /// Undistortion - maps a distorted pixel to a point in the camera z=1 plane
    fn undistort<PA, PO>(params: PA, distorted_point: PO) -> S::Vector<2>
    where
        PA: Borrow<S::Vector<PARAMS>>,
        PO: Borrow<S::Vector<2>>;

    /// Derivative of the distortion w.r.t. the point in the camera z=1 plane
    fn dx_distort_x<PA, PO>(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Matrix<2, 2>
    where
        PA: Borrow<S::Vector<PARAMS>>,
        PO: Borrow<S::Vector<2>>;

    /// Derivative of the distortion w.r.t. the parameters
    fn dx_distort_params<PA, PO>(
        params: PA,
        proj_point_in_camera_z1_plane: PO,
    ) -> S::Matrix<2, PARAMS>
    where
        PA: Borrow<S::Vector<PARAMS>>,
        PO: Borrow<S::Vector<2>>;
}

/// Camera projection implementation trait
pub trait IsProjection<
    S: IsScalar<BATCH, DM, DN>,
    const BATCH: usize,
    const DM: usize,
    const DN: usize,
>: Debug + Clone + Send + Sync + 'static
{
    /// Projects a 3D point in the camera frame to a 2D point in the z=1 plane
    fn proj<P>(point_in_camera: P) -> S::Vector<2>
    where
        P: Borrow<S::Vector<3>>;

    /// Unprojects a 2D point in the z=1 plane to a 3D point in the camera frame
    fn unproj<P>(point_in_camera: P, extension: S) -> S::Vector<3>
    where
        P: Borrow<S::Vector<2>>;

    /// Derivative of the projection w.r.t. the point in the camera frame
    fn dx_proj_x<P>(point_in_camera: P) -> S::Matrix<2, 3>
    where
        P: Borrow<S::Vector<3>>;
}

/// Camera trait
pub trait IsCamera<
    S: IsScalar<BATCH, DM, DN> + 'static + Send + Sync,
    const BATCH: usize,
    const DM: usize,
    const DN: usize,
>
{
    /// Creates a new pinhole camera
    fn new_pinhole(params: S::Vector<4>, image_size: ImageSize) -> Self;
    /// Creates a new Kannala-Brandt camera
    fn new_kannala_brandt(params: S::Vector<8>, image_size: ImageSize) -> Self;
    /// Creates a new Brown-Conrady camera
    fn new_brown_conrady(params: S::Vector<12>, image_size: ImageSize) -> Self;
    /// Creates a new Enhanced Unified camera
    fn new_enhanced_unified(params: S::Vector<6>, image_size: ImageSize) -> Self;

    /// Returns the image size
    fn image_size(&self) -> ImageSize;

    /// Projects a 3D point in the camera frame to a pixel in the image
    fn cam_proj<P>(&self, point_in_camera: P) -> S::Vector<2>
    where
        P: Borrow<S::Vector<3>>;
    /// Unprojects a pixel in the image to a 3D point in the camera frame
    fn cam_unproj_with_z<P>(&self, pixel: P, z: S) -> S::Vector<3>
    where
        P: Borrow<S::Vector<2>>;
    /// Distortion - maps a point in the camera z=1 plane to a distorted point
    fn distort<P>(&self, proj_point_in_camera_z1_plane: P) -> S::Vector<2>
    where
        P: Borrow<S::Vector<2>>;
    /// Undistortion - maps a distorted pixel to a point in the camera z=1 plane
    fn undistort<P>(&self, pixel: P) -> S::Vector<2>
    where
        P: Borrow<S::Vector<2>>;
    /// Derivative of the distortion w.r.t. the point in the camera z=1 plane
    fn dx_distort_x<P>(&self, proj_point_in_camera_z1_plane: P) -> S::Matrix<2, 2>
    where
        P: Borrow<S::Vector<2>>;

    /// Returns the brown-conrady camera
    fn try_get_brown_conrady(self) -> Option<BrownConradyCamera<S, BATCH, DM, DN>>;

    /// Returns the kannala-brandt camera
    fn try_get_kannala_brandt(self) -> Option<KannalaBrandtCamera<S, BATCH, DM, DN>>;

    /// Returns the pinhole camera
    fn try_get_pinhole(self) -> Option<PinholeCamera<S, BATCH, DM, DN>>;

    /// Returns the enhanced unified camera
    fn try_get_enhanced_unified(self) -> Option<EnhancedUnifiedCamera<S, BATCH, DM, DN>>;
}

/// Dynamic camera trait
pub trait IsPerspectiveCamera<
    S: IsScalar<BATCH, DM, DN>,
    const BATCH: usize,
    const DM: usize,
    const DN: usize,
>
{
    /// Return the first four parameters: fx, fy, cx, cy
    fn pinhole_params(&self) -> S::Vector<4>;
}