camera_intrinsic_model/
generic_model.rs

1use std::str::FromStr;
2
3use super::generic_functions::*;
4use super::{EUCM, EUCMT, FovCamera, Ftheta, KannalaBrandt4, OpenCVModel5, UCM};
5use image::DynamicImage;
6use nalgebra as na;
7use rayon::prelude::*;
8use serde::{Deserialize, Serialize};
9
10#[derive(Serialize, Deserialize, Clone, Copy, Debug)]
11pub enum GenericModel<T: na::RealField> {
12    EUCM(EUCM<T>),
13    UCM(UCM<T>),
14    OpenCVModel5(OpenCVModel5<T>),
15    KannalaBrandt4(KannalaBrandt4<T>),
16    EUCMT(EUCMT<T>),
17    Ftheta(Ftheta<T>),
18    FovCamera(FovCamera<T>),
19}
20macro_rules! generic_impl {
21    ($fn_name:tt, $out:ty, $($v:tt: $t:ty),+) => {
22        pub fn $fn_name(&self, $($v: $t),+) -> $out{
23            match self {
24                GenericModel::EUCM(eucm) => $fn_name(eucm, $($v),+),
25                GenericModel::UCM(ucm) => $fn_name(ucm, $($v),+),
26                GenericModel::OpenCVModel5(open_cvmodel5) => $fn_name(open_cvmodel5, $($v),+),
27                GenericModel::KannalaBrandt4(kannala_brandt4) => $fn_name(kannala_brandt4, $($v),+),
28                GenericModel::EUCMT(eucmt) => $fn_name(eucmt, $($v),+),
29                GenericModel::Ftheta(ftheta) => $fn_name(ftheta, $($v),+),
30                GenericModel::FovCamera(fov_camera) => $fn_name(fov_camera, $($v),+),
31            }
32        }
33    };
34}
35macro_rules! generic_impl_self {
36    ($fn_name:tt -> $out:ty) => {
37        pub fn $fn_name(&self) -> $out{
38            match self {
39                GenericModel::EUCM(eucm) => eucm.$fn_name(),
40                GenericModel::UCM(ucm) => ucm.$fn_name(),
41                GenericModel::OpenCVModel5(open_cvmodel5) => open_cvmodel5.$fn_name(),
42                GenericModel::KannalaBrandt4(kannala_brandt4) => kannala_brandt4.$fn_name(),
43                GenericModel::EUCMT(eucmt) => eucmt.$fn_name(),
44                GenericModel::Ftheta(ftheta) => ftheta.$fn_name(),
45                GenericModel::FovCamera(fov_camera) => fov_camera.$fn_name(),
46            }
47        }
48    };
49    ($fn_name:tt, $out:ty, $($v:tt: $t:ty),+) => {
50        pub fn $fn_name(&self, $($v: $t),+) -> $out{
51            match self {
52                GenericModel::EUCM(eucm) => eucm.$fn_name($($v),+),
53                GenericModel::UCM(ucm) => ucm.$fn_name($($v),+),
54                GenericModel::OpenCVModel5(open_cvmodel5) => open_cvmodel5.$fn_name($($v),+),
55                GenericModel::KannalaBrandt4(kannala_brandt4) => kannala_brandt4.$fn_name($($v),+),
56                GenericModel::EUCMT(eucmt) => eucmt.$fn_name($($v),+),
57                GenericModel::Ftheta(ftheta) => ftheta.$fn_name($($v),+),
58                GenericModel::FovCamera(fov_camera) => fov_camera.$fn_name($($v),+),
59            }
60        }
61    };
62    ($fn_name:tt, $($v:tt: $t:ty),+) => {
63        pub fn $fn_name(&mut self, $($v: $t),+){
64            match self {
65                GenericModel::EUCM(eucm) => eucm.$fn_name($($v),+),
66                GenericModel::UCM(ucm) => ucm.$fn_name($($v),+),
67                GenericModel::OpenCVModel5(open_cvmodel5) => open_cvmodel5.$fn_name($($v),+),
68                GenericModel::KannalaBrandt4(kannala_brandt4) => kannala_brandt4.$fn_name($($v),+),
69                GenericModel::EUCMT(eucmt) => eucmt.$fn_name($($v),+),
70                GenericModel::Ftheta(ftheta) => ftheta.$fn_name($($v),+),
71                GenericModel::FovCamera(fov_camera) => fov_camera.$fn_name($($v),+),
72            }
73        }
74    };
75}
76impl GenericModel<f64> {
77    generic_impl!(init_undistort_map, (na::DMatrix<f32>, na::DMatrix<f32>), projection_mat: &na::Matrix3<f64>, new_w_h: (u32, u32), rotation: Option<na::Rotation3<f64>>);
78    generic_impl!(estimate_new_camera_matrix_for_undistort, na::Matrix3<f64>, balance: f64, new_image_w_h: Option<(u32, u32)>);
79}
80
81impl FromStr for GenericModel<f64> {
82    type Err = std::fmt::Error;
83
84    fn from_str(s: &str) -> Result<Self, Self::Err> {
85        match s {
86            "ucm" | "UCM" => Ok(GenericModel::UCM(UCM::zeros())),
87            "eucm" | "EUCM" => Ok(GenericModel::EUCM(EUCM::zeros())),
88            "kb4" | "KB4" => Ok(GenericModel::KannalaBrandt4(KannalaBrandt4::zeros())),
89            "opencv5" | "OPENCV5" => Ok(GenericModel::OpenCVModel5(OpenCVModel5::zeros())),
90            "eucmt" | "EUCMT" => Ok(GenericModel::EUCMT(EUCMT::zeros())),
91            "ftheta" | "FTHETA" => Ok(GenericModel::Ftheta(Ftheta::zeros())),
92            "fov_camera" | "FOV_CAMERA" => Ok(GenericModel::FovCamera(FovCamera::zeros())),
93            _ => Err(std::fmt::Error),
94        }
95    }
96}
97
98impl<T: na::RealField + Clone> GenericModel<T> {
99    generic_impl_self!(width -> T);
100    generic_impl_self!(height -> T);
101    generic_impl_self!(params -> na::DVector<T>);
102    generic_impl_self!(set_params, params: &na::DVector<T>);
103    generic_impl_self!(set_w_h, w: u32, h: u32);
104    generic_impl_self!(camera_params -> na::DVector<T>);
105    generic_impl_self!(distortion_params -> na::DVector<T>);
106    generic_impl_self!(distortion_params_bound -> Vec<(usize, (f64, f64))>);
107    generic_impl_self!(project_one, na::Vector2<T>, pt: &na::Vector3<T>);
108    generic_impl_self!(unproject_one, na::Vector3<T>, pt: &na::Vector2<T>);
109    generic_impl_self!(project, Vec<Option<na::Vector2<T>>>, p3d: &[na::Vector3<T>]);
110    generic_impl_self!(unproject, Vec<Option<na::Vector3<T>>>, p2d: &[na::Vector2<T>]);
111    pub fn cast<U: na::RealField + Clone>(&self) -> GenericModel<U> {
112        match self {
113            GenericModel::EUCM(eucm) => GenericModel::EUCM(EUCM::from(eucm)),
114            GenericModel::UCM(ucm) => GenericModel::UCM(UCM::from(ucm)),
115            GenericModel::OpenCVModel5(open_cvmodel5) => {
116                GenericModel::OpenCVModel5(OpenCVModel5::from(open_cvmodel5))
117            }
118            GenericModel::KannalaBrandt4(kannala_brandt4) => {
119                GenericModel::KannalaBrandt4(KannalaBrandt4::from(kannala_brandt4))
120            }
121            GenericModel::EUCMT(eucmt) => GenericModel::EUCMT(EUCMT::from(eucmt)),
122            GenericModel::Ftheta(ftheta) => GenericModel::Ftheta(Ftheta::from(ftheta)),
123            GenericModel::FovCamera(fov_camera) => {
124                GenericModel::FovCamera(FovCamera::from(fov_camera))
125            }
126        }
127    }
128    pub fn new_from_params(&self, params: &na::DVector<T>) -> GenericModel<T> {
129        match self {
130            GenericModel::EUCM(m) => GenericModel::EUCM(EUCM::new(params, m.width, m.height)),
131            GenericModel::UCM(m) => GenericModel::UCM(UCM::new(params, m.width, m.height)),
132            GenericModel::OpenCVModel5(m) => {
133                GenericModel::OpenCVModel5(OpenCVModel5::new(params, m.width, m.height))
134            }
135            GenericModel::KannalaBrandt4(m) => {
136                GenericModel::KannalaBrandt4(KannalaBrandt4::new(params, m.width, m.height))
137            }
138            GenericModel::EUCMT(m) => GenericModel::EUCMT(EUCMT::new(params, m.width, m.height)),
139            GenericModel::Ftheta(m) => GenericModel::Ftheta(Ftheta::new(params, m.width, m.height)),
140            GenericModel::FovCamera(m) => {
141                GenericModel::FovCamera(FovCamera::new(params, m.width, m.height))
142            }
143        }
144    }
145}
146
147macro_rules! remap_impl {
148    ($src:expr, $map0:expr, $map1:expr, $($img_type:ident => ($inner_type:ident, $default_value:expr)),*) => {
149        match $src {
150            $(
151                DynamicImage::$img_type(img) => {
152                    let (r, c) = $map0.shape();
153                    let out_img = image::ImageBuffer::from_par_fn(c as u32, r as u32, |x, y| {
154                        let idx = y as usize * c + x as usize;
155                        let (x_cor, y_cor) = unsafe { ($map0.get_unchecked(idx), $map1.get_unchecked(idx)) };
156                        if x_cor.is_nan() || y_cor.is_nan() {
157                            return image::$inner_type($default_value);
158                        }
159                        image::imageops::interpolate_bilinear(img, *x_cor, *y_cor)
160                            .unwrap_or(image::$inner_type($default_value))
161                    });
162                    DynamicImage::$img_type(out_img)
163                }
164            )*
165            _ => {
166                panic!("Not support this image type.");
167            }
168        }
169    };
170}
171
172pub fn remap(src: &DynamicImage, map0: &na::DMatrix<f32>, map1: &na::DMatrix<f32>) -> DynamicImage {
173    remap_impl!(src, map0, map1,
174        ImageLuma8 => (Luma, [0]),
175        ImageLumaA8 => (LumaA, [0, 0]),
176        ImageLuma16 => (Luma, [0]),
177        ImageLumaA16 => (LumaA, [0, 0]),
178        ImageRgb8 => (Rgb, [0, 0, 0]),
179        ImageRgba8 => (Rgba, [0, 0, 0, 0]),
180        ImageRgb16 => (Rgb, [0, 0, 0]),
181        ImageRgba16 => (Rgba, [0, 0, 0, 0]),
182        ImageRgb32F => (Rgb, [0.0, 0.0, 0.0]),
183        ImageRgba32F => (Rgba, [0.0, 0.0, 0.0, 0.0])
184    )
185}
186
187pub trait ModelCast<T: na::RealField + Clone>: CameraModel<T> {
188    fn cast<U: na::RealField>(&self) -> na::DVector<U> {
189        let v: Vec<_> = self
190            .params()
191            .iter()
192            .map(|i| U::from_f64(i.to_subset().unwrap()).unwrap())
193            .collect();
194        na::DVector::from_vec(v)
195    }
196}
197
198pub trait CameraModel<T: na::RealField + Clone>
199where
200    Self: Sync,
201{
202    /// Sets the intrinsic parameters of the camera model.
203    fn set_params(&mut self, params: &na::DVector<T>);
204
205    /// Returns the intrinsic parameters of the camera model.
206    fn params(&self) -> na::DVector<T>;
207
208    /// Returns the camera system parameters (e.g., fx, fy, cx, cy).
209    fn camera_params(&self) -> na::DVector<T>;
210
211    /// Returns the distortion parameters.
212    fn distortion_params(&self) -> na::DVector<T>;
213
214    /// Returns the image width.
215    fn width(&self) -> T;
216
217    /// Returns the image height.
218    fn height(&self) -> T;
219
220    /// Sets the image width and height.
221    fn set_w_h(&mut self, w: u32, h: u32);
222
223    /// Projects a single 3D point to the 2D image plane.
224    ///
225    /// # Arguments
226    /// * `pt` - A 3D point in camera coordinates.
227    fn project_one(&self, pt: &na::Vector3<T>) -> na::Vector2<T>;
228
229    /// Returns the valid bounds for distortion parameters during optimization.
230    fn distortion_params_bound(&self) -> Vec<(usize, (f64, f64))>;
231
232    /// Projects multiple 3D points to the 2D image plane in parallel.
233    /// Returns None if the point projects outside the image bounds.
234    fn project(&self, p3d: &[na::Vector3<T>]) -> Vec<Option<na::Vector2<T>>> {
235        p3d.par_iter()
236            .map(|pt| {
237                let p2d = self.project_one(pt);
238                if p2d[0] < T::from_f64(0.0).unwrap()
239                    || p2d[0] > self.width()
240                    || p2d[1] < T::from_f64(0.0).unwrap()
241                    || p2d[1] > self.height()
242                {
243                    None
244                } else {
245                    Some(p2d)
246                }
247            })
248            .collect()
249    }
250    /// Unprojects a single 2D point to a 3D unit vector.
251    ///
252    /// # Arguments
253    /// * `pt` - A 2D point in pixel coordinates.
254    fn unproject_one(&self, pt: &na::Vector2<T>) -> na::Vector3<T>;
255
256    /// Unprojects multiple 2D points to 3D unit vectors in parallel.
257    /// Returns None if the point is outside the image bounds.
258    fn unproject(&self, p2d: &[na::Vector2<T>]) -> Vec<Option<na::Vector3<T>>> {
259        p2d.par_iter()
260            .map(|pt| {
261                if pt[0] < T::from_f64(0.0).unwrap()
262                    || pt[0] > self.width() - T::from_f64(1.0).unwrap()
263                    || pt[1] < T::from_f64(0.0).unwrap()
264                    || pt[1] > self.height() - T::from_f64(1.0).unwrap()
265                {
266                    None
267                } else {
268                    let p3d = self.unproject_one(pt);
269                    Some(p3d)
270                }
271            })
272            .collect()
273    }
274}
camera_intrinsic_model/generic_model.rs

camera_intrinsic_model/
generic_model.rs
