sophus_sensor/distortions/
affine.rs

1use core::{
2    borrow::Borrow,
3    marker::PhantomData,
4};
5
6use sophus_autodiff::params::IsParamsImpl;
7
8use crate::{
9    prelude::*,
10    traits::IsCameraDistortionImpl,
11};
12
13extern crate alloc;
14
15/// Affine "distortion" implementation
16///
17/// This is not a distortion in the traditional sense, but rather a simple affine transformation.
18#[derive(Debug, Clone, Copy)]
19pub struct AffineDistortionImpl<
20    S: IsScalar<BATCH, DM, DN>,
21    const BATCH: usize,
22    const DM: usize,
23    const DN: usize,
24> {
25    phantom: PhantomData<S>,
26}
27
28impl<S: IsScalar<BATCH, DM, DN>, const BATCH: usize, const DM: usize, const DN: usize>
29    IsParamsImpl<S, 4, BATCH, DM, DN> for AffineDistortionImpl<S, BATCH, DM, DN>
30{
31    fn are_params_valid(_params: S::Vector<4>) -> S::Mask {
32        S::Mask::all_true()
33    }
34
35    fn params_examples() -> alloc::vec::Vec<S::Vector<4>> {
36        alloc::vec![S::Vector::<4>::from_f64_array([1.0, 1.0, 0.0, 0.0])]
37    }
38
39    fn invalid_params_examples() -> alloc::vec::Vec<S::Vector<4>> {
40        alloc::vec![
41            S::Vector::<4>::from_f64_array([0.0, 1.0, 0.0, 0.0]),
42            S::Vector::<4>::from_f64_array([1.0, 0.0, 0.0, 0.0]),
43        ]
44    }
45}
46
47impl<S: IsScalar<BATCH, DM, DN>, const BATCH: usize, const DM: usize, const DN: usize>
48    IsCameraDistortionImpl<S, 0, 4, BATCH, DM, DN> for AffineDistortionImpl<S, BATCH, DM, DN>
49{
50    fn distort<PA, PO>(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Vector<2>
51    where
52        PA: Borrow<S::Vector<4>>,
53        PO: Borrow<S::Vector<2>>,
54    {
55        let params = params.borrow();
56        let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow();
57        S::Vector::<2>::from_array([
58            proj_point_in_camera_z1_plane.elem(0) * params.elem(0) + params.elem(2),
59            proj_point_in_camera_z1_plane.elem(1) * params.elem(1) + params.elem(3),
60        ])
61    }
62
63    fn undistort<PA, PO>(params: PA, distorted_point: PO) -> S::Vector<2>
64    where
65        PA: Borrow<S::Vector<4>>,
66        PO: Borrow<S::Vector<2>>,
67    {
68        let params = params.borrow();
69        let distorted_point = distorted_point.borrow();
70
71        S::Vector::<2>::from_array([
72            (distorted_point.elem(0) - params.elem(2)) / params.elem(0),
73            (distorted_point.elem(1) - params.elem(3)) / params.elem(1),
74        ])
75    }
76
77    fn dx_distort_x<PA, PO>(params: PA, _proj_point_in_camera_z1_plane: PO) -> S::Matrix<2, 2>
78    where
79        PA: Borrow<S::Vector<4>>,
80        PO: Borrow<S::Vector<2>>,
81    {
82        let params = params.borrow();
83
84        S::Matrix::<2, 2>::from_array2([[params.elem(0), S::zeros()], [S::zeros(), params.elem(1)]])
85    }
86
87    fn dx_distort_params<PA, PO>(_params: PA, proj_point_in_camera_z1_plane: PO) -> S::Matrix<2, 4>
88    where
89        PA: Borrow<S::Vector<4>>,
90        PO: Borrow<S::Vector<2>>,
91    {
92        let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow();
93
94        S::Matrix::<2, 4>::from_array2([
95            [
96                proj_point_in_camera_z1_plane.elem(0),
97                S::zeros(),
98                S::ones(),
99                S::zeros(),
100            ],
101            [
102                S::zeros(),
103                proj_point_in_camera_z1_plane.elem(1),
104                S::zeros(),
105                S::ones(),
106            ],
107        ])
108    }
109}
sophus_sensor/distortions/affine.rs

sophus_sensor/distortions/
affine.rs
