apex_camera_models/
rad_tan.rs

1//! Radial-Tangential (Brown-Conrady) camera model.
2//!
3//! Standard OpenCV distortion model combining radial and tangential terms, suitable for narrow
4//! to moderate field-of-view lenses. Has 9 intrinsic parameters. See the
5//! [rad-tan cookbook chapter](../doc/cookbook/src/rad-tan.html) for the full projection,
6//! unprojection, and Jacobian derivations.
7
8use crate::{CameraModel, CameraModelError, DistortionModel, PinholeParams};
9use nalgebra::{DVector, Matrix2, SMatrix, Vector2, Vector3};
10
11/// A Radial-Tangential camera model with 9 intrinsic parameters.
12#[derive(Debug, Clone, Copy, PartialEq)]
13pub struct RadTanCamera {
14    pub pinhole: PinholeParams,
15    pub distortion: DistortionModel,
16}
17
18impl RadTanCamera {
19    /// Creates a new Radial-Tangential (Brown-Conrady) camera.
20    ///
21    /// # Errors
22    ///
23    /// Returns [`CameraModelError::InvalidParams`] if `distortion` is not
24    /// [`DistortionModel::BrownConrady`].
25    ///
26    /// # Example
27    ///
28    /// ```
29    /// use apex_camera_models::{CameraModel, DistortionModel, PinholeParams, RadTanCamera};
30    ///
31    /// let pinhole = PinholeParams::new(300.0, 300.0, 320.0, 240.0)?;
32    /// let distortion = DistortionModel::BrownConrady { k1: 0.1, k2: 0.01, p1: 0.001, p2: 0.002, k3: 0.001 };
33    /// let camera = RadTanCamera::new(pinhole, distortion)?;
34    /// assert_eq!(camera.get_model_name(), "rad_tan");
35    /// # Ok::<(), apex_camera_models::CameraModelError>(())
36    /// ```
37    pub fn new(
38        pinhole: PinholeParams,
39        distortion: DistortionModel,
40    ) -> Result<Self, CameraModelError> {
41        let camera = Self {
42            pinhole,
43            distortion,
44        };
45        camera.validate_params()?;
46        Ok(camera)
47    }
48
49    /// Returns `true` if the point's z-coordinate is at or above [`crate::GEOMETRIC_PRECISION`].
50    fn check_projection_condition(&self, z: f64) -> bool {
51        z >= crate::GEOMETRIC_PRECISION
52    }
53
54    /// Returns the Brown-Conrady distortion parameters as `(k1, k2, p1, p2, k3)`.
55    /// Returns zeros if the model is not Brown-Conrady.
56    fn distortion_params(&self) -> (f64, f64, f64, f64, f64) {
57        match self.distortion {
58            DistortionModel::BrownConrady { k1, k2, p1, p2, k3 } => (k1, k2, p1, p2, k3),
59            _ => (0.0, 0.0, 0.0, 0.0, 0.0),
60        }
61    }
62
63    /// Initializes `[k1, k2, k3]` via linear least-squares given 3D–2D correspondences.
64    /// Tangential parameters `[p1, p2]` are reset to zero. Requires the intrinsics
65    /// `[fx, fy, cx, cy]` to already be set; needs at least 3 correspondences.
66    pub fn linear_estimation(
67        &mut self,
68        points_3d: &nalgebra::Matrix3xX<f64>,
69        points_2d: &nalgebra::Matrix2xX<f64>,
70    ) -> Result<(), CameraModelError> {
71        if points_2d.ncols() != points_3d.ncols() {
72            return Err(CameraModelError::InvalidParams(
73                "Number of 2D and 3D points must match".to_string(),
74            ));
75        }
76
77        let num_points = points_2d.ncols();
78        if num_points < 3 {
79            return Err(CameraModelError::InvalidParams(
80                "Need at least 3 points for RadTan linear estimation".to_string(),
81            ));
82        }
83
84        let mut a = nalgebra::DMatrix::zeros(num_points * 2, 3);
85        let mut b = nalgebra::DVector::zeros(num_points * 2);
86
87        let fx = self.pinhole.fx;
88        let fy = self.pinhole.fy;
89        let cx = self.pinhole.cx;
90        let cy = self.pinhole.cy;
91
92        for i in 0..num_points {
93            let x = points_3d[(0, i)];
94            let y = points_3d[(1, i)];
95            let z = points_3d[(2, i)];
96            let u = points_2d[(0, i)];
97            let v = points_2d[(1, i)];
98
99            let x_norm = x / z;
100            let y_norm = y / z;
101            let r2 = x_norm * x_norm + y_norm * y_norm;
102            let r4 = r2 * r2;
103            let r6 = r4 * r2;
104
105            let u_undist = fx * x_norm + cx;
106            let v_undist = fy * y_norm + cy;
107
108            a[(i * 2, 0)] = fx * x_norm * r2;
109            a[(i * 2, 1)] = fx * x_norm * r4;
110            a[(i * 2, 2)] = fx * x_norm * r6;
111
112            a[(i * 2 + 1, 0)] = fy * y_norm * r2;
113            a[(i * 2 + 1, 1)] = fy * y_norm * r4;
114            a[(i * 2 + 1, 2)] = fy * y_norm * r6;
115
116            b[i * 2] = u - u_undist;
117            b[i * 2 + 1] = v - v_undist;
118        }
119
120        let svd = a.svd(true, true);
121        let distortion_coeffs = match svd.solve(&b, 1e-10) {
122            Ok(sol) => sol,
123            Err(err_msg) => {
124                return Err(CameraModelError::NumericalError {
125                    operation: "svd_solve".to_string(),
126                    details: err_msg.to_string(),
127                });
128            }
129        };
130
131        self.distortion = DistortionModel::BrownConrady {
132            k1: distortion_coeffs[0],
133            k2: distortion_coeffs[1],
134            p1: 0.0,
135            p2: 0.0,
136            k3: distortion_coeffs[2],
137        };
138
139        self.validate_params()?;
140        Ok(())
141    }
142}
143
144/// Converts the camera parameters to a dynamic vector with layout
145/// `[fx, fy, cx, cy, k1, k2, p1, p2, k3]`.
146impl From<&RadTanCamera> for DVector<f64> {
147    fn from(camera: &RadTanCamera) -> Self {
148        let (k1, k2, p1, p2, k3) = camera.distortion_params();
149        DVector::from_vec(vec![
150            camera.pinhole.fx,
151            camera.pinhole.fy,
152            camera.pinhole.cx,
153            camera.pinhole.cy,
154            k1,
155            k2,
156            p1,
157            p2,
158            k3,
159        ])
160    }
161}
162
163/// Converts the camera parameters to a fixed-size array with layout
164/// `[fx, fy, cx, cy, k1, k2, p1, p2, k3]`.
165impl From<&RadTanCamera> for [f64; 9] {
166    fn from(camera: &RadTanCamera) -> Self {
167        let (k1, k2, p1, p2, k3) = camera.distortion_params();
168        [
169            camera.pinhole.fx,
170            camera.pinhole.fy,
171            camera.pinhole.cx,
172            camera.pinhole.cy,
173            k1,
174            k2,
175            p1,
176            p2,
177            k3,
178        ]
179    }
180}
181
182/// Creates a camera from a slice of intrinsic parameters with layout
183/// `[fx, fy, cx, cy, k1, k2, p1, p2, k3]`. Returns an error if the slice
184/// has fewer than 9 elements.
185impl TryFrom<&[f64]> for RadTanCamera {
186    type Error = CameraModelError;
187
188    fn try_from(params: &[f64]) -> Result<Self, Self::Error> {
189        if params.len() < 9 {
190            return Err(CameraModelError::InvalidParams(format!(
191                "RadTanCamera requires at least 9 parameters, got {}",
192                params.len()
193            )));
194        }
195        Ok(Self {
196            pinhole: PinholeParams {
197                fx: params[0],
198                fy: params[1],
199                cx: params[2],
200                cy: params[3],
201            },
202            distortion: DistortionModel::BrownConrady {
203                k1: params[4],
204                k2: params[5],
205                p1: params[6],
206                p2: params[7],
207                k3: params[8],
208            },
209        })
210    }
211}
212
213/// Creates a camera from a fixed-size array with layout
214/// `[fx, fy, cx, cy, k1, k2, p1, p2, k3]`.
215impl From<[f64; 9]> for RadTanCamera {
216    fn from(params: [f64; 9]) -> Self {
217        Self {
218            pinhole: PinholeParams {
219                fx: params[0],
220                fy: params[1],
221                cx: params[2],
222                cy: params[3],
223            },
224            distortion: DistortionModel::BrownConrady {
225                k1: params[4],
226                k2: params[5],
227                p1: params[6],
228                p2: params[7],
229                k3: params[8],
230            },
231        }
232    }
233}
234
235/// Creates a `RadTanCamera` from a parameter slice with full validation.
236/// Unlike [`<RadTanCamera as TryFrom<&[f64]>>::try_from`], this also calls
237/// [`CameraModel::validate_params`] and returns any validation errors.
238pub fn try_from_params(params: &[f64]) -> Result<RadTanCamera, CameraModelError> {
239    let camera = RadTanCamera::try_from(params)?;
240    camera.validate_params()?;
241    Ok(camera)
242}
243
244impl CameraModel for RadTanCamera {
245    const INTRINSIC_DIM: usize = 9;
246    type IntrinsicJacobian = SMatrix<f64, 2, 9>;
247    type PointJacobian = SMatrix<f64, 2, 3>;
248
249    /// Projects a 3D point in the camera frame to 2D image coordinates.
250    /// Returns [`CameraModelError::PointBehindCamera`] if the point lies at or behind
251    /// the camera, and [`CameraModelError::PointOutsideImage`] if the radial polynomial
252    /// becomes non-positive (the model cannot represent such rays).
253    fn project(&self, p_cam: &Vector3<f64>) -> Result<Vector2<f64>, CameraModelError> {
254        if !self.check_projection_condition(p_cam.z) {
255            return Err(CameraModelError::PointBehindCamera {
256                z: p_cam.z,
257                min_z: crate::GEOMETRIC_PRECISION,
258            });
259        }
260
261        let inv_z = 1.0 / p_cam.z;
262        let x_prime = p_cam.x * inv_z;
263        let y_prime = p_cam.y * inv_z;
264
265        let r2 = x_prime * x_prime + y_prime * y_prime;
266        let r4 = r2 * r2;
267        let r6 = r4 * r2;
268
269        let (k1, k2, p1, p2, k3) = self.distortion_params();
270
271        let radial = 1.0 + k1 * r2 + k2 * r4 + k3 * r6;
272
273        if radial <= crate::GEOMETRIC_PRECISION {
274            return Err(CameraModelError::PointOutsideImage {
275                x: x_prime,
276                y: y_prime,
277            });
278        }
279
280        let xy = x_prime * y_prime;
281        let dx = 2.0 * p1 * xy + p2 * (r2 + 2.0 * x_prime * x_prime);
282        let dy = p1 * (r2 + 2.0 * y_prime * y_prime) + 2.0 * p2 * xy;
283
284        let x_distorted = radial * x_prime + dx;
285        let y_distorted = radial * y_prime + dy;
286
287        Ok(Vector2::new(
288            self.pinhole.fx * x_distorted + self.pinhole.cx,
289            self.pinhole.fy * y_distorted + self.pinhole.cy,
290        ))
291    }
292
293    /// Unprojects a 2D image point to a unit 3D ray. Uses Newton-Raphson on the
294    /// distortion residual; returns [`CameraModelError::NumericalError`] on singular
295    /// Jacobian or non-convergence.
296    fn unproject(&self, point_2d: &Vector2<f64>) -> Result<Vector3<f64>, CameraModelError> {
297        let u = point_2d.x;
298        let v = point_2d.y;
299
300        let x_distorted = (u - self.pinhole.cx) / self.pinhole.fx;
301        let y_distorted = (v - self.pinhole.cy) / self.pinhole.fy;
302        let target_distorted_point = Vector2::new(x_distorted, y_distorted);
303
304        let mut point = target_distorted_point;
305
306        const CONVERGENCE_EPS: f64 = crate::CONVERGENCE_THRESHOLD;
307        const MAX_ITERATIONS: u32 = 100;
308
309        let (k1, k2, p1, p2, k3) = self.distortion_params();
310
311        for iteration in 0..MAX_ITERATIONS {
312            let x = point.x;
313            let y = point.y;
314
315            let r2 = x * x + y * y;
316            let r4 = r2 * r2;
317            let r6 = r4 * r2;
318
319            let radial = 1.0 + k1 * r2 + k2 * r4 + k3 * r6;
320
321            let xy = x * y;
322            let dx = 2.0 * p1 * xy + p2 * (r2 + 2.0 * x * x);
323            let dy = p1 * (r2 + 2.0 * y * y) + 2.0 * p2 * xy;
324
325            let x_dist = radial * x + dx;
326            let y_dist = radial * y + dy;
327
328            let fx = x_dist - target_distorted_point.x;
329            let fy = y_dist - target_distorted_point.y;
330
331            if fx.abs() < CONVERGENCE_EPS && fy.abs() < CONVERGENCE_EPS {
332                break;
333            }
334
335            let dradial_dr2 = k1 + 2.0 * k2 * r2 + 3.0 * k3 * r4;
336
337            let dfx_dx = radial + 2.0 * x * dradial_dr2 * x + 2.0 * p1 * y + 2.0 * p2 * (3.0 * x);
338            let dfx_dy = 2.0 * x * dradial_dr2 * y + 2.0 * p1 * x + 2.0 * p2 * y;
339            let dfy_dx = 2.0 * y * dradial_dr2 * x + 2.0 * p1 * x + 2.0 * p2 * y;
340            let dfy_dy = radial + 2.0 * y * dradial_dr2 * y + 2.0 * p1 * (3.0 * y) + 2.0 * p2 * x;
341
342            let jacobian = Matrix2::new(dfx_dx, dfx_dy, dfy_dx, dfy_dy);
343
344            let det = jacobian[(0, 0)] * jacobian[(1, 1)] - jacobian[(0, 1)] * jacobian[(1, 0)];
345
346            if det.abs() < crate::GEOMETRIC_PRECISION {
347                return Err(CameraModelError::NumericalError {
348                    operation: "unprojection".to_string(),
349                    details: "Singular Jacobian in RadTan unprojection".to_string(),
350                });
351            }
352
353            let inv_det = 1.0 / det;
354            let delta_x = inv_det * (jacobian[(1, 1)] * (-fx) - jacobian[(0, 1)] * (-fy));
355            let delta_y = inv_det * (-jacobian[(1, 0)] * (-fx) + jacobian[(0, 0)] * (-fy));
356
357            point.x += delta_x;
358            point.y += delta_y;
359
360            if iteration == MAX_ITERATIONS - 1 {
361                return Err(CameraModelError::NumericalError {
362                    operation: "unprojection".to_string(),
363                    details: "RadTan unprojection did not converge".to_string(),
364                });
365            }
366        }
367
368        let r2 = point.x * point.x + point.y * point.y;
369        let norm = (1.0 + r2).sqrt();
370        let norm_inv = 1.0 / norm;
371
372        Ok(Vector3::new(
373            point.x * norm_inv,
374            point.y * norm_inv,
375            norm_inv,
376        ))
377    }
378
379    /// 2×3 Jacobian ∂(u,v)/∂(x,y,z). See the
380    /// [cookbook](../doc/cookbook/src/rad-tan.html#jacobians) for the full derivation.
381    fn jacobian_point(&self, p_cam: &Vector3<f64>) -> Self::PointJacobian {
382        let inv_z = 1.0 / p_cam.z;
383        let x_prime = p_cam.x * inv_z;
384        let y_prime = p_cam.y * inv_z;
385
386        let r2 = x_prime * x_prime + y_prime * y_prime;
387        let r4 = r2 * r2;
388        let r6 = r4 * r2;
389
390        let (k1, k2, p1, p2, k3) = self.distortion_params();
391
392        let radial = 1.0 + k1 * r2 + k2 * r4 + k3 * r6;
393        let dradial_dr2 = k1 + 2.0 * k2 * r2 + 3.0 * k3 * r4;
394
395        let dx_dist_dx_prime = radial
396            + 2.0 * x_prime * x_prime * dradial_dr2
397            + 2.0 * p1 * y_prime
398            + 6.0 * p2 * x_prime;
399
400        let dx_dist_dy_prime =
401            2.0 * x_prime * y_prime * dradial_dr2 + 2.0 * p1 * x_prime + 2.0 * p2 * y_prime;
402
403        let dy_dist_dx_prime =
404            2.0 * y_prime * x_prime * dradial_dr2 + 2.0 * p1 * x_prime + 2.0 * p2 * y_prime;
405
406        let dy_dist_dy_prime = radial
407            + 2.0 * y_prime * y_prime * dradial_dr2
408            + 6.0 * p1 * y_prime
409            + 2.0 * p2 * x_prime;
410
411        let du_dx = self.pinhole.fx * (dx_dist_dx_prime * inv_z);
412        let du_dy = self.pinhole.fx * (dx_dist_dy_prime * inv_z);
413        let du_dz = self.pinhole.fx
414            * (dx_dist_dx_prime * (-x_prime * inv_z) + dx_dist_dy_prime * (-y_prime * inv_z));
415
416        let dv_dx = self.pinhole.fy * (dy_dist_dx_prime * inv_z);
417        let dv_dy = self.pinhole.fy * (dy_dist_dy_prime * inv_z);
418        let dv_dz = self.pinhole.fy
419            * (dy_dist_dx_prime * (-x_prime * inv_z) + dy_dist_dy_prime * (-y_prime * inv_z));
420
421        SMatrix::<f64, 2, 3>::new(du_dx, du_dy, du_dz, dv_dx, dv_dy, dv_dz)
422    }
423
424    /// 2×9 Jacobian ∂(u,v)/∂[fx, fy, cx, cy, k1, k2, p1, p2, k3]. See the
425    /// [cookbook](../doc/cookbook/src/rad-tan.html#jacobians) for the full derivation.
426    fn jacobian_intrinsics(&self, p_cam: &Vector3<f64>) -> Self::IntrinsicJacobian {
427        let inv_z = 1.0 / p_cam.z;
428        let x_prime = p_cam.x * inv_z;
429        let y_prime = p_cam.y * inv_z;
430
431        let r2 = x_prime * x_prime + y_prime * y_prime;
432        let r4 = r2 * r2;
433        let r6 = r4 * r2;
434
435        let (k1, k2, p1, p2, k3) = self.distortion_params();
436
437        let radial = 1.0 + k1 * r2 + k2 * r4 + k3 * r6;
438
439        let xy = x_prime * y_prime;
440        let dx = 2.0 * p1 * xy + p2 * (r2 + 2.0 * x_prime * x_prime);
441        let dy = p1 * (r2 + 2.0 * y_prime * y_prime) + 2.0 * p2 * xy;
442
443        let x_distorted = radial * x_prime + dx;
444        let y_distorted = radial * y_prime + dy;
445
446        let du_dk1 = self.pinhole.fx * x_prime * r2;
447        let du_dk2 = self.pinhole.fx * x_prime * r4;
448        let du_dp1 = self.pinhole.fx * 2.0 * xy;
449        let du_dp2 = self.pinhole.fx * (r2 + 2.0 * x_prime * x_prime);
450        let du_dk3 = self.pinhole.fx * x_prime * r6;
451
452        let dv_dk1 = self.pinhole.fy * y_prime * r2;
453        let dv_dk2 = self.pinhole.fy * y_prime * r4;
454        let dv_dp1 = self.pinhole.fy * (r2 + 2.0 * y_prime * y_prime);
455        let dv_dp2 = self.pinhole.fy * 2.0 * xy;
456        let dv_dk3 = self.pinhole.fy * y_prime * r6;
457
458        SMatrix::<f64, 2, 9>::from_row_slice(&[
459            x_distorted,
460            0.0,
461            1.0,
462            0.0,
463            du_dk1,
464            du_dk2,
465            du_dp1,
466            du_dp2,
467            du_dk3,
468            0.0,
469            y_distorted,
470            0.0,
471            1.0,
472            dv_dk1,
473            dv_dk2,
474            dv_dp1,
475            dv_dp2,
476            dv_dk3,
477        ])
478    }
479
480    /// Validates the camera parameters.
481    ///
482    /// # Validation Rules
483    ///
484    /// - `fx`, `fy` must be positive (> 0) and finite
485    /// - `cx`, `cy` must be finite
486    /// - `k₁`, `k₂`, `p₁`, `p₂`, `k₃` must be finite
487    ///
488    /// # Errors
489    ///
490    /// Returns [`CameraModelError`] if any rule is violated.
491    fn validate_params(&self) -> Result<(), CameraModelError> {
492        self.pinhole.validate()?;
493        self.get_distortion().validate()
494    }
495
496    /// Returns the pinhole parameters.
497    fn get_pinhole_params(&self) -> PinholeParams {
498        self.pinhole
499    }
500
501    /// Returns the distortion model (must be [`DistortionModel::BrownConrady`]).
502    fn get_distortion(&self) -> DistortionModel {
503        self.distortion
504    }
505
506    /// Returns the model name: `"rad_tan"`.
507    fn get_model_name(&self) -> &'static str {
508        "rad_tan"
509    }
510}
511
512#[cfg(test)]
513mod tests {
514    use super::*;
515    use nalgebra::{Matrix2xX, Matrix3xX};
516
517    type TestResult = Result<(), Box<dyn std::error::Error>>;
518
519    #[test]
520    fn test_radtan_camera_creation() -> TestResult {
521        let pinhole = PinholeParams::new(300.0, 300.0, 320.0, 240.0)?;
522        let distortion = DistortionModel::BrownConrady {
523            k1: 0.1,
524            k2: 0.01,
525            p1: 0.001,
526            p2: 0.002,
527            k3: 0.001,
528        };
529        let camera = RadTanCamera::new(pinhole, distortion)?;
530        assert_eq!(camera.pinhole.fx, 300.0);
531        let (k1, _, p1, _, _) = camera.distortion_params();
532        assert_eq!(k1, 0.1);
533        assert_eq!(p1, 0.001);
534        Ok(())
535    }
536
537    #[test]
538    fn test_projection_at_optical_axis() -> TestResult {
539        let pinhole = PinholeParams::new(300.0, 300.0, 320.0, 240.0)?;
540        let distortion = DistortionModel::BrownConrady {
541            k1: 0.0,
542            k2: 0.0,
543            p1: 0.0,
544            p2: 0.0,
545            k3: 0.0,
546        };
547        let camera = RadTanCamera::new(pinhole, distortion)?;
548        let p_cam = Vector3::new(0.0, 0.0, 1.0);
549        let uv = camera.project(&p_cam)?;
550
551        assert!((uv.x - 320.0).abs() < crate::PROJECTION_TEST_TOLERANCE);
552        assert!((uv.y - 240.0).abs() < crate::PROJECTION_TEST_TOLERANCE);
553
554        Ok(())
555    }
556
557    #[test]
558    fn test_jacobian_point_numerical() -> TestResult {
559        let pinhole = PinholeParams::new(300.0, 300.0, 320.0, 240.0)?;
560        let distortion = DistortionModel::BrownConrady {
561            k1: 0.1,
562            k2: 0.01,
563            p1: 0.001,
564            p2: 0.002,
565            k3: 0.001,
566        };
567        let camera = RadTanCamera::new(pinhole, distortion)?;
568        let p_cam = Vector3::new(0.1, 0.2, 1.0);
569
570        let jac_analytical = camera.jacobian_point(&p_cam);
571        let eps = crate::NUMERICAL_DERIVATIVE_EPS;
572
573        for i in 0..3 {
574            let mut p_plus = p_cam;
575            let mut p_minus = p_cam;
576            p_plus[i] += eps;
577            p_minus[i] -= eps;
578
579            let uv_plus = camera.project(&p_plus)?;
580            let uv_minus = camera.project(&p_minus)?;
581            let num_jac = (uv_plus - uv_minus) / (2.0 * eps);
582
583            for r in 0..2 {
584                assert!(
585                    jac_analytical[(r, i)].is_finite(),
586                    "Jacobian [{r},{i}] is not finite"
587                );
588                let diff = (jac_analytical[(r, i)] - num_jac[r]).abs();
589                assert!(
590                    diff < crate::JACOBIAN_TEST_TOLERANCE,
591                    "Mismatch at ({}, {})",
592                    r,
593                    i
594                );
595            }
596        }
597        Ok(())
598    }
599
600    #[test]
601    fn test_jacobian_intrinsics_numerical() -> TestResult {
602        let pinhole = PinholeParams::new(300.0, 300.0, 320.0, 240.0)?;
603        let distortion = DistortionModel::BrownConrady {
604            k1: 0.1,
605            k2: 0.01,
606            p1: 0.001,
607            p2: 0.002,
608            k3: 0.001,
609        };
610        let camera = RadTanCamera::new(pinhole, distortion)?;
611        let p_cam = Vector3::new(0.1, 0.2, 1.0);
612
613        let jac_analytical = camera.jacobian_intrinsics(&p_cam);
614        let params: DVector<f64> = (&camera).into();
615        let eps = crate::NUMERICAL_DERIVATIVE_EPS;
616
617        for i in 0..9 {
618            let mut params_plus = params.clone();
619            let mut params_minus = params.clone();
620            params_plus[i] += eps;
621            params_minus[i] -= eps;
622
623            let cam_plus = RadTanCamera::try_from(params_plus.as_slice())?;
624            let cam_minus = RadTanCamera::try_from(params_minus.as_slice())?;
625
626            let uv_plus = cam_plus.project(&p_cam)?;
627            let uv_minus = cam_minus.project(&p_cam)?;
628            let num_jac = (uv_plus - uv_minus) / (2.0 * eps);
629
630            for r in 0..2 {
631                assert!(
632                    jac_analytical[(r, i)].is_finite(),
633                    "Jacobian [{r},{i}] is not finite"
634                );
635                let diff = (jac_analytical[(r, i)] - num_jac[r]).abs();
636                assert!(
637                    diff < crate::JACOBIAN_TEST_TOLERANCE,
638                    "Mismatch at ({}, {})",
639                    r,
640                    i
641                );
642            }
643        }
644        Ok(())
645    }
646
647    #[test]
648    fn test_rad_tan_from_into_traits() -> TestResult {
649        let pinhole = PinholeParams::new(300.0, 300.0, 320.0, 240.0)?;
650        let distortion = DistortionModel::BrownConrady {
651            k1: 0.1,
652            k2: 0.01,
653            p1: 0.001,
654            p2: 0.002,
655            k3: 0.001,
656        };
657        let camera = RadTanCamera::new(pinhole, distortion)?;
658
659        // Test conversion to DVector
660        let params: DVector<f64> = (&camera).into();
661        assert_eq!(params.len(), 9);
662        assert_eq!(params[0], 300.0);
663        assert_eq!(params[1], 300.0);
664        assert_eq!(params[2], 320.0);
665        assert_eq!(params[3], 240.0);
666        assert_eq!(params[4], 0.1);
667        assert_eq!(params[5], 0.01);
668        assert_eq!(params[6], 0.001);
669        assert_eq!(params[7], 0.002);
670        assert_eq!(params[8], 0.001);
671
672        // Test conversion to array
673        let arr: [f64; 9] = (&camera).into();
674        assert_eq!(
675            arr,
676            [300.0, 300.0, 320.0, 240.0, 0.1, 0.01, 0.001, 0.002, 0.001]
677        );
678
679        // Test conversion from slice
680        let params_slice = [350.0, 350.0, 330.0, 250.0, 0.2, 0.02, 0.002, 0.003, 0.002];
681        let camera2 = RadTanCamera::try_from(&params_slice[..])?;
682        assert_eq!(camera2.pinhole.fx, 350.0);
683        assert_eq!(camera2.pinhole.fy, 350.0);
684        assert_eq!(camera2.pinhole.cx, 330.0);
685        assert_eq!(camera2.pinhole.cy, 250.0);
686        let (k1, k2, p1, p2, k3) = camera2.distortion_params();
687        assert_eq!(k1, 0.2);
688        assert_eq!(k2, 0.02);
689        assert_eq!(p1, 0.002);
690        assert_eq!(p2, 0.003);
691        assert_eq!(k3, 0.002);
692
693        // Test conversion from array
694        let camera3 =
695            RadTanCamera::from([400.0, 400.0, 340.0, 260.0, 0.3, 0.03, 0.003, 0.004, 0.003]);
696        assert_eq!(camera3.pinhole.fx, 400.0);
697        assert_eq!(camera3.pinhole.fy, 400.0);
698        let (k1, k2, p1, p2, k3) = camera3.distortion_params();
699        assert_eq!(k1, 0.3);
700        assert_eq!(k2, 0.03);
701        assert_eq!(p1, 0.003);
702        assert_eq!(p2, 0.004);
703        assert_eq!(k3, 0.003);
704
705        Ok(())
706    }
707
708    #[test]
709    fn test_linear_estimation() -> TestResult {
710        // Ground truth camera with radial distortion only (p1=p2=0)
711        let gt_pinhole = PinholeParams::new(300.0, 300.0, 320.0, 240.0)?;
712        let gt_distortion = DistortionModel::BrownConrady {
713            k1: 0.05,
714            k2: 0.01,
715            p1: 0.0,
716            p2: 0.0,
717            k3: 0.001,
718        };
719        let gt_camera = RadTanCamera::new(gt_pinhole, gt_distortion)?;
720
721        // Generate synthetic 3D points in camera frame
722        let n_points = 50;
723        let mut pts_3d = Matrix3xX::zeros(n_points);
724        let mut pts_2d = Matrix2xX::zeros(n_points);
725        let mut valid = 0;
726
727        for i in 0..n_points {
728            let angle = i as f64 * 2.0 * std::f64::consts::PI / n_points as f64;
729            let r = 0.1 + 0.3 * (i as f64 / n_points as f64);
730            let p3d = Vector3::new(r * angle.cos(), r * angle.sin(), 1.0);
731
732            if let Ok(p2d) = gt_camera.project(&p3d) {
733                pts_3d.set_column(valid, &p3d);
734                pts_2d.set_column(valid, &p2d);
735                valid += 1;
736            }
737        }
738        let pts_3d = pts_3d.columns(0, valid).into_owned();
739        let pts_2d = pts_2d.columns(0, valid).into_owned();
740
741        // Initial camera with zero distortion
742        let init_pinhole = PinholeParams::new(300.0, 300.0, 320.0, 240.0)?;
743        let init_distortion = DistortionModel::BrownConrady {
744            k1: 0.0,
745            k2: 0.0,
746            p1: 0.0,
747            p2: 0.0,
748            k3: 0.0,
749        };
750        let mut camera = RadTanCamera::new(init_pinhole, init_distortion)?;
751
752        camera.linear_estimation(&pts_3d, &pts_2d)?;
753
754        // Verify reprojection error
755        for i in 0..valid {
756            let col = pts_3d.column(i);
757            let projected = camera.project(&Vector3::new(col[0], col[1], col[2]))?;
758            let err = ((projected.x - pts_2d[(0, i)]).powi(2)
759                + (projected.y - pts_2d[(1, i)]).powi(2))
760            .sqrt();
761            assert!(err < 1.0, "Reprojection error too large: {err}");
762        }
763
764        Ok(())
765    }
766
767    #[test]
768    fn test_project_unproject_round_trip() -> TestResult {
769        let pinhole = PinholeParams::new(300.0, 300.0, 320.0, 240.0)?;
770        let distortion = DistortionModel::BrownConrady {
771            k1: 0.1,
772            k2: 0.01,
773            p1: 0.001,
774            p2: 0.002,
775            k3: 0.001,
776        };
777        let camera = RadTanCamera::new(pinhole, distortion)?;
778
779        let test_points = [
780            Vector3::new(0.1, 0.2, 1.0),
781            Vector3::new(-0.3, 0.1, 2.0),
782            Vector3::new(0.05, -0.1, 0.5),
783        ];
784
785        for p_cam in &test_points {
786            let uv = camera.project(p_cam)?;
787            let ray = camera.unproject(&uv)?;
788            let dot = ray.dot(&p_cam.normalize());
789            assert!(
790                (dot - 1.0).abs() < 1e-5,
791                "Round-trip failed: dot={dot}, expected ~1.0"
792            );
793        }
794
795        Ok(())
796    }
797
798    #[test]
799    fn test_project_returns_error_behind_camera() -> TestResult {
800        let pinhole = PinholeParams::new(300.0, 300.0, 320.0, 240.0)?;
801        let distortion = DistortionModel::BrownConrady {
802            k1: 0.0,
803            k2: 0.0,
804            p1: 0.0,
805            p2: 0.0,
806            k3: 0.0,
807        };
808        let camera = RadTanCamera::new(pinhole, distortion)?;
809        assert!(camera.project(&Vector3::new(0.0, 0.0, -1.0)).is_err());
810        Ok(())
811    }
812
813    #[test]
814    fn test_project_at_min_depth_boundary() -> TestResult {
815        let pinhole = PinholeParams::new(300.0, 300.0, 320.0, 240.0)?;
816        let distortion = DistortionModel::BrownConrady {
817            k1: 0.0,
818            k2: 0.0,
819            p1: 0.0,
820            p2: 0.0,
821            k3: 0.0,
822        };
823        let camera = RadTanCamera::new(pinhole, distortion)?;
824        let p_min = Vector3::new(0.0, 0.0, crate::MIN_DEPTH);
825        if let Ok(uv) = camera.project(&p_min) {
826            assert!(uv.x.is_finite() && uv.y.is_finite());
827        }
828        Ok(())
829    }
830
831    #[test]
832    fn test_projection_off_axis() -> TestResult {
833        let pinhole = PinholeParams::new(300.0, 300.0, 320.0, 240.0)?;
834        let distortion = DistortionModel::BrownConrady {
835            k1: 0.1,
836            k2: 0.01,
837            p1: 0.001,
838            p2: 0.002,
839            k3: 0.001,
840        };
841        let camera = RadTanCamera::new(pinhole, distortion)?;
842        let p_cam = Vector3::new(0.3, 0.0, 1.0);
843        let uv = camera.project(&p_cam)?;
844        assert!(
845            uv.x > 320.0,
846            "off-axis point should project right of principal point"
847        );
848        assert!(
849            (uv.y - 240.0).abs() < 5.0,
850            "y should be close to cy for horizontal offset"
851        );
852        Ok(())
853    }
854
855    #[test]
856    fn test_unproject_center_pixel() -> TestResult {
857        let pinhole = PinholeParams::new(300.0, 300.0, 320.0, 240.0)?;
858        let distortion = DistortionModel::BrownConrady {
859            k1: 0.1,
860            k2: 0.01,
861            p1: 0.0,
862            p2: 0.0,
863            k3: 0.001,
864        };
865        let camera = RadTanCamera::new(pinhole, distortion)?;
866        let uv = Vector2::new(320.0, 240.0);
867        let ray = camera.unproject(&uv)?;
868        assert!(ray.x.abs() < 1e-5, "x should be ~0, got {}", ray.x);
869        assert!(ray.y.abs() < 1e-5, "y should be ~0, got {}", ray.y);
870        assert!((ray.z - 1.0).abs() < 1e-5, "z should be ~1, got {}", ray.z);
871        Ok(())
872    }
873
874    #[test]
875    fn test_batch_projection_matches_individual() -> TestResult {
876        let pinhole = PinholeParams::new(300.0, 300.0, 320.0, 240.0)?;
877        let distortion = DistortionModel::BrownConrady {
878            k1: 0.1,
879            k2: 0.01,
880            p1: 0.001,
881            p2: 0.002,
882            k3: 0.001,
883        };
884        let camera = RadTanCamera::new(pinhole, distortion)?;
885        let pts = Matrix3xX::from_columns(&[
886            Vector3::new(0.0, 0.0, 1.0),
887            Vector3::new(0.3, 0.2, 1.5),
888            Vector3::new(-0.4, 0.1, 2.0),
889        ]);
890        let batch = camera.project_batch(&pts);
891        for i in 0..3 {
892            let col = pts.column(i);
893            let p = camera.project(&Vector3::new(col[0], col[1], col[2]))?;
894            assert!(
895                (batch[(0, i)] - p.x).abs() < 1e-10,
896                "batch u mismatch at col {i}"
897            );
898            assert!(
899                (batch[(1, i)] - p.y).abs() < 1e-10,
900                "batch v mismatch at col {i}"
901            );
902        }
903        Ok(())
904    }
905
906    #[test]
907    fn test_jacobian_dimensions() -> TestResult {
908        let pinhole = PinholeParams::new(300.0, 300.0, 320.0, 240.0)?;
909        let distortion = DistortionModel::BrownConrady {
910            k1: 0.1,
911            k2: 0.01,
912            p1: 0.001,
913            p2: 0.002,
914            k3: 0.001,
915        };
916        let camera = RadTanCamera::new(pinhole, distortion)?;
917        let p_cam = Vector3::new(0.1, 0.2, 1.0);
918        let jac_point = camera.jacobian_point(&p_cam);
919        assert_eq!(jac_point.nrows(), 2);
920        assert_eq!(jac_point.ncols(), 3);
921        let jac_intr = camera.jacobian_intrinsics(&p_cam);
922        assert_eq!(jac_intr.nrows(), 2);
923        assert_eq!(jac_intr.ncols(), 9); // RadTanCamera::INTRINSIC_DIM = 9
924        Ok(())
925    }
926}
apex_camera_models/rad_tan.rs

apex_camera_models/
rad_tan.rs
