vision_calibration_optim/problems/
scheimpflug_intrinsics.rs

1//! Scheimpflug planar intrinsics optimization using the backend-agnostic IR.
2
3use crate::Error;
4use crate::backend::{BackendKind, BackendSolveOptions, SolveReport, solve_with_backend};
5use crate::ir::RobustLoss;
6use crate::params::distortion::unpack_distortion;
7use crate::params::intrinsics::unpack_intrinsics;
8use crate::params::pose_se3::se3_dvec_to_iso3;
9use crate::problems::planar_family_shared::{
10    PlanarReprojectionFactorModel, PlanarReprojectionIrOptions, PlanarSensorIrOptions,
11    build_planar_reprojection_ir,
12};
13use anyhow::{Result as AnyhowResult, anyhow, ensure};
14use nalgebra::DVectorView;
15use serde::{Deserialize, Serialize};
16use vision_calibration_core::{
17    BrownConrady5, Camera, DistortionFixMask, FxFyCxCySkew, IntrinsicsFixMask, Iso3, Pinhole,
18    PlanarDataset, Real, ScheimpflugParams,
19};
20
21/// Mask for Scheimpflug tilt parameters.
22#[derive(Debug, Clone, Copy, PartialEq, Eq, Default, Serialize, Deserialize)]
23pub struct ScheimpflugFixMask {
24    /// Keep `tilt_x` fixed during optimization.
25    pub tilt_x: bool,
26    /// Keep `tilt_y` fixed during optimization.
27    pub tilt_y: bool,
28}
29
30impl ScheimpflugFixMask {
31    fn as_flags(self) -> [bool; 2] {
32        [self.tilt_x, self.tilt_y]
33    }
34}
35
36/// Initial/refined parameters for Scheimpflug intrinsics optimization.
37#[derive(Debug, Clone, Serialize, Deserialize)]
38pub struct ScheimpflugIntrinsicsParams {
39    /// Camera intrinsics.
40    pub intrinsics: FxFyCxCySkew<Real>,
41    /// Brown-Conrady distortion parameters.
42    pub distortion: BrownConrady5<Real>,
43    /// Scheimpflug sensor tilt parameters.
44    pub sensor: ScheimpflugParams,
45    /// Target poses per view (`camera_se3_target`).
46    pub camera_se3_target: Vec<Iso3>,
47}
48
49impl ScheimpflugIntrinsicsParams {
50    /// Construct parameter pack with validation.
51    pub fn new(
52        intrinsics: FxFyCxCySkew<Real>,
53        distortion: BrownConrady5<Real>,
54        sensor: ScheimpflugParams,
55        camera_se3_target: Vec<Iso3>,
56    ) -> Result<Self, Error> {
57        if camera_se3_target.is_empty() {
58            return Err(Error::InsufficientData { need: 1, got: 0 });
59        }
60        Ok(Self {
61            intrinsics,
62            distortion,
63            sensor,
64            camera_se3_target,
65        })
66    }
67}
68
69/// Solve options for Scheimpflug intrinsics optimization.
70#[derive(Debug, Clone, Serialize, Deserialize)]
71pub struct ScheimpflugIntrinsicsSolveOptions {
72    /// Robust loss applied per observation.
73    pub robust_loss: RobustLoss,
74    /// Mask for fixing intrinsics parameters.
75    pub fix_intrinsics: IntrinsicsFixMask,
76    /// Mask for fixing distortion parameters.
77    pub fix_distortion: DistortionFixMask,
78    /// Mask for fixing Scheimpflug tilt parameters.
79    pub fix_scheimpflug: ScheimpflugFixMask,
80    /// Indices of poses to keep fixed.
81    pub fix_poses: Vec<usize>,
82}
83
84impl Default for ScheimpflugIntrinsicsSolveOptions {
85    fn default() -> Self {
86        Self {
87            robust_loss: RobustLoss::None,
88            fix_intrinsics: IntrinsicsFixMask::default(),
89            fix_distortion: DistortionFixMask::radial_only(),
90            fix_scheimpflug: ScheimpflugFixMask::default(),
91            fix_poses: vec![0],
92        }
93    }
94}
95
96/// Optimization result for Scheimpflug intrinsics.
97#[derive(Debug, Clone, Serialize, Deserialize)]
98pub struct ScheimpflugIntrinsicsEstimate {
99    /// Refined parameters.
100    pub params: ScheimpflugIntrinsicsParams,
101    /// Backend solve report.
102    pub report: SolveReport,
103    /// Mean reprojection error in pixels.
104    pub mean_reproj_error: f64,
105}
106
107fn build_scheimpflug_intrinsics_ir(
108    dataset: &PlanarDataset,
109    initial: &ScheimpflugIntrinsicsParams,
110    opts: &ScheimpflugIntrinsicsSolveOptions,
111) -> AnyhowResult<(
112    crate::ir::ProblemIR,
113    std::collections::HashMap<String, nalgebra::DVector<f64>>,
114)> {
115    build_planar_reprojection_ir(
116        dataset,
117        &initial.intrinsics,
118        &initial.distortion,
119        &initial.camera_se3_target,
120        &PlanarReprojectionIrOptions {
121            robust_loss: opts.robust_loss,
122            fix_intrinsics_indices: opts.fix_intrinsics.to_indices(),
123            fix_distortion_indices: opts.fix_distortion.to_indices(),
124            fix_pose_indices: opts.fix_poses.clone(),
125            sensor: Some(PlanarSensorIrOptions {
126                params: initial.sensor,
127                fix_indices: opts.fix_scheimpflug.as_flags(),
128            }),
129            factor_model: PlanarReprojectionFactorModel::PinholeDistortionScheimpflug,
130        },
131    )
132}
133
134/// Optimize Scheimpflug intrinsics using the default tiny-solver backend.
135///
136/// # Errors
137///
138/// Returns [`Error`] if IR construction or solver backend fails.
139pub fn optimize_scheimpflug_intrinsics(
140    dataset: &PlanarDataset,
141    initial: &ScheimpflugIntrinsicsParams,
142    opts: ScheimpflugIntrinsicsSolveOptions,
143    backend_opts: BackendSolveOptions,
144) -> Result<ScheimpflugIntrinsicsEstimate, Error> {
145    optimize_scheimpflug_intrinsics_with_backend(
146        dataset,
147        initial,
148        opts,
149        BackendKind::TinySolver,
150        backend_opts,
151    )
152}
153
154/// Optimize Scheimpflug intrinsics using the selected backend.
155///
156/// # Errors
157///
158/// Returns [`Error`] if IR construction or solver backend fails.
159pub fn optimize_scheimpflug_intrinsics_with_backend(
160    dataset: &PlanarDataset,
161    initial: &ScheimpflugIntrinsicsParams,
162    opts: ScheimpflugIntrinsicsSolveOptions,
163    backend: BackendKind,
164    backend_opts: BackendSolveOptions,
165) -> Result<ScheimpflugIntrinsicsEstimate, Error> {
166    let (ir, initial_map) = build_scheimpflug_intrinsics_ir(dataset, initial, &opts)?;
167    let solution = solve_with_backend(backend, &ir, &initial_map, &backend_opts)?;
168
169    let intrinsics = unpack_intrinsics(
170        solution
171            .params
172            .get("cam")
173            .ok_or_else(|| anyhow!("missing intrinsics solution block"))?
174            .as_view(),
175    )?;
176    let distortion = unpack_distortion(
177        solution
178            .params
179            .get("dist")
180            .ok_or_else(|| anyhow!("missing distortion solution block"))?
181            .as_view(),
182    )?;
183    let sensor = unpack_scheimpflug(
184        solution
185            .params
186            .get("sensor")
187            .ok_or_else(|| anyhow!("missing sensor solution block"))?
188            .as_view(),
189    )?;
190
191    let mut optimized_poses = Vec::with_capacity(dataset.num_views());
192    for view_idx in 0..dataset.num_views() {
193        let key = format!("pose/{view_idx}");
194        let pose = solution
195            .params
196            .get(&key)
197            .ok_or_else(|| anyhow!("missing {key} solution block"))?;
198        optimized_poses.push(se3_dvec_to_iso3(pose.as_view())?);
199    }
200
201    let mean_reproj_error =
202        compute_mean_reproj_error(dataset, intrinsics, distortion, sensor, &optimized_poses);
203
204    Ok(ScheimpflugIntrinsicsEstimate {
205        params: ScheimpflugIntrinsicsParams {
206            intrinsics,
207            distortion,
208            sensor,
209            camera_se3_target: optimized_poses,
210        },
211        report: solution.solve_report,
212        mean_reproj_error,
213    })
214}
215
216fn unpack_scheimpflug(values: DVectorView<'_, f64>) -> AnyhowResult<ScheimpflugParams> {
217    ensure!(values.len() == 2, "scheimpflug block must have 2 entries");
218    Ok(ScheimpflugParams {
219        tilt_x: values[0],
220        tilt_y: values[1],
221    })
222}
223
224fn compute_mean_reproj_error(
225    dataset: &PlanarDataset,
226    intrinsics: FxFyCxCySkew<f64>,
227    distortion: BrownConrady5<f64>,
228    sensor: ScheimpflugParams,
229    poses: &[Iso3],
230) -> f64 {
231    let camera = Camera::new(Pinhole, distortion, sensor.compile(), intrinsics);
232    let mut sum = 0.0;
233    let mut count = 0usize;
234
235    for (view, pose) in dataset.views.iter().zip(poses.iter()) {
236        for (p3d, p2d) in view.obs.points_3d.iter().zip(view.obs.points_2d.iter()) {
237            let p_cam = pose.transform_point(p3d);
238            let Some(projected) = camera.project_point(&p_cam) else {
239                continue;
240            };
241            let err = (projected - *p2d).norm();
242            if err.is_finite() {
243                sum += err;
244                count += 1;
245            }
246        }
247    }
248
249    if count == 0 { 0.0 } else { sum / count as f64 }
250}
vision_calibration_optim/problems/scheimpflug_intrinsics.rs

vision_calibration_optim/problems/
scheimpflug_intrinsics.rs
