vision_calibration_optim/problems/
laserline_bundle.rs

1//! Laserline bundle adjustment: joint optimization of camera intrinsics, distortion,
2//! target poses, and laser plane parameters.
3//!
4//! This problem builder combines standard planar calibration observations (corners)
5//! with laser line observations to estimate laser plane parameters in camera frame.
6
7use crate::Error;
8use crate::backend::{BackendKind, BackendSolveOptions, SolveReport, solve_with_backend};
9use crate::factors::laserline::{
10    laser_line_dist_normalized_generic, laser_plane_pixel_residual_generic,
11};
12use crate::ir::{FactorKind, FixedMask, ManifoldKind, ProblemIR, ResidualBlock, RobustLoss};
13use crate::params::distortion::{DISTORTION_DIM, pack_distortion, unpack_distortion};
14use crate::params::intrinsics::{INTRINSICS_DIM, pack_intrinsics, unpack_intrinsics};
15use crate::params::laser_plane::LaserPlane;
16use crate::params::pose_se3::{iso3_to_se3_dvec, se3_dvec_to_iso3};
17use anyhow::{Result as AnyhowResult, anyhow, ensure};
18use nalgebra::{DVector, DVectorView};
19use serde::{Deserialize, Serialize};
20use std::collections::HashMap;
21use vision_calibration_core::{
22    BrownConrady5, Camera, FxFyCxCySkew, Iso3, Pinhole, Pt2, Real, ScheimpflugParams, View,
23};
24
25/// Metadata for a laserline view (laser pixels + optional weights).
26#[derive(Debug, Clone, Serialize, Deserialize)]
27pub struct LaserlineMeta {
28    /// Laser line pixel observations.
29    pub laser_pixels: Vec<Pt2>,
30    /// Per-pixel weights (empty = unweighted, all default to 1.0).
31    #[serde(default, skip_serializing_if = "Vec::is_empty")]
32    pub laser_weights: Vec<f64>,
33}
34
35impl LaserlineMeta {
36    /// Validate metadata consistency (non-empty pixels and optional weights).
37    ///
38    /// # Errors
39    ///
40    /// Returns [`Error::InsufficientData`] if `laser_pixels` is empty.
41    /// Returns [`Error::InvalidInput`] if weight count mismatches pixel count, or if any weight is negative.
42    pub fn validate(&self) -> Result<(), Error> {
43        if self.laser_pixels.is_empty() {
44            return Err(Error::InsufficientData { need: 1, got: 0 });
45        }
46        if !self.laser_weights.is_empty() {
47            if self.laser_weights.len() != self.laser_pixels.len() {
48                return Err(Error::invalid_input(format!(
49                    "laser weight count {} must match pixel count {}",
50                    self.laser_weights.len(),
51                    self.laser_pixels.len()
52                )));
53            }
54            if !self.laser_weights.iter().all(|w| *w >= 0.0) {
55                return Err(Error::invalid_input("laser weights must be non-negative"));
56            }
57        }
58        Ok(())
59    }
60
61    /// Return per-pixel weight, defaulting to `1.0` when absent.
62    pub fn laser_weight(&self, idx: usize) -> f64 {
63        if self.laser_weights.is_empty() {
64            1.0
65        } else {
66            self.laser_weights[idx]
67        }
68    }
69}
70
71/// A single view with calibration correspondences and laserline pixels.
72pub type LaserlineView = View<LaserlineMeta>;
73
74/// Dataset for laserline bundle adjustment.
75pub type LaserlineDataset = Vec<LaserlineView>;
76
77/// Initial values for laserline bundle adjustment.
78#[derive(Debug, Clone, Serialize, Deserialize)]
79pub struct LaserlineParams {
80    /// Camera intrinsics.
81    pub intrinsics: FxFyCxCySkew<Real>,
82    /// Brown-Conrady distortion parameters.
83    pub distortion: BrownConrady5<Real>,
84    /// Scheimpflug sensor parameters (use defaults for identity sensor).
85    pub sensor: ScheimpflugParams,
86    /// Per-view target poses (`camera_se3_target`).
87    pub poses: Vec<Iso3>,
88    /// Laser plane parameters in camera frame.
89    pub plane: LaserPlane,
90}
91
92impl LaserlineParams {
93    /// Construct parameter pack with basic cardinality validation.
94    ///
95    /// # Errors
96    ///
97    /// Returns [`Error::InsufficientData`] if `poses` is empty.
98    pub fn new(
99        intrinsics: FxFyCxCySkew<Real>,
100        distortion: BrownConrady5<Real>,
101        sensor: ScheimpflugParams,
102        poses: Vec<Iso3>,
103        plane: LaserPlane,
104    ) -> Result<Self, Error> {
105        if poses.is_empty() {
106            return Err(Error::InsufficientData { need: 1, got: 0 });
107        }
108        Ok(Self {
109            intrinsics,
110            distortion,
111            sensor,
112            poses,
113            plane,
114        })
115    }
116}
117
118/// Result of laserline bundle adjustment.
119#[derive(Debug, Clone, Serialize, Deserialize)]
120pub struct LaserlineEstimate {
121    /// Refined bundle parameters.
122    pub params: LaserlineParams,
123    /// Backend solve report.
124    pub report: SolveReport,
125}
126
127/// Summary statistics for a laserline calibration result.
128#[derive(Debug, Clone, Serialize, Deserialize)]
129pub struct LaserlineStats {
130    /// Mean reprojection error for calibration points (pixels).
131    pub mean_reproj_error: f64,
132    /// Mean laser residual (units depend on residual type).
133    pub mean_laser_error: f64,
134    /// Per-view mean reprojection error (pixels).
135    pub per_view_reproj_errors: Vec<f64>,
136    /// Per-view mean laser residual (same units as `mean_laser_error`).
137    pub per_view_laser_errors: Vec<f64>,
138}
139
140/// Type of laser plane residual to use.
141#[derive(Debug, Clone, Copy, PartialEq, Eq, Default, serde::Serialize, serde::Deserialize)]
142pub enum LaserlineResidualType {
143    /// Point-to-plane distance (original approach).
144    ///
145    /// Undistorts pixel, back-projects to 3D ray, intersects with target plane,
146    /// computes 3D point in camera frame, measures signed distance to laser plane.
147    /// Residual: 1D distance in meters.
148    PointToPlane,
149    /// Line-distance in normalized plane (alternative approach).
150    ///
151    /// Computes 3D intersection line of laser plane and target plane, projects
152    /// line onto z=1 normalized camera plane, undistorts laser pixels to normalized
153    /// coordinates, measures perpendicular distance from pixel to projected line,
154    /// scales by sqrt(fx*fy) for pixel-comparable residual.
155    /// Residual: 1D distance in pixels.
156    #[default]
157    LineDistNormalized,
158}
159
160/// Solve options for laserline bundle adjustment.
161#[derive(Debug, Clone)]
162pub struct LaserlineSolveOptions {
163    /// Robust loss applied to calibration reprojection residuals
164    pub calib_loss: RobustLoss,
165    /// Global weight multiplier for calibration reprojection residuals.
166    pub calib_weight: f64,
167    /// Robust loss applied to laser plane residuals
168    pub laser_loss: RobustLoss,
169    /// Global weight multiplier for laser plane residuals.
170    pub laser_weight: f64,
171    /// Fix camera intrinsics during optimization
172    pub fix_intrinsics: bool,
173    /// Fix distortion parameters during optimization
174    pub fix_distortion: bool,
175    /// Fix k3 distortion parameter (common for typical lenses)
176    pub fix_k3: bool,
177    /// Fix Scheimpflug sensor parameters during optimization
178    pub fix_sensor: bool,
179    /// Indices of poses to fix (e.g., \[0\] to fix first pose for gauge freedom)
180    pub fix_poses: Vec<usize>,
181    /// Indices of planes to fix
182    pub fix_plane: bool,
183    /// Laser residual type: point-to-plane distance or line-distance in normalized plane
184    pub laser_residual_type: LaserlineResidualType,
185}
186
187impl Default for LaserlineSolveOptions {
188    fn default() -> Self {
189        Self {
190            calib_loss: RobustLoss::Huber { scale: 1.0 },
191            calib_weight: 1.0,
192            laser_loss: RobustLoss::Huber { scale: 0.01 }, // Smaller scale for plane residuals
193            laser_weight: 1.0,
194            fix_intrinsics: false,
195            fix_distortion: false,
196            fix_k3: true,
197            fix_sensor: true,
198            fix_poses: vec![0], // Fix first pose by default
199            fix_plane: false,
200            laser_residual_type: LaserlineResidualType::LineDistNormalized, // New default
201        }
202    }
203}
204
205fn pack_scheimpflug(sensor: &ScheimpflugParams) -> DVector<f64> {
206    DVector::from_vec(vec![sensor.tilt_x, sensor.tilt_y])
207}
208
209fn unpack_scheimpflug(sensor: DVectorView<'_, f64>) -> AnyhowResult<ScheimpflugParams> {
210    ensure!(
211        sensor.len() == 2,
212        "Scheimpflug sensor params require 2D vector, got {}",
213        sensor.len()
214    );
215    Ok(ScheimpflugParams {
216        tilt_x: sensor[0],
217        tilt_y: sensor[1],
218    })
219}
220
221/// Compute reprojection and laser residual statistics for a solution.
222///
223/// - Reprojection errors are reported in pixels (mean Euclidean error).
224/// - Laser residual units depend on `residual_type`:
225///   - `PointToPlane`: meters (signed distance to plane)
226///   - `LineDistNormalized`: pixels (normalized line distance scaled by sqrt(fx*fy))
227///
228/// # Errors
229///
230/// Returns [`Error::InvalidInput`] if the dataset and params have different view counts, or if
231/// intrinsics/distortion packing fails.
232pub fn compute_laserline_stats(
233    dataset: &LaserlineDataset,
234    params: &LaserlineParams,
235    residual_type: LaserlineResidualType,
236) -> Result<LaserlineStats, Error> {
237    if dataset.len() != params.poses.len() {
238        return Err(Error::invalid_input(format!(
239            "dataset has {} views but {} poses",
240            dataset.len(),
241            params.poses.len()
242        )));
243    }
244
245    let camera = Camera::new(
246        Pinhole,
247        params.distortion,
248        params.sensor.compile(),
249        params.intrinsics,
250    );
251
252    let intr_vec = pack_intrinsics(&params.intrinsics)?;
253    let dist_vec = pack_distortion(&params.distortion);
254    let sensor_vec = pack_scheimpflug(&params.sensor);
255    let plane_normal = params.plane.normal_to_dvec();
256    let plane_distance = params.plane.distance_to_dvec();
257
258    let mut per_view_reproj_errors = Vec::with_capacity(dataset.len());
259    let mut per_view_laser_errors = Vec::with_capacity(dataset.len());
260
261    let mut total_reproj_sum = 0.0;
262    let mut total_reproj_count = 0usize;
263    let mut total_laser_sum = 0.0;
264    let mut total_laser_count = 0usize;
265
266    for (view_idx, view) in dataset.iter().enumerate() {
267        let pose = &params.poses[view_idx];
268        let pose_vec = iso3_to_se3_dvec(pose);
269
270        let mut view_reproj_sum = 0.0;
271        let mut view_reproj_count = 0usize;
272        for (p3d, p2d) in view.obs.points_3d.iter().zip(view.obs.points_2d.iter()) {
273            let p_cam = pose.transform_point(p3d);
274            if let Some(projected) = camera.project_point(&p_cam) {
275                let err = (projected - *p2d).norm();
276                view_reproj_sum += err;
277                view_reproj_count += 1;
278            }
279        }
280
281        let view_reproj_mean = if view_reproj_count > 0 {
282            view_reproj_sum / view_reproj_count as f64
283        } else {
284            0.0
285        };
286        per_view_reproj_errors.push(view_reproj_mean);
287        total_reproj_sum += view_reproj_sum;
288        total_reproj_count += view_reproj_count;
289
290        let mut view_laser_sum = 0.0;
291        let mut view_laser_count = 0usize;
292        for laser_pixel in &view.meta.laser_pixels {
293            let residual = match residual_type {
294                LaserlineResidualType::PointToPlane => laser_plane_pixel_residual_generic(
295                    intr_vec.as_view(),
296                    dist_vec.as_view(),
297                    sensor_vec.as_view(),
298                    pose_vec.as_view(),
299                    plane_normal.as_view(),
300                    plane_distance.as_view(),
301                    [laser_pixel.x, laser_pixel.y],
302                    1.0,
303                )[0],
304                LaserlineResidualType::LineDistNormalized => laser_line_dist_normalized_generic(
305                    intr_vec.as_view(),
306                    dist_vec.as_view(),
307                    sensor_vec.as_view(),
308                    pose_vec.as_view(),
309                    plane_normal.as_view(),
310                    plane_distance.as_view(),
311                    [laser_pixel.x, laser_pixel.y],
312                    1.0,
313                )[0],
314            };
315            view_laser_sum += residual.abs();
316            view_laser_count += 1;
317        }
318
319        let view_laser_mean = if view_laser_count > 0 {
320            view_laser_sum / view_laser_count as f64
321        } else {
322            0.0
323        };
324        per_view_laser_errors.push(view_laser_mean);
325        total_laser_sum += view_laser_sum;
326        total_laser_count += view_laser_count;
327    }
328
329    if total_reproj_count == 0 {
330        return Err(Error::invalid_input(
331            "no valid reprojection errors for stats",
332        ));
333    }
334    if total_laser_count == 0 {
335        return Err(Error::invalid_input("no laser pixels for stats"));
336    }
337
338    Ok(LaserlineStats {
339        mean_reproj_error: total_reproj_sum / total_reproj_count as f64,
340        mean_laser_error: total_laser_sum / total_laser_count as f64,
341        per_view_reproj_errors,
342        per_view_laser_errors,
343    })
344}
345
346/// Extract solution from backend result.
347fn extract_solution(
348    solution: crate::backend::BackendSolution,
349    num_poses: usize,
350) -> AnyhowResult<LaserlineEstimate> {
351    let intrinsics = unpack_intrinsics(
352        solution
353            .params
354            .get("intrinsics")
355            .ok_or_else(|| anyhow!("missing intrinsics in solution"))?
356            .as_view(),
357    )?;
358
359    let distortion = unpack_distortion(
360        solution
361            .params
362            .get("distortion")
363            .ok_or_else(|| anyhow!("missing distortion in solution"))?
364            .as_view(),
365    )?;
366
367    let mut poses = Vec::new();
368    for i in 0..num_poses {
369        let name = format!("pose_{}", i);
370        let pose_vec = solution
371            .params
372            .get(&name)
373            .ok_or_else(|| anyhow!("missing {} in solution", name))?;
374        poses.push(se3_dvec_to_iso3(pose_vec.as_view())?);
375    }
376
377    let normal_name = "plane_normal".to_owned();
378    let distance_name = "plane_distance".to_owned();
379    let plane_normal = solution
380        .params
381        .get(&normal_name)
382        .ok_or_else(|| anyhow!("missing {} in solution", normal_name))?;
383    let plane_distance = solution
384        .params
385        .get(&distance_name)
386        .ok_or_else(|| anyhow!("missing {} in solution", distance_name))?;
387
388    let plane = LaserPlane::from_split_dvec(plane_normal.as_view(), plane_distance.as_view())?;
389    let sensor = unpack_scheimpflug(
390        solution
391            .params
392            .get("sensor")
393            .ok_or_else(|| anyhow!("missing sensor in solution"))?
394            .as_view(),
395    )?;
396
397    Ok(LaserlineEstimate {
398        params: LaserlineParams::new(intrinsics, distortion, sensor, poses, plane)?,
399        report: solution.solve_report,
400    })
401}
402
403/// Optimize laserline calibration with joint bundle adjustment.
404///
405/// This function jointly optimizes camera intrinsics, distortion parameters,
406/// camera-to-target poses, and laser plane parameters using both calibration
407/// feature observations and laser line observations.
408///
409/// # Errors
410///
411/// Returns [`Error`] if IR construction or solver backend fails.
412///
413/// # Example
414///
415/// ```ignore
416/// use vision_calibration_optim::problems::laserline_bundle::*;
417/// use vision_calibration_core::ScheimpflugParams;
418///
419/// let dataset = views;
420/// let sensor = ScheimpflugParams::default(); // identity sensor
421/// let initial = LaserlineParams::new(intrinsics, distortion, sensor, poses, plane)?;
422/// let opts = LaserlineSolveOptions::default();
423/// let backend_opts = BackendSolveOptions::default();
424///
425/// let result = optimize_laserline(&dataset, &initial, &opts, &backend_opts)?;
426/// println!(
427///     "Laser plane: normal={:?}, distance={}",
428///     result.params.plane.normal,
429///     result.params.plane.distance
430/// );
431/// ```
432pub fn optimize_laserline(
433    dataset: &LaserlineDataset,
434    initial: &LaserlineParams,
435    opts: &LaserlineSolveOptions,
436    backend_opts: &BackendSolveOptions,
437) -> Result<LaserlineEstimate, Error> {
438    let (ir, initial_map) = build_laserline_ir(dataset, initial, opts)?;
439    let solution = solve_with_backend(BackendKind::TinySolver, &ir, &initial_map, backend_opts)?;
440    extract_solution(solution, dataset.len()).map_err(Error::from)
441}
442
443/// Build IR for laserline bundle adjustment.
444fn build_laserline_ir(
445    dataset: &LaserlineDataset,
446    initial: &LaserlineParams,
447    opts: &LaserlineSolveOptions,
448) -> AnyhowResult<(ProblemIR, HashMap<String, DVector<f64>>)> {
449    ensure!(!dataset.is_empty(), "need at least one view");
450    ensure!(
451        dataset.len() == initial.poses.len(),
452        "dataset has {} views but {} initial poses",
453        dataset.len(),
454        initial.poses.len()
455    );
456    for (idx, view) in dataset.iter().enumerate() {
457        view.meta
458            .validate()
459            .map_err(|e| anyhow!("view {}: {}", idx, e))?;
460    }
461
462    let mut ir = ProblemIR::new();
463    let mut initial_map = HashMap::new();
464
465    // Add intrinsics parameter block
466    let intrinsics_fixed = if opts.fix_intrinsics {
467        FixedMask::all_fixed(INTRINSICS_DIM)
468    } else {
469        FixedMask::all_free()
470    };
471    let intrinsics_id = ir.add_param_block(
472        "intrinsics",
473        INTRINSICS_DIM,
474        ManifoldKind::Euclidean,
475        intrinsics_fixed,
476        None,
477    );
478    initial_map.insert(
479        "intrinsics".to_string(),
480        pack_intrinsics(&initial.intrinsics)?,
481    );
482
483    // Add distortion parameter block
484    let distortion_fixed = if opts.fix_distortion {
485        FixedMask::all_fixed(DISTORTION_DIM)
486    } else if opts.fix_k3 {
487        FixedMask::fix_indices(&[2]) // k3 is at index 2
488    } else {
489        FixedMask::all_free()
490    };
491    let distortion_id = ir.add_param_block(
492        "distortion",
493        DISTORTION_DIM,
494        ManifoldKind::Euclidean,
495        distortion_fixed,
496        None,
497    );
498    initial_map.insert(
499        "distortion".to_string(),
500        pack_distortion(&initial.distortion),
501    );
502
503    // Add sensor parameter block (Scheimpflug)
504    let sensor_fixed = if opts.fix_sensor {
505        FixedMask::all_fixed(2)
506    } else {
507        FixedMask::all_free()
508    };
509    let sensor_id = ir.add_param_block("sensor", 2, ManifoldKind::Euclidean, sensor_fixed, None);
510    initial_map.insert("sensor".to_string(), pack_scheimpflug(&initial.sensor));
511
512    // Add pose parameter blocks (one per view)
513    let mut pose_ids = Vec::new();
514    for (i, pose) in initial.poses.iter().enumerate() {
515        let fixed = if opts.fix_poses.contains(&i) {
516            FixedMask::all_fixed(7)
517        } else {
518            FixedMask::all_free()
519        };
520        let name = format!("pose_{}", i);
521        let id = ir.add_param_block(&name, 7, ManifoldKind::SE3, fixed, None);
522        pose_ids.push(id);
523        initial_map.insert(name, iso3_to_se3_dvec(pose));
524    }
525
526    // Add laser plane parameter blocks (unit normal on S2 + scalar distance)
527    let normal_fixed = if opts.fix_plane {
528        FixedMask::all_fixed(3)
529    } else {
530        FixedMask::all_free()
531    };
532    let distance_fixed = if opts.fix_plane {
533        FixedMask::all_fixed(1)
534    } else {
535        FixedMask::all_free()
536    };
537
538    let normal_name = "plane_normal";
539    let distance_name = "plane_distance";
540    let normal_id = ir.add_param_block(normal_name, 3, ManifoldKind::S2, normal_fixed, None);
541    let distance_id = ir.add_param_block(
542        distance_name,
543        1,
544        ManifoldKind::Euclidean,
545        distance_fixed,
546        None,
547    );
548    initial_map.insert(normal_name.to_owned(), initial.plane.normal_to_dvec());
549    initial_map.insert(distance_name.to_owned(), initial.plane.distance_to_dvec());
550
551    // Add residual blocks
552    for (view_idx, view) in dataset.iter().enumerate() {
553        let pose_id = pose_ids[view_idx];
554
555        // Calibration reprojection residuals
556        for (pt_idx, (pt_3d, pt_2d)) in view
557            .obs
558            .points_3d
559            .iter()
560            .zip(&view.obs.points_2d)
561            .enumerate()
562        {
563            let w = view.obs.weight(pt_idx) * opts.calib_weight;
564            ir.add_residual_block(ResidualBlock {
565                params: vec![intrinsics_id, distortion_id, sensor_id, pose_id],
566                loss: opts.calib_loss,
567                factor: FactorKind::ReprojPointPinhole4Dist5Scheimpflug2 {
568                    pw: [pt_3d.x, pt_3d.y, pt_3d.z],
569                    uv: [pt_2d.x, pt_2d.y],
570                    w,
571                },
572                residual_dim: 2,
573            });
574        }
575
576        // Laser plane residuals (one per plane, typically just one)
577        for (laser_idx, laser_pixel) in view.meta.laser_pixels.iter().enumerate() {
578            let w = view.meta.laser_weight(laser_idx) * opts.laser_weight;
579
580            // Select factor type based on options
581            let factor = match opts.laser_residual_type {
582                LaserlineResidualType::PointToPlane => FactorKind::LaserPlanePixel {
583                    laser_pixel: [laser_pixel.x, laser_pixel.y],
584                    w,
585                },
586                LaserlineResidualType::LineDistNormalized => FactorKind::LaserLineDist2D {
587                    laser_pixel: [laser_pixel.x, laser_pixel.y],
588                    w,
589                },
590            };
591
592            ir.add_residual_block(ResidualBlock {
593                params: vec![
594                    intrinsics_id,
595                    distortion_id,
596                    sensor_id,
597                    pose_id,
598                    normal_id,
599                    distance_id,
600                ],
601                loss: opts.laser_loss,
602                factor,
603                residual_dim: 1,
604            });
605        }
606    }
607
608    Ok((ir, initial_map))
609}
610
611#[cfg(test)]
612mod tests {
613    use super::*;
614    use vision_calibration_core::{
615        BrownConrady5, CorrespondenceView, FxFyCxCySkew, Pt3, ScheimpflugParams,
616    };
617
618    #[test]
619    fn ir_validation_catches_missing_param() {
620        let dataset = vec![View::new(
621            CorrespondenceView::new(
622                vec![Pt3::new(0.0, 0.0, 0.0); 4],
623                vec![Pt2::new(100.0, 100.0); 4],
624            )
625            .unwrap(),
626            LaserlineMeta {
627                laser_pixels: vec![Pt2::new(200.0, 200.0); 5],
628                laser_weights: Vec::new(),
629            },
630        )];
631
632        let intrinsics = FxFyCxCySkew {
633            fx: 800.0,
634            fy: 800.0,
635            cx: 512.0,
636            cy: 384.0,
637            skew: 0.0,
638        };
639        let distortion = BrownConrady5 {
640            k1: 0.0,
641            k2: 0.0,
642            k3: 0.0,
643            p1: 0.0,
644            p2: 0.0,
645            iters: 8,
646        };
647        let poses = vec![Iso3::identity()];
648        let plane = LaserPlane::new(nalgebra::Vector3::new(0.0, 0.0, 1.0), -0.5);
649        let initial = LaserlineParams::new(
650            intrinsics,
651            distortion,
652            ScheimpflugParams::default(),
653            poses,
654            plane,
655        )
656        .unwrap();
657
658        let opts = LaserlineSolveOptions::default();
659        let (ir, mut initial_map) = build_laserline_ir(&dataset, &initial, &opts).unwrap();
660
661        // Remove intrinsics from initial values
662        initial_map.remove("intrinsics");
663
664        // Should fail compilation due to missing parameter
665        use crate::backend::{OptimBackend, TinySolverBackend};
666        let backend = TinySolverBackend;
667        let backend_opts = BackendSolveOptions::default();
668        let result = backend.solve(&ir, &initial_map, &backend_opts);
669        assert!(result.is_err());
670    }
671
672    #[test]
673    #[ignore = "TODO: Fix after laserline pipeline integration"]
674    fn ir_uses_s2_for_plane_normal() {
675        let dataset = vec![View::new(
676            CorrespondenceView::new(
677                vec![Pt3::new(0.0, 0.0, 0.0); 4],
678                vec![Pt2::new(100.0, 100.0); 4],
679            )
680            .unwrap(),
681            LaserlineMeta {
682                laser_pixels: vec![Pt2::new(200.0, 200.0); 5],
683                laser_weights: Vec::new(),
684            },
685        )];
686
687        let intrinsics = FxFyCxCySkew {
688            fx: 800.0,
689            fy: 800.0,
690            cx: 512.0,
691            cy: 384.0,
692            skew: 0.0,
693        };
694        let distortion = BrownConrady5 {
695            k1: 0.0,
696            k2: 0.0,
697            k3: 0.0,
698            p1: 0.0,
699            p2: 0.0,
700            iters: 8,
701        };
702        let poses = vec![Iso3::identity()];
703        let plane = LaserPlane::new(nalgebra::Vector3::new(0.0, 0.0, 1.0), -0.5);
704        let initial = LaserlineParams::new(
705            intrinsics,
706            distortion,
707            ScheimpflugParams::default(),
708            poses,
709            plane,
710        )
711        .unwrap();
712
713        let opts = LaserlineSolveOptions::default();
714        let (ir, _) = build_laserline_ir(&dataset, &initial, &opts).unwrap();
715
716        let normal_id = ir.param_by_name("plane_normal").unwrap();
717        let distance_id = ir.param_by_name("plane_distance").unwrap();
718
719        let normal_param = &ir.params[normal_id.0];
720        let distance_param = &ir.params[distance_id.0];
721
722        assert_eq!(normal_param.dim, 3);
723        assert_eq!(normal_param.manifold, ManifoldKind::S2);
724        assert_eq!(distance_param.dim, 1);
725        assert_eq!(distance_param.manifold, ManifoldKind::Euclidean);
726    }
727}
vision_calibration_optim/problems/laserline_bundle.rs

vision_calibration_optim/problems/
laserline_bundle.rs
