cumulus3d_core/
camera.rs

1//! 카메라 모델.
2//!
3//! 좌표 규약: 카메라 좌표 (X, Y, Z) → 정규화 평면 (u, v) = (X/Z, Y/Z) → 왜곡 → 픽셀.
4//! 픽셀 원점은 영상 좌상단 모서리, 좌상단 화소 중심 = (0.5, 0.5).
5
6use crate::error::{Error, Result};
7use crate::geometry::{Mat3, Vec2, Vec3};
8use crate::ids::{CameraId, INVALID_CAMERA_ID};
9use nalgebra::{Matrix2, Matrix2x3};
10
11/// 지원 카메라 모델. 숫자 id 는 모델 파일 형식의 모델 번호.
12#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
13pub enum CameraModelKind {
14    /// 단일 초점 핀홀(f, cx, cy).
15    SingleFocalPinhole = 0,
16    /// 핀홀(fx, fy, cx, cy).
17    Pinhole = 1,
18    /// 단일 초점 + 방사 왜곡 1항(f, cx, cy, k).
19    SingleFocalRadial = 2,
20    /// 단일 초점 + 방사 왜곡 2항(f, cx, cy, k1, k2).
21    Radial = 3,
22    /// 방사 2항 + 접선 2항(fx, fy, cx, cy, k1, k2, p1, p2).
23    OpenCv = 4,
24}
25
26/// 왜곡 제거 뉴턴 반복 최대 횟수.
27pub const UNDISTORT_MAX_ITERS: usize = 100;
28/// 수렴 판정: ‖step‖² < 1e−10.
29pub const UNDISTORT_STEP_SQ_TOL: f64 = 1e-10;
30
31/// 지원 모델의 파일 형식 이름(id 순서).
32const MODEL_NAMES: [&str; 5] = ["SIMPLE_PINHOLE", "PINHOLE", "SIMPLE_RADIAL", "RADIAL", "OPENCV"];
33
34impl CameraModelKind {
35    /// 모델 번호 → 종류.
36    pub fn from_id(id: i32) -> Result<Self> {
37        match id {
38            0 => Ok(Self::SingleFocalPinhole),
39            1 => Ok(Self::Pinhole),
40            2 => Ok(Self::SingleFocalRadial),
41            3 => Ok(Self::Radial),
42            4 => Ok(Self::OpenCv),
43            _ => Err(Error::Unsupported(format!("카메라 모델 id {id}"))),
44        }
45    }
46    /// 모델 번호.
47    pub fn id(self) -> i32 {
48        self as i32
49    }
50    /// 파일 형식 모델 이름(예: `PINHOLE`).
51    pub fn name(self) -> &'static str {
52        MODEL_NAMES[self as usize]
53    }
54    /// 파일 형식 모델 이름 → 종류.
55    pub fn from_name(name: &str) -> Result<Self> {
56        match MODEL_NAMES.iter().position(|n| *n == name) {
57            Some(i) => Self::from_id(i as i32),
58            None => Err(Error::Format(format!("알 수 없는 카메라 모델 이름 {name}"))),
59        }
60    }
61    /// 파라미터 개수.
62    pub fn num_params(self) -> usize {
63        match self {
64            Self::SingleFocalPinhole => 3,
65            Self::Pinhole => 4,
66            Self::SingleFocalRadial => 4,
67            Self::Radial => 5,
68            Self::OpenCv => 8,
69        }
70    }
71    /// 파라미터 이름(쉼표 구분).
72    pub fn params_info(self) -> &'static str {
73        match self {
74            Self::SingleFocalPinhole => "f, cx, cy",
75            Self::Pinhole => "fx, fy, cx, cy",
76            Self::SingleFocalRadial => "f, cx, cy, k",
77            Self::Radial => "f, cx, cy, k1, k2",
78            Self::OpenCv => "fx, fy, cx, cy, k1, k2, p1, p2",
79        }
80    }
81    /// `params` 안 초점 파라미터 위치.
82    pub fn focal_slots(self) -> &'static [usize] {
83        match self {
84            Self::SingleFocalPinhole | Self::SingleFocalRadial | Self::Radial => &[0],
85            Self::Pinhole | Self::OpenCv => &[0, 1],
86        }
87    }
88    /// `params` 안 주점 파라미터 위치.
89    pub fn pp_slots(self) -> &'static [usize] {
90        match self {
91            Self::SingleFocalPinhole | Self::SingleFocalRadial | Self::Radial => &[1, 2],
92            Self::Pinhole | Self::OpenCv => &[2, 3],
93        }
94    }
95    /// `params` 안 왜곡(추가) 파라미터 위치.
96    pub fn extra_param_slots(self) -> &'static [usize] {
97        match self {
98            Self::SingleFocalPinhole | Self::Pinhole => &[],
99            Self::SingleFocalRadial => &[3],
100            Self::Radial => &[3, 4],
101            Self::OpenCv => &[4, 5, 6, 7],
102        }
103    }
104}
105
106/// 카메라(내부 파라미터). `focal_from_prior` 는 메모리 전용(모델 파일에 저장 안 됨).
107#[derive(Clone, Debug, PartialEq)]
108pub struct Camera {
109    /// 카메라 id.
110    pub camera_id: CameraId,
111    /// 카메라 모델.
112    pub model: CameraModelKind,
113    /// 영상 너비(픽셀).
114    pub width: u64,
115    /// 영상 높이(픽셀).
116    pub height: u64,
117    /// 모델 파라미터(순서는 `CameraModelKind::params_info`).
118    pub params: Vec<f64>,
119    /// 초점이 EXIF 등 사전 정보에서 왔는지(보정 경로 선택용).
120    pub focal_from_prior: bool,
121}
122
123impl Camera {
124    /// 파라미터 개수를 검증해 생성.
125    pub fn new(camera_id: CameraId, model: CameraModelKind, width: u64, height: u64, params: Vec<f64>) -> Result<Self> {
126        if params.len() != model.num_params() {
127            return Err(Error::Format(format!(
128                "카메라 {camera_id}: 모델 {} 의 파라미터 개수 {} != {}",
129                model.name(),
130                params.len(),
131                model.num_params()
132            )));
133        }
134        Ok(Self { camera_id, model, width, height, params, focal_from_prior: false })
135    }
136
137    /// 초점 f 로 초기화: 초점 = f, 주점 = (W/2, H/2), 왜곡 0.
138    pub fn from_focal(model: CameraModelKind, focal: f64, width: u64, height: u64) -> Self {
139        let mut params = vec![0.0; model.num_params()];
140        for &i in model.focal_slots() {
141            params[i] = focal;
142        }
143        let pp = model.pp_slots();
144        params[pp[0]] = width as f64 / 2.0;
145        params[pp[1]] = height as f64 / 2.0;
146        Self { camera_id: INVALID_CAMERA_ID, model, width, height, params, focal_from_prior: false }
147    }
148
149    /// 파라미터 개수가 모델과 맞는지.
150    pub fn verify_params(&self) -> bool {
151        self.params.len() == self.model.num_params()
152    }
153
154    /// 평균 초점거리.
155    pub fn mean_focal(&self) -> f64 {
156        let idx = self.model.focal_slots();
157        idx.iter().map(|&i| self.params[i]).sum::<f64>() / idx.len() as f64
158    }
159    /// x 초점거리.
160    pub fn focal_length_x(&self) -> f64 {
161        self.params[self.model.focal_slots()[0]]
162    }
163    /// y 초점거리(단일 초점 모델이면 x 와 같음).
164    pub fn focal_length_y(&self) -> f64 {
165        let idx = self.model.focal_slots();
166        self.params[idx[idx.len() - 1]]
167    }
168    /// 모든 초점 파라미터를 f 로 설정.
169    pub fn set_focal_length(&mut self, f: f64) {
170        for &i in self.model.focal_slots() {
171            self.params[i] = f;
172        }
173    }
174    /// 주점 x.
175    pub fn principal_point_x(&self) -> f64 {
176        self.params[self.model.pp_slots()[0]]
177    }
178    /// 주점 y.
179    pub fn principal_point_y(&self) -> f64 {
180        self.params[self.model.pp_slots()[1]]
181    }
182    /// 주점 설정.
183    pub fn set_principal_point(&mut self, cx: f64, cy: f64) {
184        let pp = self.model.pp_slots();
185        self.params[pp[0]] = cx;
186        self.params[pp[1]] = cy;
187    }
188    /// 왜곡(추가) 파라미터 복사본.
189    pub fn extra_params(&self) -> Vec<f64> {
190        self.model.extra_param_slots().iter().map(|&i| self.params[i]).collect()
191    }
192    /// 왜곡 없음 판정: 추가 파라미터가 모두 0.
193    pub fn is_undistorted(&self) -> bool {
194        self.model.extra_param_slots().iter().all(|&i| self.params[i] == 0.0)
195    }
196    /// 보정 행렬 K (왜곡 무시).
197    pub fn calibration_matrix(&self) -> Mat3 {
198        Mat3::new(
199            self.focal_length_x(),
200            0.0,
201            self.principal_point_x(),
202            0.0,
203            self.focal_length_y(),
204            self.principal_point_y(),
205            0.0,
206            0.0,
207            1.0,
208        )
209    }
210    /// 비정상 파라미터 판정.
211    pub fn has_implausible_params(&self, min_focal_ratio: f64, max_focal_ratio: f64, max_extra_param: f64) -> bool {
212        let (w, h) = (self.width as f64, self.height as f64);
213        let (cx, cy) = (self.principal_point_x(), self.principal_point_y());
214        if cx < 0.0 || cx > w || cy < 0.0 || cy > h {
215            return true;
216        }
217        let m = w.max(h);
218        for &i in self.model.focal_slots() {
219            let r = self.params[i] / m;
220            if r < min_focal_ratio || r > max_focal_ratio {
221                return true;
222            }
223        }
224        self.model.extra_param_slots().iter().any(|&i| self.params[i].abs() > max_extra_param)
225    }
226
227    /// 균일 크기 조정: 초점·주점에 s 곱, 크기는 반올림. 왜곡 계수 불변.
228    pub fn rescale(&mut self, scale: f64) {
229        let nw = (self.width as f64 * scale).round().max(1.0) as u64;
230        let nh = (self.height as f64 * scale).round().max(1.0) as u64;
231        self.scale_params(scale, scale);
232        self.width = nw;
233        self.height = nh;
234    }
235
236    /// 새 크기로 조정: 축별 sx = W'/W, sy = H'/H 를 초점·주점에 곱함.
237    pub fn rescale_to(&mut self, new_width: u64, new_height: u64) {
238        let sx = new_width as f64 / self.width as f64;
239        let sy = new_height as f64 / self.height as f64;
240        self.scale_params(sx, sy);
241        self.width = new_width;
242        self.height = new_height;
243    }
244
245    fn scale_params(&mut self, sx: f64, sy: f64) {
246        let f = self.model.focal_slots();
247        if f.len() == 1 {
248            // 설계 결정: 단일 초점 모델의 비등방 축척은 두 축 평균을 사용.
249            self.params[f[0]] *= 0.5 * (sx + sy);
250        } else {
251            self.params[f[0]] *= sx;
252            self.params[f[1]] *= sy;
253        }
254        let pp = self.model.pp_slots();
255        self.params[pp[0]] *= sx;
256        self.params[pp[1]] *= sy;
257    }
258
259    /// 픽셀 오차 임계를 정규화 평면 임계로: τ / 평균 초점.
260    pub fn img_to_cam_threshold(&self, threshold_px: f64) -> f64 {
261        threshold_px / self.mean_focal()
262    }
263
264    /// 정규화 평면 왜곡량 Δ(u,v) 와 ∂Δ/∂(u,v).
265    fn distortion(&self, u: f64, v: f64) -> (f64, f64, Matrix2<f64>) {
266        let p = &self.params;
267        match self.model {
268            CameraModelKind::SingleFocalPinhole | CameraModelKind::Pinhole => (0.0, 0.0, Matrix2::zeros()),
269            CameraModelKind::SingleFocalRadial | CameraModelKind::Radial => {
270                let (k1, k2) = if self.model == CameraModelKind::SingleFocalRadial { (p[3], 0.0) } else { (p[3], p[4]) };
271                let r2 = u * u + v * v;
272                let radial = k1 * r2 + k2 * r2 * r2;
273                let g = 2.0 * k1 + 4.0 * k2 * r2; // ∂radial/∂u = g·u
274                let du = u * radial;
275                let dv = v * radial;
276                let j = Matrix2::new(radial + u * g * u, u * g * v, v * g * u, radial + v * g * v);
277                (du, dv, j)
278            }
279            CameraModelKind::OpenCv => {
280                let (k1, k2, p1, p2) = (p[4], p[5], p[6], p[7]);
281                let r2 = u * u + v * v;
282                let radial = k1 * r2 + k2 * r2 * r2;
283                let du = u * radial + 2.0 * p1 * u * v + p2 * (r2 + 2.0 * u * u);
284                let dv = v * radial + 2.0 * p2 * u * v + p1 * (r2 + 2.0 * v * v);
285                let du_du = radial + u * (2.0 * k1 * u + 4.0 * k2 * r2 * u) + 2.0 * p1 * v + 6.0 * p2 * u;
286                let du_dv = u * (2.0 * k1 * v + 4.0 * k2 * r2 * v) + 2.0 * p1 * u + 2.0 * p2 * v;
287                let dv_du = v * (2.0 * k1 * u + 4.0 * k2 * r2 * u) + 2.0 * p2 * v + 2.0 * p1 * u;
288                let dv_dv = radial + v * (2.0 * k1 * v + 4.0 * k2 * r2 * v) + 2.0 * p2 * u + 6.0 * p1 * v;
289                (du, dv, Matrix2::new(du_du, du_dv, dv_du, dv_dv))
290            }
291        }
292    }
293
294    fn fxfycxcy(&self) -> (f64, f64, f64, f64) {
295        (self.focal_length_x(), self.focal_length_y(), self.principal_point_x(), self.principal_point_y())
296    }
297
298    /// 정규화 평면 (u, v) → 픽셀 (왜곡 포함).
299    pub fn normalized_to_img(&self, uv: &Vec2) -> Vec2 {
300        let (fx, fy, cx, cy) = self.fxfycxcy();
301        let (du, dv, _) = self.distortion(uv.x, uv.y);
302        Vec2::new(fx * (uv.x + du) + cx, fy * (uv.y + dv) + cy)
303    }
304
305    /// 카메라 좌표 3D 점 → 픽셀. Z < f64 ε 이면 None.
306    pub fn cam_to_img(&self, xc: &Vec3) -> Option<Vec2> {
307        if xc.z < f64::EPSILON {
308            return None;
309        }
310        Some(self.normalized_to_img(&Vec2::new(xc.x / xc.z, xc.y / xc.z)))
311    }
312
313    /// 픽셀 → 정규화 평면(왜곡 제거). 뉴턴 100회 내 미수렴이면 None.
314    pub fn img_to_normalized(&self, xy: &Vec2) -> Option<Vec2> {
315        let (fx, fy, cx, cy) = self.fxfycxcy();
316        let p0 = Vec2::new((xy.x - cx) / fx, (xy.y - cy) / fy);
317        if self.model.extra_param_slots().is_empty() || self.is_undistorted() {
318            return if p0.x.is_finite() && p0.y.is_finite() { Some(p0) } else { None };
319        }
320        let mut p = p0;
321        for _ in 0..UNDISTORT_MAX_ITERS {
322            let (du, dv, jd) = self.distortion(p.x, p.y);
323            let j = Matrix2::identity() + jd;
324            let res = Vec2::new(p.x + du - p0.x, p.y + dv - p0.y);
325            let mut step = j.lu().solve(&res)?;
326            if !step.x.is_finite() || !step.y.is_finite() {
327                return None;
328            }
329            let rho2 = (p.norm_squared() * 0.01).max(0.01);
330            let s2 = step.norm_squared();
331            if s2 > rho2 {
332                step *= (rho2 / s2).sqrt();
333            }
334            p -= step;
335            if step.norm_squared() < UNDISTORT_STEP_SQ_TOL {
336                return Some(p);
337            }
338        }
339        None
340    }
341
342    /// 픽셀 → 단위 광선 (u, v, 1)/‖·‖.
343    pub fn img_to_ray(&self, xy: &Vec2) -> Option<Vec3> {
344        self.img_to_normalized(xy).map(|uv| Vec3::new(uv.x, uv.y, 1.0).normalize())
345    }
346
347    /// 투영과 해석적 야코비안.
348    ///
349    /// 반환: (픽셀, ∂픽셀/∂카메라좌표 2×3). `d_params` 가 주어지면 행 우선 2×num_params
350    /// (첫 행 = x, 둘째 행 = y, 열 = 파라미터 순서)로 ∂픽셀/∂파라미터를 채운다.
351    /// Z < ε 이면 None (BA 는 이때 잔차·야코비안 0 으로 둔다).
352    pub fn cam_to_img_with_jacobian(&self, xc: &Vec3, d_params: Option<&mut [f64]>) -> Option<(Vec2, Matrix2x3<f64>)> {
353        if xc.z < f64::EPSILON {
354            return None;
355        }
356        let iz = 1.0 / xc.z;
357        let u = xc.x * iz;
358        let v = xc.y * iz;
359        let (fx, fy, cx, cy) = self.fxfycxcy();
360        let (du, dv, jd) = self.distortion(u, v);
361        let xd = u + du;
362        let yd = v + dv;
363        let pix = Vec2::new(fx * xd + cx, fy * yd + cy);
364        let d = Matrix2::identity() + jd;
365        let nm = Matrix2x3::new(iz, 0.0, -xc.x * iz * iz, 0.0, iz, -xc.y * iz * iz);
366        let fd = Matrix2::new(fx, 0.0, 0.0, fy);
367        let j_point = fd * d * nm;
368        if let Some(out) = d_params {
369            let n = self.model.num_params();
370            assert!(out.len() >= 2 * n, "d_params 길이 부족");
371            out[..2 * n].iter_mut().for_each(|x| *x = 0.0);
372            let r2 = u * u + v * v;
373            let (rx, ry) = out.split_at_mut(n);
374            match self.model {
375                CameraModelKind::SingleFocalPinhole => {
376                    rx[0] = u;
377                    rx[1] = 1.0;
378                    ry[0] = v;
379                    ry[2] = 1.0;
380                }
381                CameraModelKind::Pinhole => {
382                    rx[0] = u;
383                    rx[2] = 1.0;
384                    ry[1] = v;
385                    ry[3] = 1.0;
386                }
387                CameraModelKind::SingleFocalRadial | CameraModelKind::Radial => {
388                    let f = fx;
389                    rx[0] = xd;
390                    rx[1] = 1.0;
391                    rx[3] = f * u * r2;
392                    ry[0] = yd;
393                    ry[2] = 1.0;
394                    ry[3] = f * v * r2;
395                    if self.model == CameraModelKind::Radial {
396                        rx[4] = f * u * r2 * r2;
397                        ry[4] = f * v * r2 * r2;
398                    }
399                }
400                CameraModelKind::OpenCv => {
401                    rx[0] = xd;
402                    rx[2] = 1.0;
403                    rx[4] = fx * u * r2;
404                    rx[5] = fx * u * r2 * r2;
405                    rx[6] = fx * 2.0 * u * v;
406                    rx[7] = fx * (r2 + 2.0 * u * u);
407                    ry[1] = yd;
408                    ry[3] = 1.0;
409                    ry[4] = fy * v * r2;
410                    ry[5] = fy * v * r2 * r2;
411                    ry[6] = fy * (r2 + 2.0 * v * v);
412                    ry[7] = fy * 2.0 * u * v;
413                }
414            }
415        }
416        Some((pix, j_point))
417    }
418}
419
420#[cfg(test)]
421mod tests {
422    use super::*;
423    use rand::{RngExt, SeedableRng};
424    use rand_pcg::Pcg64;
425
426    fn opencv(params: [f64; 8]) -> Camera {
427        Camera::new(1, CameraModelKind::OpenCv, 1000, 800, params.to_vec()).unwrap()
428    }
429
430    #[test]
431    fn opencv_hand_computed() {
432        let cam = opencv([1000.0, 1000.0, 500.0, 400.0, 0.1, 0.01, 0.001, -0.002]);
433        let p = cam.cam_to_img(&Vec3::new(0.2, -0.1, 1.0)).unwrap();
434        assert!((p.x - 700.705).abs() < 1e-9, "{}", p.x);
435        assert!((p.y - 299.6475).abs() < 1e-9, "{}", p.y);
436        assert!(cam.cam_to_img(&Vec3::new(0.0, 0.0, 0.0)).is_none());
437        assert!(cam.cam_to_img(&Vec3::new(0.0, 0.0, -1.0)).is_none());
438    }
439
440    #[test]
441    fn roundtrip_all_models() {
442        let mut rng = Pcg64::seed_from_u64(42);
443        for model in [
444            CameraModelKind::SingleFocalPinhole,
445            CameraModelKind::Pinhole,
446            CameraModelKind::SingleFocalRadial,
447            CameraModelKind::Radial,
448            CameraModelKind::OpenCv,
449        ] {
450            for _ in 0..300 {
451                let mut cam = Camera::from_focal(model, 900.0 + rng.random_range(0.0..200.0), 1000, 800);
452                let extra = model.extra_param_slots();
453                let ranges = [0.3, 0.1, 0.01, 0.01];
454                for (k, &i) in extra.iter().enumerate() {
455                    cam.params[i] = rng.random_range(-ranges[k]..ranges[k]);
456                }
457                if model == CameraModelKind::OpenCv || model == CameraModelKind::Pinhole {
458                    cam.params[1] = cam.params[0] * 1.01;
459                }
460                let uv = Vec2::new(rng.random_range(-0.8..0.8), rng.random_range(-0.8..0.8));
461                // 왜곡 사상이 국소적으로 가역이 아닌 표본(야코비안 행렬식 ≤ 0.2)은 해가 유일하지 않아 제외.
462                if (Matrix2::identity() + cam.distortion(uv.x, uv.y).2).determinant() <= 0.2 {
463                    continue;
464                }
465                let px = cam.normalized_to_img(&uv);
466                let uv2 = cam.img_to_normalized(&px).expect("수렴해야 함");
467                assert!((uv2 - uv).norm() <= 1e-8, "{model:?} {uv:?} {uv2:?}");
468                let px2 = cam.normalized_to_img(&uv2);
469                assert!((px2 - px).norm() <= 1e-6);
470            }
471        }
472    }
473
474    #[test]
475    fn undistort_failure_no_nan() {
476        let cam = opencv([1000.0, 1000.0, 500.0, 400.0, -5.0, 0.0, 0.0, 0.0]);
477        // 큰 반경: k1 = −5 이면 F(p) 의 최대값(r≈0.26 에서 ~0.17)을 넘는 p0 는 해가 없다.
478        let r = cam.img_to_normalized(&Vec2::new(500.0 + 900.0, 400.0 + 900.0));
479        assert!(r.is_none());
480    }
481
482    #[test]
483    fn distortion_jacobian_numeric() {
484        let mut rng = Pcg64::seed_from_u64(7);
485        for _ in 0..100 {
486            let cam = opencv([
487                1000.0,
488                1000.0,
489                500.0,
490                400.0,
491                rng.random_range(-0.3..0.3),
492                rng.random_range(-0.1..0.1),
493                rng.random_range(-0.01..0.01),
494                rng.random_range(-0.01..0.01),
495            ]);
496            let (u, v) = (rng.random_range(-0.8..0.8), rng.random_range(-0.8..0.8));
497            let (_, _, j) = cam.distortion(u, v);
498            let h = 1e-7;
499            let (a1, b1, _) = cam.distortion(u + h, v);
500            let (a0, b0, _) = cam.distortion(u - h, v);
501            let (c1, d1, _) = cam.distortion(u, v + h);
502            let (c0, d0, _) = cam.distortion(u, v - h);
503            let num = Matrix2::new((a1 - a0) / (2.0 * h), (c1 - c0) / (2.0 * h), (b1 - b0) / (2.0 * h), (d1 - d0) / (2.0 * h));
504            assert!((num - j).abs().max() <= 1e-6);
505        }
506    }
507
508    #[test]
509    fn projection_jacobian_numeric() {
510        let mut rng = Pcg64::seed_from_u64(9);
511        for model in [
512            CameraModelKind::SingleFocalPinhole,
513            CameraModelKind::Pinhole,
514            CameraModelKind::SingleFocalRadial,
515            CameraModelKind::Radial,
516            CameraModelKind::OpenCv,
517        ] {
518            for _ in 0..50 {
519                let mut cam = Camera::from_focal(model, 1200.0, 2000, 1500);
520                for &i in model.extra_param_slots() {
521                    cam.params[i] = rng.random_range(-0.2..0.2);
522                }
523                if model == CameraModelKind::OpenCv {
524                    cam.params[6] = rng.random_range(-0.01..0.01);
525                    cam.params[7] = rng.random_range(-0.01..0.01);
526                }
527                let xc = Vec3::new(rng.random_range(-1.0..1.0), rng.random_range(-1.0..1.0), rng.random_range(2.0..5.0));
528                let n = model.num_params();
529                let mut dp = vec![0.0; 2 * n];
530                let (pix, jp) = cam.cam_to_img_with_jacobian(&xc, Some(&mut dp)).unwrap();
531                assert!((pix - cam.cam_to_img(&xc).unwrap()).norm() < 1e-12);
532                // point
533                for k in 0..3 {
534                    let h = 1e-6 * xc[k].abs().max(1.0);
535                    let mut a = xc;
536                    a[k] += h;
537                    let mut b = xc;
538                    b[k] -= h;
539                    let num = (cam.cam_to_img(&a).unwrap() - cam.cam_to_img(&b).unwrap()) / (2.0 * h);
540                    for r in 0..2 {
541                        let an = jp[(r, k)];
542                        assert!((num[r] - an).abs() <= 1e-6 * an.abs().max(1.0), "{model:?} pt {r},{k}: {} vs {}", num[r], an);
543                    }
544                }
545                // params
546                for k in 0..n {
547                    let h = 1e-6 * cam.params[k].abs().max(1.0);
548                    let mut a = cam.clone();
549                    a.params[k] += h;
550                    let mut b = cam.clone();
551                    b.params[k] -= h;
552                    let num = (a.cam_to_img(&xc).unwrap() - b.cam_to_img(&xc).unwrap()) / (2.0 * h);
553                    for r in 0..2 {
554                        let an = dp[r * n + k];
555                        assert!((num[r] - an).abs() <= 1e-6 * an.abs().max(1.0), "{model:?} param {r},{k}: {} vs {}", num[r], an);
556                    }
557                }
558            }
559        }
560        let cam = Camera::from_focal(CameraModelKind::OpenCv, 1000.0, 100, 100);
561        assert!(cam.cam_to_img_with_jacobian(&Vec3::new(0.0, 0.0, -1.0), None).is_none());
562    }
563
564    #[test]
565    fn model_tables_and_rescale() {
566        let cam = Camera::from_focal(CameraModelKind::OpenCv, 4800.0, 4000, 3000);
567        assert_eq!(cam.params, vec![4800.0, 4800.0, 2000.0, 1500.0, 0.0, 0.0, 0.0, 0.0]);
568        assert_eq!(CameraModelKind::from_name("OPENCV").unwrap(), CameraModelKind::OpenCv);
569        assert!(CameraModelKind::from_id(6).is_err());
570        assert!(Camera::new(1, CameraModelKind::OpenCv, 1, 1, vec![0.0; 7]).is_err());
571        let mut c = cam.clone();
572        c.rescale_to(960, 720);
573        assert!((c.params[0] - 4800.0 * 0.24).abs() < 1e-9);
574        assert!((c.params[3] - 1500.0 * 0.24).abs() < 1e-9);
575        assert!((cam.img_to_cam_threshold(4.0) - 4.0 / 4800.0).abs() < 1e-15);
576        assert!(!cam.has_implausible_params(0.1, 10.0, 1.0));
577        let mut b = cam.clone();
578        b.params[4] = 1.5;
579        assert!(b.has_implausible_params(0.1, 10.0, 1.0));
580    }
581}
cumulus3d_core/camera.rs

cumulus3d_core/
camera.rs
