sophus_sensor/distortions/
enhanced_unified.rs

1use core::{
2    borrow::Borrow,
3    marker::PhantomData,
4};
5
6use sophus_autodiff::{
7    params::IsParamsImpl,
8    prelude::{
9        IsMatrix,
10        IsScalar,
11        IsVector,
12    },
13};
14
15use crate::{
16    prelude::*,
17    traits::IsCameraDistortionImpl,
18};
19
20extern crate alloc;
21
22/// Enhanced unified distortion implementation
23#[derive(Debug, Clone, Copy)]
24pub struct EnhancedUnifiedDistortionImpl<
25    S: IsScalar<BATCH, DM, DN>,
26    const BATCH: usize,
27    const DM: usize,
28    const DN: usize,
29> {
30    phantom: PhantomData<S>,
31}
32
33impl<S: IsScalar<BATCH, DM, DN>, const BATCH: usize, const DM: usize, const DN: usize>
34    IsParamsImpl<S, 6, BATCH, DM, DN> for EnhancedUnifiedDistortionImpl<S, BATCH, DM, DN>
35{
36    fn are_params_valid(_params: S::Vector<6>) -> S::Mask {
37        S::Mask::all_true()
38    }
39
40    fn params_examples() -> alloc::vec::Vec<S::Vector<6>> {
41        alloc::vec![S::Vector::<6>::from_f64_array([
42            1.0, 1.0, 0.0, 0.0, 0.0, 0.0,
43        ])]
44    }
45
46    fn invalid_params_examples() -> alloc::vec::Vec<S::Vector<6>> {
47        alloc::vec![
48            S::Vector::<6>::from_f64_array([0.0, 1.0, 0.0, 0.0, 0.0, 0.0]),
49            S::Vector::<6>::from_f64_array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
50        ]
51    }
52}
53
54/// enhanced unified for z==1
55impl<S: IsScalar<BATCH, DM, DN>, const BATCH: usize, const DM: usize, const DN: usize>
56    IsCameraDistortionImpl<S, 2, 6, BATCH, DM, DN>
57    for EnhancedUnifiedDistortionImpl<S, BATCH, DM, DN>
58{
59    fn distort<PA, PO>(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Vector<2>
60    where
61        PA: Borrow<S::Vector<6>>,
62        PO: Borrow<S::Vector<2>>,
63    {
64        let params = params.borrow();
65        let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow();
66
67        // https://gitlab.com/VladyslavUsenko/basalt-headers/-/blob/master/include/basalt/camera/extended_camera.hpp?ref_type=heads#L125
68        let fx = params.elem(0);
69        let fy = params.elem(1);
70        let cx = params.elem(2);
71        let cy = params.elem(3);
72        let alpha = params.elem(4);
73        let beta = params.elem(5);
74
75        let x = proj_point_in_camera_z1_plane.elem(0);
76        let y = proj_point_in_camera_z1_plane.elem(1);
77
78        let r2 = x * x + y * y;
79        let rho2 = beta * r2 + S::from_f64(1.0);
80        let rho = rho2.sqrt();
81
82        let norm = alpha * rho + (S::from_f64(1.0) - alpha);
83
84        let mx = x / norm;
85        let my = y / norm;
86
87        S::Vector::<2>::from_array([fx * mx + cx, fy * my + cy])
88    }
89
90    fn undistort<PA, PO>(params: PA, distorted_point: PO) -> S::Vector<2>
91    where
92        PA: Borrow<S::Vector<6>>,
93        PO: Borrow<S::Vector<2>>,
94    {
95        let params = params.borrow();
96        let distorted_point = distorted_point.borrow();
97        let fx = params.elem(0);
98        let fy = params.elem(1);
99        let cx = params.elem(2);
100        let cy = params.elem(3);
101        let alpha = params.elem(4);
102        let beta = params.elem(5);
103
104        let mx = (distorted_point.elem(0) - cx) / fx;
105        let my = (distorted_point.elem(1) - cy) / fy;
106
107        let r2 = mx * mx + my * my;
108        let gamma = S::from_f64(1.0) - alpha;
109
110        let nominator = S::from_f64(1.0) - alpha * alpha * beta * r2;
111        let denominator = alpha * (S::from_f64(1.0) - (alpha - gamma) * beta * r2).sqrt() + gamma;
112
113        let k = nominator / denominator;
114
115        S::Vector::<2>::from_array([mx / k, my / k])
116    }
117    fn dx_distort_x<PA, PO>(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Matrix<2, 2>
118    where
119        PA: Borrow<S::Vector<6>>,
120        PO: Borrow<S::Vector<2>>,
121    {
122        let params = params.borrow();
123        let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow();
124
125        let fx = params.elem(0);
126        let fy = params.elem(1);
127        let alpha = params.elem(4);
128        let beta = params.elem(5);
129
130        let x = proj_point_in_camera_z1_plane.elem(0);
131        let y = proj_point_in_camera_z1_plane.elem(1);
132
133        let r2 = x * x + y * y;
134        let rho2 = beta * r2 + S::from_f64(1.0);
135        let rho = rho2.sqrt();
136
137        let norm = alpha * rho + (S::from_f64(1.0) - alpha);
138
139        // Compute partial derivatives
140        let drho2_dx = S::from_f64(2.0) * beta * x;
141        let drho2_dy = S::from_f64(2.0) * beta * y;
142
143        let drho_dx = drho2_dx / (S::from_f64(2.0) * rho);
144        let drho_dy = drho2_dy / (S::from_f64(2.0) * rho);
145
146        let dnorm_dx = alpha * drho_dx;
147        let dnorm_dy = alpha * drho_dy;
148
149        let dmx_dx = (norm - x * dnorm_dx) / (norm * norm);
150        let dmx_dy = -x * dnorm_dy / (norm * norm);
151
152        let dmy_dx = -y * dnorm_dx / (norm * norm);
153        let dmy_dy = (norm - y * dnorm_dy) / (norm * norm);
154
155        S::Matrix::<2, 2>::from_array2([[fx * dmx_dx, fx * dmx_dy], [fy * dmy_dx, fy * dmy_dy]])
156    }
157
158    fn dx_distort_params<PA, PO>(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Matrix<2, 6>
159    where
160        PA: Borrow<S::Vector<6>>,
161        PO: Borrow<S::Vector<2>>,
162    {
163        let params = params.borrow();
164        let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow();
165
166        let fx = params.elem(0);
167        let fy = params.elem(1);
168        let alpha = params.elem(4);
169        let beta = params.elem(5);
170
171        let x = proj_point_in_camera_z1_plane.elem(0);
172        let y = proj_point_in_camera_z1_plane.elem(1);
173
174        let r2 = x * x + y * y;
175        let rho2 = beta * r2 + S::from_f64(1.0);
176        let rho = rho2.sqrt();
177
178        let norm = alpha * rho + (S::from_f64(1.0) - alpha);
179
180        // Compute partial derivatives
181        let drho2_dbeta = r2;
182        let drho_dbeta = drho2_dbeta / (S::from_f64(2.0) * rho);
183
184        let dnorm_dalpha = rho - S::from_f64(1.0);
185        let dnorm_dbeta = alpha * drho_dbeta;
186
187        let mx = x / norm;
188        let my = y / norm;
189
190        let dmx_dalpha = -x * dnorm_dalpha / (norm * norm);
191        let dmy_dalpha = -y * dnorm_dalpha / (norm * norm);
192
193        let dmx_dbeta = -x * dnorm_dbeta / (norm * norm);
194        let dmy_dbeta = -y * dnorm_dbeta / (norm * norm);
195
196        S::Matrix::<2, 6>::from_array2([
197            [
198                mx,
199                S::zero(),
200                S::ones(),
201                S::zero(),
202                fx * dmx_dalpha,
203                fx * dmx_dbeta,
204            ],
205            [
206                S::zero(),
207                my,
208                S::zero(),
209                S::ones(),
210                fy * dmy_dalpha,
211                fy * dmy_dbeta,
212            ],
213        ])
214    }
215
216    fn identity_params() -> S::Vector<6> {
217        let mut params = S::Vector::<6>::zeros();
218        *params.elem_mut(0) = <S>::ones();
219        *params.elem_mut(1) = <S>::ones();
220        params
221    }
222}
sophus_sensor/distortions/enhanced_unified.rs

sophus_sensor/distortions/
enhanced_unified.rs
