braid_sim/
calibration.rs

1// Copyright (C) The Strand-Braid Authors
2// SPDX-License-Identifier: MIT OR Apache-2.0
3
4//! Generate a synthetic multi-camera calibration from a [`Scenario`] and
5//! serialize it to the flydra XML format that Braid loads.
6
7use std::collections::BTreeMap;
8use std::f64::consts::PI;
9
10use braid_mvg::Camera;
11use cam_geom::ExtrinsicParameters;
12use flydra_mvg::FlydraMultiCameraSystem;
13use nalgebra::{Unit, Vector3};
14use opencv_ros_camera::RosOpenCvIntrinsics;
15
16use crate::scenario::{CalibrationPerturbation, Scenario};
17
18/// Build the perfect *generation* calibration: `count` ideal-pinhole cameras
19/// evenly spaced on a horizontal ring around the arena center, each looking at
20/// the center.
21///
22/// This is the calibration used to project ground truth (generate observations),
23/// i.e. "what was imaged". In the perfect-world baseline it is also the tracking
24/// calibration (see [`build_tracking_calibration`]), so the reconstruction
25/// recovers ground truth exactly up to numerical precision.
26pub fn build_calibration(scenario: &Scenario) -> eyre::Result<FlydraMultiCameraSystem<f64>> {
27    build(scenario, None)
28}
29
30/// Build the calibration Braid *tracks* with: the perfect generation calibration
31/// of [`build_calibration`] with the scenario's
32/// [`CalibrationPerturbation`](crate::scenario::CalibrationPerturbation) applied
33/// (pose / intrinsic error). With the default (identity) perturbation this is
34/// byte-identical to [`build_calibration`]; with a nonzero perturbation the
35/// tracker reconstructs with a slightly wrong calibration while the detections
36/// were generated with the perfect one, so reprojection error is realistic.
37pub fn build_tracking_calibration(
38    scenario: &Scenario,
39) -> eyre::Result<FlydraMultiCameraSystem<f64>> {
40    if scenario.calibration_perturbation.is_identity() {
41        return build_calibration(scenario);
42    }
43    build(scenario, Some(&scenario.calibration_perturbation))
44}
45
46/// Build a ring calibration, optionally perturbing each camera's pose and
47/// intrinsics by deterministic per-camera offsets.
48fn build(
49    scenario: &Scenario,
50    perturbation: Option<&CalibrationPerturbation>,
51) -> eyre::Result<FlydraMultiCameraSystem<f64>> {
52    let c = scenario.arena.center();
53    let up = Unit::new_normalize(Vector3::new(0.0, 0.0, 1.0));
54    let rig = &scenario.cameras;
55
56    let mut cams_by_name = BTreeMap::new();
57    for k in 0..rig.count {
58        let off = perturbation.map(|p| p.offsets(scenario.seed, k));
59        let dpos = off.map_or([0.0; 3], |o| o.d_position_m);
60        let dlook = off.map_or([0.0; 3], |o| o.d_look_at_m);
61        let dfocal = off.map_or(0.0, |o| o.d_focal_px);
62        let dcx = off.map_or(0.0, |o| o.d_cx_px);
63        let dcy = off.map_or(0.0, |o| o.d_cy_px);
64
65        let angle = 2.0 * PI * (k as f64) / (rig.count as f64);
66        let camcenter = Vector3::new(
67            c[0] + rig.radius_m * angle.cos() + dpos[0],
68            c[1] + rig.radius_m * angle.sin() + dpos[1],
69            rig.height_m + dpos[2],
70        );
71        // Perturbing the look-at target rotates the camera (a pointing error)
72        // without moving its center.
73        let target = Vector3::new(c[0] + dlook[0], c[1] + dlook[1], c[2] + dlook[2]);
74        let extrinsics = ExtrinsicParameters::from_view(&camcenter, &target, &up);
75        let f = rig.focal_length_px + dfocal;
76        let cx = rig.image_width as f64 / 2.0 + dcx;
77        let cy = rig.image_height as f64 / 2.0 + dcy;
78        let intrinsics = RosOpenCvIntrinsics::from_params(f, 0.0, f, cx, cy);
79        let cam = Camera::new(rig.image_width, rig.image_height, extrinsics, intrinsics)
80            .map_err(|e| eyre::eyre!("building camera {k}: {e}"))?;
81        cams_by_name.insert(Scenario::camera_name(k), cam);
82    }
83
84    Ok(FlydraMultiCameraSystem::new(cams_by_name, None))
85}
86
87/// Serialize a calibration to flydra XML (the format referenced by
88/// `mainbrain.cal_fname`).
89pub fn to_flydra_xml_string(system: &FlydraMultiCameraSystem<f64>) -> eyre::Result<String> {
90    let mut buf = Vec::new();
91    system
92        .to_flydra_xml(&mut buf)
93        .map_err(|e| eyre::eyre!("serializing calibration to flydra XML: {e}"))?;
94    Ok(String::from_utf8(buf)?)
95}
96
97#[cfg(test)]
98mod tests {
99    use super::*;
100    use crate::scenario::CalibrationPerturbation;
101
102    fn demo_scenario() -> Scenario {
103        Scenario::from_toml_str(include_str!("../example-sim.toml")).unwrap()
104    }
105
106    /// With the default (identity) perturbation, the tracking calibration is the
107    /// perfect generation calibration: their flydra-XML serializations match.
108    #[test]
109    fn identity_perturbation_matches_perfect() {
110        let s = demo_scenario();
111        assert!(s.calibration_perturbation.is_identity());
112        let perfect = to_flydra_xml_string(&build_calibration(&s).unwrap()).unwrap();
113        let tracking = to_flydra_xml_string(&build_tracking_calibration(&s).unwrap()).unwrap();
114        assert_eq!(perfect, tracking);
115    }
116
117    /// A nonzero perturbation changes the tracking calibration (so reprojection
118    /// error becomes nonzero) and is deterministic for a fixed `(scenario, seed)`.
119    #[test]
120    fn nonzero_perturbation_differs_and_is_deterministic() {
121        let mut s = demo_scenario();
122        s.calibration_perturbation = CalibrationPerturbation {
123            camera_position_m: 0.005,
124            look_at_m: 0.005,
125            focal_length_px: 3.0,
126            principal_point_px: 2.0,
127        };
128        let perfect = to_flydra_xml_string(&build_calibration(&s).unwrap()).unwrap();
129        let tracking = to_flydra_xml_string(&build_tracking_calibration(&s).unwrap()).unwrap();
130        assert_ne!(
131            perfect, tracking,
132            "perturbation should change the calibration"
133        );
134
135        // Deterministic: rebuilding yields the identical perturbed calibration.
136        let again = to_flydra_xml_string(&build_tracking_calibration(&s).unwrap()).unwrap();
137        assert_eq!(tracking, again);
138    }
139}
braid_sim/calibration.rs

braid_sim/
calibration.rs
