Skip to content

Commit e27657a

Browse files
powei-linpowei-lin
authored
add fov (#4)
1 parent 9ea9d55 commit e27657a

File tree

4 files changed

+182
-2
lines changed

4 files changed

+182
-2
lines changed

‎Cargo.toml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
[package]
22
name = "camera-intrinsic-model"
3-
version = "0.3.2"
3+
version = "0.4.0"
44
edition = "2021"
55
authors = ["Powei Lin <poweilin1994@gmail.com>"]
66
readme = "README.md"

‎src/generic_model.rs

Lines changed: 13 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
use std::str::FromStr;
22

33
use super::generic_functions::*;
4-
use super::{Ftheta, KannalaBrandt4, OpenCVModel5, EUCM, EUCMT, UCM};
4+
use super::{FovCamera, Ftheta, KannalaBrandt4, OpenCVModel5, EUCM, EUCMT, UCM};
55
use image::DynamicImage;
66
use nalgebra as na;
77
use rayon::prelude::*;
@@ -15,6 +15,7 @@ pub enum GenericModel<T: na::RealField> {
1515
KannalaBrandt4(KannalaBrandt4<T>),
1616
EUCMT(EUCMT<T>),
1717
Ftheta(Ftheta<T>),
18+
FovCamera(FovCamera<T>),
1819
}
1920
macro_rules! generic_impl {
2021
($fn_name:tt, $out:ty, $($v:tt: $t:ty),+) => {
@@ -26,6 +27,7 @@ macro_rules! generic_impl {
2627
GenericModel::KannalaBrandt4(kannala_brandt4) => $fn_name(kannala_brandt4, $($v),+),
2728
GenericModel::EUCMT(eucmt) => $fn_name(eucmt, $($v),+),
2829
GenericModel::Ftheta(ftheta) => $fn_name(ftheta, $($v),+),
30+
GenericModel::FovCamera(fov_camera) => $fn_name(fov_camera, $($v),+),
2931
}
3032
}
3133
};
@@ -40,6 +42,7 @@ macro_rules! generic_impl_self {
4042
GenericModel::KannalaBrandt4(kannala_brandt4) => kannala_brandt4.$fn_name(),
4143
GenericModel::EUCMT(eucmt) => eucmt.$fn_name(),
4244
GenericModel::Ftheta(ftheta) => ftheta.$fn_name(),
45+
GenericModel::FovCamera(fov_camera) => fov_camera.$fn_name(),
4346
}
4447
}
4548
};
@@ -52,6 +55,7 @@ macro_rules! generic_impl_self {
5255
GenericModel::KannalaBrandt4(kannala_brandt4) => kannala_brandt4.$fn_name($($v),+),
5356
GenericModel::EUCMT(eucmt) => eucmt.$fn_name($($v),+),
5457
GenericModel::Ftheta(ftheta) => ftheta.$fn_name($($v),+),
58+
GenericModel::FovCamera(fov_camera) => fov_camera.$fn_name($($v),+),
5559
}
5660
}
5761
};
@@ -64,6 +68,7 @@ macro_rules! generic_impl_self {
6468
GenericModel::KannalaBrandt4(kannala_brandt4) => kannala_brandt4.$fn_name($($v),+),
6569
GenericModel::EUCMT(eucmt) => eucmt.$fn_name($($v),+),
6670
GenericModel::Ftheta(ftheta) => ftheta.$fn_name($($v),+),
71+
GenericModel::FovCamera(fov_camera) => fov_camera.$fn_name($($v),+),
6772
}
6873
}
6974
};
@@ -84,6 +89,7 @@ impl FromStr for GenericModel<f64> {
8489
"opencv5" | "OPENCV5" => Ok(GenericModel::OpenCVModel5(OpenCVModel5::zeros())),
8590
"eucmt" | "EUCMT" => Ok(GenericModel::EUCMT(EUCMT::zeros())),
8691
"ftheta" | "FTHETA" => Ok(GenericModel::Ftheta(Ftheta::zeros())),
92+
"fov_camera" | "FOV_CAMERA" => Ok(GenericModel::FovCamera(FovCamera::zeros())),
8793
_ => Err(std::fmt::Error),
8894
}
8995
}
@@ -114,6 +120,9 @@ impl<T: na::RealField + Clone> GenericModel<T> {
114120
}
115121
GenericModel::EUCMT(eucmt) => GenericModel::EUCMT(EUCMT::from(eucmt)),
116122
GenericModel::Ftheta(ftheta) => GenericModel::Ftheta(Ftheta::from(ftheta)),
123+
GenericModel::FovCamera(fov_camera) => {
124+
GenericModel::FovCamera(FovCamera::from(fov_camera))
125+
}
117126
}
118127
}
119128
pub fn new_from_params(&self, params: &na::DVector<T>) -> GenericModel<T> {
@@ -128,6 +137,9 @@ impl<T: na::RealField + Clone> GenericModel<T> {
128137
}
129138
GenericModel::EUCMT(m) => GenericModel::EUCMT(EUCMT::new(params, m.width, m.height)),
130139
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+
}
131143
}
132144
}
133145
}

‎src/models/fov_camera.rs

Lines changed: 166 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,166 @@
1+
use crate::generic_model::{CameraModel, ModelCast};
2+
use nalgebra as na;
3+
use serde::{Deserialize, Serialize};
4+
5+
#[derive(Serialize, Deserialize, Clone, Copy, Debug)]
6+
pub struct FovCamera<T: na::RealField + Clone> {
7+
pub fx: T,
8+
pub fy: T,
9+
pub cx: T,
10+
pub cy: T,
11+
pub w: T,
12+
pub width: u32,
13+
pub height: u32,
14+
}
15+
impl<T: na::RealField + Clone> FovCamera<T> {
16+
pub fn new(params: &na::DVector<T>, width: u32, height: u32) -> FovCamera<T> {
17+
if params.shape() != (5, 1) {
18+
panic!("the length of the vector should be 5");
19+
}
20+
FovCamera {
21+
fx: params[0].clone(),
22+
fy: params[1].clone(),
23+
cx: params[2].clone(),
24+
cy: params[3].clone(),
25+
w: params[4].clone(),
26+
width,
27+
height,
28+
}
29+
}
30+
pub fn from<U: na::RealField + Clone>(m: &FovCamera<U>) -> FovCamera<T> {
31+
FovCamera::new(&m.cast(), m.width, m.height)
32+
}
33+
pub fn zeros() -> FovCamera<T> {
34+
FovCamera {
35+
fx: T::zero(),
36+
fy: T::zero(),
37+
cx: T::zero(),
38+
cy: T::zero(),
39+
w: T::from_f64(0.5).unwrap(),
40+
width: 0,
41+
height: 0,
42+
}
43+
}
44+
}
45+
46+
impl<T: na::RealField + Clone> ModelCast<T> for FovCamera<T> {}
47+
48+
impl<T: na::RealField + Clone> CameraModel<T> for FovCamera<T> {
49+
fn set_params(&mut self, params: &nalgebra::DVector<T>) {
50+
if params.shape() != self.params().shape() {
51+
panic!("params has wrong shape.")
52+
}
53+
self.fx = params[0].clone();
54+
self.fy = params[1].clone();
55+
self.cx = params[2].clone();
56+
self.cy = params[3].clone();
57+
self.w = params[4].clone();
58+
}
59+
#[inline]
60+
fn params(&self) -> nalgebra::DVector<T> {
61+
na::dvector![
62+
self.fx.clone(),
63+
self.fy.clone(),
64+
self.cx.clone(),
65+
self.cy.clone(),
66+
self.w.clone(),
67+
]
68+
}
69+
fn width(&self) -> T {
70+
T::from_u32(self.width).unwrap()
71+
}
72+
73+
fn height(&self) -> T {
74+
T::from_u32(self.height).unwrap()
75+
}
76+
77+
fn project_one(&self, pt: &nalgebra::Vector3<T>) -> nalgebra::Vector2<T> {
78+
let params = self.params();
79+
let fx = &params[0];
80+
let fy = &params[1];
81+
let cx = &params[2];
82+
let cy = &params[3];
83+
let w = &params[4];
84+
85+
let x = pt[0].clone();
86+
let y = pt[1].clone();
87+
let z = pt[2].clone();
88+
89+
let r2 = x.clone() * x.clone() + y.clone() * y.clone();
90+
let r = r2.clone().sqrt();
91+
92+
let two = T::from_f64(2.0).unwrap();
93+
94+
let tanwhalf = w.clone() / two.clone();
95+
let atan_wrd = (two.clone() * tanwhalf.clone() * r.clone()).atan2(z);
96+
97+
let eps_sqrt = T::from_f64(f64::EPSILON).unwrap().sqrt();
98+
99+
let rd = if r2 < eps_sqrt {
100+
two.clone() * tanwhalf / w.clone()
101+
} else {
102+
atan_wrd / (r * w.clone())
103+
};
104+
105+
let mx = x * rd.clone();
106+
let my = y * rd.clone();
107+
108+
na::Vector2::new(fx.clone() * mx + cx.clone(), fy.clone() * my + cy.clone())
109+
}
110+
111+
fn unproject_one(&self, pt: &nalgebra::Vector2<T>) -> nalgebra::Vector3<T> {
112+
let params = self.params();
113+
let fx = &params[0];
114+
let fy = &params[1];
115+
let cx = &params[2];
116+
let cy = &params[3];
117+
let w = &params[4];
118+
119+
let one = T::from_f64(1.0).unwrap();
120+
let two = T::from_f64(2.0).unwrap();
121+
122+
let tan_w_2 = (w.clone() / two.clone()).tan();
123+
let mul2tanwby2 = tan_w_2 * two;
124+
125+
let mx = (pt[0].clone() - cx.clone()) / fx.clone();
126+
let my = (pt[1].clone() - cy.clone()) / fy.clone();
127+
128+
let r2 = mx.clone() * mx.clone() + my.clone() * my.clone();
129+
let rd = r2.sqrt();
130+
131+
let eps_sqrt = T::from_f64(f64::EPSILON).unwrap().sqrt();
132+
133+
if mul2tanwby2 > eps_sqrt.clone() && rd > eps_sqrt {
134+
let sin_rd_w = (rd.clone() * w.clone()).sin();
135+
let cos_rd_w = (rd.clone() * w.clone()).cos();
136+
let ru = sin_rd_w / (rd * mul2tanwby2);
137+
138+
na::Vector3::new(mx * ru.clone() / cos_rd_w.clone(), my * ru / cos_rd_w, one)
139+
} else {
140+
na::Vector3::new(mx, my, one)
141+
}
142+
}
143+
144+
fn camera_params(&self) -> nalgebra::DVector<T> {
145+
na::dvector![
146+
self.fx.clone(),
147+
self.fy.clone(),
148+
self.cx.clone(),
149+
self.cy.clone()
150+
]
151+
}
152+
153+
fn distortion_params(&self) -> nalgebra::DVector<T> {
154+
na::dvector![self.w.clone()]
155+
}
156+
157+
fn set_w_h(&mut self, w: u32, h: u32) {
158+
self.width = w;
159+
self.height = h;
160+
}
161+
162+
fn distortion_params_bound(&self) -> Vec<(usize, (f64, f64))> {
163+
// w [eps, 3]
164+
vec![(4, (f64::EPSILON, 3.0))]
165+
}
166+
}

‎src/models/mod.rs

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,14 @@
11
pub mod eucm;
22
pub mod eucmt;
3+
pub mod fov_camera;
34
pub mod ftheta;
45
pub mod kb4;
56
pub mod opencv5;
67
pub mod ucm;
78

89
pub use eucm::*;
910
pub use eucmt::*;
11+
pub use fov_camera::*;
1012
pub use ftheta::*;
1113
pub use kb4::*;
1214
pub use opencv5::*;

0 commit comments

Comments
 (0)