extended_unified_camera_model/
lib.rs

1#![no_std]
2#![doc = include_str!("../README.md")]
3use nalgebra::{Dim, OMatrix, RealField, U2, U3};
4
5#[cfg(feature = "serde-serialize")]
6use serde::{Deserialize, Serialize};
7
8/// Parameters of an Extended Unified Camera Model
9///
10/// This implements [cam_geom::IntrinsicParameters].
11///
12/// Extended Unified Camera Model originally described in B. Khomutenko, G.
13/// Garcia, and P. Martinet. "An enhanced unified camera model". *IEEE Robotics
14/// and Automation Letters*, 1(1):137–144, Jan 2016. This formulation follows
15/// "The Double Sphere Camera Model" by Vladyslav Usenko, Nikolaus Demmel and
16/// Daniel Cremers
17/// [doi:10.1109/3DV.2018.00069](https://doi.org/10.1109/3DV.2018.00069).
18///
19/// When compiled with the `serde-serialize` feature, can be serialized and
20/// deserialized using serde.
21#[derive(Debug, Clone)]
22#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
23pub struct EucmParams<R> {
24    pub fx: R,
25    pub fy: R,
26    pub cx: R,
27    pub cy: R,
28    pub alpha: R,
29    pub beta: R,
30}
31
32impl<R: RealField> cam_geom::IntrinsicParameters<R> for EucmParams<R> {
33    type BundleType = cam_geom::ray_bundle_types::SharedOriginRayBundle<R>;
34
35    fn pixel_to_camera<IN, NPTS>(
36        &self,
37        pixels: &cam_geom::Pixels<R, NPTS, IN>,
38    ) -> cam_geom::RayBundle<
39        cam_geom::coordinate_system::CameraFrame,
40        Self::BundleType,
41        R,
42        NPTS,
43        nalgebra::Owned<R, NPTS, nalgebra::U3>,
44    >
45    where
46        Self::BundleType: cam_geom::Bundle<R>,
47        IN: nalgebra::Storage<R, NPTS, nalgebra::U2>,
48        NPTS: nalgebra::Dim,
49        nalgebra::DefaultAllocator: nalgebra::allocator::Allocator<nalgebra::U1, nalgebra::U2>,
50        nalgebra::DefaultAllocator: nalgebra::allocator::Allocator<NPTS, nalgebra::U2>,
51        nalgebra::DefaultAllocator: nalgebra::allocator::Allocator<NPTS, nalgebra::U3>,
52    {
53        // Implementation of Usenko et al. equations 18-22.
54        let mut result = cam_geom::RayBundle::new_shared_zero_origin(OMatrix::zeros_generic(
55            NPTS::from_usize(pixels.data.nrows()),
56            U3::from_usize(3),
57        ));
58
59        let one: R = nalgebra::convert(1.0);
60        let two: R = nalgebra::convert(2.0);
61
62        for i in 0..pixels.data.nrows() {
63            let u = pixels.data[(i, 0)].clone();
64            let v = pixels.data[(i, 1)].clone();
65            let mx = (u - self.cx.clone()) / self.fx.clone();
66            let my = (v - self.cy.clone()) / self.fy.clone();
67            let r2 = mx.clone() * mx.clone() + my.clone() * my.clone();
68            let mz_num = one.clone() - self.beta.clone() * self.alpha.clone().powi(2) * r2.clone();
69            let mz_denom = self.alpha.clone()
70                * (one.clone()
71                    - (two.clone() * self.alpha.clone() - one.clone())
72                        * self.beta.clone()
73                        * r2.clone())
74                .sqrt()
75                + (one.clone() - self.alpha.clone());
76            let mz = mz_num.clone() / mz_denom.clone();
77            let norm =
78                one.clone() / (mx.clone().powi(2) + my.clone().powi(2) + mz.clone().powi(2)).sqrt();
79            let x = mx.clone() * norm.clone();
80            let y = my.clone() * norm.clone();
81            let z = mz * norm;
82            result.data[(i, 0)] = x;
83            result.data[(i, 1)] = y;
84            result.data[(i, 2)] = z;
85        }
86
87        result
88    }
89
90    fn camera_to_pixel<IN, NPTS>(
91        &self,
92        camera: &cam_geom::Points<cam_geom::coordinate_system::CameraFrame, R, NPTS, IN>,
93    ) -> cam_geom::Pixels<R, NPTS, nalgebra::Owned<R, NPTS, nalgebra::U2>>
94    where
95        IN: nalgebra::Storage<R, NPTS, nalgebra::U3>,
96        NPTS: nalgebra::Dim,
97        nalgebra::DefaultAllocator: nalgebra::allocator::Allocator<NPTS, nalgebra::U2>,
98    {
99        let one: R = nalgebra::convert(1.0);
100
101        // Implementation of Usenko et al. equations 16 and 17.
102        let mut result = cam_geom::Pixels {
103            data: OMatrix::zeros_generic(NPTS::from_usize(camera.data.nrows()), U2::from_usize(2)),
104        };
105
106        for i in 0..camera.data.nrows() {
107            let x = camera.data[(i, 0)].clone();
108            let y = camera.data[(i, 1)].clone();
109            let z = camera.data[(i, 2)].clone();
110
111            let d = (self.beta.clone() * (x.clone() * x.clone() + y.clone() * y.clone())
112                + z.clone() * z.clone())
113            .sqrt();
114            let denom =
115                self.alpha.clone() * d.clone() + (one.clone() - self.alpha.clone()) * z.clone();
116            let u = self.fx.clone() * (x.clone() / denom.clone()) + self.cx.clone();
117            let v = self.fy.clone() * (y / denom) + self.cy.clone();
118
119            result.data[(i, 0)] = u;
120            result.data[(i, 1)] = v;
121        }
122        result
123    }
124}
extended_unified_camera_model/lib.rs

extended_unified_camera_model/
lib.rs
