vision_calibration_optim/ir/
types.rs

1use anyhow::{Result, anyhow, ensure};
2use serde::{Deserialize, Serialize};
3use std::collections::HashSet;
4
5/// Identifier for a parameter block in the IR.
6///
7/// This is stable within a `ProblemIR` instance and is used by residual blocks
8/// to reference their parameter dependencies.
9#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
10pub struct ParamId(pub usize);
11
12/// Supported manifold types for parameter blocks.
13///
14/// Each variant implies an expected ambient parameter dimension.
15#[derive(Debug, Clone, Copy, PartialEq, Eq)]
16pub enum ManifoldKind {
17    /// Standard Euclidean vector space.
18    Euclidean,
19    /// SE(3) pose stored as `[qx, qy, qz, qw, tx, ty, tz]`.
20    SE3,
21    /// SO(3) rotation stored as quaternion `[qx, qy, qz, qw]`.
22    SO3,
23    /// S2 unit sphere stored as `[x, y, z]`.
24    S2,
25}
26
27impl ManifoldKind {
28    /// Returns `true` if the given ambient dimension matches the manifold storage.
29    pub fn compatible_dim(self, dim: usize) -> bool {
30        match self {
31            ManifoldKind::Euclidean => true,
32            ManifoldKind::SE3 => dim == 7,
33            ManifoldKind::SO3 => dim == 4,
34            ManifoldKind::S2 => dim == 3,
35        }
36    }
37}
38
39/// Bounds for a single parameter index.
40///
41/// Bounds are applied after each update in backends that support them.
42#[derive(Debug, Clone, Copy, PartialEq)]
43pub struct Bound {
44    pub idx: usize,
45    pub lower: f64,
46    pub upper: f64,
47}
48
49/// Fixed parameter mask for a block.
50///
51/// Backends interpret this as per-index fixing for Euclidean blocks.
52#[derive(Debug, Clone, PartialEq, Eq)]
53pub struct FixedMask {
54    fixed_indices: HashSet<usize>,
55}
56
57impl FixedMask {
58    /// Creates a mask with no fixed indices.
59    pub fn all_free() -> Self {
60        Self {
61            fixed_indices: HashSet::new(),
62        }
63    }
64
65    /// Creates a mask with all indices fixed.
66    pub fn all_fixed(dim: usize) -> Self {
67        Self {
68            fixed_indices: (0..dim).collect(),
69        }
70    }
71
72    /// Creates a mask from an explicit list of indices.
73    pub fn fix_indices(indices: &[usize]) -> Self {
74        Self {
75            fixed_indices: indices.iter().copied().collect(),
76        }
77    }
78
79    /// Returns `true` if the index is fixed.
80    pub fn is_fixed(&self, idx: usize) -> bool {
81        self.fixed_indices.contains(&idx)
82    }
83
84    /// Returns `true` if all indices `[0, dim)` are fixed.
85    pub fn is_all_fixed(&self, dim: usize) -> bool {
86        self.fixed_indices.len() == dim
87    }
88
89    /// Iterates over fixed indices.
90    pub fn iter(&self) -> impl Iterator<Item = usize> + '_ {
91        self.fixed_indices.iter().copied()
92    }
93
94    /// Returns `true` if no indices are fixed.
95    pub fn is_empty(&self) -> bool {
96        self.fixed_indices.is_empty()
97    }
98}
99
100/// Robust loss applied to a residual block.
101///
102/// Each residual block has its own loss; per-point robustification is achieved
103/// by using one residual block per observation.
104#[derive(Debug, Clone, Copy, PartialEq, Default, Serialize, Deserialize)]
105pub enum RobustLoss {
106    /// No robustification (plain squared residual).
107    #[default]
108    None,
109    /// Huber loss with transition scale.
110    Huber {
111        /// Scale parameter controlling quadratic-to-linear transition.
112        scale: f64,
113    },
114    /// Cauchy loss with scale parameter.
115    Cauchy {
116        /// Scale parameter controlling outlier down-weighting.
117        scale: f64,
118    },
119    /// Arctangent loss with bounded influence.
120    Arctan {
121        /// Scale parameter controlling curvature.
122        scale: f64,
123    },
124}
125
126/// Hand-eye calibration mode.
127///
128/// Specifies the transform chain used for hand-eye calibration.
129#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
130pub enum HandEyeMode {
131    /// Camera mounted on robot end-effector (gripper).
132    ///
133    /// Transform chain: P_camera = extr^-1 * handeye^-1 * robot^-1 * target * P_world
134    EyeInHand,
135    /// Camera fixed in workspace, observes robot end-effector.
136    ///
137    /// Transform chain: P_camera = extr^-1 * handeye * robot * target * P_world
138    EyeToHand,
139}
140
141/// Backend-agnostic factor kinds.
142///
143/// Each factor kind implies its parameter layout and residual dimension.
144#[derive(Debug, Clone, PartialEq)]
145pub enum FactorKind {
146    /// Reprojection residual for a pinhole camera with 4 intrinsics and an SE3 pose.
147    ReprojPointPinhole4 {
148        /// World/target point coordinates.
149        pw: [f64; 3],
150        /// Observed pixel coordinates.
151        uv: [f64; 2],
152        /// Residual weight.
153        w: f64,
154    },
155    /// Reprojection residual with pinhole intrinsics, Brown-Conrady distortion, and SE3 pose.
156    ReprojPointPinhole4Dist5 {
157        /// World/target point coordinates.
158        pw: [f64; 3],
159        /// Observed pixel coordinates.
160        uv: [f64; 2],
161        /// Residual weight.
162        w: f64,
163    },
164    /// Reprojection with pinhole, distortion, Scheimpflug sensor, and SE3 pose.
165    ReprojPointPinhole4Dist5Scheimpflug2 {
166        /// World/target point coordinates.
167        pw: [f64; 3],
168        /// Observed pixel coordinates.
169        uv: [f64; 2],
170        /// Residual weight.
171        w: f64,
172    },
173    /// Reprojection with two composed SE3 transforms for rig extrinsics.
174    ///
175    /// Parameters: [intrinsics, distortion, extr_camera_to_rig, pose_target_to_rig]
176    /// Transform chain: P_camera = extr^-1 * pose * P_world
177    ReprojPointPinhole4Dist5TwoSE3 {
178        /// World/target point coordinates.
179        pw: [f64; 3],
180        /// Observed pixel coordinates.
181        uv: [f64; 2],
182        /// Residual weight.
183        w: f64,
184    },
185    /// Reprojection for hand-eye calibration with robot pose as measurement.
186    ///
187    /// Parameters: [intrinsics, distortion, extr, handeye, target]
188    /// Robot pose (`base_se3_gripper`, gripper in base frame) is stored in the factor as known data.
189    ReprojPointPinhole4Dist5HandEye {
190        /// World/target point coordinates.
191        pw: [f64; 3],
192        /// Observed pixel coordinates.
193        uv: [f64; 2],
194        /// Residual weight.
195        w: f64,
196        /// Measured robot pose `T_B_G` packed as `[qx,qy,qz,qw,tx,ty,tz]`.
197        base_to_gripper_se3: [f64; 7],
198        /// Hand-eye mode defining transform chain.
199        mode: HandEyeMode,
200    },
201    /// Reprojection for hand-eye calibration with per-view robot pose correction.
202    ///
203    /// Parameters: [intrinsics, distortion, extr, handeye, target, robot_delta]
204    /// Robot pose (`base_se3_gripper`) is stored in the factor as known data.
205    /// `robot_delta` is a 6D se(3) tangent correction applied via exp(delta) * T_B_E.
206    ReprojPointPinhole4Dist5HandEyeRobotDelta {
207        /// World/target point coordinates.
208        pw: [f64; 3],
209        /// Observed pixel coordinates.
210        uv: [f64; 2],
211        /// Residual weight.
212        w: f64,
213        /// Measured robot pose `T_B_G` packed as `[qx,qy,qz,qw,tx,ty,tz]`.
214        base_to_gripper_se3: [f64; 7],
215        /// Hand-eye mode defining transform chain.
216        mode: HandEyeMode,
217    },
218    /// Laser line pixel constrained to lie on laser plane.
219    ///
220    /// Parameters: [intrinsics, distortion, sensor, pose_cam_to_target, plane_normal, plane_distance]
221    /// Residual: point-to-plane distance for ray-target intersection point.
222    /// Note: Target is always planar (Z=0), so 3D point is computed as ray intersection.
223    LaserPlanePixel {
224        /// Observed laser pixel coordinates.
225        laser_pixel: [f64; 2],
226        /// Residual weight.
227        w: f64,
228    },
229    /// Laser line pixel constrained by line-distance in normalized plane.
230    ///
231    /// Parameters: [intrinsics, distortion, sensor, pose_cam_to_target, plane_normal, plane_distance]
232    /// Residual: perpendicular distance from normalized pixel to projected
233    ///           laser-target intersection line, scaled by sqrt(fx*fy).
234    /// Note: Alternative to LaserPlanePixel using normalized plane geometry.
235    LaserLineDist2D {
236        /// Observed laser pixel coordinates.
237        laser_pixel: [f64; 2],
238        /// Residual weight.
239        w: f64,
240    },
241    /// Placeholder for future prior factors.
242    Prior,
243    /// Zero-mean prior on a 6D se(3) tangent vector.
244    ///
245    /// Parameters: \[se3_delta\] (6D Euclidean).
246    Se3TangentPrior {
247        /// Diagonal square-root information for `[rx, ry, rz, tx, ty, tz]`.
248        sqrt_info: [f64; 6],
249    },
250    /// Placeholder for future distortion-aware reprojection.
251    ReprojPointWithDistortion,
252}
253
254impl FactorKind {
255    /// Residual dimension implied by the factor.
256    pub fn residual_dim(&self) -> usize {
257        match self {
258            FactorKind::ReprojPointPinhole4 { .. } => 2,
259            FactorKind::ReprojPointPinhole4Dist5 { .. } => 2,
260            FactorKind::ReprojPointPinhole4Dist5Scheimpflug2 { .. } => 2,
261            FactorKind::ReprojPointPinhole4Dist5TwoSE3 { .. } => 2,
262            FactorKind::ReprojPointPinhole4Dist5HandEye { .. } => 2,
263            FactorKind::ReprojPointPinhole4Dist5HandEyeRobotDelta { .. } => 2,
264            FactorKind::LaserPlanePixel { .. } => 1,
265            FactorKind::LaserLineDist2D { .. } => 1,
266            FactorKind::Prior => 0,
267            FactorKind::Se3TangentPrior { .. } => 6,
268            FactorKind::ReprojPointWithDistortion => 2,
269        }
270    }
271}
272
273/// Parameter block definition in the IR.
274///
275/// This describes the storage layout and constraints for a single variable.
276#[derive(Debug, Clone)]
277pub struct ParamBlock {
278    /// Stable parameter block ID within this IR.
279    pub id: ParamId,
280    /// Human-readable parameter block name.
281    pub name: String,
282    /// Ambient parameter dimension.
283    pub dim: usize,
284    /// Manifold type for updates/projection.
285    pub manifold: ManifoldKind,
286    /// Per-index fixed mask.
287    pub fixed: FixedMask,
288    /// Optional per-index bounds.
289    pub bounds: Option<Vec<Bound>>,
290}
291
292/// Residual block definition in the IR.
293///
294/// The order of `params` must match the factor's expected parameter order.
295#[derive(Debug, Clone)]
296pub struct ResidualBlock {
297    /// Parameter block IDs used by this residual (factor-dependent order).
298    pub params: Vec<ParamId>,
299    /// Robust loss applied to this residual block.
300    pub loss: RobustLoss,
301    /// Residual factor model.
302    pub factor: FactorKind,
303    /// Residual vector dimension.
304    pub residual_dim: usize,
305}
306
307/// Backend-agnostic optimization problem representation.
308///
309/// Backends compile this IR into solver-specific problems.
310#[derive(Debug, Default, Clone)]
311pub struct ProblemIR {
312    /// Parameter blocks in this optimization problem.
313    pub params: Vec<ParamBlock>,
314    /// Residual blocks in this optimization problem.
315    pub residuals: Vec<ResidualBlock>,
316}
317
318impl ProblemIR {
319    /// Creates an empty IR.
320    pub fn new() -> Self {
321        Self::default()
322    }
323
324    /// Adds a parameter block and returns its `ParamId`.
325    pub fn add_param_block(
326        &mut self,
327        name: impl Into<String>,
328        dim: usize,
329        manifold: ManifoldKind,
330        fixed: FixedMask,
331        bounds: Option<Vec<Bound>>,
332    ) -> ParamId {
333        let id = ParamId(self.params.len());
334        self.params.push(ParamBlock {
335            id,
336            name: name.into(),
337            dim,
338            manifold,
339            fixed,
340            bounds,
341        });
342        id
343    }
344
345    /// Adds a residual block to the IR.
346    pub fn add_residual_block(&mut self, residual: ResidualBlock) {
347        self.residuals.push(residual);
348    }
349
350    /// Finds a parameter by name.
351    pub fn param_by_name(&self, name: &str) -> Option<ParamId> {
352        self.params.iter().find(|p| p.name == name).map(|p| p.id)
353    }
354
355    /// Validates internal consistency and factor expectations.
356    ///
357    /// # Errors
358    ///
359    /// Returns [`crate::Error::Numerical`] (converted from the internal
360    /// structural error) if the IR has inconsistent parameter indices,
361    /// malformed factor blocks, or residual-dimension mismatches.
362    pub fn validate(&self) -> std::result::Result<(), crate::Error> {
363        self.validate_inner()?;
364        Ok(())
365    }
366
367    fn validate_inner(&self) -> Result<()> {
368        for (idx, param) in self.params.iter().enumerate() {
369            ensure!(
370                param.id.0 == idx,
371                "param id mismatch: expected {}, got {:?}",
372                idx,
373                param.id
374            );
375            ensure!(
376                param.manifold.compatible_dim(param.dim),
377                "param {} manifold {:?} incompatible with dim {}",
378                param.name,
379                param.manifold,
380                param.dim
381            );
382            for fixed_idx in param.fixed.iter() {
383                ensure!(
384                    fixed_idx < param.dim,
385                    "param {} fixed index {} out of range",
386                    param.name,
387                    fixed_idx
388                );
389            }
390            if let Some(bounds) = &param.bounds {
391                for bound in bounds {
392                    ensure!(
393                        bound.idx < param.dim,
394                        "param {} bound index {} out of range",
395                        param.name,
396                        bound.idx
397                    );
398                    ensure!(
399                        bound.lower <= bound.upper,
400                        "param {} bound lower {} > upper {}",
401                        param.name,
402                        bound.lower,
403                        bound.upper
404                    );
405                }
406            }
407        }
408
409        for (r_idx, residual) in self.residuals.iter().enumerate() {
410            ensure!(
411                residual.residual_dim == residual.factor.residual_dim(),
412                "residual {} dim {} does not match factor expectation {}",
413                r_idx,
414                residual.residual_dim,
415                residual.factor.residual_dim()
416            );
417            for param in &residual.params {
418                ensure!(
419                    param.0 < self.params.len(),
420                    "residual {} references missing param {:?}",
421                    r_idx,
422                    param
423                );
424            }
425
426            match &residual.factor {
427                FactorKind::ReprojPointPinhole4 { .. } => {
428                    ensure!(
429                        residual.params.len() == 2,
430                        "reprojection factor requires 2 params"
431                    );
432                    let cam = &self.params[residual.params[0].0];
433                    let pose = &self.params[residual.params[1].0];
434                    ensure!(
435                        cam.dim == 4 && cam.manifold == ManifoldKind::Euclidean,
436                        "reprojection factor expects 4D Euclidean intrinsics"
437                    );
438                    ensure!(
439                        pose.dim == 7 && pose.manifold == ManifoldKind::SE3,
440                        "reprojection factor expects 7D SE3 pose"
441                    );
442                }
443                FactorKind::ReprojPointPinhole4Dist5 { .. } => {
444                    ensure!(
445                        residual.params.len() == 3,
446                        "distortion reprojection factor requires 3 params [cam, dist, pose]"
447                    );
448                    let cam = &self.params[residual.params[0].0];
449                    let dist = &self.params[residual.params[1].0];
450                    let pose = &self.params[residual.params[2].0];
451                    ensure!(
452                        cam.dim == 4 && cam.manifold == ManifoldKind::Euclidean,
453                        "distortion reprojection expects 4D Euclidean intrinsics, got dim={} manifold={:?}",
454                        cam.dim,
455                        cam.manifold
456                    );
457                    ensure!(
458                        dist.dim == 5 && dist.manifold == ManifoldKind::Euclidean,
459                        "distortion reprojection expects 5D Euclidean distortion, got dim={} manifold={:?}",
460                        dist.dim,
461                        dist.manifold
462                    );
463                    ensure!(
464                        pose.dim == 7 && pose.manifold == ManifoldKind::SE3,
465                        "distortion reprojection expects 7D SE3 pose, got dim={} manifold={:?}",
466                        pose.dim,
467                        pose.manifold
468                    );
469                }
470                FactorKind::ReprojPointPinhole4Dist5Scheimpflug2 { .. } => {
471                    ensure!(
472                        residual.params.len() == 4,
473                        "Scheimpflug distortion reprojection factor requires 4 params [cam, dist, sensor, pose]"
474                    );
475                    let cam = &self.params[residual.params[0].0];
476                    let dist = &self.params[residual.params[1].0];
477                    let sensor = &self.params[residual.params[2].0];
478                    let pose = &self.params[residual.params[3].0];
479                    ensure!(
480                        cam.dim == 4 && cam.manifold == ManifoldKind::Euclidean,
481                        "Scheimpflug reprojection expects 4D Euclidean intrinsics, got dim={} manifold={:?}",
482                        cam.dim,
483                        cam.manifold
484                    );
485                    ensure!(
486                        dist.dim == 5 && dist.manifold == ManifoldKind::Euclidean,
487                        "Scheimpflug reprojection expects 5D Euclidean distortion, got dim={} manifold={:?}",
488                        dist.dim,
489                        dist.manifold
490                    );
491                    ensure!(
492                        sensor.dim == 2 && sensor.manifold == ManifoldKind::Euclidean,
493                        "Scheimpflug reprojection expects 2D Euclidean sensor, got dim={} manifold={:?}",
494                        sensor.dim,
495                        sensor.manifold
496                    );
497                    ensure!(
498                        pose.dim == 7 && pose.manifold == ManifoldKind::SE3,
499                        "Scheimpflug reprojection expects 7D SE3 pose, got dim={} manifold={:?}",
500                        pose.dim,
501                        pose.manifold
502                    );
503                }
504                FactorKind::ReprojPointPinhole4Dist5TwoSE3 { .. } => {
505                    ensure!(
506                        residual.params.len() == 4,
507                        "TwoSE3 factor requires 4 params [cam, dist, extr, pose]"
508                    );
509                    let cam = &self.params[residual.params[0].0];
510                    let dist = &self.params[residual.params[1].0];
511                    let extr = &self.params[residual.params[2].0];
512                    let pose = &self.params[residual.params[3].0];
513                    ensure!(
514                        cam.dim == 4 && cam.manifold == ManifoldKind::Euclidean,
515                        "TwoSE3 factor expects 4D Euclidean intrinsics, got dim={} manifold={:?}",
516                        cam.dim,
517                        cam.manifold
518                    );
519                    ensure!(
520                        dist.dim == 5 && dist.manifold == ManifoldKind::Euclidean,
521                        "TwoSE3 factor expects 5D Euclidean distortion, got dim={} manifold={:?}",
522                        dist.dim,
523                        dist.manifold
524                    );
525                    ensure!(
526                        extr.dim == 7 && extr.manifold == ManifoldKind::SE3,
527                        "TwoSE3 factor expects 7D SE3 extrinsics, got dim={} manifold={:?}",
528                        extr.dim,
529                        extr.manifold
530                    );
531                    ensure!(
532                        pose.dim == 7 && pose.manifold == ManifoldKind::SE3,
533                        "TwoSE3 factor expects 7D SE3 pose, got dim={} manifold={:?}",
534                        pose.dim,
535                        pose.manifold
536                    );
537                }
538                FactorKind::ReprojPointPinhole4Dist5HandEye { .. } => {
539                    ensure!(
540                        residual.params.len() == 5,
541                        "HandEye factor requires 5 params [cam, dist, extr, handeye, target]"
542                    );
543                    let cam = &self.params[residual.params[0].0];
544                    let dist = &self.params[residual.params[1].0];
545                    let extr = &self.params[residual.params[2].0];
546                    let handeye = &self.params[residual.params[3].0];
547                    let target = &self.params[residual.params[4].0];
548                    ensure!(
549                        cam.dim == 4 && cam.manifold == ManifoldKind::Euclidean,
550                        "HandEye factor expects 4D Euclidean intrinsics, got dim={} manifold={:?}",
551                        cam.dim,
552                        cam.manifold
553                    );
554                    ensure!(
555                        dist.dim == 5 && dist.manifold == ManifoldKind::Euclidean,
556                        "HandEye factor expects 5D Euclidean distortion, got dim={} manifold={:?}",
557                        dist.dim,
558                        dist.manifold
559                    );
560                    ensure!(
561                        extr.dim == 7 && extr.manifold == ManifoldKind::SE3,
562                        "HandEye factor expects 7D SE3 extrinsics, got dim={} manifold={:?}",
563                        extr.dim,
564                        extr.manifold
565                    );
566                    ensure!(
567                        handeye.dim == 7 && handeye.manifold == ManifoldKind::SE3,
568                        "HandEye factor expects 7D SE3 hand-eye transform, got dim={} manifold={:?}",
569                        handeye.dim,
570                        handeye.manifold
571                    );
572                    ensure!(
573                        target.dim == 7 && target.manifold == ManifoldKind::SE3,
574                        "HandEye factor expects 7D SE3 target pose, got dim={} manifold={:?}",
575                        target.dim,
576                        target.manifold
577                    );
578                }
579                FactorKind::ReprojPointPinhole4Dist5HandEyeRobotDelta { .. } => {
580                    ensure!(
581                        residual.params.len() == 6,
582                        "HandEye factor requires 6 params [cam, dist, extr, handeye, target, robot_delta]"
583                    );
584                    let cam = &self.params[residual.params[0].0];
585                    let dist = &self.params[residual.params[1].0];
586                    let extr = &self.params[residual.params[2].0];
587                    let handeye = &self.params[residual.params[3].0];
588                    let target = &self.params[residual.params[4].0];
589                    let robot_delta = &self.params[residual.params[5].0];
590                    ensure!(
591                        cam.dim == 4 && cam.manifold == ManifoldKind::Euclidean,
592                        "HandEye factor expects 4D Euclidean intrinsics, got dim={} manifold={:?}",
593                        cam.dim,
594                        cam.manifold
595                    );
596                    ensure!(
597                        dist.dim == 5 && dist.manifold == ManifoldKind::Euclidean,
598                        "HandEye factor expects 5D Euclidean distortion, got dim={} manifold={:?}",
599                        dist.dim,
600                        dist.manifold
601                    );
602                    ensure!(
603                        extr.dim == 7 && extr.manifold == ManifoldKind::SE3,
604                        "HandEye factor expects 7D SE3 extrinsics, got dim={} manifold={:?}",
605                        extr.dim,
606                        extr.manifold
607                    );
608                    ensure!(
609                        handeye.dim == 7 && handeye.manifold == ManifoldKind::SE3,
610                        "HandEye factor expects 7D SE3 hand-eye transform, got dim={} manifold={:?}",
611                        handeye.dim,
612                        handeye.manifold
613                    );
614                    ensure!(
615                        target.dim == 7 && target.manifold == ManifoldKind::SE3,
616                        "HandEye factor expects 7D SE3 target pose, got dim={} manifold={:?}",
617                        target.dim,
618                        target.manifold
619                    );
620                    ensure!(
621                        robot_delta.dim == 6 && robot_delta.manifold == ManifoldKind::Euclidean,
622                        "HandEye factor expects 6D Euclidean robot delta, got dim={} manifold={:?}",
623                        robot_delta.dim,
624                        robot_delta.manifold
625                    );
626                }
627                FactorKind::LaserPlanePixel { .. } => {
628                    ensure!(
629                        residual.params.len() == 6,
630                        "LaserPlanePixel factor requires 6 params [cam, dist, sensor, pose, plane_normal, plane_distance]"
631                    );
632                    let cam = &self.params[residual.params[0].0];
633                    let dist = &self.params[residual.params[1].0];
634                    let sensor = &self.params[residual.params[2].0];
635                    let pose = &self.params[residual.params[3].0];
636                    let plane_normal = &self.params[residual.params[4].0];
637                    let plane_distance = &self.params[residual.params[5].0];
638                    ensure!(
639                        cam.dim == 4 && cam.manifold == ManifoldKind::Euclidean,
640                        "LaserPlanePixel factor expects 4D Euclidean intrinsics, got dim={} manifold={:?}",
641                        cam.dim,
642                        cam.manifold
643                    );
644                    ensure!(
645                        dist.dim == 5 && dist.manifold == ManifoldKind::Euclidean,
646                        "LaserPlanePixel factor expects 5D Euclidean distortion, got dim={} manifold={:?}",
647                        dist.dim,
648                        dist.manifold
649                    );
650                    ensure!(
651                        sensor.dim == 2 && sensor.manifold == ManifoldKind::Euclidean,
652                        "LaserPlanePixel factor expects 2D Euclidean sensor params, got dim={} manifold={:?}",
653                        sensor.dim,
654                        sensor.manifold
655                    );
656                    ensure!(
657                        pose.dim == 7 && pose.manifold == ManifoldKind::SE3,
658                        "LaserPlanePixel factor expects 7D SE3 pose, got dim={} manifold={:?}",
659                        pose.dim,
660                        pose.manifold
661                    );
662                    ensure!(
663                        plane_normal.dim == 3 && plane_normal.manifold == ManifoldKind::S2,
664                        "LaserPlanePixel factor expects 3D S2 plane normal, got dim={} manifold={:?}",
665                        plane_normal.dim,
666                        plane_normal.manifold
667                    );
668                    ensure!(
669                        plane_distance.dim == 1
670                            && plane_distance.manifold == ManifoldKind::Euclidean,
671                        "LaserPlanePixel factor expects 1D Euclidean plane distance, got dim={} manifold={:?}",
672                        plane_distance.dim,
673                        plane_distance.manifold
674                    );
675                }
676                FactorKind::LaserLineDist2D { .. } => {
677                    ensure!(
678                        residual.params.len() == 6,
679                        "LaserLineDist2D factor requires 6 params [cam, dist, sensor, pose, plane_normal, plane_distance]"
680                    );
681                    let cam = &self.params[residual.params[0].0];
682                    let dist = &self.params[residual.params[1].0];
683                    let sensor = &self.params[residual.params[2].0];
684                    let pose = &self.params[residual.params[3].0];
685                    let plane_normal = &self.params[residual.params[4].0];
686                    let plane_distance = &self.params[residual.params[5].0];
687                    ensure!(
688                        cam.dim == 4 && cam.manifold == ManifoldKind::Euclidean,
689                        "LaserLineDist2D factor expects 4D Euclidean intrinsics, got dim={} manifold={:?}",
690                        cam.dim,
691                        cam.manifold
692                    );
693                    ensure!(
694                        dist.dim == 5 && dist.manifold == ManifoldKind::Euclidean,
695                        "LaserLineDist2D factor expects 5D Euclidean distortion, got dim={} manifold={:?}",
696                        dist.dim,
697                        dist.manifold
698                    );
699                    ensure!(
700                        sensor.dim == 2 && sensor.manifold == ManifoldKind::Euclidean,
701                        "LaserLineDist2D factor expects 2D Euclidean sensor params, got dim={} manifold={:?}",
702                        sensor.dim,
703                        sensor.manifold
704                    );
705                    ensure!(
706                        pose.dim == 7 && pose.manifold == ManifoldKind::SE3,
707                        "LaserLineDist2D factor expects 7D SE3 pose, got dim={} manifold={:?}",
708                        pose.dim,
709                        pose.manifold
710                    );
711                    ensure!(
712                        plane_normal.dim == 3 && plane_normal.manifold == ManifoldKind::S2,
713                        "LaserLineDist2D factor expects 3D S2 plane normal, got dim={} manifold={:?}",
714                        plane_normal.dim,
715                        plane_normal.manifold
716                    );
717                    ensure!(
718                        plane_distance.dim == 1
719                            && plane_distance.manifold == ManifoldKind::Euclidean,
720                        "LaserLineDist2D factor expects 1D Euclidean plane distance, got dim={} manifold={:?}",
721                        plane_distance.dim,
722                        plane_distance.manifold
723                    );
724                }
725                FactorKind::Se3TangentPrior { .. } => {
726                    ensure!(
727                        residual.params.len() == 1,
728                        "Se3TangentPrior requires 1 param [robot_delta]"
729                    );
730                    let delta = &self.params[residual.params[0].0];
731                    ensure!(
732                        delta.dim == 6 && delta.manifold == ManifoldKind::Euclidean,
733                        "Se3TangentPrior expects 6D Euclidean delta, got dim={} manifold={:?}",
734                        delta.dim,
735                        delta.manifold
736                    );
737                }
738                FactorKind::Prior | FactorKind::ReprojPointWithDistortion => {
739                    return Err(anyhow!(
740                        "factor kind {:?} not implemented in validation",
741                        residual.factor
742                    ));
743                }
744            }
745        }
746
747        Ok(())
748    }
749}
vision_calibration_optim/ir/types.rs

vision_calibration_optim/ir/
types.rs
